diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 0039466146..036eef72d1 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -5,10 +5,17 @@ import os import sys import threading +import select # 新增:跨平台输入兼容 +import queue # 新增:输入队列,避免阻塞 -# ===================== 新增:ROS话题接收模块(可选兼容,无ROS不报错) ===================== +# ===================== 全局输入队列(解决Windows输入阻塞) ===================== +input_queue = queue.Queue() + + +# ===================== ROS话题接收模块(原有逻辑完全保留) ===================== class ROSCmdVelHandler(threading.Thread): """ROS /cmd_vel话题接收线程,映射linear.x→速度,angular.z→转向""" + def __init__(self, stabilizer): super().__init__(daemon=True) self.stabilizer = stabilizer @@ -33,7 +40,8 @@ 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 + "/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=最快)") @@ -68,44 +76,66 @@ def run(self): return while self.running and not self.rospy.is_shutdown(): self.rospy.spin_once() - time.sleep(0.01) + time.sleep(0.001) # 缩短sleep,适配Windows线程 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("========================\n") + while self.running: try: - # 非阻塞键盘输入(兼容不同系统) - if sys.platform == "win32": + key = None + # 修复:Windows下msvcrt输入逻辑(Python3.13兼容) + if self.is_windows: import msvcrt if msvcrt.kbhit(): - key = msvcrt.getch().decode('utf-8').lower() - self._handle_key(key) + 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输入逻辑(保留) else: - import select if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: key = sys.stdin.read(1).lower() - self._handle_key(key) + + if key: + input_queue.put(key) # 放入队列,避免阻塞 + self._handle_key(key) + time.sleep(0.001) # 降低CPU占用,适配Windows + except Exception as e: + # 容错:避免输入异常导致线程退出 + print(f"[键盘输入警告] {e}(忽略,继续监听)") time.sleep(0.01) - except: continue def _handle_key(self, key): if key == 'w': self.stabilizer.set_state("WALK") - print(f"[指令] 切换为行走状态 | 当前速度: {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("[指令] 切换为停止状态") @@ -131,24 +161,33 @@ def _handle_key(self, key): self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1) print(f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f}") -# ===================== 原始代码: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) - self.base_amp = amp # 基础振幅(对应原始步长) + self.base_amp = amp # 基础振幅(对应原始步长) self.freq = freq self.amp = amp - self.phase = phase # 初始相位(左右腿差π) - self.coupling = coupling_strength # 腿间耦合强度 + self.phase = phase # 初始相位(左右腿差π) + self.base_coupling = coupling_strength # 基础耦合强度 + self.coupling = coupling_strength # 动态耦合强度 self.state = np.array([np.sin(phase), np.cos(phase)]) # 振荡器状态(x,y) - def update(self, dt, target_phase=0.0): - """更新CPG状态,返回关节目标偏移量""" + def update(self, dt, target_phase=0.0, speed_factor=1.0, turn_factor=0.0): + """ + 更新CPG状态,返回关节目标偏移量 + 新增:speed_factor(速度系数)、turn_factor(转向系数)动态调整耦合强度 + """ + # 动态调整耦合强度:速度越快/转向越大,耦合越强(提升步态稳定性) + 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) + 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]) # 更新相位 @@ -159,6 +198,7 @@ def reset(self): """重置CPG状态""" 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.state = np.array([np.sin(self.phase), np.cos(self.phase)]) @@ -166,12 +206,13 @@ def reset(self): def base_phase(self): return 0.0 if self.phase < np.pi else np.pi -# ===================== 原始代码:人形机器人控制器(完全保留,仅新增ROS线程启动) ===================== + +# ===================== 人形机器人控制器(修复Windows仿真逻辑) ===================== class HumanoidStabilizer: - """适配humanoid.xml模型的稳定站立与行走控制器(新增转向/变速/状态机)""" + """适配humanoid.xml模型的稳定站立与行走控制器(新增步态鲁棒性优化)""" def __init__(self, model_path): - # 类型检查与模型加载(原始逻辑完全保留) + # 类型检查与模型加载(原有逻辑完全保留) if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") @@ -181,15 +222,16 @@ 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 = [ "abdomen_z", "abdomen_y", "abdomen_x", "hip_x_right", "hip_z_right", "hip_y_right", @@ -202,72 +244,75 @@ def __init__(self, model_path): self.joint_name_to_idx = {name: i for i, name in enumerate(self.joint_names)} self.num_joints = len(self.joint_names) - # PD控制增益(原始逻辑完全保留) + # PD控制增益(原有逻辑保留,新增动态增益系数) self.kp_roll = 120.0 self.kd_roll = 40.0 self.kp_pitch = 100.0 self.kd_pitch = 35.0 self.kp_yaw = 30.0 self.kd_yaw = 15.0 - self.kp_hip = 250.0 - self.kd_hip = 45.0 - self.kp_knee = 280.0 - self.kd_knee = 50.0 - self.kp_ankle = 200.0 - self.kd_ankle = 60.0 + self.base_kp_hip = 250.0 # 基础KP(新增) + self.base_kd_hip = 45.0 # 基础KD(新增) + self.base_kp_knee = 280.0 + self.base_kd_knee = 50.0 + self.base_kp_ankle = 200.0 + self.base_kd_ankle = 60.0 self.kp_waist = 40.0 self.kd_waist = 20.0 self.kp_arm = 20.0 self.kd_arm = 20.0 - # 重心目标(原始逻辑保留) + # 重心目标(原有逻辑保留,新增重心保护参数) self.com_target = np.array([0.05, 0.0, 0.78]) self.kp_com = 50.0 self.foot_contact_threshold = 1.5 + self.com_safety_threshold = 0.6 # 重心z轴安全阈值(新增) + self.speed_reduction_factor = 0.5 # 重心过低时的降速系数(新增) - # 状态变量(原始逻辑保留,新增状态机/CPG/转向/变速参数) + # 状态变量(原有逻辑保留,新增鲁棒优化参数) self.joint_targets = np.zeros(self.num_joints) + self.prev_joint_targets = np.zeros(self.num_joints) # 低通滤波用(新增) self.prev_com = np.zeros(3) self.foot_contact = np.zeros(2) self.integral_roll = 0.0 self.integral_pitch = 0.0 self.integral_limit = 0.15 + self.filter_alpha = 0.1 # 低通滤波系数(新增,0.1=更平滑) + self.enable_robust_optim = True # 鲁棒优化开关(新增,默认启用) - # 新增:运动状态机(核心) + # 运动状态机(原有逻辑完全保留) self.state = "STAND" # 初始状态:STAND/WALK/STOP/EMERGENCY self.state_map = { - "STAND": self._state_stand, # 站立(初始稳定) - "WALK": self._state_walk, # 行走(CPG驱动) - "STOP": self._state_stop, # 停止(关节归零) + "STAND": self._state_stand, # 站立(初始稳定) + "WALK": self._state_walk, # 行走(CPG驱动) + "STOP": self._state_stop, # 停止(关节归零) "EMERGENCY": self._state_emergency # 急停(力矩清零) } - # 新增:CPG振荡器(替代原始固定正弦步态,兼容变速) + # CPG振荡器(原有逻辑保留,使用优化后的CPG类) self.right_leg_cpg = CPGOscillator(freq=0.5, amp=0.4, phase=0.0) # 右腿初始相位0 self.left_leg_cpg = CPGOscillator(freq=0.5, amp=0.4, phase=np.pi) # 左腿初始相位π(交替) self.gait_phase = 0.0 # 保留原始相位变量,兼容调试输出 - # 新增:转向参数 + # 转向/变速参数(原有逻辑完全保留) self.turn_angle = 0.0 # 转向角度(左正右负,范围±0.3rad) - self.turn_gain = 0.1 # 躯干偏航增益 - - # 新增:变速参数 + self.turn_gain = 0.1 # 躯干偏航增益 self.walk_speed = 0.5 # 行走速度(0.1~1.0,映射到CPG频率/振幅) self.speed_freq_gain = 0.4 # 速度→频率增益(0.3~0.7Hz) - self.speed_amp_gain = 0.2 # 速度→振幅增益(0.3~0.5) + self.speed_amp_gain = 0.2 # 速度→振幅增益(0.3~0.5) - # 行走功能参数(原始逻辑保留,CPG替代后不再使用,仅做兼容) + # 行走功能参数(原有逻辑保留) self.gait_cycle = 2.0 self.step_offset_hip = 0.4 self.step_offset_knee = 0.6 self.step_offset_ankle = 0.3 self.walk_start_time = None - # 初始化稳定姿态(原始逻辑保留) + # 初始化稳定姿态(原有逻辑完全保留) self._init_stable_pose() def _init_stable_pose(self): - """初始化稳定姿态(原始逻辑完全保留)""" + """初始化稳定姿态(原有逻辑完全保留)""" mujoco.mj_resetData(self.model, self.data) self.data.qpos[2] = 1.282 self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] @@ -305,7 +350,7 @@ def _init_stable_pose(self): mujoco.mj_forward(self.model, self.data) - # 新增:状态机核心方法 + # 状态机方法(原有逻辑保留,WALK状态新增鲁棒优化) def _state_stand(self): """站立状态:维持初始稳定姿态,CPG重置""" self.right_leg_cpg.reset() @@ -313,19 +358,26 @@ def _state_stand(self): self._init_stable_pose() # 恢复站立姿态 def _state_walk(self): - """行走状态:CPG驱动+转向+变速""" + """行走状态:CPG驱动+转向+变速 + 鲁棒性优化""" if self.walk_start_time is None: self.walk_start_time = self.data.time - # 1. 变速联动:速度→CPG频率+振幅 + + # 新增:重心保护 - 重心过低时自动降速 + current_com_z = self.data.subtree_com[0][2] + if current_com_z < self.com_safety_threshold and self.enable_robust_optim: + current_speed = self.walk_speed * self.speed_reduction_factor + self.walk_speed = np.clip(current_speed, 0.1, self.walk_speed) + if self.data.time % 1 < 0.1: + print(f"[鲁棒优化] 重心过低({current_com_z:.2f}m),自动降速到{self.walk_speed:.2f}") + + # 1. 变速联动:速度→CPG频率+振幅(原有逻辑保留) self.right_leg_cpg.freq = 0.3 + self.walk_speed * self.speed_freq_gain self.left_leg_cpg.freq = 0.3 + self.walk_speed * self.speed_freq_gain self.right_leg_cpg.amp = 0.3 + self.walk_speed * self.speed_amp_gain self.left_leg_cpg.amp = 0.3 + self.walk_speed * self.speed_amp_gain - # 2. 转向联动:躯干偏航 + 左右腿步长差 - # 躯干偏航(abdomen_z)由转向角度控制 + # 2. 转向联动:躯干偏航 + 左右腿步长差(原有逻辑保留) self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = self.turn_angle * self.turn_gain - # 左右腿步长差(CPG振幅差):左转→左腿振幅减小/右腿增大,右转反之 if self.turn_angle > 0: # 左转 self.right_leg_cpg.amp *= 1.1 self.left_leg_cpg.amp *= 0.9 @@ -333,26 +385,39 @@ def _state_walk(self): self.right_leg_cpg.amp *= 0.9 self.left_leg_cpg.amp *= 1.1 - # 3. CPG更新(替代原始固定正弦步态) - right_hip_offset = self.right_leg_cpg.update(self.dt, target_phase=self.left_leg_cpg.phase) - left_hip_offset = self.left_leg_cpg.update(self.dt, target_phase=self.right_leg_cpg.phase) - - # 4. 更新关节目标(兼容原始关节映射) + # 3. CPG更新(新增:传入速度/转向系数,动态调整耦合) + speed_factor = self.walk_speed / 1.0 # 速度归一化(0~1) + turn_factor = self.turn_angle / 0.3 # 转向归一化(-1~1) + right_hip_offset = self.right_leg_cpg.update( + self.dt, target_phase=self.left_leg_cpg.phase, + speed_factor=speed_factor, turn_factor=turn_factor + ) + left_hip_offset = self.left_leg_cpg.update( + self.dt, target_phase=self.right_leg_cpg.phase, + speed_factor=speed_factor, turn_factor=turn_factor + ) + + # 4. 更新关节目标(原有逻辑保留) self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.1 + right_hip_offset - self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.4 - right_hip_offset * 1.2 # 膝髋联动 - self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 + right_hip_offset * 0.5 # 踝髋联动 - + self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.4 - right_hip_offset * 1.2 + self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 + right_hip_offset * 0.5 self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.1 + left_hip_offset self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.4 - left_hip_offset * 1.2 self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0 + left_hip_offset * 0.5 - # 更新原始步态相位(兼容调试输出) + # 新增:关节目标低通滤波(平滑指令,减少抖动) + if self.enable_robust_optim: + 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() + + # 更新原始步态相位(原有逻辑保留) self.gait_phase = self.right_leg_cpg.phase / (2 * np.pi) % 1.0 def _state_stop(self): """停止状态:关节目标归零,缓慢减速""" self.joint_targets *= 0.95 # 渐进归零,避免突变 - self.data.qvel[:] *= 0.9 # 速度阻尼 + self.data.qvel[:] *= 0.9 # 速度阻尼 def _state_emergency(self): """急停状态:力矩清零,速度归零""" @@ -360,23 +425,22 @@ def _state_emergency(self): self.data.qvel[:] = 0.0 self.joint_targets[:] = 0.0 - # 新增:外部控制接口(设置状态/转向/速度) + # 外部控制接口(原有逻辑完全保留) def set_state(self, state): if state in self.state_map.keys(): self.state = state - # 状态切换时重置行走开始时间 if state == "WALK": self.walk_start_time = None elif state == "STAND": self._init_stable_pose() def set_turn_angle(self, angle): - self.turn_angle = np.clip(angle, -0.3, 0.3) # 限制转向角度,避免失衡 + self.turn_angle = np.clip(angle, -0.3, 0.3) def set_walk_speed(self, speed): - self.walk_speed = np.clip(speed, 0.1, 1.0) # 限制速度范围 + self.walk_speed = np.clip(speed, 0.1, 1.0) - # 原始方法:四元数转欧拉角(完全保留) + # 四元数转欧拉角(原有逻辑完全保留) def _quat_to_euler_xyz(self, quat): w, x, y, z = quat sinr_cosp = 2 * (w * x + y * z) @@ -389,14 +453,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) - # 原始方法:检测脚部接触(完全保留) + # 检测脚部接触(原有逻辑保留,新增接触力强度计算) def _detect_foot_contact(self): try: left_foot_geoms = ["foot1_left", "foot2_left"] @@ -419,19 +483,25 @@ 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 + except Exception as e: print(f"接触检测警告: {e}") self.foot_contact = np.ones(2) + self.left_foot_force = self.foot_contact_threshold + self.right_foot_force = self.foot_contact_threshold - # 原始方法:计算稳定力矩(修改:加入状态机逻辑,保留所有原始PD/补偿逻辑) + # 计算稳定力矩(核心:新增动态PD增益、接触力自适应) def _calculate_stabilizing_torques(self): - # 状态机驱动:根据当前状态更新关节目标 + # 状态机驱动(原有逻辑保留) self.state_map[self.state]() - # 原始力矩计算逻辑(完全保留) + # 原始力矩计算逻辑(保留,新增鲁棒优化) 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() root_vel = np.clip(root_vel, -3.0, 3.0) @@ -452,19 +522,19 @@ def _calculate_stabilizing_torques(self): torso_torque = np.array([roll_torque, pitch_torque, yaw_torque]) torso_torque = np.clip(torso_torque, -30.0, 30.0) - # 重心补偿 + # 重心补偿(原有逻辑完全保留) com = self.data.subtree_com[0].astype(np.float64).copy() com_error = self.com_target - com 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) - # 腰部关节控制 + # 腰部关节控制(原有逻辑完全保留) waist_joints = ["abdomen_z", "abdomen_y", "abdomen_x"] for joint_name in waist_joints: idx = self.joint_name_to_idx[joint_name] @@ -472,7 +542,7 @@ def _calculate_stabilizing_torques(self): joint_error = np.clip(joint_error, -0.3, 0.3) torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx] - # 腿部关节控制 + # 腿部关节控制(新增:基于接触力的动态PD增益) leg_joints = [ "hip_x_right", "hip_z_right", "hip_y_right", "knee_right", "ankle_y_right", "ankle_x_right", @@ -485,9 +555,20 @@ def _calculate_stabilizing_torques(self): joint_error = self.joint_targets[idx] - current_joints[idx] joint_error = np.clip(joint_error, -0.3, 0.3) + # 新增:动态PD增益 - 接触力越小,增益越低(避免打滑) + if self.enable_robust_optim: + # 计算接触力归一化系数(0~1) + if "right" in joint_name: + force_factor = np.clip(self.right_foot_force / (self.foot_contact_threshold * 2), 0.5, 1.0) + else: + force_factor = np.clip(self.left_foot_force / (self.foot_contact_threshold * 2), 0.5, 1.0) + else: + force_factor = 1.0 # 关闭优化则使用原始增益 + + # 原有PD增益逻辑,替换为动态增益 if "hip" in joint_name: - kp = self.kp_hip - kd = self.kd_hip + kp = self.base_kp_hip * force_factor + kd = self.base_kd_hip * force_factor if "y" in joint_name: if "right" in joint_name: joint_error += torso_torque[1] * 0.02 @@ -495,24 +576,25 @@ def _calculate_stabilizing_torques(self): joint_error += torso_torque[1] * 0.02 elif "knee" in joint_name: - kp = self.kp_knee - kd = self.kd_knee + kp = self.base_kp_knee * force_factor + kd = self.base_kd_knee * force_factor joint_error += com_compensation[2] * 0.05 elif "ankle" in joint_name: - kp = self.kp_ankle - kd = self.kd_ankle + kp = self.base_kp_ankle * force_factor + kd = self.base_kd_ankle * force_factor if "y" in joint_name: joint_error += torso_torque[1] * 0.01 + # 原有接触判断逻辑(保留,与动态增益叠加) if ("left" in joint_name and self.foot_contact[1] == 0) or \ - ("right" in joint_name and self.foot_contact[0] == 0): + ("right" in joint_name and self.foot_contact[0] == 0): kp *= 0.8 kd *= 0.9 torques[idx] = kp * joint_error - kd * current_vel[idx] - # 手臂关节控制 + # 手臂关节控制(原有逻辑完全保留) arm_joints = [ "shoulder1_right", "shoulder2_right", "elbow_right", "shoulder1_left", "shoulder2_left", "elbow_left" @@ -522,7 +604,7 @@ def _calculate_stabilizing_torques(self): joint_error = self.joint_targets[idx] - current_joints[idx] torques[idx] = self.kp_arm * joint_error - self.kd_arm * current_vel[idx] - # 力矩限幅 + # 力矩限幅(原有逻辑完全保留) torque_limits = { "abdomen_z": 50, "abdomen_y": 50, "abdomen_x": 50, "hip_x_right": 150, "hip_z_right": 150, "hip_y_right": 150, @@ -536,38 +618,40 @@ 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(f"状态: {self.state} | 步态相位: {self.gait_phase:.2f} | 速度: {self.walk_speed:.2f} | 转向: {self.turn_angle:.2f}") + print( + f"状态: {self.state} | 步态相位: {self.gait_phase:.2f} | 速度: {self.walk_speed:.2f} | 转向: {self.turn_angle:.2f}") 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]}") + print( + f"右脚接触: {self.foot_contact[0]}, 左脚接触: {self.foot_contact[1]} | 鲁棒优化: {'开启' if self.enable_robust_optim else '关闭'}") self.prev_com = com return torques - # 原始方法:仿真循环(修改:新增ROS线程启动,保留所有原始逻辑) + # 仿真循环(修复:Windows线程/时间兼容) def simulate_stable_standing(self): - # 新增:启动ROS /cmd_vel监听线程 + # 启动ROS /cmd_vel监听线程 self.ros_handler = ROSCmdVelHandler(self) self.ros_handler.start() - # 原始:启动键盘监听线程 + # 启动键盘监听线程 keyboard_handler = KeyboardInputHandler(self) keyboard_handler.start() with viewer.launch_passive(self.model, self.data) as v: - # 优化相机视角(原始逻辑保留) + # 优化相机视角(原有逻辑保留) v.cam.distance = 3.0 v.cam.azimuth = 90 v.cam.elevation = -25 v.cam.lookat = [0, 0, 0.6] - print("人形机器人稳定站立+行走仿真启动...") + print("人形机器人稳定站立+行走仿真启动(已启用步态鲁棒性优化)...") print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持转向/变速/启停/急停") - # 初始落地阶段(原始逻辑保留) + # 初始落地阶段(修复: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) @@ -576,16 +660,16 @@ def simulate_stable_standing(self): mujoco.mj_step(self.model, self.data) self.data.qvel[:] *= 0.97 v.sync() - time.sleep(self.dt) + time.sleep(self.dt * self.sim_sleep_scale) # 适配Windows - # 主仿真循环(原始逻辑保留,状态机驱动) + # 主仿真循环(修复:Windows sleep时间) print("=== 初始稳定完成,可输入控制指令 ===") while self.data.time < self.sim_duration: torques = self._calculate_stabilizing_torques() self.data.ctrl[:] = torques mujoco.mj_step(self.model, self.data) - # 状态监测(原始逻辑保留) + # 状态监测(原有逻辑保留) if self.data.time % 2 < 0.1: com = self.data.subtree_com[0] euler = self._get_root_euler() @@ -595,9 +679,9 @@ def simulate_stable_standing(self): ) v.sync() - time.sleep(self.dt * 0.5) + time.sleep(self.dt * self.sim_sleep_scale) # 缩短sleep,提升响应 - # 跌倒判定(原始逻辑保留) + # 跌倒判定(原有逻辑保留) com = self.data.subtree_com[0] euler = self._get_root_euler() if com[2] < 0.4 or abs(euler[0]) > 0.6 or abs(euler[1]) > 0.6: @@ -606,7 +690,6 @@ def simulate_stable_standing(self): f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad" ) self.set_state("STAND") # 跌倒后自动恢复站立 - # break # 注释掉break,跌倒后可继续控制 # 停止ROS线程 self.ros_handler.stop() @@ -614,17 +697,34 @@ 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_robust_optim = False stabilizer.simulate_stable_standing() except Exception as e: print(f"错误:{e}") import traceback - traceback.print_exc() \ No newline at end of file + + traceback.print_exc() + input("按Enter键退出...") # Windows下避免闪退 \ No newline at end of file