搭一套双臂移动操作平台的仿真，最让人头疼的从来不是"把模型导进来"，而是模型进来之后那堆说不清道不明的抖动、穿模、末端漂移。我前后在 MuJoCo 里折腾过四五版 Dual-UR5-Husky 的配置，从最早的"两条 UR5 焊死在 Husky 上、跑两步就翻车"，到后来能稳定跑双臂协同抓取和移动操作任务，中间踩的坑基本能写一本小册子。这篇就把这套Dual-UR5-Husky-MuJoCo模型从头拆一遍：它是什么、为什么这么搭、关键参数怎么定、代码怎么落地、出问题怎么查。不管你是在做移动操作算法验证、双臂协调控制研究，还是只想找个能跑通的仿真环境练手，下面的内容都能直接拿去复现。
先说清楚这套东西的定位。它本质上是一个**移动双臂操作平台（mobile dual-arm manipulator）**的物理仿真模型：底部是一台四轮差速移动底盘，上面并排立着两条六自由度机械臂，全部在 MuJoCo 的 MJCF 描述体系里用 XML 定义，配合 Python 接口做控制回路。它能干的事包括但不限于：验证底盘-机械臂耦合动力学、测试双臂协作的避碰与抓取规划、跑阻抗/导纳控制、给强化学习提供一个确定性的物理环境。适合的人群跨度挺大——刚接触 MuJoCo 的同学可以拿它当"多体系统建模"的完整范例，做控制的人可以拿它当算法验证台，做具身智能的可以拿它当训练场。
1. 这套双臂移动平台的整体设计与选型拆解
1.1 为什么要把双臂架到移动底盘上
固定基座的机械臂有个天然天花板：工作空间是一个以基座为球心的球壳，出了这个球壳什么都干不了。工业生产线上这个问题靠"工件送来送去"解决，但在服务、巡检、仓储这类场景里，让机器人自己走过去显然更划算。移动底盘把可达范围从"一个球"扩展成"整张地图"，这是移动操作最朴素的动机。
但只把单臂架上去，能力提升是线性的；把双臂架上去，能力提升是组合式的。双臂能做的事情单臂做不了：双手协同搬运大尺寸物体（比如一个箱子，单臂抓一侧会翻转）、双手操作柔性物体（折叠、拧、拉伸）、单手固定单手操作（拧螺丝、开柜门时另一只手扶住本体）、双臂互为冗余绕开奇异位形。这些能力在底盘移动的加持下，就从"够得着的范围内双手协作"变成了"整个场地内双手协作"。
代价也很直接。第一个代价是标定：底盘每移动一次，双臂相对于世界的位姿都变了，需要定位系统（轮式里程计、视觉、激光）实时提供基座位姿；而双臂之间的相对位姿虽然在机械上是固定的，但装配误差和底盘结构柔性会带来毫米级的偏差，在精细操作里这是致命的。第二个代价是稳定性：底盘靠四个轮子接触地面，接触面积小、支撑多边形窄，机械臂伸出去抓东西的时候重心偏移，很容易接近倾覆边界。第三个代价是耦合：机械臂加减速产生的反作用力会通过底盘传导到轮地接触点，引起打滑或者底盘点头，反过来又污染末端精度。
这三点决定了模型的搭建方式：不能简单地"底盘 + 机械臂"拼起来完事，必须把质量分布、接触参数、驱动限幅这些东西认真对待。后面几节会逐一展开。
1.2 MuJoCo 相对其他仿真器的取舍
选 MuJoCo 不是因为它最流行，而是因为它在"接触密集 + 需要高频控制回路"的场景里有明显优势。
首先说接触建模。MuJoCo 用的是软约束（soft constraint）方式处理接触，接触力由约束求解器在容差范围内求解，可以显式设置solref（时间常数和阻尼比）和solimp（阻抗参数）来调节接触的"软硬"。这带来的好处是数值上非常稳，不容易出现那种"轮子在地上疯狂抖动"的现象。躯干式仿真器里常见的接触穿透、能量注入问题，在 MuJoCo 里只要参数调对了基本不会出现。四轮底盘 + 双臂一共二十来个自由度、十来个接触点，这个规模 MuJoCo 在普通笔记本上跑 1kHz 实时完全没问题。
其次是计算性能。MuJoCo 的广义坐标动力学用递归算法实现，复杂度对自由度是线性的，二十几个自由度单步耗时在微秒级。这意味着你可以把仿真步长压到 0.5ms 甚至更小，这对研究轮地高频接触、机械臂高频抖动的场景很重要。
再就是不那么常被提到但很实用的一点：MuJoCo 提供逆向动力学相关的量。data.qfrc_bias直接给你科氏力、离心力加重力的合力项，mj_inverse可以算逆动力学，mj_jacSite/mj_jacBody给雅可比。写重力补偿、计算操作力、做阻抗控制的时候，这些东西省了大量手推公式的时间。
它的短板也要认：自带可视化只能算"够用"，做演示差点意思；场景资产（家具、工具、材质贴图）几乎没有，全靠自己建模；和 ROS 生态的对接不像 Gazebo 那样开箱即用，需要自己写桥接层。如果你的项目重度依赖 ROS 的现成工具链，可能需要额外的工作量。
1.3 模型文件的分层组织
一个实用的组织方式是把模型拆成三层，别写成一个巨型 XML。
- 组件层：
ur5.xml描述单条 UR5，只包含 body 树、关节、几何、执行器，用<default class="ur5">统一定义同类元素的属性；husky.xml描述底盘，包含车体、四个轮子、悬挂（可选）。 - 整机层：
dual_ur5_husky.xml用<include>把两个组件引进来，定义两臂的安装位置、底盘的初始位姿、整体的执行器和传感器。 - 场景层：
scene.xml引入整机模型，加上地面、光照、摄像机、可交互物体。
这么拆的好处很实际：调单臂参数的时候不用在几千行 XML 里翻找；想换底盘不用动机械臂；想做对比实验（单臂 vs 双臂）只要改整机层的 include。
提示：
<include>是文本级的拼接，被包含文件里如果定义了同名的<default>或其他顶层元素，会产生冲突。稳妥的做法是组件文件里只用带 class 的 default，且 class 名全局唯一。
2. 模型搭建中的核心参数与关键细节
2.1 UR5 的运动学参数与关节建模要点
UR5 是六轴串联臂，公开的 DH 参数大致是：d1 = 0.0892 m，a2 = -0.425 m，a3 = -0.3923 m，d4 = 0.1092 m，d5 = 0.0947 m，d6 = 0.0823 m。臂展约 0.85 m，本体重量 18.4 kg 左右，额定负载 5 kg。在 MJCF 里不需要严格按 DH 建，用 body 嵌套 + 相对位姿（pos和quat或euler）把每个连杆摆在正确位置就行，关节用<joint type="hinge" axis="0 0 1">。
有几个参数必须调，否则模型跑起来就是一团抖：
armature（电枢惯量）是第一个。它给关节加上一个附加的转动惯量，用来模拟电机转子、减速器的惯量。UR5 的输出端减速比很大（约 100:1 量级），转子惯量折算到关节侧很可观，如果不设，仿真出来的关节会比真机"轻"很多，高频响应的抖动会非常明显。实际操作中从armature="0.01"起调，往复运动时看关节速度曲线有没有高频毛刺，有就往上加。
damping（阻尼）是第二个。真实关节有减速器摩擦和粘滞阻尼，MuJoCo 里用damping近似粘滞部分，用frictionloss近似库仑摩擦。这两个值配合起来才能让关节"停下来就停住"，不会出现给零力矩还在慢慢滑的情况。经验值：damping="0.3~1.0"、frictionloss="0.05~0.2"，具体要看你的控制频率和增益。
几何体的碰撞代理是第三个，也是最容易被忽视的。视觉上用的精细网格（mesh）直接拿来做碰撞体，接触计算会变得又慢又不稳定。正确做法是每个连杆配一个简化的碰撞几何：连杆用 capsule（胶囊体），关节处用 sphere，末端用一个小 box 或 cylinder 近似夹爪。碰撞体的尺寸要略小于视觉网格，留出几毫米的余量，避免视觉上还没碰到、物理上已经顶上了。
2.2 Husky 底盘的运动学与接触参数
Husky 是四轮差速平台，公开参数大致是：车体长约 0.99 m、宽 0.67 m、高 0.39 m，自重 50 kg 量级，额定载荷 75 kg，最大速度约 1 m/s。轮子半径 0.165 m 左右，左右轮间距（track）约 0.555 m。
四个轮子分成左右两组，同侧两轮用同一个速度指令驱动。差速运动学很基础：
v = r * (ω_left + ω_right) / 2 ω_yaw = r * (ω_left - ω_right) / W其中 r 是轮半径，W 是左右轮中心距。反解就是给定期望的线速度 v 和角速度 ω_yaw，求两侧轮速：
ω_left = (v - ω_yaw * W / 2) / r ω_right = (v + ω_yaw * W / 2) / r在 MuJoCo 里，如果四个轮子都设成独立的 hinge 关节并分别给速度，转弯时前后轮会互相"较劲"——因为真实车体是刚性的，四个轮子不可能同时纯滚动，必然有滑移。这个问题有两种解法：一是把四个轮子的摩擦系数调低一点（friction="1.0 0.02 0.001"），让滑移自然发生，代价是直线行驶时会有轻微侧滑；二是用<equality>约束把同侧前后轮的关节角度绑定，只保留一个自由度，物理上更"干净"，但会牺牲一点真实感。
我一般选第一种，因为移动操作任务里底盘的定位精度本来就要靠上层定位系统补，轮地滑移的几毫米误差不影响大局，而且实现简单。但如果你的任务是"底盘停在精确位置然后双臂做精细操作"，第二种更合适。
轮子的碰撞几何用 cylinder，注意 cylinder 在 MuJoCo 里默认轴向是 z，要让轴向沿 y（车体横向），得旋转。另外condim建议设为 4（法向 + 两个切向摩擦 + 滚阻），滚阻系数给个小的值（比如 0.001），能让底盘停下来时不会一直微微滚动。
2.3 双臂基座布局与工作空间重叠的量化
两臂装多宽，直接决定这套平台能干什么。装太近，两臂互相碰撞；装太远，重叠工作空间太小，双臂协作就没意义了。
设单臂最大伸展半径 R = 0.85 m，两肩基座间距 d。以两基座连线的中点为原点，两臂的球形工作空间交叠区域的"半宽"可以近似为：
w = sqrt(R² - (d/2)²)代入几组数看看：
| 基座间距 d (m) | 交叠半宽 w (m) | 交叠区特征 |
|---|---|---|
| 0.4 | 0.822 | 极度重叠，自碰撞风险高，几乎必须常开避碰 |
| 0.6 | 0.789 | 重叠充分，适合双手协同操作 |
| 0.8 | 0.754 | 重叠适中，适合"分工协作"型任务 |
| 1.0 | 0.687 | 重叠收窄，仅前方中轴附近可协作 |
| 1.2 | 0.597 | 协作区很小，接近两台独立单臂 |
注意这个估算是把工作空间当球算的，实际 UR5 的工作空间是个复杂曲面，靠近基座的正下方和正后方是够不到的。所以真实可用重叠区比这个估算还要再小一点。
我的建议是 d 取 0.6~0.8 m。在这个区间里，两臂能在身前 0.5 m 左右的位置共同够到一个物体，同时肩部之间有足够的间隙容纳躯干旋转。安装高度也要考虑：如果两臂基座直接落在底盘顶面上，肩部高度大约在 0.55 m，末端向下够能到地面附近，向上够到 1.3 m 左右，覆盖桌面操作和地面拾取两个场景。
注意：两臂都朝向正前方时，前臂在身前中轴附近的碰撞风险最高。实操上给每臂设一个"默认收拢姿态"（肘部弯曲、末端收在肩前下方），在此基础上做规划，能显著降低初始穿模的概率。
2.4 执行器建模：位置驱动还是力矩驱动
MuJoCo 的<actuator>提供好几种驱动方式，最常用的是position、velocity和motor。
position执行器内部就是一个 PD 控制器，你给目标位置，它算输出力。上手快，适合快速验证运动学。缺点是它对所有场景用同一套增益，做力控、阻抗控制的时候不够灵活。
motor执行器是纯力矩源，你给多少力矩它就输出多少，PD 需要自己在 Python 里写。看起来麻烦，但换来的是完全的控制自由度——同一套模型可以随时切换成 PD 控制、阻抗控制、力矩前馈、甚至强化学习输出的原始动作。
我的做法是统一用motor，控制回路写在 Python 侧。这样调试的时候增益可以热改，不用重新加载模型。UR5 各关节的力矩限幅不一样（基座关节最大，腕部最小），ctrlrange要分开设：
| 关节 | 典型力矩上限 (N·m) | 说明 |
|---|---|---|
| shoulder_pan | ±150 | 承担整臂重量和负载，力矩需求最大 |
| shoulder_lift | ±150 | 同上 |
| elbow | ±100 | 中等 |
| wrist_1 | ±28 | 腕部，力矩小 |
| wrist_2 | ±28 | 同上 |
| wrist_3 | ±28 | 同上 |
这些值不是精确的铭牌参数，是按公开资料的量级估的，用来做仿真足够。真正要复现特定型号的性能，需要查数据手册。
3. 从零跑通仿真的完整实操流程
3.1 环境准备与依赖确认
环境这块没什么玄学，Python 3.9 以上，装三个包就够起步：
pip install mujoco numpyMuJoCo 从 2.3 版本开始完全开源，pip install mujoco直接能装，不需要额外配许可证文件。可视化用自带的mujoco.viewer，要更好的渲染效果再装dm_control或用离屏渲染。
验证安装：
import mujoco print(mujoco.__version__)版本建议 3.1 以上，这个版本之后mj_step的性能和接触求解器都有改进。如果你打算跑大规模并行（比如强化学习训练），可以看看 MJX，它把 MuJoCo 的计算图搬到了 JAX 上，能上 GPU 批量跑，但 API 和原生版差别不小，建议先把原生版跑通再考虑。
3.2 MJCF 主文件的结构骨架
整机 XML 的核心结构大致长这样，我把关键部分摘出来：
<mujoco model="dual_ur5_husky"> <compiler angle="radian" meshdir="assets" autolimits="true"/> <option timestep="0.002" integrator="implicitfast" solver="Newton" iterations="50" cone="elliptic" impratio="10"/> <default> <default class="ur5_arm"> <joint type="hinge" armature="0.01" damping="0.6" frictionloss="0.1"/> <geom type="capsule" density="1100" contype="1" conaffinity="2"/> <motor ctrllimited="true" ctrlrange="-150 150"/> </default> <default class="husky_wheel"> <joint type="hinge" armature="0.05" damping="0.8" frictionloss="0.4"/> <geom type="cylinder" size="0.165 0.06" friction="1.1 0.02 0.001" condim="4" contype="4" conaffinity="8"/> </default> </default> <worldbody> <geom name="floor" type="plane" size="10 10 0.1" contype="1" conaffinity="12" friction="1.0 0.01 0.001"/> <body name="husky_base" pos="0 0 0.2"> <!-- 车体、轮子 --> <body name="left_arm_mount" pos="0.05 0.35 0.35"> <include file="ur5.xml"/> </body> <body name="right_arm_mount" pos="0.05 -0.35 0.35"> <include file="ur5.xml"/> </body> </body> </worldbody> </mujoco>几个参数的取值逻辑值得说清楚。
timestep="0.002"对应 500 Hz 物理步进。UR5 的关节刚度在闭环下很高，1 ms 的步长会让显式积分器不稳定，2 ms 配合implicitfast积分器是个甜点。implicitfast对关节阻尼和被动力做隐式处理，稳定性比Euler好很多，速度又比RK4快。
solver="Newton"和cone="elliptic"是针对接触场景的。椭圆摩擦锥比默认的金字塔锥更接近真实摩擦行为，代价是求解稍慢；Newton 求解器在约束数量多的时候收敛快。impratio="10"提高法向约束相对于摩擦约束的阻抗比，能减少接触面"陷进去"的感觉。iterations="50"是安全值，实际二十来个自由度通常 30 次以内就收敛。
contype/conaffinity的位掩码设计是为了控制碰撞对，这个必须想清楚，否则要么该碰的不碰、要么不该碰的乱碰。规则是：两个 geom 之间发生碰撞检测的条件是(contype_A & conaffinity_B) != 0或(contype_B & conaffinity_A) != 0。
上面这套配置的含义是：
| 位 | 归属 | contype | conaffinity |
|---|---|---|---|
| 1 | 机械臂本体 / 地面 | 1 | 2 |
| 2 | 机械臂之间的互碰 | — | — |
| 4 | 轮子 | 4 | 8 |
| 8 | 地面 | — | 12 |
具体到实际效果：机械臂 contype=1、conaffinity=2，地面 contype=1、conaffinity=12，两者1 & 12 = 0、1 & 1 = 1，会检测——臂能碰到地面。轮子 contype=4、conaffinity=8，和地面8 & 1 = 0、4 & 12 = 4，会检测——轮子能压在地面上。而两臂之间1 & 2和1 & 2都非零，也会互检，这是我们要的。
如果想让两臂之间不做碰撞检测（比如做规划时想让它穿过去），把臂的 conaffinity 改成不含 2 的位就行。但正常跑物理仿真的时候不建议关，自碰撞检测是安全的最后一道防线。
3.3 加载模型与基础控制回路
Python 侧的骨架：
import mujoco import numpy as np model = mujoco.MjModel.from_xml_path("scene.xml") data = mujoco.MjData(model) # 找到每条臂的关节索引 def arm_qpos_idx(model, prefix): idx = [] for j in range(model.njnt): name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j) if name and name.startswith(prefix): idx.append(model.jnt_qposadr[j]) return np.array(idx) left_q = arm_qpos_idx(model, "left_") right_q = arm_qpos_idx(model, "right_") left_dof = left_q - model.jnt_qposadr[0] + 0 # 实际用 dofadr 更稳更稳妥的做法是直接用jnt_dofadr取速度索引，避免 qpos 和 qvel 索引错位（自由关节的 qpos 占 7 维、qvel 占 6 维，这种不一致最容易踩）。
PD 控制回路：
kp = np.array([200, 200, 180, 60, 40, 40], dtype=float) kd = np.array([12, 12, 10, 4, 2.5, 2.5], dtype=float) def pd_control(model, data, dof_idx, q_des, kp, kd, gravity_comp=True): q = data.qpos[dof_idx] dq = data.qvel[dof_idx] tau = kp * (q_des - q) - kd * dq if gravity_comp: tau += data.qfrc_bias[dof_idx] return tau增益怎么定？有个粗略的工程方法：先只给重力补偿，看关节能不能静止住；然后逐步加 kp，直到阶跃响应出现轻微超调；最后加 kd 把超调压下去。kp/kd 的比例大致满足kd ≈ 2 * sqrt(kp * m_eff)，m_eff是该关节驱动的等效惯量。实际调的时候这个公式只做起点，真正的判据是看data.qvel的曲线有没有高频振荡。
data.qfrc_bias这个量非常好用，它等于 C(q,q̇)q̇ + g(q)，直接拿来当前馈补偿，省去手动算重力项。要注意它包含的是整个模型的 bias 力，按自由度索引切片取你要的部分。
3.4 双臂协作的实现路径
双臂协作在控制层有三个递进的层次，随便挑一个起步都行。
第一层：独立位置控制 + 碰撞规避。两条臂各自跟踪自己的轨迹，用 MuJoCo 的接触检测（data.ncon、data.contact）实时监控，一旦接近碰撞就暂停或回退。这是最简单也最稳的做法，适合先把模型跑通。缺点是两臂只在"不打架"的层面协调，没有真正的协同。
第二层：主从协同。一条臂做主臂，另一条跟随，两者之间保持一个相对位姿约束。实现方式是在末端 site 上定义约束：
p_slave_des = p_master + R_master * offset_local然后用雅可比把末端误差映射回关节空间，作为从臂的修正量。这种模式适合"双手搬箱子"——箱子是刚体，两手之间的相对位姿固定。
第三层：阻抗/导纳控制。两臂各自对外呈现一个期望的刚度-阻尼特性，遇到外力时按预设的柔顺性让开。核心公式：
τ = Jᵀ ( K_p (x_d - x) + K_d (ẋ_d - ẋ) ) + τ_biasK_p和K_d是对角矩阵，分别控制三个平移方向和三个转动方向的刚度。做插孔、拧螺丝、开门这类任务时，把垂直于插入方向的刚度调低、沿插入方向调高，机器人在接触时会自己"找正"，不需要精确的位姿对齐。
雅可比直接调mj_jacSite：
jacp = np.zeros((3, model.nv)) jacr = np.zeros((3, model.nv)) mujoco.mj_jacSite(model, data, jacp, jacr, site_id) # 3x3 位置 + 3x3 姿态，拼成 6xn J = np.vstack([jacp[:, dof_idx], jacr[:, dof_idx]]) tau_task = J.T @ wrench_desired有个细节容易翻车：mj_jacSite算出的雅可比是相对于世界坐标系的，如果你的期望力是在末端坐标系里定义的，需要先乘一个旋转矩阵转换。转动的处理更麻烦，因为姿态误差不能用欧拉角直接相减，要用旋转矩阵的对数映射或者四元数的向量部分。这块建议直接用mujoco.mju_subQuat之类的工具函数，别自己手推。
3.5 底盘与机械臂的耦合稳定性处理
这是整套系统里最容易出问题的地方。表现是：底盘静止、机械臂快速伸展，车体开始前后点头，严重时轮子离地、模型开始抽搐。
根因是重心偏移导致的轮地法向力重新分配。机械臂伸出去 0.7 m，末端带 2 kg 负载，相对底盘中心产生的倾覆力矩是0.7 × 2 × 9.81 ≈ 13.7 N·m；如果整车重 70 kg、前后轮距 0.5 m，每侧轮子的法向力大概是 340 N 量级，倾覆力矩引起的法向力变化约 27 N，还不至于翻车。但如果机械臂是加速运动，惯性力矩能放大好几倍。
处理思路有四条，从易到难：
降低重心。把电池、配重块这些密度大的东西放在底盘底部。在 MJCF 里体现为把车体的pos调低、密度调高，或者单独加一个质心很低的 body。
限幅。给机械臂的关节速度和加速度设上限，在做移动操作时把速度压到最大值的 40% 左右。这个最省事，效果也最直接。
底盘支撑过渡。如果底盘带悬挂，可以调高悬挂刚度，让车体在受力时下沉得少一点；或者把轮子的solref调硬（比如0.01 1），减少接触变形。
解耦控制。在底盘控制器里加入机械臂状态的反馈：用mj_inverse算出机械臂对底盘基座的反作用力，前馈补偿到轮子的驱动上。这条最彻底，但实现复杂，一般研究项目不太需要做到这一步。
我在实测里的经验是：把timestep压到 0.001、轮地摩擦调到 1.1、车体密度加大 20%、关节速度限幅到 60%，基本就能把这套平台跑稳。再往上加复杂度，收益就开始递减了。
4. 常见问题与排查技巧实录
4.1 症状导向的速查表
跑了这么多版本，问题基本能归成几类。下面这张表是我自己的经验总结，遇到问题时先对照着定位方向。
| 症状 | 最可能的原因 | 排查方向 | 处理办法 |
|---|---|---|---|
| 模型加载后瞬间弹飞 | 初始位姿干涉，body 相互穿插 | 打印data.ncon和首个接触的 geom 名 | 调整初始pos，或开启mj_step前先做mj_forward检查 |
| 关节持续高频抖动 | 增益过高 / timestep 过大 / 缺 armature | 看data.qvel是否有高频分量 | 降 kp、减 timestep 到 0.001、加 armature |
| 底盘满载时缓慢下沉 | 轮地接触刚度过软 | 检查轮子solref | 调小solref时间常数，或提高impratio |
| 机械臂末端定位漂移 | 重力补偿缺失 / 积分器误差 | 静止时看data.qfrc_bias是否被用上 | 加前馈补偿，或换implicitfast积分器 |
| 转弯时底盘"跳" | 四轮运动学冲突 | 观察四个轮子的滑移率 | 降轮地摩擦、或改用 equality 约束联动 |
| 两臂自碰撞检测失效 | contype/conaffinity 位掩码配错 | 打印model.geom_contype数组 | 重新设计位掩码，确保互检位非零 |
| 仿真跑几秒就变慢 | 接触数量爆炸 / 求解器不收敛 | 打印data.ncon、data.solver_niter | 简化碰撞几何、加iterations、检查是否有穿模 |
| 视觉网格消失但碰撞还在 | mesh 尺寸与碰撞体不一致 | 关掉可视化看物理行为 | 统一视觉和碰撞的几何尺寸基准 |
4.2 数值稳定性问题的定位方法
MuJoCo 出问题的时候，最有效的手段是"打印中间量"。三个必看的量：
data.ncon是当前接触点数量。正常情况下这套平台静止时应该是 4（四个轮子各一个接触点，每个接触点可能有多个接触约束）。如果这个数突然变成几十，说明有东西穿进去了，去data.contact数组里挨个看geom1/geom2是谁。
data.solver_niter是求解器实际迭代次数。如果它经常顶到iterations上限，说明约束系统接近奇异，最常见的原因是接触点太多或者约束之间互相冲突（比如同一对 body 上同时有 weld 约束和接触）。
data.warning里的计数器，mujoco.mjtWarning枚举下有mwCONTACTFULL、mwBADQACC这些。出现CONTACTFULL说明接触数组满了，得调大nconmax。
for w in range(mujoco.mjtWarning.mjNWARNING): count = data.warning[w].number if count > 0: print(f"warning {w}: {count}")4.3 性能优化的几个实操手段
这套模型在普通笔记本上跑 1kHz 没什么压力，但如果要跑强化学习或者大规模参数扫描，就得考虑优化了。
第一步是砍碰撞几何。机械臂的连杆之间，如果某些组合永远不会碰到（比如基座和末端），可以从前面的位掩码里去掉。这个能省不少接触检测的开销。
第二步是关掉不需要的传感器。<sensor>里的东西每步都要计算，不用的话直接注释掉。尤其是touch和rangefinder这类。
第三步是选对积分器和求解器。implicitfast+Newton是通用场景的最佳组合；如果模型里约束很少（比如机械臂静态仿真），CG求解器更快。timestep也不是越小越好，0.002 通常就够了，压到 0.0005 只会让仿真变慢四倍，换不来什么精度提升。
第四步，如果确实需要大规模并行，可以考虑把模型导出成 MJX 格式在 JAX 上跑。但要注意 MJX 目前对某些特性（比如 tendon、某些类型的 equality 约束）支持不完整，移植前先确认你的模型用到的特性在支持列表里。
4.4 几个只有踩过才知道的坑
第一个坑：<include>的相对路径。<include file="ur5.xml"/>里的路径是相对于主文件所在目录解析的，不是相对于当前文件。所以当你把dual_ur5_husky.xml和ur5.xml放在不同目录下时，路径很容易写错。而且报错信息不一定会直接告诉你"文件找不到"，有时候是玄学的解析错误。
第二个坑：mesh 的坐标系。MuJoCo 的 mesh 加载进来后，默认原点在 STL/OBJ 文件的坐标原点。如果你的 CAD 导出时没有归零，模型会莫名其妙地偏出去好远。建议导出时就把每个连杆的局部坐标系原点定在关节轴线上，能给后面省大量时间。
第三个坑：关节限位的方向。range="-3.14 3.14"这类设置，如果 joint 的 axis 方向定义反了，限位也会反过来，表现是"关节转到某个角度就卡住"。UR5 的肩部关节实际可转范围是 ±360°，但仿真里如果给的是-1.57 1.57，那从视觉上就是转不过去。检查方法很简单：print(model.jnt_range)。
第四个坑：两臂命名冲突。用<include>引入两次同一个ur5.xml时，里面所有 body、joint、geom 的名字都会重复。MuJoCo 允许重名但会警告，而且按名字查找时永远只能拿到第一个。正确做法是在 include 之前把名字改掉，或者干脆复制两份、手动加left_/right_前缀。
第五个坑：mj_step和mj_forward的区别。mj_step是完整的一步仿真（前向动力学 + 积分），mj_forward只是算一遍动力学量但不推进时间。如果你只改ctrl然后调用mj_forward，模型是不会动的。调试控制律的时候经常犯这个错。
5. 这套模型还能往哪些方向扩展
跑通基础版本之后，往上长东西的空间挺大。
一个自然的方向是接入外部定位。现在底盘的位置是从仿真真值里直接读的，真实系统里需要靠轮式里程计 + 视觉 + 激光融合。可以写一个简单的里程计模拟器，给轮子速度加上噪声和漂移，再看看机械臂的末端定位误差怎么累积——这个实验能很直观地说明"为什么移动操作需要闭环定位"。
另一个是任务层的搭建。用 MuJoCo 的 mocap body 定义目标位姿，写一个简单的逆运动学求解器（阻尼最小二乘dq = Jᵀ(JJᵀ + λI)⁻¹ e，λ 取 0.01 左右防奇异），然后就能做"末端跟踪目标点"的完整闭环。把两条臂的目标点分开设，就能观察双臂协作时的相互影响。
再往后可以接强化学习。这套模型的状态空间大概 40 维（底盘位姿 7 + 四轮速度 4 + 两臂关节位置速度 24 + 少量接触信息），动作空间 16 维（四轮 + 12 个关节）。用 PPO 跑底盘导航 + 机械臂避障的组合任务，或者用 SAC 跑阻抗参数的在线调节，都是合适的题目。要注意 reward 设计里一定要包含能量项和平滑项，否则学出来的策略会疯狂抖动。
最后一个不太起眼但很实用的方向是参数辨识。把仿真里的关节摩擦、armature、接触参数当成待辨识量，用真机数据去做拟合。MuJoCo 提供mjd_transitionFD这个接口，能一次性给出离散系统的状态转移矩阵，对基于梯度的参数优化非常友好。
我在实际使用这套模型的过程中体会到，仿真模型的"好不好用"和"精不精确"经常是两回事。参数调到和真机一模一样，未必就能做出好算法；反倒是把模型的层次划清楚、把关键的动力学项（重力、接触、摩擦限幅）保留下来，用起来更顺手。上面这些配置和代码在我的几轮迭代里都验证过，直接拿去改改就能用。真正花时间的往往不是建模本身，而是搞清楚"为什么这么建"——这也是我把每个参数取舍都展开写的理由。
