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/45] 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/45] 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/45] =?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/45] 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/45] =?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/45] =?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 @@
-
-
diff --git a/src/humanoid_motion control/main.py b/src/humanoid_motion control/main.py
new file mode 100644
index 0000000000..c9765e2cae
--- /dev/null
+++ b/src/humanoid_motion control/main.py
@@ -0,0 +1,57 @@
+# 标准库
+# 导入时间模块,用于控制模拟帧率
+import time
+
+# 第三方库
+# 导入MuJoCo核心库,提供物理引擎功能
+import mujoco
+# 从MuJoCo导入可视化工具,用于实时显示模拟过程
+from mujoco import viewer
+
+def main():
+ """
+ 主函数:加载人形机器人模型并运行物理模拟
+
+ 流程:
+ 1. 加载XML格式的人形机器人模型文件
+ 2. 初始化模拟数据结构
+ 3. 设置初始姿势为深蹲姿态
+ 4. 启动可视化窗口
+ 5. 无限循环运行模拟(Ctrl+C退出)
+ 6. 输出关键模拟数据并更新可视化
+ """
+ # 加载MJCF模型文件
+ try:
+ model = mujoco.MjModel.from_xml_path("src\humanoid_motion control\humanoid.xml")
+ except Exception as e:
+ print(f"模型加载失败: {e}")
+ return
+
+ # 创建与模型对应的动态数据结构
+ data = mujoco.MjData(model)
+
+ # 设置初始姿势为深蹲姿态
+ mujoco.mj_resetDataKeyframe(model, data, 1)
+
+ # 启动被动式可视化窗口
+ with viewer.launch_passive(model, data) as v:
+ try:
+ # 无限循环
+ while True:
+ # 推进模拟一步
+ mujoco.mj_step(model, data)
+
+ # 打印机器人关键位置信息
+ print(f"时间: {data.time:.2f}, "
+ f"躯干位置: ({data.qpos[0]:.2f}, {data.qpos[1]:.2f}, {data.qpos[2]:.2f})")
+
+ # 更新可视化窗口
+ v.sync()
+
+ # 控制可视化帧率
+ time.sleep(0.005)
+ except KeyboardInterrupt:
+ print("/n正在退出模拟....")
+# 程序入口点
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/mujoco01/main.py b/src/mujoco01/main.py
deleted file mode 100644
index c8e4ea7a76..0000000000
--- a/src/mujoco01/main.py
+++ /dev/null
@@ -1,66 +0,0 @@
-# 标准库
-# 导入时间模块,用于控制模拟帧率
-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 63997017eced28d26165e9a7a6875f689e2e2c98 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 22 Sep 2025 09:53:01 +0800
Subject: [PATCH 07/45] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=B8=8D=E5=BF=85?=
=?UTF-8?q?=E8=A6=81=E7=9A=84=E6=97=A5=E5=BF=97=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
README.md | 3 ---
src/MUJOCO_LOG.TXT | 6 ------
src/humanoid_motion control/README.md | 4 ++--
3 files changed, 2 insertions(+), 11 deletions(-)
delete mode 100644 src/MUJOCO_LOG.TXT
diff --git a/README.md b/README.md
index cac53e74c4..dcee5566b7 100644
--- a/README.md
+++ b/README.md
@@ -1,8 +1,5 @@
# 神经网络实现代理
-
-
-
实现基于Carla的车辆、行人的感知、规划、控制。
## 环境配置
diff --git a/src/MUJOCO_LOG.TXT b/src/MUJOCO_LOG.TXT
deleted file mode 100644
index 1cc12eb1d7..0000000000
--- a/src/MUJOCO_LOG.TXT
+++ /dev/null
@@ -1,6 +0,0 @@
-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/humanoid_motion control/README.md b/src/humanoid_motion control/README.md
index 5c950c04e9..7780ec5e27 100644
--- a/src/humanoid_motion control/README.md
+++ b/src/humanoid_motion control/README.md
@@ -25,7 +25,7 @@ humanoid_mujoco_py/
- 设置初始姿势为「深蹲(squat)」(可修改代码切换其他姿势)
- 启动可视化窗口,实时渲染机器人运动
- 在终端输出实时仿真时间与躯干位置(格式:`时间: X.XX, 躯干位置: (X.XX, X.XX, X.XX)`)
- - 模拟持续20秒(1200步,每步0.005秒)后自动结束
+ - 无限循环启动模拟
## 交互操作
@@ -49,7 +49,7 @@ humanoid_mujoco_py/
- 数据初始化:通过`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)`控制帧率
+ - 仿真循环:循环中,通过`mujoco.mj_step()`执行物理计算,`viewer.sync()`更新视图,`time.sleep(0.005)`控制帧率
## 扩展方向
From ed8cdcc07e93a8c3a6fc1e6786cb2cb6f5e951c8 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Fri, 26 Sep 2025 11:02:13 +0800
Subject: [PATCH 08/45] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E4=BB=BF=E7=9C=9F?=
=?UTF-8?q?=E6=9C=BA=E5=99=A8=E4=BA=BA=E7=9A=84=E5=BC=B9=E8=B7=B3=E5=8A=9F?=
=?UTF-8?q?=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/README.md | 89 ++++++++
src/humanoid_motion_control/humanoid.xml | 252 +++++++++++++++++++++++
src/humanoid_motion_control/main.py | 44 ++++
3 files changed, 385 insertions(+)
create mode 100644 src/humanoid_motion_control/README.md
create mode 100644 src/humanoid_motion_control/humanoid.xml
create mode 100644 src/humanoid_motion_control/main.py
diff --git a/src/humanoid_motion_control/README.md b/src/humanoid_motion_control/README.md
new file mode 100644
index 0000000000..36abb2e2ca
--- /dev/null
+++ b/src/humanoid_motion_control/README.md
@@ -0,0 +1,89 @@
+
+
+# 软体人形机器人弹跳动作仿真(基于MuJoCo)
+
+## 项目概述
+本项目是一个基于**MuJoCo物理引擎**的软体人形机器人物理仿真示例。实际运行中,机器人会呈现**原地短暂跳起后回落蹲下**的循环动作——这是由于软体机器人的柔性特性(弹性、形变)与控制参数共同作用的结果:关节电机驱动力过强时,软体结构会储存弹性势能,释放时形成向上的弹跳力,随后在重力作用下回落至蹲下姿势,形成往复运动。
+
+
+## 环境准备
+### 依赖库
+项目依赖以下Python库,需提前安装:
+```bash
+pip install mujoco mujoco-viewer
+```
+
+
+## 项目结构
+| 文件名 | 功能描述 |
+| -------------- | ------------------------------------------------------------ |
+| `humanoid.xml` | MJCF格式模型文件,定义软体机器人的结构、关节参数(阻尼、弹性)、关键帧姿势(深蹲、单腿站立)及电机执行器 |
+| `main.py` | 控制主程序,实现模型加载、比例控制逻辑、仿真循环及可视化,驱动机器人运动 |
+
+
+## 核心文件说明
+### 1. `humanoid.xml`(软体机器人模型)
+模型的关键特性直接影响弹跳效果:
+- **软体物理属性**:关节设置了较低阻尼(`damping`)和一定弹性,允许较大形变(为弹跳提供储能基础);
+- **关键帧姿势**:
+ - 索引`0`:`squat`(深蹲姿势)—— 初始状态,关节弯曲储存势能;
+ - 索引`1`:`stand_on_left_leg`(单左腿站立)—— 目标姿势,关节伸展;
+- **电机参数**:`ctrlrange="-1 1"`限制控制信号范围,但软体弹性会放大实际驱动力,导致动作过冲。
+
+
+### 2. `main.py`(控制逻辑)
+程序通过**比例控制**驱动关节运动,是弹跳动作的直接诱因:
+```python
+# 核心控制逻辑:计算关节误差,生成驱动信号
+qpos_error = target_qpos[7:] - data.qpos[7:] # 目标与当前关节位置差
+data.ctrl[:] = Kp * qpos_error # 比例增益放大误差,生成电机信号
+```
+- **参数影响**:代码中`Kp=5.0`的比例增益,对软体机器人而言可能过大——驱动力超过维持姿势所需,导致关节快速伸展,软体结构弹性形变加剧,最终形成“跳起”动作;
+- **循环机制**:跳起后重力主导回落,关节位置偏离目标,控制信号再次触发驱动力,形成“跳起→蹲下→再跳起”的循环。
+
+
+## 实际运行效果解析
+1. **初始状态**:机器人从深蹲姿势(`squat`)开始,关节弯曲,软体结构处于压缩状态;
+2. **驱动阶段**:控制信号驱动关节向站立姿势(`stand_on_left_leg`)伸展,软体材料因快速形变储存弹性势能;
+3. **弹跳阶段**:弹性势能释放,机器人获得向上的力,短暂离开初始位置(跳起);
+4. **回落阶段**:重力作用下,机器人下落,关节在冲击力和控制信号调整下重新弯曲,回到接近深蹲的姿势;
+5. **循环往复**:关节位置误差再次增大,控制信号重复驱动,形成周期性弹跳动作。
+
+
+## 使用步骤
+1. 确认`humanoid.xml`与`main.py`在同一目录(默认路径:`src/humanoid_motion_control/`);
+2. 终端执行命令启动仿真:
+ ```bash
+ python main.py
+ ```
+3. 可视化窗口将显示机器人的弹跳动作,按`Ctrl+C`终止仿真。
+
+
+## 参数调整指南(优化动作)
+若希望减弱弹跳、实现更平稳的蹲站动作,可调整以下参数:
+
+| 参数名 | 调整方向 | 效果说明 |
+| -------- | ------------------------------------- | ------------------------------------------------------------ |
+| `Kp` | 减小至`1.0~3.0` | 降低比例增益,减弱电机驱动力,避免软体结构过度形变和弹性势能积累 |
+| 关节阻尼 | 在`humanoid.xml`中增大关节`damping`值 | 增加软体关节的阻尼,消耗部分弹性势能,减少弹跳幅度 |
+| 仿真步长 | 减小`time.sleep(0.005)`至`0.002` | 提高仿真精度,使控制信号更细腻地响应关节位置变化 |
+
+
+## 常见问题
+1. **弹跳幅度过大甚至翻倒**
+ → 解决方案:将`Kp`降至`1.0`,同时在`humanoid.xml`中找到关节节点(如``),增大`damping`参数(如从`1.0`改为`5.0`)。
+
+2. **动作卡顿或延迟**
+ → 解决方案:检查`time.sleep()`的数值,若电脑性能较好,可减小至`0.001`以提升流畅度。
+
+3. **模型加载失败**
+ → 解决方案:确认`main.py`中模型路径与`humanoid.xml`实际位置一致,例如修改为:
+ ```python
+ model = mujoco.MjModel.from_xml_path("humanoid.xml") # 若文件在当前目录
+ ```
+
+
+## 参考资料
+- [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/):了解软体材料参数(弹性、阻尼)的配置方法
+- [比例控制算法详解](https://en.wikipedia.org/wiki/Proportional_control):理解`Kp`对控制效果的影响
+
diff --git a/src/humanoid_motion_control/humanoid.xml b/src/humanoid_motion_control/humanoid.xml
new file mode 100644
index 0000000000..38b07c0342
--- /dev/null
+++ b/src/humanoid_motion_control/humanoid.xml
@@ -0,0 +1,252 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/humanoid_motion_control/main.py b/src/humanoid_motion_control/main.py
new file mode 100644
index 0000000000..de42eac3b6
--- /dev/null
+++ b/src/humanoid_motion_control/main.py
@@ -0,0 +1,44 @@
+# 标准库
+import time
+# 第三方库
+import mujoco
+from mujoco import viewer
+
+def main():
+ try:
+ model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
+ except Exception as e:
+ print(f"模型加载失败: {e}")
+ return
+
+ data = mujoco.MjData(model)
+ # 临时数据用于获取目标关键帧(站立姿势,索引1)
+ target_data = mujoco.MjData(model)
+ mujoco.mj_resetDataKeyframe(model, target_data, 1)
+ target_qpos = target_data.qpos.copy()
+
+ # 初始姿势设为深蹲(索引0),从蹲下开始站起
+ mujoco.mj_resetDataKeyframe(model, data, 0)
+
+ # 控制参数(软体机器人需较小增益,避免震荡)
+ Kp = 5.0 # 比例增益(根据软体特性调小)
+
+ with viewer.launch_passive(model, data) as v:
+ try:
+ while True:
+ mujoco.mj_step(model, data)
+
+ # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
+ qpos_error = target_qpos[7:] - data.qpos[7:]
+ # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
+ data.ctrl[:] = Kp * qpos_error
+
+ print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
+
+ v.sync()
+ time.sleep(0.005)
+ except KeyboardInterrupt:
+ print("\n正在退出模拟....")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From 556131f23ab7bbcf93f1c28e99705f79d19b41e3 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Fri, 26 Sep 2025 14:20:48 +0800
Subject: [PATCH 09/45] =?UTF-8?q?=E5=88=A0=E9=99=A4=E9=87=8D=E5=A4=8D?=
=?UTF-8?q?=E7=9A=84=E5=B8=A6=E7=A9=BA=E6=A0=BC=E6=96=87=E4=BB=B6=E5=A4=B9?=
=?UTF-8?q?=20humanoid=5Fmotion=20control?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion control/README.md | 78 -------
src/humanoid_motion control/humanoid.xml | 252 -----------------------
src/humanoid_motion control/main.py | 57 -----
3 files changed, 387 deletions(-)
delete mode 100644 src/humanoid_motion control/README.md
delete mode 100644 src/humanoid_motion control/humanoid.xml
delete mode 100644 src/humanoid_motion control/main.py
diff --git a/src/humanoid_motion control/README.md b/src/humanoid_motion control/README.md
deleted file mode 100644
index 7780ec5e27..0000000000
--- a/src/humanoid_motion control/README.md
+++ /dev/null
@@ -1,78 +0,0 @@
-# 人形机器人
-# 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)`)
- - 无限循环启动模拟
-
-## 交互操作
-
-在可视化窗口中,可以使用以下鼠标操作调整视角或交互:
-- 左键拖动:旋转场景视角
-- 右键拖动:平移场景位置
-- 滚轮:缩放视图(拉近/拉远)
-- 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(手动控制仿真步长与视图更新)
- - 仿真循环:循环中,通过`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/humanoid_motion control/humanoid.xml b/src/humanoid_motion control/humanoid.xml
deleted file mode 100644
index 4383f172c0..0000000000
--- a/src/humanoid_motion control/humanoid.xml
+++ /dev/null
@@ -1,252 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/humanoid_motion control/main.py b/src/humanoid_motion control/main.py
deleted file mode 100644
index c9765e2cae..0000000000
--- a/src/humanoid_motion control/main.py
+++ /dev/null
@@ -1,57 +0,0 @@
-# 标准库
-# 导入时间模块,用于控制模拟帧率
-import time
-
-# 第三方库
-# 导入MuJoCo核心库,提供物理引擎功能
-import mujoco
-# 从MuJoCo导入可视化工具,用于实时显示模拟过程
-from mujoco import viewer
-
-def main():
- """
- 主函数:加载人形机器人模型并运行物理模拟
-
- 流程:
- 1. 加载XML格式的人形机器人模型文件
- 2. 初始化模拟数据结构
- 3. 设置初始姿势为深蹲姿态
- 4. 启动可视化窗口
- 5. 无限循环运行模拟(Ctrl+C退出)
- 6. 输出关键模拟数据并更新可视化
- """
- # 加载MJCF模型文件
- try:
- model = mujoco.MjModel.from_xml_path("src\humanoid_motion control\humanoid.xml")
- except Exception as e:
- print(f"模型加载失败: {e}")
- return
-
- # 创建与模型对应的动态数据结构
- data = mujoco.MjData(model)
-
- # 设置初始姿势为深蹲姿态
- mujoco.mj_resetDataKeyframe(model, data, 1)
-
- # 启动被动式可视化窗口
- with viewer.launch_passive(model, data) as v:
- try:
- # 无限循环
- while True:
- # 推进模拟一步
- mujoco.mj_step(model, data)
-
- # 打印机器人关键位置信息
- print(f"时间: {data.time:.2f}, "
- f"躯干位置: ({data.qpos[0]:.2f}, {data.qpos[1]:.2f}, {data.qpos[2]:.2f})")
-
- # 更新可视化窗口
- v.sync()
-
- # 控制可视化帧率
- time.sleep(0.005)
- except KeyboardInterrupt:
- print("/n正在退出模拟....")
-# 程序入口点
-if __name__ == "__main__":
- main()
\ No newline at end of file
From 9e558fea7708bd144ad0e2ff0e6d9e5a80a2cf29 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 29 Sep 2025 10:48:09 +0800
Subject: [PATCH 10/45] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=B8=80=E4=B8=AAPytho?=
=?UTF-8?q?n=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/README.md | 193 ++++++++++--------
.../{main.py => main_humanoid.py} | 4 +-
src/humanoid_motion_control/run_walls.py | 61 ++++++
3 files changed, 169 insertions(+), 89 deletions(-)
rename src/humanoid_motion_control/{main.py => main_humanoid.py} (98%)
create mode 100644 src/humanoid_motion_control/run_walls.py
diff --git a/src/humanoid_motion_control/README.md b/src/humanoid_motion_control/README.md
index 36abb2e2ca..7eee403936 100644
--- a/src/humanoid_motion_control/README.md
+++ b/src/humanoid_motion_control/README.md
@@ -1,89 +1,108 @@
+-
+
+ # 人形机器人走廊奔跑仿真(基于dm_control)
+
+ ## 项目概述
+ 本项目核心为基于**dm_control框架**构建的人形机器人走廊奔跑仿真环境。dm_control是DeepMind基于MuJoCo物理引擎开发的高级仿真工具,简化了强化学习环境的搭建、任务定义与可视化流程。本项目中,CMU人形机器人需在带随机墙壁的走廊中以目标速度奔跑,展示了dm_control在复杂运动任务仿真中的应用。
+
+ ## 环境准备
+ ### 依赖库
+ 需安装以下核心依赖(含dm_control及其底层依赖):
+ ```bash
+ pip install mujoco dm-control absl-py
+ ```
+ - `dm_control`:提供环境构建、任务定义和交互可视化功能
+ - `mujoco`:dm_control依赖的物理引擎核心
+ - `absl-py`:用于命令行参数处理
+
+ ## 项目结构
+ | 文件名 | 功能描述 |
+ | ------------------ | ------------------------------------------------------------ |
+ | `run_walls.py` | **核心文件**:基于dm_control构建带墙壁的走廊环境,实现CMU人形机器人奔跑任务 |
+ | `humanoid.xml` | MJCF格式模型文件,定义机器人结构(辅助文件) |
+ | `main_humanoid.py` | 软体机器人弹跳仿真程序 |
+
+ ## 核心功能:走廊奔跑环境(run_walls.py)
+ ### 1. dm_control框架组件应用
+ ```python
+ # dm_control核心模块使用
+ from dm_control import composer # 环境组合框架
+ from dm_control.locomotion.arenas import corridors # 走廊场景
+ from dm_control.locomotion.tasks import corridors # 走廊任务
+ from dm_control.locomotion.walkers import cmu_humanoid # 机器人模型
+ from dm_control import viewer # 可视化工具
+ ```
+
+ ### 2. 环境构建流程
+ #### (1)机器人配置
+ 创建位置控制的CMU人形机器人,启用第一视角相机观测:
+ ```python
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)}
+ )
+ ```
+
+ #### (2)走廊场景设计
+ 通过`WallsCorridor`类构建带随机墙壁的动态环境:
+ ```python
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4., # 墙壁间隙宽度(机器人需穿过的空间)
+ wall_width=distributions.Uniform(1, 7), # 墙壁宽度随机化(1-7单位)
+ wall_height=3.0, # 防止翻越的墙壁高度
+ corridor_width=10, # 走廊总宽度
+ corridor_length=100, # 走廊总长度
+ include_initial_padding=False # 无初始空白区域
+ )
+ ```
+
+ #### (3)任务定义
+ 通过`RunThroughCorridor`类定义奔跑任务规则:
+ ```python
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0), # 初始位置
+ target_velocity=3.0, # 目标速度(影响奖励计算)
+ physics_timestep=0.005, # 物理仿真步长
+ control_timestep=0.03 # 控制决策步长(≈33Hz)
+ )
+ ```
+
+ #### (4)环境封装
+ 组合场景与任务,生成强化学习可用环境:
+ ```python
+ environment = composer.Environment(
+ time_limit=30, # 单次仿真时长30秒
+ task=task,
+ strip_singleton_obs_buffer_dim=True # 简化观测数据结构
+ )
+ ```
+
+ ## 使用方法
+ 1. 运行走廊奔跑仿真:
+ ```bash
+ python run_walls.py
+ ```
+ 2. 交互操作:
+ - 可视化窗口展示机器人第一视角和全局视角
+ - 按`Ctrl+C`终止仿真
+
+ ## dm_control核心特性体现
+ 1. **模块化设计**:通过`composer`模块分离机器人(walker)、场景(arena)和任务(task),便于独立修改
+ 2. **随机性支持**:使用`distributions`生成随机墙壁宽度,增强环境多样性
+ 3. **灵活观测配置**:通过`observable_options`轻松启用第一视角相机等观测项
+ 4. **高效可视化**:`viewer`模块提供实时交互界面,支持视角切换与状态查看
+
+ ## 参数调整指南
+ | 参数 | 调整范围 | 效果说明 |
+ | ------------------ | -------- | -------------------------------- |
+ | `wall_gap` | 2.0~6.0 | 减小值增加穿过难度 |
+ | `target_velocity` | 1.0~5.0 | 提高值要求机器人跑得更快 |
+ | `control_timestep` | 0.01~0.1 | 减小值提高控制精度(增加计算量) |
+
+ ## 参考资料
+ - [dm_control官方文档](https://github.com/deepmind/dm_control)
+ - [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/)
+ - [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
-# 软体人形机器人弹跳动作仿真(基于MuJoCo)
-
-## 项目概述
-本项目是一个基于**MuJoCo物理引擎**的软体人形机器人物理仿真示例。实际运行中,机器人会呈现**原地短暂跳起后回落蹲下**的循环动作——这是由于软体机器人的柔性特性(弹性、形变)与控制参数共同作用的结果:关节电机驱动力过强时,软体结构会储存弹性势能,释放时形成向上的弹跳力,随后在重力作用下回落至蹲下姿势,形成往复运动。
-
-
-## 环境准备
-### 依赖库
-项目依赖以下Python库,需提前安装:
-```bash
-pip install mujoco mujoco-viewer
-```
-
-
-## 项目结构
-| 文件名 | 功能描述 |
-| -------------- | ------------------------------------------------------------ |
-| `humanoid.xml` | MJCF格式模型文件,定义软体机器人的结构、关节参数(阻尼、弹性)、关键帧姿势(深蹲、单腿站立)及电机执行器 |
-| `main.py` | 控制主程序,实现模型加载、比例控制逻辑、仿真循环及可视化,驱动机器人运动 |
-
-
-## 核心文件说明
-### 1. `humanoid.xml`(软体机器人模型)
-模型的关键特性直接影响弹跳效果:
-- **软体物理属性**:关节设置了较低阻尼(`damping`)和一定弹性,允许较大形变(为弹跳提供储能基础);
-- **关键帧姿势**:
- - 索引`0`:`squat`(深蹲姿势)—— 初始状态,关节弯曲储存势能;
- - 索引`1`:`stand_on_left_leg`(单左腿站立)—— 目标姿势,关节伸展;
-- **电机参数**:`ctrlrange="-1 1"`限制控制信号范围,但软体弹性会放大实际驱动力,导致动作过冲。
-
-
-### 2. `main.py`(控制逻辑)
-程序通过**比例控制**驱动关节运动,是弹跳动作的直接诱因:
-```python
-# 核心控制逻辑:计算关节误差,生成驱动信号
-qpos_error = target_qpos[7:] - data.qpos[7:] # 目标与当前关节位置差
-data.ctrl[:] = Kp * qpos_error # 比例增益放大误差,生成电机信号
-```
-- **参数影响**:代码中`Kp=5.0`的比例增益,对软体机器人而言可能过大——驱动力超过维持姿势所需,导致关节快速伸展,软体结构弹性形变加剧,最终形成“跳起”动作;
-- **循环机制**:跳起后重力主导回落,关节位置偏离目标,控制信号再次触发驱动力,形成“跳起→蹲下→再跳起”的循环。
-
-
-## 实际运行效果解析
-1. **初始状态**:机器人从深蹲姿势(`squat`)开始,关节弯曲,软体结构处于压缩状态;
-2. **驱动阶段**:控制信号驱动关节向站立姿势(`stand_on_left_leg`)伸展,软体材料因快速形变储存弹性势能;
-3. **弹跳阶段**:弹性势能释放,机器人获得向上的力,短暂离开初始位置(跳起);
-4. **回落阶段**:重力作用下,机器人下落,关节在冲击力和控制信号调整下重新弯曲,回到接近深蹲的姿势;
-5. **循环往复**:关节位置误差再次增大,控制信号重复驱动,形成周期性弹跳动作。
-
-
-## 使用步骤
-1. 确认`humanoid.xml`与`main.py`在同一目录(默认路径:`src/humanoid_motion_control/`);
-2. 终端执行命令启动仿真:
- ```bash
- python main.py
- ```
-3. 可视化窗口将显示机器人的弹跳动作,按`Ctrl+C`终止仿真。
-
-
-## 参数调整指南(优化动作)
-若希望减弱弹跳、实现更平稳的蹲站动作,可调整以下参数:
-
-| 参数名 | 调整方向 | 效果说明 |
-| -------- | ------------------------------------- | ------------------------------------------------------------ |
-| `Kp` | 减小至`1.0~3.0` | 降低比例增益,减弱电机驱动力,避免软体结构过度形变和弹性势能积累 |
-| 关节阻尼 | 在`humanoid.xml`中增大关节`damping`值 | 增加软体关节的阻尼,消耗部分弹性势能,减少弹跳幅度 |
-| 仿真步长 | 减小`time.sleep(0.005)`至`0.002` | 提高仿真精度,使控制信号更细腻地响应关节位置变化 |
-
-
-## 常见问题
-1. **弹跳幅度过大甚至翻倒**
- → 解决方案:将`Kp`降至`1.0`,同时在`humanoid.xml`中找到关节节点(如``),增大`damping`参数(如从`1.0`改为`5.0`)。
-
-2. **动作卡顿或延迟**
- → 解决方案:检查`time.sleep()`的数值,若电脑性能较好,可减小至`0.001`以提升流畅度。
-
-3. **模型加载失败**
- → 解决方案:确认`main.py`中模型路径与`humanoid.xml`实际位置一致,例如修改为:
- ```python
- model = mujoco.MjModel.from_xml_path("humanoid.xml") # 若文件在当前目录
- ```
-
-
-## 参考资料
-- [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/):了解软体材料参数(弹性、阻尼)的配置方法
-- [比例控制算法详解](https://en.wikipedia.org/wiki/Proportional_control):理解`Kp`对控制效果的影响
-
diff --git a/src/humanoid_motion_control/main.py b/src/humanoid_motion_control/main_humanoid.py
similarity index 98%
rename from src/humanoid_motion_control/main.py
rename to src/humanoid_motion_control/main_humanoid.py
index de42eac3b6..2982d10461 100644
--- a/src/humanoid_motion_control/main.py
+++ b/src/humanoid_motion_control/main_humanoid.py
@@ -17,7 +17,7 @@ def main():
mujoco.mj_resetDataKeyframe(model, target_data, 1)
target_qpos = target_data.qpos.copy()
- # 初始姿势设为深蹲(索引0),从蹲下开始站起
+ # 初始姿势设为深蹲(索引0),从蹲下开始站起'+
mujoco.mj_resetDataKeyframe(model, data, 0)
# 控制参数(软体机器人需较小增益,避免震荡)
@@ -41,4 +41,4 @@ def main():
print("\n正在退出模拟....")
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()
diff --git a/src/humanoid_motion_control/run_walls.py b/src/humanoid_motion_control/run_walls.py
new file mode 100644
index 0000000000..b187011a84
--- /dev/null
+++ b/src/humanoid_motion_control/run_walls.py
@@ -0,0 +1,61 @@
+# 导入dm_control的核心模块
+from dm_control import composer # 用于组合环境、实体和任务的框架
+from dm_control.composer.variation import distributions # 用于生成随机分布的工具(如随机墙壁宽度)
+from dm_control.locomotion.arenas import corridors as corr_arenas # 走廊类竞技场定义
+from dm_control.locomotion.tasks import corridors as corr_tasks # 走廊相关任务定义
+from dm_control.locomotion.walkers import cmu_humanoid # CMU人形机器人模型
+
+# 导入其他辅助模块
+from absl import app # Google的命令行参数处理库
+from dm_control import viewer # 用于可视化仿真的交互窗口
+
+
+def cmu_humanoid_run_walls(random_state=None):
+ """创建一个CMU人形机器人在带墙壁的走廊中奔跑的环境
+
+ Args:
+ random_state: 随机数生成器状态,用于保证实验可复现性
+ Returns:
+ composer.Environment: 构建好的强化学习环境
+ """
+
+ # 创建一个位置控制的CMU人形机器人
+ # observable_options配置观测项:启用第一视角相机(egocentric_camera)
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ # 建造带墙壁的走廊竞技场
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4., # 墙壁之间的间隙(机器人需要穿过的宽度)
+ wall_width=distributions.Uniform(1, 7), # 墙壁宽度为1到7之间的随机值(增加环境多样性)
+ wall_height=3.0, # 墙壁高度(确保机器人无法翻越)
+ corridor_width=10, # 走廊总宽度
+ corridor_length=100, # 走廊总长度
+ include_initial_padding=False) # 不添加初始空白区域(机器人直接从起点开始)
+
+ # 定义"穿过走廊"任务:奖励机器人快速通过走廊
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker, # 绑定前面创建的机器人
+ arena=arena, # 绑定前面创建的走廊环境
+ walker_spawn_position=(0.5, 0, 0), # 机器人初始 spawn 位置(走廊起点附近)
+ target_velocity=3.0, # 目标速度(机器人需要达到的速度以获得高奖励)
+ physics_timestep=0.005, # 物理仿真时间步长(越小精度越高,计算量越大)
+ control_timestep=0.03) # 控制信号时间步长(机器人决策的频率,0.03秒约33Hz)
+
+ # 创建强化学习环境
+ return composer.Environment(
+ time_limit=30, # 单次仿真最大时长30秒
+ task=task, # 绑定任务
+ random_state=random_state, # 传入随机状态(保证可复现)
+ strip_singleton_obs_buffer_dim=True) # 去除观测数据中多余的单元素维度(简化数据结构)
+
+
+def main(unused_argv):
+ """主函数:启动可视化窗口运行环境"""
+ # 启动dm_control的交互 viewer,加载上面定义的环境
+ viewer.launch(environment_loader=cmu_humanoid_run_walls)
+
+
+if __name__ == '__main__':
+ # 使用absl的app.run启动程序(处理命令行参数并调用main函数)
+ app.run(main)
\ No newline at end of file
From a2731a87fd1ed059292a47a4f05fadf02b8b972e Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 29 Sep 2025 11:18:58 +0800
Subject: [PATCH 11/45] Save local changes of README.md
---
src/humanoid_motion_control/README.md | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/src/humanoid_motion_control/README.md b/src/humanoid_motion_control/README.md
index 7eee403936..e12d246791 100644
--- a/src/humanoid_motion_control/README.md
+++ b/src/humanoid_motion_control/README.md
@@ -1,5 +1,4 @@
--
-
+-
# 人形机器人走廊奔跑仿真(基于dm_control)
## 项目概述
From bd05420f57f38ba553e54de618279030154eda70 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 29 Sep 2025 11:29:52 +0800
Subject: [PATCH 12/45] Save local changes of README.md
---
src/humanoid_motion_control/README.md | 91 ---------------------------
1 file changed, 91 deletions(-)
diff --git a/src/humanoid_motion_control/README.md b/src/humanoid_motion_control/README.md
index 28490d789b..4880ebe4e5 100644
--- a/src/humanoid_motion_control/README.md
+++ b/src/humanoid_motion_control/README.md
@@ -104,94 +104,3 @@
- [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/)
- [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
-
-
-
-
-# 软体人形机器人弹跳动作仿真(基于MuJoCo)
-
-## 项目概述
-本项目是一个基于**MuJoCo物理引擎**的软体人形机器人物理仿真示例。实际运行中,机器人会呈现**原地短暂跳起后回落蹲下**的循环动作——这是由于软体机器人的柔性特性(弹性、形变)与控制参数共同作用的结果:关节电机驱动力过强时,软体结构会储存弹性势能,释放时形成向上的弹跳力,随后在重力作用下回落至蹲下姿势,形成往复运动。
-
-
-## 环境准备
-### 依赖库
-项目依赖以下Python库,需提前安装:
-```bash
-pip install mujoco mujoco-viewer
-```
-
-
-## 项目结构
-| 文件名 | 功能描述 |
-| -------------- | ------------------------------------------------------------ |
-| `humanoid.xml` | MJCF格式模型文件,定义软体机器人的结构、关节参数(阻尼、弹性)、关键帧姿势(深蹲、单腿站立)及电机执行器 |
-| `main.py` | 控制主程序,实现模型加载、比例控制逻辑、仿真循环及可视化,驱动机器人运动 |
-
-
-## 核心文件说明
-### 1. `humanoid.xml`(软体机器人模型)
-模型的关键特性直接影响弹跳效果:
-- **软体物理属性**:关节设置了较低阻尼(`damping`)和一定弹性,允许较大形变(为弹跳提供储能基础);
-- **关键帧姿势**:
- - 索引`0`:`squat`(深蹲姿势)—— 初始状态,关节弯曲储存势能;
- - 索引`1`:`stand_on_left_leg`(单左腿站立)—— 目标姿势,关节伸展;
-- **电机参数**:`ctrlrange="-1 1"`限制控制信号范围,但软体弹性会放大实际驱动力,导致动作过冲。
-
-
-### 2. `main.py`(控制逻辑)
-程序通过**比例控制**驱动关节运动,是弹跳动作的直接诱因:
-```python
-# 核心控制逻辑:计算关节误差,生成驱动信号
-qpos_error = target_qpos[7:] - data.qpos[7:] # 目标与当前关节位置差
-data.ctrl[:] = Kp * qpos_error # 比例增益放大误差,生成电机信号
-```
-- **参数影响**:代码中`Kp=5.0`的比例增益,对软体机器人而言可能过大——驱动力超过维持姿势所需,导致关节快速伸展,软体结构弹性形变加剧,最终形成“跳起”动作;
-- **循环机制**:跳起后重力主导回落,关节位置偏离目标,控制信号再次触发驱动力,形成“跳起→蹲下→再跳起”的循环。
-
-
-## 实际运行效果解析
-1. **初始状态**:机器人从深蹲姿势(`squat`)开始,关节弯曲,软体结构处于压缩状态;
-2. **驱动阶段**:控制信号驱动关节向站立姿势(`stand_on_left_leg`)伸展,软体材料因快速形变储存弹性势能;
-3. **弹跳阶段**:弹性势能释放,机器人获得向上的力,短暂离开初始位置(跳起);
-4. **回落阶段**:重力作用下,机器人下落,关节在冲击力和控制信号调整下重新弯曲,回到接近深蹲的姿势;
-5. **循环往复**:关节位置误差再次增大,控制信号重复驱动,形成周期性弹跳动作。
-
-
-## 使用步骤
-1. 确认`humanoid.xml`与`main.py`在同一目录(默认路径:`src/humanoid_motion_control/`);
-2. 终端执行命令启动仿真:
- ```bash
- python main.py
- ```
-3. 可视化窗口将显示机器人的弹跳动作,按`Ctrl+C`终止仿真。
-
-
-## 参数调整指南(优化动作)
-若希望减弱弹跳、实现更平稳的蹲站动作,可调整以下参数:
-
-| 参数名 | 调整方向 | 效果说明 |
-| -------- | ------------------------------------- | ------------------------------------------------------------ |
-| `Kp` | 减小至`1.0~3.0` | 降低比例增益,减弱电机驱动力,避免软体结构过度形变和弹性势能积累 |
-| 关节阻尼 | 在`humanoid.xml`中增大关节`damping`值 | 增加软体关节的阻尼,消耗部分弹性势能,减少弹跳幅度 |
-| 仿真步长 | 减小`time.sleep(0.005)`至`0.002` | 提高仿真精度,使控制信号更细腻地响应关节位置变化 |
-
-
-## 常见问题
-1. **弹跳幅度过大甚至翻倒**
- → 解决方案:将`Kp`降至`1.0`,同时在`humanoid.xml`中找到关节节点(如``),增大`damping`参数(如从`1.0`改为`5.0`)。
-
-2. **动作卡顿或延迟**
- → 解决方案:检查`time.sleep()`的数值,若电脑性能较好,可减小至`0.001`以提升流畅度。
-
-3. **模型加载失败**
- → 解决方案:确认`main.py`中模型路径与`humanoid.xml`实际位置一致,例如修改为:
- ```python
- model = mujoco.MjModel.from_xml_path("humanoid.xml") # 若文件在当前目录
- ```
-
-
-## 参考资料
-- [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/):了解软体材料参数(弹性、阻尼)的配置方法
-- [比例控制算法详解](https://en.wikipedia.org/wiki/Proportional_control):理解`Kp`对控制效果的影响
-
From 1b7cbf2b5c3e36692415607d424fd4077b13a219 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Sun, 5 Oct 2025 17:01:09 +0800
Subject: [PATCH 13/45] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=9A=8F=E6=9C=BA?=
=?UTF-8?q?=E5=8A=A8=E4=BD=9C=E7=AD=96=E7=95=A5=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/run_walls.py | 103 ++++++++++++-----------
1 file changed, 55 insertions(+), 48 deletions(-)
diff --git a/src/humanoid_motion_control/run_walls.py b/src/humanoid_motion_control/run_walls.py
index b187011a84..bcdf51f6a8 100644
--- a/src/humanoid_motion_control/run_walls.py
+++ b/src/humanoid_motion_control/run_walls.py
@@ -1,61 +1,68 @@
-# 导入dm_control的核心模块
-from dm_control import composer # 用于组合环境、实体和任务的框架
-from dm_control.composer.variation import distributions # 用于生成随机分布的工具(如随机墙壁宽度)
-from dm_control.locomotion.arenas import corridors as corr_arenas # 走廊类竞技场定义
-from dm_control.locomotion.tasks import corridors as corr_tasks # 走廊相关任务定义
-from dm_control.locomotion.walkers import cmu_humanoid # CMU人形机器人模型
+# 标准库
+import numpy as np
+# 第三方库
+from absl import app
+# dm_control 模块
+from dm_control import composer
+from dm_control.composer.variation import distributions
+from dm_control.locomotion.arenas import corridors as corr_arenas
+from dm_control.locomotion.tasks import corridors as corr_tasks
+from dm_control.locomotion.walkers import cmu_humanoid
+from dm_control import viewer
-# 导入其他辅助模块
-from absl import app # Google的命令行参数处理库
-from dm_control import viewer # 用于可视化仿真的交互窗口
+# 全局环境变量,供策略函数访问
+global_env = None
-
-def cmu_humanoid_run_walls(random_state=None):
- """创建一个CMU人形机器人在带墙壁的走廊中奔跑的环境
-
- Args:
- random_state: 随机数生成器状态,用于保证实验可复现性
- Returns:
- composer.Environment: 构建好的强化学习环境
- """
-
- # 创建一个位置控制的CMU人形机器人
- # observable_options配置观测项:启用第一视角相机(egocentric_camera)
+def create_environment():
+ """创建带墙壁走廊的人形机器人环境"""
walker = cmu_humanoid.CMUHumanoidPositionControlled(
observable_options={'egocentric_camera': dict(enabled=True)})
-
- # 建造带墙壁的走廊竞技场
+
arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁之间的间隙(机器人需要穿过的宽度)
- wall_width=distributions.Uniform(1, 7), # 墙壁宽度为1到7之间的随机值(增加环境多样性)
- wall_height=3.0, # 墙壁高度(确保机器人无法翻越)
- corridor_width=10, # 走廊总宽度
- corridor_length=100, # 走廊总长度
- include_initial_padding=False) # 不添加初始空白区域(机器人直接从起点开始)
-
- # 定义"穿过走廊"任务:奖励机器人快速通过走廊
+ wall_gap=4.,
+ wall_width=distributions.Uniform(1, 7),
+ wall_height=3.0,
+ corridor_width=10,
+ corridor_length=100,
+ include_initial_padding=False)
+
task = corr_tasks.RunThroughCorridor(
- walker=walker, # 绑定前面创建的机器人
- arena=arena, # 绑定前面创建的走廊环境
- walker_spawn_position=(0.5, 0, 0), # 机器人初始 spawn 位置(走廊起点附近)
- target_velocity=3.0, # 目标速度(机器人需要达到的速度以获得高奖励)
- physics_timestep=0.005, # 物理仿真时间步长(越小精度越高,计算量越大)
- control_timestep=0.03) # 控制信号时间步长(机器人决策的频率,0.03秒约33Hz)
-
- # 创建强化学习环境
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
return composer.Environment(
- time_limit=30, # 单次仿真最大时长30秒
- task=task, # 绑定任务
- random_state=random_state, # 传入随机状态(保证可复现)
- strip_singleton_obs_buffer_dim=True) # 去除观测数据中多余的单元素维度(简化数据结构)
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+
+def random_policy(timestep):
+ """随机动作策略:使用全局环境变量获取动作空间"""
+ global global_env
+ action_spec = global_env.action_spec()
+ return np.random.uniform(
+ low=action_spec.minimum,
+ high=action_spec.maximum,
+ size=action_spec.shape
+ )
def main(unused_argv):
- """主函数:启动可视化窗口运行环境"""
- # 启动dm_control的交互 viewer,加载上面定义的环境
- viewer.launch(environment_loader=cmu_humanoid_run_walls)
+ """主函数:初始化全局环境并启动viewer"""
+ global global_env
+ # 初始化环境并赋值给全局变量
+ global_env = create_environment()
+
+ # 启动viewer
+ viewer.launch(
+ environment_loader=create_environment,
+ policy=random_policy
+ )
-if __name__ == '__main__':
- # 使用absl的app.run启动程序(处理命令行参数并调用main函数)
+if __name__ == "__main__":
app.run(main)
\ No newline at end of file
From f06d23880ee2c12786358cb60a066ca266c24781 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 6 Oct 2025 12:43:44 +0800
Subject: [PATCH 14/45] =?UTF-8?q?=E9=87=8D=E6=96=B0=E6=8F=90=E4=BA=A4?=
=?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=9A=8F=E6=9C=BA=E5=8A=A8=E4=BD=9C=E7=AD=96?=
=?UTF-8?q?=E7=95=A5=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/run_walls.py | 100 ++++++++++++-----------
1 file changed, 52 insertions(+), 48 deletions(-)
diff --git a/src/humanoid_motion_control/run_walls.py b/src/humanoid_motion_control/run_walls.py
index b187011a84..c72584f953 100644
--- a/src/humanoid_motion_control/run_walls.py
+++ b/src/humanoid_motion_control/run_walls.py
@@ -1,61 +1,65 @@
-# 导入dm_control的核心模块
-from dm_control import composer # 用于组合环境、实体和任务的框架
-from dm_control.composer.variation import distributions # 用于生成随机分布的工具(如随机墙壁宽度)
-from dm_control.locomotion.arenas import corridors as corr_arenas # 走廊类竞技场定义
-from dm_control.locomotion.tasks import corridors as corr_tasks # 走廊相关任务定义
-from dm_control.locomotion.walkers import cmu_humanoid # CMU人形机器人模型
+# 标准库
+import numpy as np
+# 第三方库
+from absl import app
+# dm_control 模块
+from dm_control import composer
+from dm_control.composer.variation import distributions
+from dm_control.locomotion.arenas import corridors as corr_arenas
+from dm_control.locomotion.tasks import corridors as corr_tasks
+from dm_control.locomotion.walkers import cmu_humanoid
+from dm_control import viewer
-# 导入其他辅助模块
-from absl import app # Google的命令行参数处理库
-from dm_control import viewer # 用于可视化仿真的交互窗口
+# 全局环境变量,供策略函数访问
+global_env = None
-
-def cmu_humanoid_run_walls(random_state=None):
- """创建一个CMU人形机器人在带墙壁的走廊中奔跑的环境
-
- Args:
- random_state: 随机数生成器状态,用于保证实验可复现性
- Returns:
- composer.Environment: 构建好的强化学习环境
- """
-
- # 创建一个位置控制的CMU人形机器人
- # observable_options配置观测项:启用第一视角相机(egocentric_camera)
+def create_environment():
+ """创建带墙壁走廊的人形机器人环境"""
walker = cmu_humanoid.CMUHumanoidPositionControlled(
observable_options={'egocentric_camera': dict(enabled=True)})
- # 建造带墙壁的走廊竞技场
arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁之间的间隙(机器人需要穿过的宽度)
- wall_width=distributions.Uniform(1, 7), # 墙壁宽度为1到7之间的随机值(增加环境多样性)
- wall_height=3.0, # 墙壁高度(确保机器人无法翻越)
- corridor_width=10, # 走廊总宽度
- corridor_length=100, # 走廊总长度
- include_initial_padding=False) # 不添加初始空白区域(机器人直接从起点开始)
-
- # 定义"穿过走廊"任务:奖励机器人快速通过走廊
+ wall_gap=4.,
+ wall_width=distributions.Uniform(1, 7),
+ wall_height=3.0,
+ corridor_width=10,
+ corridor_length=100,
+ include_initial_padding=False)
+
task = corr_tasks.RunThroughCorridor(
- walker=walker, # 绑定前面创建的机器人
- arena=arena, # 绑定前面创建的走廊环境
- walker_spawn_position=(0.5, 0, 0), # 机器人初始 spawn 位置(走廊起点附近)
- target_velocity=3.0, # 目标速度(机器人需要达到的速度以获得高奖励)
- physics_timestep=0.005, # 物理仿真时间步长(越小精度越高,计算量越大)
- control_timestep=0.03) # 控制信号时间步长(机器人决策的频率,0.03秒约33Hz)
-
- # 创建强化学习环境
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
return composer.Environment(
- time_limit=30, # 单次仿真最大时长30秒
- task=task, # 绑定任务
- random_state=random_state, # 传入随机状态(保证可复现)
- strip_singleton_obs_buffer_dim=True) # 去除观测数据中多余的单元素维度(简化数据结构)
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+def random_policy(timestep):
+ """随机动作策略:使用全局环境变量获取动作空间"""
+ global global_env
+ action_spec = global_env.action_spec()
+ return np.random.uniform(
+ low=action_spec.minimum,
+ high=action_spec.maximum,
+ size=action_spec.shape
+ )
def main(unused_argv):
- """主函数:启动可视化窗口运行环境"""
- # 启动dm_control的交互 viewer,加载上面定义的环境
- viewer.launch(environment_loader=cmu_humanoid_run_walls)
-
+ """主函数:初始化全局环境并启动viewer"""
+ global global_env
+ # 初始化环境并赋值给全局变量
+ global_env = create_environment()
+
+ # 启动viewer
+ viewer.launch(
+ environment_loader=create_environment,
+ policy=random_policy
+ )
-if __name__ == '__main__':
- # 使用absl的app.run启动程序(处理命令行参数并调用main函数)
+if __name__ == "__main__":
app.run(main)
\ No newline at end of file
From d13b32263563f5cf98e599cf07beec173b01fe5e Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Fri, 10 Oct 2025 12:22:09 +0800
Subject: [PATCH 15/45] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=B8=80=E4=B8=AA?=
=?UTF-8?q?=E6=9C=89=E7=A9=BA=E9=9A=99=E7=9A=84=E8=B5=B0=E5=BB=8A=E5=BD=A2?=
=?UTF-8?q?=E7=AB=9E=E6=8A=80=E5=9C=BA=E7=8E=AF=E5=A2=83?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/motion_main.py | 92 ++++++++++++++++++++++
1 file changed, 92 insertions(+)
create mode 100644 src/humanoid_motion_control/motion_main.py
diff --git a/src/humanoid_motion_control/motion_main.py b/src/humanoid_motion_control/motion_main.py
new file mode 100644
index 0000000000..8a48c7d975
--- /dev/null
+++ b/src/humanoid_motion_control/motion_main.py
@@ -0,0 +1,92 @@
+# 标准库
+import numpy as np
+# 第三方库
+from absl import app
+# dm_control 模块
+from dm_control import composer
+from dm_control.composer.variation import distributions
+from dm_control.locomotion.arenas import corridors as corr_arenas
+from dm_control.locomotion.tasks import corridors as corr_tasks
+from dm_control.locomotion.walkers import cmu_humanoid
+from dm_control import viewer
+
+# 全局环境变量,供策略函数访问
+global_env = None
+
+def create_environment_run_walls():
+ """创建带墙壁走廊的人形机器人环境"""
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4.,
+ wall_width=distributions.Uniform(1, 7),
+ wall_height=3.0,
+ corridor_width=10,
+ corridor_length=100,
+ include_initial_padding=False)
+
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
+ return composer.Environment(
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+def create_environment_run_gaps():
+ "创建有空隙的走廊形竞技场环境"
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ arena = corr_arenas.GapsCorridor(
+ platform_length=distributions.Uniform(.3, 2.5),
+ gap_length=distributions.Uniform(.5, 1.25),
+ corridor_width=10,
+ corridor_length=100)
+
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
+ return composer.Environment(
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+def random_policy(timestep):
+ """随机动作策略:使用全局环境变量获取动作空间"""
+ global global_env
+ action_spec = global_env.action_spec()
+ return np.random.uniform(
+ low=action_spec.minimum,
+ high=action_spec.maximum,
+ size=action_spec.shape
+ )
+
+
+def main(unused_argv):
+ """主函数:初始化全局环境并启动viewer"""
+ global global_env
+ # 初始化环境并赋值给全局变量
+ global_env = create_environment_run_gaps()
+
+ # 启动viewer
+ viewer.launch(
+ environment_loader=create_environment_run_gaps,
+ policy=random_policy
+ )
+
+
+if __name__ == "__main__":
+
+ app.run(main)
\ No newline at end of file
From 2df51c9cb8e5539dd34faed3e18323f04f86c9b9 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Fri, 10 Oct 2025 13:04:26 +0800
Subject: [PATCH 16/45] =?UTF-8?q?=E6=81=A2=E5=A4=8Drun=5Fwalls.py=E5=B9=B6?=
=?UTF-8?q?=E5=87=86=E5=A4=87=E8=A7=A3=E5=86=B3=E5=86=B2=E7=AA=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/run_walls.py | 66 ++++++++++++++++++++++++
1 file changed, 66 insertions(+)
create mode 100644 src/humanoid_motion_control/run_walls.py
diff --git a/src/humanoid_motion_control/run_walls.py b/src/humanoid_motion_control/run_walls.py
new file mode 100644
index 0000000000..08e22dfa62
--- /dev/null
+++ b/src/humanoid_motion_control/run_walls.py
@@ -0,0 +1,66 @@
+# 标准库
+import numpy as np
+# 第三方库
+from absl import app
+# dm_control 模块
+from dm_control import composer
+from dm_control.composer.variation import distributions
+from dm_control.locomotion.arenas import corridors as corr_arenas
+from dm_control.locomotion.tasks import corridors as corr_tasks
+from dm_control.locomotion.walkers import cmu_humanoid
+from dm_control import viewer
+
+# 全局环境变量,供策略函数访问
+global_env = None
+
+def create_environment():
+ """创建带墙壁走廊的人形机器人环境"""
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4.,
+ wall_width=distributions.Uniform(1, 7),
+ wall_height=3.0,
+ corridor_width=10,
+ corridor_length=100,
+ include_initial_padding=False)
+
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
+ return composer.Environment(
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+def random_policy(timestep):
+ """随机动作策略:使用全局环境变量获取动作空间"""
+ global global_env
+ action_spec = global_env.action_spec()
+ return np.random.uniform(
+ low=action_spec.minimum,
+ high=action_spec.maximum,
+ size=action_spec.shape
+ )
+
+def main(unused_argv):
+ """主函数:初始化全局环境并启动viewer"""
+ global global_env
+ # 初始化环境并赋值给全局变量
+ global_env = create_environment()
+
+ # 启动viewer
+ viewer.launch(
+ environment_loader=create_environment,
+ policy=random_policy
+ )
+
+
+if __name__ == "__main__":
+ app.run(main)
\ No newline at end of file
From 38e1b8ec4153de4b0599c7454ad05f4be3fa681d Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 20 Oct 2025 09:18:32 +0800
Subject: [PATCH 17/45] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=96=87=E4=BB=B6?=
=?UTF-8?q?=E5=90=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../README.md | 106 ++++++++
.../humanoid.xml | 252 ++++++++++++++++++
.../main.py | 44 +++
.../main_humanoid.py | 44 +++
.../motion_main.py | 92 +++++++
.../run_walls.py | 66 +++++
6 files changed, 604 insertions(+)
create mode 100644 src/humanoid_Simulation_Environment_Establish/README.md
create mode 100644 src/humanoid_Simulation_Environment_Establish/humanoid.xml
create mode 100644 src/humanoid_Simulation_Environment_Establish/main.py
create mode 100644 src/humanoid_Simulation_Environment_Establish/main_humanoid.py
create mode 100644 src/humanoid_Simulation_Environment_Establish/motion_main.py
create mode 100644 src/humanoid_Simulation_Environment_Establish/run_walls.py
diff --git a/src/humanoid_Simulation_Environment_Establish/README.md b/src/humanoid_Simulation_Environment_Establish/README.md
new file mode 100644
index 0000000000..4880ebe4e5
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/README.md
@@ -0,0 +1,106 @@
+
+ # 人形机器人走廊奔跑仿真(基于dm_control)
+
+ ## 项目概述
+ 本项目核心为基于**dm_control框架**构建的人形机器人走廊奔跑仿真环境。dm_control是DeepMind基于MuJoCo物理引擎开发的高级仿真工具,简化了强化学习环境的搭建、任务定义与可视化流程。本项目中,CMU人形机器人需在带随机墙壁的走廊中以目标速度奔跑,展示了dm_control在复杂运动任务仿真中的应用。
+
+ ## 环境准备
+ ### 依赖库
+ 需安装以下核心依赖(含dm_control及其底层依赖):
+ ```bash
+ pip install mujoco dm-control absl-py
+ ```
+ - `dm_control`:提供环境构建、任务定义和交互可视化功能
+ - `mujoco`:dm_control依赖的物理引擎核心
+ - `absl-py`:用于命令行参数处理
+
+ ## 项目结构
+ | 文件名 | 功能描述 |
+ | ------------------ | ------------------------------------------------------------ |
+ | `run_walls.py` | **核心文件**:基于dm_control构建带墙壁的走廊环境,实现CMU人形机器人奔跑任务 |
+ | `humanoid.xml` | MJCF格式模型文件,定义机器人结构(辅助文件) |
+ | `main_humanoid.py` | 软体机器人弹跳仿真程序 |
+
+ ## 核心功能:走廊奔跑环境(run_walls.py)
+ ### 1. dm_control框架组件应用
+ ```python
+ # dm_control核心模块使用
+ from dm_control import composer # 环境组合框架
+ from dm_control.locomotion.arenas import corridors # 走廊场景
+ from dm_control.locomotion.tasks import corridors # 走廊任务
+ from dm_control.locomotion.walkers import cmu_humanoid # 机器人模型
+ from dm_control import viewer # 可视化工具
+ ```
+
+ ### 2. 环境构建流程
+ #### (1)机器人配置
+ 创建位置控制的CMU人形机器人,启用第一视角相机观测:
+ ```python
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)}
+ )
+ ```
+
+ #### (2)走廊场景设计
+ 通过`WallsCorridor`类构建带随机墙壁的动态环境:
+ ```python
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4., # 墙壁间隙宽度(机器人需穿过的空间)
+ wall_width=distributions.Uniform(1, 7), # 墙壁宽度随机化(1-7单位)
+ wall_height=3.0, # 防止翻越的墙壁高度
+ corridor_width=10, # 走廊总宽度
+ corridor_length=100, # 走廊总长度
+ include_initial_padding=False # 无初始空白区域
+ )
+ ```
+
+ #### (3)任务定义
+ 通过`RunThroughCorridor`类定义奔跑任务规则:
+ ```python
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0), # 初始位置
+ target_velocity=3.0, # 目标速度(影响奖励计算)
+ physics_timestep=0.005, # 物理仿真步长
+ control_timestep=0.03 # 控制决策步长(≈33Hz)
+ )
+ ```
+
+ #### (4)环境封装
+ 组合场景与任务,生成强化学习可用环境:
+ ```python
+ environment = composer.Environment(
+ time_limit=30, # 单次仿真时长30秒
+ task=task,
+ strip_singleton_obs_buffer_dim=True # 简化观测数据结构
+ )
+ ```
+
+ ## 使用方法
+ 1. 运行走廊奔跑仿真:
+ ```bash
+ python run_walls.py
+ ```
+ 2. 交互操作:
+ - 可视化窗口展示机器人第一视角和全局视角
+ - 按`Ctrl+C`终止仿真
+
+ ## dm_control核心特性体现
+ 1. **模块化设计**:通过`composer`模块分离机器人(walker)、场景(arena)和任务(task),便于独立修改
+ 2. **随机性支持**:使用`distributions`生成随机墙壁宽度,增强环境多样性
+ 3. **灵活观测配置**:通过`observable_options`轻松启用第一视角相机等观测项
+ 4. **高效可视化**:`viewer`模块提供实时交互界面,支持视角切换与状态查看
+
+ ## 参数调整指南
+ | 参数 | 调整范围 | 效果说明 |
+ | ------------------ | -------- | -------------------------------- |
+ | `wall_gap` | 2.0~6.0 | 减小值增加穿过难度 |
+ | `target_velocity` | 1.0~5.0 | 提高值要求机器人跑得更快 |
+ | `control_timestep` | 0.01~0.1 | 减小值提高控制精度(增加计算量) |
+
+ ## 参考资料
+ - [dm_control官方文档](https://github.com/deepmind/dm_control)
+ - [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/)
+ - [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
+
diff --git a/src/humanoid_Simulation_Environment_Establish/humanoid.xml b/src/humanoid_Simulation_Environment_Establish/humanoid.xml
new file mode 100644
index 0000000000..38b07c0342
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/humanoid.xml
@@ -0,0 +1,252 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/humanoid_Simulation_Environment_Establish/main.py b/src/humanoid_Simulation_Environment_Establish/main.py
new file mode 100644
index 0000000000..de42eac3b6
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/main.py
@@ -0,0 +1,44 @@
+# 标准库
+import time
+# 第三方库
+import mujoco
+from mujoco import viewer
+
+def main():
+ try:
+ model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
+ except Exception as e:
+ print(f"模型加载失败: {e}")
+ return
+
+ data = mujoco.MjData(model)
+ # 临时数据用于获取目标关键帧(站立姿势,索引1)
+ target_data = mujoco.MjData(model)
+ mujoco.mj_resetDataKeyframe(model, target_data, 1)
+ target_qpos = target_data.qpos.copy()
+
+ # 初始姿势设为深蹲(索引0),从蹲下开始站起
+ mujoco.mj_resetDataKeyframe(model, data, 0)
+
+ # 控制参数(软体机器人需较小增益,避免震荡)
+ Kp = 5.0 # 比例增益(根据软体特性调小)
+
+ with viewer.launch_passive(model, data) as v:
+ try:
+ while True:
+ mujoco.mj_step(model, data)
+
+ # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
+ qpos_error = target_qpos[7:] - data.qpos[7:]
+ # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
+ data.ctrl[:] = Kp * qpos_error
+
+ print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
+
+ v.sync()
+ time.sleep(0.005)
+ except KeyboardInterrupt:
+ print("\n正在退出模拟....")
+
+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
new file mode 100644
index 0000000000..2982d10461
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
@@ -0,0 +1,44 @@
+# 标准库
+import time
+# 第三方库
+import mujoco
+from mujoco import viewer
+
+def main():
+ try:
+ model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
+ except Exception as e:
+ print(f"模型加载失败: {e}")
+ return
+
+ data = mujoco.MjData(model)
+ # 临时数据用于获取目标关键帧(站立姿势,索引1)
+ target_data = mujoco.MjData(model)
+ mujoco.mj_resetDataKeyframe(model, target_data, 1)
+ target_qpos = target_data.qpos.copy()
+
+ # 初始姿势设为深蹲(索引0),从蹲下开始站起'+
+ mujoco.mj_resetDataKeyframe(model, data, 0)
+
+ # 控制参数(软体机器人需较小增益,避免震荡)
+ Kp = 5.0 # 比例增益(根据软体特性调小)
+
+ with viewer.launch_passive(model, data) as v:
+ try:
+ while True:
+ mujoco.mj_step(model, data)
+
+ # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
+ qpos_error = target_qpos[7:] - data.qpos[7:]
+ # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
+ data.ctrl[:] = Kp * qpos_error
+
+ print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
+
+ v.sync()
+ time.sleep(0.005)
+ except KeyboardInterrupt:
+ print("\n正在退出模拟....")
+
+if __name__ == "__main__":
+ main()
diff --git a/src/humanoid_Simulation_Environment_Establish/motion_main.py b/src/humanoid_Simulation_Environment_Establish/motion_main.py
new file mode 100644
index 0000000000..8a48c7d975
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/motion_main.py
@@ -0,0 +1,92 @@
+# 标准库
+import numpy as np
+# 第三方库
+from absl import app
+# dm_control 模块
+from dm_control import composer
+from dm_control.composer.variation import distributions
+from dm_control.locomotion.arenas import corridors as corr_arenas
+from dm_control.locomotion.tasks import corridors as corr_tasks
+from dm_control.locomotion.walkers import cmu_humanoid
+from dm_control import viewer
+
+# 全局环境变量,供策略函数访问
+global_env = None
+
+def create_environment_run_walls():
+ """创建带墙壁走廊的人形机器人环境"""
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4.,
+ wall_width=distributions.Uniform(1, 7),
+ wall_height=3.0,
+ corridor_width=10,
+ corridor_length=100,
+ include_initial_padding=False)
+
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
+ return composer.Environment(
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+def create_environment_run_gaps():
+ "创建有空隙的走廊形竞技场环境"
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ arena = corr_arenas.GapsCorridor(
+ platform_length=distributions.Uniform(.3, 2.5),
+ gap_length=distributions.Uniform(.5, 1.25),
+ corridor_width=10,
+ corridor_length=100)
+
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
+ return composer.Environment(
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+def random_policy(timestep):
+ """随机动作策略:使用全局环境变量获取动作空间"""
+ global global_env
+ action_spec = global_env.action_spec()
+ return np.random.uniform(
+ low=action_spec.minimum,
+ high=action_spec.maximum,
+ size=action_spec.shape
+ )
+
+
+def main(unused_argv):
+ """主函数:初始化全局环境并启动viewer"""
+ global global_env
+ # 初始化环境并赋值给全局变量
+ global_env = create_environment_run_gaps()
+
+ # 启动viewer
+ viewer.launch(
+ environment_loader=create_environment_run_gaps,
+ policy=random_policy
+ )
+
+
+if __name__ == "__main__":
+
+ app.run(main)
\ No newline at end of file
diff --git a/src/humanoid_Simulation_Environment_Establish/run_walls.py b/src/humanoid_Simulation_Environment_Establish/run_walls.py
new file mode 100644
index 0000000000..08e22dfa62
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/run_walls.py
@@ -0,0 +1,66 @@
+# 标准库
+import numpy as np
+# 第三方库
+from absl import app
+# dm_control 模块
+from dm_control import composer
+from dm_control.composer.variation import distributions
+from dm_control.locomotion.arenas import corridors as corr_arenas
+from dm_control.locomotion.tasks import corridors as corr_tasks
+from dm_control.locomotion.walkers import cmu_humanoid
+from dm_control import viewer
+
+# 全局环境变量,供策略函数访问
+global_env = None
+
+def create_environment():
+ """创建带墙壁走廊的人形机器人环境"""
+ walker = cmu_humanoid.CMUHumanoidPositionControlled(
+ observable_options={'egocentric_camera': dict(enabled=True)})
+
+ arena = corr_arenas.WallsCorridor(
+ wall_gap=4.,
+ wall_width=distributions.Uniform(1, 7),
+ wall_height=3.0,
+ corridor_width=10,
+ corridor_length=100,
+ include_initial_padding=False)
+
+ task = corr_tasks.RunThroughCorridor(
+ walker=walker,
+ arena=arena,
+ walker_spawn_position=(0.5, 0, 0),
+ target_velocity=3.0,
+ physics_timestep=0.005,
+ control_timestep=0.03)
+
+ return composer.Environment(
+ time_limit=30,
+ task=task,
+ strip_singleton_obs_buffer_dim=True)
+
+def random_policy(timestep):
+ """随机动作策略:使用全局环境变量获取动作空间"""
+ global global_env
+ action_spec = global_env.action_spec()
+ return np.random.uniform(
+ low=action_spec.minimum,
+ high=action_spec.maximum,
+ size=action_spec.shape
+ )
+
+def main(unused_argv):
+ """主函数:初始化全局环境并启动viewer"""
+ global global_env
+ # 初始化环境并赋值给全局变量
+ global_env = create_environment()
+
+ # 启动viewer
+ viewer.launch(
+ environment_loader=create_environment,
+ policy=random_policy
+ )
+
+
+if __name__ == "__main__":
+ app.run(main)
\ No newline at end of file
From 33bc1b9ffa739702e37f26044e492408ec84941a Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 20 Oct 2025 09:44:11 +0800
Subject: [PATCH 18/45] =?UTF-8?q?=E5=88=A0=E9=99=A4=E9=87=8D=E5=A4=8D?=
=?UTF-8?q?=E7=9A=84humanoid=5Fmotion=5Fcontrol=E6=A8=A1=E5=9D=97?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/README.md | 106 --------
src/humanoid_motion_control/humanoid.xml | 252 -------------------
src/humanoid_motion_control/main.py | 44 ----
src/humanoid_motion_control/main_humanoid.py | 44 ----
src/humanoid_motion_control/motion_main.py | 92 -------
src/humanoid_motion_control/run_walls.py | 66 -----
6 files changed, 604 deletions(-)
delete mode 100644 src/humanoid_motion_control/README.md
delete mode 100644 src/humanoid_motion_control/humanoid.xml
delete mode 100644 src/humanoid_motion_control/main.py
delete mode 100644 src/humanoid_motion_control/main_humanoid.py
delete mode 100644 src/humanoid_motion_control/motion_main.py
delete mode 100644 src/humanoid_motion_control/run_walls.py
diff --git a/src/humanoid_motion_control/README.md b/src/humanoid_motion_control/README.md
deleted file mode 100644
index 4880ebe4e5..0000000000
--- a/src/humanoid_motion_control/README.md
+++ /dev/null
@@ -1,106 +0,0 @@
-
- # 人形机器人走廊奔跑仿真(基于dm_control)
-
- ## 项目概述
- 本项目核心为基于**dm_control框架**构建的人形机器人走廊奔跑仿真环境。dm_control是DeepMind基于MuJoCo物理引擎开发的高级仿真工具,简化了强化学习环境的搭建、任务定义与可视化流程。本项目中,CMU人形机器人需在带随机墙壁的走廊中以目标速度奔跑,展示了dm_control在复杂运动任务仿真中的应用。
-
- ## 环境准备
- ### 依赖库
- 需安装以下核心依赖(含dm_control及其底层依赖):
- ```bash
- pip install mujoco dm-control absl-py
- ```
- - `dm_control`:提供环境构建、任务定义和交互可视化功能
- - `mujoco`:dm_control依赖的物理引擎核心
- - `absl-py`:用于命令行参数处理
-
- ## 项目结构
- | 文件名 | 功能描述 |
- | ------------------ | ------------------------------------------------------------ |
- | `run_walls.py` | **核心文件**:基于dm_control构建带墙壁的走廊环境,实现CMU人形机器人奔跑任务 |
- | `humanoid.xml` | MJCF格式模型文件,定义机器人结构(辅助文件) |
- | `main_humanoid.py` | 软体机器人弹跳仿真程序 |
-
- ## 核心功能:走廊奔跑环境(run_walls.py)
- ### 1. dm_control框架组件应用
- ```python
- # dm_control核心模块使用
- from dm_control import composer # 环境组合框架
- from dm_control.locomotion.arenas import corridors # 走廊场景
- from dm_control.locomotion.tasks import corridors # 走廊任务
- from dm_control.locomotion.walkers import cmu_humanoid # 机器人模型
- from dm_control import viewer # 可视化工具
- ```
-
- ### 2. 环境构建流程
- #### (1)机器人配置
- 创建位置控制的CMU人形机器人,启用第一视角相机观测:
- ```python
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)}
- )
- ```
-
- #### (2)走廊场景设计
- 通过`WallsCorridor`类构建带随机墙壁的动态环境:
- ```python
- arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁间隙宽度(机器人需穿过的空间)
- wall_width=distributions.Uniform(1, 7), # 墙壁宽度随机化(1-7单位)
- wall_height=3.0, # 防止翻越的墙壁高度
- corridor_width=10, # 走廊总宽度
- corridor_length=100, # 走廊总长度
- include_initial_padding=False # 无初始空白区域
- )
- ```
-
- #### (3)任务定义
- 通过`RunThroughCorridor`类定义奔跑任务规则:
- ```python
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0), # 初始位置
- target_velocity=3.0, # 目标速度(影响奖励计算)
- physics_timestep=0.005, # 物理仿真步长
- control_timestep=0.03 # 控制决策步长(≈33Hz)
- )
- ```
-
- #### (4)环境封装
- 组合场景与任务,生成强化学习可用环境:
- ```python
- environment = composer.Environment(
- time_limit=30, # 单次仿真时长30秒
- task=task,
- strip_singleton_obs_buffer_dim=True # 简化观测数据结构
- )
- ```
-
- ## 使用方法
- 1. 运行走廊奔跑仿真:
- ```bash
- python run_walls.py
- ```
- 2. 交互操作:
- - 可视化窗口展示机器人第一视角和全局视角
- - 按`Ctrl+C`终止仿真
-
- ## dm_control核心特性体现
- 1. **模块化设计**:通过`composer`模块分离机器人(walker)、场景(arena)和任务(task),便于独立修改
- 2. **随机性支持**:使用`distributions`生成随机墙壁宽度,增强环境多样性
- 3. **灵活观测配置**:通过`observable_options`轻松启用第一视角相机等观测项
- 4. **高效可视化**:`viewer`模块提供实时交互界面,支持视角切换与状态查看
-
- ## 参数调整指南
- | 参数 | 调整范围 | 效果说明 |
- | ------------------ | -------- | -------------------------------- |
- | `wall_gap` | 2.0~6.0 | 减小值增加穿过难度 |
- | `target_velocity` | 1.0~5.0 | 提高值要求机器人跑得更快 |
- | `control_timestep` | 0.01~0.1 | 减小值提高控制精度(增加计算量) |
-
- ## 参考资料
- - [dm_control官方文档](https://github.com/deepmind/dm_control)
- - [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/)
- - [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
-
diff --git a/src/humanoid_motion_control/humanoid.xml b/src/humanoid_motion_control/humanoid.xml
deleted file mode 100644
index 38b07c0342..0000000000
--- a/src/humanoid_motion_control/humanoid.xml
+++ /dev/null
@@ -1,252 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/humanoid_motion_control/main.py b/src/humanoid_motion_control/main.py
deleted file mode 100644
index de42eac3b6..0000000000
--- a/src/humanoid_motion_control/main.py
+++ /dev/null
@@ -1,44 +0,0 @@
-# 标准库
-import time
-# 第三方库
-import mujoco
-from mujoco import viewer
-
-def main():
- try:
- model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
- except Exception as e:
- print(f"模型加载失败: {e}")
- return
-
- data = mujoco.MjData(model)
- # 临时数据用于获取目标关键帧(站立姿势,索引1)
- target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1)
- target_qpos = target_data.qpos.copy()
-
- # 初始姿势设为深蹲(索引0),从蹲下开始站起
- mujoco.mj_resetDataKeyframe(model, data, 0)
-
- # 控制参数(软体机器人需较小增益,避免震荡)
- Kp = 5.0 # 比例增益(根据软体特性调小)
-
- with viewer.launch_passive(model, data) as v:
- try:
- while True:
- mujoco.mj_step(model, data)
-
- # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
- qpos_error = target_qpos[7:] - data.qpos[7:]
- # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
- data.ctrl[:] = Kp * qpos_error
-
- print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
-
- v.sync()
- time.sleep(0.005)
- except KeyboardInterrupt:
- print("\n正在退出模拟....")
-
-if __name__ == "__main__":
- main()
\ No newline at end of file
diff --git a/src/humanoid_motion_control/main_humanoid.py b/src/humanoid_motion_control/main_humanoid.py
deleted file mode 100644
index 2982d10461..0000000000
--- a/src/humanoid_motion_control/main_humanoid.py
+++ /dev/null
@@ -1,44 +0,0 @@
-# 标准库
-import time
-# 第三方库
-import mujoco
-from mujoco import viewer
-
-def main():
- try:
- model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
- except Exception as e:
- print(f"模型加载失败: {e}")
- return
-
- data = mujoco.MjData(model)
- # 临时数据用于获取目标关键帧(站立姿势,索引1)
- target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1)
- target_qpos = target_data.qpos.copy()
-
- # 初始姿势设为深蹲(索引0),从蹲下开始站起'+
- mujoco.mj_resetDataKeyframe(model, data, 0)
-
- # 控制参数(软体机器人需较小增益,避免震荡)
- Kp = 5.0 # 比例增益(根据软体特性调小)
-
- with viewer.launch_passive(model, data) as v:
- try:
- while True:
- mujoco.mj_step(model, data)
-
- # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
- qpos_error = target_qpos[7:] - data.qpos[7:]
- # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
- data.ctrl[:] = Kp * qpos_error
-
- print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
-
- v.sync()
- time.sleep(0.005)
- except KeyboardInterrupt:
- print("\n正在退出模拟....")
-
-if __name__ == "__main__":
- main()
diff --git a/src/humanoid_motion_control/motion_main.py b/src/humanoid_motion_control/motion_main.py
deleted file mode 100644
index 8a48c7d975..0000000000
--- a/src/humanoid_motion_control/motion_main.py
+++ /dev/null
@@ -1,92 +0,0 @@
-# 标准库
-import numpy as np
-# 第三方库
-from absl import app
-# dm_control 模块
-from dm_control import composer
-from dm_control.composer.variation import distributions
-from dm_control.locomotion.arenas import corridors as corr_arenas
-from dm_control.locomotion.tasks import corridors as corr_tasks
-from dm_control.locomotion.walkers import cmu_humanoid
-from dm_control import viewer
-
-# 全局环境变量,供策略函数访问
-global_env = None
-
-def create_environment_run_walls():
- """创建带墙壁走廊的人形机器人环境"""
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- arena = corr_arenas.WallsCorridor(
- wall_gap=4.,
- wall_width=distributions.Uniform(1, 7),
- wall_height=3.0,
- corridor_width=10,
- corridor_length=100,
- include_initial_padding=False)
-
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0),
- target_velocity=3.0,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(
- time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-def create_environment_run_gaps():
- "创建有空隙的走廊形竞技场环境"
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- arena = corr_arenas.GapsCorridor(
- platform_length=distributions.Uniform(.3, 2.5),
- gap_length=distributions.Uniform(.5, 1.25),
- corridor_width=10,
- corridor_length=100)
-
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0),
- target_velocity=3.0,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(
- time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-def random_policy(timestep):
- """随机动作策略:使用全局环境变量获取动作空间"""
- global global_env
- action_spec = global_env.action_spec()
- return np.random.uniform(
- low=action_spec.minimum,
- high=action_spec.maximum,
- size=action_spec.shape
- )
-
-
-def main(unused_argv):
- """主函数:初始化全局环境并启动viewer"""
- global global_env
- # 初始化环境并赋值给全局变量
- global_env = create_environment_run_gaps()
-
- # 启动viewer
- viewer.launch(
- environment_loader=create_environment_run_gaps,
- policy=random_policy
- )
-
-
-if __name__ == "__main__":
-
- app.run(main)
\ No newline at end of file
diff --git a/src/humanoid_motion_control/run_walls.py b/src/humanoid_motion_control/run_walls.py
deleted file mode 100644
index 08e22dfa62..0000000000
--- a/src/humanoid_motion_control/run_walls.py
+++ /dev/null
@@ -1,66 +0,0 @@
-# 标准库
-import numpy as np
-# 第三方库
-from absl import app
-# dm_control 模块
-from dm_control import composer
-from dm_control.composer.variation import distributions
-from dm_control.locomotion.arenas import corridors as corr_arenas
-from dm_control.locomotion.tasks import corridors as corr_tasks
-from dm_control.locomotion.walkers import cmu_humanoid
-from dm_control import viewer
-
-# 全局环境变量,供策略函数访问
-global_env = None
-
-def create_environment():
- """创建带墙壁走廊的人形机器人环境"""
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- arena = corr_arenas.WallsCorridor(
- wall_gap=4.,
- wall_width=distributions.Uniform(1, 7),
- wall_height=3.0,
- corridor_width=10,
- corridor_length=100,
- include_initial_padding=False)
-
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0),
- target_velocity=3.0,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(
- time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-def random_policy(timestep):
- """随机动作策略:使用全局环境变量获取动作空间"""
- global global_env
- action_spec = global_env.action_spec()
- return np.random.uniform(
- low=action_spec.minimum,
- high=action_spec.maximum,
- size=action_spec.shape
- )
-
-def main(unused_argv):
- """主函数:初始化全局环境并启动viewer"""
- global global_env
- # 初始化环境并赋值给全局变量
- global_env = create_environment()
-
- # 启动viewer
- viewer.launch(
- environment_loader=create_environment,
- policy=random_policy
- )
-
-
-if __name__ == "__main__":
- app.run(main)
\ No newline at end of file
From 5ccfa0029745d3fea04286ef8eb525b1a167ef60 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 20 Oct 2025 10:09:59 +0800
Subject: [PATCH 19/45] =?UTF-8?q?=E5=88=A0=E9=99=A4=20main=20=E5=88=86?=
=?UTF-8?q?=E6=94=AF=E6=AE=8B=E7=95=99=E7=9A=84=E6=97=A7=E7=9B=AE=E5=BD=95?=
=?UTF-8?q?=20humanoid=5Fmotion=5Fcontrol?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_motion_control/README.md | 108 --------
src/humanoid_motion_control/humanoid.xml | 252 -------------------
src/humanoid_motion_control/main_humanoid.py | 44 ----
src/humanoid_motion_control/run_walls.py | 61 -----
4 files changed, 465 deletions(-)
delete mode 100644 src/humanoid_motion_control/README.md
delete mode 100644 src/humanoid_motion_control/humanoid.xml
delete mode 100644 src/humanoid_motion_control/main_humanoid.py
delete mode 100644 src/humanoid_motion_control/run_walls.py
diff --git a/src/humanoid_motion_control/README.md b/src/humanoid_motion_control/README.md
deleted file mode 100644
index 7eee403936..0000000000
--- a/src/humanoid_motion_control/README.md
+++ /dev/null
@@ -1,108 +0,0 @@
--
-
- # 人形机器人走廊奔跑仿真(基于dm_control)
-
- ## 项目概述
- 本项目核心为基于**dm_control框架**构建的人形机器人走廊奔跑仿真环境。dm_control是DeepMind基于MuJoCo物理引擎开发的高级仿真工具,简化了强化学习环境的搭建、任务定义与可视化流程。本项目中,CMU人形机器人需在带随机墙壁的走廊中以目标速度奔跑,展示了dm_control在复杂运动任务仿真中的应用。
-
- ## 环境准备
- ### 依赖库
- 需安装以下核心依赖(含dm_control及其底层依赖):
- ```bash
- pip install mujoco dm-control absl-py
- ```
- - `dm_control`:提供环境构建、任务定义和交互可视化功能
- - `mujoco`:dm_control依赖的物理引擎核心
- - `absl-py`:用于命令行参数处理
-
- ## 项目结构
- | 文件名 | 功能描述 |
- | ------------------ | ------------------------------------------------------------ |
- | `run_walls.py` | **核心文件**:基于dm_control构建带墙壁的走廊环境,实现CMU人形机器人奔跑任务 |
- | `humanoid.xml` | MJCF格式模型文件,定义机器人结构(辅助文件) |
- | `main_humanoid.py` | 软体机器人弹跳仿真程序 |
-
- ## 核心功能:走廊奔跑环境(run_walls.py)
- ### 1. dm_control框架组件应用
- ```python
- # dm_control核心模块使用
- from dm_control import composer # 环境组合框架
- from dm_control.locomotion.arenas import corridors # 走廊场景
- from dm_control.locomotion.tasks import corridors # 走廊任务
- from dm_control.locomotion.walkers import cmu_humanoid # 机器人模型
- from dm_control import viewer # 可视化工具
- ```
-
- ### 2. 环境构建流程
- #### (1)机器人配置
- 创建位置控制的CMU人形机器人,启用第一视角相机观测:
- ```python
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)}
- )
- ```
-
- #### (2)走廊场景设计
- 通过`WallsCorridor`类构建带随机墙壁的动态环境:
- ```python
- arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁间隙宽度(机器人需穿过的空间)
- wall_width=distributions.Uniform(1, 7), # 墙壁宽度随机化(1-7单位)
- wall_height=3.0, # 防止翻越的墙壁高度
- corridor_width=10, # 走廊总宽度
- corridor_length=100, # 走廊总长度
- include_initial_padding=False # 无初始空白区域
- )
- ```
-
- #### (3)任务定义
- 通过`RunThroughCorridor`类定义奔跑任务规则:
- ```python
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0), # 初始位置
- target_velocity=3.0, # 目标速度(影响奖励计算)
- physics_timestep=0.005, # 物理仿真步长
- control_timestep=0.03 # 控制决策步长(≈33Hz)
- )
- ```
-
- #### (4)环境封装
- 组合场景与任务,生成强化学习可用环境:
- ```python
- environment = composer.Environment(
- time_limit=30, # 单次仿真时长30秒
- task=task,
- strip_singleton_obs_buffer_dim=True # 简化观测数据结构
- )
- ```
-
- ## 使用方法
- 1. 运行走廊奔跑仿真:
- ```bash
- python run_walls.py
- ```
- 2. 交互操作:
- - 可视化窗口展示机器人第一视角和全局视角
- - 按`Ctrl+C`终止仿真
-
- ## dm_control核心特性体现
- 1. **模块化设计**:通过`composer`模块分离机器人(walker)、场景(arena)和任务(task),便于独立修改
- 2. **随机性支持**:使用`distributions`生成随机墙壁宽度,增强环境多样性
- 3. **灵活观测配置**:通过`observable_options`轻松启用第一视角相机等观测项
- 4. **高效可视化**:`viewer`模块提供实时交互界面,支持视角切换与状态查看
-
- ## 参数调整指南
- | 参数 | 调整范围 | 效果说明 |
- | ------------------ | -------- | -------------------------------- |
- | `wall_gap` | 2.0~6.0 | 减小值增加穿过难度 |
- | `target_velocity` | 1.0~5.0 | 提高值要求机器人跑得更快 |
- | `control_timestep` | 0.01~0.1 | 减小值提高控制精度(增加计算量) |
-
- ## 参考资料
- - [dm_control官方文档](https://github.com/deepmind/dm_control)
- - [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/)
- - [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
-
-
diff --git a/src/humanoid_motion_control/humanoid.xml b/src/humanoid_motion_control/humanoid.xml
deleted file mode 100644
index 38b07c0342..0000000000
--- a/src/humanoid_motion_control/humanoid.xml
+++ /dev/null
@@ -1,252 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/humanoid_motion_control/main_humanoid.py b/src/humanoid_motion_control/main_humanoid.py
deleted file mode 100644
index 2982d10461..0000000000
--- a/src/humanoid_motion_control/main_humanoid.py
+++ /dev/null
@@ -1,44 +0,0 @@
-# 标准库
-import time
-# 第三方库
-import mujoco
-from mujoco import viewer
-
-def main():
- try:
- model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
- except Exception as e:
- print(f"模型加载失败: {e}")
- return
-
- data = mujoco.MjData(model)
- # 临时数据用于获取目标关键帧(站立姿势,索引1)
- target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1)
- target_qpos = target_data.qpos.copy()
-
- # 初始姿势设为深蹲(索引0),从蹲下开始站起'+
- mujoco.mj_resetDataKeyframe(model, data, 0)
-
- # 控制参数(软体机器人需较小增益,避免震荡)
- Kp = 5.0 # 比例增益(根据软体特性调小)
-
- with viewer.launch_passive(model, data) as v:
- try:
- while True:
- mujoco.mj_step(model, data)
-
- # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
- qpos_error = target_qpos[7:] - data.qpos[7:]
- # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
- data.ctrl[:] = Kp * qpos_error
-
- print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
-
- v.sync()
- time.sleep(0.005)
- except KeyboardInterrupt:
- print("\n正在退出模拟....")
-
-if __name__ == "__main__":
- main()
diff --git a/src/humanoid_motion_control/run_walls.py b/src/humanoid_motion_control/run_walls.py
deleted file mode 100644
index b187011a84..0000000000
--- a/src/humanoid_motion_control/run_walls.py
+++ /dev/null
@@ -1,61 +0,0 @@
-# 导入dm_control的核心模块
-from dm_control import composer # 用于组合环境、实体和任务的框架
-from dm_control.composer.variation import distributions # 用于生成随机分布的工具(如随机墙壁宽度)
-from dm_control.locomotion.arenas import corridors as corr_arenas # 走廊类竞技场定义
-from dm_control.locomotion.tasks import corridors as corr_tasks # 走廊相关任务定义
-from dm_control.locomotion.walkers import cmu_humanoid # CMU人形机器人模型
-
-# 导入其他辅助模块
-from absl import app # Google的命令行参数处理库
-from dm_control import viewer # 用于可视化仿真的交互窗口
-
-
-def cmu_humanoid_run_walls(random_state=None):
- """创建一个CMU人形机器人在带墙壁的走廊中奔跑的环境
-
- Args:
- random_state: 随机数生成器状态,用于保证实验可复现性
- Returns:
- composer.Environment: 构建好的强化学习环境
- """
-
- # 创建一个位置控制的CMU人形机器人
- # observable_options配置观测项:启用第一视角相机(egocentric_camera)
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- # 建造带墙壁的走廊竞技场
- arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁之间的间隙(机器人需要穿过的宽度)
- wall_width=distributions.Uniform(1, 7), # 墙壁宽度为1到7之间的随机值(增加环境多样性)
- wall_height=3.0, # 墙壁高度(确保机器人无法翻越)
- corridor_width=10, # 走廊总宽度
- corridor_length=100, # 走廊总长度
- include_initial_padding=False) # 不添加初始空白区域(机器人直接从起点开始)
-
- # 定义"穿过走廊"任务:奖励机器人快速通过走廊
- task = corr_tasks.RunThroughCorridor(
- walker=walker, # 绑定前面创建的机器人
- arena=arena, # 绑定前面创建的走廊环境
- walker_spawn_position=(0.5, 0, 0), # 机器人初始 spawn 位置(走廊起点附近)
- target_velocity=3.0, # 目标速度(机器人需要达到的速度以获得高奖励)
- physics_timestep=0.005, # 物理仿真时间步长(越小精度越高,计算量越大)
- control_timestep=0.03) # 控制信号时间步长(机器人决策的频率,0.03秒约33Hz)
-
- # 创建强化学习环境
- return composer.Environment(
- time_limit=30, # 单次仿真最大时长30秒
- task=task, # 绑定任务
- random_state=random_state, # 传入随机状态(保证可复现)
- strip_singleton_obs_buffer_dim=True) # 去除观测数据中多余的单元素维度(简化数据结构)
-
-
-def main(unused_argv):
- """主函数:启动可视化窗口运行环境"""
- # 启动dm_control的交互 viewer,加载上面定义的环境
- viewer.launch(environment_loader=cmu_humanoid_run_walls)
-
-
-if __name__ == '__main__':
- # 使用absl的app.run启动程序(处理命令行参数并调用main函数)
- app.run(main)
\ No newline at end of file
From 30b9bd75bc102a7a2a51dcc93135b20f12619e6d Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 20 Oct 2025 11:08:36 +0800
Subject: [PATCH 20/45] =?UTF-8?q?=E5=88=A0=E9=99=A4FETCH=5FHEAD=E5=92=8Cgi?=
=?UTF-8?q?t=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
FETCH_HEAD | 0
git | 0
2 files changed, 0 insertions(+), 0 deletions(-)
delete mode 100644 FETCH_HEAD
delete mode 100644 git
diff --git a/FETCH_HEAD b/FETCH_HEAD
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/git b/git
deleted file mode 100644
index e69de29bb2..0000000000
From 2d86570ecae20dbac273dcf1ecd884eb839cf153 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 20 Oct 2025 11:28:29 +0800
Subject: [PATCH 21/45] =?UTF-8?q?=E5=AE=8C=E5=96=84README.md=E8=AF=B4?=
=?UTF-8?q?=E6=98=8E=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../README.md | 204 +++++++++---------
1 file changed, 100 insertions(+), 104 deletions(-)
diff --git a/src/humanoid_Simulation_Environment_Establish/README.md b/src/humanoid_Simulation_Environment_Establish/README.md
index 4880ebe4e5..740b6c72a8 100644
--- a/src/humanoid_Simulation_Environment_Establish/README.md
+++ b/src/humanoid_Simulation_Environment_Establish/README.md
@@ -1,106 +1,102 @@
+# 人形机器人仿真环境(基于MuJoCo与dm_control)
- # 人形机器人走廊奔跑仿真(基于dm_control)
-
- ## 项目概述
- 本项目核心为基于**dm_control框架**构建的人形机器人走廊奔跑仿真环境。dm_control是DeepMind基于MuJoCo物理引擎开发的高级仿真工具,简化了强化学习环境的搭建、任务定义与可视化流程。本项目中,CMU人形机器人需在带随机墙壁的走廊中以目标速度奔跑,展示了dm_control在复杂运动任务仿真中的应用。
-
- ## 环境准备
- ### 依赖库
- 需安装以下核心依赖(含dm_control及其底层依赖):
- ```bash
- pip install mujoco dm-control absl-py
- ```
- - `dm_control`:提供环境构建、任务定义和交互可视化功能
- - `mujoco`:dm_control依赖的物理引擎核心
- - `absl-py`:用于命令行参数处理
-
- ## 项目结构
- | 文件名 | 功能描述 |
- | ------------------ | ------------------------------------------------------------ |
- | `run_walls.py` | **核心文件**:基于dm_control构建带墙壁的走廊环境,实现CMU人形机器人奔跑任务 |
- | `humanoid.xml` | MJCF格式模型文件,定义机器人结构(辅助文件) |
- | `main_humanoid.py` | 软体机器人弹跳仿真程序 |
-
- ## 核心功能:走廊奔跑环境(run_walls.py)
- ### 1. dm_control框架组件应用
- ```python
- # dm_control核心模块使用
- from dm_control import composer # 环境组合框架
- from dm_control.locomotion.arenas import corridors # 走廊场景
- from dm_control.locomotion.tasks import corridors # 走廊任务
- from dm_control.locomotion.walkers import cmu_humanoid # 机器人模型
- from dm_control import viewer # 可视化工具
- ```
-
- ### 2. 环境构建流程
- #### (1)机器人配置
- 创建位置控制的CMU人形机器人,启用第一视角相机观测:
- ```python
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)}
- )
- ```
-
- #### (2)走廊场景设计
- 通过`WallsCorridor`类构建带随机墙壁的动态环境:
- ```python
- arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁间隙宽度(机器人需穿过的空间)
- wall_width=distributions.Uniform(1, 7), # 墙壁宽度随机化(1-7单位)
- wall_height=3.0, # 防止翻越的墙壁高度
- corridor_width=10, # 走廊总宽度
- corridor_length=100, # 走廊总长度
- include_initial_padding=False # 无初始空白区域
- )
- ```
-
- #### (3)任务定义
- 通过`RunThroughCorridor`类定义奔跑任务规则:
- ```python
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0), # 初始位置
- target_velocity=3.0, # 目标速度(影响奖励计算)
- physics_timestep=0.005, # 物理仿真步长
- control_timestep=0.03 # 控制决策步长(≈33Hz)
- )
- ```
-
- #### (4)环境封装
- 组合场景与任务,生成强化学习可用环境:
- ```python
- environment = composer.Environment(
- time_limit=30, # 单次仿真时长30秒
- task=task,
- strip_singleton_obs_buffer_dim=True # 简化观测数据结构
- )
- ```
-
- ## 使用方法
- 1. 运行走廊奔跑仿真:
- ```bash
- python run_walls.py
- ```
- 2. 交互操作:
- - 可视化窗口展示机器人第一视角和全局视角
- - 按`Ctrl+C`终止仿真
-
- ## dm_control核心特性体现
- 1. **模块化设计**:通过`composer`模块分离机器人(walker)、场景(arena)和任务(task),便于独立修改
- 2. **随机性支持**:使用`distributions`生成随机墙壁宽度,增强环境多样性
- 3. **灵活观测配置**:通过`observable_options`轻松启用第一视角相机等观测项
- 4. **高效可视化**:`viewer`模块提供实时交互界面,支持视角切换与状态查看
-
- ## 参数调整指南
- | 参数 | 调整范围 | 效果说明 |
- | ------------------ | -------- | -------------------------------- |
- | `wall_gap` | 2.0~6.0 | 减小值增加穿过难度 |
- | `target_velocity` | 1.0~5.0 | 提高值要求机器人跑得更快 |
- | `control_timestep` | 0.01~0.1 | 减小值提高控制精度(增加计算量) |
-
- ## 参考资料
- - [dm_control官方文档](https://github.com/deepmind/dm_control)
- - [MuJoCo物理引擎文档](https://mujoco.readthedocs.io/)
- - [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
+## 项目概述
+本项目构建了一系列人形机器人仿真环境,基于MuJoCo物理引擎和dm_control框架,支持机器人运动控制、走廊导航、目标追踪等多种任务。通过模块化设计,实现了机器人模型加载、环境配置、控制策略与可视化展示的完整流程,适用于机器人控制算法研究与验证。
+## 环境准备
+
+### 依赖库安装
+```bash
+pip install mujoco dm-control absl-py numpy
+```
+- `mujoco`:物理引擎核心,负责动力学计算
+- `dm-control`:DeepMind开发的高级仿真框架,简化环境与任务构建
+- `absl-py`:命令行参数处理工具
+- `numpy`:数值计算支持
+
+## 项目结构
+
+| 文件名 | 功能描述 |
+|-------------------|--------------------------------------------------------------|
+| `main.py` | 核心控制程序,实现工作目录自动切换、模型加载与基础运动控制 |
+| `main_humanoid.py`| 软体机器人弹跳仿真程序,基于比例控制实现从蹲姿到站姿的运动 |
+| `run_walls.py` | 带随机墙壁的走廊奔跑环境,使用dm_control构建强化学习任务 |
+| `motion_main.py` | 扩展运动环境,支持墙壁走廊和间隙走廊两种场景切换 |
+| `go_to_target.py` | 目标点追踪任务环境,实现机器人向指定目标位置移动的仿真 |
+| `humanoid.xml` | MJCF格式机器人模型文件,定义人形机器人的结构、关节与执行器 |
+| `README.md` | 项目说明文档 |
+
+## 核心功能
+
+### 1. 基础运动控制(main.py & main_humanoid.py)
+- 自动路径处理:程序启动时自动切换到项目根目录,确保相对路径正确
+- 模型加载验证:检查并加载人形机器人模型,提供清晰的加载状态反馈
+- 关键帧控制:基于预定义的"深蹲"和"站立"关键帧,通过比例控制实现平滑运动过渡
+- 实时可视化:使用MuJoCo viewer展示机器人运动状态,输出时间与躯干高度等关键信息
+
+```python
+# 核心控制逻辑示例
+qpos_error = target_qpos[7:] - data.qpos[7:] # 计算关节位置误差
+data.ctrl[:] = Kp * qpos_error # 比例控制输出
+```
+
+### 2. 走廊奔跑环境(run_walls.py & motion_main.py)
+- 动态场景生成:支持两种走廊类型(带墙壁/带间隙),墙壁宽度和间隙长度可随机化
+- 任务定义:设定目标速度(3.0单位/秒),通过奖励函数引导机器人高效移动
+- 多视角观测:支持第一视角相机和全局视角,便于观察与调试
+- 参数可配置:走廊长度、宽度、墙壁高度等参数可灵活调整
+
+```python
+# 走廊环境配置示例
+arena = corr_arenas.WallsCorridor(
+ wall_gap=4., # 墙壁间隙宽度
+ wall_width=distributions.Uniform(1, 7), # 随机墙壁宽度
+ wall_height=3.0, # 墙壁高度
+ corridor_length=100 # 走廊总长度
+)
+```
+
+### 3. 目标追踪任务(go_to_target.py)
+- 目标点导航:在平面环境中随机生成目标点,机器人需移动至目标位置
+- 灵活的时间限制:单次仿真时长可配置(默认30秒)
+- 兼容强化学习:环境接口符合强化学习标准,可直接用于训练策略
+
+## 使用方法
+
+1. 基础运动控制仿真:
+```bash
+python main.py
+```
+
+2. 走廊奔跑仿真(带墙壁):
+```bash
+python run_walls.py
+```
+
+3. 间隙走廊仿真:
+```bash
+python motion_main.py
+```
+
+4. 目标追踪仿真:
+```bash
+python go_to_target.py
+```
+
+### 交互操作
+- 仿真窗口支持视角旋转、缩放和平移
+- 按`Ctrl+C`终止仿真程序
+
+## 参数调整指南
+
+| 参数 | 调整范围 | 效果说明 |
+|-------------------|----------|----------------------------------|
+| `Kp`(比例增益) | 1.0~10.0 | 增大会加快响应速度,过大会导致震荡 |
+| `wall_gap` | 2.0~6.0 | 减小值增加走廊穿过难度 |
+| `target_velocity` | 1.0~5.0 | 提高值要求机器人运动速度更快 |
+| `control_timestep`| 0.01~0.1 | 减小值提高控制精度(增加计算量) |
+
+## 参考资料
+- [MuJoCo官方文档](https://mujoco.readthedocs.io/)
+- [dm_control框架文档](https://github.com/deepmind/dm_control)
+- [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
\ No newline at end of file
From e2cda972fec0a1a34128663d4892576e201a035d Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 21 Oct 2025 17:37:45 +0800
Subject: [PATCH 22/45] =?UTF-8?q?=E5=B0=86main.py=E6=B7=BB=E5=8A=A0ROS?=
=?UTF-8?q?=E8=8A=82=E7=82=B9=E5=B9=B6=E5=88=9D=E5=A7=8B=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
---
.../main.py | 122 ++++++------------
1 file changed, 40 insertions(+), 82 deletions(-)
diff --git a/src/humanoid_Simulation_Environment_Establish/main.py b/src/humanoid_Simulation_Environment_Establish/main.py
index 1f8ea19308..c581703a9a 100644
--- a/src/humanoid_Simulation_Environment_Establish/main.py
+++ b/src/humanoid_Simulation_Environment_Establish/main.py
@@ -1,96 +1,54 @@
-"""
-[路径优化] 自动切换工作目录并加载模型
-
-为了增强代码的可移植性,我们在程序启动时自动将工作目录切换到项目根目录。
-这样可以确保脚本中使用的相对路径(如 "src/humanoid_motion_control/humanoid.xml")
-无论从哪个位置运行,都能正确地指向预期的文件。
-
-修改原因:
-- 原版代码使用的是基于运行时目录的相对路径,这在不同环境或不同运行方式下容易出错。
-- 通过自动切换到项目根目录,可以统一所有相对路径的基准,避免 "No such file or directory" 错误。
-
-使用方法:
-- 保持 main.py 中原有相对路径写法不变即可。
-- 此脚本会在加载模型等操作前自动完成路径切换。
-
-注意事项:
-- 此代码段必须放置在所有依赖相对路径的导入或文件操作之前。
-- 如果项目目录结构发生重大改变,可能需要调整计算 project_root 的层级。
-"""
-
-import os
import mujoco
+from mujoco import viewer
+import numpy as np
+import rospy
+import rospkg # 用于获取ROS包路径
-# --- 你之前的切换目录代码 ---
-import sys
-file_path = os.path.abspath(__file__)
-project_root = os.path.dirname(os.path.dirname(file_path))
-os.chdir(project_root)
-print(f"当前工作目录已切换到: {os.getcwd()}")
-# ----------------------------
-
-# 1. 定义模型文件的相对路径
-# 注意:这里使用了推荐的正斜杠
-relative_model_path = "src/humanoid_motion_control/humanoid.xml"
-print(f"尝试查找的模型路径: {relative_model_path}")
+def main():
+ # 初始化ROS节点(anonymous=True确保节点名唯一)
+ rospy.init_node('humanoid_main_node', anonymous=True)
+ rospy.loginfo("人形机器人核心控制节点启动")
-# 2. 检查文件是否存在
-if os.path.exists(relative_model_path):
- print("✅ 模型文件找到了!")
+ # 通过rospkg获取模型路径(避免相对路径问题)
try:
- # 3. 如果文件存在,再尝试加载
- model = mujoco.MjModel.from_xml_path(relative_model_path)
- data = mujoco.MjData(model)
- print("🎉 模型加载成功!")
- # ... 你的其他代码 ...
+ rospack = rospkg.RosPack()
+ pkg_path = rospack.get_path('humanoid_motion') # 替换为你的功能包名
+ model_path = f"{pkg_path}/xml/humanoid.xml" # 模型相对路径
except Exception as e:
- print(f"模型加载失败: {e}")
-else:
- print(f"❌ 错误:在当前工作目录下找不到模型文件。")
- print(f"请确认文件 '{relative_model_path}' 确实存在。")
-
-
-# 标准库
-import time
-# 第三方库
-import mujoco
-from mujoco import viewer
+ rospy.logerr(f"获取包路径失败: {e}")
+ return
-def main():
+ # 加载MuJoCo模型
try:
- model = mujoco.MjModel.from_xml_path(r"humanoid_motion_control\humanoid.xml")
+ model = mujoco.MjModel.from_xml_path(model_path)
+ data = mujoco.MjData(model)
+ rospy.loginfo("模型加载成功")
except Exception as e:
- print(f"模型加载失败: {e}")
+ rospy.logerr(f"模型加载失败: {e}")
return
-
- data = mujoco.MjData(model)
- # 临时数据用于获取目标关键帧(站立姿势,索引1)
- target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1)
- target_qpos = target_data.qpos.copy()
-
- # 初始姿势设为深蹲(索引0),从蹲下开始站起
- mujoco.mj_resetDataKeyframe(model, data, 0)
-
- # 控制参数(软体机器人需较小增益,避免震荡)
- Kp = 5.0 # 比例增益(根据软体特性调小)
-
- with viewer.launch_passive(model, data) as v:
+
+ # 设置控制频率(200Hz,根据仿真需求调整)
+ control_rate = rospy.Rate(200)
+
+ # 启动仿真可视化
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ rospy.loginfo("仿真可视化启动")
try:
- while True:
- mujoco.mj_step(model, data)
-
- # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
- qpos_error = target_qpos[7:] - data.qpos[7:]
- # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
- data.ctrl[:] = Kp * qpos_error
-
- print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
-
- v.sync()
- time.sleep(0.005)
+ # ROS循环(直到节点关闭)
+ while not rospy.is_shutdown():
+ # 核心控制逻辑(示例:简单关节控制)
+ mujoco.mj_step(model, data) # 单步仿真
+ data.ctrl[:] = 0.1 * np.sin(data.time) # 示例:正弦波控制关节
+
+ # 同步可视化
+ viewer.sync()
+ # 控制循环频率
+ control_rate.sleep()
+
except KeyboardInterrupt:
- print("\n正在退出模拟....")
+ rospy.loginfo("用户中断,停止仿真")
+ finally:
+ rospy.loginfo("核心控制节点关闭")
if __name__ == "__main__":
main()
\ No newline at end of file
From 38d80f93d03cb67597fa310fc4f32ee7e9357ffa Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 27 Oct 2025 10:40:23 +0800
Subject: [PATCH 23/45] =?UTF-8?q?=E4=B8=BAmain.py=E6=B7=BB=E5=8A=A0launch?=
=?UTF-8?q?=E5=90=AF=E5=8A=A8=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.launch | 10 ++++++++++
1 file changed, 10 insertions(+)
create mode 100644 src/humanoid_Simulation_Environment_Establish/main.launch
diff --git a/src/humanoid_Simulation_Environment_Establish/main.launch b/src/humanoid_Simulation_Environment_Establish/main.launch
new file mode 100644
index 0000000000..bc868933e9
--- /dev/null
+++ b/src/humanoid_Simulation_Environment_Establish/main.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
\ No newline at end of file
From 1ea7ed7ff2c5df9c31a489fdfcd8446dccec0784 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 28 Oct 2025 14:03:32 +0800
Subject: [PATCH 24/45] =?UTF-8?q?=E4=B8=BAmain.launch=E4=B8=AD=E8=B7=AF?=
=?UTF-8?q?=E5=BE=84=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_Simulation_Environment_Establish/main.launch | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/humanoid_Simulation_Environment_Establish/main.launch b/src/humanoid_Simulation_Environment_Establish/main.launch
index bc868933e9..ea462c9807 100644
--- a/src/humanoid_Simulation_Environment_Establish/main.launch
+++ b/src/humanoid_Simulation_Environment_Establish/main.launch
@@ -3,8 +3,8 @@
pkg="humanoid_motion"
type="main.py"
output="screen"
- args="$(find humanoid_motion)/">
-
+ args="$(find humanoid_Simulation_Environment_Establish)/">
+
\ No newline at end of file
From ce34b58a31b6666629f88820bd45b0d099400612 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 28 Oct 2025 18:26:24 +0800
Subject: [PATCH 25/45] =?UTF-8?q?=E4=B8=BAmain=5Fhumanoid.py=E6=B7=BB?=
=?UTF-8?q?=E5=8A=A0ROS=E8=8A=82=E7=82=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main_humanoid.py | 77 ++++++++++++-------
1 file changed, 49 insertions(+), 28 deletions(-)
diff --git a/src/humanoid_Simulation_Environment_Establish/main_humanoid.py b/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
index 2982d10461..3678b88a75 100644
--- a/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
+++ b/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
@@ -1,44 +1,65 @@
# 标准库
-import time
+import rospy
+import rospkg # 用于获取ROS包路径
# 第三方库
import mujoco
-from mujoco import viewer
+from mujoco import viewer
def main():
+
+ rospy.init_node('humanoid_standup_node', anonymous=True)
+ rospy.loginfo("人形机器人起身控制节点启动")
+
+ kp_gain = rospy.get_param('~kp_gain', 5.0)
+ model_rel_path = rospy.get_param('~model_path', 'xml/humanoid.xml')
+
try:
- model = mujoco.MjModel.from_xml_path("src\humanoid_motion_control\humanoid.xml")
+ rospack = rospkg.RosPack()
+ pkg_path = rospack.get_path('humanoid_motion')
+ model_path = f"{pkg_path}/{model_rel_path}"
+ rospy.loginfo(f"模型路径: {model_path}")
except Exception as e:
- print(f"模型加载失败: {e}")
+ rospy.logerr(f"获取包路径失败: {e}")
+ return
+
+ try:
+ model = mujoco.MjModel.from_xml_path(model_path)
+ data = mujoco.MjData(model)
+ rospy.loginfo("模型加载成功")
+ except Exception as e:
+ rospy.logerr(f"模型加载失败: {e}")
return
- data = mujoco.MjData(model)
- # 临时数据用于获取目标关键帧(站立姿势,索引1)
target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1)
- target_qpos = target_data.qpos.copy()
-
- # 初始姿势设为深蹲(索引0),从蹲下开始站起'+
- mujoco.mj_resetDataKeyframe(model, data, 0)
-
- # 控制参数(软体机器人需较小增益,避免震荡)
- Kp = 5.0 # 比例增益(根据软体特性调小)
-
+ mujoco.mj_resetDataKeyframe(model, target_data, 1)
+ target_qpos = target_data.qpos.copy()
+ mujoco.mj_resetDataKeyframe(model, data, 0)
+
+ control_rate = rospy.Rate(200)
+
with viewer.launch_passive(model, data) as v:
+ rospy.loginfo("开始起身控制(按Ctrl+C停止)")
try:
- while True:
- mujoco.mj_step(model, data)
-
- # 计算关节位置误差(跳过前7个根关节,只控制电机关节)
+
+ while not rospy.is_shutdown():
+ mujoco.mj_step(model, data)
+
qpos_error = target_qpos[7:] - data.qpos[7:]
- # 比例控制:控制信号与误差成正比(适配软体机器人的柔性特性)
- data.ctrl[:] = Kp * qpos_error
-
- print(f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
-
- v.sync()
- time.sleep(0.005)
+ data.ctrl[:] = kp_gain * qpos_error
+
+ rospy.loginfo_throttle(1,
+ f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
+
+ v.sync()
+ control_rate.sleep()
+
except KeyboardInterrupt:
- print("\n正在退出模拟....")
+ rospy.loginfo("用户中断,停止控制")
+ finally:
+ rospy.loginfo("起身控制节点关闭")
if __name__ == "__main__":
- main()
+ try:
+ main()
+ except rospy.ROSInterruptException:
+ pass
\ No newline at end of file
From b028a355d3d3aad8d0c4a8031bd6d5119379589c Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Sun, 2 Nov 2025 17:04:27 +0800
Subject: [PATCH 26/45] =?UTF-8?q?=E4=B8=BAmain=5Fhumanoid.py=E6=96=87?=
=?UTF-8?q?=E4=BB=B6=E6=B7=BB=E5=8A=A0=E8=AF=A6=E7=BB=86=E6=B3=A8=E9=87=8A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main_humanoid.py | 36 +++++++++++--------
1 file changed, 22 insertions(+), 14 deletions(-)
diff --git a/src/humanoid_Simulation_Environment_Establish/main_humanoid.py b/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
index 3678b88a75..25795b5d9b 100644
--- a/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
+++ b/src/humanoid_Simulation_Environment_Establish/main_humanoid.py
@@ -3,25 +3,28 @@
import rospkg # 用于获取ROS包路径
# 第三方库
import mujoco
-from mujoco import viewer
+from mujoco import viewer # 注意:MuJoCo 2.x版本支持,3.x需用mujoco_viewer
def main():
-
+ # 初始化ROS节点
rospy.init_node('humanoid_standup_node', anonymous=True)
rospy.loginfo("人形机器人起身控制节点启动")
- kp_gain = rospy.get_param('~kp_gain', 5.0)
- model_rel_path = rospy.get_param('~model_path', 'xml/humanoid.xml')
+ # 从ROS参数服务器获取配置(可通过launch文件修改)
+ kp_gain = rospy.get_param('~kp_gain', 5.0) # 比例增益
+ model_rel_path = rospy.get_param('~model_path', 'xml/humanoid.xml') # 模型相对路径
+ # 获取模型绝对路径(解决相对路径问题)
try:
rospack = rospkg.RosPack()
- pkg_path = rospack.get_path('humanoid_motion')
- model_path = f"{pkg_path}/{model_rel_path}"
+ pkg_path = rospack.get_path('humanoid_motion') # 功能包路径
+ model_path = f"{pkg_path}/{model_rel_path}" # 拼接绝对路径
rospy.loginfo(f"模型路径: {model_path}")
except Exception as e:
rospy.logerr(f"获取包路径失败: {e}")
return
+ # 加载MuJoCo模型
try:
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)
@@ -29,29 +32,34 @@ def main():
except Exception as e:
rospy.logerr(f"模型加载失败: {e}")
return
-
+
+ # 初始化目标姿势(站立,关键帧1)和初始姿势(深蹲,关键帧0)
target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1)
+ mujoco.mj_resetDataKeyframe(model, target_data, 1) # 站立关键帧
target_qpos = target_data.qpos.copy()
- mujoco.mj_resetDataKeyframe(model, data, 0)
+ mujoco.mj_resetDataKeyframe(model, data, 0) # 初始深蹲姿势
+ # 设置控制频率(200Hz,与原time.sleep(0.005)对应)
control_rate = rospy.Rate(200)
+ # 启动可视化
with viewer.launch_passive(model, data) as v:
rospy.loginfo("开始起身控制(按Ctrl+C停止)")
try:
-
+ # ROS循环(直到节点关闭)
while not rospy.is_shutdown():
- mujoco.mj_step(model, data)
+ mujoco.mj_step(model, data) # 单步仿真
+ # 比例控制(只控制电机关节,跳过前7个根关节)
qpos_error = target_qpos[7:] - data.qpos[7:]
data.ctrl[:] = kp_gain * qpos_error
- rospy.loginfo_throttle(1,
+ # 输出躯干高度(ROS日志)
+ rospy.loginfo_throttle(1, # 每1秒输出一次,避免刷屏
f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
- v.sync()
- control_rate.sleep()
+ v.sync() # 同步可视化
+ control_rate.sleep() # 控制循环频率
except KeyboardInterrupt:
rospy.loginfo("用户中断,停止控制")
From 272b96a45c4d1acd3c9d2fc1ceaf54cd643fa59e Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 3 Nov 2025 09:20:31 +0800
Subject: [PATCH 27/45] =?UTF-8?q?=E8=A7=84=E8=8C=83=E6=A8=A1=E5=9D=97?=
=?UTF-8?q?=E5=90=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/driverless/RADME.md | 2 +-
.../README.md | 0
.../go_to_target.py | 0
.../humanoid.xml | 0
.../main.launch | 0
.../main.py | 0
.../main_humanoid.py | 0
.../motion_main.py | 0
.../run_walls.py | 0
src/optimized_details.md | 68 +++++++++----------
10 files changed, 35 insertions(+), 35 deletions(-)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/README.md (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/go_to_target.py (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/humanoid.xml (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/main.launch (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/main.py (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/main_humanoid.py (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/motion_main.py (100%)
rename src/{humanoid_Simulation_Environment_Establish => humanoid_simulation_environment_establish}/run_walls.py (100%)
diff --git a/src/driverless/RADME.md b/src/driverless/RADME.md
index bc9c091dc6..d82241b591 100644
--- a/src/driverless/RADME.md
+++ b/src/driverless/RADME.md
@@ -1 +1 @@
-我的选题是无人车控制中的基于mpc的车辆转向控制器。
+我的选题是无人车控制中的基于mpc的车辆转向控制器。
diff --git a/src/humanoid_Simulation_Environment_Establish/README.md b/src/humanoid_simulation_environment_establish/README.md
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/README.md
rename to src/humanoid_simulation_environment_establish/README.md
diff --git a/src/humanoid_Simulation_Environment_Establish/go_to_target.py b/src/humanoid_simulation_environment_establish/go_to_target.py
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/go_to_target.py
rename to src/humanoid_simulation_environment_establish/go_to_target.py
diff --git a/src/humanoid_Simulation_Environment_Establish/humanoid.xml b/src/humanoid_simulation_environment_establish/humanoid.xml
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/humanoid.xml
rename to src/humanoid_simulation_environment_establish/humanoid.xml
diff --git a/src/humanoid_Simulation_Environment_Establish/main.launch b/src/humanoid_simulation_environment_establish/main.launch
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/main.launch
rename to src/humanoid_simulation_environment_establish/main.launch
diff --git a/src/humanoid_Simulation_Environment_Establish/main.py b/src/humanoid_simulation_environment_establish/main.py
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/main.py
rename to src/humanoid_simulation_environment_establish/main.py
diff --git a/src/humanoid_Simulation_Environment_Establish/main_humanoid.py b/src/humanoid_simulation_environment_establish/main_humanoid.py
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/main_humanoid.py
rename to src/humanoid_simulation_environment_establish/main_humanoid.py
diff --git a/src/humanoid_Simulation_Environment_Establish/motion_main.py b/src/humanoid_simulation_environment_establish/motion_main.py
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/motion_main.py
rename to src/humanoid_simulation_environment_establish/motion_main.py
diff --git a/src/humanoid_Simulation_Environment_Establish/run_walls.py b/src/humanoid_simulation_environment_establish/run_walls.py
similarity index 100%
rename from src/humanoid_Simulation_Environment_Establish/run_walls.py
rename to src/humanoid_simulation_environment_establish/run_walls.py
diff --git a/src/optimized_details.md b/src/optimized_details.md
index 7013b41f9f..e195b9d92a 100644
--- a/src/optimized_details.md
+++ b/src/optimized_details.md
@@ -1,34 +1,34 @@
-# CARLA Manual Control 代码优化说明文档
-
-## 一、优化概述
-这次主要是改了 CARLA 官方的 manual_control.py 脚本,没动原来的功能,就是让代码看着更清楚、改起来更方便,也少了点小问题。
-
-## 二、核心优化点明细
-
-### 1. 代码结构重构:模块化拆分
-原来的代码我发现好多功能混在一起,找个东西得翻半天。现在按功能拆开了:
-工具类:专门找 CARLA 的依赖文件、拿角色蓝图这些通用操作
-传感器类:把碰撞检测、GNSS 这些传感器单独写成类,初始化和数据处理不用再跟别的代码混着
-UI 相关:HUD 显示、提示文字这些单独管,想改界面不用动别的逻辑
-控制类:就管键盘输入和车辆控制,跟显示、传感器分开
-
-
-### 2. 可读性提升:规范与清晰化
-变量名改了:原来的 t、v、c 这种看不懂的,改成 transform、velocity、control,一眼能看明白
-加了注释:每个类和主要函数都写了是干啥的,复杂点的计算步骤也标了说明
-
-
-### 3. 性能优化:效率与资源管控
-#### (1)数据结构优化
-- 碰撞历史存储:原代码使用列表 `list` 存储碰撞历史,删除头部元素时效率低(时间复杂度 O(n)),优化为 `collections.deque`(双端队列),头部删除效率提升至 O(1)。
-- 雷达数据处理:减少冗余计算,仅在需要可视化时解析雷达点云,避免不必要的内存占用。
-
-#### (2)资源管理优化
-- 传感器销毁逻辑:原代码传感器销毁分散在多个地方,优化后统一到 `World.destroy` 方法,确保程序退出时所有传感器(相机/雷达/GNSS)都能被正确销毁,避免CARLA服务器资源泄漏。
-- 同步模式适配:在 `main` 函数 `finally` 块中添加同步模式恢复逻辑,即使程序异常退出,也能将CARLA世界恢复为异步模式,避免影响后续使用。
-
-#### (3)重复计算消除
-换了存储方式:原来存碰撞记录用列表,删东西慢,现在用 deque,删起来快多了
-
-
-
+# CARLA Manual Control 代码优化说明文档
+
+## 一、优化概述
+这次主要是改了 CARLA 官方的 manual_control.py 脚本,没动原来的功能,就是让代码看着更清楚、改起来更方便,也少了点小问题。
+
+## 二、核心优化点明细
+
+### 1. 代码结构重构:模块化拆分
+原来的代码我发现好多功能混在一起,找个东西得翻半天。现在按功能拆开了:
+工具类:专门找 CARLA 的依赖文件、拿角色蓝图这些通用操作
+传感器类:把碰撞检测、GNSS 这些传感器单独写成类,初始化和数据处理不用再跟别的代码混着
+UI 相关:HUD 显示、提示文字这些单独管,想改界面不用动别的逻辑
+控制类:就管键盘输入和车辆控制,跟显示、传感器分开
+
+
+### 2. 可读性提升:规范与清晰化
+变量名改了:原来的 t、v、c 这种看不懂的,改成 transform、velocity、control,一眼能看明白
+加了注释:每个类和主要函数都写了是干啥的,复杂点的计算步骤也标了说明
+
+
+### 3. 性能优化:效率与资源管控
+#### (1)数据结构优化
+- 碰撞历史存储:原代码使用列表 `list` 存储碰撞历史,删除头部元素时效率低(时间复杂度 O(n)),优化为 `collections.deque`(双端队列),头部删除效率提升至 O(1)。
+- 雷达数据处理:减少冗余计算,仅在需要可视化时解析雷达点云,避免不必要的内存占用。
+
+#### (2)资源管理优化
+- 传感器销毁逻辑:原代码传感器销毁分散在多个地方,优化后统一到 `World.destroy` 方法,确保程序退出时所有传感器(相机/雷达/GNSS)都能被正确销毁,避免CARLA服务器资源泄漏。
+- 同步模式适配:在 `main` 函数 `finally` 块中添加同步模式恢复逻辑,即使程序异常退出,也能将CARLA世界恢复为异步模式,避免影响后续使用。
+
+#### (3)重复计算消除
+换了存储方式:原来存碰撞记录用列表,删东西慢,现在用 deque,删起来快多了
+
+
+
From 196732c17aab5527ba60eae992c52f93621984fd Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 3 Nov 2025 09:51:27 +0800
Subject: [PATCH 28/45] Standardize filename case: track main.py (lowercase)
---
src/Unmanned_vehicle_AD_DQN/Main.py | 153 ----------------------------
1 file changed, 153 deletions(-)
delete mode 100644 src/Unmanned_vehicle_AD_DQN/Main.py
diff --git a/src/Unmanned_vehicle_AD_DQN/Main.py b/src/Unmanned_vehicle_AD_DQN/Main.py
deleted file mode 100644
index e661cabbb9..0000000000
--- a/src/Unmanned_vehicle_AD_DQN/Main.py
+++ /dev/null
@@ -1,153 +0,0 @@
-import glob
-import os
-import sys
-import random
-import time
-import numpy as np
-import cv2
-import math
-import matplotlib.pyplot as plt
-from collections import deque
-from tensorflow.keras.applications.xception import Xception
-from tensorflow.keras.layers import Dense, GlobalAveragePooling2D
-from tensorflow.keras.models import Sequential, Model
-from tensorflow.keras.layers import Dense, GlobalAveragePooling2D, Input, Concatenate, Conv2D, AveragePooling2D, Activation, \
- Flatten
-from tensorflow.keras.optimizers import Adam
-from tensorflow.keras.models import Model
-from tensorflow.keras.callbacks import TensorBoard
-import tensorflow as tf
-import tensorflow.keras.backend as backend
-from threading import Thread
-
-from tqdm import tqdm
-
-import Hyperparameters
-from Environment import *
-from Model import *
-from Hyperparameters import *
-
-if __name__ == '__main__':
-
- FPS = 60
- # For stats
- ep_rewards = [-200]
-
- # For more repetitive results
- # random.seed(1)
- # np.random.seed(1)
- # tf.compat.v1.set_random_seed(1)
-
- # Memory fraction, used mostly when training multiple agents
- gpu_options = tf.compat.v1.GPUOptions(per_process_gpu_memory_fraction=MEMORY_FRACTION)
- tf.compat.v1.keras.backend.set_session(
- tf.compat.v1.Session(config=tf.compat.v1.ConfigProto(gpu_options=gpu_options)))
-
- # Create models folder
- if not os.path.isdir('models'):
- os.makedirs('models')
-
- # Create agent and environment
- agent = DQNAgent()
- env = CarEnv()
-
- # Start training thread and wait for training to be initialized
- trainer_thread = Thread(target=agent.train_in_loop, daemon=True)
- trainer_thread.start()
- while not agent.training_initialized:
- time.sleep(0.01)
-
- agent.get_qs(np.ones((env.im_height, env.im_width, 3)))
-
- # Iterate over episodes
- epds = []
- scores = []
- avg_scores = []
- for episode in tqdm(range(1, EPISODES + 1), ascii=True, unit='episodes'):
-
- env.collision_hist = []
- agent.tensorboard.step = episode
-
- # Restarting episode - reset episode reward and step number
- score = 0
- step = 1
-
-
- # Reset environment and get initial state
- current_state = env.reset()
-
- # Reset flag and start iterating until episode ends
- done = False
- episode_start = time.time()
-
- # Play for given number of seconds only
- while True:
-
- # This part stays mostly the same, the change is to query a model for Q values
- if np.random.random() > Hyperparameters.EPSILON:
- # Get action from Q table
- qs = agent.get_qs(current_state)
- action = np.argmax(qs)
- print(f'Action: [{qs[0]:>5.2f}, {qs[1]:>5.2f}, {qs[2]:>5.2f}] {action}')
- else:
- # Get random action
- action = np.random.randint(0, 3)
- # This takes no time, so we add a delay matching 60 FPS (prediction above takes longer)
- time.sleep(1 / FPS)
-
- new_state, reward, done, _ = env.step(action)
-
- # Transform new continuous state to new discrete state and count reward
- score += reward
-
- if score < REWARD_OFFSET:
- done = True
-
- # Every step we update replay memory
- agent.update_replay_memory((current_state, action, reward, new_state, done))
-
- current_state = new_state
- step += 1
-
- if done:
- break
-
- # End of episode - destroy agents
- for actor in env.actor_list:
- actor.destroy()
-
- scores.append(score)
- avg_scores.append(np.mean(scores[-10:]))
-
- if not episode % AGGREGATE_STATS_EVERY or episode == 1:
- average_reward = np.mean(scores[-AGGREGATE_STATS_EVERY:])
- min_reward = min(scores[-AGGREGATE_STATS_EVERY:])
- max_reward = max(scores[-AGGREGATE_STATS_EVERY:])
- agent.tensorboard.update_stats(reward_avg=average_reward, reward_min=min_reward, reward_max=max_reward,
- epsilon=Hyperparameters.EPSILON)
-
- # Save model, but only when min reward is greater or equal a set value
- if min_reward >= MIN_REWARD and (episode not in epds):
- agent.model.save(
- f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{avg_score:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model')
-
- epds.append(episode)
- print('episode: ', episode, 'score %.2f' % score)
- # Decay epsilon
- if Hyperparameters.EPSILON > Hyperparameters.MIN_EPSILON:
- Hyperparameters.EPSILON *= Hyperparameters.EPSILON_DECAY
- Hyperparameters.EPSILON = max(Hyperparameters.MIN_EPSILON, Hyperparameters.EPSILON)
-
- # Set termination flag for training thread and wait for it to finish
- agent.terminate = True
- trainer_thread.join()
- agent.model.save(
- f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{avg_score:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model')
-
- fig = plt.figure()
- ax = fig.add_subplot(111)
- plt.plot(scores)
- plt.plot(avg_scores)
- plt.ylabel('Score')
- plt.xlabel('Episode #')
- plt.show()
\ No newline at end of file
From 8df2fe73cbffe3c9c5cac8cc89b5c46e1b114e72 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 3 Nov 2025 11:22:40 +0800
Subject: [PATCH 29/45] Retain both Main.py and main.py from upstream
(case-sensitive enabled)
---
src/Unmanned_vehicle_AD_DQN/Main.py | 153 ++++++++++++++++++++++++++++
1 file changed, 153 insertions(+)
create mode 100644 src/Unmanned_vehicle_AD_DQN/Main.py
diff --git a/src/Unmanned_vehicle_AD_DQN/Main.py b/src/Unmanned_vehicle_AD_DQN/Main.py
new file mode 100644
index 0000000000..e661cabbb9
--- /dev/null
+++ b/src/Unmanned_vehicle_AD_DQN/Main.py
@@ -0,0 +1,153 @@
+import glob
+import os
+import sys
+import random
+import time
+import numpy as np
+import cv2
+import math
+import matplotlib.pyplot as plt
+from collections import deque
+from tensorflow.keras.applications.xception import Xception
+from tensorflow.keras.layers import Dense, GlobalAveragePooling2D
+from tensorflow.keras.models import Sequential, Model
+from tensorflow.keras.layers import Dense, GlobalAveragePooling2D, Input, Concatenate, Conv2D, AveragePooling2D, Activation, \
+ Flatten
+from tensorflow.keras.optimizers import Adam
+from tensorflow.keras.models import Model
+from tensorflow.keras.callbacks import TensorBoard
+import tensorflow as tf
+import tensorflow.keras.backend as backend
+from threading import Thread
+
+from tqdm import tqdm
+
+import Hyperparameters
+from Environment import *
+from Model import *
+from Hyperparameters import *
+
+if __name__ == '__main__':
+
+ FPS = 60
+ # For stats
+ ep_rewards = [-200]
+
+ # For more repetitive results
+ # random.seed(1)
+ # np.random.seed(1)
+ # tf.compat.v1.set_random_seed(1)
+
+ # Memory fraction, used mostly when training multiple agents
+ gpu_options = tf.compat.v1.GPUOptions(per_process_gpu_memory_fraction=MEMORY_FRACTION)
+ tf.compat.v1.keras.backend.set_session(
+ tf.compat.v1.Session(config=tf.compat.v1.ConfigProto(gpu_options=gpu_options)))
+
+ # Create models folder
+ if not os.path.isdir('models'):
+ os.makedirs('models')
+
+ # Create agent and environment
+ agent = DQNAgent()
+ env = CarEnv()
+
+ # Start training thread and wait for training to be initialized
+ trainer_thread = Thread(target=agent.train_in_loop, daemon=True)
+ trainer_thread.start()
+ while not agent.training_initialized:
+ time.sleep(0.01)
+
+ agent.get_qs(np.ones((env.im_height, env.im_width, 3)))
+
+ # Iterate over episodes
+ epds = []
+ scores = []
+ avg_scores = []
+ for episode in tqdm(range(1, EPISODES + 1), ascii=True, unit='episodes'):
+
+ env.collision_hist = []
+ agent.tensorboard.step = episode
+
+ # Restarting episode - reset episode reward and step number
+ score = 0
+ step = 1
+
+
+ # Reset environment and get initial state
+ current_state = env.reset()
+
+ # Reset flag and start iterating until episode ends
+ done = False
+ episode_start = time.time()
+
+ # Play for given number of seconds only
+ while True:
+
+ # This part stays mostly the same, the change is to query a model for Q values
+ if np.random.random() > Hyperparameters.EPSILON:
+ # Get action from Q table
+ qs = agent.get_qs(current_state)
+ action = np.argmax(qs)
+ print(f'Action: [{qs[0]:>5.2f}, {qs[1]:>5.2f}, {qs[2]:>5.2f}] {action}')
+ else:
+ # Get random action
+ action = np.random.randint(0, 3)
+ # This takes no time, so we add a delay matching 60 FPS (prediction above takes longer)
+ time.sleep(1 / FPS)
+
+ new_state, reward, done, _ = env.step(action)
+
+ # Transform new continuous state to new discrete state and count reward
+ score += reward
+
+ if score < REWARD_OFFSET:
+ done = True
+
+ # Every step we update replay memory
+ agent.update_replay_memory((current_state, action, reward, new_state, done))
+
+ current_state = new_state
+ step += 1
+
+ if done:
+ break
+
+ # End of episode - destroy agents
+ for actor in env.actor_list:
+ actor.destroy()
+
+ scores.append(score)
+ avg_scores.append(np.mean(scores[-10:]))
+
+ if not episode % AGGREGATE_STATS_EVERY or episode == 1:
+ average_reward = np.mean(scores[-AGGREGATE_STATS_EVERY:])
+ min_reward = min(scores[-AGGREGATE_STATS_EVERY:])
+ max_reward = max(scores[-AGGREGATE_STATS_EVERY:])
+ agent.tensorboard.update_stats(reward_avg=average_reward, reward_min=min_reward, reward_max=max_reward,
+ epsilon=Hyperparameters.EPSILON)
+
+ # Save model, but only when min reward is greater or equal a set value
+ if min_reward >= MIN_REWARD and (episode not in epds):
+ agent.model.save(
+ f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{avg_score:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model')
+
+ epds.append(episode)
+ print('episode: ', episode, 'score %.2f' % score)
+ # Decay epsilon
+ if Hyperparameters.EPSILON > Hyperparameters.MIN_EPSILON:
+ Hyperparameters.EPSILON *= Hyperparameters.EPSILON_DECAY
+ Hyperparameters.EPSILON = max(Hyperparameters.MIN_EPSILON, Hyperparameters.EPSILON)
+
+ # Set termination flag for training thread and wait for it to finish
+ agent.terminate = True
+ trainer_thread.join()
+ agent.model.save(
+ f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{avg_score:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model')
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+ plt.plot(scores)
+ plt.plot(avg_scores)
+ plt.ylabel('Score')
+ plt.xlabel('Episode #')
+ plt.show()
\ No newline at end of file
From 15e1d516fafbfc947b92d86348ce996aea7b5b2a Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 4 Nov 2025 08:38:54 +0800
Subject: [PATCH 30/45] =?UTF-8?q?=E4=B8=BAmain=5Fhumanoid.py=E6=B7=BB?=
=?UTF-8?q?=E5=8A=A0launch=E5=90=AF=E5=8A=A8=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main_humanoid.launch | 12 ++++++++++++
1 file changed, 12 insertions(+)
create mode 100644 src/humanoid_simulation_environment_establish/main_humanoid.launch
diff --git a/src/humanoid_simulation_environment_establish/main_humanoid.launch b/src/humanoid_simulation_environment_establish/main_humanoid.launch
new file mode 100644
index 0000000000..e26bcc35c6
--- /dev/null
+++ b/src/humanoid_simulation_environment_establish/main_humanoid.launch
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
\ No newline at end of file
From e472647508e6c36f3fd618e3e50151bc2ff17e8e Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 11 Nov 2025 13:28:38 +0800
Subject: [PATCH 31/45] =?UTF-8?q?=E4=BC=98=E5=8C=96main=5Fhumanoid.py?=
=?UTF-8?q?=EF=BC=8C=E6=B7=BB=E5=8A=A0=E9=94=AE=E7=9B=98=E6=8E=A7=E5=88=B6?=
=?UTF-8?q?=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main_humanoid.py | 201 +++++++++++++-----
1 file changed, 147 insertions(+), 54 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/main_humanoid.py b/src/humanoid_simulation_environment_establish/main_humanoid.py
index 25795b5d9b..841b8e8552 100644
--- a/src/humanoid_simulation_environment_establish/main_humanoid.py
+++ b/src/humanoid_simulation_environment_establish/main_humanoid.py
@@ -1,73 +1,166 @@
-# 标准库
+#!/usr/bin/env python3
import rospy
-import rospkg # 用于获取ROS包路径
-# 第三方库
import mujoco
-from mujoco import viewer # 注意:MuJoCo 2.x版本支持,3.x需用mujoco_viewer
+from mujoco import viewer
+import threading
+import sys
+import tty
+import termios
+import rospkg # 用于获取ROS功能包路径
-def main():
- # 初始化ROS节点
- rospy.init_node('humanoid_standup_node', anonymous=True)
- rospy.loginfo("人形机器人起身控制节点启动")
- # 从ROS参数服务器获取配置(可通过launch文件修改)
- kp_gain = rospy.get_param('~kp_gain', 5.0) # 比例增益
- model_rel_path = rospy.get_param('~model_path', 'xml/humanoid.xml') # 模型相对路径
+class HumanoidStandupController:
+ def __init__(self):
+ # 1. 初始化ROS节点与参数
+ rospy.init_node("humanoid_standup_node", anonymous=True)
+ rospy.loginfo("人形机器人起身控制器启动...")
- # 获取模型绝对路径(解决相对路径问题)
- try:
- rospack = rospkg.RosPack()
- pkg_path = rospack.get_path('humanoid_motion') # 功能包路径
- model_path = f"{pkg_path}/{model_rel_path}" # 拼接绝对路径
- rospy.loginfo(f"模型路径: {model_path}")
- except Exception as e:
- rospy.logerr(f"获取包路径失败: {e}")
- return
+ # 从ROS参数服务器获取配置(默认值为launch文件中设置)
+ self.kp_gain = rospy.get_param("kp_gain", 5.0) # 比例增益
+ self.model_path = rospy.get_param("model_path", "xml/humanoid.xml") # 模型相对路径
- # 加载MuJoCo模型
- try:
- model = mujoco.MjModel.from_xml_path(model_path)
- data = mujoco.MjData(model)
- rospy.loginfo("模型加载成功")
- except Exception as e:
- rospy.logerr(f"模型加载失败: {e}")
- return
+ # 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() # 用于控制日志输出频率
- # 初始化目标姿势(站立,关键帧1)和初始姿势(深蹲,关键帧0)
- target_data = mujoco.MjData(model)
- mujoco.mj_resetDataKeyframe(model, target_data, 1) # 站立关键帧
- target_qpos = target_data.qpos.copy()
- mujoco.mj_resetDataKeyframe(model, data, 0) # 初始深蹲姿势
+ # 6. 启动键盘监听线程
+ self.keyboard_thread = threading.Thread(target=self._keyboard_listener)
+ self.keyboard_thread.daemon = True # 主线程退出时自动结束
+ self.keyboard_thread.start()
- # 设置控制频率(200Hz,与原time.sleep(0.005)对应)
- control_rate = rospy.Rate(200)
+ # 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)
- # 启动可视化
- with viewer.launch_passive(model, data) as v:
- rospy.loginfo("开始起身控制(按Ctrl+C停止)")
try:
- # ROS循环(直到节点关闭)
- while not rospy.is_shutdown():
- mujoco.mj_step(model, data) # 单步仿真
+ 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
- # 比例控制(只控制电机关节,跳过前7个根关节)
- qpos_error = target_qpos[7:] - data.qpos[7:]
- data.ctrl[:] = kp_gain * qpos_error
+ # 3. 定时输出状态日志
+ self._log_state()
- # 输出躯干高度(ROS日志)
- rospy.loginfo_throttle(1, # 每1秒输出一次,避免刷屏
- f"时间: {data.time:.2f}, 躯干高度: {data.qpos[2]:.2f}")
+ # 4. 刷新可视化窗口
+ viewer.sync()
- v.sync() # 同步可视化
- control_rate.sleep() # 控制循环频率
+ # 5. 控制循环频率
+ rate.sleep()
- except KeyboardInterrupt:
- rospy.loginfo("用户中断,停止控制")
+ 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:
- rospy.loginfo("起身控制节点关闭")
+ # 关闭可视化窗口
+ viewer.close()
+ rospy.loginfo("程序已退出")
+
if __name__ == "__main__":
try:
- main()
+ controller = HumanoidStandupController()
+ controller.run()
except rospy.ROSInterruptException:
- pass
\ No newline at end of file
+ rospy.loginfo("ROS节点被中断")
+ except Exception as e:
+ rospy.logerr(f"程序异常: {e}")
\ No newline at end of file
From a696643de6422d98dbfb2705f54b1aded1a33c75 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 17 Nov 2025 11:39:23 +0800
Subject: [PATCH 32/45] =?UTF-8?q?=E4=BC=98=E5=8C=96motion=5Fmain.py,?=
=?UTF-8?q?=E9=87=8D=E6=9E=84=E5=B8=A6=E7=A9=BA=E9=9A=99=E7=9A=84=E8=B5=B0?=
=?UTF-8?q?=E5=BB=8A=E7=8E=AF=E5=A2=83=E6=9E=84=E5=BB=BA=E9=80=BB=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../motion_main.py | 297 +++++++++++++-----
1 file changed, 212 insertions(+), 85 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/motion_main.py b/src/humanoid_simulation_environment_establish/motion_main.py
index 8a48c7d975..856ac6c468 100644
--- a/src/humanoid_simulation_environment_establish/motion_main.py
+++ b/src/humanoid_simulation_environment_establish/motion_main.py
@@ -1,92 +1,219 @@
-# 标准库
import numpy as np
-# 第三方库
-from absl import app
-# dm_control 模块
-from dm_control import composer
-from dm_control.composer.variation import distributions
-from dm_control.locomotion.arenas import corridors as corr_arenas
-from dm_control.locomotion.tasks import corridors as corr_tasks
-from dm_control.locomotion.walkers import cmu_humanoid
-from dm_control import viewer
-
-# 全局环境变量,供策略函数访问
-global_env = None
-
-def create_environment_run_walls():
- """创建带墙壁走廊的人形机器人环境"""
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- arena = corr_arenas.WallsCorridor(
- wall_gap=4.,
- wall_width=distributions.Uniform(1, 7),
- wall_height=3.0,
- corridor_width=10,
- corridor_length=100,
- include_initial_padding=False)
-
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0),
- target_velocity=3.0,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(
- time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-def create_environment_run_gaps():
- "创建有空隙的走廊形竞技场环境"
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- arena = corr_arenas.GapsCorridor(
- platform_length=distributions.Uniform(.3, 2.5),
- gap_length=distributions.Uniform(.5, 1.25),
- corridor_width=10,
- corridor_length=100)
-
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0),
- target_velocity=3.0,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(
- time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-def random_policy(timestep):
- """随机动作策略:使用全局环境变量获取动作空间"""
- global global_env
- action_spec = global_env.action_spec()
- return np.random.uniform(
- low=action_spec.minimum,
- high=action_spec.maximum,
- size=action_spec.shape
- )
+import mujoco_py
+from mujoco_py import load_model_from_xml, MjSim, MjViewer
+import time
+import xml.etree.ElementTree as ET # 用于解析XML
+
+class GapCorridorEnvironment:
+ """基于mujoco-py的带空隙走廊环境(使用自定义人形机器人模型)"""
+ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path="/home/qiqi/mujoco_ros_ws/src/humanoid_motion/xml/humanoid_2.xml"):
+ """
+ Args:
+ corridor_length: 走廊总长度
+ corridor_width: 走廊宽度
+ robot_xml_path: 自定义人形机器人XML文件路径
+ """
+ self.corridor_length = corridor_length
+ self.corridor_width = corridor_width
+ self.robot_xml_path = robot_xml_path # 自定义机器人模型路径
+ self.model = self._build_model() # 构建整合后的模型
+ self.sim = MjSim(self.model)
+ self.viewer = MjViewer(self.sim)
+ self.timestep = self.model.opt.timestep # 物理步长(从机器人XML读取,为0.005s)
+ self.control_timestep = 0.03 # 控制步长
+ self.control_steps = int(self.control_timestep / self.timestep) # 每个控制步包含的物理步
+ self._max_episode_steps = 30 / self.control_timestep # 总步数(30秒)
+ self.current_step = 0
+
+ def _parse_robot_xml(self):
+ """解析自定义机器人XML,提取需要的节点(身体、执行器、肌腱等)"""
+ tree = ET.parse(self.robot_xml_path)
+ root = tree.getroot()
+
+ # 提取机器人的身体定义(下的torso节点)
+ worldbody = root.find("worldbody")
+ robot_body = worldbody.find("body[@name='torso']") # 机器人主体
+ # 调整机器人初始位置(放在走廊起点,x=2.0处,避免初始在空隙)
+ robot_body.set("pos", "2.0 0 1.282") # 保持高度不变,x方向移动到走廊起点
+
+ # 提取执行器()、肌腱()、接触排除()定义
+ actuator = root.find("actuator")
+ tendon = root.find("tendon")
+ contact = root.find("contact")
+ asset = root.find("asset") # 机器人的材质/纹理定义
+ visual = root.find("visual") # 可视化配置
+ keyframe = root.find("keyframe") # 关键帧(可选,用于初始姿态)
+
+ return {
+ "robot_body": ET.tostring(robot_body, encoding="unicode"),
+ "actuator": ET.tostring(actuator, encoding="unicode") if actuator is not None else "",
+ "tendon": ET.tostring(tendon, encoding="unicode") if tendon is not None else "",
+ "contact": ET.tostring(contact, encoding="unicode") if contact is not None else "",
+ "asset": ET.tostring(asset, encoding="unicode") if asset is not None else "",
+ "visual": ET.tostring(visual, encoding="unicode") if visual is not None else "",
+ "keyframe": ET.tostring(keyframe, encoding="unicode") if keyframe is not None else ""
+ }
+
+ def _build_model(self):
+ """构建带空隙的走廊环境,并整合自定义人形机器人模型"""
+ # 解析自定义机器人XML
+ robot_parts = self._parse_robot_xml()
+
+ # 基础XML结构(走廊环境+机器人)
+ xml = f"""
+
+
+
+
+
+ {robot_parts['visual']}
+ {robot_parts['asset']}
+
+
+
+
+
+
+
+
+
+
+
+
+ {self._build_gaps_corridor()}
+
+
+ {robot_parts['robot_body']}
+
+
+
+ {robot_parts['contact']}
+
+
+ {robot_parts['tendon']}
+
+
+ {robot_parts['actuator']}
+
+
+ {robot_parts['keyframe']}
+
+ """
+ return load_model_from_xml(xml)
+
+ def _build_gaps_corridor(self):
+ """构建带空隙的走廊(平台+空隙交替)"""
+ gaps_xml = ""
+ platform_length = 2.0 # 平台长度
+ gap_length = 1.0 # 空隙长度
+ platform_thickness = 0.2 # 平台厚度
+ 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
+ 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() # 刷新物理状态
+ return self._get_observation()
-def main(unused_argv):
- """主函数:初始化全局环境并启动viewer"""
- global global_env
- # 初始化环境并赋值给全局变量
- global_env = create_environment_run_gaps()
-
- # 启动viewer
- viewer.launch(
- environment_loader=create_environment_run_gaps,
- policy=random_policy
+ 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") # 躯干位置
+ 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
+
+ def step(self, action):
+ """执行动作并推进环境(动作维度需与机器人执行器数量匹配)"""
+ self.current_step += 1
+ # 应用动作到执行器(控制信号限制在[-1,1],你的机器人有20个执行器)
+ self.sim.data.ctrl[:] = np.clip(action, -1.0, 1.0)
+ # 执行多个物理步(匹配控制步长)
+ for _ in range(self.control_steps):
+ self.sim.step()
+ # 获取观测、奖励、完成状态
+ 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米视为跌倒
+ done = True
+ reward -= 1.0 # 跌倒额外惩罚
+ return obs, reward, done
+
+ def render(self):
+ """渲染画面"""
+ 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)
+
+
+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" # 确保路径正确
)
+ # 运行环境
+ while True:
+ obs = env.reset()
+ total_reward = 0.0
+ while True:
+ action = random_policy(env) # 生成随机动作(20维)
+ obs, reward, done = env.step(action) # 执行一步
+ total_reward += reward
+ env.render() # 渲染画面
+ if done:
+ print(f"Episode finished. Total reward: {total_reward:.2f}")
+ break
+ time.sleep(1) # 每轮结束后暂停1秒
if __name__ == "__main__":
-
- app.run(main)
\ No newline at end of file
+ main()
\ No newline at end of file
From f8a4241b7b14cb02ac534474a8157dcab7e5a0e3 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 18 Nov 2025 08:12:06 +0800
Subject: [PATCH 33/45] =?UTF-8?q?=E6=81=A2=E5=A4=8D=E5=B9=B6=E5=90=88?=
=?UTF-8?q?=E5=B9=B6=E6=9C=AC=E5=9C=B0button=5Fcontrol=5Fvehicle=E6=96=87?=
=?UTF-8?q?=E4=BB=B6=E5=A4=B9=E7=9A=84=E5=A4=87=E4=BB=BD=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../carla_manual_control.launch | 24 ++-----------------
1 file changed, 2 insertions(+), 22 deletions(-)
diff --git a/src/button_control_vehicle/carla_manual_control.launch b/src/button_control_vehicle/carla_manual_control.launch
index 1fd8dc8709..6a38db625f 100755
--- a/src/button_control_vehicle/carla_manual_control.launch
+++ b/src/button_control_vehicle/carla_manual_control.launch
@@ -1,30 +1,10 @@
+
-
-
-
-
-
+
From 4cbeaab5434b2a3f385abd19676d15f58aa18ebe Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 18 Nov 2025 08:20:22 +0800
Subject: [PATCH 34/45] =?UTF-8?q?=E4=BC=98=E5=8C=96motion=5Fmain=E5=86=85?=
=?UTF-8?q?=E5=AE=B9=EF=BC=8C=E9=87=8D=E6=9E=84=E7=A9=BA=E9=9A=99=E7=9A=84?=
=?UTF-8?q?=E8=B5=B0=E5=BB=8A=E7=8E=AF=E5=A2=83=E6=9E=84=E5=BB=BA=E9=80=BB?=
=?UTF-8?q?=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/humanoid_simulation_environment_establish/motion_main.py | 1 -
1 file changed, 1 deletion(-)
diff --git a/src/humanoid_simulation_environment_establish/motion_main.py b/src/humanoid_simulation_environment_establish/motion_main.py
index 856ac6c468..4cad4fafbd 100644
--- a/src/humanoid_simulation_environment_establish/motion_main.py
+++ b/src/humanoid_simulation_environment_establish/motion_main.py
@@ -214,6 +214,5 @@ def main():
break
time.sleep(1) # 每轮结束后暂停1秒
-
if __name__ == "__main__":
main()
\ No newline at end of file
From 9233641f0fd76a3971ca43ca0de014a3986ad435 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 18 Nov 2025 09:43:39 +0800
Subject: [PATCH 35/45] =?UTF-8?q?=E6=81=A2=E5=A4=8Dcarla=5Fmanual=5Fcontro?=
=?UTF-8?q?l.launch=E8=87=B3=E5=8E=9F=E4=BD=9C=E8=80=85=E7=89=88=E6=9C=AC?=
=?UTF-8?q?=EF=BC=88=E5=93=88=E5=B8=8C68c9df5=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../carla_manual_control.launch | 24 +++++++++++++++++--
1 file changed, 22 insertions(+), 2 deletions(-)
diff --git a/src/button_control_vehicle/carla_manual_control.launch b/src/button_control_vehicle/carla_manual_control.launch
index 6a38db625f..1fd8dc8709 100755
--- a/src/button_control_vehicle/carla_manual_control.launch
+++ b/src/button_control_vehicle/carla_manual_control.launch
@@ -1,10 +1,30 @@
-
+
+
+
+
+
-
From 418cea72316ea2c4234336e55da2cdf120ad2eca Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 24 Nov 2025 09:38:12 +0800
Subject: [PATCH 36/45] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=96=87=E4=BB=B6?=
=?UTF-8?q?=E7=9B=AE=E5=BD=95=E7=BB=93=E6=9E=84=EF=BC=8C=E7=A1=AE=E4=BF=9D?=
=?UTF-8?q?=E4=BB=A5main.py=E4=B8=BA=E5=90=AF=E5=8A=A8=E5=85=A5=E5=8F=A3?=
=?UTF-8?q?=EF=BC=8C=E5=B0=86=E5=8E=9F=E5=90=84=E6=96=87=E4=BB=B6=E5=AE=9E?=
=?UTF-8?q?=E7=8E=B0=E5=8A=9F=E8=83=BD=E6=95=B4=E5=90=88=E8=87=B3main.py?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../go_to_target.py | 60 -----
.../main.launch | 10 -
.../main_humanoid.launch | 12 -
.../main_humanoid.py | 166 -------------
.../motion_main.py | 218 ------------------
.../run_walls.py | 66 ------
6 files changed, 532 deletions(-)
delete mode 100644 src/humanoid_simulation_environment_establish/go_to_target.py
delete mode 100644 src/humanoid_simulation_environment_establish/main.launch
delete mode 100644 src/humanoid_simulation_environment_establish/main_humanoid.launch
delete mode 100644 src/humanoid_simulation_environment_establish/main_humanoid.py
delete mode 100644 src/humanoid_simulation_environment_establish/motion_main.py
delete mode 100644 src/humanoid_simulation_environment_establish/run_walls.py
diff --git a/src/humanoid_simulation_environment_establish/go_to_target.py b/src/humanoid_simulation_environment_establish/go_to_target.py
deleted file mode 100644
index d94c846a80..0000000000
--- a/src/humanoid_simulation_environment_establish/go_to_target.py
+++ /dev/null
@@ -1,60 +0,0 @@
-# 标准库
-import numpy as np
-# 第三方库
-from absl import app
-# dm_control 模块
-from dm_control import composer
-from dm_control.locomotion.arenas import floors
-from dm_control.locomotion.tasks import go_to_target
-from dm_control.locomotion.walkers import cmu_humanoid
-from dm_control import viewer
-
-from dm_control import composer
-
-
-
-# 全局环境变量,供策略函数访问
-global_env = None
-
-def create_environment():
- """建立一个任务,奖励代理去一个目标"""
- walker = cmu_humanoid.CMUHumanoidPositionControlled()
-
- arena = floors.Floor()
-
- task = go_to_target.GoToTarget(
- walker=walker,
- arena=arena,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-
-def random_policy(timestep):
- """随机动作策略:使用全局环境变量获取动作空间"""
- global global_env
- action_spec = global_env.action_spec()
- return np.random.uniform(
- low=action_spec.minimum,
- high=action_spec.maximum,
- size=action_spec.shape
- )
-
-def main(unused_argv):
- """主函数:初始化全局环境并启动viewer"""
- global global_env
- # 初始化环境并赋值给全局变量
- global_env = create_environment()
-
- # 启动viewer
- viewer.launch(
- environment_loader=create_environment,
- policy=random_policy
- )
-
-
-if __name__ == "__main__":
- app.run(main)
\ No newline at end of file
diff --git a/src/humanoid_simulation_environment_establish/main.launch b/src/humanoid_simulation_environment_establish/main.launch
deleted file mode 100644
index ea462c9807..0000000000
--- a/src/humanoid_simulation_environment_establish/main.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/humanoid_simulation_environment_establish/main_humanoid.launch b/src/humanoid_simulation_environment_establish/main_humanoid.launch
deleted file mode 100644
index e26bcc35c6..0000000000
--- a/src/humanoid_simulation_environment_establish/main_humanoid.launch
+++ /dev/null
@@ -1,12 +0,0 @@
-
-
-
-
-
-
-
-
-
\ 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
diff --git a/src/humanoid_simulation_environment_establish/motion_main.py b/src/humanoid_simulation_environment_establish/motion_main.py
deleted file mode 100644
index 4cad4fafbd..0000000000
--- a/src/humanoid_simulation_environment_establish/motion_main.py
+++ /dev/null
@@ -1,218 +0,0 @@
-import numpy as np
-import mujoco_py
-from mujoco_py import load_model_from_xml, MjSim, MjViewer
-import time
-import xml.etree.ElementTree as ET # 用于解析XML
-
-class GapCorridorEnvironment:
- """基于mujoco-py的带空隙走廊环境(使用自定义人形机器人模型)"""
- def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path="/home/qiqi/mujoco_ros_ws/src/humanoid_motion/xml/humanoid_2.xml"):
- """
- Args:
- corridor_length: 走廊总长度
- corridor_width: 走廊宽度
- robot_xml_path: 自定义人形机器人XML文件路径
- """
- self.corridor_length = corridor_length
- self.corridor_width = corridor_width
- self.robot_xml_path = robot_xml_path # 自定义机器人模型路径
- self.model = self._build_model() # 构建整合后的模型
- self.sim = MjSim(self.model)
- self.viewer = MjViewer(self.sim)
- self.timestep = self.model.opt.timestep # 物理步长(从机器人XML读取,为0.005s)
- self.control_timestep = 0.03 # 控制步长
- self.control_steps = int(self.control_timestep / self.timestep) # 每个控制步包含的物理步
- self._max_episode_steps = 30 / self.control_timestep # 总步数(30秒)
- self.current_step = 0
-
- def _parse_robot_xml(self):
- """解析自定义机器人XML,提取需要的节点(身体、执行器、肌腱等)"""
- tree = ET.parse(self.robot_xml_path)
- root = tree.getroot()
-
- # 提取机器人的身体定义(下的torso节点)
- worldbody = root.find("worldbody")
- robot_body = worldbody.find("body[@name='torso']") # 机器人主体
- # 调整机器人初始位置(放在走廊起点,x=2.0处,避免初始在空隙)
- robot_body.set("pos", "2.0 0 1.282") # 保持高度不变,x方向移动到走廊起点
-
- # 提取执行器()、肌腱()、接触排除()定义
- actuator = root.find("actuator")
- tendon = root.find("tendon")
- contact = root.find("contact")
- asset = root.find("asset") # 机器人的材质/纹理定义
- visual = root.find("visual") # 可视化配置
- keyframe = root.find("keyframe") # 关键帧(可选,用于初始姿态)
-
- return {
- "robot_body": ET.tostring(robot_body, encoding="unicode"),
- "actuator": ET.tostring(actuator, encoding="unicode") if actuator is not None else "",
- "tendon": ET.tostring(tendon, encoding="unicode") if tendon is not None else "",
- "contact": ET.tostring(contact, encoding="unicode") if contact is not None else "",
- "asset": ET.tostring(asset, encoding="unicode") if asset is not None else "",
- "visual": ET.tostring(visual, encoding="unicode") if visual is not None else "",
- "keyframe": ET.tostring(keyframe, encoding="unicode") if keyframe is not None else ""
- }
-
- def _build_model(self):
- """构建带空隙的走廊环境,并整合自定义人形机器人模型"""
- # 解析自定义机器人XML
- robot_parts = self._parse_robot_xml()
-
- # 基础XML结构(走廊环境+机器人)
- xml = f"""
-
-
-
-
-
- {robot_parts['visual']}
- {robot_parts['asset']}
-
-
-
-
-
-
-
-
-
-
-
-
- {self._build_gaps_corridor()}
-
-
- {robot_parts['robot_body']}
-
-
-
- {robot_parts['contact']}
-
-
- {robot_parts['tendon']}
-
-
- {robot_parts['actuator']}
-
-
- {robot_parts['keyframe']}
-
- """
- return load_model_from_xml(xml)
-
- def _build_gaps_corridor(self):
- """构建带空隙的走廊(平台+空隙交替)"""
- gaps_xml = ""
- platform_length = 2.0 # 平台长度
- gap_length = 1.0 # 空隙长度
- platform_thickness = 0.2 # 平台厚度
- 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
-
- 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() # 刷新物理状态
- 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") # 躯干位置
- 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
-
- def step(self, action):
- """执行动作并推进环境(动作维度需与机器人执行器数量匹配)"""
- self.current_step += 1
- # 应用动作到执行器(控制信号限制在[-1,1],你的机器人有20个执行器)
- self.sim.data.ctrl[:] = np.clip(action, -1.0, 1.0)
- # 执行多个物理步(匹配控制步长)
- for _ in range(self.control_steps):
- self.sim.step()
- # 获取观测、奖励、完成状态
- 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米视为跌倒
- done = True
- reward -= 1.0 # 跌倒额外惩罚
- return obs, reward, done
-
- def render(self):
- """渲染画面"""
- 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)
-
-
-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" # 确保路径正确
- )
- # 运行环境
- while True:
- obs = env.reset()
- total_reward = 0.0
- while True:
- action = random_policy(env) # 生成随机动作(20维)
- obs, reward, done = env.step(action) # 执行一步
- total_reward += reward
- env.render() # 渲染画面
- if done:
- print(f"Episode finished. Total reward: {total_reward:.2f}")
- break
- time.sleep(1) # 每轮结束后暂停1秒
-
-if __name__ == "__main__":
- main()
\ No newline at end of file
diff --git a/src/humanoid_simulation_environment_establish/run_walls.py b/src/humanoid_simulation_environment_establish/run_walls.py
deleted file mode 100644
index 08e22dfa62..0000000000
--- a/src/humanoid_simulation_environment_establish/run_walls.py
+++ /dev/null
@@ -1,66 +0,0 @@
-# 标准库
-import numpy as np
-# 第三方库
-from absl import app
-# dm_control 模块
-from dm_control import composer
-from dm_control.composer.variation import distributions
-from dm_control.locomotion.arenas import corridors as corr_arenas
-from dm_control.locomotion.tasks import corridors as corr_tasks
-from dm_control.locomotion.walkers import cmu_humanoid
-from dm_control import viewer
-
-# 全局环境变量,供策略函数访问
-global_env = None
-
-def create_environment():
- """创建带墙壁走廊的人形机器人环境"""
- walker = cmu_humanoid.CMUHumanoidPositionControlled(
- observable_options={'egocentric_camera': dict(enabled=True)})
-
- arena = corr_arenas.WallsCorridor(
- wall_gap=4.,
- wall_width=distributions.Uniform(1, 7),
- wall_height=3.0,
- corridor_width=10,
- corridor_length=100,
- include_initial_padding=False)
-
- task = corr_tasks.RunThroughCorridor(
- walker=walker,
- arena=arena,
- walker_spawn_position=(0.5, 0, 0),
- target_velocity=3.0,
- physics_timestep=0.005,
- control_timestep=0.03)
-
- return composer.Environment(
- time_limit=30,
- task=task,
- strip_singleton_obs_buffer_dim=True)
-
-def random_policy(timestep):
- """随机动作策略:使用全局环境变量获取动作空间"""
- global global_env
- action_spec = global_env.action_spec()
- return np.random.uniform(
- low=action_spec.minimum,
- high=action_spec.maximum,
- size=action_spec.shape
- )
-
-def main(unused_argv):
- """主函数:初始化全局环境并启动viewer"""
- global global_env
- # 初始化环境并赋值给全局变量
- global_env = create_environment()
-
- # 启动viewer
- viewer.launch(
- environment_loader=create_environment,
- policy=random_policy
- )
-
-
-if __name__ == "__main__":
- app.run(main)
\ No newline at end of file
From 2d20430bffb372e50a839be683e6d3d9397f58d3 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 24 Nov 2025 10:12:05 +0800
Subject: [PATCH 37/45] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=96=87=E4=BB=B6?=
=?UTF-8?q?=E7=9B=AE=E5=BD=95=E7=BB=93=E6=9E=84=EF=BC=8C=E7=A1=AE=E4=BF=9D?=
=?UTF-8?q?=E4=BB=A5main.py=E4=B8=BA=E5=90=AF=E5=8A=A8=E5=85=A5=E5=8F=A3?=
=?UTF-8?q?=EF=BC=8C=E5=B0=86=E5=8E=9F=E5=90=84=E6=96=87=E4=BB=B6=E5=AE=9E?=
=?UTF-8?q?=E7=8E=B0=E5=8A=9F=E8=83=BD=E6=95=B4=E5=90=88=E8=87=B3main.py?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.py | 461 ++++++++++++++++--
1 file changed, 415 insertions(+), 46 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py
index 84fed359a0..0b2b13ba1e 100644
--- a/src/humanoid_simulation_environment_establish/main.py
+++ b/src/humanoid_simulation_environment_establish/main.py
@@ -1,51 +1,420 @@
-import mujoco
-from mujoco import viewer
import numpy as np
-import rospy
-import rospkg # 用于获取ROS包路径
+import mujoco_py
+from mujoco_py import load_model_from_xml, MjSim, MjViewer
+import time
+import xml.etree.ElementTree as ET # 用于解析XML
+import threading
+import sys
+import platform
-def main():
- # 初始化ROS节点(anonymous=True确保节点名唯一)
- rospy.init_node('humanoid_main_node', anonymous=True)
- rospy.loginfo("人形机器人核心控制节点启动")
+# 跨平台键盘输入支持
+if platform.system() == 'Windows':
+ import msvcrt
+else:
+ import tty
+ import termios
- # 通过rospkg获取模型路径(避免相对路径问题)
- try:
- rospack = rospkg.RosPack()
- pkg_path = rospack.get_path('humanoid_motion') # 替换为你的功能包名
- model_path = f"{pkg_path}/xml/humanoid.xml" # 模型相对路径
- except Exception as e:
- rospy.logerr(f"获取包路径失败: {e}")
- return
-
- # 加载MuJoCo模型
- try:
- model = mujoco.MjModel.from_xml_path(model_path)
- data = mujoco.MjData(model)
- rospy.loginfo("模型加载成功")
- except Exception as e:
- rospy.logerr(f"模型加载失败: {e}")
- return
-
- # 设置控制频率(200Hz,根据仿真需求调整)
- control_rate = rospy.Rate(200)
-
- # 启动仿真可视化
- with mujoco.viewer.launch_passive(model, data) as viewer:
- rospy.loginfo("仿真可视化启动")
+
+class KeyboardController:
+ """键盘控制节点:监听键盘输入并生成控制动作"""
+ def __init__(self, action_dim):
+ """
+ Args:
+ action_dim: 动作维度(执行器数量)
+ """
+ self.action_dim = action_dim
+ self.current_action = np.zeros(action_dim) # 当前动作向量
+
+ # 控制状态
+ self.exit_flag = False # 退出标志
+ self.paused = False # 暂停标志
+ self.reset_flag = False # 重置标志
+
+ # 平台检测
+ self.is_windows = platform.system() == 'Windows'
+
+ # 键盘监听线程
+ self.keyboard_thread = threading.Thread(target=self._keyboard_listener)
+ self.keyboard_thread.daemon = True
+ self.keyboard_thread.start()
+
+ # 打印操作说明
+ self._print_help()
+
+ def _print_help(self):
+ """打印键盘控制指令说明"""
+ print("\n===== 键盘控制指令 =====")
+ print(" w/↑: 前进")
+ print(" s/↓: 后退")
+ print(" a/←: 左转")
+ print(" d/→: 右转")
+ print(" 空格: 暂停/继续")
+ print(" r: 重置环境")
+ print(" q: 退出程序")
+ print("=======================")
+
+ def _keyboard_listener(self):
+ """独立线程:监听键盘输入并更新控制动作"""
+ if self.is_windows:
+ self._keyboard_listener_windows()
+ else:
+ self._keyboard_listener_unix()
+
+ def _keyboard_listener_windows(self):
+ """Windows平台键盘监听"""
+ while not self.exit_flag:
+ if msvcrt.kbhit(): # 检查是否有按键按下
+ try:
+ key_byte = msvcrt.getch()
+ # 处理特殊键(方向键等以\x00或\xe0开头)
+ if key_byte == b'\x00' or key_byte == b'\xe0':
+ key_byte = msvcrt.getch() # 读取下一个字节
+ # 映射方向键
+ if key_byte == b'H': # ↑
+ key = '\x1b[A'
+ elif key_byte == b'P': # ↓
+ key = '\x1b[B'
+ elif key_byte == b'K': # ←
+ key = '\x1b[D'
+ elif key_byte == b'M': # →
+ key = '\x1b[C'
+ else:
+ key = key_byte.decode('utf-8', errors='ignore')
+ else:
+ key = key_byte.decode('utf-8', errors='ignore')
+ self._process_key(key)
+ except Exception as e:
+ pass # 忽略解码错误
+ time.sleep(0.01) # 避免CPU占用过高
+
+ def _keyboard_listener_unix(self):
+ """Linux/Mac平台键盘监听"""
+ fd = sys.stdin.fileno()
+ old_settings = termios.tcgetattr(fd)
try:
- # ROS循环(直到节点关闭)
- while not rospy.is_shutdown():
- # 核心控制逻辑(示例:简单关节控制)
- mujoco.mj_step(model, data) # 单步仿真
- data.ctrl[:] = 0.1 * np.sin(data.time) # 示例:正弦波控制关节
-
- # 同步可视化
- viewer.sync()
- # 控制循环频率
- control_rate.sleep()
-
- except KeyboardInterrupt:
- rospy.loginfo("用户中断,停止仿真")
+ tty.setraw(sys.stdin.fileno())
+ while not self.exit_flag:
+ key = sys.stdin.read(1) # 读取单个字符
+ self._process_key(key)
+ except Exception as e:
+ print(f"键盘监听异常: {e}")
finally:
- rospy.loginfo("核心控制节点关闭")
+ # 恢复终端默认设置
+ termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+
+ def _process_key(self, key):
+ """处理按键输入"""
+ key_lower = key.lower()
+
+ # 基本运动控制(可以根据需要调整控制逻辑)
+ base_action = np.zeros(self.action_dim)
+
+ if key_lower == 'w' or key == '\x1b[A': # 'w' 或 ↑ 键
+ # 前进:激活前向关节(例如腿部前后摆动)
+ base_action[:self.action_dim//2] = 0.3 # 前半部分执行器
+ base_action[self.action_dim//2:] = -0.2 # 后半部分执行器
+ self.current_action = base_action
+ print("前进")
+ elif key_lower == 's' or key == '\x1b[B': # 's' 或 ↓ 键
+ # 后退:反向激活
+ base_action[:self.action_dim//2] = -0.3
+ base_action[self.action_dim//2:] = 0.2
+ self.current_action = base_action
+ print("后退")
+ elif key_lower == 'a' or key == '\x1b[D': # 'a' 或 ← 键
+ # 左转:左侧关节激活
+ base_action[:self.action_dim//4] = 0.3
+ base_action[self.action_dim//4:self.action_dim//2] = -0.2
+ self.current_action = base_action
+ print("左转")
+ elif key_lower == 'd' or key == '\x1b[C': # 'd' 或 → 键
+ # 右转:右侧关节激活
+ base_action[self.action_dim//4:self.action_dim//2] = 0.3
+ base_action[:self.action_dim//4] = -0.2
+ self.current_action = base_action
+ print("右转")
+ elif key == ' ': # 空格键:暂停/继续
+ self.paused = not self.paused
+ if self.paused:
+ self.current_action = np.zeros(self.action_dim)
+ print("⏸️ 已暂停")
+ else:
+ print("▶️ 继续")
+ elif key_lower == 'r': # 重置标志
+ self.reset_flag = True
+ print("🔄 重置环境")
+ elif key_lower == 'q': # 退出
+ self.exit_flag = True
+ print("❌ 准备退出程序...")
+ else:
+ # 未识别的按键,停止动作
+ if not self.paused:
+ self.current_action = np.zeros(self.action_dim)
+
+ def get_action(self):
+ """获取当前控制动作"""
+ if self.paused:
+ return np.zeros(self.action_dim)
+ return self.current_action.copy()
+
+ def should_exit(self):
+ """检查是否应该退出"""
+ return self.exit_flag
+
+ def should_reset(self):
+ """检查是否应该重置"""
+ return self.reset_flag
+
+ def clear_reset_flag(self):
+ """清除重置标志"""
+ self.reset_flag = False
+
+
+class GapCorridorEnvironment:
+ """基于mujoco-py的带空隙走廊环境(使用自定义人形机器人模型)"""
+ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path="/home/qiqi/mujoco_ros_ws/src/humanoid_motion/xml/humanoid_2.xml"):
+ """
+ Args:
+ corridor_length: 走廊总长度
+ corridor_width: 走廊宽度
+ robot_xml_path: 自定义人形机器人XML文件路径
+ """
+ self.corridor_length = corridor_length
+ self.corridor_width = corridor_width
+ self.robot_xml_path = robot_xml_path # 自定义机器人模型路径
+ self.model = self._build_model() # 构建整合后的模型
+ self.sim = MjSim(self.model)
+ self.viewer = MjViewer(self.sim)
+ self.timestep = self.model.opt.timestep # 物理步长(从机器人XML读取,为0.005s)
+ self.control_timestep = 0.03 # 控制步长
+ self.control_steps = int(self.control_timestep / self.timestep) # 每个控制步包含的物理步
+ self._max_episode_steps = 30 / self.control_timestep # 总步数(30秒)
+ self.current_step = 0
+
+ def _parse_robot_xml(self):
+ """解析自定义机器人XML,提取需要的节点(身体、执行器、肌腱等)"""
+ tree = ET.parse(self.robot_xml_path)
+ root = tree.getroot()
+
+ # 提取机器人的身体定义(下的torso节点)
+ worldbody = root.find("worldbody")
+ robot_body = worldbody.find("body[@name='torso']") # 机器人主体
+ # 调整机器人初始位置(放在走廊起点,x=2.0处,避免初始在空隙)
+ robot_body.set("pos", "2.0 0 1.282") # 保持高度不变,x方向移动到走廊起点
+
+ # 提取执行器()、肌腱()、接触排除()定义
+ actuator = root.find("actuator")
+ tendon = root.find("tendon")
+ contact = root.find("contact")
+ asset = root.find("asset") # 机器人的材质/纹理定义
+ visual = root.find("visual") # 可视化配置
+ keyframe = root.find("keyframe") # 关键帧(可选,用于初始姿态)
+
+ return {
+ "robot_body": ET.tostring(robot_body, encoding="unicode"),
+ "actuator": ET.tostring(actuator, encoding="unicode") if actuator is not None else "",
+ "tendon": ET.tostring(tendon, encoding="unicode") if tendon is not None else "",
+ "contact": ET.tostring(contact, encoding="unicode") if contact is not None else "",
+ "asset": ET.tostring(asset, encoding="unicode") if asset is not None else "",
+ "visual": ET.tostring(visual, encoding="unicode") if visual is not None else "",
+ "keyframe": ET.tostring(keyframe, encoding="unicode") if keyframe is not None else ""
+ }
+
+ def _build_model(self):
+ """构建带空隙的走廊环境,并整合自定义人形机器人模型"""
+ # 解析自定义机器人XML
+ robot_parts = self._parse_robot_xml()
+
+ # 基础XML结构(走廊环境+机器人)
+ xml = f"""
+
+
+
+
+
+ {robot_parts['visual']}
+ {robot_parts['asset']}
+
+
+
+
+
+
+
+
+
+
+
+
+ {self._build_gaps_corridor()}
+
+
+ {robot_parts['robot_body']}
+
+
+
+ {robot_parts['contact']}
+
+
+ {robot_parts['tendon']}
+
+
+ {robot_parts['actuator']}
+
+
+ {robot_parts['keyframe']}
+
+ """
+ return load_model_from_xml(xml)
+
+ def _build_gaps_corridor(self):
+ """构建带空隙的走廊(平台+空隙交替)"""
+ gaps_xml = ""
+ platform_length = 2.0 # 平台长度
+ gap_length = 1.0 # 空隙长度
+ platform_thickness = 0.2 # 平台厚度
+ 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
+
+ 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() # 刷新物理状态
+ 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") # 躯干位置
+ 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
+
+ def step(self, action):
+ """执行动作并推进环境(动作维度需与机器人执行器数量匹配)"""
+ self.current_step += 1
+ # 应用动作到执行器(控制信号限制在[-1,1],你的机器人有20个执行器)
+ self.sim.data.ctrl[:] = np.clip(action, -1.0, 1.0)
+ # 执行多个物理步(匹配控制步长)
+ for _ in range(self.control_steps):
+ self.sim.step()
+ # 获取观测、奖励、完成状态
+ 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米视为跌倒
+ done = True
+ reward -= 1.0 # 跌倒额外惩罚
+ return obs, reward, done
+
+ def render(self):
+ """渲染画面"""
+ 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)
+
+
+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" # 确保路径正确
+ )
+
+ # 创建键盘控制器
+ action_dim = env.model.nu # 获取执行器数量
+ keyboard_controller = KeyboardController(action_dim)
+
+ print("\n环境已初始化,等待键盘输入...")
+
+ # 运行环境(键盘控制模式)
+ obs = env.reset()
+ total_reward = 0.0
+
+ try:
+ while not keyboard_controller.should_exit():
+ # 检查是否需要重置环境
+ if keyboard_controller.should_reset():
+ obs = env.reset()
+ total_reward = 0.0
+ keyboard_controller.clear_reset_flag()
+ print("环境已重置")
+
+ # 获取键盘控制动作
+ action = keyboard_controller.get_action()
+
+ # 执行一步
+ obs, reward, done = env.step(action)
+ total_reward += reward
+
+ # 渲染画面
+ env.render()
+
+ # 检查是否结束(机器人跌倒或达到最大步数)
+ 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()
+
+ # 控制循环频率(避免过快)
+ time.sleep(0.01)
+ except KeyboardInterrupt:
+ print("\n程序被中断")
+ finally:
+ keyboard_controller.exit_flag = True
+ print("程序已退出")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From adf9506edffc9fc057e1d094650a888f6bf17047 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 24 Nov 2025 10:34:44 +0800
Subject: [PATCH 38/45] =?UTF-8?q?=E4=BB=8E=E8=AF=BE=E7=A8=8B=E4=BB=93?=
=?UTF-8?q?=E5=BA=93main=E5=88=86=E6=94=AF=E6=81=A2=E5=A4=8Dmain=5Fhumanoi?=
=?UTF-8?q?d.py=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main_humanoid.py | 166 ++++++++++++++++++
1 file changed, 166 insertions(+)
create mode 100644 src/humanoid_simulation_environment_establish/main_humanoid.py
diff --git a/src/humanoid_simulation_environment_establish/main_humanoid.py b/src/humanoid_simulation_environment_establish/main_humanoid.py
new file mode 100644
index 0000000000..841b8e8552
--- /dev/null
+++ b/src/humanoid_simulation_environment_establish/main_humanoid.py
@@ -0,0 +1,166 @@
+#!/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 6684b9fa386d23874f4b5483288652cee9c304ed Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 25 Nov 2025 20:34:42 +0800
Subject: [PATCH 39/45] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=97=A7=E7=9A=84=20ma?=
=?UTF-8?q?in=5Fhumanoid.py=20=E6=96=87=E4=BB=B6(=E4=BC=98=E5=8C=96?=
=?UTF-8?q?=E7=9B=AE=E5=BD=95=E7=BB=93=E6=9E=84=E5=90=8E=E5=B7=B2=E4=B8=8D?=
=?UTF-8?q?=E9=9C=80=E8=A6=81=EF=BC=8C=E5=B9=B6=E4=BC=98=E5=8C=96=20main.p?=
=?UTF-8?q?y?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.py | 579 ++++++++++--------
.../main_humanoid.py | 166 -----
2 files changed, 331 insertions(+), 414 deletions(-)
delete mode 100644 src/humanoid_simulation_environment_establish/main_humanoid.py
diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py
index 0b2b13ba1e..e302c26daf 100644
--- a/src/humanoid_simulation_environment_establish/main.py
+++ b/src/humanoid_simulation_environment_establish/main.py
@@ -1,44 +1,37 @@
import numpy as np
-import mujoco_py
-from mujoco_py import load_model_from_xml, MjSim, MjViewer
+import mujoco
+from mujoco import viewer
import time
-import xml.etree.ElementTree as ET # 用于解析XML
-import threading
-import sys
-import platform
-
-# 跨平台键盘输入支持
-if platform.system() == 'Windows':
- import msvcrt
-else:
- import tty
- import termios
+from pathlib import Path
+import xml.etree.ElementTree as ET
class KeyboardController:
- """键盘控制节点:监听键盘输入并生成控制动作"""
- def __init__(self, action_dim):
+ """键盘控制节点:使用MuJoCo viewer的key_callback处理键盘输入"""
+ def __init__(self, action_dim, actuator_indices=None):
"""
Args:
action_dim: 动作维度(执行器数量)
+ actuator_indices: 执行器名称到索引的映射
"""
self.action_dim = action_dim
- self.current_action = np.zeros(action_dim) # 当前动作向量
+ self.actuator_indices = actuator_indices or {}
+ self.current_action = np.zeros(action_dim)
- # 控制状态
- self.exit_flag = False # 退出标志
- self.paused = False # 暂停标志
- self.reset_flag = False # 重置标志
+ self.exit_flag = False
+ self.paused = False
+ self.reset_flag = False
- # 平台检测
- self.is_windows = platform.system() == 'Windows'
+ # 移动控制状态
+ self.move_forward = False
+ self.move_backward = False
+ self.turn_left = False
+ self.turn_right = False
- # 键盘监听线程
- self.keyboard_thread = threading.Thread(target=self._keyboard_listener)
- self.keyboard_thread.daemon = True
- self.keyboard_thread.start()
+ # 步行动作时间计数器
+ self.step_time = 0.0
+ self.step_frequency = 1.6 # 步频 (Hz)
- # 打印操作说明
self._print_help()
def _print_help(self):
@@ -52,109 +45,176 @@ def _print_help(self):
print(" r: 重置环境")
print(" q: 退出程序")
print("=======================")
+ print("注意:请在查看器窗口内按键盘(窗口需要有焦点)\n")
- def _keyboard_listener(self):
- """独立线程:监听键盘输入并更新控制动作"""
- if self.is_windows:
- self._keyboard_listener_windows()
- else:
- self._keyboard_listener_unix()
-
- def _keyboard_listener_windows(self):
- """Windows平台键盘监听"""
- while not self.exit_flag:
- if msvcrt.kbhit(): # 检查是否有按键按下
- try:
- key_byte = msvcrt.getch()
- # 处理特殊键(方向键等以\x00或\xe0开头)
- if key_byte == b'\x00' or key_byte == b'\xe0':
- key_byte = msvcrt.getch() # 读取下一个字节
- # 映射方向键
- if key_byte == b'H': # ↑
- key = '\x1b[A'
- elif key_byte == b'P': # ↓
- key = '\x1b[B'
- elif key_byte == b'K': # ←
- key = '\x1b[D'
- elif key_byte == b'M': # →
- key = '\x1b[C'
- else:
- key = key_byte.decode('utf-8', errors='ignore')
- else:
- key = key_byte.decode('utf-8', errors='ignore')
- self._process_key(key)
- except Exception as e:
- pass # 忽略解码错误
- time.sleep(0.01) # 避免CPU占用过高
-
- def _keyboard_listener_unix(self):
- """Linux/Mac平台键盘监听"""
- fd = sys.stdin.fileno()
- old_settings = termios.tcgetattr(fd)
+ def key_callback(self, keycode):
+ """MuJoCo viewer的键盘回调函数"""
try:
- tty.setraw(sys.stdin.fileno())
- while not self.exit_flag:
- key = sys.stdin.read(1) # 读取单个字符
- self._process_key(key)
+ arrow_keys = {
+ 265: '\x1b[A', # 上箭头 (Up)
+ 264: '\x1b[B', # 下箭头 (Down)
+ 263: '\x1b[D', # 左箭头 (Left)
+ 262: '\x1b[C', # 右箭头 (Right)
+ }
+
+ if keycode in arrow_keys:
+ key = arrow_keys[keycode]
+ elif keycode == 32: # 空格键 (Space)
+ key = ' '
+ elif 32 <= keycode <= 126: # 可打印ASCII字符
+ key = chr(keycode).lower()
+ else:
+ return
+
+ self._process_key(key)
except Exception as e:
- print(f"键盘监听异常: {e}")
- finally:
- # 恢复终端默认设置
- termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+ print(f"[错误] 处理按键时出错 (keycode={keycode}): {e}")
+
+ def _set_action(self, action, name, value):
+ """根据执行器名称写入动作,自动忽略缺失的执行器"""
+ idx = self.actuator_indices.get(name)
+ if idx is not None and 0 <= idx < self.action_dim:
+ action[idx] = value
+
+ def _create_walking_action(self, forward=True, turn_direction=0):
+ """创建步行动作:基于周期的左右腿交替摆动"""
+ action = np.zeros(self.action_dim)
+
+ if not self.actuator_indices:
+ return action
+
+ # 计算步行动作相位
+ phase = 2 * np.pi * self.step_time * self.step_frequency
+ swing = np.sin(phase)
+ counter_swing = np.sin(phase + np.pi)
+ lift = np.maximum(0.0, np.sin(phase))
+ 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_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, "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_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, "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_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)
+ self._set_action(action, "hip_z_left", -turn_strength)
+
+ return action
def _process_key(self, key):
"""处理按键输入"""
- key_lower = key.lower()
+ if isinstance(key, str) and key.startswith('\x1b['):
+ key_char = None # 方向键用特殊序列表示
+ else:
+ key_char = key if isinstance(key, str) and len(key) == 1 else None
- # 基本运动控制(可以根据需要调整控制逻辑)
- base_action = np.zeros(self.action_dim)
+ # 处理移动指令(切换模式:每次按键切换状态)
+ 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
- if key_lower == 'w' or key == '\x1b[A': # 'w' 或 ↑ 键
- # 前进:激活前向关节(例如腿部前后摆动)
- base_action[:self.action_dim//2] = 0.3 # 前半部分执行器
- base_action[self.action_dim//2:] = -0.2 # 后半部分执行器
- self.current_action = base_action
- print("前进")
- elif key_lower == 's' or key == '\x1b[B': # 's' 或 ↓ 键
- # 后退:反向激活
- base_action[:self.action_dim//2] = -0.3
- base_action[self.action_dim//2:] = 0.2
- self.current_action = base_action
- print("后退")
- elif key_lower == 'a' or key == '\x1b[D': # 'a' 或 ← 键
- # 左转:左侧关节激活
- base_action[:self.action_dim//4] = 0.3
- base_action[self.action_dim//4:self.action_dim//2] = -0.2
- self.current_action = base_action
- print("左转")
- elif key_lower == 'd' or key == '\x1b[C': # 'd' 或 → 键
- # 右转:右侧关节激活
- base_action[self.action_dim//4:self.action_dim//2] = 0.3
- base_action[:self.action_dim//4] = -0.2
- self.current_action = base_action
- print("右转")
- elif key == ' ': # 空格键:暂停/继续
+ if key == ' ':
self.paused = not self.paused
if self.paused:
self.current_action = np.zeros(self.action_dim)
- print("⏸️ 已暂停")
- else:
- print("▶️ 继续")
- elif key_lower == 'r': # 重置标志
+ self.move_forward = False
+ self.move_backward = False
+ self.turn_left = False
+ self.turn_right = False
+ print(f"[键盘] {'⏸️ 已暂停' if self.paused else '▶️ 继续'}")
+ elif key_char == 'r':
self.reset_flag = True
- print("🔄 重置环境")
- elif key_lower == 'q': # 退出
+ print("[键盘] 🔄 重置环境")
+ elif key_char == 'q':
self.exit_flag = True
- print("❌ 准备退出程序...")
+ print("[键盘] ❌ 准备退出程序...")
+
+ def update_step_time(self, dt):
+ """更新步行动作时间"""
+ if not self.paused and (self.move_forward or self.move_backward or self.turn_left or self.turn_right):
+ self.step_time += dt
else:
- # 未识别的按键,停止动作
- if not self.paused:
- self.current_action = np.zeros(self.action_dim)
+ self.step_time = 0.0
- def get_action(self):
+ def get_action(self, dt=0.03):
"""获取当前控制动作"""
if self.paused:
return np.zeros(self.action_dim)
+
+ # 更新步行动作时间
+ self.update_step_time(dt)
+
+ # 根据移动状态创建动作
+ if self.move_forward:
+ turn_dir = 0
+ if self.turn_left:
+ turn_dir = -1
+ elif self.turn_right:
+ turn_dir = 1
+ self.current_action = self._create_walking_action(forward=True, turn_direction=turn_dir)
+ elif self.move_backward:
+ turn_dir = 0
+ if self.turn_left:
+ turn_dir = 1
+ elif self.turn_right:
+ 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)
+ else:
+ # 没有移动指令时,返回零动作或保持平衡的微小动作
+ self.current_action = np.zeros(self.action_dim)
+
return self.current_action.copy()
def should_exit(self):
@@ -171,8 +231,8 @@ def clear_reset_flag(self):
class GapCorridorEnvironment:
- """基于mujoco-py的带空隙走廊环境(使用自定义人形机器人模型)"""
- def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path="/home/qiqi/mujoco_ros_ws/src/humanoid_motion/xml/humanoid_2.xml"):
+ """基于mujoco的带空隙走廊环境(使用自定义人形机器人模型)"""
+ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path=None):
"""
Args:
corridor_length: 走廊总长度
@@ -181,44 +241,41 @@ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path="/home
"""
self.corridor_length = corridor_length
self.corridor_width = corridor_width
- self.robot_xml_path = robot_xml_path # 自定义机器人模型路径
- self.model = self._build_model() # 构建整合后的模型
- self.sim = MjSim(self.model)
- self.viewer = MjViewer(self.sim)
- self.timestep = self.model.opt.timestep # 物理步长(从机器人XML读取,为0.005s)
- self.control_timestep = 0.03 # 控制步长
- self.control_steps = int(self.control_timestep / self.timestep) # 每个控制步包含的物理步
- self._max_episode_steps = 30 / self.control_timestep # 总步数(30秒)
+ 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
+ xml_string = self._build_model()
+ self.model = mujoco.MjModel.from_xml_string(xml_string)
+ self.data = mujoco.MjData(self.model)
+ self.timestep = self.model.opt.timestep
+ self.control_timestep = 0.03
+ self.control_steps = int(self.control_timestep / self.timestep)
+ self._max_episode_steps = 30 / self.control_timestep
self.current_step = 0
+ self._actuator_indices = self._build_actuator_indices()
def _parse_robot_xml(self):
"""解析自定义机器人XML,提取需要的节点(身体、执行器、肌腱等)"""
tree = ET.parse(self.robot_xml_path)
root = tree.getroot()
-
- # 提取机器人的身体定义(下的torso节点)
- worldbody = root.find("worldbody")
- robot_body = worldbody.find("body[@name='torso']") # 机器人主体
- # 调整机器人初始位置(放在走廊起点,x=2.0处,避免初始在空隙)
- robot_body.set("pos", "2.0 0 1.282") # 保持高度不变,x方向移动到走廊起点
-
- # 提取执行器()、肌腱()、接触排除()定义
- actuator = root.find("actuator")
- tendon = root.find("tendon")
- contact = root.find("contact")
- asset = root.find("asset") # 机器人的材质/纹理定义
- visual = root.find("visual") # 可视化配置
- keyframe = root.find("keyframe") # 关键帧(可选,用于初始姿态)
-
- return {
- "robot_body": ET.tostring(robot_body, encoding="unicode"),
- "actuator": ET.tostring(actuator, encoding="unicode") if actuator is not None else "",
- "tendon": ET.tostring(tendon, encoding="unicode") if tendon is not None else "",
- "contact": ET.tostring(contact, encoding="unicode") if contact is not None else "",
- "asset": ET.tostring(asset, encoding="unicode") if asset is not None else "",
- "visual": ET.tostring(visual, encoding="unicode") if visual is not None else "",
- "keyframe": ET.tostring(keyframe, encoding="unicode") if keyframe is not None else ""
- }
+
+ robot_body = root.find("worldbody").find("body[@name='torso']")
+ robot_body.set("pos", "1.0 0.5 2")
+
+ # 提取XML节点并转换为字符串
+ single_nodes = ["actuator", "tendon", "contact", "asset", "visual", "keyframe", "statistic"]
+ parts = {"robot_body": ET.tostring(robot_body, encoding="unicode")}
+ for node_name in single_nodes:
+ node = root.find(node_name)
+ parts[node_name] = ET.tostring(node, encoding="unicode") if node is not None else ""
+ default_nodes = root.findall("default")
+ parts["default"] = "".join(ET.tostring(node, encoding="unicode") for node in default_nodes)
+
+ return parts
def _build_model(self):
"""构建带空隙的走廊环境,并整合自定义人形机器人模型"""
@@ -234,6 +291,7 @@ def _build_model(self):
{robot_parts['visual']}
{robot_parts['asset']}
+ {robot_parts['statistic']}
@@ -241,6 +299,7 @@ def _build_model(self):
+ {robot_parts['default']}
@@ -267,154 +326,178 @@ def _build_model(self):
{robot_parts['keyframe']}
"""
- 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/45] =?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"""
-
-
+
+
{robot_parts['visual']}
@@ -363,12 +384,37 @@ def _build_actuator_indices(self):
def get_actuator_indices(self):
return self._actuator_indices.copy()
+
+ def _find_root_joint_indices(self):
+ """找到根关节(freejoint)的位置和速度在qpos/qvel中的索引"""
+ try:
+ root_joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "root")
+ if root_joint_id >= 0:
+ self._root_joint_qpos_start = self.model.jnt_qposadr[root_joint_id]
+ self._root_joint_qvel_start = self.model.jnt_dofadr[root_joint_id]
+ self._root_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
+ print(f"[无重力模式] 找到根关节: qpos={self._root_joint_qpos_start}, qvel={self._root_joint_qvel_start}")
+ return
+ except Exception as e:
+ print(f"[警告] 查找根关节时出错: {e}")
+
+ # 使用默认值(通常freejoint是第一个关节)
+ self._root_joint_qpos_start = 0
+ self._root_joint_qvel_start = 0
+ self._root_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso") if self.model else None
+ print(f"[无重力模式] 使用默认根关节索引")
def reset(self):
"""重置环境到初始状态"""
self.current_step = 0
mujoco.mj_resetData(self.model, self.data)
mujoco.mj_forward(self.model, self.data)
+
+ # 无重力模式:记录根关节的初始Z高度和姿态
+ if not self.use_gravity and self._root_joint_qpos_start is not None:
+ self._initial_z_height = float(self.data.qpos[self._root_joint_qpos_start + 2])
+ print(f"[无重力模式] 记录初始Z高度: {self._initial_z_height:.4f},允许上身自由移动")
+
return self._get_observation()
def _get_observation(self):
@@ -404,13 +450,82 @@ def _get_reward(self):
break
return reward
+ def _apply_zero_gravity_constraints(self, action, before_step=True):
+ """应用无重力模式的约束:只固定Z高度,允许上身自由移动"""
+ if self.use_gravity or self._initial_z_height is None:
+ return
+
+ pos_start = self._root_joint_qpos_start
+ vel_start = self._root_joint_qvel_start
+
+ if pos_start is None or vel_start is None:
+ return
+
+ if before_step:
+ # mj_step前:只固定Z位置,不干扰其他物理量
+ if (pos_start + 2) < len(self.data.qpos):
+ self.data.qpos[pos_start + 2] = self._initial_z_height
+ # 清零Z方向速度,防止飘起
+ if (vel_start + 2) < len(self.data.qvel):
+ self.data.qvel[vel_start + 2] = 0.0
+ else:
+ # mj_step后:固定Z位置,应用XY速度控制
+ if (pos_start + 2) < len(self.data.qpos):
+ self.data.qpos[pos_start + 2] = self._initial_z_height
+ if (vel_start + 2) < len(self.data.qvel):
+ self.data.qvel[vel_start + 2] = 0.0
+
+ # XY速度控制(只在mj_step后)
+ if (vel_start + 2) <= len(self.data.qvel):
+ vx, vy = self.data.qvel[vel_start], self.data.qvel[vel_start + 1]
+
+ # 检测是否有主动移动
+ has_motion = False
+ if self._actuator_indices:
+ for name in ["hip_x_right", "hip_x_left"]:
+ idx = self._actuator_indices.get(name)
+ if idx is not None and abs(action[idx]) > 0.1:
+ has_motion = True
+ break
+
+ # 只在有主动移动时才应用轻微阻尼,允许自然移动
+ if has_motion:
+ # 有主动移动时,应用很小的阻尼,几乎不衰减
+ vx *= self._xy_damping
+ vy *= self._xy_damping
+ else:
+ # 没有主动移动时,应用中等阻尼以逐渐停止
+ damping = 0.90
+ vx *= damping
+ vy *= damping
+
+ # 只限制最大速度,不干扰正常移动
+ speed = np.sqrt(vx * vx + vy * vy)
+ if speed > self._max_xy_velocity:
+ scale = self._max_xy_velocity / speed
+ vx *= scale
+ vy *= scale
+
+ self.data.qvel[vel_start] = vx
+ self.data.qvel[vel_start + 1] = vy
+
def step(self, action):
"""执行动作并推进环境"""
self.current_step += 1
self.data.ctrl[:] = np.clip(action, -1.0, 1.0)
for _ in range(self.control_steps):
+ # mj_step前应用约束
+ self._apply_zero_gravity_constraints(action, before_step=True)
+
mujoco.mj_step(self.model, self.data)
+
+ # mj_step后应用约束
+ self._apply_zero_gravity_constraints(action, before_step=False)
+
+ # 更新物理状态
+ if not self.use_gravity:
+ mujoco.mj_forward(self.model, self.data)
obs = self._get_observation()
reward = self._get_reward()
@@ -431,7 +546,8 @@ def render(self, viewer_handle=None):
def main():
- env = GapCorridorEnvironment(corridor_length=100, corridor_width=10)
+ # 将环境切换为“无重力”模式
+ env = GapCorridorEnvironment(corridor_length=100, corridor_width=10, use_gravity=False)
print("\n环境已初始化")
print(f"执行器数量: {env.model.nu}")
@@ -501,4 +617,4 @@ def main():
print("程序已退出")
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()
From f045055681b53961b41a355efb9542a6daa1105b Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Mon, 8 Dec 2025 10:17:21 +0800
Subject: [PATCH 41/45] =?UTF-8?q?=E4=BC=98=E5=8C=96main.py=E4=BA=BA?=
=?UTF-8?q?=E5=BD=A2=E6=9C=BA=E5=99=A8=E4=BA=BA=E6=8E=A7=E5=88=B6=E9=80=BB?=
=?UTF-8?q?=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.py | 457 +++++++++++++++---
1 file changed, 394 insertions(+), 63 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py
index fae341266f..c1f32fa75d 100644
--- a/src/humanoid_simulation_environment_establish/main.py
+++ b/src/humanoid_simulation_environment_establish/main.py
@@ -4,6 +4,7 @@
import time
from pathlib import Path
import xml.etree.ElementTree as ET
+from collections import deque
class KeyboardController:
@@ -28,13 +29,154 @@ def __init__(self, action_dim, actuator_indices=None):
self.turn_left = False
self.turn_right = False
- # 步行动作时间计数器
+ # 步行动作时间计数器(改为基于键盘输入的脉冲式控制)
self.step_time = 0.0
- self.step_frequency = 1.6 # 步频 (Hz)
+ self.step_frequency = 1.2 # 步频 (Hz)
+ self.step_duration = 0.5 # 每次按键的移动持续时间(秒)
+ self.last_action_time = 0.0 # 上次执行动作的时间
+
+ # 动作平滑:使用低通滤波和滑动平均
+ self.action_smoothing_factor = 0.7 # 动作平滑系数(减小以更快停止)
+ self.smoothed_action = np.zeros(action_dim)
+ self.action_history = deque(maxlen=3) # 减少历史长度,更快响应
+
+ # PID控制器参数(用于速度控制)
+ self.velocity_pid = {
+ 'kp': 2.0, # 比例增益
+ 'ki': 0.1, # 积分增益
+ 'kd': 0.5, # 微分增益
+ 'integral': np.array([0.0, 0.0]), # 积分项
+ 'last_error': np.array([0.0, 0.0]) # 上次误差
+ }
+
+ # 目标速度(根据键盘输入设置)
+ self.target_velocity = np.array([0.0, 0.0]) # [vx, vy]
+ self.current_velocity = np.array([0.0, 0.0])
+
+ # 转向控制:累积转向角度,每次转向约45度
+ self.target_turn_angle = 0.0 # 目标转向角度(弧度)
+ self.current_turn_angle = 0.0 # 当前转向角度(弧度)
+ self.turn_angle_per_step = np.pi / 4.0 # 每次转向目标角度:45度(π/4弧度)
+ self.turn_speed = 2.0 # 转向速度(弧度/秒)
+ # 简单的神经网络控制器(用于动作平滑)
+ self.use_neural_smoothing = True
+ self._init_neural_smoother()
self._print_help()
+ def _init_neural_smoother(self):
+ """初始化简单的神经网络平滑器(单层感知机)"""
+ # 简单的单层神经网络,用于学习动作平滑映射
+ # 输入:当前动作 + 历史动作(最近3个)
+ # 输出:平滑后的动作
+ input_dim = self.action_dim * 4 # 当前 + 3个历史
+ hidden_dim = self.action_dim * 2
+ output_dim = self.action_dim
+
+ # 使用简单的权重矩阵(可以后续用训练数据优化)
+ np.random.seed(42)
+ self.neural_weights1 = np.random.randn(input_dim, hidden_dim) * 0.1
+ self.neural_weights2 = np.random.randn(hidden_dim, output_dim) * 0.1
+ self.neural_bias1 = np.zeros(hidden_dim)
+ self.neural_bias2 = np.zeros(output_dim)
+
+ # 激活函数:ReLU + Tanh
+ self.neural_history = deque(maxlen=3)
+
+ def _neural_smooth_action(self, action):
+ """使用神经网络平滑动作"""
+ if not self.use_neural_smoothing or len(self.neural_history) < 2:
+ # 历史不足时,使用简单平滑
+ return self._simple_smooth_action(action)
+
+ # 构建输入:当前动作 + 历史动作
+ history_actions = list(self.neural_history)
+ while len(history_actions) < 3:
+ history_actions.insert(0, np.zeros(self.action_dim))
+
+ input_vec = np.concatenate([
+ action,
+ history_actions[0],
+ history_actions[1] if len(history_actions) > 1 else np.zeros(self.action_dim),
+ history_actions[2] if len(history_actions) > 2 else np.zeros(self.action_dim)
+ ])
+
+ # 前向传播
+ hidden = np.maximum(0, input_vec @ self.neural_weights1 + self.neural_bias1) # ReLU
+ output = np.tanh(hidden @ self.neural_weights2 + self.neural_bias2) # Tanh
+
+ # 混合原始动作和平滑动作
+ smoothed = 0.7 * action + 0.3 * output
+ return np.clip(smoothed, -1.0, 1.0)
+
+ def _simple_smooth_action(self, action):
+ """简单的动作平滑(低通滤波 + 滑动平均)"""
+ # 检查动作是否为零(停止指令)
+ if np.max(np.abs(action)) < 0.01:
+ # 停止时,快速衰减
+ self.smoothed_action = self.smoothed_action * 0.6
+ if np.max(np.abs(self.smoothed_action)) < 0.01:
+ self.smoothed_action = np.zeros(self.action_dim)
+ else:
+ # 有动作时,使用低通滤波
+ self.smoothed_action = (
+ self.action_smoothing_factor * self.smoothed_action +
+ (1 - self.action_smoothing_factor) * action
+ )
+
+ # 滑动平均(只在有动作时)
+ self.action_history.append(action.copy())
+ if len(self.action_history) > 1:
+ avg_action = np.mean(list(self.action_history), axis=0)
+ # 混合低通滤波和滑动平均
+ self.smoothed_action = 0.7 * self.smoothed_action + 0.3 * avg_action
+
+ return np.clip(self.smoothed_action, -1.0, 1.0)
+
+ def _update_pid_controller(self, target_vel, current_vel, dt):
+ """更新PID控制器,计算速度修正"""
+ error = target_vel - current_vel
+
+ # 比例项
+ p_term = self.velocity_pid['kp'] * error
+
+ # 积分项(带抗饱和)
+ self.velocity_pid['integral'] += error * dt
+ self.velocity_pid['integral'] = np.clip(
+ self.velocity_pid['integral'],
+ -2.0, 2.0 # 限制积分项,防止积分饱和
+ )
+ i_term = self.velocity_pid['ki'] * self.velocity_pid['integral']
+
+ # 微分项
+ d_error = (error - self.velocity_pid['last_error']) / dt
+ d_term = self.velocity_pid['kd'] * d_error
+
+ # 更新上次误差
+ self.velocity_pid['last_error'] = error.copy()
+
+ # PID输出
+ pid_output = p_term + i_term + d_term
+ return pid_output
+
+ def _update_target_velocity(self):
+ """根据键盘输入更新目标速度"""
+ # 重置目标速度
+ self.target_velocity = np.array([0.0, 0.0])
+
+ # 根据移动状态设置目标速度
+ if self.move_forward:
+ self.target_velocity[0] = 1.0 # 前进速度
+ elif self.move_backward:
+ self.target_velocity[0] = -0.8 # 后退速度
+
+ # 转向速度(通过旋转实现,这里先设为0,由转向动作控制)
+ if self.turn_left:
+ self.target_velocity[1] = -0.3 # 左转
+ elif self.turn_right:
+ self.target_velocity[1] = 0.3 # 右转
+
def _print_help(self):
"""打印键盘控制指令说明"""
print("\n===== 键盘控制指令 =====")
@@ -78,7 +220,7 @@ def _set_action(self, action, name, value):
action[idx] = value
def _create_walking_action(self, forward=True, turn_direction=0):
- """创建步行动作:基于周期的左右腿交替摆动"""
+ """创建步行动作:基于周期的左右腿交替摆动,更自然的步态"""
action = np.zeros(self.action_dim)
if not self.actuator_indices:
@@ -86,53 +228,114 @@ def _create_walking_action(self, forward=True, turn_direction=0):
# 计算步行动作相位
phase = 2 * np.pi * self.step_time * self.step_frequency
- swing = np.sin(phase)
- counter_swing = np.sin(phase + np.pi)
- lift = np.maximum(0.0, np.sin(phase))
- counter_lift = np.maximum(0.0, np.sin(phase + np.pi))
direction = 1 if forward else -1
- # 躯干控制
- 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.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.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.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.1 * counter_lift)
- self._set_action(action, "ankle_x_left", -0.2 * counter_swing)
-
- # 转向控制
+ # 使用更自然的步态模式:区分支撑相和摆动相
+ # 右腿相位
+ right_phase = phase
+ # 左腿相位(相差180度)
+ left_phase = phase + np.pi
+
+ # 计算摆动相和支撑相(使用平滑的过渡)
+ # 摆动相:0到π,支撑相:π到2π
+ right_swing_phase = (right_phase % (2 * np.pi)) / np.pi # 归一化到0-2
+ left_swing_phase = (left_phase % (2 * np.pi)) / np.pi
+
+ # 右腿:更自然的步态
+ # 髋关节前后摆动(主要推进力)
+ # 右腿向前摆动时产生推进力
+ right_hip_swing = 0.6 * direction * np.sin(right_phase)
+ self._set_action(action, "hip_x_right", right_hip_swing)
+
+ # 髋关节上下(抬腿)
+ right_hip_lift = 0.2 * max(0, np.sin(right_phase)) # 只在摆动相抬腿
+ self._set_action(action, "hip_y_right", -right_hip_lift)
+
+ # 膝关节(在摆动相弯曲,支撑相伸直)
+ right_knee_angle = 0.5 * (1 - np.cos(right_phase)) # 0到1的平滑变化
+ self._set_action(action, "knee_right", 0.6 * right_knee_angle)
+
+ # 踝关节(配合抬腿)
+ self._set_action(action, "ankle_y_right", -0.15 * max(0, np.sin(right_phase)))
+ self._set_action(action, "ankle_x_right", 0.15 * np.sin(right_phase))
+
+ # 左腿(相位相反,摆动方向也相反以产生推进力)
+ # 关键:左腿的摆动与右腿相反(当右腿向前时,左腿向后)
+ # 右腿:0.6 * direction * sin(phase)
+ # 左腿:-0.6 * direction * sin(phase) (直接使用负号,确保与右腿相反)
+ # 这样差异 = 0.6*direction*sin(phase) - (-0.6*direction*sin(phase)) = 1.2*direction*sin(phase)
+ # 能产生有效的推进力!
+ left_hip_swing = -0.6 * direction * np.sin(right_phase) # 直接使用右腿相位的负值
+ self._set_action(action, "hip_x_left", left_hip_swing)
+
+ left_hip_lift = 0.2 * max(0, np.sin(left_phase))
+ self._set_action(action, "hip_y_left", -left_hip_lift)
+
+ left_knee_angle = 0.5 * (1 - np.cos(left_phase))
+ self._set_action(action, "knee_left", 0.6 * left_knee_angle)
+
+ self._set_action(action, "ankle_y_left", -0.15 * max(0, np.sin(left_phase)))
+ self._set_action(action, "ankle_x_left", -0.15 * np.sin(left_phase))
+
+ # 躯干控制(轻微前倾以辅助前进)
+ self._set_action(action, "abdomen_y", 0.2 * direction)
+ self._set_action(action, "abdomen_x", 0.1 * turn_direction)
+
+ # 转向控制(更平滑,增大转向幅度)
if turn_direction != 0:
- turn_strength = 0.5 * turn_direction
+ turn_strength = 0.5 * turn_direction # 从0.3增大到0.5
+ # 转向时,外侧腿稍微外展,内侧腿稍微内收
self._set_action(action, "hip_z_right", turn_strength)
self._set_action(action, "hip_z_left", -turn_strength)
+ # 添加躯干旋转辅助转向
+ self._set_action(action, "abdomen_z", 0.4 * turn_direction) # 添加躯干旋转
return action
- def _create_turning_only_action(self, turn_direction):
- """创建仅转向动作(不产生腿部摆动,只在原地转向)"""
+ def _create_turning_only_action(self, turn_direction, dt=0.03):
+ """创建仅转向动作(不产生腿部摆动,只在原地转向,目标转向45度)"""
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)
+ # 更新目标转向角度(每次按键设置目标为45度)
+ turn_velocity = 0.0
+ if turn_direction != 0:
+ # 计算转向误差
+ turn_error = self.target_turn_angle - self.current_turn_angle
+
+ # 如果接近目标角度,重置目标(允许连续转向)
+ if abs(turn_error) < 0.1: # 接近目标时,设置新的目标
+ self.target_turn_angle += turn_direction * self.turn_angle_per_step
+
+ # 计算转向速度(基于误差)
+ turn_velocity = np.clip(turn_error * 3.0, -self.turn_speed, self.turn_speed)
+
+ # 更新当前转向角度(模拟)
+ self.current_turn_angle += turn_velocity * dt
+ else:
+ # 没有转向指令时,逐渐减小转向角度
+ self.current_turn_angle *= 0.95
+ self.target_turn_angle = self.current_turn_angle # 同步目标角度
+
+ # 根据转向速度计算转向强度(归一化到-1到1)
+ if abs(turn_velocity) > 0.01:
+ normalized_turn = np.clip(turn_velocity / self.turn_speed, -1.0, 1.0)
+ else:
+ # 如果没有转向速度,直接使用方向(简化控制)
+ normalized_turn = turn_direction * 0.8 # 直接使用方向,强度0.8
- # 可以添加轻微的躯干倾斜来辅助转向
- self._set_action(action, "abdomen_x", 0.1 * turn_direction)
+ # 原地转向:通过髋关节外展和躯干旋转实现
+ # 增大转向强度,使转向更明显
+ hip_turn_strength = 0.6 * normalized_turn # 从0.25增大到0.6
+ self._set_action(action, "hip_z_right", hip_turn_strength)
+ self._set_action(action, "hip_z_left", -hip_turn_strength)
+
+ # 躯干旋转辅助转向(主要转向来源,范围±45度)
+ abdomen_turn_strength = 0.8 * normalized_turn # 从0.15增大到0.8,充分利用±45度范围
+ self._set_action(action, "abdomen_z", abdomen_turn_strength)
+ self._set_action(action, "abdomen_x", 0.1 * normalized_turn)
return action
@@ -155,12 +358,19 @@ def _process_key(self, key):
if (key_char == key1) or (key == key2):
current_state = getattr(self, attr)
if current_state:
+ # 停止移动时,立即重置相关状态
setattr(self, attr, False)
+ self.step_time = 0.0 # 重置步行动作时间
+ # 快速清零平滑动作
+ if not (self.move_forward or self.move_backward or self.turn_left or self.turn_right):
+ self.smoothed_action = np.zeros(self.action_dim)
print(f"[键盘] {stop_msg}")
else:
setattr(self, attr, True)
if hasattr(self, opposite_attr):
setattr(self, opposite_attr, False)
+ # 开始移动时,重置步行动作时间
+ self.step_time = 0.0
print(f"[键盘] {start_msg}")
return
@@ -181,18 +391,42 @@ def _process_key(self, key):
print("[键盘] ❌ 准备退出程序...")
def update_step_time(self, dt):
- """更新步行动作时间"""
+ """更新步行动作时间(只在有键盘输入时更新)"""
if not self.paused and (self.move_forward or self.move_backward or self.turn_left or self.turn_right):
self.step_time += dt
else:
+ # 没有键盘输入时,立即重置时间,停止动作
self.step_time = 0.0
- def get_action(self, dt=0.03):
- """获取当前控制动作"""
+ def get_action(self, dt=0.03, current_velocity=None):
+ """获取当前控制动作(基于键盘输入的离散控制)"""
if self.paused:
+ self.smoothed_action = np.zeros(self.action_dim)
+ self.target_velocity = np.array([0.0, 0.0])
+ self.step_time = 0.0
return np.zeros(self.action_dim)
- # 更新步行动作时间
+ # 更新当前速度(如果提供)
+ if current_velocity is not None:
+ self.current_velocity = current_velocity.copy()
+
+ # 更新目标速度
+ self._update_target_velocity()
+
+ # 检查是否有任何移动指令
+ has_movement = self.move_forward or self.move_backward or self.turn_left or self.turn_right
+
+ if not has_movement:
+ # 没有键盘输入时,立即停止并清零动作
+ self.step_time = 0.0
+ # 快速衰减到零
+ self.smoothed_action = self.smoothed_action * 0.5
+ if np.max(np.abs(self.smoothed_action)) < 0.01:
+ self.smoothed_action = np.zeros(self.action_dim)
+ self.current_action = self.smoothed_action.copy()
+ return self.current_action.copy()
+
+ # 有键盘输入时,更新步行动作时间
self.update_step_time(dt)
# 根据移动状态创建动作
@@ -202,22 +436,30 @@ def get_action(self, dt=0.03):
turn_dir = -1
elif self.turn_right:
turn_dir = 1
- self.current_action = self._create_walking_action(forward=True, turn_direction=turn_dir)
+ raw_action = self._create_walking_action(forward=True, turn_direction=turn_dir)
elif self.move_backward:
turn_dir = 0
if self.turn_left:
turn_dir = 1
elif self.turn_right:
turn_dir = -1
- self.current_action = self._create_walking_action(forward=False, turn_direction=turn_dir)
+ raw_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_turning_only_action(turn_dir)
+ raw_action = self._create_turning_only_action(turn_dir, dt=dt)
+ else:
+ # 不应该到这里,但以防万一
+ raw_action = np.zeros(self.action_dim)
+
+ # 应用动作平滑(但使用更小的平滑系数,使停止更快)
+ if self.use_neural_smoothing and len(self.neural_history) >= 2:
+ smoothed = self._neural_smooth_action(raw_action)
+ self.neural_history.append(raw_action.copy())
else:
- # 没有移动指令时,返回零动作或保持平衡的微小动作
- self.current_action = np.zeros(self.action_dim)
+ smoothed = self._simple_smooth_action(raw_action)
+ self.current_action = smoothed
return self.current_action.copy()
def should_exit(self):
@@ -274,7 +516,9 @@ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path=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._xy_damping = 0.99 # XY速度阻尼系数(减小阻尼,保持速度)
+ self._forward_velocity_gain = 2.5 # 前进速度增益(增大增益,产生明显移动)
+ self._turn_velocity_gain = 0.5 # 转向速度增益
self._find_root_joint_indices()
def _parse_robot_xml(self):
@@ -451,7 +695,7 @@ def _get_reward(self):
return reward
def _apply_zero_gravity_constraints(self, action, before_step=True):
- """应用无重力模式的约束:只固定Z高度,允许上身自由移动"""
+ """应用无重力模式的约束:只固定Z高度,允许上身自由移动,并根据动作主动施加速度"""
if self.use_gravity or self._initial_z_height is None:
return
@@ -479,27 +723,73 @@ def _apply_zero_gravity_constraints(self, action, before_step=True):
if (vel_start + 2) <= len(self.data.qvel):
vx, vy = self.data.qvel[vel_start], self.data.qvel[vel_start + 1]
- # 检测是否有主动移动
- has_motion = False
+ # 根据动作计算期望速度
+ desired_vx = 0.0
+ desired_vy = 0.0
+
+ # 获取躯干朝向(从根关节的四元数)
+ yaw = 0.0
+ if pos_start + 6 < len(self.data.qpos):
+ # 提取四元数(w, x, y, z)
+ qw = self.data.qpos[pos_start + 3]
+ qx = self.data.qpos[pos_start + 4]
+ qy = self.data.qpos[pos_start + 5]
+ qz = self.data.qpos[pos_start + 6]
+ # 计算绕Z轴的旋转角度(yaw)
+ yaw = np.arctan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+ # 检测前进/后退动作(通过髋关节前后摆动判断)
if self._actuator_indices:
- for name in ["hip_x_right", "hip_x_left"]:
- idx = self._actuator_indices.get(name)
- if idx is not None and abs(action[idx]) > 0.1:
- has_motion = True
- break
+ hip_x_right_idx = self._actuator_indices.get("hip_x_right")
+ hip_x_left_idx = self._actuator_indices.get("hip_x_left")
+
+ if hip_x_right_idx is not None and hip_x_left_idx is not None:
+ # 计算髋关节前后摆动的差异
+ # 当两腿摆动方向相反时,产生前进力
+ hip_x_right = action[hip_x_right_idx]
+ hip_x_left = action[hip_x_left_idx]
+ hip_x_diff = hip_x_right - hip_x_left
+
+ # 直接使用差异来计算速度(差异已经包含了方向和强度信息)
+ # 当右腿向前、左腿向后时,差异为正,产生前进速度
+ # 当右腿向后、左腿向前时,差异为负,产生后退速度
+ local_forward_vel = hip_x_diff * self._forward_velocity_gain
+
+ # 如果差异很小,也可以使用平均摆动幅度作为备用
+ if abs(local_forward_vel) < 0.1:
+ hip_x_avg_amplitude = (abs(hip_x_right) + abs(hip_x_left)) / 2.0
+ if hip_x_avg_amplitude > 0.1:
+ # 根据右腿的摆动方向确定前进方向
+ direction_sign = 1.0 if hip_x_right > 0 else -1.0
+ local_forward_vel = hip_x_avg_amplitude * direction_sign * self._forward_velocity_gain * 0.8
+
+ # 根据躯干朝向,将局部前进速度转换到世界坐标系
+ desired_vx = local_forward_vel * np.cos(yaw)
+ desired_vy = local_forward_vel * np.sin(yaw)
- # 只在有主动移动时才应用轻微阻尼,允许自然移动
- if has_motion:
- # 有主动移动时,应用很小的阻尼,几乎不衰减
+ # 应用速度平滑过渡(使用更平滑的混合策略,减少震荡)
+ if abs(desired_vx) > 0.01 or abs(desired_vy) > 0.01:
+ # 有主动移动时,使用更平滑的过渡
+ # 使用更小的平滑系数,减少震荡
+ alpha = 0.4 # 平滑系数
+ vx = vx * (1 - alpha) + desired_vx * alpha
+ vy = vy * (1 - alpha) + desired_vy * alpha
+ # 应用轻微阻尼(几乎不衰减,保持速度)
vx *= self._xy_damping
vy *= self._xy_damping
else:
- # 没有主动移动时,应用中等阻尼以逐渐停止
- damping = 0.90
+ # 没有主动移动时,快速停止
+ damping = 0.85 # 增大阻尼,使停止更快
vx *= damping
vy *= damping
+
+ # 如果速度很小,直接清零以避免微小震荡
+ if abs(vx) < 0.05:
+ vx = 0.0
+ if abs(vy) < 0.05:
+ vy = 0.0
- # 只限制最大速度,不干扰正常移动
+ # 限制最大速度
speed = np.sqrt(vx * vx + vy * vy)
if speed > self._max_xy_velocity:
scale = self._max_xy_velocity / speed
@@ -572,6 +862,8 @@ def main():
print("\n查看器已启动,开始仿真循环...")
step = 0
+ last_move_state = None # 记录上次移动状态,用于检测状态变化
+
while viewer_handle.is_running() and not controller.should_exit():
if controller.should_reset():
obs = env.reset()
@@ -583,10 +875,44 @@ def main():
controller.turn_left = False
controller.turn_right = False
controller.step_time = 0.0
+ # 重置PID控制器
+ controller.velocity_pid['integral'] = np.array([0.0, 0.0])
+ controller.velocity_pid['last_error'] = np.array([0.0, 0.0])
+ controller.target_velocity = np.array([0.0, 0.0])
+ controller.smoothed_action = np.zeros(controller.action_dim)
+ controller.action_history.clear()
+ controller.neural_history.clear()
+ last_move_state = None
controller.clear_reset_flag()
- # 获取动作(传入控制步长以更新步行动作)
- action = controller.get_action(dt=env.control_timestep)
+ # 检测移动状态变化,重置PID控制器以避免震荡
+ current_move_state = (
+ controller.move_forward,
+ controller.move_backward,
+ controller.turn_left,
+ controller.turn_right
+ )
+ if current_move_state != last_move_state:
+ # 状态改变时,重置PID积分项,避免累积误差导致震荡
+ controller.velocity_pid['integral'] = np.array([0.0, 0.0])
+ controller.velocity_pid['last_error'] = np.array([0.0, 0.0])
+ last_move_state = current_move_state
+
+ # 获取当前速度(用于PID控制)
+ if not env.use_gravity and env._root_joint_qvel_start is not None:
+ vel_start = env._root_joint_qvel_start
+ if (vel_start + 2) <= len(env.data.qvel):
+ current_vel = np.array([
+ env.data.qvel[vel_start],
+ env.data.qvel[vel_start + 1]
+ ])
+ else:
+ current_vel = np.array([0.0, 0.0])
+ else:
+ current_vel = np.array([0.0, 0.0])
+
+ # 获取动作(传入控制步长和当前速度)
+ action = controller.get_action(dt=env.control_timestep, current_velocity=current_vel)
obs, reward, done = env.step(action)
total_reward += reward
@@ -595,7 +921,12 @@ def main():
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}")
+ head_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "head")
+ head_pos = env.data.xpos[head_id] if head_id >= 0 else None
+ if head_pos is not None:
+ print(f"Step {step}: 躯干位置 = {torso_pos}, 头部位置 = {head_pos}, 累计奖励 = {total_reward:.2f}")
+ else:
+ print(f"Step {step}: 躯干位置 = {torso_pos}, 累计奖励 = {total_reward:.2f}")
if done:
print(f"\nEpisode finished. Total reward: {total_reward:.2f}")
From 25d5bb6ed110e2f2d492f84b24f8aed2245c4f80 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Fri, 12 Dec 2025 15:11:13 +0800
Subject: [PATCH 42/45] =?UTF-8?q?=E9=9B=86=E6=88=90=E6=B7=B1=E5=BA=A6?=
=?UTF-8?q?=E5=AD=A6=E4=B9=A0=E6=8E=A7=E5=88=B6=E5=99=A8=EF=BC=8C=E4=BC=98?=
=?UTF-8?q?=E5=8C=96=E6=AD=A5=E6=80=81=E8=87=AA=E7=84=B6=E5=BA=A6=E4=B8=8E?=
=?UTF-8?q?=E6=97=A0=E9=87=8D=E5=8A=9B=E6=A8=A1=E5=BC=8F=E7=A8=B3=E5=AE=9A?=
=?UTF-8?q?=E6=80=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.py | 1075 +++++++++++++++--
1 file changed, 951 insertions(+), 124 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py
index 2a5cfbf775..9b72a715d9 100644
--- a/src/humanoid_simulation_environment_establish/main.py
+++ b/src/humanoid_simulation_environment_establish/main.py
@@ -5,6 +5,327 @@
from pathlib import Path
import xml.etree.ElementTree as ET
from collections import deque
+import pickle
+import os
+
+
+class DeepLearningController:
+ """深度学习控制器:使用神经网络学习最优步态和姿态控制"""
+
+ def __init__(self, action_dim, state_dim, actuator_indices=None, learning_rate=0.001):
+ """
+ Args:
+ action_dim: 动作维度(执行器数量)
+ state_dim: 状态维度(观测空间大小)
+ actuator_indices: 执行器名称到索引的映射
+ learning_rate: 学习率
+ """
+ self.action_dim = action_dim
+ self.state_dim = state_dim
+ self.actuator_indices = actuator_indices or {}
+ self.learning_rate = learning_rate
+
+ # 策略网络:根据状态预测动作
+ self.policy_network = self._build_policy_network()
+
+ # 价值网络:评估状态价值(用于强化学习)
+ self.value_network = self._build_value_network()
+
+ # 经验回放缓冲区
+ self.replay_buffer = deque(maxlen=10000)
+ self.batch_size = 64
+
+ # 训练相关
+ self.training_enabled = True
+ self.update_frequency = 10 # 每10步更新一次
+ self.step_count = 0
+
+ # 历史状态和动作(用于时序学习)
+ self.state_history = deque(maxlen=10)
+ self.action_history = deque(maxlen=10)
+
+ # 步态学习参数
+ self.gait_phase = 0.0 # 步态相位
+ self.gait_frequency = 1.2 # 步频
+
+ print(f"[深度学习控制器] 初始化完成: 动作维度={action_dim}, 状态维度={state_dim}")
+
+ def _build_policy_network(self):
+ """构建策略网络(MLP + LSTM混合)"""
+ # 输入:状态 + 步态相位编码(sin, cos)+ 历史动作
+ input_dim = self.state_dim + 2 + self.action_dim # 状态 + 相位编码(2维) + 上次动作
+ hidden1_dim = 128
+ hidden2_dim = 64
+ lstm_dim = 32
+ output_dim = self.action_dim
+
+ # 初始化权重(使用Xavier初始化)
+ np.random.seed(42)
+
+ # 第一层MLP
+ self.policy_w1 = np.random.randn(input_dim, hidden1_dim) * np.sqrt(2.0 / input_dim)
+ self.policy_b1 = np.zeros(hidden1_dim)
+
+ # 第二层MLP
+ self.policy_w2 = np.random.randn(hidden1_dim, hidden2_dim) * np.sqrt(2.0 / hidden1_dim)
+ self.policy_b2 = np.zeros(hidden2_dim)
+
+ # LSTM层(简化版:只保留隐藏状态)
+ self.policy_lstm_h = np.zeros(lstm_dim)
+ self.policy_lstm_c = np.zeros(lstm_dim)
+ # w_lstm需要分成两部分:forget_gate和input_gate,所以需要2*lstm_dim列
+ self.policy_w_lstm = np.random.randn(hidden2_dim + lstm_dim, 2 * lstm_dim) * 0.1
+ self.policy_w_lstm_out = np.random.randn(hidden2_dim + lstm_dim, lstm_dim) * 0.1
+
+ # 输出层
+ self.policy_w3 = np.random.randn(lstm_dim, output_dim) * np.sqrt(2.0 / lstm_dim)
+ self.policy_b3 = np.zeros(output_dim)
+
+ return {
+ 'w1': self.policy_w1, 'b1': self.policy_b1,
+ 'w2': self.policy_w2, 'b2': self.policy_b2,
+ 'w3': self.policy_w3, 'b3': self.policy_b3,
+ 'lstm_h': self.policy_lstm_h, 'lstm_c': self.policy_lstm_c,
+ 'w_lstm': self.policy_w_lstm, 'w_lstm_out': self.policy_w_lstm_out
+ }
+
+ def _build_value_network(self):
+ """构建价值网络(评估状态价值)"""
+ input_dim = self.state_dim
+ hidden1_dim = 64
+ hidden2_dim = 32
+ output_dim = 1
+
+ np.random.seed(43)
+
+ self.value_w1 = np.random.randn(input_dim, hidden1_dim) * np.sqrt(2.0 / input_dim)
+ self.value_b1 = np.zeros(hidden1_dim)
+ self.value_w2 = np.random.randn(hidden1_dim, hidden2_dim) * np.sqrt(2.0 / hidden1_dim)
+ self.value_b2 = np.zeros(hidden2_dim)
+ self.value_w3 = np.random.randn(hidden2_dim, output_dim) * np.sqrt(2.0 / hidden2_dim)
+ self.value_b3 = np.zeros(output_dim)
+
+ return {
+ 'w1': self.value_w1, 'b1': self.value_b1,
+ 'w2': self.value_w2, 'b2': self.value_b2,
+ 'w3': self.value_w3, 'b3': self.value_b3
+ }
+
+ def _relu(self, x):
+ """ReLU激活函数"""
+ return np.maximum(0, x)
+
+ def _tanh(self, x):
+ """Tanh激活函数"""
+ return np.tanh(x)
+
+ def _sigmoid(self, x):
+ """Sigmoid激活函数"""
+ return 1.0 / (1.0 + np.exp(-np.clip(x, -500, 500)))
+
+ def predict_action(self, state, gait_phase, last_action, command=None):
+ """
+ 预测动作
+
+ Args:
+ state: 当前状态(观测)
+ gait_phase: 步态相位 [0, 2π]
+ last_action: 上次动作
+ command: 用户命令 (forward, backward, turn_left, turn_right)
+
+ Returns:
+ 预测的动作
+ """
+ # 构建输入
+ if last_action is None:
+ last_action = np.zeros(self.action_dim)
+
+ # 归一化状态(防止数值过大)
+ state_normalized = np.tanh(state / 10.0) # 简单归一化
+
+ # 确保状态维度匹配
+ if len(state_normalized) > self.state_dim:
+ state_normalized = state_normalized[:self.state_dim]
+ elif len(state_normalized) < self.state_dim:
+ state_normalized = np.pad(state_normalized, (0, self.state_dim - len(state_normalized)))
+
+ # 确保动作维度匹配
+ if len(last_action) > self.action_dim:
+ last_action = last_action[:self.action_dim]
+ elif len(last_action) < self.action_dim:
+ last_action = np.pad(last_action, (0, self.action_dim - len(last_action)))
+
+ # 构建输入向量:状态 + 相位编码 + 上次动作
+ input_vec = np.concatenate([
+ state_normalized,
+ [np.sin(gait_phase), np.cos(gait_phase)], # 相位编码(2维)
+ last_action
+ ])
+
+ # 目标维度:state_dim + 2 + action_dim
+ target_dim = self.state_dim + 2 + self.action_dim
+ if len(input_vec) != target_dim:
+ # 如果维度不匹配,调整
+ if len(input_vec) < target_dim:
+ input_vec = np.pad(input_vec, (0, target_dim - len(input_vec)))
+ else:
+ input_vec = input_vec[:target_dim]
+
+ # 前向传播
+ # 第一层
+ h1 = self._relu(input_vec @ self.policy_network['w1'] + self.policy_network['b1'])
+
+ # 第二层
+ h2 = self._relu(h1 @ self.policy_network['w2'] + self.policy_network['b2'])
+
+ # 简化的LSTM更新
+ lstm_input = np.concatenate([h2, self.policy_network['lstm_h']])
+ forget_gate = self._sigmoid(lstm_input @ self.policy_network['w_lstm'][:, :self.policy_network['lstm_h'].shape[0]])
+ input_gate = self._sigmoid(lstm_input @ self.policy_network['w_lstm'][:, self.policy_network['lstm_h'].shape[0]:])
+
+ # 更新LSTM状态
+ new_c = forget_gate * self.policy_network['lstm_c'] + input_gate * np.tanh(lstm_input @ self.policy_network['w_lstm_out'])
+ new_h = self._tanh(new_c)
+
+ self.policy_network['lstm_h'] = new_h
+ self.policy_network['lstm_c'] = new_c
+
+ # 输出层
+ output = self._tanh(new_h @ self.policy_network['w3'] + self.policy_network['b3'])
+
+ # 根据用户命令调整动作
+ if command is not None:
+ output = self._apply_command(output, command)
+
+ return np.clip(output, -1.0, 1.0)
+
+ def _apply_command(self, action, command):
+ """根据用户命令调整动作"""
+ if not self.actuator_indices:
+ return action
+
+ # 解析命令
+ forward = command.get('forward', False)
+ backward = command.get('backward', False)
+ turn_left = command.get('turn_left', False)
+ turn_right = command.get('turn_right', False)
+
+ # 调整髋关节前后摆动(前进/后退)
+ if forward or backward:
+ direction = 1.0 if forward else -1.0
+ hip_x_right_idx = self.actuator_indices.get("hip_x_right")
+ hip_x_left_idx = self.actuator_indices.get("hip_x_left")
+
+ if hip_x_right_idx is not None:
+ # 增强前进/后退动作
+ action[hip_x_right_idx] = np.clip(action[hip_x_right_idx] + 0.3 * direction, -1.0, 1.0)
+ if hip_x_left_idx is not None:
+ # 左腿相反方向
+ action[hip_x_left_idx] = np.clip(action[hip_x_left_idx] - 0.3 * direction, -1.0, 1.0)
+
+ # 调整转向
+ if turn_left or turn_right:
+ turn_dir = -1.0 if turn_left else 1.0
+ hip_z_right_idx = self.actuator_indices.get("hip_z_right")
+ hip_z_left_idx = self.actuator_indices.get("hip_z_left")
+ abdomen_z_idx = self.actuator_indices.get("abdomen_z")
+
+ if hip_z_right_idx is not None:
+ action[hip_z_right_idx] = np.clip(action[hip_z_right_idx] + 0.2 * turn_dir, -1.0, 1.0)
+ if hip_z_left_idx is not None:
+ action[hip_z_left_idx] = np.clip(action[hip_z_left_idx] - 0.2 * turn_dir, -1.0, 1.0)
+ if abdomen_z_idx is not None:
+ action[abdomen_z_idx] = np.clip(action[abdomen_z_idx] + 0.3 * turn_dir, -1.0, 1.0)
+
+ return action
+
+ def predict_value(self, state):
+ """预测状态价值"""
+ state_normalized = np.tanh(state / 10.0)
+
+ # 确保维度匹配
+ if len(state_normalized) > self.state_dim:
+ state_normalized = state_normalized[:self.state_dim]
+ elif len(state_normalized) < self.state_dim:
+ state_normalized = np.pad(state_normalized, (0, self.state_dim - len(state_normalized)))
+
+ h1 = self._relu(state_normalized @ self.value_network['w1'] + self.value_network['b1'])
+ h2 = self._relu(h1 @ self.value_network['w2'] + self.value_network['b2'])
+ value = h2 @ self.value_network['w3'] + self.value_network['b3']
+
+ return value[0]
+
+ def store_experience(self, state, action, reward, next_state, done):
+ """存储经验到回放缓冲区"""
+ self.replay_buffer.append({
+ 'state': state.copy(),
+ 'action': action.copy(),
+ 'reward': reward,
+ 'next_state': next_state.copy() if next_state is not None else None,
+ 'done': done
+ })
+
+ def train_step(self):
+ """执行一步训练(使用经验回放和策略梯度)"""
+ if len(self.replay_buffer) < self.batch_size:
+ return
+
+ # 采样批次
+ batch_indices = np.random.choice(len(self.replay_buffer), self.batch_size, replace=False)
+ batch = [self.replay_buffer[i] for i in batch_indices]
+
+ # 简化的策略梯度更新(使用REINFORCE算法)
+ for experience in batch:
+ state = experience['state']
+ action = experience['action']
+ reward = experience['reward']
+
+ # 计算策略梯度(简化版)
+ predicted_action = self.predict_action(state, self.gait_phase, None)
+ action_error = action - predicted_action
+
+ # 更新策略网络(使用奖励加权)
+ learning_rate = self.learning_rate * reward # 奖励越大,学习越快
+
+ # 反向传播(简化版,只更新输出层)
+ if abs(learning_rate) > 1e-6:
+ grad = action_error * learning_rate
+ self.policy_network['w3'] += np.outer(self.policy_network['lstm_h'], grad) * 0.01
+ self.policy_network['b3'] += grad * 0.01
+
+ def update_gait_phase(self, dt):
+ """更新步态相位"""
+ self.gait_phase += 2 * np.pi * self.gait_frequency * dt
+ if self.gait_phase > 2 * np.pi:
+ self.gait_phase -= 2 * np.pi
+
+ def reset_lstm_state(self):
+ """重置LSTM状态"""
+ self.policy_network['lstm_h'] = np.zeros_like(self.policy_network['lstm_h'])
+ self.policy_network['lstm_c'] = np.zeros_like(self.policy_network['lstm_c'])
+
+ def save_model(self, filepath):
+ """保存模型"""
+ model_data = {
+ 'policy_network': self.policy_network,
+ 'value_network': self.value_network,
+ 'action_dim': self.action_dim,
+ 'state_dim': self.state_dim
+ }
+ with open(filepath, 'wb') as f:
+ pickle.dump(model_data, f)
+ print(f"[深度学习控制器] 模型已保存到: {filepath}")
+
+ def load_model(self, filepath):
+ """加载模型"""
+ if os.path.exists(filepath):
+ with open(filepath, 'rb') as f:
+ model_data = pickle.load(f)
+ self.policy_network = model_data['policy_network']
+ self.value_network = model_data['value_network']
+ print(f"[深度学习控制器] 模型已从 {filepath} 加载")
+ else:
+ print(f"[深度学习控制器] 模型文件不存在: {filepath}")
class KeyboardController:
@@ -31,7 +352,7 @@ def __init__(self, action_dim, actuator_indices=None):
# 步行动作时间计数器(改为基于键盘输入的脉冲式控制)
self.step_time = 0.0
- self.step_frequency = 1.2 # 步频 (Hz)
+ self.step_frequency = 0.9 # 步频 (Hz) - 降低步频,让动作更自然
self.step_duration = 0.5 # 每次按键的移动持续时间(秒)
self.last_action_time = 0.0 # 上次执行动作的时间
@@ -59,9 +380,19 @@ def __init__(self, action_dim, actuator_indices=None):
self.turn_angle_per_step = np.pi / 4.0 # 每次转向目标角度:45度(π/4弧度)
self.turn_speed = 2.0 # 转向速度(弧度/秒)
+ # 键盘输入防抖:避免重复触发
+ self.key_debounce_time = 0.15 # 防抖时间(秒)
+ self.last_key_time = {} # 记录每个按键的最后触发时间
+
# 简单的神经网络控制器(用于动作平滑)
self.use_neural_smoothing = True
self._init_neural_smoother()
+
+ # 深度学习控制器(用于学习最优步态)
+ self.use_deep_learning = True
+ self.deep_controller = None # 将在get_action中初始化(需要state_dim)
+ self.last_state = None
+ self.last_reward = 0.0
self._print_help()
@@ -220,75 +551,197 @@ def _set_action(self, action, name, value):
action[idx] = value
def _create_walking_action(self, forward=True, turn_direction=0):
- """创建步行动作:基于周期的左右腿交替摆动,更自然的步态"""
+ """创建步行动作:更自然的人类步态,包含支撑相和摆动相的协调"""
action = np.zeros(self.action_dim)
if not self.actuator_indices:
return action
- # 计算步行动作相位
+ # 计算步行动作相位(保持连续性)
phase = 2 * np.pi * self.step_time * self.step_frequency
direction = 1 if forward else -1
- # 使用更自然的步态模式:区分支撑相和摆动相
+ # 计算步态强度(基于step_time,用于平滑停止)
+ # 当step_time衰减时,动作幅度也平滑减小
+ gait_strength = min(1.0, self.step_time * self.step_frequency * 2.0) # 在第一个周期内从0到1
+ # 如果step_time很小,进一步减小强度,实现平滑停止
+ if self.step_time < 0.1:
+ gait_strength *= self.step_time / 0.1 # 在最后0.1秒内平滑衰减到0
+
+ # 人类步态特点:
+ # 1. 支撑相约占60%,摆动相约占40%
+ # 2. 摆动相时:抬腿、膝关节弯曲、踝关节背屈
+ # 3. 支撑相时:腿伸直、踝关节跖屈、推进身体
+
# 右腿相位
right_phase = phase
- # 左腿相位(相差180度)
+ # 左腿相位(相差180度,形成交替步态)
left_phase = phase + np.pi
- # 计算摆动相和支撑相(使用平滑的过渡)
- # 摆动相:0到π,支撑相:π到2π
- right_swing_phase = (right_phase % (2 * np.pi)) / np.pi # 归一化到0-2
- left_swing_phase = (left_phase % (2 * np.pi)) / np.pi
-
- # 右腿:更自然的步态
- # 髋关节前后摆动(主要推进力)
- # 右腿向前摆动时产生推进力
- right_hip_swing = 0.6 * direction * np.sin(right_phase)
+ # 定义摆动相和支撑相的平滑过渡函数
+ # 摆动相:0到π(约40%的时间),支撑相:π到2π(约60%的时间)
+ def swing_phase_weight(phi):
+ """计算摆动相权重:在0到π之间为1,在π到2π之间平滑过渡到0"""
+ phi_norm = phi % (2 * np.pi)
+ if phi_norm < np.pi:
+ # 摆动相:使用平滑的上升和下降
+ return 0.5 * (1 - np.cos(phi_norm)) # 0到1的平滑上升
+ else:
+ # 支撑相:快速下降到0
+ support_phase = phi_norm - np.pi
+ return max(0, 0.5 * (1 + np.cos(support_phase))) # 1到0的平滑下降
+
+ def support_phase_weight(phi):
+ """计算支撑相权重:与摆动相相反"""
+ return 1.0 - swing_phase_weight(phi)
+
+ # 右腿动作
+ right_swing = swing_phase_weight(right_phase)
+ right_support = support_phase_weight(right_phase)
+
+ # 髋关节前后摆动(主要推进力)- 更自然的协调
+ # 使用更平滑的正弦波,在摆动相向前,支撑相向后推
+ # 添加轻微的相位偏移,让动作更自然
+ right_hip_swing = 0.45 * direction * np.sin(right_phase + 0.1) * gait_strength
self._set_action(action, "hip_x_right", right_hip_swing)
- # 髋关节上下(抬腿)
- right_hip_lift = 0.2 * max(0, np.sin(right_phase)) # 只在摆动相抬腿
+ # 髋关节上下(抬腿)- 更自然的抬腿动作
+ # 在摆动相早期开始抬腿,中期达到最高,后期下降
+ swing_phase_norm = (right_phase % (2 * np.pi)) / (2 * np.pi)
+ if swing_phase_norm < 0.5: # 摆动相(前50%)
+ # 抬腿:使用平滑的曲线,在摆动相中期(25%)达到最高
+ lift_curve = np.sin(swing_phase_norm * 2 * np.pi) # 0到1再到0
+ # 后退时减少抬腿幅度,保持脚部更接近地面
+ lift_amplitude = 0.2 if forward else 0.1 # 后退时抬腿幅度减半
+ right_hip_lift = lift_amplitude * lift_curve * gait_strength
+ else: # 支撑相(后50%)
+ right_hip_lift = 0.0
self._set_action(action, "hip_y_right", -right_hip_lift)
- # 膝关节(在摆动相弯曲,支撑相伸直)
- right_knee_angle = 0.5 * (1 - np.cos(right_phase)) # 0到1的平滑变化
- self._set_action(action, "knee_right", 0.6 * right_knee_angle)
-
- # 踝关节(配合抬腿)
- self._set_action(action, "ankle_y_right", -0.15 * max(0, np.sin(right_phase)))
- self._set_action(action, "ankle_x_right", 0.15 * np.sin(right_phase))
-
- # 左腿(相位相反,摆动方向也相反以产生推进力)
- # 关键:左腿的摆动与右腿相反(当右腿向前时,左腿向后)
- # 右腿:0.6 * direction * sin(phase)
- # 左腿:-0.6 * direction * sin(phase) (直接使用负号,确保与右腿相反)
- # 这样差异 = 0.6*direction*sin(phase) - (-0.6*direction*sin(phase)) = 1.2*direction*sin(phase)
- # 能产生有效的推进力!
- left_hip_swing = -0.6 * direction * np.sin(right_phase) # 直接使用右腿相位的负值
+ # 膝关节 - 更自然的协调,与髋关节配合
+ # 摆动相:早期快速弯曲(配合抬腿),中期保持弯曲,后期开始伸直准备落地
+ # 支撑相:完全伸直
+ if swing_phase_norm < 0.5: # 摆动相
+ # 膝关节弯曲曲线:早期快速弯曲,中期保持,后期开始伸直
+ if swing_phase_norm < 0.3:
+ # 早期:快速弯曲到最大
+ knee_curve = swing_phase_norm / 0.3 # 0到1
+ elif swing_phase_norm < 0.4:
+ # 中期:保持弯曲
+ knee_curve = 1.0
+ else:
+ # 后期:开始伸直
+ knee_curve = 1.0 - (swing_phase_norm - 0.4) / 0.1 # 1到0
+ # 后退时减少膝关节弯曲幅度,保持腿部更直,脚部更接近地面
+ knee_amplitude = 0.6 if forward else 0.3
+ right_knee_angle = knee_amplitude * knee_curve * gait_strength
+ else: # 支撑相
+ right_knee_angle = 0.0
+ self._set_action(action, "knee_right", right_knee_angle)
+
+ # 踝关节 - 更自然的协调,与膝关节配合
+ # 摆动相:早期背屈(脚尖向上,配合抬腿),中期保持,后期开始跖屈准备落地
+ # 支撑相:跖屈(脚尖向下,推进)
+ if swing_phase_norm < 0.5: # 摆动相
+ # 背屈:在摆动相早期和中期
+ if swing_phase_norm < 0.35:
+ # 后退时减少背屈幅度,保持脚部更平
+ dorsiflex_amplitude = -0.15 if forward else -0.08
+ ankle_dorsiflex = dorsiflex_amplitude * (1 - swing_phase_norm / 0.35) * gait_strength
+ else:
+ ankle_dorsiflex = 0.0
+ ankle_plantarflex = 0.0
+ else: # 支撑相
+ # 跖屈:在支撑相早期和中期推进
+ support_phase_norm = (swing_phase_norm - 0.5) * 2 # 0到1
+ if support_phase_norm < 0.6:
+ ankle_plantarflex = 0.12 * np.sin(support_phase_norm * np.pi) * gait_strength
+ else:
+ ankle_plantarflex = 0.0
+ ankle_dorsiflex = 0.0
+ self._set_action(action, "ankle_y_right", ankle_dorsiflex + ankle_plantarflex)
+ # 踝关节内外翻(配合步态,轻微)
+ self._set_action(action, "ankle_x_right", 0.08 * np.sin(right_phase) * gait_strength)
+
+ # 左腿动作(相位相反,与右腿完全对称)
+ left_phase_norm = (left_phase % (2 * np.pi)) / (2 * np.pi)
+
+ # 左腿髋关节前后摆动(与右腿相反)
+ left_hip_swing = -0.45 * direction * np.sin(left_phase + 0.1) * gait_strength
self._set_action(action, "hip_x_left", left_hip_swing)
- left_hip_lift = 0.2 * max(0, np.sin(left_phase))
+ # 左腿髋关节上下(抬腿)
+ if left_phase_norm < 0.5: # 摆动相
+ lift_curve = np.sin(left_phase_norm * 2 * np.pi)
+ # 后退时减少抬腿幅度,保持脚部更接近地面
+ lift_amplitude = 0.2 if forward else 0.1 # 后退时抬腿幅度减半
+ left_hip_lift = lift_amplitude * lift_curve * gait_strength
+ else: # 支撑相
+ left_hip_lift = 0.0
self._set_action(action, "hip_y_left", -left_hip_lift)
- left_knee_angle = 0.5 * (1 - np.cos(left_phase))
- self._set_action(action, "knee_left", 0.6 * left_knee_angle)
+ # 左腿膝关节
+ if left_phase_norm < 0.5: # 摆动相
+ if left_phase_norm < 0.3:
+ knee_curve = left_phase_norm / 0.3
+ elif left_phase_norm < 0.4:
+ knee_curve = 1.0
+ else:
+ knee_curve = 1.0 - (left_phase_norm - 0.4) / 0.1
+ # 后退时减少膝关节弯曲幅度,保持腿部更直,脚部更接近地面
+ knee_amplitude = 0.6 if forward else 0.3
+ left_knee_angle = knee_amplitude * knee_curve * gait_strength
+ else: # 支撑相
+ left_knee_angle = 0.0
+ self._set_action(action, "knee_left", left_knee_angle)
+
+ # 左腿踝关节
+ if left_phase_norm < 0.5: # 摆动相
+ if left_phase_norm < 0.35:
+ # 后退时减少背屈幅度,保持脚部更平
+ dorsiflex_amplitude = -0.15 if forward else -0.08
+ ankle_dorsiflex = dorsiflex_amplitude * (1 - left_phase_norm / 0.35) * gait_strength
+ else:
+ ankle_dorsiflex = 0.0
+ ankle_plantarflex = 0.0
+ else: # 支撑相
+ support_phase_norm = (left_phase_norm - 0.5) * 2
+ if support_phase_norm < 0.6:
+ ankle_plantarflex = 0.12 * np.sin(support_phase_norm * np.pi) * gait_strength
+ else:
+ ankle_plantarflex = 0.0
+ ankle_dorsiflex = 0.0
+ self._set_action(action, "ankle_y_left", ankle_dorsiflex + ankle_plantarflex)
+ self._set_action(action, "ankle_x_left", -0.08 * np.sin(left_phase) * gait_strength)
+
+ # 侧向平衡控制
+ if turn_direction == 0:
+ # 直行时,保持髋关节外展对称
+ hip_z_balance = 0.0
+ self._set_action(action, "hip_z_right", hip_z_balance)
+ self._set_action(action, "hip_z_left", -hip_z_balance)
+ else:
+ # 转向时,外侧腿稍微外展,内侧腿稍微内收
+ turn_strength = 0.4 * turn_direction
+ self._set_action(action, "hip_z_right", turn_strength)
+ self._set_action(action, "hip_z_left", -turn_strength)
- self._set_action(action, "ankle_y_left", -0.15 * max(0, np.sin(left_phase)))
- self._set_action(action, "ankle_x_left", -0.15 * np.sin(left_phase))
+ # 躯干控制 - 更自然的轻微摆动
+ # 轻微前倾以辅助前进(减小前倾幅度,更自然)
+ abdomen_pitch = 0.08 * direction * gait_strength
+ # 添加轻微的上下摆动(配合步态,与腿部动作协调)
+ # 在支撑相时稍微下沉,在摆动相时稍微上升
+ abdomen_pitch += 0.02 * np.sin(phase + np.pi/4) * gait_strength
+ self._set_action(action, "abdomen_y", abdomen_pitch)
- # 躯干控制(轻微前倾以辅助前进)
- self._set_action(action, "abdomen_y", 0.2 * direction)
- self._set_action(action, "abdomen_x", 0.1 * turn_direction)
+ # 转向时允许侧倾(减小侧倾幅度)
+ self._set_action(action, "abdomen_x", 0.05 * turn_direction * gait_strength)
- # 转向控制(更平滑,增大转向幅度)
+ # 转向控制(减小转向幅度,更自然)
if turn_direction != 0:
- turn_strength = 0.5 * turn_direction # 从0.3增大到0.5
- # 转向时,外侧腿稍微外展,内侧腿稍微内收
- self._set_action(action, "hip_z_right", turn_strength)
- self._set_action(action, "hip_z_left", -turn_strength)
- # 添加躯干旋转辅助转向
- self._set_action(action, "abdomen_z", 0.4 * turn_direction) # 添加躯干旋转
+ self._set_action(action, "abdomen_z", 0.25 * turn_direction * gait_strength)
+ else:
+ self._set_action(action, "abdomen_z", 0.0)
return action
@@ -358,11 +811,25 @@ def _create_turning_only_action(self, turn_direction):
return action
def _process_key(self, key):
- """处理按键输入"""
+ """处理按键输入(带防抖机制)"""
+ import time
+ current_time = time.time()
+
if isinstance(key, str) and key.startswith('\x1b['):
key_char = None # 方向键用特殊序列表示
+ key_id = key # 使用特殊序列作为ID
else:
key_char = key if isinstance(key, str) and len(key) == 1 else None
+ key_id = key_char if key_char else key
+
+ # 防抖检查:如果距离上次按键时间太短,忽略此次按键
+ if key_id in self.last_key_time:
+ time_since_last = current_time - self.last_key_time[key_id]
+ if time_since_last < self.key_debounce_time:
+ return # 忽略重复按键
+
+ # 更新按键时间
+ self.last_key_time[key_id] = current_time
# 处理移动指令(切换模式:每次按键切换状态)
move_commands = {
@@ -376,9 +843,9 @@ def _process_key(self, key):
if (key_char == key1) or (key == key2):
current_state = getattr(self, attr)
if current_state:
- # 停止移动时,立即重置相关状态
+ # 停止移动时,不立即重置step_time,让当前步态周期平滑完成
setattr(self, attr, False)
- self.step_time = 0.0 # 重置步行动作时间
+ # 注意:不重置step_time,让它自然衰减,保持步态连续性
# 快速清零平滑动作
if not (self.move_forward or self.move_backward or self.turn_left or self.turn_right):
self.smoothed_action = np.zeros(self.action_dim)
@@ -387,8 +854,8 @@ def _process_key(self, key):
setattr(self, attr, True)
if hasattr(self, opposite_attr):
setattr(self, opposite_attr, False)
- # 开始移动时,重置步行动作时间
- self.step_time = 0.0
+ # 开始移动时,不重置step_time,保持步态相位连续性
+ # 如果step_time为0(首次启动),保持为0;否则继续累积
print(f"[键盘] {start_msg}")
return
@@ -409,15 +876,21 @@ def _process_key(self, key):
print("[键盘] ❌ 准备退出程序...")
def update_step_time(self, dt):
- """更新步行动作时间(只在有键盘输入时更新)"""
+ """更新步行动作时间(保持步态连续性)"""
if not self.paused and (self.move_forward or self.move_backward or self.turn_left or self.turn_right):
+ # 有键盘输入时,持续累积时间,保持步态连续性
self.step_time += dt
else:
- # 没有键盘输入时,立即重置时间,停止动作
- self.step_time = 0.0
+ # 没有键盘输入时,平滑衰减step_time,让步态平滑停止
+ # 使用指数衰减,而不是立即重置,保持动作连续性
+ decay_rate = 0.95 # 每步衰减5%
+ self.step_time *= decay_rate
+ # 当step_time很小时,重置为0,避免无限小的值
+ if self.step_time < 0.01:
+ self.step_time = 0.0
- def get_action(self, dt=0.03, current_velocity=None):
- """获取当前控制动作(基于键盘输入的离散控制)"""
+ def get_action(self, dt=0.03, current_velocity=None, state=None, reward=None):
+ """获取当前控制动作(基于键盘输入的离散控制 + 深度学习增强)"""
if self.paused:
self.smoothed_action = np.zeros(self.action_dim)
self.target_velocity = np.array([0.0, 0.0])
@@ -434,43 +907,99 @@ def get_action(self, dt=0.03, current_velocity=None):
# 检查是否有任何移动指令
has_movement = self.move_forward or self.move_backward or self.turn_left or self.turn_right
- if not has_movement:
- # 没有键盘输入时,立即停止并清零动作
- self.step_time = 0.0
- # 快速衰减到零
- self.smoothed_action = self.smoothed_action * 0.5
- if np.max(np.abs(self.smoothed_action)) < 0.01:
- self.smoothed_action = np.zeros(self.action_dim)
- self.current_action = self.smoothed_action.copy()
- return self.current_action.copy()
-
- # 有键盘输入时,更新步行动作时间
- self.update_step_time(dt)
+ # 初始化深度学习控制器(如果启用且未初始化)
+ if self.use_deep_learning and self.deep_controller is None and state is not None:
+ state_dim = len(state)
+ self.deep_controller = DeepLearningController(
+ self.action_dim,
+ state_dim,
+ self.actuator_indices
+ )
+ print("[键盘控制器] 深度学习控制器已初始化")
- # 根据移动状态创建动作
- if self.move_forward:
- turn_dir = 0
- if self.turn_left:
- turn_dir = -1
- elif self.turn_right:
- turn_dir = 1
- raw_action = self._create_walking_action(forward=True, turn_direction=turn_dir)
- elif self.move_backward:
- turn_dir = 0
- if self.turn_left:
- turn_dir = 1
- elif self.turn_right:
- turn_dir = -1
- raw_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
- raw_action = self._create_turning_only_action(turn_dir, dt=dt)
+ # 使用深度学习控制器生成动作(如果启用)
+ if self.use_deep_learning and self.deep_controller is not None and state is not None:
+ # 更新步态相位
+ self.deep_controller.update_gait_phase(dt)
+ gait_phase = self.deep_controller.gait_phase
+
+ # 构建用户命令
+ command = {
+ 'forward': self.move_forward,
+ 'backward': self.move_backward,
+ 'turn_left': self.turn_left,
+ 'turn_right': self.turn_right
+ }
+
+ # 使用深度学习控制器预测动作
+ last_action = self.current_action if hasattr(self, 'current_action') else None
+ dl_action = self.deep_controller.predict_action(state, gait_phase, last_action, command)
+
+ # 如果用户有移动指令,混合传统动作和深度学习动作
+ if has_movement:
+ # 更新步行动作时间
+ self.update_step_time(dt)
+
+ # 生成传统动作
+ if self.move_forward:
+ turn_dir = 0
+ if self.turn_left:
+ turn_dir = -1
+ elif self.turn_right:
+ turn_dir = 1
+ traditional_action = self._create_walking_action(forward=True, turn_direction=turn_dir)
+ elif self.move_backward:
+ turn_dir = 0
+ if self.turn_left:
+ turn_dir = 1
+ elif self.turn_right:
+ turn_dir = -1
+ traditional_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
+ traditional_action = self._create_turning_only_action(turn_dir, dt=dt)
+ else:
+ traditional_action = np.zeros(self.action_dim)
+
+ # 混合传统动作和深度学习动作(70%传统,30%深度学习)
+ raw_action = 0.7 * traditional_action + 0.3 * dl_action
+ else:
+ # 没有移动指令时,返回零动作,不生成任何移动
+ raw_action = np.zeros(self.action_dim)
+ self.step_time = 0.0
else:
- # 不应该到这里,但以防万一
- raw_action = np.zeros(self.action_dim)
+ # 传统方法:没有深度学习控制器时使用原有逻辑
+ if not has_movement:
+ self.step_time = 0.0
+ self.smoothed_action = self.smoothed_action * 0.5
+ if np.max(np.abs(self.smoothed_action)) < 0.01:
+ self.smoothed_action = np.zeros(self.action_dim)
+ self.current_action = self.smoothed_action.copy()
+ return self.current_action.copy()
+
+ self.update_step_time(dt)
+
+ if self.move_forward:
+ turn_dir = 0
+ if self.turn_left:
+ turn_dir = -1
+ elif self.turn_right:
+ turn_dir = 1
+ raw_action = self._create_walking_action(forward=True, turn_direction=turn_dir)
+ elif self.move_backward:
+ turn_dir = 0
+ if self.turn_left:
+ turn_dir = 1
+ elif self.turn_right:
+ turn_dir = -1
+ raw_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
+ raw_action = self._create_turning_only_action(turn_dir, dt=dt)
+ else:
+ raw_action = np.zeros(self.action_dim)
- # 应用动作平滑(但使用更小的平滑系数,使停止更快)
+ # 应用动作平滑
if self.use_neural_smoothing and len(self.neural_history) >= 2:
smoothed = self._neural_smooth_action(raw_action)
self.neural_history.append(raw_action.copy())
@@ -537,6 +1066,14 @@ def __init__(self, corridor_length=100, corridor_width=10, robot_xml_path=None,
self._xy_damping = 0.99 # XY速度阻尼系数(减小阻尼,保持速度)
self._forward_velocity_gain = 2.5 # 前进速度增益(增大增益,产生明显移动)
self._turn_velocity_gain = 0.5 # 转向速度增益
+
+ # 姿态稳定控制参数
+ self._initial_head_height = None # 初始头部高度
+ self._head_stability_gain = 5.0 # 头部高度稳定增益
+ self._torso_pitch_target = 0.0 # 目标躯干俯仰角(前倾角度)
+ self._torso_roll_target = 0.0 # 目标躯干侧倾角
+ self._torso_stability_gain = 2.0 # 躯干姿态稳定增益
+
self._find_root_joint_indices()
def _parse_robot_xml(self):
@@ -672,10 +1209,37 @@ def reset(self):
mujoco.mj_resetData(self.model, self.data)
mujoco.mj_forward(self.model, self.data)
- # 无重力模式:记录根关节的初始Z高度和姿态
+ # 无重力模式:记录根关节的初始Z高度、Y位置和姿态(保持身体直立)
if not self.use_gravity and self._root_joint_qpos_start is not None:
self._initial_z_height = float(self.data.qpos[self._root_joint_qpos_start + 2])
- print(f"[无重力模式] 记录初始Z高度: {self._initial_z_height:.4f},允许上身自由移动")
+ self._initial_y_position = float(self.data.qpos[self._root_joint_qpos_start + 1]) # 记录初始Y位置
+ # 记录初始姿态(四元数),用于保持身体直立
+ if (self._root_joint_qpos_start + 6) < len(self.data.qpos):
+ self._initial_quat = self.data.qpos[self._root_joint_qpos_start + 3:self._root_joint_qpos_start + 7].copy()
+ else:
+ self._initial_quat = np.array([1.0, 0.0, 0.0, 0.0]) # 默认单位四元数(无旋转)
+ # 记录初始头部高度(用于姿态稳定控制)
+ head_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "head")
+ if head_id >= 0:
+ self._initial_head_height = float(self.data.xpos[head_id][2])
+ else:
+ self._initial_head_height = None
+
+ # 记录初始根关节位置(用于计算脚部相对位置)
+ if hasattr(self, '_root_body_id') and self._root_body_id is not None:
+ self._initial_root_pos = self.data.xpos[self._root_body_id].copy()
+ else:
+ self._initial_root_pos = None
+
+ # 记录脚部初始位置(用于保持脚部着地)
+ self._initial_foot_positions = {}
+ foot_names = ["foot_right", "foot_left", "right_foot", "left_foot"]
+ for foot_name in foot_names:
+ foot_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, foot_name)
+ if foot_id >= 0:
+ self._initial_foot_positions[foot_name] = self.data.xpos[foot_id].copy()
+
+ print(f"[无重力模式] 记录初始Z高度: {self._initial_z_height:.4f},初始Y位置: {self._initial_y_position:.4f},保持身体直立")
return self._get_observation()
@@ -688,7 +1252,7 @@ def _get_observation(self):
return np.concatenate([qpos, qvel, torso_pos])
def _get_reward(self):
- """计算奖励:前进速度(沿走廊X轴)+ 空隙掉落惩罚"""
+ """计算奖励:前进速度(沿走廊X轴)+ 稳定性奖励 + 空隙掉落惩罚"""
torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
geom_vel = np.zeros(6)
@@ -713,7 +1277,7 @@ def _get_reward(self):
return reward
def _apply_zero_gravity_constraints(self, action, before_step=True):
- """应用无重力模式的约束:只固定Z高度,允许上身自由移动,并根据动作主动施加速度"""
+ """应用无重力模式的约束:固定Z高度和Y位置(保持在走廊中心),允许X方向移动,并根据动作主动施加速度"""
if self.use_gravity or self._initial_z_height is None:
return
@@ -724,22 +1288,119 @@ def _apply_zero_gravity_constraints(self, action, before_step=True):
return
if before_step:
- # mj_step前:只固定Z位置,不干扰其他物理量
+ # mj_step前:固定Z位置、Y位置和姿态(保持身体直立),不干扰其他物理量
if (pos_start + 2) < len(self.data.qpos):
self.data.qpos[pos_start + 2] = self._initial_z_height
- # 清零Z方向速度,防止飘起
+ if (pos_start + 1) < len(self.data.qpos) and hasattr(self, '_initial_y_position'):
+ self.data.qpos[pos_start + 1] = self._initial_y_position
+
+ # 稳定姿态:保持身体直立,只允许绕Z轴旋转(yaw)
+ if (pos_start + 6) < len(self.data.qpos) and hasattr(self, '_initial_quat'):
+ # 获取当前四元数
+ current_quat = self.data.qpos[pos_start + 3:pos_start + 7]
+
+ # 从初始四元数提取yaw角(绕Z轴旋转)
+ qw0, qx0, qy0, qz0 = self._initial_quat
+ initial_yaw = np.arctan2(2.0 * (qw0 * qz0 + qx0 * qy0), 1.0 - 2.0 * (qy0 * qy0 + qz0 * qz0))
+
+ # 从当前四元数提取yaw角
+ qw, qx, qy, qz = current_quat
+ current_yaw = np.arctan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+ # 保持roll和pitch为0,但保留yaw角(允许转向)
+ # 从yaw角重建四元数(只绕Z轴旋转)
+ target_yaw = current_yaw # 保留当前yaw角,允许转向
+ yaw_quat = np.array([
+ np.cos(target_yaw / 2), # w
+ 0.0, # x (roll = 0)
+ 0.0, # y (pitch = 0)
+ np.sin(target_yaw / 2) # z (yaw)
+ ])
+
+ # 平滑应用姿态修正(防止突然变化)
+ correction_strength = 0.3 # 姿态修正强度
+ self.data.qpos[pos_start + 3:pos_start + 7] = (
+ current_quat * (1 - correction_strength) + yaw_quat * correction_strength
+ )
+ # 归一化四元数
+ quat_norm = np.linalg.norm(self.data.qpos[pos_start + 3:pos_start + 7])
+ if quat_norm > 1e-6:
+ self.data.qpos[pos_start + 3:pos_start + 7] /= quat_norm
+
+ # 清零Z方向和Y方向速度,以及roll和pitch角速度,防止飘起、左右移动和倾斜
if (vel_start + 2) < len(self.data.qvel):
- self.data.qvel[vel_start + 2] = 0.0
+ self.data.qvel[vel_start + 2] = 0.0 # Z方向速度
+ if (vel_start + 1) < len(self.data.qvel):
+ self.data.qvel[vel_start + 1] = 0.0 # Y方向速度
+ if (vel_start + 3) < len(self.data.qvel):
+ self.data.qvel[vel_start + 3] = 0.0 # 绕X轴角速度(roll)
+ if (vel_start + 4) < len(self.data.qvel):
+ self.data.qvel[vel_start + 4] = 0.0 # 绕Y轴角速度(pitch)
+
+ # 固定脚部位置(保持脚部着地)- 在mj_step前应用
+ if (hasattr(self, '_initial_foot_positions') and hasattr(self, '_root_body_id') and
+ self._root_body_id is not None and hasattr(self, '_initial_root_pos') and
+ self._initial_root_pos is not None):
+ root_pos = self.data.xpos[self._root_body_id]
+ for foot_name, initial_pos in self._initial_foot_positions.items():
+ foot_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, foot_name)
+ if foot_id >= 0:
+ # 计算初始时脚部相对于根关节的偏移
+ foot_offset = initial_pos - self._initial_root_pos
+ # 计算期望的脚部位置(相对于当前根关节位置)
+ expected_foot_pos = root_pos + foot_offset
+ # 平滑修正脚部位置(特别是Z位置)
+ current_foot_pos = self.data.xpos[foot_id].copy()
+ # 只修正Z位置,保持X和Y相对位置
+ self.data.xpos[foot_id][2] = current_foot_pos[2] * 0.5 + expected_foot_pos[2] * 0.5
else:
- # mj_step后:固定Z位置,应用XY速度控制
+ # mj_step后:固定Z位置、Y位置和姿态(保持身体直立),应用X方向速度控制
if (pos_start + 2) < len(self.data.qpos):
self.data.qpos[pos_start + 2] = self._initial_z_height
+ if (pos_start + 1) < len(self.data.qpos) and hasattr(self, '_initial_y_position'):
+ self.data.qpos[pos_start + 1] = self._initial_y_position
+
+ # 稳定姿态:保持身体直立,只允许绕Z轴旋转(yaw)
+ if (pos_start + 6) < len(self.data.qpos) and hasattr(self, '_initial_quat'):
+ # 获取当前四元数
+ current_quat = self.data.qpos[pos_start + 3:pos_start + 7]
+
+ # 从当前四元数提取yaw角
+ qw, qx, qy, qz = current_quat
+ current_yaw = np.arctan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+ # 保持roll和pitch为0,但保留yaw角(允许转向)
+ yaw_quat = np.array([
+ np.cos(current_yaw / 2), # w
+ 0.0, # x (roll = 0)
+ 0.0, # y (pitch = 0)
+ np.sin(current_yaw / 2) # z (yaw)
+ ])
+
+ # 平滑应用姿态修正
+ correction_strength = 0.3
+ self.data.qpos[pos_start + 3:pos_start + 7] = (
+ current_quat * (1 - correction_strength) + yaw_quat * correction_strength
+ )
+ # 归一化四元数
+ quat_norm = np.linalg.norm(self.data.qpos[pos_start + 3:pos_start + 7])
+ if quat_norm > 1e-6:
+ self.data.qpos[pos_start + 3:pos_start + 7] /= quat_norm
+
if (vel_start + 2) < len(self.data.qvel):
- self.data.qvel[vel_start + 2] = 0.0
+ self.data.qvel[vel_start + 2] = 0.0 # Z方向速度
+ if (vel_start + 1) < len(self.data.qvel):
+ self.data.qvel[vel_start + 1] = 0.0 # Y方向速度
+ # 清零roll和pitch角速度,防止倾斜
+ if (vel_start + 3) < len(self.data.qvel):
+ self.data.qvel[vel_start + 3] = 0.0 # 绕X轴角速度(roll)
+ if (vel_start + 4) < len(self.data.qvel):
+ self.data.qvel[vel_start + 4] = 0.0 # 绕Y轴角速度(pitch)
- # XY速度控制(只在mj_step后)
+ # X方向速度控制(只在mj_step后,Y方向已固定)
if (vel_start + 2) <= len(self.data.qvel):
- vx, vy = self.data.qvel[vel_start], self.data.qvel[vel_start + 1]
+ vx = self.data.qvel[vel_start]
+ vy = 0.0 # Y方向速度固定为0
# 根据动作计算期望速度
desired_vx = 0.0
@@ -781,41 +1442,129 @@ def _apply_zero_gravity_constraints(self, action, before_step=True):
direction_sign = 1.0 if hip_x_right > 0 else -1.0
local_forward_vel = hip_x_avg_amplitude * direction_sign * self._forward_velocity_gain * 0.8
- # 根据躯干朝向,将局部前进速度转换到世界坐标系
+ # 根据躯干朝向,将局部前进速度转换到世界坐标系(只计算X方向,Y方向已固定)
desired_vx = local_forward_vel * np.cos(yaw)
- desired_vy = local_forward_vel * np.sin(yaw)
+ # desired_vy = 0.0 # Y方向已固定,不需要计算
- # 应用速度平滑过渡(使用更平滑的混合策略,减少震荡)
- if abs(desired_vx) > 0.01 or abs(desired_vy) > 0.01:
+ # 应用X方向速度平滑过渡(Y方向已固定,不需要控制)
+ if abs(desired_vx) > 0.01:
# 有主动移动时,使用更平滑的过渡
- # 使用更小的平滑系数,减少震荡
alpha = 0.4 # 平滑系数
vx = vx * (1 - alpha) + desired_vx * alpha
- vy = vy * (1 - alpha) + desired_vy * alpha
# 应用轻微阻尼(几乎不衰减,保持速度)
vx *= self._xy_damping
- vy *= self._xy_damping
else:
# 没有主动移动时,快速停止
damping = 0.85 # 增大阻尼,使停止更快
vx *= damping
- vy *= damping
# 如果速度很小,直接清零以避免微小震荡
if abs(vx) < 0.05:
vx = 0.0
- if abs(vy) < 0.05:
- vy = 0.0
- # 限制最大速度
- speed = np.sqrt(vx * vx + vy * vy)
- if speed > self._max_xy_velocity:
- scale = self._max_xy_velocity / speed
- vx *= scale
- vy *= scale
+ # 限制最大速度(只限制X方向,Y方向已固定)
+ if abs(vx) > self._max_xy_velocity:
+ vx = np.sign(vx) * self._max_xy_velocity
self.data.qvel[vel_start] = vx
- self.data.qvel[vel_start + 1] = vy
+ self.data.qvel[vel_start + 1] = 0.0 # Y方向速度固定为0,保持在走廊中心
+
+ # 转向角速度控制:检测转向动作并应用绕Z轴的角速度
+ if (vel_start + 5) < len(self.data.qvel) and self._actuator_indices:
+ # 检测转向动作(通过hip_z或abdomen_z关节)
+ hip_z_right_idx = self._actuator_indices.get("hip_z_right")
+ hip_z_left_idx = self._actuator_indices.get("hip_z_left")
+ abdomen_z_idx = self._actuator_indices.get("abdomen_z")
+
+ turn_angular_vel = 0.0
+ if hip_z_right_idx is not None and hip_z_left_idx is not None:
+ # 计算转向强度(通过髋关节外展差异)
+ hip_z_right = action[hip_z_right_idx] if hip_z_right_idx < len(action) else 0.0
+ hip_z_left = action[hip_z_left_idx] if hip_z_left_idx < len(action) else 0.0
+ hip_z_diff = hip_z_right - hip_z_left
+ turn_angular_vel += hip_z_diff * 0.5 # 转向角速度增益
+
+ if abdomen_z_idx is not None and abdomen_z_idx < len(action):
+ # 躯干旋转也贡献转向角速度
+ abdomen_z = action[abdomen_z_idx]
+ turn_angular_vel += abdomen_z * 0.8 # 躯干旋转的转向增益更大
+
+ # 应用转向角速度(绕Z轴旋转,索引vel_start+5是绕Z轴的角速度)
+ current_angular_vel_z = self.data.qvel[vel_start + 5]
+ # 平滑过渡转向角速度
+ if abs(turn_angular_vel) > 0.01:
+ alpha = 0.5 # 转向角速度平滑系数
+ new_angular_vel_z = current_angular_vel_z * (1 - alpha) + turn_angular_vel * alpha
+ # 限制最大转向角速度
+ max_turn_angular_vel = 2.0 # 最大转向角速度(弧度/秒)
+ new_angular_vel_z = np.clip(new_angular_vel_z, -max_turn_angular_vel, max_turn_angular_vel)
+ self.data.qvel[vel_start + 5] = new_angular_vel_z
+ else:
+ # 没有转向指令时,逐渐减小转向角速度
+ self.data.qvel[vel_start + 5] *= 0.9
+
+ # 姿态稳定控制:防止头部高度持续下降
+ if not before_step and self._initial_head_height is not None:
+ head_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "head")
+ if head_id >= 0:
+ current_head_height = self.data.xpos[head_id][2]
+ head_height_error = self._initial_head_height - current_head_height
+
+ # 如果头部高度下降超过阈值,应用姿态稳定控制
+ if head_height_error > 0.05: # 下降超过5cm
+ # 通过调整躯干俯仰角来恢复姿态
+ # 计算需要的俯仰角修正(前倾以恢复高度)
+ pitch_correction = min(head_height_error * self._head_stability_gain, 0.3) # 限制最大修正
+
+ # 获取当前躯干俯仰角(从四元数计算)
+ if pos_start + 6 < len(self.data.qpos):
+ qw = self.data.qpos[pos_start + 3]
+ qx = self.data.qpos[pos_start + 4]
+ qy = self.data.qpos[pos_start + 5]
+ qz = self.data.qpos[pos_start + 6]
+
+ # 计算当前俯仰角(pitch)
+ sin_pitch = 2.0 * (qw * qy - qz * qx)
+ current_pitch = np.arcsin(np.clip(sin_pitch, -1.0, 1.0))
+
+ # 计算目标俯仰角(稍微前倾以恢复高度)
+ target_pitch = current_pitch + pitch_correction
+
+ # 通过调整abdomen_y执行器来修正姿态(在动作中应用)
+ # 注意:这里只是记录修正值,实际应用在动作生成时
+ # 由于动作已经生成,这里通过直接调整根关节姿态来快速响应
+ # 但为了不影响动作生成,我们只在严重偏差时应用
+ if head_height_error > 0.15: # 下降超过15cm时,直接修正姿态
+ # 计算新的四元数(绕X轴旋转)
+ pitch_quat = np.array([
+ np.cos(target_pitch / 2),
+ np.sin(target_pitch / 2),
+ 0.0,
+ 0.0
+ ])
+ # 简化处理:只在小范围内修正
+ correction_factor = 0.1 # 每次只修正10%
+ self.data.qpos[pos_start + 3] = self.data.qpos[pos_start + 3] * (1 - correction_factor) + pitch_quat[0] * correction_factor
+ self.data.qpos[pos_start + 4] = self.data.qpos[pos_start + 4] * (1 - correction_factor) + pitch_quat[1] * correction_factor
+
+ # 固定脚部位置(保持脚部着地)- 在mj_step后应用
+ if (not before_step and hasattr(self, '_initial_foot_positions') and
+ hasattr(self, '_root_body_id') and self._root_body_id is not None and
+ hasattr(self, '_initial_root_pos') and self._initial_root_pos is not None):
+ # 需要先更新物理状态以获取最新的xpos
+ mujoco.mj_forward(self.model, self.data)
+ root_pos = self.data.xpos[self._root_body_id]
+ for foot_name, initial_pos in self._initial_foot_positions.items():
+ foot_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, foot_name)
+ if foot_id >= 0:
+ # 计算初始时脚部相对于根关节的偏移
+ foot_offset = initial_pos - self._initial_root_pos
+ # 计算期望的脚部位置(相对于当前根关节位置)
+ expected_foot_pos = root_pos + foot_offset
+ # 平滑修正脚部位置(特别是Z位置)
+ current_foot_pos = self.data.xpos[foot_id].copy()
+ # 只修正Z位置,保持X和Y相对位置
+ self.data.xpos[foot_id][2] = current_foot_pos[2] * 0.5 + expected_foot_pos[2] * 0.5
def step(self, action):
"""执行动作并推进环境"""
@@ -900,6 +1649,10 @@ def main():
controller.smoothed_action = np.zeros(controller.action_dim)
controller.action_history.clear()
controller.neural_history.clear()
+ # 重置深度学习控制器状态
+ if controller.deep_controller is not None:
+ controller.deep_controller.reset_lstm_state()
+ controller.last_state = None
last_move_state = None
controller.clear_reset_flag()
@@ -929,28 +1682,102 @@ def main():
else:
current_vel = np.array([0.0, 0.0])
- # 获取动作(传入控制步长和当前速度)
- action = controller.get_action(dt=env.control_timestep, current_velocity=current_vel)
+ # 获取动作(传入控制步长、当前速度、状态和奖励)
+ action = controller.get_action(
+ dt=env.control_timestep,
+ current_velocity=current_vel,
+ state=obs,
+ reward=total_reward
+ )
obs, reward, done = env.step(action)
total_reward += reward
+ # 存储经验并训练深度学习控制器
+ if controller.use_deep_learning and controller.deep_controller is not None:
+ # 存储经验(使用上一个状态)
+ if controller.last_state is not None:
+ next_obs = obs if not done else None
+ controller.deep_controller.store_experience(
+ controller.last_state, action, reward, next_obs, done
+ )
+
+ # 更新上一个状态
+ controller.last_state = obs.copy()
+
+ # 定期训练
+ controller.deep_controller.step_count += 1
+ if controller.deep_controller.step_count % controller.deep_controller.update_frequency == 0:
+ controller.deep_controller.train_step()
+
+ # 重置时清空LSTM状态
+ if done:
+ controller.deep_controller.reset_lstm_state()
+
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]
+ torso_pos = env.data.xpos[torso_id] if torso_id >= 0 else None
+
head_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "head")
head_pos = env.data.xpos[head_id] if head_id >= 0 else None
+
+ # 尝试获取左右脚位置(可能有不同的命名)
+ foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_right")
+ if foot_right_id < 0:
+ foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_foot")
+ foot_right_pos = env.data.xpos[foot_right_id] if foot_right_id >= 0 else None
+
+ foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_left")
+ if foot_left_id < 0:
+ foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_foot")
+ foot_left_pos = env.data.xpos[foot_left_id] if foot_left_id >= 0 else None
+
+ # 尝试获取左右手位置
+ hand_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "hand_right")
+ if hand_right_id < 0:
+ hand_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_hand")
+ hand_right_pos = env.data.xpos[hand_right_id] if hand_right_id >= 0 else None
+
+ hand_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "hand_left")
+ if hand_left_id < 0:
+ hand_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_hand")
+ hand_left_pos = env.data.xpos[hand_left_id] if hand_left_id >= 0 else None
+
+ # 尝试获取骨盆位置
+ pelvis_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis")
+ pelvis_pos = env.data.xpos[pelvis_id] if pelvis_id >= 0 else None
+
+ # 格式化输出
+ info_parts = []
+ if torso_pos is not None:
+ info_parts.append(f"躯干={torso_pos}")
if head_pos is not None:
- print(f"Step {step}: 躯干位置 = {torso_pos}, 头部位置 = {head_pos}, 累计奖励 = {total_reward:.2f}")
- else:
- print(f"Step {step}: 躯干位置 = {torso_pos}, 累计奖励 = {total_reward:.2f}")
+ info_parts.append(f"头部={head_pos}")
+ if pelvis_pos is not None:
+ info_parts.append(f"骨盆={pelvis_pos}")
+ if foot_right_pos is not None:
+ info_parts.append(f"右脚={foot_right_pos}")
+ if foot_left_pos is not None:
+ info_parts.append(f"左脚={foot_left_pos}")
+ if hand_right_pos is not None:
+ info_parts.append(f"右手={hand_right_pos}")
+ if hand_left_pos is not None:
+ info_parts.append(f"左手={hand_left_pos}")
+
+ info_str = ", ".join(info_parts)
+ print(f"Step {step}: {info_str}, 累计奖励 = {total_reward:.2f}")
if done:
print(f"\nEpisode finished. Total reward: {total_reward:.2f}")
obs = env.reset()
total_reward = 0.0
step = 0
+ # 重置深度学习控制器状态
+ if controller.deep_controller is not None:
+ controller.deep_controller.reset_lstm_state()
+ controller.last_state = None
step += 1
time.sleep(0.01)
From 756c044eba160e35d7e616b2217fe114c8b282bb Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Sat, 13 Dec 2025 14:15:05 +0800
Subject: [PATCH 43/45] =?UTF-8?q?=E5=AE=8C=E5=96=84README.md=E8=AF=B4?=
=?UTF-8?q?=E6=98=8E=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../README.md | 297 ++++++++++++++----
1 file changed, 232 insertions(+), 65 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/README.md b/src/humanoid_simulation_environment_establish/README.md
index fb8ff0fd3d..1ef8200663 100644
--- a/src/humanoid_simulation_environment_establish/README.md
+++ b/src/humanoid_simulation_environment_establish/README.md
@@ -1,102 +1,269 @@
-# 人形机器人仿真环境(基于MuJoCo与dm_control)
+# 人形机器人仿真环境(基于MuJoCo)
## 项目概述
-本项目构建了一系列人形机器人仿真环境,基于MuJoCo物理引擎和dm_control框架,支持机器人运动控制、走廊导航、目标追踪等多种任务。通过模块化设计,实现了机器人模型加载、环境配置、控制策略与可视化展示的完整流程,适用于机器人控制算法研究与验证。
+
+本项目实现了一个基于MuJoCo物理引擎的人形机器人仿真环境,支持带空隙的走廊导航任务。项目包含交互式键盘控制、深度学习步态学习、无重力模式等多种功能,适用于机器人控制算法研究、步态优化和强化学习训练。
+
+## 核心特性
+
+- **交互式键盘控制**:支持实时键盘输入控制机器人前进、后退、转向
+- **深度学习控制器**:使用神经网络(MLP + LSTM)学习最优步态和姿态控制
+- **无重力模式**:支持无重力环境下的机器人运动仿真
+- **带空隙走廊环境**:构建包含平台和空隙的复杂走廊环境
+- **实时可视化**:使用MuJoCo viewer进行实时3D可视化
## 环境准备
### 依赖库安装
+
```bash
-pip install mujoco dm-control absl-py numpy
+pip install mujoco numpy
```
-- `mujoco`:物理引擎核心,负责动力学计算
-- `dm-control`:DeepMind开发的高级仿真框架,简化环境与任务构建
-- `absl-py`:命令行参数处理工具
+
+- `mujoco`:MuJoCo物理引擎(版本 >= 2.3.0)
- `numpy`:数值计算支持
+### 模型文件
+
+确保项目目录下存在 `humanoid.xml` 文件,该文件定义了人形机器人的结构、关节和执行器。
+
## 项目结构
-| 文件名 | 功能描述 |
-|-------------------|--------------------------------------------------------------|
-| `main.py` | 核心控制程序,实现工作目录自动切换、模型加载与基础运动控制 |
-| `main_humanoid.py`| 软体机器人弹跳仿真程序,基于比例控制实现从蹲姿到站姿的运动 |
-| `run_walls.py` | 带随机墙壁的走廊奔跑环境,使用dm_control构建强化学习任务 |
-| `motion_main.py` | 扩展运动环境,支持墙壁走廊和间隙走廊两种场景切换 |
-| `go_to_target.py` | 目标点追踪任务环境,实现机器人向指定目标位置移动的仿真 |
-| `humanoid.xml` | MJCF格式机器人模型文件,定义人形机器人的结构、关节与执行器 |
-| `README.md` | 项目说明文档 |
+| 文件名 | 功能描述 |
+|--------|----------|
+| `main.py` | 主程序文件,包含环境、控制器和主循环 |
+| `humanoid.xml` | 人形机器人模型文件(MJCF格式) |
+| `README.md` | 项目说明文档 |
-## 核心功能
+## 核心类说明
-### 1. 基础运动控制(main.py & main_humanoid.py)
-- 自动路径处理:程序启动时自动切换到项目根目录,确保相对路径正确
-- 模型加载验证:检查并加载人形机器人模型,提供清晰的加载状态反馈
-- 关键帧控制:基于预定义的"深蹲"和"站立"关键帧,通过比例控制实现平滑运动过渡
-- 实时可视化:使用MuJoCo viewer展示机器人运动状态,输出时间与躯干高度等关键信息
+### 1. GapCorridorEnvironment(带空隙走廊环境)
-```python
-# 核心控制逻辑示例
-qpos_error = target_qpos[7:] - data.qpos[7:] # 计算关节位置误差
-data.ctrl[:] = Kp * qpos_error # 比例控制输出
-```
+基于MuJoCo的仿真环境,支持带空隙的走廊场景。
-### 2. 走廊奔跑环境(run_walls.py & motion_main.py)
-- 动态场景生成:支持两种走廊类型(带墙壁/带间隙),墙壁宽度和间隙长度可随机化
-- 任务定义:设定目标速度(3.0单位/秒),通过奖励函数引导机器人高效移动
-- 多视角观测:支持第一视角相机和全局视角,便于观察与调试
-- 参数可配置:走廊长度、宽度、墙壁高度等参数可灵活调整
+**主要功能:**
+- 构建包含平台和空隙的走廊环境
+- 支持有重力/无重力两种模式
+- 提供观测、奖励和终止条件
+- 无重力模式下的姿态稳定控制
-```python
-# 走廊环境配置示例
-arena = corr_arenas.WallsCorridor(
- wall_gap=4., # 墙壁间隙宽度
- wall_width=distributions.Uniform(1, 7), # 随机墙壁宽度
- wall_height=3.0, # 墙壁高度
- corridor_length=100 # 走廊总长度
-)
-```
+**关键参数:**
+- `corridor_length`:走廊总长度(默认100米)
+- `corridor_width`:走廊宽度(默认10米)
+- `use_gravity`:是否启用重力(默认False,无重力模式)
+
+### 2. KeyboardController(键盘控制器)
+
+交互式键盘控制器,支持实时控制机器人运动。
+
+**控制指令:**
+- `w` / `↑`:前进
+- `s` / `↓`:后退
+- `a` / `←`:左转
+- `d` / `→`:右转
+- `空格`:暂停/继续
+- `r`:重置环境
+- `q`:退出程序
-### 3. 目标追踪任务(go_to_target.py)
-- 目标点导航:在平面环境中随机生成目标点,机器人需移动至目标位置
-- 灵活的时间限制:单次仿真时长可配置(默认30秒)
-- 兼容强化学习:环境接口符合强化学习标准,可直接用于训练策略
+**核心特性:**
+- 自然的人类步态生成(支撑相和摆动相协调)
+- 动作平滑处理(低通滤波 + 神经网络平滑)
+- PID速度控制
+- 与深度学习控制器混合使用
+
+### 3. DeepLearningController(深度学习控制器)
+
+使用神经网络学习最优步态和姿态控制策略。
+
+**网络结构:**
+- **策略网络**:MLP(2层) + LSTM + 输出层
+ - 输入:状态 + 步态相位编码 + 上次动作
+ - 输出:动作(执行器控制信号)
+- **价值网络**:MLP(3层)
+ - 输入:状态
+ - 输出:状态价值估计
+
+**学习机制:**
+- 经验回放缓冲区(最大10000条经验)
+- 策略梯度更新(REINFORCE算法)
+- 步态相位编码(sin/cos编码)
## 使用方法
-1. 基础运动控制仿真:
+### 基本运行
+
```bash
python main.py
```
-2. 走廊奔跑仿真(带墙壁):
-```bash
-python run_walls.py
+程序启动后会:
+1. 初始化带空隙的走廊环境(无重力模式)
+2. 启动MuJoCo交互式查看器
+3. 等待键盘输入控制机器人运动
+
+### 交互操作
+
+1. **在查看器窗口内按键盘**(窗口需要有焦点)
+2. 使用 `w/a/s/d` 或方向键控制机器人移动
+3. 按 `空格` 暂停/继续仿真
+4. 按 `r` 重置环境
+5. 按 `q` 或关闭窗口退出程序
+
+### 代码示例
+
+```python
+# 创建环境(无重力模式)
+env = GapCorridorEnvironment(
+ corridor_length=100,
+ corridor_width=10,
+ use_gravity=False
+)
+
+# 创建键盘控制器
+controller = KeyboardController(
+ env.model.nu,
+ env.get_actuator_indices()
+)
+
+# 主循环
+while viewer_handle.is_running():
+ action = controller.get_action(
+ dt=env.control_timestep,
+ current_velocity=current_vel,
+ state=obs,
+ reward=total_reward
+ )
+ obs, reward, done = env.step(action)
```
-3. 间隙走廊仿真:
-```bash
-python motion_main.py
+## 步态生成原理
+
+### 人类步态特点
+
+- **支撑相**:约占60%的时间,腿伸直、踝关节跖屈、推进身体
+- **摆动相**:约占40%的时间,抬腿、膝关节弯曲、踝关节背屈
+
+### 步态协调
+
+- 左右腿相位相差180度,形成交替步态
+- 髋关节前后摆动产生主要推进力
+- 膝关节和踝关节配合髋关节动作
+- 躯干轻微摆动保持平衡
+
+## 无重力模式特性
+
+在无重力模式下,系统实现了以下约束和控制:
+
+1. **位置约束**:
+ - 固定Z高度(防止飘起)
+ - 固定Y位置(保持在走廊中心)
+ - 允许X方向自由移动
+
+2. **姿态稳定**:
+ - 保持身体直立(roll和pitch为0)
+ - 允许绕Z轴旋转(yaw,用于转向)
+ - 头部高度稳定控制
+
+3. **速度控制**:
+ - 根据髋关节摆动计算前进速度
+ - 根据髋关节外展和躯干旋转计算转向角速度
+ - PID控制器平滑速度过渡
+
+## 深度学习控制器配置
+
+### 启用/禁用深度学习
+
+在 `KeyboardController` 初始化时设置:
+
+```python
+controller.use_deep_learning = True # 启用深度学习控制器
```
-4. 目标追踪仿真:
-```bash
-python go_to_target.py
+### 混合比例
+
+传统动作和深度学习动作的混合比例(在 `get_action` 方法中):
+
+```python
+raw_action = 0.7 * traditional_action + 0.3 * dl_action
```
-### 交互操作
-- 仿真窗口支持视角旋转、缩放和平移
-- 按`Ctrl+C`终止仿真程序
+### 模型保存/加载
+
+```python
+# 保存模型
+controller.deep_controller.save_model("model.pkl")
+
+# 加载模型
+controller.deep_controller.load_model("model.pkl")
+```
## 参数调整指南
-| 参数 | 调整范围 | 效果说明 |
-|-------------------|----------|----------------------------------|
-| `Kp`(比例增益) | 1.0~10.0 | 增大会加快响应速度,过大会导致震荡 |
-| `wall_gap` | 2.0~6.0 | 减小值增加走廊穿过难度 |
-| `target_velocity` | 1.0~5.0 | 提高值要求机器人运动速度更快 |
-| `control_timestep`| 0.01~0.1 | 减小值提高控制精度(增加计算量) |
+### 步态参数
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `step_frequency` | 0.9 Hz | 步频,控制步行速度 |
+| `step_duration` | 0.5 秒 | 每次按键的移动持续时间 |
+
+### 动作平滑参数
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `action_smoothing_factor` | 0.7 | 动作平滑系数(0-1,越大越平滑) |
+
+### 速度控制参数
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `_forward_velocity_gain` | 2.5 | 前进速度增益 |
+| `_turn_velocity_gain` | 0.5 | 转向速度增益 |
+| `_max_xy_velocity` | 2.0 m/s | 最大XY速度 |
+
+### 深度学习参数
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `learning_rate` | 0.001 | 学习率 |
+| `batch_size` | 64 | 批次大小 |
+| `update_frequency` | 10 | 每N步更新一次网络 |
+
+## 输出信息
+
+程序运行时会输出以下信息:
+
+- 每100步输出一次身体部位位置(躯干、头部、骨盆、脚部、手部)
+- 累计奖励值
+- 键盘控制状态变化
+- Episode结束时的总奖励
+
+## 注意事项
+
+1. **窗口焦点**:键盘输入需要在MuJoCo查看器窗口有焦点时才能生效
+2. **模型文件**:确保 `humanoid.xml` 文件存在于项目目录
+3. **执行器映射**:程序会自动从模型文件中提取执行器名称到索引的映射
+4. **无重力模式**:当前默认使用无重力模式,如需启用重力,修改 `main()` 函数中的 `use_gravity=True`
+
+## 技术细节
+
+### 步态相位编码
+
+使用 sin/cos 编码将步态相位(0-2π)编码为2维向量,使神经网络能够学习周期性步态模式。
+
+### LSTM状态管理
+
+策略网络使用简化的LSTM结构,保留隐藏状态和细胞状态,用于学习时序依赖关系。
+
+### 经验回放
+
+使用双端队列(deque)实现经验回放缓冲区,最大容量10000条经验,支持随机采样进行批量训练。
## 参考资料
+
- [MuJoCo官方文档](https://mujoco.readthedocs.io/)
-- [dm_control框架文档](https://github.com/deepmind/dm_control)
-- [CMU人形机器人模型说明](https://github.com/deepmind/dm_control/tree/main/dm_control/locomotion/walkers)
+- [MuJoCo Python API](https://mujoco.readthedocs.io/en/latest/python.html)
+- [MuJoCo Viewer使用指南](https://mujoco.readthedocs.io/en/latest/programming.html#viewer)
+
+## 许可证
+
+本项目基于MuJoCo物理引擎开发,请遵循MuJoCo的许可证要求。
From e0937a4f00e05607c26128936debcd48fb80ade7 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Sat, 20 Dec 2025 21:06:16 +0800
Subject: [PATCH 44/45] =?UTF-8?q?=E5=A2=9E=E5=BC=BA=E7=9B=91=E6=8E=A7?=
=?UTF-8?q?=E6=A8=A1=E5=9D=97+=E4=BC=98=E5=8C=96=E6=A0=BC=E5=BC=8F+?=
=?UTF-8?q?=E6=96=B0=E5=A2=9E=E8=A1=8D=E7=94=9F=E6=8C=87=E6=A0=87?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.py | 179 ++++++++++++++----
1 file changed, 141 insertions(+), 38 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py
index 9b72a715d9..ad736022be 100644
--- a/src/humanoid_simulation_environment_establish/main.py
+++ b/src/humanoid_simulation_environment_establish/main.py
@@ -1606,16 +1606,25 @@ def main():
# 将环境切换为“无重力”模式
env = GapCorridorEnvironment(corridor_length=100, corridor_width=10, use_gravity=False)
- print("\n环境已初始化")
- print(f"执行器数量: {env.model.nu}")
- print(f"关节数量: {env.model.nq}")
-
controller = KeyboardController(env.model.nu, env.get_actuator_indices())
obs = env.reset()
total_reward = 0.0
- print("\n启动MuJoCo交互式查看器...")
- print("按 ESC 或关闭窗口退出程序")
+ print("\n" + "="*80)
+ print("🚀 环境初始化完成")
+ print("-"*80)
+ print(f" 执行器数量: {env.model.nu}")
+ print(f" 关节数量: {env.model.nq}")
+ print(f" 观测维度: {len(obs)}")
+ print(f" 重力模式: {'启用' if env.use_gravity else '禁用(无重力模式)'}")
+ print(f" 控制时间步: {env.control_timestep:.3f}s")
+ print(f" 物理时间步: {env.timestep:.3f}s")
+ print(f" 最大Episode步数: {env._max_episode_steps}")
+ print("="*80)
+
+ print("\n📺 启动MuJoCo交互式查看器...")
+ print(" 提示: 在查看器窗口中按键盘进行控制")
+ print(" 提示: 按 ESC 或关闭窗口退出程序")
try:
viewer_handle = mujoco.viewer.launch_passive(
@@ -1626,7 +1635,9 @@ def main():
show_right_ui=True
)
- print("\n查看器已启动,开始仿真循环...")
+ print("\n✅ 查看器已启动,开始仿真循环...")
+ print(f" 状态报告将每100步输出一次")
+ print("")
step = 0
last_move_state = None # 记录上次移动状态,用于检测状态变化
@@ -1718,59 +1729,151 @@ def main():
if step % 100 == 0:
# 获取各个身体部位的位置
torso_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
- torso_pos = env.data.xpos[torso_id] if torso_id >= 0 else None
+ torso_pos = env.data.xpos[torso_id].copy() if torso_id >= 0 else None
head_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "head")
- head_pos = env.data.xpos[head_id] if head_id >= 0 else None
+ head_pos = env.data.xpos[head_id].copy() if head_id >= 0 else None
# 尝试获取左右脚位置(可能有不同的命名)
foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_right")
if foot_right_id < 0:
foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_foot")
- foot_right_pos = env.data.xpos[foot_right_id] if foot_right_id >= 0 else None
+ foot_right_pos = env.data.xpos[foot_right_id].copy() if foot_right_id >= 0 else None
foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_left")
if foot_left_id < 0:
foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_foot")
- foot_left_pos = env.data.xpos[foot_left_id] if foot_left_id >= 0 else None
+ foot_left_pos = env.data.xpos[foot_left_id].copy() if foot_left_id >= 0 else None
+
+ # 获取速度信息
+ linear_vel = np.zeros(6)
+ angular_vel = np.zeros(6)
+ if torso_id >= 0:
+ mujoco.mj_objectVelocity(env.model, env.data, mujoco.mjtObj.mjOBJ_BODY, torso_id, linear_vel, 0)
+ mujoco.mj_objectVelocity(env.model, env.data, mujoco.mjtObj.mjOBJ_BODY, torso_id, angular_vel, 1)
+ vx, vy, vz = linear_vel[0], linear_vel[1], linear_vel[2]
+ angular_vz = angular_vel[5] # 绕Z轴角速度(转向)
+ speed = np.sqrt(vx**2 + vy**2)
+
+ # 获取步态相位信息(如果有深度学习控制器)
+ gait_info = ""
+ if controller.deep_controller is not None:
+ gait_phase_deg = np.degrees(controller.deep_controller.gait_phase) % 360
+ gait_info = f"步态相位: {gait_phase_deg:.1f}°, 步频: {controller.deep_controller.gait_frequency:.2f}Hz"
+
+ # 获取动作统计信息
+ action_magnitude = np.max(np.abs(action))
+ action_mean = np.mean(np.abs(action))
+ action_std = np.std(action)
+
+ # 获取键盘控制状态
+ control_state = []
+ if controller.move_forward:
+ control_state.append("前进")
+ if controller.move_backward:
+ control_state.append("后退")
+ if controller.turn_left:
+ control_state.append("左转")
+ if controller.turn_right:
+ control_state.append("右转")
+ if not control_state:
+ control_state.append("静止")
+ control_str = "+".join(control_state) if control_state else "静止"
+
+ # 获取步态时间信息
+ step_time_info = f"步态时间: {controller.step_time:.2f}s"
+
+ # 计算运行时间(模拟)
+ sim_time = step * env.control_timestep
- # 尝试获取左右手位置
- hand_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "hand_right")
- if hand_right_id < 0:
- hand_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_hand")
- hand_right_pos = env.data.xpos[hand_right_id] if hand_right_id >= 0 else None
+ # 获取奖励信息(当前步奖励和累计奖励)
+ recent_reward = reward # 当前步的奖励
+ avg_reward_per_step = total_reward / max(step, 1)
- hand_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "hand_left")
- if hand_left_id < 0:
- hand_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_hand")
- hand_left_pos = env.data.xpos[hand_left_id] if hand_left_id >= 0 else None
+ # 打印分隔线
+ print("\n" + "="*80)
+ print(f"📊 状态报告 [Step {step} | 模拟时间: {sim_time:.2f}s]")
+ print("-"*80)
- # 尝试获取骨盆位置
- pelvis_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis")
- pelvis_pos = env.data.xpos[pelvis_id] if pelvis_id >= 0 else None
+ # 控制状态
+ print(f"🎮 控制状态: {control_str:20s} | {step_time_info}")
+ if gait_info:
+ print(f"🚶 {gait_info}")
- # 格式化输出
- info_parts = []
+ # 速度和运动信息
+ print(f"\n⚡ 运动信息:")
+ print(f" 线性速度: X={vx:+.3f} m/s, Y={vy:+.3f} m/s, Z={vz:+.3f} m/s")
+ print(f" 速度大小: {speed:.3f} m/s")
+ print(f" 角速度: Z轴旋转={np.degrees(angular_vz):+.2f} °/s")
+
+ # 位置信息(关键部位)
+ print(f"\n📍 身体部位位置:")
if torso_pos is not None:
- info_parts.append(f"躯干={torso_pos}")
+ print(f" 躯干: X={torso_pos[0]:+.3f}, Y={torso_pos[1]:+.3f}, Z={torso_pos[2]:+.3f} m")
if head_pos is not None:
- info_parts.append(f"头部={head_pos}")
- if pelvis_pos is not None:
- info_parts.append(f"骨盆={pelvis_pos}")
+ head_height = head_pos[2] - (torso_pos[2] if torso_pos is not None else 0)
+ print(f" 头部: X={head_pos[0]:+.3f}, Y={head_pos[1]:+.3f}, Z={head_pos[2]:+.3f} m (相对高度: {head_height:+.3f} m)")
if foot_right_pos is not None:
- info_parts.append(f"右脚={foot_right_pos}")
+ print(f" 右脚: X={foot_right_pos[0]:+.3f}, Y={foot_right_pos[1]:+.3f}, Z={foot_right_pos[2]:+.3f} m")
if foot_left_pos is not None:
- info_parts.append(f"左脚={foot_left_pos}")
- if hand_right_pos is not None:
- info_parts.append(f"右手={hand_right_pos}")
- if hand_left_pos is not None:
- info_parts.append(f"左手={hand_left_pos}")
+ print(f" 左脚: X={foot_left_pos[0]:+.3f}, Y={foot_left_pos[1]:+.3f}, Z={foot_left_pos[2]:+.3f} m")
+ # 计算步长(两脚间的X距离)
+ if foot_right_pos is not None:
+ step_length = abs(foot_right_pos[0] - foot_left_pos[0])
+ print(f" 步长: {step_length:.3f} m")
+
+ # 动作信息
+ print(f"\n🎯 动作统计:")
+ print(f" 动作维度: {len(action)}")
+ print(f" 最大幅度: {action_magnitude:.3f}")
+ print(f" 平均幅度: {action_mean:.3f}")
+ print(f" 标准差: {action_std:.3f}")
+
+ # 奖励信息
+ print(f"\n🏆 奖励信息:")
+ print(f" 当前步奖励: {recent_reward:+.4f}")
+ print(f" 累计奖励: {total_reward:+.4f}")
+ print(f" 平均奖励/步: {avg_reward_per_step:+.4f}")
- info_str = ", ".join(info_parts)
- print(f"Step {step}: {info_str}, 累计奖励 = {total_reward:.2f}")
+ # 深度学习信息(如果启用)
+ if controller.use_deep_learning and controller.deep_controller is not None:
+ buffer_size = len(controller.deep_controller.replay_buffer)
+ print(f"\n🧠 深度学习控制器:")
+ print(f" 经验缓冲区: {buffer_size}/{controller.deep_controller.replay_buffer.maxlen}")
+ print(f" 训练步数: {controller.deep_controller.step_count}")
+ print(f" 状态维度: {controller.deep_controller.state_dim}, 动作维度: {controller.deep_controller.action_dim}")
+
+ print("="*80 + "\n")
if done:
- print(f"\nEpisode finished. Total reward: {total_reward:.2f}")
+ # 计算Episode统计信息
+ episode_duration = step * env.control_timestep
+ avg_reward_per_step = total_reward / max(step, 1)
+
+ # 获取最终位置信息
+ final_torso_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
+
+ print("\n" + "="*80)
+ print("🎯 Episode 结束")
+ print("-"*80)
+ print(f" 总步数: {step}")
+ print(f" 持续时间: {episode_duration:.2f}s")
+ print(f" 累计奖励: {total_reward:+.4f}")
+ print(f" 平均奖励/步: {avg_reward_per_step:+.4f}")
+
+ # 获取最终位置信息
+ if final_torso_id >= 0:
+ final_torso_pos = env.data.xpos[final_torso_id]
+ print(f" 最终位置: X={final_torso_pos[0]:+.3f}, Y={final_torso_pos[1]:+.3f}, Z={final_torso_pos[2]:+.3f} m")
+ # 计算前进距离(从初始位置)
+ initial_pos = env._root_joint_qpos_start
+ if initial_pos is not None and (initial_pos + 2) < len(env.data.qpos):
+ initial_x = env.data.qpos[initial_pos]
+ distance_traveled = final_torso_pos[0] - initial_x
+ print(f" 前进距离: {distance_traveled:+.3f} m")
+
+ print("="*80 + "\n")
+
obs = env.reset()
total_reward = 0.0
step = 0
From 9a4271e0aa832df6358920b04ab278c8c941d369 Mon Sep 17 00:00:00 2001
From: qiqi678 <3490247469@qq.com>
Date: Tue, 23 Dec 2025 17:19:02 +0800
Subject: [PATCH 45/45] =?UTF-8?q?git=20commit=20-m=20=E4=BC=98=E5=8C=96?=
=?UTF-8?q?=E5=90=8E=E9=80=80/=E8=BD=AC=E5=90=91=E5=8A=A8=E4=BD=9C?=
=?UTF-8?q?=E5=B9=B6=E7=B2=BE=E7=AE=80=E7=BB=88=E7=AB=AF=E8=BE=93=E5=87=BA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../main.py | 116 ++++++------------
1 file changed, 37 insertions(+), 79 deletions(-)
diff --git a/src/humanoid_simulation_environment_establish/main.py b/src/humanoid_simulation_environment_establish/main.py
index ad736022be..a2c6ad2bd1 100644
--- a/src/humanoid_simulation_environment_establish/main.py
+++ b/src/humanoid_simulation_environment_establish/main.py
@@ -557,17 +557,26 @@ def _create_walking_action(self, forward=True, turn_direction=0):
if not self.actuator_indices:
return action
+ # 根据方向调节步频与强度:后退更慢、更谨慎
+ step_freq = self.step_frequency if forward else self.step_frequency * 0.8
+
# 计算步行动作相位(保持连续性)
- phase = 2 * np.pi * self.step_time * self.step_frequency
+ phase = 2 * np.pi * self.step_time * step_freq
direction = 1 if forward else -1
# 计算步态强度(基于step_time,用于平滑停止)
# 当step_time衰减时,动作幅度也平滑减小
- gait_strength = min(1.0, self.step_time * self.step_frequency * 2.0) # 在第一个周期内从0到1
+ gait_strength = min(1.0, self.step_time * step_freq * 2.0) # 在第一个周期内从0到1
# 如果step_time很小,进一步减小强度,实现平滑停止
if self.step_time < 0.1:
gait_strength *= self.step_time / 0.1 # 在最后0.1秒内平滑衰减到0
+ # 后退或转向时整体动作幅度更柔和
+ if not forward:
+ gait_strength *= 0.75
+ if turn_direction != 0:
+ gait_strength *= 0.9
+
# 人类步态特点:
# 1. 支撑相约占60%,摆动相约占40%
# 2. 摆动相时:抬腿、膝关节弯曲、踝关节背屈
@@ -602,7 +611,8 @@ def support_phase_weight(phi):
# 髋关节前后摆动(主要推进力)- 更自然的协调
# 使用更平滑的正弦波,在摆动相向前,支撑相向后推
# 添加轻微的相位偏移,让动作更自然
- right_hip_swing = 0.45 * direction * np.sin(right_phase + 0.1) * gait_strength
+ base_hip_amp = 0.45 if forward else 0.32
+ right_hip_swing = base_hip_amp * direction * np.sin(right_phase + 0.1) * gait_strength
self._set_action(action, "hip_x_right", right_hip_swing)
# 髋关节上下(抬腿)- 更自然的抬腿动作
@@ -722,31 +732,31 @@ def support_phase_weight(phi):
self._set_action(action, "hip_z_left", -hip_z_balance)
else:
# 转向时,外侧腿稍微外展,内侧腿稍微内收
- turn_strength = 0.4 * turn_direction
+ turn_strength = 0.25 * turn_direction * gait_strength
self._set_action(action, "hip_z_right", turn_strength)
self._set_action(action, "hip_z_left", -turn_strength)
# 躯干控制 - 更自然的轻微摆动
# 轻微前倾以辅助前进(减小前倾幅度,更自然)
- abdomen_pitch = 0.08 * direction * gait_strength
+ abdomen_pitch = 0.07 * direction * gait_strength
# 添加轻微的上下摆动(配合步态,与腿部动作协调)
# 在支撑相时稍微下沉,在摆动相时稍微上升
abdomen_pitch += 0.02 * np.sin(phase + np.pi/4) * gait_strength
self._set_action(action, "abdomen_y", abdomen_pitch)
# 转向时允许侧倾(减小侧倾幅度)
- self._set_action(action, "abdomen_x", 0.05 * turn_direction * gait_strength)
+ self._set_action(action, "abdomen_x", 0.04 * turn_direction * gait_strength)
# 转向控制(减小转向幅度,更自然)
if turn_direction != 0:
- self._set_action(action, "abdomen_z", 0.25 * turn_direction * gait_strength)
+ self._set_action(action, "abdomen_z", 0.22 * turn_direction * gait_strength)
else:
self._set_action(action, "abdomen_z", 0.0)
return action
def _create_turning_only_action(self, turn_direction, dt=0.03):
- """创建仅转向动作(不产生腿部摆动,只在原地转向,目标转向45度)"""
+ """创建仅转向动作:更平滑的原地转身"""
action = np.zeros(self.action_dim)
if not self.actuator_indices:
@@ -762,8 +772,8 @@ def _create_turning_only_action(self, turn_direction, dt=0.03):
if abs(turn_error) < 0.1: # 接近目标时,设置新的目标
self.target_turn_angle += turn_direction * self.turn_angle_per_step
- # 计算转向速度(基于误差)
- turn_velocity = np.clip(turn_error * 3.0, -self.turn_speed, self.turn_speed)
+ # 计算转向速度(基于误差),限制更小避免生硬
+ turn_velocity = np.clip(turn_error * 2.0, -self.turn_speed * 0.7, self.turn_speed * 0.7)
# 更新当前转向角度(模拟)
self.current_turn_angle += turn_velocity * dt
@@ -780,33 +790,19 @@ def _create_turning_only_action(self, turn_direction, dt=0.03):
normalized_turn = turn_direction * 0.8 # 直接使用方向,强度0.8
# 原地转向:通过髋关节外展和躯干旋转实现
- # 增大转向强度,使转向更明显
- hip_turn_strength = 0.6 * normalized_turn # 从0.25增大到0.6
+ # 略微减小强度并加入轻微屈膝,让转身更稳
+ hip_turn_strength = 0.45 * normalized_turn
self._set_action(action, "hip_z_right", hip_turn_strength)
self._set_action(action, "hip_z_left", -hip_turn_strength)
# 躯干旋转辅助转向(主要转向来源,范围±45度)
- abdomen_turn_strength = 0.8 * normalized_turn # 从0.15增大到0.8,充分利用±45度范围
+ abdomen_turn_strength = 0.65 * normalized_turn
self._set_action(action, "abdomen_z", abdomen_turn_strength)
self._set_action(action, "abdomen_x", 0.1 * normalized_turn)
- 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)
+ # 轻微屈膝降低质心
+ self._set_action(action, "knee_right", 0.12 * abs(normalized_turn))
+ self._set_action(action, "knee_left", 0.12 * abs(normalized_turn))
return action
@@ -1726,25 +1722,11 @@ def main():
env.render(viewer_handle)
- if step % 100 == 0:
- # 获取各个身体部位的位置
+ if step % 200 == 0:
+ # 获取身体位置
torso_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
torso_pos = env.data.xpos[torso_id].copy() if torso_id >= 0 else None
-
- head_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "head")
- head_pos = env.data.xpos[head_id].copy() if head_id >= 0 else None
-
- # 尝试获取左右脚位置(可能有不同的命名)
- foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_right")
- if foot_right_id < 0:
- foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_foot")
- foot_right_pos = env.data.xpos[foot_right_id].copy() if foot_right_id >= 0 else None
-
- foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_left")
- if foot_left_id < 0:
- foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_foot")
- foot_left_pos = env.data.xpos[foot_left_id].copy() if foot_left_id >= 0 else None
-
+
# 获取速度信息
linear_vel = np.zeros(6)
angular_vel = np.zeros(6)
@@ -1800,48 +1782,24 @@ def main():
if gait_info:
print(f"🚶 {gait_info}")
- # 速度和运动信息
- print(f"\n⚡ 运动信息:")
- print(f" 线性速度: X={vx:+.3f} m/s, Y={vy:+.3f} m/s, Z={vz:+.3f} m/s")
- print(f" 速度大小: {speed:.3f} m/s")
- print(f" 角速度: Z轴旋转={np.degrees(angular_vz):+.2f} °/s")
+ # 速度和运动信息(精简)
+ print(f"\n⚡ 速度: |V|={speed:.3f} m/s, vx={vx:+.3f}, vy={vy:+.3f}, yaw_rate={np.degrees(angular_vz):+.2f} °/s")
- # 位置信息(关键部位)
- print(f"\n📍 身体部位位置:")
+ # 关键位置
if torso_pos is not None:
- print(f" 躯干: X={torso_pos[0]:+.3f}, Y={torso_pos[1]:+.3f}, Z={torso_pos[2]:+.3f} m")
- if head_pos is not None:
- head_height = head_pos[2] - (torso_pos[2] if torso_pos is not None else 0)
- print(f" 头部: X={head_pos[0]:+.3f}, Y={head_pos[1]:+.3f}, Z={head_pos[2]:+.3f} m (相对高度: {head_height:+.3f} m)")
- if foot_right_pos is not None:
- print(f" 右脚: X={foot_right_pos[0]:+.3f}, Y={foot_right_pos[1]:+.3f}, Z={foot_right_pos[2]:+.3f} m")
- if foot_left_pos is not None:
- print(f" 左脚: X={foot_left_pos[0]:+.3f}, Y={foot_left_pos[1]:+.3f}, Z={foot_left_pos[2]:+.3f} m")
- # 计算步长(两脚间的X距离)
- if foot_right_pos is not None:
- step_length = abs(foot_right_pos[0] - foot_left_pos[0])
- print(f" 步长: {step_length:.3f} m")
+ print(f"📍 位置: X={torso_pos[0]:+.3f}, Y={torso_pos[1]:+.3f}, Z={torso_pos[2]:+.3f} m")
# 动作信息
- print(f"\n🎯 动作统计:")
- print(f" 动作维度: {len(action)}")
- print(f" 最大幅度: {action_magnitude:.3f}")
- print(f" 平均幅度: {action_mean:.3f}")
- print(f" 标准差: {action_std:.3f}")
+ print(f"🎯 动作: max={action_magnitude:.3f}, mean={action_mean:.3f}, std={action_std:.3f}")
# 奖励信息
- print(f"\n🏆 奖励信息:")
- print(f" 当前步奖励: {recent_reward:+.4f}")
- print(f" 累计奖励: {total_reward:+.4f}")
- print(f" 平均奖励/步: {avg_reward_per_step:+.4f}")
+ print(f"🏆 奖励: step={recent_reward:+.4f}, total={total_reward:+.4f}, avg/step={avg_reward_per_step:+.4f}")
# 深度学习信息(如果启用)
if controller.use_deep_learning and controller.deep_controller is not None:
buffer_size = len(controller.deep_controller.replay_buffer)
- print(f"\n🧠 深度学习控制器:")
- print(f" 经验缓冲区: {buffer_size}/{controller.deep_controller.replay_buffer.maxlen}")
- print(f" 训练步数: {controller.deep_controller.step_count}")
- print(f" 状态维度: {controller.deep_controller.state_dim}, 动作维度: {controller.deep_controller.action_dim}")
+ max_buffer = controller.deep_controller.replay_buffer.maxlen
+ print(f"🧠 训练: buffer={buffer_size}/{max_buffer}, steps={controller.deep_controller.step_count}")
print("="*80 + "\n")