From 95e6753c1fd126177ff006f4012ebb81313aaa97 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Sat, 20 Dec 2025 21:44:17 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96ros=E8=8A=82?= =?UTF-8?q?=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/CMakeLists.txt | 82 ++++++++ src/Mujoco_manrun/models/humanoid.xml | 267 ++++++++++++++++++++++++++ src/Mujoco_manrun/package.xml | 19 ++ src/Mujoco_manrun/scripts/__init__.py | 1 + 4 files changed, 369 insertions(+) create mode 100644 src/Mujoco_manrun/CMakeLists.txt create mode 100644 src/Mujoco_manrun/models/humanoid.xml create mode 100644 src/Mujoco_manrun/package.xml create mode 100644 src/Mujoco_manrun/scripts/__init__.py diff --git a/src/Mujoco_manrun/CMakeLists.txt b/src/Mujoco_manrun/CMakeLists.txt new file mode 100644 index 0000000000..ca69c45f0b --- /dev/null +++ b/src/Mujoco_manrun/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.0.2) +project(mujoco_manrun) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + geometry_msgs +) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES mujoco_manrun +# CATKIN_DEPENDS rospy geometry_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/main.py + scripts/ros_handler.py + scripts/keyboard_handler.py + scripts/cpg_oscillator.py + scripts/humanoid_stabilizer.py + scripts/utils.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY launch models + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mujoco_manrun.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/Mujoco_manrun/models/humanoid.xml b/src/Mujoco_manrun/models/humanoid.xml new file mode 100644 index 0000000000..a2c4a16fb5 --- /dev/null +++ b/src/Mujoco_manrun/models/humanoid.xml @@ -0,0 +1,267 @@ + + + + diff --git a/src/Mujoco_manrun/package.xml b/src/Mujoco_manrun/package.xml new file mode 100644 index 0000000000..65847b2ac2 --- /dev/null +++ b/src/Mujoco_manrun/package.xml @@ -0,0 +1,19 @@ + + + mujoco_manrun + 0.0.0 + The mujoco_manrun package + + yskk + TODO + + catkin + rospy + geometry_msgs + rospy + geometry_msgs + + + catkin + + diff --git a/src/Mujoco_manrun/scripts/__init__.py b/src/Mujoco_manrun/scripts/__init__.py new file mode 100644 index 0000000000..8fe5934ab3 --- /dev/null +++ b/src/Mujoco_manrun/scripts/__init__.py @@ -0,0 +1 @@ +#占位,让scripts成为合法的Python包 \ No newline at end of file From 3fa348c66c87e3112f4187723e46275ed241748d Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Sun, 21 Dec 2025 15:14:12 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=B7=A5=E5=85=B7?= =?UTF-8?q?=E5=87=BD=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/scripts/cpg_oscillator.py | 50 +++++++++++++++++++++ src/Mujoco_manrun/scripts/utils.py | 29 ++++++++++++ 2 files changed, 79 insertions(+) create mode 100644 src/Mujoco_manrun/scripts/cpg_oscillator.py create mode 100644 src/Mujoco_manrun/scripts/utils.py diff --git a/src/Mujoco_manrun/scripts/cpg_oscillator.py b/src/Mujoco_manrun/scripts/cpg_oscillator.py new file mode 100644 index 0000000000..d4c594bada --- /dev/null +++ b/src/Mujoco_manrun/scripts/cpg_oscillator.py @@ -0,0 +1,50 @@ +import numpy as np + +class CPGOscillator: + """CPG中枢模式发生器(独立算法,无业务依赖)""" + def __init__(self, freq=0.5, amp=0.4, phase=0.0, coupling_strength=0.2): + self.base_freq = freq # 基础频率 + self.base_amp = amp # 基础振幅 + self.base_phase = phase # 初始相位 + self.base_coupling = coupling_strength # 基础耦合强度 + + # 动态参数 + self.freq = freq + self.amp = amp + self.phase = phase + self.coupling = coupling_strength + self.state = np.array([np.sin(phase), np.cos(phase)]) # 振荡器状态(x,y) + + def update(self, dt, target_phase=0.0, speed_factor=1.0, turn_factor=0.0): + """ + 更新CPG状态 + :param dt: 时间步长 + :param target_phase: 目标相位(用于耦合) + :param speed_factor: 速度系数(0~1) + :param turn_factor: 转向系数(-1~1) + :return: 关节目标偏移量 + """ + # 动态调整耦合强度:速度/转向越大,耦合越强 + self.coupling = self.base_coupling * (1.0 + 0.5 * speed_factor + 0.8 * abs(turn_factor)) + self.coupling = np.clip(self.coupling, 0.1, 0.5) + + # 范德波尔振荡器方程(生物节律更自然) + mu = 1.0 # 非线性系数 + x, y = self.state + dx = 2 * np.pi * self.freq * y + self.coupling * np.sin(target_phase - self.phase) + dy = 2 * np.pi * self.freq * (mu * (1 - x**2) * y - x) + + # 积分更新状态 + self.state += np.array([dx, dy]) * dt + self.phase = np.arctan2(self.state[0], self.state[1]) # 更新相位 + + # 返回当前输出 + return self.amp * self.state[0] + + def reset(self): + """重置CPG到初始状态""" + self.freq = self.base_freq + self.amp = self.base_amp + self.coupling = self.base_coupling + self.phase = self.base_phase + self.state = np.array([np.sin(self.phase), np.cos(self.phase)]) diff --git a/src/Mujoco_manrun/scripts/utils.py b/src/Mujoco_manrun/scripts/utils.py new file mode 100644 index 0000000000..cb979f8409 --- /dev/null +++ b/src/Mujoco_manrun/scripts/utils.py @@ -0,0 +1,29 @@ +import numpy as np + +def quat_to_euler_xyz(quat): + """四元数转欧拉角(roll/pitch/yaw)""" + w, x, y, z = quat + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (w * y - z * x) + pitch = np.where(np.abs(sinp) >= 1, np.copysign(np.pi / 2, sinp), np.arcsin(sinp)) + + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return np.array([roll, pitch, yaw]) + +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"): + """带日志的数值限幅""" + clipped = np.clip(value, min_val, max_val) + if clipped != value: + print(f"[警告] {name}超出范围[{min_val}, {max_val}],已限幅为{clipped}") + return clipped From 089b67e161c4251f0c0b121cca1ed9adfcb012f0 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Sun, 21 Dec 2025 20:44:16 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E4=BA=86=E5=89=A9?= =?UTF-8?q?=E4=BD=99=E7=9A=84=E4=BB=A3=E7=A0=81=E6=8B=86=E5=88=86=EF=BC=8C?= =?UTF-8?q?=E4=BF=9D=E7=95=99main.py=E4=BD=9C=E4=B8=BA=E5=85=A5=E5=8F=A3?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/config/gait_params.yaml | 37 ++++ src/Mujoco_manrun/launch/mujoco_manrun.launch | 4 + .../scripts/humanoid_stabilizer.py | 184 ++++++++++++++++++ src/Mujoco_manrun/scripts/keyboard_handler.py | 85 ++++++++ src/Mujoco_manrun/scripts/main.py | 45 +++++ src/Mujoco_manrun/scripts/ros_handler.py | 78 ++++++++ src/Mujoco_manrun/scripts/sensor_simulator.py | 153 +++++++++++++++ src/Mujoco_manrun/scripts/setup.py | 10 + src/Mujoco_manrun/setup.py | 12 ++ 9 files changed, 608 insertions(+) create mode 100644 src/Mujoco_manrun/config/gait_params.yaml create mode 100644 src/Mujoco_manrun/launch/mujoco_manrun.launch create mode 100644 src/Mujoco_manrun/scripts/humanoid_stabilizer.py create mode 100644 src/Mujoco_manrun/scripts/keyboard_handler.py create mode 100644 src/Mujoco_manrun/scripts/main.py create mode 100644 src/Mujoco_manrun/scripts/ros_handler.py create mode 100644 src/Mujoco_manrun/scripts/sensor_simulator.py create mode 100644 src/Mujoco_manrun/scripts/setup.py create mode 100644 src/Mujoco_manrun/setup.py diff --git a/src/Mujoco_manrun/config/gait_params.yaml b/src/Mujoco_manrun/config/gait_params.yaml new file mode 100644 index 0000000000..f5107c5997 --- /dev/null +++ b/src/Mujoco_manrun/config/gait_params.yaml @@ -0,0 +1,37 @@ +gait_config: + SLOW: + freq: 0.3 + amp: 0.3 + coupling: 0.3 + speed_freq_gain: 0.2 + speed_amp_gain: 0.1 + com_z_offset: 0.02 + NORMAL: + freq: 0.5 + amp: 0.4 + coupling: 0.2 + speed_freq_gain: 0.4 + speed_amp_gain: 0.2 + com_z_offset: 0.0 + TROT: + freq: 0.8 + amp: 0.5 + coupling: 0.25 + speed_freq_gain: 0.5 + speed_amp_gain: 0.3 + com_z_offset: -0.01 + STEP_IN_PLACE: + freq: 0.4 + amp: 0.2 + coupling: 0.3 + speed_freq_gain: 0.0 + speed_amp_gain: 0.0 + com_z_offset: 0.01 + lock_torso: true + +sim_params: + init_wait_time: 4.0 + sim_duration: 120.0 + foot_contact_threshold: 1.5 + com_safety_threshold: 0.6 + speed_reduction_factor: 0.5 diff --git a/src/Mujoco_manrun/launch/mujoco_manrun.launch b/src/Mujoco_manrun/launch/mujoco_manrun.launch new file mode 100644 index 0000000000..f141c4abd7 --- /dev/null +++ b/src/Mujoco_manrun/launch/mujoco_manrun.launch @@ -0,0 +1,4 @@ + + + + diff --git a/src/Mujoco_manrun/scripts/humanoid_stabilizer.py b/src/Mujoco_manrun/scripts/humanoid_stabilizer.py new file mode 100644 index 0000000000..3c07b1991a --- /dev/null +++ b/src/Mujoco_manrun/scripts/humanoid_stabilizer.py @@ -0,0 +1,184 @@ +import sys +import os +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, SCRIPT_DIR) + +import mujoco +import numpy as np +import time +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 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.has_ros = False + self.ros_handler = None + + def _update_control(self): + """更新机器人控制指令(融合键盘/ROS输入)""" + # 1. 从CPG生成步态关节指令(修复原代码中cpg.generate_gait不存在的问题) + # 这里使用基础的CPG输出作为关节目标(实际需根据模型关节结构调整) + 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}, + "FAST": {"freq": 0.7, "amp": 0.5}, + "TROT": {"freq": 0.8, "amp": 0.6}, + "STEP_IN_PLACE": {"freq": 0.4, "amp": 0.3} + } + self.cpg.base_freq = gait_params[self.gait_mode]["freq"] + 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, "转向角度") + + 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 # 紧急停止时归零控制 + + 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 + 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: + break + 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.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") + # 检查备用路径 + if not os.path.exists(model_path): + model_path = os.path.join(SCRIPT_DIR, "models/humanoid.xml") + stabilizer = HumanoidStabilizer(model_path) + stabilizer.simulate() diff --git a/src/Mujoco_manrun/scripts/keyboard_handler.py b/src/Mujoco_manrun/scripts/keyboard_handler.py new file mode 100644 index 0000000000..55b8b57d78 --- /dev/null +++ b/src/Mujoco_manrun/scripts/keyboard_handler.py @@ -0,0 +1,85 @@ +import threading +import time +import sys +import numpy as np + +class KeyboardInputHandler(threading.Thread): + """键盘输入监听线程(独立模块,仅处理按键指令)""" + def __init__(self, stabilizer): + super().__init__(daemon=True) + self.stabilizer = stabilizer + self.running = True + + def run(self): + """打印控制说明并启动监听""" + self._print_help() + while self.running: + try: + key = self._get_key_non_blocking() + if key: + self._handle_key(key) + time.sleep(0.01) + except: + continue + + def _print_help(self): + """打印控制指令说明""" + print("\n===== 控制指令说明 =====") + print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") + print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") + print("m: 传感器模拟开关 | p: 打印传感器数据") + print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步") + print("========================\n") + + def _get_key_non_blocking(self): + """非阻塞获取键盘输入(跨平台)""" + if sys.platform == "win32": + import msvcrt + if msvcrt.kbhit(): + return msvcrt.getch().decode('utf-8').lower() + else: + import select + if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: + return sys.stdin.read(1).lower() + return None + + 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} | 速度: {self.stabilizer.walk_speed:.2f}") + 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 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}模式") diff --git a/src/Mujoco_manrun/scripts/main.py b/src/Mujoco_manrun/scripts/main.py new file mode 100644 index 0000000000..b07d87bc98 --- /dev/null +++ b/src/Mujoco_manrun/scripts/main.py @@ -0,0 +1,45 @@ +import sys +import os +# 强制添加源码目录到Python路径(解决ROS编译后导入问题) +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, SCRIPT_DIR) + +def main(): + """主函数:初始化+启动仿真""" + # 1. 检查模型路径 + model_path = os.path.join(SCRIPT_DIR, "../models/humanoid.xml") + if not os.path.exists(model_path): + # 备用路径(ROS编译后) + model_path = os.path.join(SCRIPT_DIR, "models/humanoid.xml") + if not os.path.exists(model_path): + print(f"错误:找不到模型文件 {model_path}") + sys.exit(1) + print(f"加载模型:{model_path}") + + # 2. 导入核心模块(修复后) + try: + from humanoid_stabilizer import HumanoidStabilizer + from ros_handler import ROSHandler + except ImportError as e: + print(f"导入模块失败:{e}") + sys.exit(1) + + # 3. 初始化机器人控制器 + stabilizer = HumanoidStabilizer(model_path) + + # 4. 启动ROS监听线程 + ros_handler = ROSHandler(stabilizer) + ros_handler.start() + + # 5. 启动仿真 + try: + stabilizer.simulate() + except KeyboardInterrupt: + print("用户中断仿真") + finally: + # 停止ROS线程 + ros_handler.stop() + ros_handler.join() + +if __name__ == "__main__": + main() diff --git a/src/Mujoco_manrun/scripts/ros_handler.py b/src/Mujoco_manrun/scripts/ros_handler.py new file mode 100644 index 0000000000..7a2c3e7b62 --- /dev/null +++ b/src/Mujoco_manrun/scripts/ros_handler.py @@ -0,0 +1,78 @@ +import sys +import os +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, SCRIPT_DIR) + +import threading +import time + +class ROSHandler(threading.Thread): + def __init__(self, stabilizer): + super().__init__(daemon=True) + self.stabilizer = stabilizer + self.running = True + self.has_ros = False + self.rospy = None + self.Twist = None + + # 尝试初始化ROS + try: + import rospy + from geometry_msgs.msg import Twist + self.rospy = rospy + self.Twist = Twist + self.has_ros = True + + # 初始化ROS节点 + if not self.rospy.core.is_initialized(): + self.rospy.init_node('humanoid_cmd_vel_listener', anonymous=True) + + # 订阅/cmd_vel话题 + self.sub = self.rospy.Subscriber( + "/cmd_vel", + self.Twist, + self._cmd_vel_callback, + queue_size=1, + tcp_nodelay=True + ) + print("[ROS提示] 已启动/cmd_vel话题监听") + except ImportError: + print("[ROS提示] 未检测到ROS环境,跳过/cmd_vel话题监听") + except Exception as e: + print(f"[ROS提示] ROS初始化失败:{e}") + self.has_ros = False + + def _cmd_vel_callback(self, msg): + """处理/cmd_vel话题消息""" + # 提取速度和转向指令 + linear_x = msg.linear.x # 前进/后退(-1~1) + angular_z = msg.angular.z # 左转/右转(-1~1) + + # 设置到机器人控制器 + self.stabilizer.set_velocity(linear_x) + self.stabilizer.set_turn_rate(angular_z) + + # 打印日志 + gait_mode = self.stabilizer.gait_mode + print(f"[ROS指令] 速度={linear_x:.2f} | 转向={angular_z:.2f} | 步态={gait_mode}") + + def run(self): + """ROS监听线程主循环(修复spin_once错误)""" + if not self.has_ros: + return + + # ROS 1兼容的周期监听(替代spin_once) + rate = self.rospy.Rate(100) # 100Hz刷新频率 + while self.running and not self.rospy.is_shutdown(): + try: + rate.sleep() # 非阻塞式等待,替代spin_once + except Exception as e: + print(f"[ROS提示] 监听线程异常:{e}") + break + + def stop(self): + """停止监听线程""" + self.running = False + if self.has_ros: + self.rospy.signal_shutdown("Simulation stopped") + print("[ROS提示] 监听线程已停止") diff --git a/src/Mujoco_manrun/scripts/sensor_simulator.py b/src/Mujoco_manrun/scripts/sensor_simulator.py new file mode 100644 index 0000000000..667c83e99d --- /dev/null +++ b/src/Mujoco_manrun/scripts/sensor_simulator.py @@ -0,0 +1,153 @@ +import numpy as np +from collections import deque +from utils import quat_to_euler_xyz + +class SensorSimulator: + """传感器模拟模块(IMU+足底力,独立可复用)""" + def __init__(self, model, data): + self.model = model + self.data = data + + # 开关 + self.enable_sensor_simulation = True + + # IMU参数 + self.imu_angle_noise = 0.01 # 欧拉角噪声(rad) + self.imu_vel_noise = 0.05 # 角速度噪声(rad/s) + self.imu_delay_frames = 2 # 延迟帧数 + + # 足底力参数 + self.foot_force_noise = 0.3 # 力噪声(N) + self.foot_force_offset = 0.1 # 零漂 + self.foot_contact_threshold = 1.5 # 接触判定阈值 + + # 缓存(模拟延迟) + self.imu_data_buffer = deque(maxlen=self.imu_delay_frames) + self.foot_data_buffer = deque(maxlen=self.imu_delay_frames) + + # 存储当前数据 + self.current_sensor_data = {} + + def simulate_imu(self): + """模拟带噪声+延迟的IMU数据""" + # 真实数据 + true_quat = self.data.qpos[3:7].astype(np.float64).copy() + true_euler = quat_to_euler_xyz(true_quat) + true_ang_vel = self.data.qvel[3:6].astype(np.float64).copy() + + # 添加噪声 + noisy_euler = true_euler + np.random.normal(0, self.imu_angle_noise, 3) + noisy_ang_vel = true_ang_vel + np.random.normal(0, self.imu_vel_noise, 3) + + # 限幅 + noisy_euler = np.clip(noisy_euler, -np.pi/2, np.pi/2) + noisy_ang_vel = np.clip(noisy_ang_vel, -5.0, 5.0) + + # 缓存 + self.imu_data_buffer.append({ + "euler": noisy_euler, + "ang_vel": noisy_ang_vel, + "true_euler": true_euler, + "true_ang_vel": true_ang_vel + }) + + # 返回延迟后的数据 + if len(self.imu_data_buffer) < self.imu_delay_frames: + return {"euler": true_euler, "ang_vel": true_ang_vel, "true_euler": true_euler, "true_ang_vel": true_ang_vel} + return self.imu_data_buffer[0] + + def simulate_foot_force(self): + """模拟带噪声+零漂的足底力数据""" + # 真实接触力 + left_force = self._get_foot_force("left") + right_force = self._get_foot_force("right") + + # 添加噪声和零漂 + noisy_left = left_force + np.random.normal(0, self.foot_force_noise) + self.foot_force_offset + noisy_right = right_force + np.random.normal(0, self.foot_force_noise) + self.foot_force_offset + + # 限幅 + noisy_left = max(0.0, noisy_left) + noisy_right = max(0.0, noisy_right) + + # 接触判定 + left_contact = 1 if noisy_left > self.foot_contact_threshold else 0 + right_contact = 1 if noisy_right > self.foot_contact_threshold else 0 + + # 缓存 + self.foot_data_buffer.append({ + "left_force": noisy_left, + "right_force": noisy_right, + "left_contact": left_contact, + "right_contact": right_contact, + "true_left_force": left_force, + "true_right_force": right_force + }) + + # 返回延迟后的数据 + if len(self.foot_data_buffer) < self.imu_delay_frames: + return { + "left_force": left_force, "right_force": right_force, + "left_contact": 1 if left_force > self.foot_contact_threshold else 0, + "right_contact": 1 if right_force > self.foot_contact_threshold else 0, + "true_left_force": left_force, "true_right_force": right_force + } + return self.foot_data_buffer[0] + + def _get_foot_force(self, side): + """获取单只脚的真实接触力""" + import mujoco + foot_geoms = ["foot1_" + side, "foot2_" + side] + total_force = 0.0 + for geom_name in foot_geoms: + geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) + force = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(self.model, self.data, geom_id, force) + total_force += np.linalg.norm(force[:3]) + return total_force + + def get_sensor_data(self, gait_mode="NORMAL"): + """获取最终传感器数据(模拟/真实可切换)""" + if not self.enable_sensor_simulation: + # 关闭模拟:返回真实数据 + imu_data = { + "euler": quat_to_euler_xyz(self.data.qpos[3:7]), + "ang_vel": self.data.qvel[3:6].copy(), + "true_euler": quat_to_euler_xyz(self.data.qpos[3:7]), + "true_ang_vel": self.data.qvel[3:6].copy() + } + foot_data = { + "left_force": self._get_foot_force("left"), + "right_force": self._get_foot_force("right"), + "left_contact": 1 if self._get_foot_force("left") > self.foot_contact_threshold else 0, + "right_contact": 1 if self._get_foot_force("right") > self.foot_contact_threshold else 0, + "true_left_force": self._get_foot_force("left"), + "true_right_force": self._get_foot_force("right") + } + else: + # 开启模拟:返回带噪声+延迟的数据 + imu_data = self.simulate_imu() + foot_data = self.simulate_foot_force() + + # 整合数据 + self.current_sensor_data = { + "imu": imu_data, + "foot": foot_data, + "time": self.data.time, + "gait_mode": gait_mode + } + return self.current_sensor_data + + def print_sensor_data(self): + """打印当前传感器数据""" + if not self.current_sensor_data: + print("[传感器] 暂无数据") + return + imu = self.current_sensor_data["imu"] + foot = self.current_sensor_data["foot"] + print("\n=== 传感器数据 ===") + print(f"时间: {self.current_sensor_data['time']:.2f}s | 模拟: {'开启' if self.enable_sensor_simulation else '关闭'} | 步态: {self.current_sensor_data['gait_mode']}") + print(f"IMU姿态: {imu['euler'][0]:.3f}/{imu['euler'][1]:.3f}/{imu['euler'][2]:.3f}rad (真实: {imu['true_euler'][0]:.3f}/{imu['true_euler'][1]:.3f}/{imu['true_euler'][2]:.3f})") + print(f"左脚力: {foot['left_force']:.2f}N (真实: {foot['true_left_force']:.2f}N) | 接触: {foot['left_contact']}") + print(f"右脚力: {foot['right_force']:.2f}N (真实: {foot['true_right_force']:.2f}N) | 接触: {foot['right_contact']}") + print("==================\n") diff --git a/src/Mujoco_manrun/scripts/setup.py b/src/Mujoco_manrun/scripts/setup.py new file mode 100644 index 0000000000..06c125f317 --- /dev/null +++ b/src/Mujoco_manrun/scripts/setup.py @@ -0,0 +1,10 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# 获取ROS包配置 +d = generate_distutils_setup( + packages=['mujoco_manrun'], # 包名 + package_dir={'': 'scripts'} # 源码目录 +) + +setup(**d) diff --git a/src/Mujoco_manrun/setup.py b/src/Mujoco_manrun/setup.py new file mode 100644 index 0000000000..6f5630e1a7 --- /dev/null +++ b/src/Mujoco_manrun/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['mujoco_manrun'], + package_dir={'': 'scripts'}, +) + +setup(**setup_args) From af1d613d3a12a0276a473039d4c6922abddf04f5 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Mon, 22 Dec 2025 09:43:47 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E5=AE=8C=E6=88=90ros=E5=8C=85=EF=BC=8C?= =?UTF-8?q?=E8=A7=84=E8=8C=83=E4=BA=86=E6=96=87=E4=BB=B6=E7=9A=84=E4=BD=8D?= =?UTF-8?q?=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/CMakeLists.txt | 94 +++--- src/Mujoco_manrun/humanoid.xml | 267 ------------------ src/Mujoco_manrun/launch/mujoco_manrun.launch | 13 +- src/Mujoco_manrun/{ => models}/g1_23dof.xml | 0 .../{ => models}/mimic_sites.xml | 0 src/Mujoco_manrun/{ => models}/scene.xml | 0 src/Mujoco_manrun/package.xml | 22 +- src/Mujoco_manrun/setup.py | 10 +- 8 files changed, 70 insertions(+), 336 deletions(-) delete mode 100644 src/Mujoco_manrun/humanoid.xml rename src/Mujoco_manrun/{ => models}/g1_23dof.xml (100%) rename src/Mujoco_manrun/{ => models}/mimic_sites.xml (100%) rename src/Mujoco_manrun/{ => models}/scene.xml (100%) diff --git a/src/Mujoco_manrun/CMakeLists.txt b/src/Mujoco_manrun/CMakeLists.txt index ca69c45f0b..3defea3a09 100644 --- a/src/Mujoco_manrun/CMakeLists.txt +++ b/src/Mujoco_manrun/CMakeLists.txt @@ -1,82 +1,62 @@ cmake_minimum_required(VERSION 3.0.2) project(mujoco_manrun) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if($ENV{ROS_DISTRO} STREQUAL "noetic") + set(PYTHON_EXECUTABLE /usr/bin/python3) +else() + set(PYTHON_EXECUTABLE /usr/bin/python2) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - rospy geometry_msgs + rospy + sensor_msgs + std_msgs ) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES mujoco_manrun -# CATKIN_DEPENDS rospy geometry_msgs -# DEPENDS system_lib + CATKIN_DEPENDS + geometry_msgs + rospy + sensor_msgs + std_msgs ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( -# include ${catkin_INCLUDE_DIRS} ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +install(DIRECTORY scripts/ + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + FILES_MATCHING PATTERN "*.py" + PATTERN "__pycache__" EXCLUDE +) -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -catkin_install_python(PROGRAMS +catkin_install_python(PROGRAMS scripts/main.py - scripts/ros_handler.py - scripts/keyboard_handler.py - scripts/cpg_oscillator.py - scripts/humanoid_stabilizer.py - scripts/utils.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch models - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY models/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models ) -############# -## Testing ## -############# +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mujoco_manrun.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +if(CATKIN_ENABLE_TESTING) + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/test/") + install(DIRECTORY test/ + DESTINATION ${CATKIN_PACKAGE_TEST_DESTINATION} + FILES_MATCHING PATTERN "*.py" + ) + catkin_add_nosetests(test) + endif() +endif() diff --git a/src/Mujoco_manrun/humanoid.xml b/src/Mujoco_manrun/humanoid.xml deleted file mode 100644 index a2c4a16fb5..0000000000 --- a/src/Mujoco_manrun/humanoid.xml +++ /dev/null @@ -1,267 +0,0 @@ - - - - diff --git a/src/Mujoco_manrun/launch/mujoco_manrun.launch b/src/Mujoco_manrun/launch/mujoco_manrun.launch index f141c4abd7..1ba87cd3aa 100644 --- a/src/Mujoco_manrun/launch/mujoco_manrun.launch +++ b/src/Mujoco_manrun/launch/mujoco_manrun.launch @@ -1,4 +1,15 @@ - + + + + + diff --git a/src/Mujoco_manrun/g1_23dof.xml b/src/Mujoco_manrun/models/g1_23dof.xml similarity index 100% rename from src/Mujoco_manrun/g1_23dof.xml rename to src/Mujoco_manrun/models/g1_23dof.xml diff --git a/src/Mujoco_manrun/mimic_sites.xml b/src/Mujoco_manrun/models/mimic_sites.xml similarity index 100% rename from src/Mujoco_manrun/mimic_sites.xml rename to src/Mujoco_manrun/models/mimic_sites.xml diff --git a/src/Mujoco_manrun/scene.xml b/src/Mujoco_manrun/models/scene.xml similarity index 100% rename from src/Mujoco_manrun/scene.xml rename to src/Mujoco_manrun/models/scene.xml diff --git a/src/Mujoco_manrun/package.xml b/src/Mujoco_manrun/package.xml index 65847b2ac2..93cbfc91c3 100644 --- a/src/Mujoco_manrun/package.xml +++ b/src/Mujoco_manrun/package.xml @@ -1,18 +1,30 @@ mujoco_manrun - 0.0.0 - The mujoco_manrun package - - yskk - TODO + 1.0.0 + ROS封装的人形机器人Mujoco仿真包,支持CPG步态控制、标准ROS接口通信 + your_name + MIT + catkin rospy geometry_msgs + sensor_msgs + std_msgs + tf2_ros + std_srvs + + rospy geometry_msgs + sensor_msgs + std_msgs + tf2_ros + std_srvs + rviz + catkin diff --git a/src/Mujoco_manrun/setup.py b/src/Mujoco_manrun/setup.py index 6f5630e1a7..a027a9fdc9 100644 --- a/src/Mujoco_manrun/setup.py +++ b/src/Mujoco_manrun/setup.py @@ -1,12 +1,10 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup +from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup -# fetch values from package.xml setup_args = generate_distutils_setup( - packages=['mujoco_manrun'], - package_dir={'': 'scripts'}, + packages=[], + package_dir={"": "scripts"}, + scripts=["scripts/main.py"] ) setup(**setup_args)