From 060711b05b3d55cca1eedc068b167076826788ff Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Thu, 18 Dec 2025 16:32:11 +0800 Subject: [PATCH 1/7] update --- src/Mujoco_manrun/main.py | 514 ++++++++++++++++++++------------------ 1 file changed, 277 insertions(+), 237 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 29db331ce3..b0b29ef42e 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -5,11 +5,10 @@ import os -class G1Stabilizer: - """修复踮脚后仰的G1机器人稳定站立控制器(新增行走功能)""" +class Stabilizer: def __init__(self, model_path): - # 类型检查与模型加载 + # 基础初始化(保留原有核心逻辑) if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") @@ -19,147 +18,163 @@ def __init__(self, model_path): except Exception as e: raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性") - # 仿真核心参数(强化稳定性) + # 1. 仿真参数优化:固定时间步+更保守的初始稳定 self.sim_duration = 120.0 - self.dt = 0.001 + self.dt = 0.005 # 固定0.005s步长,提升仿真稳定性 self.model.opt.timestep = self.dt - self.init_wait_time = 4.0 # 延长初始稳定时间 + self.init_wait_time = 5.0 # 延长初始稳定时间至5s self.model.opt.gravity[2] = -9.81 - self.model.opt.iterations = 200 - self.model.opt.tolerance = 1e-8 + self.model.opt.iterations = 300 # 增加迭代次数提升接触求解精度 + self.model.opt.tolerance = 1e-9 # 降低容差,提升计算精度 - # 关节名称映射 self.joint_names = [ - # 左腿关节 - "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", - "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", - # 右腿关节 - "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", - "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", - # 腰部关节 - "waist_yaw_joint", - # 左臂关节 - "left_shoulder_pitch_joint", "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", "left_elbow_joint", "left_wrist_roll_joint", - # 右臂关节 - "right_shoulder_pitch_joint", "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", "right_elbow_joint", "right_wrist_roll_joint" + "abdomen_z", "abdomen_y", "abdomen_x", + "hip_x_right", "hip_z_right", "hip_y_right", + "knee_right", "ankle_y_right", "ankle_x_right", + "hip_x_left", "hip_z_left", "hip_y_left", + "knee_left", "ankle_y_left", "ankle_x_left", + "shoulder1_right", "shoulder2_right", "elbow_right", + "shoulder1_left", "shoulder2_left", "elbow_left" ] - self.joint_name_to_idx = {name: i for i, name in enumerate(self.joint_names)} self.num_joints = len(self.joint_names) - # 关键修复: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.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]) # 重心前移5cm,高度降至0.78m - self.kp_com = 50.0 # 降低重心补偿,避免过度修正 - self.foot_contact_threshold = 1.5 # 降低接触阈值,提高检测灵敏度 - - # 状态变量 + # PD增益优化:分阶段动态增益(核心) + # 基础增益(支撑腿) + self.kp_roll = 150.0 # 提高侧倾增益,增强横向稳定 + self.kd_roll = 50.0 + self.kp_pitch = 130.0 # 提高俯仰增益,抑制后仰 + self.kd_pitch = 45.0 + self.kp_yaw = 40.0 + self.kd_yaw = 20.0 + + # 腿部增益(分支撑/摆动) + self.kp_hip_support = 300.0 # 支撑腿髋关节高增益 + self.kd_hip_support = 60.0 + self.kp_hip_swing = 150.0 # 摆动腿髋关节低增益,避免过度摆动 + self.kd_hip_swing = 30.0 + + self.kp_knee_support = 320.0 # 支撑腿膝关节高增益 + self.kd_knee_support = 70.0 + self.kp_knee_swing = 180.0 # 摆动腿膝关节低增益 + self.kd_knee_swing = 40.0 + + self.kp_ankle_support = 250.0 # 支撑腿踝关节高增益(核心防踮脚) + self.kd_ankle_support = 80.0 + self.kp_ankle_swing = 120.0 # 摆动腿踝关节低增益 + self.kd_ankle_swing = 40.0 + + # 腰部/手臂增益(小幅提升) + self.kp_waist = 50.0 + self.kd_waist = 25.0 + self.kp_arm = 25.0 + self.kd_arm = 25.0 + + # 重心控制优化:动态目标+速度反馈 + self.com_base_target = np.array([0.0, 0.0, 0.85]) # 基础重心高度提升,降低跌倒风险 + # ========== 关键修复1:初始化com_target,避免属性不存在 ========== + self.com_target = self.com_base_target.copy() # 初始赋值,与基础重心一致 + self.com_gait_offset = np.array([0.08, 0.0, 0.02]) # 步态相位对应的重心偏移 + self.kp_com = 80.0 # 提高重心比例增益 + self.kd_com = 30.0 # 新增重心速度阻尼 + self.foot_contact_threshold = 2.0 # 提高接触阈值,减少误判 + + # 状态变量优化:新增滤波和平滑参数 self.joint_targets = np.zeros(self.num_joints) + self.joint_targets_prev = np.zeros(self.num_joints) # 上一帧关节目标(滤波用) self.prev_com = np.zeros(3) - self.foot_contact = np.zeros(2) + self.com_vel = np.zeros(3) # 重心速度 + self.foot_contact = np.zeros(2) # [right, left] + self.foot_contact_history = np.zeros((2, 5)) # 接触状态历史(5帧滤波) self.integral_roll = 0.0 self.integral_pitch = 0.0 - self.integral_limit = 0.15 # 进一步收紧积分限幅 - - # 【行走功能新增】步态参数(核心:增大偏移量确保动作明显) - self.gait_phase = 0.0 # 步态相位(0-1循环) - self.gait_cycle = 2.0 # 步态周期(秒,放慢周期让动作更明显) - self.step_offset_hip = 0.8 # 髋关节俯仰偏移量(【大幅增大】原0.1→0.8) - self.step_offset_knee = 1.2 # 膝关节偏移量(【大幅增大】原0.4→1.2) - self.step_offset_ankle = 0.5 # 踝关节偏移量 - self.walk_start_time = None # 行走开始时间标记 - - # 初始化稳定姿态(核心修复:关节角度) + self.integral_limit = 0.1 # 进一步收紧积分限幅 + self.integral_deadband = 0.01 # 积分死区,小误差不积分 + + # 步态规划优化:平滑插值+慢周期 + self.gait_phase = 0.0 + self.gait_cycle = 2.5 # 延长步态周期至2.5s,动作更平缓 + self.step_offset_hip = 0.35 # 降低髋关节偏移,减少重心波动 + self.step_offset_knee = 0.5 # 降低膝关节偏移 + self.step_offset_ankle = 0.25 + self.walk_start_time = None + self.filter_alpha = 0.2 # 关节目标低通滤波系数 + + # 力矩输出滤波:减少力矩突变 + self.torque_prev = np.zeros(self.num_joints) + self.torque_filter_alpha = 0.15 + + # 初始化稳定姿态 self._init_stable_pose() def _init_stable_pose(self): - """修复:调整关节角度,让脚掌完全落地,重心前移""" + """优化初始姿态:更贴近自然站立,降低初始冲击""" mujoco.mj_resetData(self.model, self.data) - # 躯干初始位置(更低+更稳) - self.data.qpos[2] = 0.78 # 初始高度匹配重心目标 + # 躯干初始位置:略高+零偏航,减少初始调整 + self.data.qpos[2] = 1.3 # 初始高度略高,落地更平稳 self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] self.data.qvel[:] = 0.0 self.data.xfrc_applied[:] = 0.0 - # 核心修复:腿部关节角度(避免踮脚/后仰) - # 左腿关节(脚掌完全落地,无背屈) - self.joint_targets[self.joint_name_to_idx["left_hip_pitch_joint"]] = 0.1 # 降低髋俯仰,避免重心后移 - self.joint_targets[self.joint_name_to_idx["left_hip_roll_joint"]] = 0.02 - self.joint_targets[self.joint_name_to_idx["left_hip_yaw_joint"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["left_knee_joint"]] = -0.4 # 膝盖稍弯,增加缓冲 - self.joint_targets[self.joint_name_to_idx["left_ankle_pitch_joint"]] = 0.0 # 踝关节0度,脚掌完全落地(核心) - self.joint_targets[self.joint_name_to_idx["left_ankle_roll_joint"]] = 0.0 - - # 右腿关节(镜像左腿) - self.joint_targets[self.joint_name_to_idx["right_hip_pitch_joint"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["right_hip_roll_joint"]] = -0.02 - self.joint_targets[self.joint_name_to_idx["right_hip_yaw_joint"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["right_knee_joint"]] = -0.4 - self.joint_targets[self.joint_name_to_idx["right_ankle_pitch_joint"]] = 0.0 # 踝关节0度(核心) - self.joint_targets[self.joint_name_to_idx["right_ankle_roll_joint"]] = 0.0 - - # 腰部/手臂(固定) - self.joint_targets[self.joint_name_to_idx["waist_yaw_joint"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["left_shoulder_pitch_joint"]] = 0.6 - self.joint_targets[self.joint_name_to_idx["left_elbow_joint"]] = 1.8 - self.joint_targets[self.joint_name_to_idx["right_shoulder_pitch_joint"]] = 0.6 - self.joint_targets[self.joint_name_to_idx["right_elbow_joint"]] = 1.8 + # 腰部关节:微前倾,匹配自然站立 + self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.05 # 微前倾,防止后仰 + self.joint_targets[self.joint_name_to_idx["abdomen_x"]] = 0.0 + + # 右腿关节:微弯,增加缓冲 + self.joint_targets[self.joint_name_to_idx["hip_x_right"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["hip_z_right"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 + self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 # 膝盖微弯,减少落地冲击 + self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["ankle_x_right"]] = 0.0 + + # 左腿关节:镜像右腿,保证对称 + self.joint_targets[self.joint_name_to_idx["hip_x_left"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["hip_z_left"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 + self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 + self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["ankle_x_left"]] = 0.0 + + # 手臂关节:自然下垂,减少干扰 + self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.2 + self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.6 + self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.2 + self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.6 mujoco.mj_forward(self.model, self.data) def _quat_to_euler_xyz(self, quat): - """纯Numpy四元数转欧拉角""" + """四元数转欧拉角(保留)""" w, x, y, z = quat - # Roll (X) sinr_cosp = 2 * (w * x + y * z) cosr_cosp = 1 - 2 * (x * x + y * y) roll = np.arctan2(sinr_cosp, cosr_cosp) - # Pitch (Y) sinp = 2 * (w * y - z * x) pitch = np.where(np.abs(sinp) >= 1, np.copysign(np.pi / 2, sinp), np.arcsin(sinp)) - # Yaw (Z) 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 _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) # 进一步限制最大倾角 + return np.clip(euler, -0.2, 0.2) # 更严格的倾角限制,防止过度倾斜 def _detect_foot_contact(self): - """优化接触检测,确保脚掌落地判定准确""" + """优化接触检测:5帧历史滤波+迟滞,避免频繁切换""" try: - left_foot_geoms = ["left_foot_1_col", "left_foot_2_col", "left_foot_3_col", "left_foot_4_col"] - right_foot_geoms = ["right_foot_1_col", "right_foot_2_col", "right_foot_3_col", "right_foot_4_col"] + left_foot_geoms = ["foot1_left", "foot2_left"] + right_foot_geoms = ["foot1_right", "foot2_right"] + # 计算当前接触力 left_force = 0.0 for geom_name in left_foot_geoms: geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) @@ -174,244 +189,270 @@ def _detect_foot_contact(self): mujoco.mj_contactForce(self.model, self.data, geom_id, force) right_force += np.linalg.norm(force[:3]) - # 迟滞优化:防止接触状态频繁切换 - 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 + # 1. 更新接触历史(滑动窗口) + self.foot_contact_history[0] = np.roll(self.foot_contact_history[0], -1) + self.foot_contact_history[1] = np.roll(self.foot_contact_history[1], -1) + self.foot_contact_history[0, -1] = 1 if right_force > self.foot_contact_threshold else 0 + self.foot_contact_history[1, -1] = 1 if left_force > self.foot_contact_threshold else 0 + + # 2. 多数表决:5帧中≥3帧接触则判定为支撑 + self.foot_contact[0] = 1 if np.sum(self.foot_contact_history[0]) >= 3 else 0 + self.foot_contact[1] = 1 if np.sum(self.foot_contact_history[1]) >= 3 else 0 except Exception as e: print(f"接触检测警告: {e}") self.foot_contact = np.ones(2) + self.foot_contact_history = np.ones((2, 5)) - # 【行走功能新增】更新步态相位 def _update_gait_phase(self): - """更新步态相位,确保初始稳定后触发行走""" + """步态相位更新(保留)""" if self.walk_start_time is None: - self.walk_start_time = self.data.time # 标记行走开始时间 - # 计算归一化步态相位(0-1循环) + self.walk_start_time = self.data.time self.gait_phase = (self.data.time - self.walk_start_time) % self.gait_cycle / self.gait_cycle - # 【行走功能新增】生成行走的关节目标角度 def _update_walk_joint_targets(self): - """基于步态相位更新关节目标角度(核心行走逻辑)""" - # 仅在初始稳定期结束后启动行走 + """优化步态规划:余弦插值+动态重心,动作更平滑""" if self.data.time < self.init_wait_time: - return # 初始稳定期不修改目标角度 + # 初始稳定阶段:渐进式提升目标,避免突变 + alpha = min(1.0, (self.data.time) / self.init_wait_time) + self.joint_targets = self.joint_targets * alpha + self.joint_targets_prev * (1 - alpha) + # ========== 关键修复2:初始阶段保持com_target为基础值 ========== + self.com_target = self.com_base_target.copy() + return self._update_gait_phase() - phase = self.gait_phase # 0-1 + phase = self.gait_phase + + # 核心优化:使用余弦插值代替正弦,加速度更平滑(减少冲击) + phase_rad = phase * 2 * np.pi + cos_phase = (1 - np.cos(phase_rad)) / 2 # 0→1→0,无加速度突变 - # ========== 左腿/右腿交替摆动逻辑 ========== + # 动态重心目标:随步态相位前移/后移,匹配迈步节奏 if phase < 0.5: - # 0-0.5周期:右腿摆动(向前迈),左腿支撑 - # 右腿髋关节:向前摆(正弦曲线,0→最大值→0) - right_hip_pitch = 0.1 + self.step_offset_hip * np.sin(phase * 2 * np.pi) - # 右腿膝关节:弯曲抬腿(正弦曲线,0→最小值→0) - right_knee = -0.4 - self.step_offset_knee * np.sin(phase * 2 * np.pi) - # 右腿踝关节:协调(随膝关节联动) - right_ankle = 0.0 + self.step_offset_ankle * np.sin(phase * 2 * np.pi) - - # 更新右腿目标角度 - self.joint_targets[self.joint_name_to_idx["right_hip_pitch_joint"]] = right_hip_pitch - self.joint_targets[self.joint_name_to_idx["right_knee_joint"]] = right_knee - self.joint_targets[self.joint_name_to_idx["right_ankle_pitch_joint"]] = right_ankle - - # 左腿保持支撑姿态(小幅调整平衡) - self.joint_targets[self.joint_name_to_idx["left_hip_pitch_joint"]] = 0.1 - self.step_offset_hip * 0.2 - self.joint_targets[self.joint_name_to_idx["left_knee_joint"]] = -0.4 + self.step_offset_knee * 0.1 + # 右腿摆动:重心小幅右移+前移 + self.com_target = self.com_base_target + np.array([0.05, 0.03, 0.0]) + # 右腿关节目标:余弦插值,动作更平滑 + right_hip_yaw = 0.05 + self.step_offset_hip * cos_phase + right_knee = -0.35 - self.step_offset_knee * cos_phase + right_ankle = 0.0 + self.step_offset_ankle * cos_phase + + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = right_hip_yaw + self.joint_targets[self.joint_name_to_idx["knee_right"]] = right_knee + self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = right_ankle + + # 左腿支撑:微调整,保持平衡 + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 - self.step_offset_hip * 0.1 + self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 + self.step_offset_knee * 0.05 else: - # 0.5-1.0周期:左腿摆动(向前迈),右腿支撑 - # 左腿髋关节:向前摆 - left_hip_pitch = 0.1 + self.step_offset_hip * np.sin((phase - 0.5) * 2 * np.pi) - # 左腿膝关节:弯曲抬腿 - left_knee = -0.4 - self.step_offset_knee * np.sin((phase - 0.5) * 2 * np.pi) - # 左腿踝关节:协调 - left_ankle = 0.0 + self.step_offset_ankle * np.sin((phase - 0.5) * 2 * np.pi) - - # 更新左腿目标角度 - self.joint_targets[self.joint_name_to_idx["left_hip_pitch_joint"]] = left_hip_pitch - self.joint_targets[self.joint_name_to_idx["left_knee_joint"]] = left_knee - self.joint_targets[self.joint_name_to_idx["left_ankle_pitch_joint"]] = left_ankle - - # 右腿保持支撑姿态 - self.joint_targets[self.joint_name_to_idx["right_hip_pitch_joint"]] = 0.1 - self.step_offset_hip * 0.2 - self.joint_targets[self.joint_name_to_idx["right_knee_joint"]] = -0.4 + self.step_offset_knee * 0.1 + # 左腿摆动:重心小幅左移+前移 + self.com_target = self.com_base_target + np.array([0.05, -0.03, 0.0]) + # 左腿关节目标:余弦插值 + left_phase_rad = (phase - 0.5) * 2 * np.pi + left_cos_phase = (1 - np.cos(left_phase_rad)) / 2 + left_hip_yaw = 0.05 + self.step_offset_hip * left_cos_phase + left_knee = -0.35 - self.step_offset_knee * left_cos_phase + left_ankle = 0.0 + self.step_offset_ankle * left_cos_phase + + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = left_hip_yaw + self.joint_targets[self.joint_name_to_idx["knee_left"]] = left_knee + self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = left_ankle + + # 右腿支撑:微调整 + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 - self.step_offset_hip * 0.1 + self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 + self.step_offset_knee * 0.05 + + # 关节目标低通滤波:平滑目标值,减少抖动 + self.joint_targets = self.filter_alpha * self.joint_targets + (1 - self.filter_alpha) * self.joint_targets_prev + self.joint_targets_prev = self.joint_targets.copy() def _calculate_stabilizing_torques(self): - """优化力矩计算,抑制踮脚/后仰 + 集成行走控制""" - # 【新增】先更新行走的关节目标角度 + """优化力矩计算:动态增益+积分分离+速度反馈""" self._update_walk_joint_targets() - torques = np.zeros(self.num_joints, dtype=np.float64) - # 1. 躯干姿态控制(更保守) + # 躯干姿态控制:积分分离(核心防积分饱和) 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) # 严格限制躯干角速度 + root_vel = np.clip(root_vel, -2.0, 2.0) # 更严格的速度限制 + # 积分分离:误差超过死区时不积分 roll_error = -root_euler[0] - self.integral_roll += roll_error * self.dt + if abs(roll_error) > self.integral_deadband: + self.integral_roll = 0.0 + else: + self.integral_roll += roll_error * self.dt self.integral_roll = np.clip(self.integral_roll, -self.integral_limit, self.integral_limit) - roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 5.0 * self.integral_roll + roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 3.0 * self.integral_roll pitch_error = -root_euler[1] - self.integral_pitch += pitch_error * self.dt + if abs(pitch_error) > self.integral_deadband: + self.integral_pitch = 0.0 + else: + self.integral_pitch += pitch_error * self.dt self.integral_pitch = np.clip(self.integral_pitch, -self.integral_limit, self.integral_limit) - pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 4.0 * self.integral_pitch + pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 2.0 * self.integral_pitch yaw_error = -root_euler[2] yaw_torque = self.kp_yaw * yaw_error + self.kd_yaw * (-root_vel[2]) torso_torque = np.array([roll_torque, pitch_torque, yaw_torque]) - torso_torque = np.clip(torso_torque, -30.0, 30.0) # 降低躯干修正力矩上限 + torso_torque = np.clip(torso_torque, -25.0, 25.0) # 降低躯干力矩上限,减少抖动 - # 2. 重心补偿(更柔和) + # 重心补偿:新增速度反馈,抑制重心波动 com = self.data.subtree_com[0].astype(np.float64).copy() + self.com_vel = (com - self.prev_com) / self.dt # 计算重心速度 + self.com_vel = np.clip(self.com_vel, -0.5, 0.5) # 速度限幅 com_error = self.com_target - com - com_error = np.clip(com_error, -0.03, 0.03) - com_compensation = self.kp_com * com_error + com_error = np.clip(com_error, -0.05, 0.05) + # 比例+微分补偿,减少重心震荡 + com_compensation = self.kp_com * com_error + self.kd_com * (-self.com_vel) - # 3. 关节控制(重点优化踝关节) + # 关节控制:动态增益(支撑/摆动腿区分) 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) # 限制关节速度 + current_vel = np.clip(current_vel, -5.0, 5.0) # 降低关节速度上限 - # 腿部关节控制 + # 腰部关节控制(小幅调整) + waist_joints = ["abdomen_z", "abdomen_y", "abdomen_x"] + for joint_name in waist_joints: + idx = self.joint_name_to_idx[joint_name] + joint_error = self.joint_targets[idx] - current_joints[idx] + joint_error = np.clip(joint_error, -0.2, 0.2) + torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx] + + # 腿部关节控制:动态增益(核心优化) leg_joints = [ - "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", - "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", - "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", - "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint" + "hip_x_right", "hip_z_right", "hip_y_right", + "knee_right", "ankle_y_right", "ankle_x_right", + "hip_x_left", "hip_z_left", "hip_y_left", + "knee_left", "ankle_y_left", "ankle_x_left" ] for joint_name in leg_joints: idx = self.joint_name_to_idx[joint_name] joint_error = self.joint_targets[idx] - current_joints[idx] - joint_error = np.clip(joint_error, -0.3, 0.3) # 【放宽】关节误差限制,允许更大动作 + joint_error = np.clip(joint_error, -0.2, 0.2) - # 关节增益匹配 - if "hip" in joint_name: - kp = self.kp_hip - kd = self.kd_hip - # 核心:降低髋关节俯仰的姿态补偿,避免后仰 - if "pitch" in joint_name: - if "left" in joint_name: - joint_error += torso_torque[1] * 0.02 # 大幅降低补偿系数 - else: - joint_error += torso_torque[1] * 0.02 + # 判断是支撑腿还是摆动腿,分配不同增益 + is_support = False + if "right" in joint_name and self.foot_contact[0] == 1: + is_support = True + elif "left" in joint_name and self.foot_contact[1] == 1: + is_support = True + # 动态分配增益 + if "hip" in joint_name: + kp = self.kp_hip_support if is_support else self.kp_hip_swing + kd = self.kd_hip_support if is_support else self.kd_hip_swing + if "y" in joint_name: + joint_error += torso_torque[1] * 0.01 # 降低姿态补偿,减少后仰 elif "knee" in joint_name: - kp = self.kp_knee - kd = self.kd_knee - joint_error += com_compensation[2] * 0.05 - + kp = self.kp_knee_support if is_support else self.kp_knee_swing + kd = self.kd_knee_support if is_support else self.kd_knee_swing + joint_error += com_compensation[2] * 0.03 # 降低重心补偿,减少抖动 elif "ankle" in joint_name: - kp = self.kp_ankle - kd = self.kd_ankle - # 核心:踝关节仅保留极小的姿态补偿,防止踮脚 - if "pitch" 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): - kp *= 0.8 # 原0.1→0.8,大幅提高摆动腿增益 - kd *= 0.9 + kp = self.kp_ankle_support if is_support else self.kp_ankle_swing + kd = self.kd_ankle_support if is_support else self.kd_ankle_swing + if "y" in joint_name: + joint_error += torso_torque[1] * 0.005 # 极小的姿态补偿,防踮脚 torques[idx] = kp * joint_error - kd * current_vel[idx] - # 腰部/手臂控制(几乎无动作) - waist_joint = "waist_yaw_joint" - idx = self.joint_name_to_idx[waist_joint] - joint_error = self.joint_targets[idx] - current_joints[idx] - torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx] - + # 手臂关节控制(保留,小幅调整) arm_joints = [ - "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", - "left_elbow_joint", "left_wrist_roll_joint", "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", - "right_wrist_roll_joint" + "shoulder1_right", "shoulder2_right", "elbow_right", + "shoulder1_left", "shoulder2_left", "elbow_left" ] for joint_name in arm_joints: idx = self.joint_name_to_idx[joint_name] joint_error = self.joint_targets[idx] - current_joints[idx] torques[idx] = self.kp_arm * joint_error - self.kd_arm * current_vel[idx] - # 力矩限幅(【放宽】腿部力矩限制,确保能驱动行走) + # 力矩限幅:根据支撑/摆动调整,支撑腿更高 torque_limits = { - "left_hip_pitch_joint": 150, "left_hip_roll_joint": 150, "left_hip_yaw_joint": 150, # 原70→150 - "left_knee_joint": 200, "left_ankle_pitch_joint": 120, "left_ankle_roll_joint": 100, # 原100→200 - "right_hip_pitch_joint": 150, "right_hip_roll_joint": 150, "right_hip_yaw_joint": 150, - "right_knee_joint": 200, "right_ankle_pitch_joint": 120, "right_ankle_roll_joint": 100, - "waist_yaw_joint": 50, - "left_shoulder_pitch_joint": 10, "left_shoulder_roll_joint": 10, - "left_shoulder_yaw_joint": 10, "left_elbow_joint": 10, "left_wrist_roll_joint": 10, - "right_shoulder_pitch_joint": 10, "right_shoulder_roll_joint": 10, - "right_shoulder_yaw_joint": 10, "right_elbow_joint": 10, "right_wrist_roll_joint": 10 + "abdomen_z": 60, "abdomen_y": 60, "abdomen_x": 60, + "hip_x_right": 180 if self.foot_contact[0] == 1 else 100, + "hip_z_right": 180 if self.foot_contact[0] == 1 else 100, + "hip_y_right": 180 if self.foot_contact[0] == 1 else 100, + "knee_right": 220 if self.foot_contact[0] == 1 else 120, + "ankle_y_right": 150 if self.foot_contact[0] == 1 else 80, + "ankle_x_right": 120 if self.foot_contact[0] == 1 else 70, + "hip_x_left": 180 if self.foot_contact[1] == 1 else 100, + "hip_z_left": 180 if self.foot_contact[1] == 1 else 100, + "hip_y_left": 180 if self.foot_contact[1] == 1 else 100, + "knee_left": 220 if self.foot_contact[1] == 1 else 120, + "ankle_y_left": 150 if self.foot_contact[1] == 1 else 80, + "ankle_x_left": 120 if self.foot_contact[1] == 1 else 70, + "shoulder1_right": 25, "shoulder2_right": 25, "elbow_right": 25, + "shoulder1_left": 25, "shoulder2_left": 25, "elbow_left": 25 } for joint_name, limit in torque_limits.items(): 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: + # 力矩输出低通滤波:减少力矩突变,提升平滑性 + torques = self.torque_filter_alpha * torques + (1 - self.torque_filter_alpha) * self.torque_prev + self.torque_prev = torques.copy() + + # 调试输出(保留) + if self.data.time % 2 < 0.1 and self.data.time > self.init_wait_time: print(f"=== 行走调试 ===") - print(f"步态相位: {self.gait_phase:.2f}") - print(f"右腿髋俯仰目标: {self.joint_targets[self.joint_name_to_idx['right_hip_pitch_joint']]:.2f}") - print(f"左腿髋俯仰目标: {self.joint_targets[self.joint_name_to_idx['left_hip_pitch_joint']]:.2f}") + print(f"步态相位: {self.gait_phase:.2f} | 重心: {com[0]:.3f}/{com[2]:.3f}m") print(f"右脚接触: {self.foot_contact[0]}, 左脚接触: {self.foot_contact[1]}") + print(f"躯干倾角: roll={root_euler[0]:.3f}, pitch={root_euler[1]:.3f}") self.prev_com = com return torques def simulate_stable_standing(self): - """运行稳定站立+行走仿真(原方法保留,新增行走逻辑)""" + """优化仿真循环:渐进式启动+更严格的跌倒判定""" 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] + # 优化相机视角:更清晰观察行走 + v.cam.distance = 4.0 + v.cam.azimuth = 75 # 调整视角,避免遮挡 + v.cam.elevation = -20 + v.cam.lookat = [0, 0, 0.8] - print("G1机器人稳定站立+行走仿真启动...") + print("人形机器人高稳定性行走仿真启动...") print(f"初始稳定{self.init_wait_time}秒后自动开始行走") - # 初始落地阶段(更强阻尼) + # 初始稳定阶段:渐进式提升控制力度,避免冲击 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) torques = self._calculate_stabilizing_torques() * alpha self.data.ctrl[:] = torques mujoco.mj_step(self.model, self.data) - self.data.qvel[:] *= 0.97 # 更强的速度衰减,防止初始后仰 + self.data.qvel[:] *= 0.98 # 稍弱的速度衰减,避免过度制动 v.sync() - time.sleep(self.dt) + time.sleep(self.dt * 0.8) # 稍快的仿真,提升流畅度 - # 主仿真循环(行走阶段) - print("=== 开始行走 ===") + # 主仿真循环:更严格的跌倒判定 + 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() print( - f"时间:{self.data.time:.1f}s | 重心(x/z):{com[0]:.3f}/{com[2]:.3f}m | " + f"时间:{self.data.time:.1f}s | 重心(x/z/vel):{com[0]:.3f}/{com[2]:.3f}/{self.com_vel[0]:.3f}m/s | " f"姿态(roll/pitch):{euler[0]:.3f}/{euler[1]:.3f}rad | 脚接触:{self.foot_contact}" ) v.sync() - time.sleep(self.dt * 0.5) + time.sleep(self.dt * 0.6) - # 跌倒判定 + # 优化跌倒判定:更严格的阈值,提前预警 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: + if com[2] < 0.6 or abs(euler[0]) > 0.4 or abs(euler[1]) > 0.4: print( - f"跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " + f"⚠️ 跌倒预警/跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad" ) break @@ -421,17 +462,16 @@ def simulate_stable_standing(self): if __name__ == "__main__": current_directory = os.path.dirname(os.path.abspath(__file__)) - model_file_path = os.path.join(current_directory, "g1_23dof.xml") + model_file_path = os.path.join(current_directory, "humanoid.xml") print(f"模型路径:{model_file_path}") if not os.path.exists(model_file_path): raise FileNotFoundError(f"模型文件不存在:{model_file_path}") try: - stabilizer = G1Stabilizer(model_file_path) - stabilizer.simulate_stable_standing() # 原方法不变,内部已集成行走 + stabilizer = Stabilizer(model_file_path) + stabilizer.simulate_stable_standing() except Exception as e: print(f"错误:{e}") import traceback - traceback.print_exc() \ No newline at end of file From 38e46b29eff2e8e6f30c60aeb64abeabcdc74739 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Thu, 18 Dec 2025 19:50:59 +0800 Subject: [PATCH 2/7] =?UTF-8?q?=E6=9C=BA=E5=99=A8=E4=BA=BA=E8=BF=90?= =?UTF-8?q?=E5=8A=A8=E8=87=AA=E7=94=B1=E5=BA=A6=E8=A1=A5=E5=85=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 591 +++++++++++++++++++++----------------- 1 file changed, 335 insertions(+), 256 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index b0b29ef42e..72e6e6d43a 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -3,12 +3,107 @@ from mujoco import viewer import time import os - - -class Stabilizer: +import sys +import threading + +# 新增:键盘输入监听线程(用于手动控制状态/转向/速度,不阻塞仿真循环) +class KeyboardInputHandler(threading.Thread): + def __init__(self, stabilizer): + super().__init__(daemon=True) + self.stabilizer = stabilizer + self.running = True + + 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": + import msvcrt + if msvcrt.kbhit(): + 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() + self._handle_key(key) + 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}") + 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}") + +# 新增: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.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): + """更新CPG状态,返回关节目标偏移量""" + # 范德波尔振荡器方程(生物节律更自然,抗干扰) + 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.phase = 0.0 if self.base_phase == 0.0 else np.pi + self.state = np.array([np.sin(self.phase), np.cos(self.phase)]) + + @property + def base_phase(self): + return 0.0 if self.phase < np.pi else np.pi + +class HumanoidStabilizer: + """适配humanoid.xml模型的稳定站立与行走控制器(新增转向/变速/状态机)""" def __init__(self, model_path): - # 基础初始化(保留原有核心逻辑) + # 类型检查与模型加载(原始逻辑完全保留) if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") @@ -18,15 +113,15 @@ def __init__(self, model_path): except Exception as e: raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性") - # 1. 仿真参数优化:固定时间步+更保守的初始稳定 + # 仿真核心参数(原始逻辑保留) self.sim_duration = 120.0 - self.dt = 0.005 # 固定0.005s步长,提升仿真稳定性 - self.model.opt.timestep = self.dt - self.init_wait_time = 5.0 # 延长初始稳定时间至5s + self.dt = self.model.opt.timestep + self.init_wait_time = 4.0 self.model.opt.gravity[2] = -9.81 - self.model.opt.iterations = 300 # 增加迭代次数提升接触求解精度 - self.model.opt.tolerance = 1e-9 # 降低容差,提升计算精度 + self.model.opt.iterations = 200 + self.model.opt.tolerance = 1e-8 + # 关节名称映射(原始逻辑完全保留) self.joint_names = [ "abdomen_z", "abdomen_y", "abdomen_x", "hip_x_right", "hip_z_right", "hip_y_right", @@ -39,117 +134,182 @@ 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增益优化:分阶段动态增益(核心) - # 基础增益(支撑腿) - self.kp_roll = 150.0 # 提高侧倾增益,增强横向稳定 - self.kd_roll = 50.0 - self.kp_pitch = 130.0 # 提高俯仰增益,抑制后仰 - self.kd_pitch = 45.0 - self.kp_yaw = 40.0 - self.kd_yaw = 20.0 - - # 腿部增益(分支撑/摆动) - self.kp_hip_support = 300.0 # 支撑腿髋关节高增益 - self.kd_hip_support = 60.0 - self.kp_hip_swing = 150.0 # 摆动腿髋关节低增益,避免过度摆动 - self.kd_hip_swing = 30.0 - - self.kp_knee_support = 320.0 # 支撑腿膝关节高增益 - self.kd_knee_support = 70.0 - self.kp_knee_swing = 180.0 # 摆动腿膝关节低增益 - self.kd_knee_swing = 40.0 - - self.kp_ankle_support = 250.0 # 支撑腿踝关节高增益(核心防踮脚) - self.kd_ankle_support = 80.0 - self.kp_ankle_swing = 120.0 # 摆动腿踝关节低增益 - self.kd_ankle_swing = 40.0 - - # 腰部/手臂增益(小幅提升) - self.kp_waist = 50.0 - self.kd_waist = 25.0 - self.kp_arm = 25.0 - self.kd_arm = 25.0 - - # 重心控制优化:动态目标+速度反馈 - self.com_base_target = np.array([0.0, 0.0, 0.85]) # 基础重心高度提升,降低跌倒风险 - # ========== 关键修复1:初始化com_target,避免属性不存在 ========== - self.com_target = self.com_base_target.copy() # 初始赋值,与基础重心一致 - self.com_gait_offset = np.array([0.08, 0.0, 0.02]) # 步态相位对应的重心偏移 - self.kp_com = 80.0 # 提高重心比例增益 - self.kd_com = 30.0 # 新增重心速度阻尼 - self.foot_contact_threshold = 2.0 # 提高接触阈值,减少误判 - - # 状态变量优化:新增滤波和平滑参数 + # 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.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 + + # 状态变量(原始逻辑保留,新增状态机/CPG/转向/变速参数) self.joint_targets = np.zeros(self.num_joints) - self.joint_targets_prev = np.zeros(self.num_joints) # 上一帧关节目标(滤波用) self.prev_com = np.zeros(3) - self.com_vel = np.zeros(3) # 重心速度 - self.foot_contact = np.zeros(2) # [right, left] - self.foot_contact_history = np.zeros((2, 5)) # 接触状态历史(5帧滤波) + self.foot_contact = np.zeros(2) self.integral_roll = 0.0 self.integral_pitch = 0.0 - self.integral_limit = 0.1 # 进一步收紧积分限幅 - self.integral_deadband = 0.01 # 积分死区,小误差不积分 - - # 步态规划优化:平滑插值+慢周期 - self.gait_phase = 0.0 - self.gait_cycle = 2.5 # 延长步态周期至2.5s,动作更平缓 - self.step_offset_hip = 0.35 # 降低髋关节偏移,减少重心波动 - self.step_offset_knee = 0.5 # 降低膝关节偏移 - self.step_offset_ankle = 0.25 - self.walk_start_time = None - self.filter_alpha = 0.2 # 关节目标低通滤波系数 + self.integral_limit = 0.15 + + # 新增:运动状态机(核心) + self.state = "STAND" # 初始状态:STAND/WALK/STOP/EMERGENCY + self.state_map = { + "STAND": self._state_stand, # 站立(初始稳定) + "WALK": self._state_walk, # 行走(CPG驱动) + "STOP": self._state_stop, # 停止(关节归零) + "EMERGENCY": self._state_emergency # 急停(力矩清零) + } - # 力矩输出滤波:减少力矩突变 - self.torque_prev = np.zeros(self.num_joints) - self.torque_filter_alpha = 0.15 + # 新增: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.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) + + # 行走功能参数(原始逻辑保留,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.3 # 初始高度略高,落地更平稳 + self.data.qpos[2] = 1.282 self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] self.data.qvel[:] = 0.0 self.data.xfrc_applied[:] = 0.0 - # 腰部关节:微前倾,匹配自然站立 + # 腰部关节 self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.05 # 微前倾,防止后仰 + self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.0 self.joint_targets[self.joint_name_to_idx["abdomen_x"]] = 0.0 - # 右腿关节:微弯,增加缓冲 + # 右腿关节 self.joint_targets[self.joint_name_to_idx["hip_x_right"]] = 0.0 self.joint_targets[self.joint_name_to_idx["hip_z_right"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 - self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 # 膝盖微弯,减少落地冲击 + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.1 + self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.4 self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 self.joint_targets[self.joint_name_to_idx["ankle_x_right"]] = 0.0 - # 左腿关节:镜像右腿,保证对称 + # 左腿关节 self.joint_targets[self.joint_name_to_idx["hip_x_left"]] = 0.0 self.joint_targets[self.joint_name_to_idx["hip_z_left"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 - self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.1 + self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.4 self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0 self.joint_targets[self.joint_name_to_idx["ankle_x_left"]] = 0.0 - # 手臂关节:自然下垂,减少干扰 - self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.2 - self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.6 - self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.2 - self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.6 + # 手臂关节 + self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.1 + self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.1 + self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.5 + self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.1 + self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.1 + self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.5 mujoco.mj_forward(self.model, self.data) + # 新增:状态机核心方法 + def _state_stand(self): + """站立状态:维持初始稳定姿态,CPG重置""" + self.right_leg_cpg.reset() + self.left_leg_cpg.reset() + self._init_stable_pose() # 恢复站立姿态 + + def _state_walk(self): + """行走状态:CPG驱动+转向+变速""" + if self.walk_start_time is None: + self.walk_start_time = self.data.time + # 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)由转向角度控制 + 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 + elif self.turn_angle < 0: # 右转 + 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. 更新关节目标(兼容原始关节映射) + 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["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 + + # 更新原始步态相位(兼容调试输出) + 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 # 速度阻尼 + + def _state_emergency(self): + """急停状态:力矩清零,速度归零""" + self.data.ctrl[:] = 0.0 + 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) # 限制转向角度,避免失衡 + + def set_walk_speed(self, speed): + 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) cosr_cosp = 1 - 2 * (x * x + y * y) @@ -161,20 +321,19 @@ 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.2, 0.2) # 更严格的倾角限制,防止过度倾斜 + return np.clip(euler, -0.3, 0.3) + # 原始方法:检测脚部接触(完全保留) def _detect_foot_contact(self): - """优化接触检测:5帧历史滤波+迟滞,避免频繁切换""" try: left_foot_geoms = ["foot1_left", "foot2_left"] right_foot_geoms = ["foot1_right", "foot2_right"] - # 计算当前接触力 left_force = 0.0 for geom_name in left_foot_geoms: geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) @@ -189,139 +348,63 @@ def _detect_foot_contact(self): mujoco.mj_contactForce(self.model, self.data, geom_id, force) right_force += np.linalg.norm(force[:3]) - # 1. 更新接触历史(滑动窗口) - self.foot_contact_history[0] = np.roll(self.foot_contact_history[0], -1) - self.foot_contact_history[1] = np.roll(self.foot_contact_history[1], -1) - self.foot_contact_history[0, -1] = 1 if right_force > self.foot_contact_threshold else 0 - self.foot_contact_history[1, -1] = 1 if left_force > self.foot_contact_threshold else 0 - - # 2. 多数表决:5帧中≥3帧接触则判定为支撑 - self.foot_contact[0] = 1 if np.sum(self.foot_contact_history[0]) >= 3 else 0 - self.foot_contact[1] = 1 if np.sum(self.foot_contact_history[1]) >= 3 else 0 + 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 except Exception as e: print(f"接触检测警告: {e}") self.foot_contact = np.ones(2) - self.foot_contact_history = np.ones((2, 5)) - - def _update_gait_phase(self): - """步态相位更新(保留)""" - if self.walk_start_time is None: - self.walk_start_time = self.data.time - self.gait_phase = (self.data.time - self.walk_start_time) % self.gait_cycle / self.gait_cycle - - def _update_walk_joint_targets(self): - """优化步态规划:余弦插值+动态重心,动作更平滑""" - if self.data.time < self.init_wait_time: - # 初始稳定阶段:渐进式提升目标,避免突变 - alpha = min(1.0, (self.data.time) / self.init_wait_time) - self.joint_targets = self.joint_targets * alpha + self.joint_targets_prev * (1 - alpha) - # ========== 关键修复2:初始阶段保持com_target为基础值 ========== - self.com_target = self.com_base_target.copy() - return - - self._update_gait_phase() - phase = self.gait_phase - - # 核心优化:使用余弦插值代替正弦,加速度更平滑(减少冲击) - phase_rad = phase * 2 * np.pi - cos_phase = (1 - np.cos(phase_rad)) / 2 # 0→1→0,无加速度突变 - - # 动态重心目标:随步态相位前移/后移,匹配迈步节奏 - if phase < 0.5: - # 右腿摆动:重心小幅右移+前移 - self.com_target = self.com_base_target + np.array([0.05, 0.03, 0.0]) - # 右腿关节目标:余弦插值,动作更平滑 - right_hip_yaw = 0.05 + self.step_offset_hip * cos_phase - right_knee = -0.35 - self.step_offset_knee * cos_phase - right_ankle = 0.0 + self.step_offset_ankle * cos_phase - - self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = right_hip_yaw - self.joint_targets[self.joint_name_to_idx["knee_right"]] = right_knee - self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = right_ankle - - # 左腿支撑:微调整,保持平衡 - self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 - self.step_offset_hip * 0.1 - self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 + self.step_offset_knee * 0.05 - else: - # 左腿摆动:重心小幅左移+前移 - self.com_target = self.com_base_target + np.array([0.05, -0.03, 0.0]) - # 左腿关节目标:余弦插值 - left_phase_rad = (phase - 0.5) * 2 * np.pi - left_cos_phase = (1 - np.cos(left_phase_rad)) / 2 - left_hip_yaw = 0.05 + self.step_offset_hip * left_cos_phase - left_knee = -0.35 - self.step_offset_knee * left_cos_phase - left_ankle = 0.0 + self.step_offset_ankle * left_cos_phase - - self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = left_hip_yaw - self.joint_targets[self.joint_name_to_idx["knee_left"]] = left_knee - self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = left_ankle - - # 右腿支撑:微调整 - self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 - self.step_offset_hip * 0.1 - self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 + self.step_offset_knee * 0.05 - - # 关节目标低通滤波:平滑目标值,减少抖动 - self.joint_targets = self.filter_alpha * self.joint_targets + (1 - self.filter_alpha) * self.joint_targets_prev - self.joint_targets_prev = self.joint_targets.copy() + # 原始方法:计算稳定力矩(修改:加入状态机逻辑,保留所有原始PD/补偿逻辑) def _calculate_stabilizing_torques(self): - """优化力矩计算:动态增益+积分分离+速度反馈""" - self._update_walk_joint_targets() + # 状态机驱动:根据当前状态更新关节目标 + 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, -2.0, 2.0) # 更严格的速度限制 + root_vel = np.clip(root_vel, -3.0, 3.0) - # 积分分离:误差超过死区时不积分 roll_error = -root_euler[0] - if abs(roll_error) > self.integral_deadband: - self.integral_roll = 0.0 - else: - self.integral_roll += roll_error * self.dt + self.integral_roll += roll_error * self.dt self.integral_roll = np.clip(self.integral_roll, -self.integral_limit, self.integral_limit) - roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 3.0 * self.integral_roll + roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 5.0 * self.integral_roll pitch_error = -root_euler[1] - if abs(pitch_error) > self.integral_deadband: - self.integral_pitch = 0.0 - else: - self.integral_pitch += pitch_error * self.dt + self.integral_pitch += pitch_error * self.dt self.integral_pitch = np.clip(self.integral_pitch, -self.integral_limit, self.integral_limit) - pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 2.0 * self.integral_pitch + pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 4.0 * self.integral_pitch yaw_error = -root_euler[2] yaw_torque = self.kp_yaw * yaw_error + self.kd_yaw * (-root_vel[2]) torso_torque = np.array([roll_torque, pitch_torque, yaw_torque]) - torso_torque = np.clip(torso_torque, -25.0, 25.0) # 降低躯干力矩上限,减少抖动 + torso_torque = np.clip(torso_torque, -30.0, 30.0) - # 重心补偿:新增速度反馈,抑制重心波动 + # 重心补偿 com = self.data.subtree_com[0].astype(np.float64).copy() - self.com_vel = (com - self.prev_com) / self.dt # 计算重心速度 - self.com_vel = np.clip(self.com_vel, -0.5, 0.5) # 速度限幅 com_error = self.com_target - com - com_error = np.clip(com_error, -0.05, 0.05) - # 比例+微分补偿,减少重心震荡 - com_compensation = self.kp_com * com_error + self.kd_com * (-self.com_vel) + com_error = np.clip(com_error, -0.03, 0.03) + com_compensation = self.kp_com * com_error - # 关节控制:动态增益(支撑/摆动腿区分) + # 关节控制 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, -5.0, 5.0) # 降低关节速度上限 + 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] joint_error = self.joint_targets[idx] - current_joints[idx] - joint_error = np.clip(joint_error, -0.2, 0.2) + joint_error = np.clip(joint_error, -0.3, 0.3) torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx] - # 腿部关节控制:动态增益(核心优化) + # 腿部关节控制 leg_joints = [ "hip_x_right", "hip_z_right", "hip_y_right", "knee_right", "ankle_y_right", "ankle_x_right", @@ -332,34 +415,36 @@ def _calculate_stabilizing_torques(self): for joint_name in leg_joints: idx = self.joint_name_to_idx[joint_name] joint_error = self.joint_targets[idx] - current_joints[idx] - joint_error = np.clip(joint_error, -0.2, 0.2) + joint_error = np.clip(joint_error, -0.3, 0.3) - # 判断是支撑腿还是摆动腿,分配不同增益 - is_support = False - if "right" in joint_name and self.foot_contact[0] == 1: - is_support = True - elif "left" in joint_name and self.foot_contact[1] == 1: - is_support = True - - # 动态分配增益 if "hip" in joint_name: - kp = self.kp_hip_support if is_support else self.kp_hip_swing - kd = self.kd_hip_support if is_support else self.kd_hip_swing + kp = self.kp_hip + kd = self.kd_hip if "y" in joint_name: - joint_error += torso_torque[1] * 0.01 # 降低姿态补偿,减少后仰 + if "right" in joint_name: + joint_error += torso_torque[1] * 0.02 + else: + joint_error += torso_torque[1] * 0.02 + elif "knee" in joint_name: - kp = self.kp_knee_support if is_support else self.kp_knee_swing - kd = self.kd_knee_support if is_support else self.kd_knee_swing - joint_error += com_compensation[2] * 0.03 # 降低重心补偿,减少抖动 + kp = self.kp_knee + kd = self.kd_knee + joint_error += com_compensation[2] * 0.05 + elif "ankle" in joint_name: - kp = self.kp_ankle_support if is_support else self.kp_ankle_swing - kd = self.kd_ankle_support if is_support else self.kd_ankle_swing + kp = self.kp_ankle + kd = self.kd_ankle if "y" in joint_name: - joint_error += torso_torque[1] * 0.005 # 极小的姿态补偿,防踮脚 + 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): + 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" @@ -369,93 +454,87 @@ 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": 60, "abdomen_y": 60, "abdomen_x": 60, - "hip_x_right": 180 if self.foot_contact[0] == 1 else 100, - "hip_z_right": 180 if self.foot_contact[0] == 1 else 100, - "hip_y_right": 180 if self.foot_contact[0] == 1 else 100, - "knee_right": 220 if self.foot_contact[0] == 1 else 120, - "ankle_y_right": 150 if self.foot_contact[0] == 1 else 80, - "ankle_x_right": 120 if self.foot_contact[0] == 1 else 70, - "hip_x_left": 180 if self.foot_contact[1] == 1 else 100, - "hip_z_left": 180 if self.foot_contact[1] == 1 else 100, - "hip_y_left": 180 if self.foot_contact[1] == 1 else 100, - "knee_left": 220 if self.foot_contact[1] == 1 else 120, - "ankle_y_left": 150 if self.foot_contact[1] == 1 else 80, - "ankle_x_left": 120 if self.foot_contact[1] == 1 else 70, - "shoulder1_right": 25, "shoulder2_right": 25, "elbow_right": 25, - "shoulder1_left": 25, "shoulder2_left": 25, "elbow_left": 25 + "abdomen_z": 50, "abdomen_y": 50, "abdomen_x": 50, + "hip_x_right": 150, "hip_z_right": 150, "hip_y_right": 150, + "knee_right": 200, "ankle_y_right": 120, "ankle_x_right": 100, + "hip_x_left": 150, "hip_z_left": 150, "hip_y_left": 150, + "knee_left": 200, "ankle_y_left": 120, "ankle_x_left": 100, + "shoulder1_right": 20, "shoulder2_right": 20, "elbow_right": 20, + "shoulder1_left": 20, "shoulder2_left": 20, "elbow_left": 20 } for joint_name, limit in torque_limits.items(): idx = self.joint_name_to_idx[joint_name] torques[idx] = np.clip(torques[idx], -limit, limit) - # 力矩输出低通滤波:减少力矩突变,提升平滑性 - torques = self.torque_filter_alpha * torques + (1 - self.torque_filter_alpha) * self.torque_prev - self.torque_prev = torques.copy() - - # 调试输出(保留) - if self.data.time % 2 < 0.1 and self.data.time > self.init_wait_time: + # 调试输出(保留原始逻辑,新增状态/速度/转向输出) + if self.data.time % 1 < 0.1 and self.data.time > self.init_wait_time: print(f"=== 行走调试 ===") - print(f"步态相位: {self.gait_phase:.2f} | 重心: {com[0]:.3f}/{com[2]:.3f}m") + 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"躯干倾角: roll={root_euler[0]:.3f}, pitch={root_euler[1]:.3f}") self.prev_com = com return torques + # 原始方法:仿真循环(修改:加入键盘监听线程,保留所有原始逻辑) def simulate_stable_standing(self): - """优化仿真循环:渐进式启动+更严格的跌倒判定""" + # 新增:启动键盘监听线程(控制状态/转向/速度) + keyboard_handler = KeyboardInputHandler(self) + keyboard_handler.start() + with viewer.launch_passive(self.model, self.data) as v: - # 优化相机视角:更清晰观察行走 - v.cam.distance = 4.0 - v.cam.azimuth = 75 # 调整视角,避免遮挡 - v.cam.elevation = -20 - v.cam.lookat = [0, 0, 0.8] + # 优化相机视角(原始逻辑保留) + v.cam.distance = 3.0 + v.cam.azimuth = 90 + v.cam.elevation = -25 + v.cam.lookat = [0, 0, 0.6] - print("人形机器人高稳定性行走仿真启动...") - print(f"初始稳定{self.init_wait_time}秒后自动开始行走") + print("人形机器人稳定站立+行走仿真启动...") + print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持转向/变速/启停/急停") - # 初始稳定阶段:渐进式提升控制力度,避免冲击 + # 初始落地阶段(原始逻辑保留) 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) torques = self._calculate_stabilizing_torques() * alpha self.data.ctrl[:] = torques mujoco.mj_step(self.model, self.data) - self.data.qvel[:] *= 0.98 # 稍弱的速度衰减,避免过度制动 + self.data.qvel[:] *= 0.97 v.sync() - time.sleep(self.dt * 0.8) # 稍快的仿真,提升流畅度 + time.sleep(self.dt) - # 主仿真循环:更严格的跌倒判定 - print("=== 开始稳定行走 ===") + # 主仿真循环(原始逻辑保留,状态机驱动) + 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() print( - f"时间:{self.data.time:.1f}s | 重心(x/z/vel):{com[0]:.3f}/{com[2]:.3f}/{self.com_vel[0]:.3f}m/s | " + 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 * 0.6) + time.sleep(self.dt * 0.5) - # 优化跌倒判定:更严格的阈值,提前预警 + # 跌倒判定(原始逻辑保留) com = self.data.subtree_com[0] euler = self._get_root_euler() - if com[2] < 0.6 or abs(euler[0]) > 0.4 or abs(euler[1]) > 0.4: + 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"跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad" ) - break + self.set_state("STAND") # 跌倒后自动恢复站立 + # break # 注释掉break,跌倒后可继续控制 print("仿真完成!") @@ -469,7 +548,7 @@ def simulate_stable_standing(self): raise FileNotFoundError(f"模型文件不存在:{model_file_path}") try: - stabilizer = Stabilizer(model_file_path) + stabilizer = HumanoidStabilizer(model_file_path) stabilizer.simulate_stable_standing() except Exception as e: print(f"错误:{e}") From c9b6b48771c8f753285d771a394bd80c9125cc07 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Thu, 18 Dec 2025 20:35:53 +0800 Subject: [PATCH 3/7] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=BA=E6=9C=BA?= =?UTF-8?q?=E9=80=9A=E4=BF=A1=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 82 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 78 insertions(+), 4 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 72e6e6d43a..0039466146 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -6,7 +6,74 @@ import sys import threading -# 新增:键盘输入监听线程(用于手动控制状态/转向/速度,不阻塞仿真循环) +# ===================== 新增:ROS话题接收模块(可选兼容,无ROS不报错) ===================== +class ROSCmdVelHandler(threading.Thread): + """ROS /cmd_vel话题接收线程,映射linear.x→速度,angular.z→转向""" + def __init__(self, stabilizer): + super().__init__(daemon=True) + self.stabilizer = stabilizer + self.running = True + self.has_ros = False + self.twist_msg = None + + # 尝试导入ROS库,无ROS则跳过 + try: + import rospy + from geometry_msgs.msg import Twist + self.rospy = rospy + self.Twist = Twist + self.has_ros = True + except ImportError: + print("[ROS提示] 未检测到ROS环境,跳过/cmd_vel话题监听(仅保留键盘控制)") + return + + # 初始化ROS节点 + try: + if not self.rospy.core.is_initialized(): + 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 + ) + print("[ROS提示] 已启动/cmd_vel话题监听:") + print(" - linear.x (0.1~1.0) → 行走速度(0.1=最慢,1.0=最快)") + print(" - angular.z (-1.0~1.0) → 转向角度(正=左转,负=右转,映射±0.3rad)") + except Exception as e: + print(f"[ROS提示] ROS节点初始化失败:{e}") + self.has_ros = False + + def _cmd_vel_callback(self, msg): + """回调函数:解析/cmd_vel并映射到速度/转向""" + self.twist_msg = msg + # 1. linear.x → 行走速度(限幅0.1~1.0) + target_speed = np.clip(msg.linear.x, 0.1, 1.0) + # 2. angular.z → 转向角度(-1.0~1.0映射到-0.3~0.3rad) + target_turn = np.clip(msg.angular.z, -1.0, 1.0) * 0.3 # 缩放系数0.3 + + # 更新到控制器(优先级高于键盘,避免冲突) + self.stabilizer.set_walk_speed(target_speed) + self.stabilizer.set_turn_angle(target_turn) + + # 自动触发行走(如果接收到速度指令且当前是停止状态) + if target_speed > 0.1 and self.stabilizer.state == "STAND": + 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") + + def run(self): + """线程主循环(ROS自旋)""" + if not self.has_ros: + return + while self.running and not self.rospy.is_shutdown(): + self.rospy.spin_once() + time.sleep(0.01) + + def stop(self): + self.running = False + +# ===================== 原始代码:键盘输入监听线程(完全保留) ===================== class KeyboardInputHandler(threading.Thread): def __init__(self, stabilizer): super().__init__(daemon=True) @@ -64,7 +131,7 @@ 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) @@ -99,6 +166,7 @@ def reset(self): def base_phase(self): return 0.0 if self.phase < np.pi else np.pi +# ===================== 原始代码:人形机器人控制器(完全保留,仅新增ROS线程启动) ===================== class HumanoidStabilizer: """适配humanoid.xml模型的稳定站立与行走控制器(新增转向/变速/状态机)""" @@ -479,9 +547,13 @@ def _calculate_stabilizing_torques(self): self.prev_com = com return torques - # 原始方法:仿真循环(修改:加入键盘监听线程,保留所有原始逻辑) + # 原始方法:仿真循环(修改:新增ROS线程启动,保留所有原始逻辑) def simulate_stable_standing(self): - # 新增:启动键盘监听线程(控制状态/转向/速度) + # 新增:启动ROS /cmd_vel监听线程 + self.ros_handler = ROSCmdVelHandler(self) + self.ros_handler.start() + + # 原始:启动键盘监听线程 keyboard_handler = KeyboardInputHandler(self) keyboard_handler.start() @@ -536,6 +608,8 @@ def simulate_stable_standing(self): self.set_state("STAND") # 跌倒后自动恢复站立 # break # 注释掉break,跌倒后可继续控制 + # 停止ROS线程 + self.ros_handler.stop() print("仿真完成!") From 20b6baf967e31c3827403b01b1a7631c124d64cf Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Fri, 19 Dec 2025 09:01:58 +0800 Subject: [PATCH 4/7] =?UTF-8?q?=E6=A0=B8=E5=BF=83=E9=B2=81=E6=A3=92?= =?UTF-8?q?=E6=80=A7=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 304 +++++++++++++++++++++++++------------- 1 file changed, 202 insertions(+), 102 deletions(-) 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 From 355ecec290618da854b0b61b9d5184190f05643a Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Fri, 19 Dec 2025 13:45:36 +0800 Subject: [PATCH 5/7] =?UTF-8?q?=E4=BC=A0=E6=84=9F=E5=99=A8=E6=A8=A1?= =?UTF-8?q?=E6=8B=9F=E6=A0=B8=E5=BF=83=E7=89=B9=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 294 ++++++++++++++++++++++++++++---------- 1 file changed, 216 insertions(+), 78 deletions(-) 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 From 010646757ac47834cde6a3239afac731bb468c27 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Fri, 19 Dec 2025 15:43:47 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=AD=A5=E6=80=81?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 250 ++++++++++++++++++++++++++++---------- 1 file changed, 186 insertions(+), 64 deletions(-) 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}") From 2f78fdbad61b73457168ab06d7bcbf464433cf0b Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Fri, 19 Dec 2025 16:54:31 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=A0=B8=E5=BF=83?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 504 +++++++++++++------------------------- 1 file changed, 166 insertions(+), 338 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index bdb58f0546..0bc74c4d91 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -8,10 +8,7 @@ from collections import deque -# ===================== ROS话题接收模块(原有逻辑完全保留) ===================== class ROSCmdVelHandler(threading.Thread): - """ROS /cmd_vel话题接收线程,映射linear.x→速度,angular.z→转向""" - def __init__(self, stabilizer): super().__init__(daemon=True) self.stabilizer = stabilizer @@ -19,7 +16,6 @@ def __init__(self, stabilizer): self.has_ros = False self.twist_msg = None - # 尝试导入ROS库,无ROS则跳过 try: import rospy from geometry_msgs.msg import Twist @@ -30,11 +26,9 @@ def __init__(self, stabilizer): print("[ROS提示] 未检测到ROS环境,跳过/cmd_vel话题监听(仅保留键盘控制)") return - # 初始化ROS节点 try: if not self.rospy.core.is_initialized(): 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 ) @@ -46,28 +40,21 @@ def __init__(self, stabilizer): self.has_ros = False def _cmd_vel_callback(self, msg): - """回调函数:解析/cmd_vel并映射到速度/转向""" self.twist_msg = msg - # 1. linear.x → 行走速度(限幅0.1~1.0) target_speed = np.clip(msg.linear.x, 0.1, 1.0) - # 2. angular.z → 转向角度(-1.0~1.0映射到-0.3~0.3rad) - target_turn = np.clip(msg.angular.z, -1.0, 1.0) * 0.3 # 缩放系数0.3 + target_turn = np.clip(msg.angular.z, -1.0, 1.0) * 0.3 - # 更新到控制器(优先级高于键盘,避免冲突) self.stabilizer.set_walk_speed(target_speed) self.stabilizer.set_turn_angle(target_turn) - # 自动触发行走(如果接收到速度指令且当前是停止状态) 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秒输出一次 + if self.stabilizer.data.time % 0.5 < 0.1: print( f"[ROS指令] 速度={target_speed:.2f} | 转向={target_turn:.2f}rad | 当前步态: {self.stabilizer.gait_mode}") def run(self): - """线程主循环(ROS自旋)""" if not self.has_ros: return while self.running and not self.rospy.is_shutdown(): @@ -78,7 +65,6 @@ def stop(self): self.running = False -# ===================== 键盘输入监听线程(新增步态切换按键) ===================== class KeyboardInputHandler(threading.Thread): def __init__(self, stabilizer): super().__init__(daemon=True) @@ -90,11 +76,10 @@ def run(self): print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") print("m: 传感器模拟开关 | p: 打印传感器数据") - print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步") # 新增步态切换 + print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步") print("========================\n") while self.running: try: - # 非阻塞键盘输入(兼容不同系统) if sys.platform == "win32": import msvcrt if msvcrt.kbhit(): @@ -110,96 +95,69 @@ def run(self): continue 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} | 转向: {self.stabilizer.turn_angle:.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} | 当前步态: {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} | 当前步态: {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"[指令] 切换为原地踏步模式 | 步幅减半,躯干锁定") + key_actions = { + 'w': lambda: (self.stabilizer.set_state("WALK"), self.stabilizer.set_gait_mode(self.stabilizer.gait_mode), + print( + f"[指令] 切换为行走状态 | 当前步态: {self.stabilizer.gait_mode} | 速度: {self.stabilizer.walk_speed:.2f} | 转向: {self.stabilizer.turn_angle:.2f}")), + 's': lambda: (self.stabilizer.set_state("STOP"), print("[指令] 切换为停止状态")), + 'e': lambda: (self.stabilizer.set_state("EMERGENCY"), print("[指令] 触发紧急停止")), + 'r': lambda: (self.stabilizer.set_state("STAND"), print("[指令] 恢复站立姿态")), + 'a': lambda: (self.stabilizer.set_turn_angle(self.stabilizer.turn_angle + 0.05), + print(f"[指令] 左转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad")), + 'd': lambda: (self.stabilizer.set_turn_angle(self.stabilizer.turn_angle - 0.05), + print(f"[指令] 右转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad")), + ' ': lambda: (self.stabilizer.set_turn_angle(0.2 if self.stabilizer.turn_angle <= 0 else -0.2), + print(f"[指令] 原地转向 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad")), + 'z': lambda: (self.stabilizer.set_walk_speed(self.stabilizer.walk_speed - 0.1), + print( + f"[指令] 减速 | 当前速度: {self.stabilizer.walk_speed:.2f} | 当前步态: {self.stabilizer.gait_mode}")), + 'x': lambda: (self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1), + print( + f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f} | 当前步态: {self.stabilizer.gait_mode}")), + 'm': lambda: ( + setattr(self.stabilizer, 'enable_sensor_simulation', not self.stabilizer.enable_sensor_simulation), + print(f"[指令] 传感器模拟{'开启' if self.stabilizer.enable_sensor_simulation else '关闭'}")), + 'p': lambda: self.stabilizer.print_sensor_data(), + '1': lambda: (self.stabilizer.set_gait_mode("SLOW"), + print( + f"[指令] 切换为慢走模式 | CPG频率: {self.stabilizer.gait_config['SLOW']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['SLOW']['amp']:.2f}")), + '2': lambda: (self.stabilizer.set_gait_mode("NORMAL"), + print( + f"[指令] 切换为正常走模式 | CPG频率: {self.stabilizer.gait_config['NORMAL']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['NORMAL']['amp']:.2f}")), + '3': lambda: (self.stabilizer.set_gait_mode("TROT"), + print( + f"[指令] 切换为小跑模式 | CPG频率: {self.stabilizer.gait_config['TROT']['freq']:.2f}Hz | 振幅: {self.stabilizer.gait_config['TROT']['amp']:.2f}")), + '4': lambda: (self.stabilizer.set_gait_mode("STEP_IN_PLACE"), + print(f"[指令] 切换为原地踏步模式 | 步幅减半,躯干锁定")) + } + if key in key_actions: + key_actions[key]() -# ===================== 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_freq = freq + self.base_amp = amp self.freq = freq self.amp = amp - self.phase = phase # 初始相位(左右腿差π) - self.base_coupling = coupling_strength # 基础耦合强度 - self.coupling = coupling_strength # 动态耦合强度 - self.state = np.array([np.sin(phase), np.cos(phase)]) # 振荡器状态(x,y) + self.phase = phase + self.base_coupling = coupling_strength + self.coupling = coupling_strength + self.state = np.array([np.sin(phase), np.cos(phase)]) 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) # 限幅避免耦合过强/过弱 + self.coupling = np.clip(self.coupling, 0.1, 0.5) - # 范德波尔振荡器方程(生物节律更自然,抗干扰) - mu = 1.0 # 非线性系数,控制振荡收敛性 + 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]) # 更新相位 - # 返回当前输出(关节目标偏移量) + 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 @@ -211,12 +169,8 @@ def base_phase(self): return 0.0 if self.phase < np.pi else np.pi -# ===================== 人形机器人控制器(新增多步态模式) ===================== class HumanoidStabilizer: - """适配humanoid.xml模型的稳定站立与行走控制器(新增多步态+传感器模拟+鲁棒性优化)""" - def __init__(self, model_path): - # 类型检查与模型加载(原有逻辑完全保留) if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") @@ -226,7 +180,6 @@ def __init__(self, model_path): except Exception as e: raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性") - # 仿真核心参数(原有逻辑保留) self.sim_duration = 120.0 self.dt = self.model.opt.timestep self.init_wait_time = 4.0 @@ -234,7 +187,6 @@ def __init__(self, model_path): self.model.opt.iterations = 200 self.model.opt.tolerance = 1e-8 - # 关节名称映射(原有逻辑完全保留) self.joint_names = [ "abdomen_z", "abdomen_y", "abdomen_x", "hip_x_right", "hip_z_right", "hip_y_right", @@ -247,15 +199,14 @@ 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控制增益(原有逻辑保留,新增动态增益系数) 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.base_kp_hip = 250.0 # 基础KP(新增) - self.base_kd_hip = 45.0 # 基础KD(新增) + self.base_kp_hip = 250.0 + self.base_kd_hip = 45.0 self.base_kp_knee = 280.0 self.base_kd_knee = 50.0 self.base_kp_ankle = 200.0 @@ -265,36 +216,32 @@ def __init__(self, model_path): 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 # 重心过低时的降速系数(新增) + self.com_safety_threshold = 0.6 + self.speed_reduction_factor = 0.5 - # 状态变量(原有逻辑保留,新增鲁棒优化参数) self.joint_targets = np.zeros(self.num_joints) - self.prev_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.filter_alpha = 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轴抬高(更稳定) + "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": { # 正常走:原有参数 + "NORMAL": { "freq": 0.5, "amp": 0.4, "coupling": 0.2, @@ -302,37 +249,35 @@ def __init__(self, model_path): "speed_amp_gain": 0.2, "com_z_offset": 0.0 }, - "TROT": { # 小跑:高频、大步幅、中耦合(快速) + "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 # 重心略降低(提升步幅) + "com_z_offset": -0.01 }, - "STEP_IN_PLACE": { # 原地踏步:步幅减半、躯干锁定 + "STEP_IN_PLACE": { "freq": 0.4, - "amp": 0.2, # 步幅减半 + "amp": 0.2, "coupling": 0.3, - "speed_freq_gain": 0.0, # 速度不影响频率 - "speed_amp_gain": 0.0, # 速度不影响振幅 + "speed_freq_gain": 0.0, + "speed_amp_gain": 0.0, "com_z_offset": 0.01, - "lock_torso": True # 锁定躯干偏航/俯仰 + "lock_torso": True } } - self.gait_mode = "NORMAL" # 默认步态:正常走 + self.gait_mode = "NORMAL" self.current_gait_params = self.gait_config[self.gait_mode] - # 运动状态机(扩展步态相关状态,原有状态保留) - self.state = "STAND" # 初始状态:STAND/WALK/STOP/EMERGENCY + self.state = "STAND" self.state_map = { - "STAND": self._state_stand, # 站立(初始稳定) - "WALK": self._state_walk, # 行走(根据当前步态模式调整) - "STOP": self._state_stop, # 停止(关节归零) - "EMERGENCY": self._state_emergency # 急停(力矩清零) + "STAND": self._state_stand, + "WALK": self._state_walk, + "STOP": self._state_stop, + "EMERGENCY": self._state_emergency } - # CPG振荡器(初始化时加载默认步态参数) self.right_leg_cpg = CPGOscillator( freq=self.current_gait_params["freq"], amp=self.current_gait_params["amp"], @@ -345,43 +290,33 @@ def __init__(self, model_path): phase=np.pi, coupling_strength=self.current_gait_params["coupling"] ) - self.gait_phase = 0.0 # 保留原始相位变量,兼容调试输出 + 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) + self.turn_angle = 0.0 + self.turn_gain = 0.1 + self.walk_speed = 0.5 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 self.step_offset_hip = 0.4 self.step_offset_knee = 0.6 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.enable_sensor_simulation = True + self.imu_angle_noise = 0.01 + self.imu_vel_noise = 0.05 + self.imu_delay_frames = 2 + self.foot_force_noise = 0.3 + 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() - # ===================== 新增:步态模式切换接口 ===================== def set_gait_mode(self, mode): - """切换步态模式""" if mode not in self.gait_config.keys(): print(f"[警告] 无效的步态模式:{mode},默认使用NORMAL") mode = "NORMAL" @@ -389,7 +324,6 @@ def set_gait_mode(self, mode): 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"] @@ -398,73 +332,46 @@ def set_gait_mode(self, mode): 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) self.data.qpos[2] = 1.282 self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] self.data.qvel[:] = 0.0 self.data.xfrc_applied[:] = 0.0 - # 腰部关节 - self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["abdomen_x"]] = 0.0 - - # 右腿关节 - self.joint_targets[self.joint_name_to_idx["hip_x_right"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_z_right"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.4 - self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["ankle_x_right"]] = 0.0 - - # 左腿关节 - self.joint_targets[self.joint_name_to_idx["hip_x_left"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_z_left"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.4 - self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["ankle_x_left"]] = 0.0 - - # 手臂关节 - self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.5 - self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.5 + joint_init_vals = { + "abdomen_z": 0.0, "abdomen_y": 0.0, "abdomen_x": 0.0, + "hip_x_right": 0.0, "hip_z_right": 0.0, "hip_y_right": 0.1, + "knee_right": -0.4, "ankle_y_right": 0.0, "ankle_x_right": 0.0, + "hip_x_left": 0.0, "hip_z_left": 0.0, "hip_y_left": 0.1, + "knee_left": -0.4, "ankle_y_left": 0.0, "ankle_x_left": 0.0, + "shoulder1_right": 0.1, "shoulder2_right": 0.1, "elbow_right": 1.5, + "shoulder1_left": 0.1, "shoulder2_left": 0.1, "elbow_left": 1.5 + } + for name, val in joint_init_vals.items(): + self.joint_targets[self.joint_name_to_idx[name]] = val 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, @@ -472,7 +379,6 @@ def _simulate_imu_data(self): "true_ang_vel": true_ang_vel }) - # 获取延迟后的传感器数据(缓存不足时用真实数据) if len(self.imu_data_buffer) < self.imu_delay_frames: return { "euler": true_euler, @@ -481,41 +387,33 @@ def _simulate_imu_data(self): "true_ang_vel": true_ang_vel } else: - return self.imu_data_buffer[0] # 返回最早的缓存数据 + return self.imu_data_buffer[0] def _simulate_foot_force_data(self): - """模拟带噪声+零漂的足底力传感器数据""" - # 获取真实接触力(原有逻辑) + def get_foot_force(foot_geoms): + force = 0.0 + for geom_name in foot_geoms: + geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) + f = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(self.model, self.data, geom_id, f) + force += np.linalg.norm(f[:3]) + return force + 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]) - - # 添加噪声和零漂(模拟真实传感器) + true_left_force = get_foot_force(left_foot_geoms) + true_right_force = get_foot_force(right_foot_geoms) + 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, @@ -525,7 +423,6 @@ def _simulate_foot_force_data(self): "true_right_force": true_right_force }) - # 获取延迟后的传感器数据 if len(self.foot_data_buffer) < self.imu_delay_frames: return { "left_force": true_left_force, @@ -539,9 +436,7 @@ def _simulate_foot_force_data(self): 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(), @@ -549,7 +444,6 @@ def _get_sensor_data(self): "true_ang_vel": self.data.qvel[3:6].astype(np.float64).copy() } - # 原始接触力 self._detect_foot_contact() foot_data = { "left_force": self.left_foot_force, @@ -560,22 +454,19 @@ def _get_sensor_data(self): "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, - "gait_mode": self.gait_mode # 新增:记录当前步态 + "gait_mode": self.gait_mode } return self.current_sensor_data def print_sensor_data(self): - """打印当前传感器数据(调试用,新增步态信息)""" if not self.current_sensor_data: print("[传感器数据] 暂无数据") return @@ -593,19 +484,15 @@ def print_sensor_data(self): f"右脚力: {foot['right_force']:.2f}N (真实: {foot['true_right_force']:.2f}N) | 接触: {foot['right_contact']}") print("==================\n") - # 状态机方法(WALK状态适配多步态模式) def _state_stand(self): - """站立状态:维持初始稳定姿态,CPG重置""" self.right_leg_cpg.reset() self.left_leg_cpg.reset() - self._init_stable_pose() # 恢复站立姿态 + self._init_stable_pose() def _state_walk(self): - """行走状态:根据当前步态模式调整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 @@ -614,36 +501,29 @@ def _state_walk(self): 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.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: # 左转 + if self.turn_angle > 0: self.right_leg_cpg.amp *= 1.1 self.left_leg_cpg.amp *= 0.9 - elif self.turn_angle < 0: # 右转 + 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) + speed_factor = self.walk_speed / 1.0 + turn_factor = self.turn_angle / 0.3 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 @@ -653,35 +533,33 @@ def _state_walk(self): speed_factor=speed_factor, turn_factor=turn_factor ) - # 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 - 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 + leg_joint_offsets = { + "hip_y_right": 0.1 + right_hip_offset, + "knee_right": -0.4 - right_hip_offset * 1.2, + "ankle_y_right": 0.0 + right_hip_offset * 0.5, + "hip_y_left": 0.1 + left_hip_offset, + "knee_left": -0.4 - left_hip_offset * 1.2, + "ankle_y_left": 0.0 + left_hip_offset * 0.5 + } + for name, val in leg_joint_offsets.items(): + self.joint_targets[self.joint_name_to_idx[name]] = val - # 关节目标低通滤波(原有逻辑) 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.joint_targets *= 0.95 + self.data.qvel[:] *= 0.9 def _state_emergency(self): - """急停状态:力矩清零,速度归零""" self.data.ctrl[:] = 0.0 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 @@ -691,18 +569,15 @@ def set_state(self, state): self._init_stable_pose() def set_turn_angle(self, angle): - # 原地踏步模式下禁止转向 if self.gait_mode != "STEP_IN_PLACE": self.turn_angle = np.clip(angle, -0.3, 0.3) def set_walk_speed(self, speed): - # 原地踏步模式下速度固定 if self.gait_mode != "STEP_IN_PLACE": self.walk_speed = np.clip(speed, 0.1, 1.0) else: - self.walk_speed = 0.5 # 原地踏步固定速度 + self.walk_speed = 0.5 - # 四元数转欧拉角(原有逻辑完全保留) def _quat_to_euler_xyz(self, quat): w, x, y, z = quat sinr_cosp = 2 * (w * x + y * z) @@ -715,60 +590,42 @@ 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"] - right_foot_geoms = ["foot1_right", "foot2_right"] - - left_force = 0.0 - for geom_name in left_foot_geoms: + def get_foot_force(foot_geoms): + 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) - left_force += np.linalg.norm(force[:3]) + f = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(self.model, self.data, geom_id, f) + force += np.linalg.norm(f[:3]) + return force - 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) - right_force += np.linalg.norm(force[:3]) + left_foot_geoms = ["foot1_left", "foot2_left"] + right_foot_geoms = ["foot1_right", "foot2_right"] - 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 - self.left_foot_force = left_force - self.right_foot_force = right_force + self.left_foot_force = get_foot_force(left_foot_geoms) + self.right_foot_force = get_foot_force(right_foot_geoms) - 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 + self.foot_contact[1] = 1 if self.left_foot_force > self.foot_contact_threshold else 0 + self.foot_contact[0] = 1 if self.right_foot_force > self.foot_contact_threshold else 0 - # 计算稳定力矩(核心:基于传感器数据的反馈控制,新增步态信息) 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) - # 躯干姿态控制(改用传感器IMU数据) - root_euler = imu["euler"] # 带噪声的欧拉角 - root_vel = imu["ang_vel"] # 带噪声的角速度 + root_euler = imu["euler"] + root_vel = imu["ang_vel"] root_vel = np.clip(root_vel, -3.0, 3.0) roll_error = -root_euler[0] @@ -787,23 +644,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 - # 关节控制(改用传感器足底力数据) 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: idx = self.joint_name_to_idx[joint_name] @@ -811,7 +664,6 @@ 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", @@ -819,43 +671,35 @@ def _calculate_stabilizing_torques(self): "knee_left", "ankle_y_left", "ankle_x_left" ] + torque_config = { + "hip": {"base_kp": self.base_kp_hip, "base_kd": self.base_kd_hip}, + "knee": {"base_kp": self.base_kp_knee, "base_kd": self.base_kd_knee}, + "ankle": {"base_kp": self.base_kp_ankle, "base_kd": self.base_kd_ankle} + } + for joint_name in leg_joints: idx = self.joint_name_to_idx[joint_name] joint_error = self.joint_targets[idx] - current_joints[idx] joint_error = np.clip(joint_error, -0.3, 0.3) - # 动态PD增益 - 接触力越小,增益越低(避免打滑) + force_factor = 1.0 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.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 - else: - joint_error += torso_torque[1] * 0.02 - - elif "knee" in joint_name: - 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.base_kp_ankle * force_factor - kd = self.base_kd_ankle * force_factor - if "y" in joint_name: - joint_error += torso_torque[1] * 0.01 + joint_type = next(key for key in torque_config.keys() if key in joint_name) + kp = torque_config[joint_type]["base_kp"] * force_factor + kd = torque_config[joint_type]["base_kd"] * force_factor + + if joint_type == "hip" and "y" in joint_name: + joint_error += torso_torque[1] * 0.02 + elif joint_type == "knee": + joint_error += com_compensation[2] * 0.05 + elif joint_type == "ankle" and "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): kp *= 0.8 @@ -863,7 +707,6 @@ def _calculate_stabilizing_torques(self): torques[idx] = kp * joint_error - kd * current_vel[idx] - # 手臂关节控制(原有逻辑完全保留) arm_joints = [ "shoulder1_right", "shoulder2_right", "elbow_right", "shoulder1_left", "shoulder2_left", "elbow_left" @@ -873,7 +716,6 @@ 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, @@ -887,7 +729,6 @@ 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( @@ -900,18 +741,14 @@ def _calculate_stabilizing_torques(self): self.prev_com = com return torques - # 仿真循环(原有逻辑完全保留,新增步态信息输出) def simulate_stable_standing(self): - # 启动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 @@ -921,7 +758,6 @@ def simulate_stable_standing(self): print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持多步态切换(1=慢走/2=正常/3=小跑/4=原地踏步)") print(f"默认步态模式:{self.gait_mode}\n") - # 初始落地阶段(原有逻辑保留) 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) @@ -932,14 +768,12 @@ def simulate_stable_standing(self): v.sync() time.sleep(self.dt) - # 主仿真循环(原有逻辑保留) 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.current_sensor_data["imu"][ @@ -953,7 +787,6 @@ def simulate_stable_standing(self): v.sync() time.sleep(self.dt * 0.5) - # 跌倒判定(原有逻辑保留) com = self.data.subtree_com[0] 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: @@ -961,9 +794,8 @@ def simulate_stable_standing(self): f"跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad | 当前步态:{self.gait_mode}" ) - self.set_state("STAND") # 跌倒后自动恢复站立 + self.set_state("STAND") - # 停止ROS线程 self.ros_handler.stop() print("仿真完成!") @@ -978,10 +810,6 @@ 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}")