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