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)