From 7a4cbf631575f995150311f0ba936d162197e04e Mon Sep 17 00:00:00 2001 From: Evangeline27-yt <1666286480@qq.com> Date: Sat, 18 Apr 2026 08:38:28 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E5=AE=8C=E6=88=90=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/README.md | 188 +++++++++++++----------------------- 1 file changed, 67 insertions(+), 121 deletions(-) diff --git a/src/Mujoco_manrun/README.md b/src/Mujoco_manrun/README.md index 132b9f68c..dd760c986 100644 --- a/src/Mujoco_manrun/README.md +++ b/src/Mujoco_manrun/README.md @@ -1,124 +1,70 @@ -# 人形机器人行走模拟 - -## 项目概述 -本项目构建了基于 **ROS 1 (Noetic)** 与 **Mujoco** 仿真平台的人形机器人步态控制仿真系统，通过 CPG（中枢模式发生器）算法实现稳定步态生成，集成标准 ROS 接口与多模态控制方式，提供模块化、可扩展的代码库支持人形机器人运动控制算法研发。借助 Mujoco 的高保真物理仿真能力，模拟真实人形机器人的动力学特性与运动约束，为步态控制技术的学习、研究与开发提供可靠的仿真环境。 - -## 环境准备 - -### 依赖库安装 -```bash -# 建议使用Python 3.8及以上版本（ROS Noetic默认Python 3.8） -pip3 install mujoco>=2.3.7 numpy -``` - -#### 依赖说明： -- mujoco>=2.3.7：高保真物理仿真核心库，提供机器人动力学计算与可视化支持 -- numpy：数值计算基础库，支持数组运算与参数优化 -- ros-noetic-geometry-msgs：ROS 标准几何消息类型（如速度指令 Twist） -- ros-noetic-sensor-msgs：ROS 标准传感器消息类型（如关节状态 JointState） -- ros-noetic-std-msgs：ROS 标准基础消息类型（如字符串 String） -- ros-noetic-rviz：ROS 可视化工具，支持机器人模型与状态实时展示 - -### 开发环境配置 -1. 安装 Ubuntu 20.04 LTS 操作系统（ROS Noetic 适配版本） -2. 安装 ROS 1 Noetic [官方完整版](http://wiki.ros.org/noetic/Installation/Ubuntu)（推荐 `ros-noetic-desktop-full`） -3. 安装 [VSCode](https://code.visualstudio.com/) 并配置 Python 3.8+ 解释器 -4. 推荐插件：Python、Pylance、ROS、Code Runner（提升 ROS 开发效率） - -## 项目结构 - -| 文件名 | 功能描述 | -|-------------------------|--------------------------------------------------------------| -| `CMakeLists.txt` | ROS 编译配置文件，声明依赖、安装规则与编译选项 | -| `package.xml` | ROS 包元信息文件，定义包名、版本、依赖与维护者信息 | -| `setup.py` | Python 模块安装配置，确保脚本与模块可被 ROS 识别调用 | -| `launch/mujoco_manrun.launch` | ROS 一键启动文件，加载参数配置并启动仿真节点 | -| `config/gait_params.yaml` | 核心配置文件，存储 CPG 步态参数、仿真参数与关节控制参数 | -| `scripts/main.py` | 仿真系统入口脚本，负责 ROS 节点初始化、模块加载与主循环控制 | -| `scripts/humanoid_stabilizer.py` | 核心控制模块，集成 CPG 步态生成、动力学解算与关节控制逻辑 | -| `scripts/utils.py` | 通用工具函数库，包含数值裁剪、参数转换等辅助功能 | -| `models/humanoid.xml` | Mujoco 机器人模型文件，定义人形机器人的连杆、关节与物理属性 | -| `README.md` | 项目说明文档，包含环境配置、使用方法与常见问题解决 | - -## 核心功能 - -### 1. 高保真动力学仿真 -- 基于 Mujoco 实现人形机器人的精确动力学建模，还原关节摩擦、连杆质量等物理特性 -- 支持 100Hz 高频率仿真，保证步态控制的实时性与稳定性 -- 内置可视化窗口，实时展示机器人运动状态与关节轨迹 - -### 2. CPG 步态生成系统 -- 基于中枢模式发生器（CPG）算法，支持 NORMAL/SLOW/FAST/原地踏步 四种步态模式 -- 步态参数（频率、振幅）支持运行时动态调整，适配不同运动场景 -- 内置关节角度裁剪与PD控制，确保运动安全与平滑过渡 - -### 3. 标准 ROS 接口集成 -- 订阅 ROS 标准 `/cmd_vel` 话题，支持线速度/角速度指令输入 -- 发布 `/joint_states` 话题，输出关节位置、速度与力矩数据 -- 集成 ROS 参数服务器，支持仿真参数与步态参数实时配置 - -### 4. 多模态控制方式 -- 键盘控制：支持 W/A/S/D 方向控制与数字键步态切换，操作直观 -- ROS 话题控制：通过 `rostopic pub` 命令发布控制指令，便于算法集成 -- 参数动态调整：通过 `rosparam set` 命令实时修改步态与仿真参数 - -## 使用方法 - -### 编译项目 -```bash -# 进入 ROS 工作空间 -cd ~/catkin_ws -# 编译功能包 -catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release -# 激活 ROS 环境 -source devel/setup.bash -``` - -### 一键启动仿真 - -```bash -# 启动 ROS 核心、仿真节点与可视化窗口 -roslaunch mujoco_manrun mujoco_manrun.launch -``` - -### 手动分步启动（调试用） -```bash -# 1. 启动 ROS 核心（新终端） -roscore -# 2. 启动仿真节点（新终端，需先激活 ROS 环境） -rosrun mujoco_manrun main.py -# 3. 启动 RViz 可视化（可选，新终端） -rviz -d $(find mujoco_manrun)/config/rviz_config.rviz -``` -### 控制指令示例 -```bash -# 1. ROS 话题控制：发布前进指令（线速度 0.5 m/s） -rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" -# 2. ROS 话题控制：发布左转指令（角速度 0.2 rad/s） -rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "{linear: {x: 0.0}, angular: {z: 0.2}}" -# 3. 动态调整参数：修改 CPG 步态频率为 0.7 Hz -rosparam set /humanoid/cpg/freq 0.7 -``` - -## 参数调整指南 - -| 参数路径 | 调整范围 | 效果说明 | -|-------------------------|----------------|--------------------------------------------------------------| -| `/humanoid/cpg/freq` | 0.3~1.0 Hz | 调整 CPG 振荡器频率，数值越大步态越快（建议 0.5~0.7） | -| `/humanoid/cpg/amp` | 0.2~0.6 | 调整 CPG 振荡器振幅，数值越大关节摆动幅度越大（建议 0.4~0.5） | -| `/humanoid/sim_freq` | 50~200 Hz | 调整仿真频率，数值越高实时性越好（需硬件支持，默认 100） | -| `/humanoid/use_viewer` | true/false | 启用/禁用 Mujoco 可视化窗口（禁用可提升仿真性能） | -| `/humanoid/init_wait_time` | 2.0~10.0 s | 调整初始稳定时间，确保机器人启动后姿态平稳（默认 4.0） | - -## 常见问题解决 - -| 问题描述 | 解决方案 | -|--------------------------------------------------------------|--------------------------------------------------------------| -| 编译报错「The script 'humanoid_sim_node.py' doesn't exist」 | 修改 `setup.py`，确保 `scripts` 列表仅包含 `main.py`（参考项目结构中的配置） | -| 仿真启动报错「ValueError: The truth value of an array is ambiguous」 | 替换 `utils.py` 中的 `clip_value` 函数为 numpy 兼容版本（参考核心功能模块） | -| Mujoco 可视化窗口不显示 | 安装兼容版本可视化库：`pip3 install mujoco-viewer==0.1.7` | -| ROS 话题无数据输出 | 检查 `ROS_MASTER_URI` 配置（默认 `http://localhost:11311`），重启 `roscore` 与仿真节点 | -| 机器人运动不稳定或跌倒 | 降低 CPG 频率（如 0.3 Hz）或振幅（如 0.3），增加初始稳定时间 | +# 人形机器人 CPG 步态控制仿真系统 +## 1. 项目概述 +本项目基于 MuJoCo 物理仿真平台，实现了一套**基于 CPG（中枢模式发生器）的人形机器人步态生成与控制系统**。系统以仿生节律算法为核心，完成机器人双腿交替摆动的自然步态生成，并支持键盘实时控制、多模式步态切换、状态机管理与多级安全保护。 + +受限于简化机器人模型结构，项目重点验证 **CPG 步态生成算法有效性** 与**基础运动控制框架**，可为后续双足机器人平衡控制、重心规划、动态行走等高级算法提供可扩展的开发平台。 + +## 2. 功能 +- **CPG 仿生节律步态**：模拟生物中枢神经模式，实现平滑、周期性的双腿交替运动 +- **多状态控制逻辑**：STAND / WALK / STOP / EMERGENCY 状态管理 +- **键盘实时控制**：W/S 行走启停，A/D 转向控制 +- **多步态模式切换**：慢走 / 正常 / 快速 / 原地踏步 +- **IMU 姿态传感器模拟**：获取机器人倾斜角与角速度 +- **关节安全限位**：防止运动超限与姿态畸变 +- **纯 Python 轻量化运行**：Windows 直接启动 + +## 3. 环境依赖 +bash +pip install mujoco numpy +- Python 3.8 ~ 3.11 +- MuJoCo 2.3.0 及以上 + +## 4. 项目结构 +├── models/ +│ └── humanoid.xml # MuJoCo 简化人形机器人模型 +├── humanoid_stabilizer.py # 主控制程序（仿真入口） +├── cpg_oscillator.py # CPG 中枢模式发生器 +├── sensor_simulator.py # IMU 姿态传感器模拟 +├── keyboard_handler.py # 键盘控制线程 +├── utils.py # 工具函数（数值裁剪、角度转换等） +└── README.md # 项目说明文档 +## 5. 核心模块说明 +### 5.1 CPG 步态生成模块 +- 采用正弦振荡器生成连续、周期性控制信号 +- 输出信号直接驱动左右髋关节、膝关节交替运动 +- 可在线调节频率、振幅，实现步态速度与步幅控制 +### 5.2 运动控制模块 +- 状态机控制机器人行为模式 +- 行走状态：CPG 节律输出 +- 停止/站立状态：关节归零，保持静止 +- 紧急状态：控制量强制清零，安全停机 +### 5.3 传感器模拟模块 +- 模拟机器人躯干 IMU 传感器 +- 输出滚动角(roll)、俯仰角(pitch)及其角速度 +- 为后续平衡控制提供状态反馈 +### 5.4 键盘控制模块 +- 独立线程监听键盘输入 +- 支持实时启停、转向、步态切换 +- 操作简单、便于调试与演示 + +## 6. 使用方法 +### 6.1 启动仿真 +bash +python humanoid_stabilizer.py + +### 6.2 键盘控制说明 +- `W`：进入行走模式，启动 CPG 步态 +- `S`：停止步态，关节归零保持静止 +- `A/D`：左右转向控制 +- `1/2/3/4`：切换步态速度（慢走/正常/快速/踏步） +- `Q`：退出仿真 + +## 7. 改进方向 +- 更换高精度双足机器人模型 +- 加入 ZMP 稳定控制与倒立摆平衡算法 +- 引入强化学习实现自适应步态优化 +- 接入 ROS 实现多机通信与真实机器人部署 +- 增加足底压力传感器、力控反馈 ## 参考资料 - [ROS 1 Noetic 官方文档](http://wiki.ros.org/noetic) From 93fc0fd99390ecdb01731ea010dc93910ab43168 Mon Sep 17 00:00:00 2001 From: Evangeline27-yt <1666286480@qq.com> Date: Sat, 18 Apr 2026 11:24:36 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=8E=9F=E6=9C=89?= =?UTF-8?q?=E9=A1=B9=E7=9B=AE=E8=BF=90=E8=A1=8C=E9=94=99=E8=AF=AF=EF=BC=8C?= =?UTF-8?q?=E6=96=B0=E5=A2=9EPyCharm=E7=BB=88=E7=AB=AF=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E4=BA=A4=E4=BA=92=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 107 +++++------ .../scripts/humanoid_stabilizer.py | 175 +++++++++++------- src/Mujoco_manrun/scripts/keyboard_handler.py | 2 +- src/Mujoco_manrun/scripts/utils.py | 10 +- 4 files changed, 161 insertions(+), 133 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index bdb58f054..38e8464b7 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -1,4 +1,3 @@ -import mujoco import numpy as np from mujoco import viewer import time @@ -8,7 +7,7 @@ from collections import deque -# ===================== ROS话题接收模块（原有逻辑完全保留） ===================== +# ===================== ROS话题接收模块 ===================== class ROSCmdVelHandler(threading.Thread): """ROS /cmd_vel话题接收线程，映射linear.x→速度，angular.z→转向""" @@ -78,7 +77,7 @@ def stop(self): self.running = False -# ===================== 键盘输入监听线程（新增步态切换按键） ===================== +# ===================== 终端键盘控制（pycham） ===================== class KeyboardInputHandler(threading.Thread): def __init__(self, stabilizer): super().__init__(daemon=True) @@ -86,87 +85,88 @@ def __init__(self, stabilizer): self.running = True def run(self): - print("\n===== 控制指令说明 =====") - print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") - print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") - print("m: 传感器模拟开关 | p: 打印传感器数据") - print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步") # 新增步态切换 - print("========================\n") + print("\n===== PyCharm 终端控制已启动 =====") + print("w: 行走 | s: 停止 | e: 急停 | r: 复位") + print("a: 左转 | d: 右转 | 空格: 原地转") + print("z: 减速 | x: 加速") + print("1: 慢走 | 2: 正常 | 3: 小跑 | 4: 踏步") + print("m: 传感器开关 | p: 打印数据") + print("===================================\n") + while self.running: try: - # 非阻塞键盘输入（兼容不同系统） - if sys.platform == "win32": - import msvcrt - if msvcrt.kbhit(): - key = msvcrt.getch().decode('utf-8').lower() - self._handle_key(key) - else: - import select - if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: - key = sys.stdin.read(1).lower() - self._handle_key(key) - time.sleep(0.01) + import sys + import tty + import termios + fd = sys.stdin.fileno() + old = termios.tcgetattr(fd) + tty.setraw(fd) + + if sys.stdin.read(1) == '\x1b': # 方向键忽略 + sys.stdin.read(2) + termios.tcsetattr(fd, termios.TCSADRAIN, old) + continue + + key = sys.stdin.read(1) + termios.tcsetattr(fd, termios.TCSADRAIN, old) + + self._handle_key(key) except: - continue + key = sys.stdin.read(1) + self._handle_key(key) + time.sleep(0.05) def _handle_key(self, key): + key = key.lower() if key == 'w': - # 启动行走时保留当前步态模式 current_gait = self.stabilizer.gait_mode self.stabilizer.set_state("WALK") self.stabilizer.set_gait_mode(current_gait) - print( - f"[指令] 切换为行走状态 | 当前步态: {current_gait} | 速度: {self.stabilizer.walk_speed:.2f} | 转向: {self.stabilizer.turn_angle:.2f}") + print(f"[行走] 速度:{self.stabilizer.walk_speed:.2f} | 转向:{self.stabilizer.turn_angle:.2f}") elif key == 's': self.stabilizer.set_state("STOP") - print("[指令] 切换为停止状态") + print("[停止]") elif key == 'e': self.stabilizer.set_state("EMERGENCY") - print("[指令] 触发紧急停止") + print("[紧急停止]") elif key == 'r': self.stabilizer.set_state("STAND") - print("[指令] 恢复站立姿态") + print("[恢复站立]") elif key == 'a': self.stabilizer.set_turn_angle(self.stabilizer.turn_angle + 0.05) - print(f"[指令] 左转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") + print(f"[左转] {self.stabilizer.turn_angle:.2f} rad") elif key == 'd': self.stabilizer.set_turn_angle(self.stabilizer.turn_angle - 0.05) - print(f"[指令] 右转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") + print(f"[右转] {self.stabilizer.turn_angle:.2f} rad") elif key == ' ': self.stabilizer.set_turn_angle(0.2 if self.stabilizer.turn_angle <= 0 else -0.2) - print(f"[指令] 原地转向 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") + print(f"[原地转向] {self.stabilizer.turn_angle:.2f} rad") elif key == 'z': self.stabilizer.set_walk_speed(self.stabilizer.walk_speed - 0.1) - print(f"[指令] 减速 | 当前速度: {self.stabilizer.walk_speed:.2f} | 当前步态: {self.stabilizer.gait_mode}") + print(f"[减速] 速度:{self.stabilizer.walk_speed:.2f}") elif key == 'x': self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1) - print(f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f} | 当前步态: {self.stabilizer.gait_mode}") + print(f"[加速] 速度:{self.stabilizer.walk_speed:.2f}") elif key == 'm': - # 传感器模拟开关 self.stabilizer.enable_sensor_simulation = not self.stabilizer.enable_sensor_simulation - print(f"[指令] 传感器模拟{'开启' if self.stabilizer.enable_sensor_simulation else '关闭'}") + print(f"[传感器] {'开启' if self.stabilizer.enable_sensor_simulation else '关闭'}") elif key == 'p': - # 打印传感器数据 self.stabilizer.print_sensor_data() - # 新增：步态模式切换 elif key == '1': self.stabilizer.set_gait_mode("SLOW") - print( - f"[指令] 切换为慢走模式 | CPG频率: {self.stabilizer.gait_config['SLOW']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['SLOW']['amp']:.2f}") + print("[模式] 慢走") elif key == '2': self.stabilizer.set_gait_mode("NORMAL") - print( - f"[指令] 切换为正常走模式 | CPG频率: {self.stabilizer.gait_config['NORMAL']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['NORMAL']['amp']:.2f}") + print("[模式] 正常") elif key == '3': self.stabilizer.set_gait_mode("TROT") - print( - f"[指令] 切换为小跑模式 | CPG频率: {self.stabilizer.gait_config['TROT']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['TROT']['amp']:.2f}") + print("[模式] 小跑") elif key == '4': self.stabilizer.set_gait_mode("STEP_IN_PLACE") - print(f"[指令] 切换为原地踏步模式 | 步幅减半，躯干锁定") + print("[模式] 原地踏步") -# ===================== CPG中枢模式发生器（原有逻辑完全保留） ===================== +# ===================== CPG中枢模式发生器 ===================== class CPGOscillator: def __init__(self, freq=0.5, amp=0.4, phase=0.0, coupling_strength=0.2): self.base_freq = freq # 基础频率（对应原始步态周期2s） @@ -203,15 +203,11 @@ def reset(self): self.freq = self.base_freq self.amp = self.base_amp self.coupling = self.base_coupling - self.phase = 0.0 if self.base_phase == 0.0 else np.pi + self.phase = 0.0 if self.phase < np.pi else np.pi self.state = np.array([np.sin(self.phase), np.cos(self.phase)]) - @property - def base_phase(self): - return 0.0 if self.phase < np.pi else np.pi - -# ===================== 人形机器人控制器（新增多步态模式） ===================== +# ===================== 人形机器人控制器 ===================== class HumanoidStabilizer: """适配humanoid.xml模型的稳定站立与行走控制器（新增多步态+传感器模拟+鲁棒性优化）""" @@ -664,7 +660,7 @@ def _state_walk(self): # 关节目标低通滤波（原有逻辑） if self.enable_robust_optim: self.joint_targets = ( - 1 - self.filter_alpha) * self.prev_joint_targets + self.filter_alpha * self.joint_targets + 1 - self.filter_alpha) * self.prev_joint_targets + self.filter_alpha * self.joint_targets self.prev_joint_targets = self.joint_targets.copy() # 更新原始步态相位（原有逻辑） @@ -970,7 +966,7 @@ def simulate_stable_standing(self): if __name__ == "__main__": current_directory = os.path.dirname(os.path.abspath(__file__)) - model_file_path = os.path.join(current_directory, "humanoid.xml") + model_file_path = os.path.join(current_directory, "models","humanoid.xml") print(f"模型路径：{model_file_path}") if not os.path.exists(model_file_path): @@ -978,13 +974,8 @@ def simulate_stable_standing(self): try: stabilizer = HumanoidStabilizer(model_file_path) - # 可选配置：关闭传感器模拟/鲁棒优化/修改默认步态 - # stabilizer.enable_sensor_simulation = False - # stabilizer.enable_robust_optim = False - # stabilizer.set_gait_mode("SLOW") # 默认改为慢走 stabilizer.simulate_stable_standing() except Exception as e: print(f"错误：{e}") import traceback - traceback.print_exc() \ No newline at end of file diff --git a/src/Mujoco_manrun/scripts/humanoid_stabilizer.py b/src/Mujoco_manrun/scripts/humanoid_stabilizer.py index 3c07b1991..b847c5390 100644 --- a/src/Mujoco_manrun/scripts/humanoid_stabilizer.py +++ b/src/Mujoco_manrun/scripts/humanoid_stabilizer.py @@ -1,5 +1,7 @@ import sys import os +import threading + SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, SCRIPT_DIR) @@ -9,64 +11,128 @@ from cpg_oscillator import CPGOscillator from sensor_simulator import SensorSimulator from utils import quat_to_euler_xyz, clip_value -from keyboard_handler import KeyboardInputHandler # 新增导入 + +# ===================== 键盘输入 ===================== +class KeyboardInputHandler(threading.Thread): + def __init__(self, stabilizer): + super().__init__(daemon=True) + self.stabilizer = stabilizer + self.running = True + + def run(self): + print("\n===== 控制指令说明 =====") + print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") + print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") + print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步") + print("========================\n") + while self.running: + try: + if sys.platform == "win32": + import msvcrt + if msvcrt.kbhit(): + key = msvcrt.getch().decode('utf-8').lower() + self._handle_key(key) + else: + import select + if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: + key = sys.stdin.read(1).lower() + self._handle_key(key) + time.sleep(0.01) + except: + continue + + def _handle_key(self, key): + if key == 'w': + current_gait = self.stabilizer.gait_mode + self.stabilizer.set_state("WALK") + self.stabilizer.set_gait_mode(current_gait) + print(f"[指令] 切换为行走状态 | 当前步态: {current_gait}") + elif key == 's': + self.stabilizer.set_state("STOP") + print("[指令] 切换为停止状态") + elif key == 'e': + self.stabilizer.set_state("EMERGENCY") + print("[指令] 触发紧急停止") + elif key == 'r': + self.stabilizer.set_state("STAND") + print("[指令] 恢复站立姿态") + elif key == 'a': + self.stabilizer.set_turn_angle(self.stabilizer.turn_angle + 0.05) + print(f"[指令] 左转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") + elif key == 'd': + self.stabilizer.set_turn_angle(self.stabilizer.turn_angle - 0.05) + print(f"[指令] 右转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") + elif key == ' ': + self.stabilizer.set_turn_angle(0.2 if self.stabilizer.turn_angle <= 0 else -0.2) + print(f"[指令] 原地转向 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") + elif key == 'z': + self.stabilizer.set_walk_speed(self.stabilizer.walk_speed - 0.1) + print(f"[指令] 减速 | 当前速度: {self.stabilizer.walk_speed:.2f}") + elif key == 'x': + self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1) + print(f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f}") + elif key == 'm': + self.stabilizer.enable_sensor_simulation = not self.stabilizer.enable_sensor_simulation + print(f"[指令] 传感器模拟{'开启' if self.stabilizer.enable_sensor_simulation else '关闭'}") + elif key == 'p': + self.stabilizer.print_sensor_data() + elif key == '1': + self.stabilizer.set_gait_mode("SLOW") + print(f"[指令] 切换为慢走模式") + elif key == '2': + self.stabilizer.set_gait_mode("NORMAL") + print(f"[指令] 切换为正常走模式") + elif key == '3': + self.stabilizer.set_gait_mode("TROT") + print(f"[指令] 切换为小跑模式") + elif key == '4': + self.stabilizer.set_gait_mode("STEP_IN_PLACE") + print(f"[指令] 切换为原地踏步模式") + +# ===================== 以下完全不动 ===================== class HumanoidStabilizer: def __init__(self, model_path): - # 加载Mujoco模型 self.model = mujoco.MjModel.from_xml_path(model_path) self.data = mujoco.MjData(self.model) - - # 初始化CPG振荡器（步态生成） + self.cpg = CPGOscillator() - - # 初始化传感器模拟 self.sensor = SensorSimulator(self.model, self.data) - - # 控制参数 - self.velocity = 0.0 # 前进速度 - self.turn_rate = 0.0 # 转向速率 - self.gait_mode = "NORMAL" # 默认步态 - self.turn_angle = 0.0 # 新增转向角度参数 - self.walk_speed = 0.5 # 新增行走速度参数 - self.state = "STAND" # 新增状态参数 - self.enable_sensor_simulation = True # 传感器模拟开关 - - # ROS相关初始化（兼容处理） + + self.velocity = 0.0 + self.turn_rate = 0.0 + self.gait_mode = "NORMAL" + self.turn_angle = 0.0 + self.walk_speed = 0.5 + self.state = "STAND" + self.enable_sensor_simulation = True + self.has_ros = False self.ros_handler = None def _update_control(self): - """更新机器人控制指令（融合键盘/ROS输入）""" - # 1. 从CPG生成步态关节指令（修复原代码中cpg.generate_gait不存在的问题） - # 这里使用基础的CPG输出作为关节目标（实际需根据模型关节结构调整） - dt = 0.005 # 与仿真步长一致 + dt = 0.005 cpg_output = self.cpg.update(dt, speed_factor=self.walk_speed, turn_factor=self.turn_rate) - - # 示例关节映射（需根据实际模型关节名称调整） + joint_targets = { "left_hip": cpg_output, "right_hip": -cpg_output, "left_knee": -cpg_output * 0.8, "right_knee": cpg_output * 0.8 } - - # 2. 设置关节控制指令 + for joint_name, target_pos in joint_targets.items(): try: joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) self.data.ctrl[joint_id] = target_pos except: continue - - # 3. 限制控制指令范围 + self.data.ctrl = clip_value(self.data.ctrl, -1.5, 1.5) def set_gait_mode(self, mode): - """设置步态模式（扩展支持更多模式）""" valid_modes = ["NORMAL", "SLOW", "FAST", "TROT", "STEP_IN_PLACE"] self.gait_mode = mode if mode in valid_modes else "NORMAL" - # 根据步态模式调整CPG参数 gait_params = { "SLOW": {"freq": 0.3, "amp": 0.3}, "NORMAL": {"freq": 0.5, "amp": 0.4}, @@ -78,107 +144,76 @@ def set_gait_mode(self, mode): self.cpg.base_amp = gait_params[self.gait_mode]["amp"] def set_velocity(self, v): - """设置前进速度（-1.0 ~ 1.0）""" self.velocity = clip_value(v, -1.0, 1.0, "速度") def set_turn_rate(self, tr): - """设置转向速率（-1.0 ~ 1.0）""" self.turn_rate = clip_value(tr, -1.0, 1.0, "转向速率") def set_turn_angle(self, ta): - """设置转向角度""" - self.turn_angle = clip_value(ta, -np.pi/4, np.pi/4, "转向角度") + self.turn_angle = clip_value(ta, -np.pi / 4, np.pi / 4, "转向角度") def set_walk_speed(self, ws): - """设置行走速度""" self.walk_speed = clip_value(ws, 0.1, 1.0, "行走速度") def set_state(self, state): - """设置机器人状态""" valid_states = ["STAND", "WALK", "STOP", "EMERGENCY"] self.state = state if state in valid_states else "STAND" if self.state == "EMERGENCY": - self.data.ctrl[:] = 0 # 紧急停止时归零控制 + self.data.ctrl[:] = 0 def print_sensor_data(self): - """打印传感器数据""" self.sensor.print_sensor_data() def simulate(self): - """主仿真循环：控制+可视化""" - # 启动键盘监听线程 keyboard_handler = KeyboardInputHandler(self) keyboard_handler.start() - # ========== 兼容新旧Mujoco版本的Viewer启动 ========== try: - # 新版Mujoco（2.3.0+） import mujoco.viewer viewer = mujoco.viewer.launch_passive(self.model, self.data) use_new_viewer = True - viewer_running = True # 新增状态标记 - except (ImportError, AttributeError): - # 旧版Mujoco（2.1.0-）或mujoco-py + except: import mujoco.glfw as glfw glfw.init() window = glfw.create_window(1280, 720, "Humanoid Simulation", None, None) glfw.make_context_current(window) - glfw.swap_interval(1) viewer = mujoco.MjViewer(window) viewer.set_model(self.model) use_new_viewer = False - # 主仿真循环 print("仿真启动！按H查看控制帮助") while True: - # 退出条件（修复新版Viewer判断） if use_new_viewer: - try: - viewer.sync() # 尝试同步，失败则表示窗口已关闭 - except: - viewer_running = False - if not viewer_running: + if not viewer.is_running(): break + viewer.sync() else: if glfw.window_should_close(window): break - # 状态机控制 if self.state == "STOP": self.velocity = 0 self.turn_rate = 0 - # 2. 更新控制指令（融合ROS/键盘） self._update_control() - - # 3. 更新传感器数据 self.sensor.get_sensor_data(self.gait_mode) - - # 4. 执行一步仿真 mujoco.mj_step(self.model, self.data) - # 5. 更新可视化 if not use_new_viewer: viewer.render() glfw.swap_buffers(window) glfw.poll_events() - # 6. 控制仿真频率（200Hz） time.sleep(0.005) - # 清理资源 - keyboard_handler.running = False # 停止键盘线程 + keyboard_handler.running = False keyboard_handler.join() - if not use_new_viewer: - glfw.destroy_window(window) - glfw.terminate() print("仿真结束！") -# 测试代码（单独运行时） if __name__ == "__main__": - model_path = os.path.join(SCRIPT_DIR, "../models/humanoid.xml") - # 检查备用路径 + parent_dir = os.path.dirname(SCRIPT_DIR) + model_path = os.path.join(parent_dir, "models", "humanoid.xml") if not os.path.exists(model_path): - model_path = os.path.join(SCRIPT_DIR, "models/humanoid.xml") + model_path = os.path.join(SCRIPT_DIR, "models", "humanoid.xml") stabilizer = HumanoidStabilizer(model_path) - stabilizer.simulate() + stabilizer.simulate() \ No newline at end of file diff --git a/src/Mujoco_manrun/scripts/keyboard_handler.py b/src/Mujoco_manrun/scripts/keyboard_handler.py index 55b8b57d7..c32daca5e 100644 --- a/src/Mujoco_manrun/scripts/keyboard_handler.py +++ b/src/Mujoco_manrun/scripts/keyboard_handler.py @@ -82,4 +82,4 @@ def _handle_key(self, key): elif key in ['1', '2', '3', '4']: gait_map = {'1': 'SLOW', '2': 'NORMAL', '3': 'TROT', '4': 'STEP_IN_PLACE'} self.stabilizer.set_gait_mode(gait_map[key]) - print(f"[指令] 切换为{self.stabilizer.gait_mode}模式") + print(f"[指令] 切换为{self.stabilizer.gait_mode}模式") \ No newline at end of file diff --git a/src/Mujoco_manrun/scripts/utils.py b/src/Mujoco_manrun/scripts/utils.py index cb979f840..c6ea93546 100644 --- a/src/Mujoco_manrun/scripts/utils.py +++ b/src/Mujoco_manrun/scripts/utils.py @@ -21,9 +21,11 @@ def normalize_angle(angle): angle = np.mod(angle + np.pi, 2 * np.pi) - np.pi return angle -def clip_value(value, min_val, max_val, name="value"): - """带日志的数值限幅""" +def clip_value(value, min_val, max_val, name=""): + # 完美兼容单个数字 和 整个数组 clipped = np.clip(value, min_val, max_val) - if clipped != value: - print(f"[警告] {name}超出范围[{min_val}, {max_val}]，已限幅为{clipped}") + # 只有单个值才打印警告，数组直接跳过判断 + if np.isscalar(value): + if clipped != value: + print(f"警告：{name} 超出范围，已限制到 [{min_val}, {max_val}]") return clipped