diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 036eef72d1..346c1b93b0 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -5,11 +5,7 @@ import os import sys import threading -import select # 新增:跨平台输入兼容 -import queue # 新增:输入队列,避免阻塞 - -# ===================== 全局输入队列(解决Windows输入阻塞) ===================== -input_queue = queue.Queue() +from collections import deque # ===================== ROS话题接收模块(原有逻辑完全保留) ===================== @@ -40,8 +36,7 @@ def __init__(self, stabilizer): self.rospy.init_node('humanoid_cmd_vel_listener', anonymous=True) # 订阅/cmd_vel话题(队列大小1,避免延迟) self.sub = self.rospy.Subscriber( - "/cmd_vel", self.Twist, self._cmd_vel_callback, - queue_size=1, tcp_nodelay=True + "/cmd_vel", self.Twist, self._cmd_vel_callback, queue_size=1, tcp_nodelay=True ) print("[ROS提示] 已启动/cmd_vel话题监听:") print(" - linear.x (0.1~1.0) → 行走速度(0.1=最慢,1.0=最快)") @@ -76,59 +71,40 @@ def run(self): return while self.running and not self.rospy.is_shutdown(): self.rospy.spin_once() - time.sleep(0.001) # 缩短sleep,适配Windows线程 + time.sleep(0.01) def stop(self): self.running = False -# ===================== 键盘输入监听线程(修复Windows兼容) ===================== +# ===================== 键盘输入监听线程(原有逻辑完全保留) ===================== class KeyboardInputHandler(threading.Thread): def __init__(self, stabilizer): super().__init__(daemon=True) self.stabilizer = stabilizer self.running = True - self.is_windows = sys.platform == "win32" def run(self): print("\n===== 控制指令说明 =====") print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") + print("m: 传感器模拟开关 | p: 打印传感器数据") print("========================\n") - while self.running: try: - key = None - # 修复:Windows下msvcrt输入逻辑(Python3.13兼容) - if self.is_windows: + # 非阻塞键盘输入(兼容不同系统) + if sys.platform == "win32": import msvcrt if msvcrt.kbhit(): - ch = msvcrt.getch() - # 处理Python3.13编码问题:区分字节串和字符 - if isinstance(ch, bytes): - try: - key = ch.decode('utf-8', errors='ignore').lower() - except: - key = None - else: - key = ch.lower() - # 处理特殊键(如箭头键,避免乱码) - if key in ['\x00', '\xe0']: - msvcrt.getch() # 跳过扩展键 - key = None - # Linux/Mac输入逻辑(保留) + 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() - - if key: - input_queue.put(key) # 放入队列,避免阻塞 - self._handle_key(key) - time.sleep(0.001) # 降低CPU占用,适配Windows - except Exception as e: - # 容错:避免输入异常导致线程退出 - print(f"[键盘输入警告] {e}(忽略,继续监听)") + self._handle_key(key) time.sleep(0.01) + except: continue def _handle_key(self, key): @@ -160,9 +136,16 @@ def _handle_key(self, key): 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() -# ===================== 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) @@ -207,9 +190,9 @@ def base_phase(self): return 0.0 if self.phase < np.pi else np.pi -# ===================== 人形机器人控制器(修复Windows仿真逻辑) ===================== +# ===================== 人形机器人控制器(新增传感器模拟功能) ===================== class HumanoidStabilizer: - """适配humanoid.xml模型的稳定站立与行走控制器(新增步态鲁棒性优化)""" + """适配humanoid.xml模型的稳定站立与行走控制器(新增传感器模拟+鲁棒性优化)""" def __init__(self, model_path): # 类型检查与模型加载(原有逻辑完全保留) @@ -222,14 +205,13 @@ def __init__(self, model_path): except Exception as e: raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性") - # 仿真核心参数(修复:适配Windows时间精度) + # 仿真核心参数(原有逻辑保留) self.sim_duration = 120.0 self.dt = self.model.opt.timestep self.init_wait_time = 4.0 self.model.opt.gravity[2] = -9.81 self.model.opt.iterations = 200 self.model.opt.tolerance = 1e-8 - self.sim_sleep_scale = 0.1 # 修复:Windows下缩短sleep时间,提升响应 # 关节名称映射(原有逻辑完全保留) self.joint_names = [ @@ -308,6 +290,21 @@ def __init__(self, model_path): self.step_offset_ankle = 0.3 self.walk_start_time = None + # ===================== 新增:传感器模拟相关参数 ===================== + 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 # IMU延迟帧数(模拟硬件传输延迟) + # 足底力传感器噪声参数 + self.foot_force_noise = 0.3 # 足底力噪声标准差(N) + self.foot_force_offset = 0.1 # 足底力偏移误差(模拟零漂) + # 传感器数据缓存(用于模拟延迟) + self.imu_data_buffer = deque(maxlen=self.imu_delay_frames) + self.foot_data_buffer = deque(maxlen=self.imu_delay_frames) + # 存储当前传感器数据(用于调试打印) + self.current_sensor_data = {} + # 初始化稳定姿态(原有逻辑完全保留) self._init_stable_pose() @@ -350,6 +347,150 @@ def _init_stable_pose(self): mujoco.mj_forward(self.model, self.data) + # ===================== 新增:传感器数据模拟方法 ===================== + def _simulate_imu_data(self): + """模拟带噪声+延迟的IMU数据(欧拉角+角速度)""" + # 获取真实姿态和角速度 + true_quat = self.data.qpos[3:7].astype(np.float64).copy() + true_euler = self._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 + } + else: + return self.imu_data_buffer[0] # 返回最早的缓存数据 + + def _simulate_foot_force_data(self): + """模拟带噪声+零漂的足底力传感器数据""" + # 获取真实接触力(原有逻辑) + left_foot_geoms = ["foot1_left", "foot2_left"] + right_foot_geoms = ["foot1_right", "foot2_right"] + + true_left_force = 0.0 + for geom_name in left_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) + true_left_force += np.linalg.norm(force[:3]) + + true_right_force = 0.0 + for geom_name in right_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) + true_right_force += np.linalg.norm(force[:3]) + + # 添加噪声和零漂(模拟真实传感器) + noisy_left_force = true_left_force + np.random.normal(0, self.foot_force_noise) + self.foot_force_offset + noisy_right_force = true_right_force + np.random.normal(0, self.foot_force_noise) + self.foot_force_offset + + # 限幅(避免负力) + noisy_left_force = max(0.0, noisy_left_force) + noisy_right_force = max(0.0, noisy_right_force) + + # 接触判定(基于带噪声的力) + left_contact = 1 if noisy_left_force > self.foot_contact_threshold else 0 + right_contact = 1 if noisy_right_force > self.foot_contact_threshold else 0 + + # 加入缓存(模拟延迟) + self.foot_data_buffer.append({ + "left_force": noisy_left_force, + "right_force": noisy_right_force, + "left_contact": left_contact, + "right_contact": right_contact, + "true_left_force": true_left_force, + "true_right_force": true_right_force + }) + + # 获取延迟后的传感器数据 + if len(self.foot_data_buffer) < self.imu_delay_frames: + return { + "left_force": true_left_force, + "right_force": true_right_force, + "left_contact": 1 if true_left_force > self.foot_contact_threshold else 0, + "right_contact": 1 if true_right_force > self.foot_contact_threshold else 0, + "true_left_force": true_left_force, + "true_right_force": true_right_force + } + else: + return self.foot_data_buffer[0] + + def _get_sensor_data(self): + """获取传感器数据(模拟/真实可切换)""" + if not self.enable_sensor_simulation: + # 关闭模拟:返回原始数据(兼容原有逻辑) + imu_data = { + "euler": self._get_root_euler(), + "ang_vel": self.data.qvel[3:6].astype(np.float64).copy(), + "true_euler": self._get_root_euler(), + "true_ang_vel": self.data.qvel[3:6].astype(np.float64).copy() + } + + # 原始接触力 + self._detect_foot_contact() + foot_data = { + "left_force": self.left_foot_force, + "right_force": self.right_foot_force, + "left_contact": self.foot_contact[1], + "right_contact": self.foot_contact[0], + "true_left_force": self.left_foot_force, + "true_right_force": self.right_foot_force + } + else: + # 开启模拟:返回带噪声+延迟的传感器数据 + imu_data = self._simulate_imu_data() + foot_data = self._simulate_foot_force_data() + + # 整合传感器数据并存储(用于调试) + self.current_sensor_data = { + "imu": imu_data, + "foot": foot_data, + "time": self.data.time + } + + 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 '关闭'}") + print(f"IMU欧拉角(roll/pitch/yaw): {imu['euler'][0]:.3f}/{imu['euler'][1]:.3f}/{imu['euler'][2]:.3f}rad") + print(f"IMU真实值: {imu['true_euler'][0]:.3f}/{imu['true_euler'][1]:.3f}/{imu['true_euler'][2]:.3f}rad") + 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") + # 状态机方法(原有逻辑保留,WALK状态新增鲁棒优化) def _state_stand(self): """站立状态:维持初始稳定姿态,CPG重置""" @@ -453,14 +594,14 @@ def _quat_to_euler_xyz(self, quat): yaw = np.arctan2(siny_cosp, cosy_cosp) return np.array([roll, pitch, yaw]) - # 提取躯干欧拉角(原有逻辑完全保留) + # 提取躯干欧拉角(原有逻辑,现在仅用于传感器模拟的真实值参考) def _get_root_euler(self): quat = self.data.qpos[3:7].astype(np.float64).copy() euler = self._quat_to_euler_xyz(quat) euler = np.mod(euler + np.pi, 2 * np.pi) - np.pi return np.clip(euler, -0.3, 0.3) - # 检测脚部接触(原有逻辑保留,新增接触力强度计算) + # 原有接触检测方法(保留,用于关闭传感器模拟时的 fallback) def _detect_foot_contact(self): try: left_foot_geoms = ["foot1_left", "foot2_left"] @@ -482,8 +623,6 @@ def _detect_foot_contact(self): self.foot_contact[1] = 1 if left_force > self.foot_contact_threshold else 0 self.foot_contact[0] = 1 if right_force > self.foot_contact_threshold else 0 - - # 新增:保存接触力强度(用于动态PD增益) self.left_foot_force = left_force self.right_foot_force = right_force @@ -493,17 +632,22 @@ def _detect_foot_contact(self): self.left_foot_force = self.foot_contact_threshold self.right_foot_force = self.foot_contact_threshold - # 计算稳定力矩(核心:新增动态PD增益、接触力自适应) + # 计算稳定力矩(核心:新增基于传感器数据的反馈控制) def _calculate_stabilizing_torques(self): # 状态机驱动(原有逻辑保留) self.state_map[self.state]() - # 原始力矩计算逻辑(保留,新增鲁棒优化) + # 新增:获取传感器数据(模拟/真实可切换) + sensor_data = self._get_sensor_data() + imu = sensor_data["imu"] + foot = sensor_data["foot"] + + # 原始力矩计算逻辑(替换为传感器数据) torques = np.zeros(self.num_joints, dtype=np.float64) - # 躯干姿态控制(原有逻辑完全保留) - root_euler = self._get_root_euler() - root_vel = self.data.qvel[3:6].astype(np.float64).copy() + # 躯干姿态控制(改用传感器IMU数据) + root_euler = imu["euler"] # 带噪声的欧拉角 + root_vel = imu["ang_vel"] # 带噪声的角速度 root_vel = np.clip(root_vel, -3.0, 3.0) roll_error = -root_euler[0] @@ -528,12 +672,16 @@ def _calculate_stabilizing_torques(self): com_error = np.clip(com_error, -0.03, 0.03) com_compensation = self.kp_com * com_error - # 关节控制(原有逻辑保留,新增动态PD增益) - self._detect_foot_contact() + # 关节控制(改用传感器足底力数据) current_joints = self.data.qpos[7:7 + self.num_joints].astype(np.float64) current_vel = self.data.qvel[6:6 + self.num_joints].astype(np.float64) current_vel = np.clip(current_vel, -8.0, 8.0) + # 更新接触状态(来自传感器) + self.foot_contact = np.array([foot["right_contact"], foot["left_contact"]]) + self.left_foot_force = foot["left_force"] + self.right_foot_force = foot["right_force"] + # 腰部关节控制(原有逻辑完全保留) waist_joints = ["abdomen_z", "abdomen_y", "abdomen_x"] for joint_name in waist_joints: @@ -618,7 +766,7 @@ def _calculate_stabilizing_torques(self): idx = self.joint_name_to_idx[joint_name] torques[idx] = np.clip(torques[idx], -limit, limit) - # 调试输出(原有逻辑保留,新增鲁棒优化状态) + # 调试输出(新增传感器模拟状态) if self.data.time % 1 < 0.1 and self.data.time > self.init_wait_time: print(f"=== 行走调试 ===") print( @@ -626,12 +774,12 @@ def _calculate_stabilizing_torques(self): print(f"右腿髋目标: {self.joint_targets[self.joint_name_to_idx['hip_y_right']]:.2f}") print(f"左腿髋目标: {self.joint_targets[self.joint_name_to_idx['hip_y_left']]:.2f}") print( - f"右脚接触: {self.foot_contact[0]}, 左脚接触: {self.foot_contact[1]} | 鲁棒优化: {'开启' if self.enable_robust_optim else '关闭'}") + f"右脚接触: {self.foot_contact[0]}, 左脚接触: {self.foot_contact[1]} | 鲁棒优化: {'开启' if self.enable_robust_optim else '关闭'} | 传感器模拟: {'开启' if self.enable_sensor_simulation else '关闭'}") self.prev_com = com return torques - # 仿真循环(修复:Windows线程/时间兼容) + # 仿真循环(原有逻辑完全保留) def simulate_stable_standing(self): # 启动ROS /cmd_vel监听线程 self.ros_handler = ROSCmdVelHandler(self) @@ -648,10 +796,11 @@ def simulate_stable_standing(self): v.cam.elevation = -25 v.cam.lookat = [0, 0, 0.6] - print("人形机器人稳定站立+行走仿真启动(已启用步态鲁棒性优化)...") + print("人形机器人稳定站立+行走仿真启动(已启用传感器模拟+步态鲁棒性优化)...") print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持转向/变速/启停/急停") + print(f"传感器模拟默认开启,按M键可切换 | 按P键查看传感器数据") - # 初始落地阶段(修复:Windows时间精度) + # 初始落地阶段(原有逻辑保留) start_time = time.time() while time.time() - start_time < self.init_wait_time: alpha = min(1.0, (time.time() - start_time) / self.init_wait_time) @@ -660,9 +809,9 @@ def simulate_stable_standing(self): mujoco.mj_step(self.model, self.data) self.data.qvel[:] *= 0.97 v.sync() - time.sleep(self.dt * self.sim_sleep_scale) # 适配Windows + time.sleep(self.dt) - # 主仿真循环(修复:Windows sleep时间) + # 主仿真循环(原有逻辑保留) print("=== 初始稳定完成,可输入控制指令 ===") while self.data.time < self.sim_duration: torques = self._calculate_stabilizing_torques() @@ -672,18 +821,20 @@ def simulate_stable_standing(self): # 状态监测(原有逻辑保留) if self.data.time % 2 < 0.1: com = self.data.subtree_com[0] - euler = self._get_root_euler() + # 使用传感器数据显示姿态 + euler = self.current_sensor_data["imu"][ + "euler"] if self.current_sensor_data else self._get_root_euler() print( f"时间:{self.data.time:.1f}s | 重心(x/z):{com[0]:.3f}/{com[2]:.3f}m | " f"姿态(roll/pitch):{euler[0]:.3f}/{euler[1]:.3f}rad | 脚接触:{self.foot_contact}" ) v.sync() - time.sleep(self.dt * self.sim_sleep_scale) # 缩短sleep,提升响应 + time.sleep(self.dt * 0.5) # 跌倒判定(原有逻辑保留) com = self.data.subtree_com[0] - euler = self._get_root_euler() + euler = self.current_sensor_data["imu"]["euler"] if self.current_sensor_data else self._get_root_euler() if com[2] < 0.4 or abs(euler[0]) > 0.6 or abs(euler[1]) > 0.6: print( f"跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " @@ -697,34 +848,21 @@ def simulate_stable_standing(self): if __name__ == "__main__": - # 修复:Windows路径兼容(处理中文/空格路径) current_directory = os.path.dirname(os.path.abspath(__file__)) model_file_path = os.path.join(current_directory, "humanoid.xml") - model_file_path = os.path.normpath(model_file_path) # 标准化Windows路径 print(f"模型路径:{model_file_path}") if not os.path.exists(model_file_path): raise FileNotFoundError(f"模型文件不存在:{model_file_path}") - # 修复:Python3.13 Mujoco版本强制适配 - try: - import mujoco - - mujoco_version = mujoco.__version__ - print(f"Mujoco版本:{mujoco_version}(Python3.13兼容)") - if mujoco_version < "2.5.0": - print("[警告] Mujoco版本过低,建议升级:pip install --upgrade mujoco>=2.5.0") - except: - raise ImportError("Mujoco未安装,请执行:pip install mujoco>=2.5.0") - try: stabilizer = HumanoidStabilizer(model_file_path) - # 可选:关闭鲁棒优化(恢复原始逻辑) + # 可选配置:关闭传感器模拟/鲁棒优化 + # stabilizer.enable_sensor_simulation = False # stabilizer.enable_robust_optim = False stabilizer.simulate_stable_standing() except Exception as e: print(f"错误:{e}") import traceback - traceback.print_exc() - input("按Enter键退出...") # Windows下避免闪退 \ No newline at end of file + traceback.print_exc() \ No newline at end of file