diff --git a/src/Mujoco_manrun/g1_23dof.xml b/src/Mujoco_manrun/g1_23dof.xml new file mode 100644 index 0000000000..c10b366f02 --- /dev/null +++ b/src/Mujoco_manrun/g1_23dof.xml @@ -0,0 +1,371 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 475eea388e..f948aff245 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -5,8 +5,7 @@ import os -class HumanoidStabilizer: - """专注于机器人稳定站立的控制器""" +class G1Stabilizer: def __init__(self, model_path): # 类型检查 @@ -21,186 +20,385 @@ def __init__(self, model_path): raise RuntimeError(f"模型加载失败：{e}\n请检查：1.路径是否为字符串 2.文件是否存在 3.文件是否完整") # 仿真参数 - self.sim_duration = 60.0 # 延长仿真时间，测试长时间站立 + self.sim_duration = 120.0 self.dt = self.model.opt.timestep - self.init_wait_time = 2.0 # 延长初始等待，让模型稳定 - - # 站立控制参数（针对重心不佳优化） - self.kp_root = 120.0 # 躯干姿态比例增益（增强直立控制） - self.kd_root = 15.0 # 躯干阻尼增益（抑制晃动） - self.kp_legs = 200.0 # 腿部关节比例增益（增强支撑） - self.kd_legs = 20.0 # 腿部关节阻尼增益 - self.hip_bias = 0.1 # 髋关节偏置，调整重心前后位置 - self.knee_bias = -0.2 # 膝关节偏置，微屈以降低重心 + self.init_wait_time = 2.0 + + # 关节名称映射表，与g1_23dof.xml对应 + 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" + ] + + # 创建关节名称到索引的映射 + self.joint_name_to_idx = {name: i for i, name in enumerate(self.joint_names)} + self.num_joints = len(self.joint_names) + + # 站立控制参数（针对G1模型优化） + self.kp_roll = 200.0 + self.kd_roll = 28.0 + self.kp_pitch = 180.0 + self.kd_pitch = 22.0 + self.kp_yaw = 60.0 + self.kd_yaw = 12.0 + + # 腿部关节增益（针对G1的关节结构优化） + self.kp_hip = 300.0 + self.kd_hip = 35.0 + self.kp_knee = 350.0 + self.kd_knee = 40.0 + self.kp_ankle = 250.0 + self.kd_ankle = 30.0 + + # 腰部和手臂关节增益 + self.kp_waist = 100.0 + self.kd_waist = 15.0 + self.kp_arm = 80.0 + self.kd_arm = 10.0 + + # 重心补偿参数 + self.com_target = np.array([0.0, 0.0, 0.85]) # G1的目标重心位置 + self.kp_com = 90.0 + self.foot_contact_threshold = 3.0 # 适应G1的足部结构 # 状态变量 - self.prev_root_rot = np.zeros(3) # 上一帧躯干欧拉角 - self.leg_joint_targets = None # 腿部关节目标角度 - self.torso_target_euler = np.zeros(3) # 躯干目标欧拉角（直立） + self.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._init_stable_pose() def _init_stable_pose(self): - """初始化稳定的站立姿态，调整重心位置""" - # 重置模型到初始状态 + """初始化G1机器人的稳定站立姿态""" mujoco.mj_resetData(self.model, self.data) - # 手动调整初始位置和姿态，降低重心并调整到双脚中心 - self.data.qpos[2] = 1.1 # 降低躯干高度（原可能过高，调整重心） - self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] # 躯干直立（四元数，w,x,y,z顺序） - self.data.qvel[:] = 0.0 # 初始速度归零，防止模型飞起 - - # 初始化腿部关节目标角度（微屈，形成稳定支撑） - num_actuators = self.model.nu - self.leg_joint_targets = np.zeros(num_actuators) - - # 假设腿部关节索引：0-5为右腿，6-11为左腿（根据常见humanoid.xml结构） - leg_joint_count = min(12, num_actuators) - for i in range(leg_joint_count): - if i % 6 == 0: # 髋x - self.leg_joint_targets[i] = self.hip_bias - elif i % 6 == 3: # 膝y - self.leg_joint_targets[i] = self.knee_bias + # 设置初始位置（根据G1模型调整） + self.data.qpos[2] = 0.85 # 躯干初始高度 + self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] # 躯干直立 + self.data.qvel[:] = 0.0 + self.data.xfrc_applied[:] = 0.0 + + # 设置各关节目标角度（针对G1的23自由度优化） + # 左腿关节目标角度 + 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.05 + 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.2 + 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.05 + 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.2 + 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.5 + self.joint_targets[self.joint_name_to_idx["left_shoulder_roll_joint"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["left_shoulder_yaw_joint"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["left_elbow_joint"]] = 1.5 + self.joint_targets[self.joint_name_to_idx["left_wrist_roll_joint"]] = 0.0 + + # 右臂关节（自然下垂） + self.joint_targets[self.joint_name_to_idx["right_shoulder_pitch_joint"]] = 0.5 + self.joint_targets[self.joint_name_to_idx["right_shoulder_roll_joint"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["right_shoulder_yaw_joint"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["right_elbow_joint"]] = 1.5 + self.joint_targets[self.joint_name_to_idx["right_wrist_roll_joint"]] = 0.0 - # 前向计算更新状态 mujoco.mj_forward(self.model, self.data) def _get_root_euler(self): - """提取躯干欧拉角（roll, pitch, yaw），用于姿态控制""" - # 修正mju_quat2Mat的参数格式（需要一维数组，且内存连续） - rot_mat = np.zeros(9, dtype=np.float64) # 一维数组存储9个元素 - quat = self.data.qpos[3:7].astype(np.float64).copy() # 确保float64且连续 + """提取躯干欧拉角（roll, pitch, yaw）""" + rot_mat = np.zeros(9, dtype=np.float64) + quat = self.data.qpos[3:7].astype(np.float64).copy() mujoco.mju_quat2Mat(rot_mat, quat) - rot_mat = rot_mat.reshape(3, 3) # 转为3x3矩阵 - # 计算欧拉角（XYZ顺序） euler = np.zeros(3, dtype=np.float64) - mujoco.mju_mat2Euler(euler, rot_mat.flatten(), 1) # 第二个参数为一维数组 - return euler + mujoco.mju_mat2Euler(euler, rot_mat, 1) # XYZ顺序 - def _calculate_stabilizing_torques(self): - """计算维持站立的稳定力矩，重点补偿重心偏移""" - num_actuators = self.model.nu - torques = np.zeros(num_actuators, dtype=np.float64) + # 角度限幅（-π~π） + euler = np.mod(euler + np.pi, 2 * np.pi) - np.pi + return euler - # 1. 躯干姿态控制（保持直立） + def _detect_foot_contact(self): + """检测左右脚与地面的接触力（适配G1的足部结构）""" try: - root_euler = self._get_root_euler() - except: - root_euler = np.zeros(3) - root_euler_error = self.torso_target_euler - root_euler - root_angular_vel = self.data.qvel[3:6].copy() # 躯干角速度 - root_angular_vel = np.clip(root_angular_vel, -5.0, 5.0) # 限制角速度 - - # 躯干稳定力矩（通过根关节虚拟力矩，实际作用于腿部支撑调整） - torso_torque = self.kp_root * root_euler_error - self.kd_root * root_angular_vel - torso_torque = np.clip(torso_torque, -20.0, 20.0) # 限制躯干力矩 - - # 2. 腿部关节控制（刚性支撑+重心补偿） - leg_joint_count = min(12, num_actuators) - leg_joint_indices = list(range(leg_joint_count)) - - # 获取当前腿部关节位置（确保数组长度匹配） - current_joints = self.data.qpos[7:] if len(self.data.qpos) > 7 else np.zeros(num_actuators) - current_joints = current_joints[:num_actuators].astype(np.float64) - - # 获取当前腿部关节速度（确保数组长度匹配） - current_vel = self.data.qvel[6:] if len(self.data.qvel) > 6 else np.zeros(num_actuators) - current_vel = current_vel[:num_actuators].astype(np.float64) - current_vel = np.clip(current_vel, -10.0, 10.0) # 限制关节速度 - - # 腿部关节误差（加入躯干姿态补偿，调整支撑脚位置） - leg_joint_error = self.leg_joint_targets.copy() - if len(leg_joint_indices) >= 2: - leg_joint_error[leg_joint_indices[0]] += torso_torque[0] * 0.05 # 左髋补偿roll - leg_joint_error[leg_joint_indices[6]] -= torso_torque[0] * 0.05 # 右髋补偿roll - if len(leg_joint_indices) >= 1: - leg_joint_error[leg_joint_indices[1]] += torso_torque[1] * 0.05 # 髋补偿pitch - - # 腿部力矩计算（高刚性+阻尼） - torque_error = leg_joint_error - current_joints - torque_error = np.clip(torque_error, -0.5, 0.5) # 限制误差范围，防止力矩突变 - torques = self.kp_legs * torque_error - self.kd_legs * current_vel - - # 3. 力矩限幅（防止过载） - torque_limit = 50.0 # 增大力矩上限，提供足够支撑力 - torques = np.clip(torques, -torque_limit, torque_limit) - - # 4. 重力补偿（针对重心不佳的模型，添加固定偏置力矩） - if leg_joint_count > 0: - torques[leg_joint_indices] += np.sign(leg_joint_error[leg_joint_indices]) * 2.0 # 小幅重力补偿 + # 检测左脚接触（使用g1_23dof.xml中定义的四个足部碰撞体） + 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_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) + left_force += force[2] # z轴分量 + + # 计算右脚总接触力 + 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 += force[2] # z轴分量 + + # 更新接触状态 + self.foot_contact[0] = 1 if right_force > self.foot_contact_threshold else 0 # 右脚 + self.foot_contact[1] = 1 if left_force > self.foot_contact_threshold else 0 # 左脚 + except Exception as e: + print(f"接触检测警告: {e}") + self.foot_contact = np.ones(2) # 出错时默认双脚接触 + + def _calculate_stabilizing_torques(self): + """计算稳定站立所需的关节力矩""" + 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, -10.0, 10.0) + + # 侧倾控制（带积分补偿） + roll_error = -root_euler[0] + self.integral_roll += roll_error * self.dt + self.integral_roll = np.clip(self.integral_roll, -0.5, 0.5) + roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 12.0 * self.integral_roll + + # 俯仰控制（带积分补偿） + pitch_error = -root_euler[1] + self.integral_pitch += pitch_error * self.dt + self.integral_pitch = np.clip(self.integral_pitch, -0.5, 0.5) + pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 10.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, -60.0, 60.0) + + # 2. 重心位置补偿 + com = self.data.subtree_com[0].astype(np.float64).copy() + com_error = self.com_target - com + com_error[2] = np.clip(com_error[2], -0.1, 0.1) + com_compensation = self.kp_com * com_error + + # 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, -15.0, 15.0) + + # 处理腿部关节 + 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" + ] + + for joint_name in leg_joints: + idx = self.joint_name_to_idx[joint_name] + joint_error = self.joint_targets[idx] - current_joints[idx] + + # 根据关节类型设置增益 + if "hip" in joint_name: + kp = self.kp_hip + kd = self.kd_hip + # 髋关节加入姿态补偿 + if "left" in joint_name: + if "roll" in joint_name: + joint_error -= torso_torque[0] * 0.07 + elif "pitch" in joint_name: + joint_error += torso_torque[1] * 0.06 + else: # 右腿 + if "roll" in joint_name: + joint_error += torso_torque[0] * 0.07 + elif "pitch" in joint_name: + joint_error += torso_torque[1] * 0.06 + + elif "knee" in joint_name: + kp = self.kp_knee + kd = self.kd_knee + # 膝关节加入重心补偿 + joint_error += com_compensation[2] * 0.12 + + elif "ankle" in joint_name: + kp = self.kp_ankle + kd = self.kd_ankle + # 踝关节加入姿态补偿 + if "pitch" in joint_name: + joint_error += torso_torque[1] * 0.08 + + # 根据接触状态调整增益 + if ("left" in joint_name and self.foot_contact[1] == 0) or \ + ("right" in joint_name and self.foot_contact[0] == 0): + kp *= 0.4 + kd *= 0.6 + + 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] + # 腰部加入偏航补偿 + joint_error += torso_torque[2] * 0.05 + 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" + ] + + 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] + + # 4. 力矩限幅（根据g1_23dof.xml中的actuatorfrcrange设置） + torque_limits = { + # 腿部关节限幅 + "left_hip_pitch_joint": 88, "left_hip_roll_joint": 88, "left_hip_yaw_joint": 88, + "left_knee_joint": 139, "left_ankle_pitch_joint": 50, "left_ankle_roll_joint": 50, + "right_hip_pitch_joint": 88, "right_hip_roll_joint": 88, "right_hip_yaw_joint": 88, + "right_knee_joint": 139, "right_ankle_pitch_joint": 50, "right_ankle_roll_joint": 50, + # 腰部关节 + "waist_yaw_joint": 88, + # 手臂关节 + "left_shoulder_pitch_joint": 25, "left_shoulder_roll_joint": 25, + "left_shoulder_yaw_joint": 25, "left_elbow_joint": 25, "left_wrist_roll_joint": 25, + "right_shoulder_pitch_joint": 25, "right_shoulder_roll_joint": 25, + "right_shoulder_yaw_joint": 25, "right_elbow_joint": 25, "right_wrist_roll_joint": 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) + + self.prev_com = com return torques def simulate_stable_standing(self): - """启动稳定站立仿真""" - # 关闭重力扰动（可选，若模型仍飞起） - self.model.opt.gravity[2] = -9.81 # 标准重力，防止重力设置错误 - self.model.opt.timestep = 0.005 # 合理的时间步长，提高仿真稳定性 + """运行稳定站立仿真""" + # 优化仿真参数 + self.model.opt.gravity[2] = -9.81 + self.model.opt.timestep = 0.002 + self.model.opt.iterations = 100 + self.model.opt.tolerance = 1e-6 with viewer.launch_passive(self.model, self.data) as v: - print("可视化窗口已启动，机器人将尝试稳定站立...") - print("操作：鼠标拖动旋转视角，滚轮缩放，W/A/S/D平移，关闭窗口结束") + print("G1机器人稳定站立仿真启动...") + print("适配g1_23dof模型 | 23自由度控制 | 足部接触检测") - # 初始等待（让模型稳定落地） + # 初始落地阶段 start_time = time.time() while time.time() - start_time < self.init_wait_time: - # 初始阶段施加零力矩，让模型自然落地 self.data.ctrl[:] = 0.0 mujoco.mj_step(self.model, self.data) - # 强制速度归零，防止初始抖动 - self.data.qvel[:] = 0.0 + self.data.qvel[:] *= 0.9 # 速度衰减，减少冲击 v.sync() time.sleep(0.01) - # 主仿真循环（稳定站立控制） + # 主仿真循环 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 % 5 < 0.1: # 每5秒打印一次 - com = self.data.subtree_com[0] # 整体重心（第一个子树为根节点） + # 状态监测（每2秒打印） + 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,y,z): {com[0]:.3f}, {com[1]:.3f}, {com[2]:.3f}") + f"时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " + f"姿态(roll/pitch):{euler[0]:.3f}/{euler[1]:.3f}rad | 脚接触:{self.foot_contact}" + ) - # 可视化同步 v.sync() time.sleep(0.001) - # 紧急停止（若跌倒严重） + # 跌倒判定 com = self.data.subtree_com[0] - if com[2] < 0.5: # 重心过低判定为跌倒 - print(f"机器人跌倒！仿真时间: {self.data.time:.1f}s") + euler = self._get_root_euler() + if com[2] < 0.5 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" + ) break print("仿真完成！") if __name__ == "__main__": - # 模型路径处理 current_directory = os.path.dirname(os.path.abspath(__file__)) - model_file_path = os.path.join(current_directory, "humanoid.xml") - - print(f"当前脚本所在目录：{current_directory}") - print(f"模型文件完整路径：{model_file_path}") + model_file_path = os.path.join(current_directory, "g1_23dof.xml") # 适配G1模型文件名 - # 检查模型文件存在性 + print(f"模型路径：{model_file_path}") if not os.path.exists(model_file_path): - raise FileNotFoundError( - f"模型文件不存在！\n查找路径：{model_file_path}\n" - f"请确认 'humanoid.xml' 文件放在以下目录中：{current_directory}" - ) + raise FileNotFoundError(f"模型文件不存在：{model_file_path}") - # 启动稳定站立仿真 try: - stabilizer = HumanoidStabilizer(model_file_path) - print("\n开始稳定站立仿真...") + stabilizer = G1Stabilizer(model_file_path) stabilizer.simulate_stable_standing() except Exception as e: - print(f"\n仿真过程中发生错误：{e}") \ No newline at end of file + print(f"错误：{e}") \ No newline at end of file