diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 346c1b93b0..bdb58f0546 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -59,11 +59,12 @@ def _cmd_vel_callback(self, msg): # 自动触发行走(如果接收到速度指令且当前是停止状态) if target_speed > 0.1 and self.stabilizer.state == "STAND": - self.stabilizer.set_state("WALK") + self.stabilizer.set_state("WALK") # 默认切换到正常走 # 调试输出 if self.stabilizer.data.time % 0.5 < 0.1: # 避免刷屏,每0.5秒输出一次 - print(f"[ROS指令] 速度={target_speed:.2f} | 转向={target_turn:.2f}rad") + print( + f"[ROS指令] 速度={target_speed:.2f} | 转向={target_turn:.2f}rad | 当前步态: {self.stabilizer.gait_mode}") def run(self): """线程主循环(ROS自旋)""" @@ -77,7 +78,7 @@ def stop(self): self.running = False -# ===================== 键盘输入监听线程(原有逻辑完全保留) ===================== +# ===================== 键盘输入监听线程(新增步态切换按键) ===================== class KeyboardInputHandler(threading.Thread): def __init__(self, stabilizer): super().__init__(daemon=True) @@ -89,6 +90,7 @@ def run(self): print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") print("m: 传感器模拟开关 | p: 打印传感器数据") + print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步") # 新增步态切换 print("========================\n") while self.running: try: @@ -109,9 +111,12 @@ def run(self): 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"[指令] 切换为行走状态 | 当前速度: {self.stabilizer.walk_speed:.2f} | 转向: {self.stabilizer.turn_angle:.2f}") + f"[指令] 切换为行走状态 | 当前步态: {current_gait} | 速度: {self.stabilizer.walk_speed:.2f} | 转向: {self.stabilizer.turn_angle:.2f}") elif key == 's': self.stabilizer.set_state("STOP") print("[指令] 切换为停止状态") @@ -132,17 +137,33 @@ def _handle_key(self, key): 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}") + print(f"[指令] 减速 | 当前速度: {self.stabilizer.walk_speed:.2f} | 当前步态: {self.stabilizer.gait_mode}") elif key == 'x': self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1) - print(f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f}") + print(f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f} | 当前步态: {self.stabilizer.gait_mode}") 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"[指令] 切换为慢走模式 | CPG频率: {self.stabilizer.gait_config['SLOW']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['SLOW']['amp']:.2f}") + 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}") + 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}") + elif key == '4': + self.stabilizer.set_gait_mode("STEP_IN_PLACE") + print(f"[指令] 切换为原地踏步模式 | 步幅减半,躯干锁定") # ===================== CPG中枢模式发生器(原有逻辑完全保留) ===================== @@ -190,9 +211,9 @@ def base_phase(self): return 0.0 if self.phase < np.pi else np.pi -# ===================== 人形机器人控制器(新增传感器模拟功能) ===================== +# ===================== 人形机器人控制器(新增多步态模式) ===================== class HumanoidStabilizer: - """适配humanoid.xml模型的稳定站立与行走控制器(新增传感器模拟+鲁棒性优化)""" + """适配humanoid.xml模型的稳定站立与行走控制器(新增多步态+传感器模拟+鲁棒性优化)""" def __init__(self, model_path): # 类型检查与模型加载(原有逻辑完全保留) @@ -262,26 +283,76 @@ def __init__(self, model_path): self.filter_alpha = 0.1 # 低通滤波系数(新增,0.1=更平滑) self.enable_robust_optim = True # 鲁棒优化开关(新增,默认启用) - # 运动状态机(原有逻辑完全保留) + # ===================== 新增:多步态模式配置 ===================== + # 步态参数配置(可根据需求调整) + self.gait_config = { + "SLOW": { # 慢走:低频、小步幅、高耦合(稳定) + "freq": 0.3, # CPG基础频率 + "amp": 0.3, # CPG基础振幅 + "coupling": 0.3, # CPG耦合强度 + "speed_freq_gain": 0.2, # 速度→频率增益 + "speed_amp_gain": 0.1, # 速度→振幅增益 + "com_z_offset": 0.02 # 重心z轴抬高(更稳定) + }, + "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 # 锁定躯干偏航/俯仰 + } + } + self.gait_mode = "NORMAL" # 默认步态:正常走 + self.current_gait_params = self.gait_config[self.gait_mode] + + # 运动状态机(扩展步态相关状态,原有状态保留) self.state = "STAND" # 初始状态:STAND/WALK/STOP/EMERGENCY self.state_map = { "STAND": self._state_stand, # 站立(初始稳定) - "WALK": self._state_walk, # 行走(CPG驱动) + "WALK": self._state_walk, # 行走(根据当前步态模式调整) "STOP": self._state_stop, # 停止(关节归零) "EMERGENCY": self._state_emergency # 急停(力矩清零) } - # 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) # 左腿初始相位π(交替) + # CPG振荡器(初始化时加载默认步态参数) + self.right_leg_cpg = CPGOscillator( + freq=self.current_gait_params["freq"], + amp=self.current_gait_params["amp"], + phase=0.0, + coupling_strength=self.current_gait_params["coupling"] + ) + self.left_leg_cpg = CPGOscillator( + freq=self.current_gait_params["freq"], + amp=self.current_gait_params["amp"], + phase=np.pi, + coupling_strength=self.current_gait_params["coupling"] + ) self.gait_phase = 0.0 # 保留原始相位变量,兼容调试输出 - # 转向/变速参数(原有逻辑完全保留) + # 转向/变速参数(原有逻辑保留,步态切换时动态更新) self.turn_angle = 0.0 # 转向角度(左正右负,范围±0.3rad) 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.walk_speed = 0.5 # 行走速度(0.1~1.0) + self.speed_freq_gain = self.current_gait_params["speed_freq_gain"] + self.speed_amp_gain = self.current_gait_params["speed_amp_gain"] # 行走功能参数(原有逻辑保留) self.gait_cycle = 2.0 @@ -290,7 +361,7 @@ 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) @@ -308,6 +379,36 @@ def __init__(self, model_path): # 初始化稳定姿态(原有逻辑完全保留) self._init_stable_pose() + # ===================== 新增:步态模式切换接口 ===================== + def set_gait_mode(self, mode): + """切换步态模式""" + if mode not in self.gait_config.keys(): + print(f"[警告] 无效的步态模式:{mode},默认使用NORMAL") + mode = "NORMAL" + + self.gait_mode = mode + self.current_gait_params = self.gait_config[mode] + + # 更新CPG基础参数 + self.right_leg_cpg.base_freq = self.current_gait_params["freq"] + self.right_leg_cpg.base_amp = self.current_gait_params["amp"] + self.right_leg_cpg.base_coupling = self.current_gait_params["coupling"] + + self.left_leg_cpg.base_freq = self.current_gait_params["freq"] + self.left_leg_cpg.base_amp = self.current_gait_params["amp"] + self.left_leg_cpg.base_coupling = self.current_gait_params["coupling"] + + # 更新速度增益 + self.speed_freq_gain = self.current_gait_params["speed_freq_gain"] + self.speed_amp_gain = self.current_gait_params["speed_amp_gain"] + + # 更新重心目标(步态适配) + self.com_target[2] = 0.78 + self.current_gait_params["com_z_offset"] + + # 重置CPG状态 + self.right_leg_cpg.reset() + self.left_leg_cpg.reset() + def _init_stable_pose(self): """初始化稳定姿态(原有逻辑完全保留)""" mujoco.mj_resetData(self.model, self.data) @@ -347,7 +448,7 @@ def _init_stable_pose(self): mujoco.mj_forward(self.model, self.data) - # ===================== 新增:传感器数据模拟方法 ===================== + # ===================== 传感器模拟相关方法(原有新增逻辑保留) ===================== def _simulate_imu_data(self): """模拟带噪声+延迟的IMU数据(欧拉角+角速度)""" # 获取真实姿态和角速度 @@ -467,13 +568,14 @@ def _get_sensor_data(self): self.current_sensor_data = { "imu": imu_data, "foot": foot_data, - "time": self.data.time + "time": self.data.time, + "gait_mode": self.gait_mode # 新增:记录当前步态 } return self.current_sensor_data def print_sensor_data(self): - """打印当前传感器数据(调试用)""" + """打印当前传感器数据(调试用,新增步态信息)""" if not self.current_sensor_data: print("[传感器数据] 暂无数据") return @@ -482,7 +584,7 @@ def print_sensor_data(self): foot = self.current_sensor_data["foot"] print("\n=== 传感器数据 ===") print( - f"仿真时间: {self.current_sensor_data['time']:.2f}s | 模拟状态: {'开启' if self.enable_sensor_simulation else '关闭'}") + f"仿真时间: {self.current_sensor_data['time']:.2f}s | 模拟状态: {'开启' if self.enable_sensor_simulation else '关闭'} | 当前步态: {self.gait_mode}") 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( @@ -491,7 +593,7 @@ def print_sensor_data(self): f"右脚力: {foot['right_force']:.2f}N (真实: {foot['true_right_force']:.2f}N) | 接触: {foot['right_contact']}") print("==================\n") - # 状态机方法(原有逻辑保留,WALK状态新增鲁棒优化) + # 状态机方法(WALK状态适配多步态模式) def _state_stand(self): """站立状态:维持初始稳定姿态,CPG重置""" self.right_leg_cpg.reset() @@ -499,34 +601,47 @@ 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 - # 新增:重心保护 - 重心过低时自动降速 + # 重心保护 - 重心过低时自动降速(原有逻辑) 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. 转向联动:躯干偏航 + 左右腿步长差(原有逻辑保留) - self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = self.turn_angle * self.turn_gain - if self.turn_angle > 0: # 左转 - self.right_leg_cpg.amp *= 1.1 - self.left_leg_cpg.amp *= 0.9 - elif self.turn_angle < 0: # 右转 - self.right_leg_cpg.amp *= 0.9 - self.left_leg_cpg.amp *= 1.1 - - # 3. CPG更新(新增:传入速度/转向系数,动态调整耦合) + print( + f"[鲁棒优化] 重心过低({current_com_z:.2f}m),自动降速到{self.walk_speed:.2f} | 当前步态: {self.gait_mode}") + + # ===================== 适配多步态的CPG参数更新 ===================== + # 1. 加载当前步态的基础参数 + gait_params = self.current_gait_params + + # 2. 变速联动:根据当前步态的增益调整CPG频率+振幅 + self.right_leg_cpg.freq = gait_params["freq"] + self.walk_speed * gait_params["speed_freq_gain"] + self.left_leg_cpg.freq = gait_params["freq"] + self.walk_speed * gait_params["speed_freq_gain"] + self.right_leg_cpg.amp = gait_params["amp"] + self.walk_speed * gait_params["speed_amp_gain"] + self.left_leg_cpg.amp = gait_params["amp"] + self.walk_speed * gait_params["speed_amp_gain"] + + # 3. 原地踏步模式:锁定躯干偏航,步幅固定 + if self.gait_mode == "STEP_IN_PLACE": + self.turn_angle = 0.0 # 强制归零转向 + self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = 0.0 # 锁定躯干偏航 + # 步幅固定,不受速度影响 + self.right_leg_cpg.amp = gait_params["amp"] + self.left_leg_cpg.amp = gait_params["amp"] + else: + # 转向联动:躯干偏航 + 左右腿步长差(原有逻辑) + self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = self.turn_angle * self.turn_gain + if self.turn_angle > 0: # 左转 + self.right_leg_cpg.amp *= 1.1 + self.left_leg_cpg.amp *= 0.9 + elif self.turn_angle < 0: # 右转 + self.right_leg_cpg.amp *= 0.9 + self.left_leg_cpg.amp *= 1.1 + + # 4. 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( @@ -538,7 +653,7 @@ def _state_walk(self): speed_factor=speed_factor, turn_factor=turn_factor ) - # 4. 更新关节目标(原有逻辑保留) + # 5. 更新关节目标(原有逻辑保留) 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 @@ -546,13 +661,13 @@ def _state_walk(self): 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): @@ -576,10 +691,16 @@ def set_state(self, state): self._init_stable_pose() def set_turn_angle(self, angle): - self.turn_angle = np.clip(angle, -0.3, 0.3) + # 原地踏步模式下禁止转向 + if self.gait_mode != "STEP_IN_PLACE": + 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) + # 原地踏步模式下速度固定 + if self.gait_mode != "STEP_IN_PLACE": + self.walk_speed = np.clip(speed, 0.1, 1.0) + else: + self.walk_speed = 0.5 # 原地踏步固定速度 # 四元数转欧拉角(原有逻辑完全保留) def _quat_to_euler_xyz(self, quat): @@ -632,12 +753,12 @@ def _detect_foot_contact(self): self.left_foot_force = self.foot_contact_threshold self.right_foot_force = self.foot_contact_threshold - # 计算稳定力矩(核心:新增基于传感器数据的反馈控制) + # 计算稳定力矩(核心:基于传感器数据的反馈控制,新增步态信息) def _calculate_stabilizing_torques(self): # 状态机驱动(原有逻辑保留) self.state_map[self.state]() - # 新增:获取传感器数据(模拟/真实可切换) + # 获取传感器数据(模拟/真实可切换) sensor_data = self._get_sensor_data() imu = sensor_data["imu"] foot = sensor_data["foot"] @@ -703,7 +824,7 @@ 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增益 - 接触力越小,增益越低(避免打滑) + # 动态PD增益 - 接触力越小,增益越低(避免打滑) if self.enable_robust_optim: # 计算接触力归一化系数(0~1) if "right" in joint_name: @@ -766,11 +887,11 @@ 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}") + f"状态: {self.state} | 步态模式: {self.gait_mode} | 步态相位: {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( @@ -779,7 +900,7 @@ def _calculate_stabilizing_torques(self): self.prev_com = com return torques - # 仿真循环(原有逻辑完全保留) + # 仿真循环(原有逻辑完全保留,新增步态信息输出) def simulate_stable_standing(self): # 启动ROS /cmd_vel监听线程 self.ros_handler = ROSCmdVelHandler(self) @@ -796,9 +917,9 @@ def simulate_stable_standing(self): v.cam.elevation = -25 v.cam.lookat = [0, 0, 0.6] - print("人形机器人稳定站立+行走仿真启动(已启用传感器模拟+步态鲁棒性优化)...") - print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持转向/变速/启停/急停") - print(f"传感器模拟默认开启,按M键可切换 | 按P键查看传感器数据") + print("人形机器人稳定站立+行走仿真启动(已启用多步态+传感器模拟+步态鲁棒性优化)...") + print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持多步态切换(1=慢走/2=正常/3=小跑/4=原地踏步)") + print(f"默认步态模式:{self.gait_mode}\n") # 初始落地阶段(原有逻辑保留) start_time = time.time() @@ -818,15 +939,15 @@ def simulate_stable_standing(self): 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.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}" + f"姿态(roll/pitch):{euler[0]:.3f}/{euler[1]:.3f}rad | 脚接触:{self.foot_contact} | " + f"当前步态:{self.gait_mode}" ) v.sync() @@ -838,7 +959,7 @@ def simulate_stable_standing(self): 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 | " - f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad" + f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad | 当前步态:{self.gait_mode}" ) self.set_state("STAND") # 跌倒后自动恢复站立 @@ -857,9 +978,10 @@ 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}")