From d113f751aa9675c69f854e4950281b51801c3888 Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Mon, 8 Sep 2025 23:29:39 +0800 Subject: [PATCH 01/44] init project --- src/humanoid_control | 1 + 1 file changed, 1 insertion(+) create mode 160000 src/humanoid_control diff --git a/src/humanoid_control b/src/humanoid_control new file mode 160000 index 0000000000..e33ecfa933 --- /dev/null +++ b/src/humanoid_control @@ -0,0 +1 @@ +Subproject commit e33ecfa933f63a32f855551b6b4d8e377eff4426 From 4e99912130751ef7e0c9a2c71515c5952865a1cd Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Mon, 8 Sep 2025 23:47:48 +0800 Subject: [PATCH 02/44] init project --- src/humanoid_control | 1 - src/humanoid_control/README.md.txt | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 160000 src/humanoid_control create mode 100644 src/humanoid_control/README.md.txt diff --git a/src/humanoid_control b/src/humanoid_control deleted file mode 160000 index e33ecfa933..0000000000 --- a/src/humanoid_control +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e33ecfa933f63a32f855551b6b4d8e377eff4426 diff --git a/src/humanoid_control/README.md.txt b/src/humanoid_control/README.md.txt new file mode 100644 index 0000000000..7c09cab9a2 --- /dev/null +++ b/src/humanoid_control/README.md.txt @@ -0,0 +1 @@ +人形机器人 From df58c95679b6d128d6a80d1f5e0bd82069d9e1a5 Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Tue, 9 Sep 2025 22:24:26 +0800 Subject: [PATCH 03/44] =?UTF-8?q?=E8=A1=A5=E5=85=85=E8=AF=A6=E7=BB=86?= =?UTF-8?q?=E6=B3=A8=E9=87=8A=E3=80=81=E7=BB=93=E6=9E=84=E5=8C=96=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 + src/humanoid_control/README.md.txt | 1 - src/humanoid_mujoco_py/README.md | 107 +++++++++++++++++++++++++++++ src/humanoid_mujoco_py/hello.xml | 10 +++ src/humanoid_mujoco_py/main.py | 59 ++++++++++++++++ 5 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 .vscode/settings.json delete mode 100644 src/humanoid_control/README.md.txt create mode 100644 src/humanoid_mujoco_py/README.md create mode 100644 src/humanoid_mujoco_py/hello.xml create mode 100644 src/humanoid_mujoco_py/main.py diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..7e68766ae6 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python-envs.pythonProjects": [] +} \ No newline at end of file diff --git a/src/humanoid_control/README.md.txt b/src/humanoid_control/README.md.txt deleted file mode 100644 index 7c09cab9a2..0000000000 --- a/src/humanoid_control/README.md.txt +++ /dev/null @@ -1 +0,0 @@ -人形机器人 diff --git a/src/humanoid_mujoco_py/README.md b/src/humanoid_mujoco_py/README.md new file mode 100644 index 0000000000..369561520a --- /dev/null +++ b/src/humanoid_mujoco_py/README.md @@ -0,0 +1,107 @@ +# 人形机器人 +# MuJoCo Python 模拟示例 + +这是一个使用MuJoCo物理引擎的Python示例项目,展示了如何加载模型、运行物理模拟并实时可视化结果。示例模拟了一个简单场景:一个盒子在重力作用下自由下落到地面。 + +## 项目结构 +humanoid_mujoco_py/ +├─ hello.xml # MJCF模型文件,定义物理场景 +├─ main.py # Python主程序,运行模拟 +└─ README.md # 项目说明文档 +## 环境要求 + +- Python 3.8 或更高版本 +- MuJoCo 2.3.0 或更高版本 +- 操作系统:Windows、macOS 或 Linux + +## 安装步骤 + +1. **克隆或下载项目** + + ```bash + git clone <仓库地址> + cd mujoco_py_demo + ``` + +2. **创建并激活虚拟环境(推荐)** + + ```bash + # 创建虚拟环境 + python -m venv mujoco_env + + # 激活虚拟环境 + # Windows: + mujoco_env\Scripts\activate + # macOS/Linux: + source mujoco_env/bin/activate + ``` + +3. **安装依赖包** + + ```bash + # 安装MuJoCo Python绑定 + pip install mujoco + + # 如需增强可视化功能(可选) + pip install mujoco-viewer + ``` + +## 使用方法 + +1. 确保`hello.xml`和`demo.py`在同一目录下 + +2. 运行模拟程序 + + ```bash + python demo.py + ``` + +3. 程序将: + - 加载`hello.xml`中定义的物理模型 + - 启动可视化窗口显示模拟过程 + - 在终端输出盒子的实时位置信息 + - 模拟持续10秒后自动结束 + +## 交互操作 + +在可视化窗口中,可以使用以下鼠标操作: +- 左键拖动:旋转视角 +- 右键拖动:平移视角 +- 滚轮:缩放视图 +- Shift+左键:拖动场景中的物体 + +## 代码说明 + +- `hello.xml`:使用MJCF(MuJoCo XML格式)定义物理场景,包含: + - 一个地面平面 + - 一个光源 + - 一个带自由关节的盒子(可在6个自由度上运动) + +- `demo.py`:Python程序实现以下功能: + - 加载MJCF模型 + - 初始化物理模拟 + - 启动可视化界面 + - 运行模拟循环并输出关键数据 + +## 扩展方向 + +1. **修改模型**:编辑`hello.xml`更改场景元素,如: + - 添加更多物体 + - 改变物体形状、质量或颜色 + - 添加关节约束 + +2. **增强模拟**:修改`demo.py`实现: + - 施加外力或扭矩 + - 改变重力方向或大小 + - 实现简单控制算法 + +3. **数据记录**:添加代码记录模拟数据,如: + - 物体轨迹 + - 接触力 + - 速度和加速度变化 + +## 参考资料 + +- [MuJoCo 官方文档](https://mujoco.readthedocs.io/) +- [MuJoCo GitHub 仓库](https://github.com/google-deepmind/mujoco) +- [MJCF 模型格式参考](https://mujoco.readthedocs.io/en/stable/XMLreference.html) diff --git a/src/humanoid_mujoco_py/hello.xml b/src/humanoid_mujoco_py/hello.xml new file mode 100644 index 0000000000..ef8a26b1b1 --- /dev/null +++ b/src/humanoid_mujoco_py/hello.xml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/src/humanoid_mujoco_py/main.py b/src/humanoid_mujoco_py/main.py new file mode 100644 index 0000000000..d0578f1db2 --- /dev/null +++ b/src/humanoid_mujoco_py/main.py @@ -0,0 +1,59 @@ +# 标准库 +# 导入时间模块,用于控制模拟帧率 +import time + +# 第三方库 +# 导入MuJoCo核心库,提供物理引擎功能 +import mujoco +# 从MuJoCo导入可视化工具,用于实时显示模拟过程 +from mujoco import viewer + +def main(): + """ + 主函数:加载MuJoCo模型并运行物理模拟 + + 流程: + 1. 加载XML格式的模型文件 + 2. 初始化模拟数据结构 + 3. 启动可视化窗口 + 4. 运行指定时长的模拟循环 + 5. 输出关键模拟数据并更新可视化 + """ + # 加载MJCF模型文件 + # 模型定义了物理场景:包含平面、光源和一个自由下落的盒子 + # from_xml_path方法支持从XML文件加载模型,替代C++中的mj_loadXML + model = mujoco.MjModel.from_xml_path("src\humanoid_control\hello.xml") + + # 创建与模型对应的动态数据结构 + # 存储模拟过程中的状态变量(位置、速度等),类似C++中的mjData + data = mujoco.MjData(model) + + # 启动被动式可视化窗口 + # 被动模式意味着我们手动控制模拟步骤和画面更新 + # 使用with语句确保资源正确释放 + with viewer.launch_passive(model, data) as v: + # 运行模拟(10秒,按每秒60步计算) + # 600步 ≈ 10秒(默认时间步长为1/60秒) + for _ in range(600): + # 推进模拟一步 + # 该函数执行一次完整的前向动力学计算 + mujoco.mj_step(model, data) + + # 打印盒子的位置信息(可选) + # data.xpos存储所有刚体的位置,是一个(n, 3)的数组 + # 索引1对应示例中的盒子(索引0是地面) + print(f"时间: {data.time:.2f}, " + f"位置: ({data.xpos[1, 0]:.2f}, {data.xpos[1, 1]:.2f}, {data.xpos[1, 2]:.2f})") + + # 更新可视化窗口 + # 将当前模拟状态同步到视图 + v.sync() + + # 控制可视化帧率 + # 暂停0.01秒,使可视化更流畅(实际模拟不受影响) + time.sleep(0.01) + +# 程序入口点 +if __name__ == "__main__": + # 调用主函数启动模拟 + main() From b56ed65f877863c5a66e28671406a18fe330697e Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Wed, 10 Sep 2025 08:53:41 +0800 Subject: [PATCH 04/44] Remove .vscode directory (personal editor config) --- .vscode/settings.json | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 7e68766ae6..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "python-envs.pythonProjects": [] -} \ No newline at end of file From 4a1042d3cf0185dac5aa021b8125b5bb18df4b1e Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Mon, 15 Sep 2025 20:17:56 +0800 Subject: [PATCH 05/44] =?UTF-8?q?=E5=9F=BA=E7=A1=80=E4=BA=BA=E5=BD=A2?= =?UTF-8?q?=E6=9C=BA=E5=99=A8=E4=BA=BA=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mujoco01/README.md | 78 +++++++++++ src/mujoco01/humanoid.xml | 267 ++++++++++++++++++++++++++++++++++++++ src/mujoco01/main.py | 66 ++++++++++ 3 files changed, 411 insertions(+) create mode 100644 src/mujoco01/README.md create mode 100644 src/mujoco01/humanoid.xml create mode 100644 src/mujoco01/main.py diff --git a/src/mujoco01/README.md b/src/mujoco01/README.md new file mode 100644 index 0000000000..5c950c04e9 --- /dev/null +++ b/src/mujoco01/README.md @@ -0,0 +1,78 @@ +# 人形机器人 +# MuJoCo Python 模拟示例 + +这是一个使用MuJoCo物理引擎的Python示例项目,展示了如何加载人形机器人模型、运行物理模拟并实时可视化结果。示例模拟了人形机器人在预设初始姿势下的物理运动,支持实时查看关节状态与躯干位置。 + +## 项目结构 +humanoid_mujoco_py/ +├─ humanoid.xml # MJCF模型文件,定义人形机器人结构与物理参数 +├─ main.py # Python主程序,加载模型、运行模拟与可视化 +└─ README.md # 项目说明文档 + +## 使用方法 + +1. 确认文件路径:确保`humanoid.xml`与`main.py`在同一目录下(或在`main.py`中修改模型路径为绝对路径)。 + +2. 运行模拟程序 + + ```bash + python main.py + ``` + +3. 程序将: + - 加载`humanoid.xml`中定义的人形机器人模型(含躯干、四肢、关节与执行器) + - 初始化模拟数据(关节位置、速度、物理状态) + - 设置初始姿势为「深蹲(squat)」(可修改代码切换其他姿势) + - 启动可视化窗口,实时渲染机器人运动 + - 在终端输出实时仿真时间与躯干位置(格式:`时间: X.XX, 躯干位置: (X.XX, X.XX, X.XX)`) + - 模拟持续20秒(1200步,每步0.005秒)后自动结束 + +## 交互操作 + +在可视化窗口中,可以使用以下鼠标操作调整视角或交互: +- 左键拖动:旋转场景视角 +- 右键拖动:平移场景位置 +- 滚轮:缩放视图(拉近/拉远) +- Shift+左键:拖动场景中的刚体(如机器人躯干、四肢) + +## 代码说明 + +- `humanoid.xml`:使用MJCF(MuJoCo XML格式)定义人形机器人的完整物理场景,核心内容包括: + - **机器人结构**:躯干(torso)、头部(head)、四肢(大腿、小腿、手臂、手掌)的几何形状与连接关系 + - **关节参数**:各关节(髋关节、膝关节、肩关节等)的旋转轴、角度范围(如髋关节Y轴范围`-150~20°`)、阻尼与刚度 + - **物理属性**:材料(身体肤色、地面网格)、摩擦系数(`0.7`)、碰撞排除规则(避免腰部与大腿误碰撞) + - **执行器**:21个电机(控制关节运动),含齿轮比(如髋关节电机`gear=120`)与控制范围(`-1~1`) + - **初始关键帧**:4种预设姿势(深蹲`squat`、单左腿站立`stand_on_left_leg`、俯卧`prone`、仰卧`supine`) + +- `main.py`:Python程序实现模拟全流程控制,核心功能包括: + - 模型加载:通过`mujoco.MjModel.from_xml_path()`读取`humanoid.xml`,捕获加载错误(如路径错误) + - 数据初始化:通过`mujoco.MjData(model)`创建仿真状态容器(存储关节位置`qpos`、时间`time`等) + - 初始姿势设置:通过`mujoco.mj_resetDataKeyframe(model, data, 0)`设置为深蹲姿势(索引`0~3`对应4种预设姿势) + - 可视化启动:通过`viewer.launch_passive()`启动被动式Viewer(手动控制仿真步长与视图更新) + - 仿真循环:1200步循环中,通过`mujoco.mj_step()`执行物理计算,`viewer.sync()`更新视图,`time.sleep(0.005)`控制帧率 + +## 扩展方向 + +1. **修改初始姿势与模拟时长**: + - 切换姿势:修改`main.py`中`mujoco.mj_resetDataKeyframe()`的第三个参数(`0=深蹲`、`1=单左腿站立`、`2=俯卧`、`3=仰卧`) + - 延长/缩短模拟:调整`for _ in range(1200)`的循环次数(公式:循环次数 = 目标时长(秒)/ 0.005) + +2. **自定义机器人物理属性**: + - 编辑`humanoid.xml`:调整关节角度范围(如`joint range="-30 10"`)、电机齿轮比(`actuator gear="40"`)、地面摩擦系数(`geom friction=".7"`) + - 添加新部件:在``中新增``节点,定义额外几何形状(如机器人手持物体) + +3. **增强模拟功能**: + - 施加控制信号:在仿真循环中修改`data.ctrl`数组(如`data.ctrl[0] = 0.5`控制腹部Z轴电机),实现行走、抬手等动作 + - 记录仿真数据:添加代码将`data.qpos`(关节位置)、`data.qvel`(关节速度)写入CSV文件,用于后续分析 + - 添加传感器:在`humanoid.xml`中添加``节点(如力传感器、加速度传感器),在`main.py`中读取`data.sensordata`获取数据 + +4. **优化可视化**: + - 调整相机视角:修改`humanoid.xml`中``节点的`pos`(位置)、`xyaxes`(朝向)参数 + - 自定义颜色:修改``中`texture`的`rgb1`/`rgb2`值,更改机器人身体或地面颜色 + +## 参考资料 + +- [MuJoCo 官方文档](https://mujoco.readthedocs.io/):学习MJCF格式、API接口与物理仿真原理 +- [MuJoCo GitHub 仓库](https://github.com/google-deepmind/mujoco):获取最新版MuJoCo库与官方示例模型 +- [MJCF 模型格式参考](https://mujoco.readthedocs.io/en/stable/XMLreference.html):详细了解`humanoid.xml`各节点配置规则 +- [MuJoCo Python 教程](https://mujoco.readthedocs.io/en/stable/python.html):学习Python调用MuJoCo的进阶用法(如控制、传感器、数据记录) \ No newline at end of file diff --git a/src/mujoco01/humanoid.xml b/src/mujoco01/humanoid.xml new file mode 100644 index 0000000000..e45b8f01cd --- /dev/null +++ b/src/mujoco01/humanoid.xml @@ -0,0 +1,267 @@ + + + + diff --git a/src/mujoco01/main.py b/src/mujoco01/main.py new file mode 100644 index 0000000000..c8e4ea7a76 --- /dev/null +++ b/src/mujoco01/main.py @@ -0,0 +1,66 @@ +# 标准库 +# 导入时间模块,用于控制模拟帧率 +import time + +# 第三方库 +# 导入MuJoCo核心库,提供物理引擎功能 +import mujoco +# 从MuJoCo导入可视化工具,用于实时显示模拟过程 +from mujoco import viewer + +def main(): + """ + 主函数:加载人形机器人模型并运行物理模拟 + + 流程: + 1. 加载XML格式的人形机器人模型文件 + 2. 初始化模拟数据结构 + 3. 设置初始姿势为深蹲姿态 + 4. 启动可视化窗口 + 5. 运行指定时长的模拟循环 + 6. 输出关键模拟数据并更新可视化 + """ + # 加载MJCF模型文件 + # 模型定义了完整的人形机器人结构:包括躯干、四肢、关节和执行器 + try: + model = mujoco.MjModel.from_xml_path("mujoco01\humanoid.xml") + except Exception as e: + print(f"模型加载失败: {e}") + return + + # 创建与模型对应的动态数据结构 + # 存储模拟过程中的状态变量(位置、速度、力等) + data = mujoco.MjData(model) + + # 设置初始姿势为深蹲姿态 + # 使用keyframe索引0,对应XML中定义的"squat"姿势 + mujoco.mj_resetDataKeyframe(model, data, 0) + + # 启动被动式可视化窗口 + # 被动模式意味着我们手动控制模拟步骤和画面更新 + # 使用with语句确保资源正确释放 + with viewer.launch_passive(model, data) as v: + # 运行模拟(20秒,按每秒60步计算) + # 1200步 ≈ 20秒(模型定义的时间步长为0.005秒) + for _ in range(1200): + # 推进模拟一步 + # 该函数执行一次完整的前向动力学计算 + mujoco.mj_step(model, data) + + # 打印机器人关键位置信息 + # data.qpos存储所有关节的位置,前3个值是躯干位置 + print(f"时间: {data.time:.2f}, " + f"躯干位置: ({data.qpos[0]:.2f}, {data.qpos[1]:.2f}, {data.qpos[2]:.2f})") + + # 更新可视化窗口 + # 将当前模拟状态同步到视图 + v.sync() + + # 控制可视化帧率 + # 暂停一小段时间,使可视化更流畅(实际模拟不受影响) + time.sleep(0.005) + +# 程序入口点 +if __name__ == "__main__": + # 调用主函数启动模拟 + main() From 0c6043339fa61fdb66d113171bebec91f4f92d84 Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Mon, 22 Sep 2025 09:30:50 +0800 Subject: [PATCH 06/44] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E5=90=8D=E5=8F=8A=E4=B8=BB=E5=87=BD=E6=95=B0=E4=B8=AD=E5=BE=AA?= =?UTF-8?q?=E7=8E=AF=E6=A8=A1=E6=8B=9F=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/MUJOCO_LOG.TXT | 6 ++ .../README.md | 0 .../humanoid.xml | 15 ----- src/humanoid_motion control/main.py | 57 ++++++++++++++++ src/mujoco01/main.py | 66 ------------------- 5 files changed, 63 insertions(+), 81 deletions(-) create mode 100644 src/MUJOCO_LOG.TXT rename src/{mujoco01 => humanoid_motion control}/README.md (100%) rename src/{mujoco01 => humanoid_motion control}/humanoid.xml (94%) create mode 100644 src/humanoid_motion control/main.py delete mode 100644 src/mujoco01/main.py diff --git a/src/MUJOCO_LOG.TXT b/src/MUJOCO_LOG.TXT new file mode 100644 index 0000000000..1cc12eb1d7 --- /dev/null +++ b/src/MUJOCO_LOG.TXT @@ -0,0 +1,6 @@ +Mon Sep 15 19:31:15 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 5. The simulation is unstable. Time = 0.0080. + +Mon Sep 15 19:37:38 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 5. The simulation is unstable. Time = 0.0080. + diff --git a/src/mujoco01/README.md b/src/humanoid_motion control/README.md similarity index 100% rename from src/mujoco01/README.md rename to src/humanoid_motion control/README.md diff --git a/src/mujoco01/humanoid.xml b/src/humanoid_motion control/humanoid.xml similarity index 94% rename from src/mujoco01/humanoid.xml rename to src/humanoid_motion control/humanoid.xml index e45b8f01cd..4383f172c0 100644 --- a/src/mujoco01/humanoid.xml +++ b/src/humanoid_motion control/humanoid.xml @@ -1,18 +1,3 @@ - - """ - return load_model_from_xml(xml) + return xml def _build_gaps_corridor(self): """构建带空隙的走廊(平台+空隙交替)""" - gaps_xml = "" - platform_length = 2.0 # 平台长度 - gap_length = 1.0 # 空隙长度 - platform_thickness = 0.2 # 平台厚度 - current_pos = 0.0 # 起始位置 - # 交替添加平台和空隙 + platform_length, gap_length, platform_thickness = 2.0, 1.0, 0.2 + platform_width = self.corridor_width / 4 - 0.1 + gaps = [] + + current_pos = 0.0 while current_pos < self.corridor_length: - # 平台左半部分(y负方向) - gaps_xml += f""" - - """ - # 平台右半部分(y正方向) - gaps_xml += f""" - - """ - current_pos += platform_length + gap_length # 移动到下一个平台起点 - return gaps_xml + """) + current_pos += platform_length + gap_length + + return ''.join(gaps) + + def _build_actuator_indices(self): + """建立执行器名称到索引的映射,方便控制器按名称写入动作""" + indices = {} + for i in range(self.model.nu): + name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) + if name: + indices[name] = i + return indices + + def get_actuator_indices(self): + return self._actuator_indices.copy() def reset(self): - """重置环境到初始状态(使用机器人的默认姿态)""" + """重置环境到初始状态""" self.current_step = 0 - # 重置到默认姿态(或关键帧"stand",如果有的话) - self.sim.reset() - # 可选:设置初始姿态为"stand_on_left_leg"关键帧 - # self.sim.set_state_from_keyframe("stand_on_left_leg") - self.sim.forward() # 刷新物理状态 + mujoco.mj_resetData(self.model, self.data) + mujoco.mj_forward(self.model, self.data) return self._get_observation() def _get_observation(self): """获取观测(关节位置、速度、躯干位置)""" - qpos = self.sim.data.qpos.copy() # 关节位置 - qvel = self.sim.data.qvel.copy() # 关节速度 - torso_pos = self.sim.data.get_body_xpos("torso") # 躯干位置 + qpos = self.data.qpos.copy() + qvel = self.data.qvel.copy() + torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso") + torso_pos = self.data.xpos[torso_id].copy() return np.concatenate([qpos, qvel, torso_pos]) def _get_reward(self): """计算奖励:前进速度(沿走廊X轴)+ 空隙掉落惩罚""" - # 前进速度奖励(X方向速度) - torso_vel = self.sim.data.get_body_xvelp("torso")[0] # X方向线速度 - reward = torso_vel * 0.1 # 速度越大奖励越高 - - # 掉落惩罚(踩到空隙区域) - fall_penalty = 0.0 - for i in range(self.sim.data.ncon): - contact = self.sim.data.contact[i] - geom1_name = self.model.geom_names[contact.geom1] - geom2_name = self.model.geom_names[contact.geom2] - # 若接触的不是平台(即踩到空隙的地面),扣分 - if "platform" not in geom1_name and "platform" not in geom2_name: - fall_penalty -= 0.3 - return reward + fall_penalty + torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso") + + geom_vel = np.zeros(6) + mujoco.mj_objectVelocity( + self.model, + self.data, + mujoco.mjtObj.mjOBJ_BODY, + torso_id, + geom_vel, + 0 + ) + reward = geom_vel[0] * 0.1 + + for i in range(self.data.ncon): + contact = self.data.contact[i] + geom1_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, contact.geom1) + geom2_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, contact.geom2) + geom_names = [geom1_name, geom2_name] + if not any(name and "platform" in name for name in geom_names): + reward -= 0.3 + break + return reward def step(self, action): - """执行动作并推进环境(动作维度需与机器人执行器数量匹配)""" + """执行动作并推进环境""" self.current_step += 1 - # 应用动作到执行器(控制信号限制在[-1,1],你的机器人有20个执行器) - self.sim.data.ctrl[:] = np.clip(action, -1.0, 1.0) - # 执行多个物理步(匹配控制步长) + self.data.ctrl[:] = np.clip(action, -1.0, 1.0) + for _ in range(self.control_steps): - self.sim.step() - # 获取观测、奖励、完成状态 + mujoco.mj_step(self.model, self.data) + obs = self._get_observation() reward = self._get_reward() done = self.current_step >= self._max_episode_steps - # 若机器人跌倒(躯干高度过低),提前结束 - torso_z = self.sim.data.get_body_xpos("torso")[2] - if torso_z < 0.5: # 躯干高度低于0.5米视为跌倒 + + torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso") + torso_z = self.data.xpos[torso_id][2] + if torso_z < 0.5: done = True - reward -= 1.0 # 跌倒额外惩罚 + reward -= 1.0 return obs, reward, done - def render(self): + def render(self, viewer_handle=None): """渲染画面""" - self.viewer.render() - - -def random_policy(env): - """随机策略(生成符合动作空间的随机动作,你的机器人有20个执行器)""" - action_dim = env.model.nu # 动作维度 = 执行器数量(20) - return np.random.uniform(low=-1.0, high=1.0, size=action_dim) + if viewer_handle is not None: + with viewer_handle.lock(): + viewer_handle.sync() def main(): - # 创建带空隙的走廊环境(使用自定义人形机器人) - env = GapCorridorEnvironment( - corridor_length=100, - corridor_width=10, - robot_xml_path="/home/qiqi/mujoco_ros_ws/src/humanoid_motion/xml/humanoid_2.xml" # 确保路径正确 - ) + env = GapCorridorEnvironment(corridor_length=100, corridor_width=10) - # 创建键盘控制器 - action_dim = env.model.nu # 获取执行器数量 - keyboard_controller = KeyboardController(action_dim) + print("\n环境已初始化") + print(f"执行器数量: {env.model.nu}") + print(f"关节数量: {env.model.nq}") - print("\n环境已初始化,等待键盘输入...") - - # 运行环境(键盘控制模式) + controller = KeyboardController(env.model.nu, env.get_actuator_indices()) obs = env.reset() total_reward = 0.0 + print("\n启动MuJoCo交互式查看器...") + print("按 ESC 或关闭窗口退出程序") + try: - while not keyboard_controller.should_exit(): - # 检查是否需要重置环境 - if keyboard_controller.should_reset(): + viewer_handle = mujoco.viewer.launch_passive( + env.model, + env.data, + key_callback=controller.key_callback, + show_left_ui=True, + show_right_ui=True + ) + + print("\n查看器已启动,开始仿真循环...") + + step = 0 + while viewer_handle.is_running() and not controller.should_exit(): + if controller.should_reset(): obs = env.reset() total_reward = 0.0 - keyboard_controller.clear_reset_flag() - print("环境已重置") + step = 0 + # 重置移动状态 + controller.move_forward = False + controller.move_backward = False + controller.turn_left = False + controller.turn_right = False + controller.step_time = 0.0 + controller.clear_reset_flag() - # 获取键盘控制动作 - action = keyboard_controller.get_action() - - # 执行一步 + # 获取动作(传入控制步长以更新步行动作) + action = controller.get_action(dt=env.control_timestep) obs, reward, done = env.step(action) total_reward += reward - # 渲染画面 - env.render() + env.render(viewer_handle) + + if step % 100 == 0: + torso_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "torso") + torso_pos = env.data.xpos[torso_id] + print(f"Step {step}: 躯干位置 = {torso_pos}, 累计奖励 = {total_reward:.2f}") - # 检查是否结束(机器人跌倒或达到最大步数) if done: - print(f"Episode finished. Total reward: {total_reward:.2f}") - print("按 'r' 重置环境,或按 'q' 退出...") - # 等待用户操作 - while done and not keyboard_controller.should_exit() and not keyboard_controller.should_reset(): - time.sleep(0.1) - if keyboard_controller.should_reset(): - obs = env.reset() - total_reward = 0.0 - keyboard_controller.clear_reset_flag() + print(f"\nEpisode finished. Total reward: {total_reward:.2f}") + obs = env.reset() + total_reward = 0.0 + step = 0 - # 控制循环频率(避免过快) + step += 1 time.sleep(0.01) - except KeyboardInterrupt: - print("\n程序被中断") - finally: - keyboard_controller.exit_flag = True - print("程序已退出") + + viewer_handle.close() + print("\n查看器已关闭") + + except Exception as e: + print(f"无法启动查看器: {e}") + import traceback + traceback.print_exc() + + print("程序已退出") if __name__ == "__main__": main() \ No newline at end of file diff --git a/src/humanoid_simulation_environment_establish/main_humanoid.py b/src/humanoid_simulation_environment_establish/main_humanoid.py deleted file mode 100644 index 841b8e8552..0000000000 --- a/src/humanoid_simulation_environment_establish/main_humanoid.py +++ /dev/null @@ -1,166 +0,0 @@ -#!/usr/bin/env python3 -import rospy -import mujoco -from mujoco import viewer -import threading -import sys -import tty -import termios -import rospkg # 用于获取ROS功能包路径 - - -class HumanoidStandupController: - def __init__(self): - # 1. 初始化ROS节点与参数 - rospy.init_node("humanoid_standup_node", anonymous=True) - rospy.loginfo("人形机器人起身控制器启动...") - - # 从ROS参数服务器获取配置(默认值为launch文件中设置) - self.kp_gain = rospy.get_param("kp_gain", 5.0) # 比例增益 - self.model_path = rospy.get_param("model_path", "xml/humanoid.xml") # 模型相对路径 - - # 2. 解析模型绝对路径(通过rospkg获取功能包路径) - self.rospack = rospkg.RosPack() - try: - pkg_path = self.rospack.get_path("humanoid_motion") # 功能包路径 - self.full_model_path = f"{pkg_path}/{self.model_path}" # 拼接绝对路径 - rospy.loginfo(f"模型路径: {self.full_model_path}") - except rospkg.ResourceNotFound: - rospy.logerr("功能包'humanoid_motion'未找到,请确认包已安装") - sys.exit(1) - - # 3. 加载MuJoCo模型与初始化仿真数据 - try: - self.model = mujoco.MjModel.from_xml_path(self.full_model_path) # 模型对象 - self.data = mujoco.MjData(self.model) # 仿真数据(关节角度、力矩等) - self.target_data = mujoco.MjData(self.model) # 目标状态数据(用于存储目标姿势) - except Exception as e: - rospy.logerr(f"模型加载失败: {e}") - sys.exit(1) - - # 4. 设置初始姿势与目标姿势(通过关键帧,需模型xml中定义keyframe) - # 关键帧0: 初始姿势(如深蹲);关键帧1: 目标姿势(如站立) - mujoco.mj_resetDataKeyframe(self.model, self.data, 0) # 初始姿势 - mujoco.mj_resetDataKeyframe(self.model, self.target_data, 1) # 目标姿势 - rospy.loginfo("初始姿势与目标姿势加载完成") - - # 打印关键信息(用于调试:确认关节数和控制信号数) - rospy.loginfo(f"总关节数(njnt): {self.model.njnt} | 可控制信号数(ctrl_size): {len(self.data.ctrl)}") - - # 5. 键盘控制状态变量 - self.running = False # 是否执行起身控制 - self.exit_flag = False # 是否退出程序 - self.last_log_time = rospy.Time.now() # 用于控制日志输出频率 - - # 6. 启动键盘监听线程 - self.keyboard_thread = threading.Thread(target=self._keyboard_listener) - self.keyboard_thread.daemon = True # 主线程退出时自动结束 - self.keyboard_thread.start() - - # 7. 打印操作说明 - self._print_help() - - def _print_help(self): - """打印键盘控制指令说明""" - print("\n===== 键盘控制指令 =====") - print(" s: 开始/继续起身控制") - print(" p: 暂停起身控制(保持当前姿势)") - print(" +: 增大比例增益KP (+0.5)") - print(" -: 减小比例增益KP (-0.5)") - print(" q: 退出程序") - print("=======================") - - def _keyboard_listener(self): - """独立线程:监听键盘输入并更新控制状态""" - # 配置终端为非阻塞模式(无需按回车即可捕获按键) - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - try: - tty.setraw(sys.stdin.fileno()) - while not self.exit_flag: - key = sys.stdin.read(1) # 读取单个字符 - if key == 's': - self.running = True - rospy.loginfo("▶️ 开始起身控制") - elif key == 'p': - self.running = False - rospy.loginfo("⏸️ 已暂停起身控制") - elif key == '+': - self.kp_gain += 0.5 - rospy.loginfo(f"📈 KP增益调整为: {self.kp_gain:.1f}") - elif key == '-': - self.kp_gain = max(0.5, self.kp_gain - 0.5) # 限制最小增益为0.5 - rospy.loginfo(f"📉 KP增益调整为: {self.kp_gain:.1f}") - elif key == 'q': - self.exit_flag = True - rospy.loginfo("❌ 准备退出程序...") - finally: - # 恢复终端默认设置(避免程序退出后终端异常) - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - - def _log_state(self): - """定时输出机器人状态(避免日志刷屏)""" - current_time = rospy.Time.now() - if (current_time - self.last_log_time).to_sec() > 1.0: # 每1秒输出一次 - # 躯干高度(假设模型root关节的z坐标为躯干高度) - torso_height = self.data.qpos[2] # 需根据模型结构调整索引 - #rospy.loginfo(f"当前躯干高度: {torso_height:.2f}m | KP增益: {self.kp_gain:.1f}") - self.last_log_time = current_time - - def run(self): - """主控制循环:执行仿真与控制逻辑""" - # 启动MuJoCo可视化窗口(被动模式,由主循环驱动) - viewer = mujoco.viewer.launch_passive(self.model, self.data) - rospy.loginfo("可视化窗口启动,等待指令...") - - # 控制频率:200Hz(物理仿真步长与控制频率匹配) - rate = rospy.Rate(200) - - try: - while not rospy.is_shutdown() and not self.exit_flag: - # 1. 推进物理仿真(单步) - mujoco.mj_step(self.model, self.data) - - # 2. 若处于运行状态,执行起身控制(比例控制) - if self.running: - # 关键修改:按可控制信号数遍历(而非总关节数) - # 假设可控制关节对应原关节索引的[7, 7+ctrl_size),需与模型结构匹配 - ctrl_size = len(self.data.ctrl) - for ctrl_idx in range(ctrl_size): - joint_idx = 7 + ctrl_idx # 跳过前7个根关节,映射到可控制关节 - # 确保joint_idx不超出qpos的索引范围(双重保险) - if joint_idx >= len(self.data.qpos) or joint_idx >= len(self.target_data.qpos): - rospy.logwarn(f"关节索引{joint_idx}超出qpos范围,跳过该关节") - continue - # 计算关节角度误差(目标-当前) - error = self.target_data.qpos[joint_idx] - self.data.qpos[joint_idx] - # 比例控制:力矩 = KP * 误差(赋值给对应控制信号索引) - self.data.ctrl[ctrl_idx] = self.kp_gain * error - - # 3. 定时输出状态日志 - self._log_state() - - # 4. 刷新可视化窗口 - viewer.sync() - - # 5. 控制循环频率 - rate.sleep() - - except Exception as e: - rospy.logerr(f"主循环异常: {e}") - # 输出详细调试信息 - rospy.logerr(f"当前ctrl索引范围: 0~{len(self.data.ctrl)-1} | 映射后的关节索引: 7~{7+len(self.data.ctrl)-1}") - finally: - # 关闭可视化窗口 - viewer.close() - rospy.loginfo("程序已退出") - - -if __name__ == "__main__": - try: - controller = HumanoidStandupController() - controller.run() - except rospy.ROSInterruptException: - rospy.loginfo("ROS节点被中断") - except Exception as e: - rospy.logerr(f"程序异常: {e}") \ No newline at end of file From fefd95ce6b5a3fc0a8d7a3dea7d24689dfda7d45 Mon Sep 17 00:00:00 2001 From: qiqi678 <3490247469@qq.com> Date: Tue, 2 Dec 2025 22:11:48 +0800 Subject: [PATCH 40/44] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=97=A0=E9=87=8D?= =?UTF-8?q?=E5=8A=9B=E6=A8=A1=E5=BC=8F=E6=94=AF=E6=8C=81=E5=B9=B6=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E9=94=AE=E7=9B=98=E6=8E=A7=E5=88=B6=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main.py | 232 +++++++++++++----- 1 file changed, 174 insertions(+), 58 deletions(-) diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py index c9f374da6c..fae341266f 100644 --- a/src/humanoid_simulation_environment_establish/main.py +++ b/src/humanoid_simulation_environment_establish/main.py @@ -92,25 +92,25 @@ def _create_walking_action(self, forward=True, turn_direction=0): counter_lift = np.maximum(0.0, np.sin(phase + np.pi)) direction = 1 if forward else -1 - torso_pitch = 0.25 * direction - self._set_action(action, "abdomen_y", torso_pitch) + # 躯干控制 + self._set_action(action, "abdomen_y", 0.25 * direction) self._set_action(action, "abdomen_x", 0.15 * turn_direction) - # 右腿 + # 右腿(减小抬腿幅度,防止在无重力环境下飞得太高) self._set_action(action, "hip_x_right", 0.6 * direction * swing) - self._set_action(action, "hip_y_right", -0.4 * lift) + self._set_action(action, "hip_y_right", -0.15 * lift) self._set_action(action, "knee_right", 0.7 * (0.5 - 0.5 * np.cos(phase))) - self._set_action(action, "ankle_y_right", -0.3 * lift) + self._set_action(action, "ankle_y_right", -0.1 * lift) self._set_action(action, "ankle_x_right", 0.2 * swing) # 左腿(相位相反) self._set_action(action, "hip_x_left", -0.6 * direction * counter_swing) - self._set_action(action, "hip_y_left", -0.4 * counter_lift) + self._set_action(action, "hip_y_left", -0.15 * counter_lift) self._set_action(action, "knee_left", 0.7 * (0.5 - 0.5 * np.cos(phase + np.pi))) - self._set_action(action, "ankle_y_left", -0.3 * counter_lift) + self._set_action(action, "ankle_y_left", -0.1 * counter_lift) self._set_action(action, "ankle_x_left", -0.2 * counter_swing) - # 转向控制通过髋关节外展实现 + # 转向控制 if turn_direction != 0: turn_strength = 0.5 * turn_direction self._set_action(action, "hip_z_right", turn_strength) @@ -118,6 +118,24 @@ def _create_walking_action(self, forward=True, turn_direction=0): return action + def _create_turning_only_action(self, turn_direction): + """创建仅转向动作(不产生腿部摆动,只在原地转向)""" + action = np.zeros(self.action_dim) + + if not self.actuator_indices: + return action + + # 只设置转向相关的动作,不产生腿部摆动 + # 转向控制通过髋关节外展实现 + turn_strength = 0.3 * turn_direction # 减小转向强度 + self._set_action(action, "hip_z_right", turn_strength) + self._set_action(action, "hip_z_left", -turn_strength) + + # 可以添加轻微的躯干倾斜来辅助转向 + self._set_action(action, "abdomen_x", 0.1 * turn_direction) + + return action + def _process_key(self, key): """处理按键输入""" if isinstance(key, str) and key.startswith('\x1b['): @@ -126,42 +144,25 @@ def _process_key(self, key): key_char = key if isinstance(key, str) and len(key) == 1 else None # 处理移动指令(切换模式:每次按键切换状态) - if (key_char and key_char == 'w') or key == '\x1b[A': - if self.move_forward: - self.move_forward = False - print("[键盘] 停止前进") - else: - self.move_forward = True - self.move_backward = False - print("[键盘] 前进") - return - elif (key_char and key_char == 's') or key == '\x1b[B': - if self.move_backward: - self.move_backward = False - print("[键盘] 停止后退") - else: - self.move_backward = True - self.move_forward = False - print("[键盘] 后退") - return - elif (key_char and key_char == 'a') or key == '\x1b[D': - if self.turn_left: - self.turn_left = False - print("[键盘] 停止左转") - else: - self.turn_left = True - self.turn_right = False - print("[键盘] 左转") - return - elif (key_char and key_char == 'd') or key == '\x1b[C': - if self.turn_right: - self.turn_right = False - print("[键盘] 停止右转") - else: - self.turn_right = True - self.turn_left = False - print("[键盘] 右转") - return + move_commands = { + ('w', '\x1b[A'): ('move_forward', 'move_backward', '前进', '停止前进'), + ('s', '\x1b[B'): ('move_backward', 'move_forward', '后退', '停止后退'), + ('a', '\x1b[D'): ('turn_left', 'turn_right', '左转', '停止左转'), + ('d', '\x1b[C'): ('turn_right', 'turn_left', '右转', '停止右转'), + } + + for (key1, key2), (attr, opposite_attr, start_msg, stop_msg) in move_commands.items(): + if (key_char == key1) or (key == key2): + current_state = getattr(self, attr) + if current_state: + setattr(self, attr, False) + print(f"[键盘] {stop_msg}") + else: + setattr(self, attr, True) + if hasattr(self, opposite_attr): + setattr(self, opposite_attr, False) + print(f"[键盘] {start_msg}") + return if key == ' ': self.paused = not self.paused @@ -210,8 +211,9 @@ def get_action(self, dt=0.03): turn_dir = -1 self.current_action = self._create_walking_action(forward=False, turn_direction=turn_dir) elif self.turn_left or self.turn_right: + # 只转向时,不产生腿部摆动,只在原地转向 turn_dir = -1 if self.turn_left else 1 - self.current_action = self._create_walking_action(forward=False, turn_direction=turn_dir) + self.current_action = self._create_turning_only_action(turn_dir) else: # 没有移动指令时,返回零动作或保持平衡的微小动作 self.current_action = np.zeros(self.action_dim) @@ -233,24 +235,30 @@ def clear_reset_flag(self): class GapCorridorEnvironment: """基于mujoco的带空隙走廊环境(使用自定义人形机器人模型)""" - def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path=None): + def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path=None, use_gravity=True): """ Args: corridor_length: 走廊总长度 corridor_width: 走廊宽度 robot_xml_path: 自定义人形机器人XML文件路径 + use_gravity: 是否启用重力(False 表示无重力) """ self.corridor_length = corridor_length self.corridor_width = corridor_width - if robot_xml_path is None: - default_path = Path(__file__).resolve().parent / "model" / "humanoid" / "humanoid.xml" - else: - default_path = Path(robot_xml_path) - if not default_path.is_file(): - raise FileNotFoundError(f"无法找到机器人XML文件: {default_path}") - self.robot_xml_path = default_path + self.use_gravity = use_gravity + # if robot_xml_path is None: + # default_path = Path(__file__).resolve().parent / "model" / "humanoid" / "humanoid.xml" + # else: + # default_path = Path(robot_xml_path) + # if not default_path.is_file(): + # raise FileNotFoundError(f"无法找到机器人XML文件: {default_path}") + # self.robot_xml_path = default_path + self.robot_xml_path = "humanoid.xml" xml_string = self._build_model() self.model = mujoco.MjModel.from_xml_string(xml_string) + # 保险起见,在模型创建后再次根据标志位设置重力(即使 XML 中已经设置) + if not self.use_gravity: + self.model.opt.gravity[:] = 0.0 self.data = mujoco.MjData(self.model) self.timestep = self.model.opt.timestep self.control_timestep = 0.03 @@ -258,6 +266,16 @@ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path=None): self._max_episode_steps = 30 / self.control_timestep self.current_step = 0 self._actuator_indices = self._build_actuator_indices() + + # 无重力模式:只固定Z高度,允许XY平移和姿态变化 + if not self.use_gravity: + self._initial_z_height = None + self._root_joint_qpos_start = None + self._root_joint_qvel_start = None + self._root_body_id = None + self._max_xy_velocity = 2.0 # 最大XY速度 (m/s) + self._xy_damping = 0.995 # XY速度阻尼系数(减小阻尼,允许更大移动) + self._find_root_joint_indices() def _parse_robot_xml(self): """解析自定义机器人XML,提取需要的节点(身体、执行器、肌腱等)""" @@ -265,7 +283,7 @@ def _parse_robot_xml(self): root = tree.getroot() robot_body = root.find("worldbody").find("body[@name='torso']") - robot_body.set("pos", "1.0 0.5 2") + robot_body.set("pos", "1.0 0.5 1.5") # 提取XML节点并转换为字符串 single_nodes = ["actuator", "tendon", "contact", "asset", "visual", "keyframe", "statistic"] @@ -283,11 +301,14 @@ def _build_model(self): # 解析自定义机器人XML robot_parts = self._parse_robot_xml() + # 根据是否使用重力设置 gravity 参数 + gravity_z = -9.81 if self.use_gravity else 0.0 + # 基础XML结构(走廊环境+机器人) xml = f""" - -