From e3e3dbfc09bcae8690ce8f5e5e5e7f0092cc7d39 Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Mon, 8 Dec 2025 11:18:05 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E5=9F=BA=E7=A1=80=E4=BF=9D=E6=8C=81?= =?UTF-8?q?=E7=9B=B4=E7=AB=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 329 ++++++++++++++++++-------------------- 1 file changed, 157 insertions(+), 172 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 3fe3b3ba4d..475eea388e 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -1,221 +1,206 @@ -# 导入必要的库 -import mujoco # 导入MuJoCo物理仿真引擎核心库 -import numpy as np # 导入NumPy库,用于高效的数值计算,特别是数组操作 -from mujoco import viewer # 从MuJoCo导入viewer模块,用于创建交互式可视化窗口 -import time # 导入time模块,用于控制仿真速度和等待时间 +import mujoco +import numpy as np +from mujoco import viewer +import time +import os -# 人形机器人行走控制器类 -class HumanoidWalker: - """ - 一个封装了人形机器人行走控制逻辑的类。 - 它负责加载模型、生成步态、计算控制力矩、运行仿真和可视化。 - """ +class HumanoidStabilizer: + """专注于机器人稳定站立的控制器""" def __init__(self, model_path): - """ - 类的初始化方法,在创建类的实例时自动调用。 - - :param model_path: 字符串类型,指向机器人模型XML文件的路径。 - """ - # 强制确认传入的路径是字符串类型,避免因路径类型错误导致后续加载失败 + # 类型检查 if not isinstance(model_path, str): - # 如果不是字符串,抛出类型错误异常 raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") - # 尝试从指定路径加载并编译MuJoCo模型 + # 模型加载 try: - # 从XML文件路径加载模型,返回一个MjModel对象,包含了机器人的所有物理和几何信息 self.model = mujoco.MjModel.from_xml_path(model_path) - # 创建一个与模型关联的MjData对象,用于存储仿真过程中的所有动态状态(如位置、速度、力等) self.data = mujoco.MjData(self.model) except Exception as e: - # 如果加载过程中出现任何错误,抛出运行时错误,并给出排查建议 raise RuntimeError(f"模型加载失败:{e}\n请检查:1.路径是否为字符串 2.文件是否存在 3.文件是否完整") - # 设置仿真相关的参数 - self.sim_duration = 20.0 # 设定仿真的总时长,单位为秒 - self.dt = self.model.opt.timestep # 获取模型中定义的仿真步长,单位为秒。这是物理世界更新一次的时间 - self.init_wait_time = 1.0 # 设定仿真开始前的等待时间,单位为秒,用于让可视化窗口稳定显示初始状态 + # 仿真参数 + self.sim_duration = 60.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.prev_root_rot = np.zeros(3) # 上一帧躯干欧拉角 + self.leg_joint_targets = None # 腿部关节目标角度 + self.torso_target_euler = np.zeros(3) # 躯干目标欧拉角(直立) + + # 初始化稳定姿态 + self._init_stable_pose() + + def _init_stable_pose(self): + """初始化稳定的站立姿态,调整重心位置""" + # 重置模型到初始状态 + 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 # 初始速度归零,防止模型飞起 - # 设置PID控制器的参数 - self.kp = 30.0 # 比例增益 (Proportional Gain),用于快速响应当前的位置误差 - self.ki = 0.01 # 积分增益 (Integral Gain),用于消除长期存在的静差(稳态误差) - self.kd = 5.0 # 微分增益 (Derivative Gain),用于抑制系统震荡,提高稳定性 + # 初始化腿部关节目标角度(微屈,形成稳定支撑) + num_actuators = self.model.nu + self.leg_joint_targets = np.zeros(num_actuators) - # 初始化PID控制器所需的状态变量,用于存储历史误差信息 - # self.model.nu 是机器人可驱动关节的数量 - self.joint_errors = np.zeros(self.model.nu) # 存储上一次的关节位置误差 - self.joint_integrals = np.zeros(self.model.nu) # 存储关节位置误差的积分值 + # 假设腿部关节索引: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 - # 将模型的所有数据重置到其初始状态(如初始位置、速度为零等) - mujoco.mj_resetData(self.model, self.data) - # 执行一次前向动力学计算,根据当前状态更新所有派生量(如世界坐标系下的位置、 Jacobian 等) + # 前向计算更新状态 mujoco.mj_forward(self.model, self.data) - def get_gait_trajectory(self, t): - """ - 根据当前仿真时间t,生成一个目标步态轨迹(即每个关节的期望角度)。 - 这是一个基于正弦函数的简单步态生成器。 - - :param t: 当前的仿真时间,单位为秒。 - :return: 一个NumPy数组,包含了每个可驱动关节的目标角度(弧度)。 - """ - # 在仿真开始的前2秒,让机器人保持初始的零角度姿势,不进行任何动作 - if t < 2.0: - return np.zeros(self.model.nu) # 返回一个全为零的数组作为目标 - - # 从第2秒开始,调整时间基准,使得步态周期从0开始计算 - t_adjusted = t - 2.0 - cycle = t_adjusted % 1.5 # 设定步态周期为1.5秒,计算当前时间处于哪个周期内 - phase = 2 * np.pi * cycle / 1.5 # 将周期转换为相位(0到2π之间),用于正弦函数 - - # 定义步态中各个关节的摆动幅度(最大角度) - leg_amp = 0.3 # 腿部关节的摆动幅度 - arm_amp = 0.2 # 手臂关节的摆动幅度 - torso_amp = 0.05 # 躯干关节的摆动幅度 - - # 初始化一个全为零的目标关节角度数组 - target = np.zeros(self.model.nu) - - # 假设模型中腿部关节从索引5开始(这取决于你的XML模型定义) - leg_joint_offset = 5 - # 检查目标数组长度是否足够容纳腿部关节 - if len(target) > leg_joint_offset + 5: - # 为左右腿的髋、膝、踝关节设置目标角度 - # 通过不同相位的正弦函数组合,模拟出腿部的摆动和蹬地动作 - target[leg_joint_offset] = -leg_amp * np.sin(phase) - target[leg_joint_offset + 1] = leg_amp * 1.5 * np.sin(phase + np.pi) - target[leg_joint_offset + 2] = leg_amp * 0.5 * np.sin(phase) - target[leg_joint_offset + 3] = -leg_amp * np.sin(phase + np.pi) - target[leg_joint_offset + 4] = leg_amp * 1.5 * np.sin(phase) - target[leg_joint_offset + 5] = leg_amp * 0.5 * np.sin(phase + np.pi) - - # 为躯干和手臂关节设置目标角度,以协调行走姿态,保持平衡 - if len(target) > 0: - target[0] = torso_amp * np.sin(phase + np.pi / 2) - if len(target) > 16: - target[16] = arm_amp * np.sin(phase + np.pi) - if len(target) > 20: - target[20] = arm_amp * np.sin(phase) - - # 返回计算好的目标关节角度数组 - return target - - def pid_controller(self, target_pos): - """ - PID控制器的核心计算函数。根据当前关节位置和目标位置,计算出每个关节需要施加的力矩。 - - :param target_pos: 目标关节位置数组(弧度)。 - :return: 计算出的关节控制力矩数组(牛顿·米)。 - """ - # 从仿真数据中获取当前关节的实际位置。 - # self.data.qpos 包含了所有自由度的位置(7个根节点自由度 + 关节自由度) - # [7:] 切片操作,跳过前7个根节点自由度,获取所有关节的位置 - current_pos = self.data.qpos[7:] - - # 安全检查:确保目标位置数组和当前位置数组长度一致 - if len(current_pos) != len(target_pos): - # 如果不一致,返回一个全为零的力矩数组,防止程序崩溃 - return np.zeros_like(target_pos) - - # 1. 计算比例项 (P) - # error 是一个数组,每个元素代表对应关节的目标位置与当前位置的差值 - error = target_pos - current_pos - - # 2. 计算积分项 (I) - # 将当前误差乘以时间步dt,累加到积分误差数组上 - self.joint_integrals += error * self.dt - # 对积分值进行限幅(clipping),防止积分饱和导致控制器失控 - self.joint_integrals = np.clip(self.joint_integrals, -2.0, 2.0) - - # 3. 计算微分项 (D) - # 微分项是误差的变化率,即 (当前误差 - 上次误差) / 时间步 - derivative = (error - self.joint_errors) / self.dt if self.dt != 0 else 0 - - # 更新上次误差记录,为下一次计算微分项做准备 - self.joint_errors = error.copy() - - # 4. 计算PID总输出力矩 - torque = self.kp * error + self.ki * self.joint_integrals + self.kd * derivative - - # 对计算出的力矩进行限幅,防止超过电机或关节的最大承受能力 - return np.clip(torque, -5.0, 5.0) - - def simulate_with_visualization(self): - """ - 启动带可视化的仿真主循环。这是整个程序的核心执行部分。 - """ - # 使用viewer.launch_passive创建一个被动模式的可视化窗口。 - # "被动"意味着我们需要手动调用v.sync()来更新画面,这给了我们对仿真循环的完全控制。 - # 这是一个上下文管理器(with语句),确保窗口能被正确关闭。 + 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且连续 + 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 + + def _calculate_stabilizing_torques(self): + """计算维持站立的稳定力矩,重点补偿重心偏移""" + num_actuators = self.model.nu + torques = np.zeros(num_actuators, dtype=np.float64) + + # 1. 躯干姿态控制(保持直立) + 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 # 小幅重力补偿 + + return torques + + def simulate_stable_standing(self): + """启动稳定站立仿真""" + # 关闭重力扰动(可选,若模型仍飞起) + self.model.opt.gravity[2] = -9.81 # 标准重力,防止重力设置错误 + self.model.opt.timestep = 0.005 # 合理的时间步长,提高仿真稳定性 + with viewer.launch_passive(self.model, self.data) as v: - print("可视化窗口已启动(前2秒保持静止,随后开始步行)") + print("可视化窗口已启动,机器人将尝试稳定站立...") print("操作:鼠标拖动旋转视角,滚轮缩放,W/A/S/D平移,关闭窗口结束") - # 初始等待阶段 - start_time = time.time() # 记录开始等待的时间 - # 循环等待,直到等待时间超过设定的self.init_wait_time + # 初始等待(让模型稳定落地) + start_time = time.time() while time.time() - start_time < self.init_wait_time: - v.sync() # 同步可视化窗口,更新一帧画面 - time.sleep(0.01) # 短暂休眠,降低CPU占用 + # 初始阶段施加零力矩,让模型自然落地 + self.data.ctrl[:] = 0.0 + mujoco.mj_step(self.model, self.data) + # 强制速度归零,防止初始抖动 + self.data.qvel[:] = 0.0 + v.sync() + time.sleep(0.01) - # 仿真主循环 - # 持续运行,直到仿真时间达到设定的总时长self.sim_duration + # 主仿真循环(稳定站立控制) while self.data.time < self.sim_duration: - # 1. 规划:根据当前仿真时间获取目标步态 - target_joint_positions = self.get_gait_trajectory(self.data.time) + # 计算稳定力矩 + torques = self._calculate_stabilizing_torques() + self.data.ctrl[:] = torques - # 2. 控制:根据目标位置和当前位置,使用PID控制器计算控制力矩 - control_torques = self.pid_controller(target_joint_positions) - - # 3. 执行:将计算出的控制力矩赋值给机器人的执行器 - self.data.ctrl[:] = control_torques - - # 4. 步进:执行一步物理仿真。MuJoCo会根据当前状态和控制力矩计算下一个状态 + # 步进仿真 mujoco.mj_step(self.model, self.data) - # 5. 可视化:同步可视化窗口,将新的仿真状态绘制出来 - v.sync() + # 实时监测重心位置(调试用) + if self.data.time % 5 < 0.1: # 每5秒打印一次 + com = self.data.subtree_com[0] # 整体重心(第一个子树为根节点) + print( + f"仿真时间: {self.data.time:.1f}s | 重心位置(x,y,z): {com[0]:.3f}, {com[1]:.3f}, {com[2]:.3f}") - # 6. 速度控制:短暂休眠,以控制仿真的实时速度,使其不至于过快 + # 可视化同步 + v.sync() time.sleep(0.001) - # 当仿真循环结束后(窗口关闭或时间到期),打印提示信息 + # 紧急停止(若跌倒严重) + com = self.data.subtree_com[0] + if com[2] < 0.5: # 重心过低判定为跌倒 + print(f"机器人跌倒!仿真时间: {self.data.time:.1f}s") + break + print("仿真完成!") if __name__ == "__main__": - # 这部分代码只有在当前脚本作为主程序直接运行时才会执行 - - # 1. 构建模型文件的完整路径 - import os # 导入os模块,用于处理文件路径 - - # os.path.abspath(__file__) 获取当前脚本的绝对路径 - # os.path.dirname(...) 获取该路径的目录部分 + # 模型路径处理 current_directory = os.path.dirname(os.path.abspath(__file__)) - # os.path.join(...) 安全地拼接目录和文件名,跨平台兼容 model_file_path = os.path.join(current_directory, "humanoid.xml") - # 2. 打印路径信息,方便用户排查路径问题 print(f"当前脚本所在目录:{current_directory}") print(f"模型文件完整路径:{model_file_path}") - # 3. 检查模型文件是否存在 + # 检查模型文件存在性 if not os.path.exists(model_file_path): - # 如果文件不存在,抛出文件未找到异常 raise FileNotFoundError( f"模型文件不存在!\n查找路径:{model_file_path}\n" f"请确认 'humanoid.xml' 文件放在以下目录中:{current_directory}" ) - # 4. 实例化控制器并启动仿真 + # 启动稳定站立仿真 try: - # 创建HumanoidWalker类的实例,并传入模型文件路径 - walker = HumanoidWalker(model_file_path) - print("\n开始仿真...") - # 调用实例的simulate_with_visualization方法,启动仿真 - walker.simulate_with_visualization() + stabilizer = HumanoidStabilizer(model_file_path) + print("\n开始稳定站立仿真...") + stabilizer.simulate_stable_standing() except Exception as e: - # 如果在整个过程中发生任何未捕获的异常,打印错误信息 print(f"\n仿真过程中发生错误:{e}") \ No newline at end of file From a7a3d2e9e4e5356231b4c426dc98239d3cb3041d Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Tue, 16 Dec 2025 09:29:14 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E6=9B=B4=E6=8D=A2=E4=BA=86=E6=A8=A1?= =?UTF-8?q?=E5=9E=8B=EF=BC=8C=E4=BF=AE=E6=94=B9=E4=BB=A3=E7=A0=81=E5=AF=B9?= =?UTF-8?q?=E5=BA=94=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/g1_23dof.xml | 371 +++++++++++++++++++++++++++ src/Mujoco_manrun/main.py | 450 ++++++++++++++++++++++++--------- 2 files changed, 695 insertions(+), 126 deletions(-) create mode 100644 src/Mujoco_manrun/g1_23dof.xml 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 From 99db51329cccc75412b9e27f8ad6a8e6bc11a55d Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Tue, 16 Dec 2025 13:23:42 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E7=AE=80?= =?UTF-8?q?=E5=8D=95=E6=AD=A5=E8=A1=8C=E7=9A=84=E4=BB=A3=E7=A0=81=E3=80=82?= =?UTF-8?q?=E9=99=84=E5=B8=A6g1=E6=89=80=E9=9C=80=E7=9A=84=E7=8E=AF?= =?UTF-8?q?=E5=A2=83=E5=BB=BA=E6=A8=A1=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 443 ++++++++++++++++-------------- src/Mujoco_manrun/mimic_sites.xml | 7 + src/Mujoco_manrun/scene.xml | 19 ++ 3 files changed, 264 insertions(+), 205 deletions(-) create mode 100644 src/Mujoco_manrun/mimic_sites.xml create mode 100644 src/Mujoco_manrun/scene.xml diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index f948aff245..29db331ce3 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -6,245 +6,280 @@ class G1Stabilizer: + """修复踮脚后仰的G1机器人稳定站立控制器(新增行走功能)""" def __init__(self, model_path): - # 类型检查 + # 类型检查与模型加载 if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") - # 模型加载 try: self.model = mujoco.MjModel.from_xml_path(model_path) self.data = mujoco.MjData(self.model) except Exception as e: - raise RuntimeError(f"模型加载失败:{e}\n请检查:1.路径是否为字符串 2.文件是否存在 3.文件是否完整") + raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性") - # 仿真参数 + # 仿真核心参数(强化稳定性) self.sim_duration = 120.0 - self.dt = self.model.opt.timestep - self.init_wait_time = 2.0 + self.dt = 0.001 + self.model.opt.timestep = self.dt + self.init_wait_time = 4.0 # 延长初始稳定时间 + self.model.opt.gravity[2] = -9.81 + self.model.opt.iterations = 200 + self.model.opt.tolerance = 1e-8 - # 关节名称映射表,与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", + "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", + "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", + "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" + "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的足部结构 + # 关键修复: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 # 降低接触阈值,提高检测灵敏度 # 状态变量 - self.joint_targets = np.zeros(self.num_joints) # 所有关节的目标角度 + self.joint_targets = np.zeros(self.num_joints) self.prev_com = np.zeros(3) - self.foot_contact = np.zeros(2) # [右脚, 左脚] + self.foot_contact = np.zeros(2) 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._init_stable_pose() def _init_stable_pose(self): - """初始化G1机器人的稳定站立姿态""" + """修复:调整关节角度,让脚掌完全落地,重心前移""" mujoco.mj_resetData(self.model, self.data) - # 设置初始位置(根据G1模型调整) - self.data.qpos[2] = 0.85 # 躯干初始高度 - self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] # 躯干直立 + # 躯干初始位置(更低+更稳) + self.data.qpos[2] = 0.78 # 初始高度匹配重心目标 + 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_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.2 + 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.05 + 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.2 + 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.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 + 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 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): - """提取躯干欧拉角(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) - - euler = np.zeros(3, dtype=np.float64) - mujoco.mju_mat2Euler(euler, rot_mat, 1) # XYZ顺序 - - # 角度限幅(-π~π) + euler = self._quat_to_euler_xyz(quat) euler = np.mod(euler + np.pi, 2 * np.pi) - np.pi - return euler + return np.clip(euler, -0.3, 0.3) # 进一步限制最大倾角 def _detect_foot_contact(self): - """检测左右脚与地面的接触力(适配G1的足部结构)""" + """优化接触检测,确保脚掌落地判定准确""" try: - # 检测左脚接触(使用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_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轴分量 + left_force += np.linalg.norm(force[:3]) - # 计算右脚总接触力 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轴分量 + right_force += np.linalg.norm(force[:3]) - # 更新接触状态 - 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 # 左脚 + # 迟滞优化:防止接触状态频繁切换 + 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 = np.ones(2) + + # 【行走功能新增】更新步态相位 + def _update_gait_phase(self): + """更新步态相位,确保初始稳定后触发行走""" + if self.walk_start_time is None: + self.walk_start_time = self.data.time # 标记行走开始时间 + # 计算归一化步态相位(0-1循环) + 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 # 初始稳定期不修改目标角度 + + self._update_gait_phase() + phase = self.gait_phase # 0-1 + + # ========== 左腿/右腿交替摆动逻辑 ========== + 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 + 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 def _calculate_stabilizing_torques(self): - """计算稳定站立所需的关节力矩""" + """优化力矩计算,抑制踮脚/后仰 + 集成行走控制""" + # 【新增】先更新行走的关节目标角度 + self._update_walk_joint_targets() + torques = np.zeros(self.num_joints, dtype=np.float64) - # 1. 躯干姿态控制 + # 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) + root_vel = np.clip(root_vel, -3.0, 3.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 + 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 - # 俯仰控制(带积分补偿) 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 + 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 - # 偏航控制 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) + torso_torque = np.clip(torso_torque, -30.0, 30.0) # 降低躯干修正力矩上限 - # 2. 重心位置补偿 + # 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_error = np.clip(com_error, -0.03, 0.03) com_compensation = self.kp_com * com_error - # 3. 关节控制 + # 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) + current_vel = np.clip(current_vel, -8.0, 8.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" ] @@ -252,134 +287,129 @@ 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.3, 0.3) # 【放宽】关节误差限制,允许更大动作 - # 根据关节类型设置增益 + # 关节增益匹配 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 + # 核心:降低髋关节俯仰的姿态补偿,避免后仰 + 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 elif "knee" in joint_name: kp = self.kp_knee kd = self.kd_knee - # 膝关节加入重心补偿 - joint_error += com_compensation[2] * 0.12 + joint_error += com_compensation[2] * 0.05 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 + 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.4 - kd *= 0.6 + kp *= 0.8 # 原0.1→0.8,大幅提高摆动腿增益 + kd *= 0.9 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" + "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 + "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 } - 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: + 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.foot_contact[0]}, 左脚接触: {self.foot_contact[1]}") + self.prev_com = com return torques def simulate_stable_standing(self): - """运行稳定站立仿真""" - # 优化仿真参数 - 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("G1机器人稳定站立仿真启动...") - print("适配g1_23dof模型 | 23自由度控制 | 足部接触检测") + # 优化相机视角 + v.cam.distance = 3.0 # 拉远相机,方便看行走 + v.cam.azimuth = 90 + v.cam.elevation = -25 + v.cam.lookat = [0, 0, 0.6] - # 初始落地阶段 + print("G1机器人稳定站立+行走仿真启动...") + print(f"初始稳定{self.init_wait_time}秒后自动开始行走") + + # 初始落地阶段(更强阻尼) start_time = time.time() while time.time() - start_time < self.init_wait_time: - self.data.ctrl[:] = 0.0 + 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.9 # 速度衰减,减少冲击 + self.data.qvel[:] *= 0.97 # 更强的速度衰减,防止初始后仰 v.sync() - time.sleep(0.01) + 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) - # 状态监测(每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 | 重心(z):{com[2]:.3f}m | " + 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(0.001) + time.sleep(self.dt * 0.5) # 跌倒判定 com = self.data.subtree_com[0] euler = self._get_root_euler() - if com[2] < 0.5 or abs(euler[0]) > 0.6 or abs(euler[1]) > 0.6: + 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" @@ -391,7 +421,7 @@ 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") # 适配G1模型文件名 + model_file_path = os.path.join(current_directory, "g1_23dof.xml") print(f"模型路径:{model_file_path}") if not os.path.exists(model_file_path): @@ -399,6 +429,9 @@ def simulate_stable_standing(self): try: stabilizer = G1Stabilizer(model_file_path) - stabilizer.simulate_stable_standing() + stabilizer.simulate_stable_standing() # 原方法不变,内部已集成行走 except Exception as e: - print(f"错误:{e}") \ No newline at end of file + print(f"错误:{e}") + import traceback + + traceback.print_exc() \ No newline at end of file diff --git a/src/Mujoco_manrun/mimic_sites.xml b/src/Mujoco_manrun/mimic_sites.xml new file mode 100644 index 0000000000..0228eefe9b --- /dev/null +++ b/src/Mujoco_manrun/mimic_sites.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/Mujoco_manrun/scene.xml b/src/Mujoco_manrun/scene.xml new file mode 100644 index 0000000000..a279b8d689 --- /dev/null +++ b/src/Mujoco_manrun/scene.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file From f31ffeafc9321b0a6c4fc05b01a859aad1f7806a Mon Sep 17 00:00:00 2001 From: kakayishi <3311973426@qq.com> Date: Wed, 17 Dec 2025 16:31:24 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E6=9C=BA?= =?UTF-8?q?=E5=99=A8=E4=BA=BA=E7=9A=84=E7=A8=B3=E5=AE=9A=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mujoco_manrun/main.py | 516 +++++++++++++++++++++----------------- 1 file changed, 279 insertions(+), 237 deletions(-) diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 29db331ce3..3e74d0510f 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -5,11 +5,11 @@ import os -class G1Stabilizer: - """修复踮脚后仰的G1机器人稳定站立控制器(新增行走功能)""" +class HumanoidStabilizer: + """适配humanoid.xml的高稳定性行走控制器(修复com_target属性错误)""" def __init__(self, model_path): - # 类型检查与模型加载 + # 基础初始化(保留原有核心逻辑) if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") @@ -19,147 +19,164 @@ 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 # 降低容差,提升计算精度 - # 关节名称映射 + # 2. 关节名称映射(保留适配humanoid.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" + "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 # 降低接触阈值,提高检测灵敏度 - - # 状态变量 + # 3. 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 + + # 4. 重心控制优化:动态目标+速度反馈 + 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 # 提高接触阈值,减少误判 + + # 5. 状态变量优化:新增滤波和平滑参数 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 # 积分死区,小误差不积分 + + # 6. 步态规划优化:平滑插值+慢周期 + 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 # 关节目标低通滤波系数 + + # 7. 力矩输出滤波:减少力矩突变 + 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 +191,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. 躯干姿态控制(更保守) + # 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. 重心补偿(更柔和) + # 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. 关节控制(重点优化踝关节) + # 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 +464,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 = HumanoidStabilizer(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