Mujoco轨迹控制问题：如何正确设置刮刀架的控制量？

我是MuJoCo新手，已创建一个包含固定桌子、可移动书本及带驱动刮刀架的仿真场景，场景XML及初始化代码如下：

xml = """
<mujoco>
  <worldbody>
    <light name="top" pos="0 0 1"/>
    
    <body name="table" pos="0 0 0.025">
      <geom name="plate" type="box" size="0.25 0.2 0.025" rgba=".8 .8 .8 1"/>
      <geom name="bound0" type="box" size=".25 .01 .05" pos="0 -.19 .075"/>
      <geom name="bound1" type="box" size=".01 .19 .05" pos="-.24 .01 .075"/>
    </body>

    <body name="booklink" pos="0 0 0.065">
      <freejoint/>
      <geom name="book" type="box" size=".1 .05 .0125" rgba="0 1 0 1" mass=".1"/>
    </body>  

    <body name="spatulalink" pos="0 0 .2">
        <joint name="transX" type="slide" axis="1 0 0" limited="true" range="-.6 .6"/>
        <joint name="transY" type="slide" axis="0 1 0" limited="true" range="-.6 .6"/>
        <joint name="transZ" type="slide" axis="0 0 1" limited="true" range="-.6 .6"/>
        <joint name="hingeX" type="hinge" axis="1 0 0" limited="true" range="-3.2 3.2"/>
        <joint name="hingeY" type="hinge" axis="0 1 0" limited="true" range="-3.2 3.2"/>
        <joint name="hingeZ" type="hinge" axis="0 0 1" limited="true" range="-3.2 3.2"/>
        <geom name="spatula" type="box" size=".05 .05 5e-4" rgba="1 1 0 1" friction=".1 .1 .1" mass=".1"/>
    </body>
  </worldbody>
  <actuator>
    <position ctrllimited="true" ctrlrange="-.6 .6" joint="transX"/>
    <position ctrllimited="true" ctrlrange="-.6 .6" joint="transY"/>
    <position ctrllimited="true" ctrlrange="-.6 .6" joint="transZ"/>
    <position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeX"/>
    <position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeY"/>
    <position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeZ"/>
  </actuator>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

最初我直接修改data.qpos和data.qvel实现运动，虽能得到预期轨迹，但出现物体穿模碰撞问题（黄色刮刀穿过边界）。

之后了解到控制MuJoCo关节/驱动器的正确方式是设置data.ctrl，于是通过B样条生成参考轨迹并设置控制量，代码如下：

# Construct a BSpline to interpolate the via-points
spline_ref = BSpline_reference(data.qpos[7:], data.qvel[6:], data.time)
spline_ref.append(path, times, data.time)

with mujoco.viewer.launch_passive(model, data) as viewer:
  # Close the viewer automatically after 30 wall-seconds.
  start = time.time()
  sim_steps = int(n_steps // tau)
  i = 0
  while viewer.is_running(): #and i < sim_steps:
    # Get a target position from the reference spline using current sim state
    qreal = data.qpos[7:]
    qDot_real = data.qvel[6:]
    qref, qDotref = spline_ref.getReference(qreal, qDot_real, data.time)
    data.ctrl = q_ref
    mujoco.mj_step(model, data)

    # Pick up changes to the physics state, apply perturbations, update options from GUI.
    viewer.sync()

    # Slow down sim. for visualization
    time.sleep(1e-2)
    i += 1

但该方式得到的运动效果与直接修改data.qpos时完全不同，特此请教如何正确设置控制量以实现预期轨迹控制？

1. 理解位置控制器的本质 #

你使用的<position>类型驱动器是闭环位置控制器，和直接修改data.qpos的强制瞬移完全不同：

• 直接改data.qpos是跳过物理引擎的约束和碰撞检测，强制关节到目标位置，必然会穿模；
• 位置控制器是通过计算关节力矩，让关节逐渐向目标位置移动，会遵守物理规则（碰撞、摩擦力、惯性等），所以运动效果和瞬移完全不同是正常的。

2. 修正轨迹索引错误 #

你的代码中存在两处关键错误，导致参考轨迹无法正确控制关节：

• booklink的freejoint占用了6个qpos维度（3个平移+3个旋转），所以刮刀架的关节位置应该从data.qpos[6:]开始，而不是data.qpos[7:]；
• 代码中data.ctrl = q_ref是变量名拼写错误，应该是data.ctrl = qref，这会导致控制量没有正确设置。

修正后的代码片段：

# Construct a BSpline to interpolate the via-points
spline_ref = BSpline_reference(data.qpos[6:], data.qvel[6:], data.time)
spline_ref.append(path, times, data.time)

with mujoco.viewer.launch_passive(model, data) as viewer:
    start = time.time()
    sim_steps = int(n_steps // tau)
    i = 0
    while viewer.is_running():
        # 获取当前刮刀关节状态
        qreal = data.qpos[6:]
        qDot_real = data.qvel[6:]
        # 获取参考轨迹
        qref, qDotref = spline_ref.getReference(qreal, qDot_real, data.time)
        # 修正变量名拼写
        data.ctrl = qref
        mujoco.mj_step(model, data)
        
        viewer.sync()
        time.sleep(1e-2)
        i += 1

3. 调整控制器增益提升跟踪精度 #

默认的位置控制器增益kp较低，导致关节跟踪参考轨迹的速度慢，和瞬移效果差异大。可以在XML的<actuator>标签中添加kp参数（比例增益），数值越大跟踪越精准，但过大容易震荡：

<actuator>
  <position ctrllimited="true" ctrlrange="-.6 .6" joint="transX" kp="200"/>
  <position ctrllimited="true" ctrlrange="-.6 .6" joint="transY" kp="200"/>
  <position ctrllimited="true" ctrlrange="-.6 .6" joint="transZ" kp="200"/>
  <position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeX" kp="100"/>
  <position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeY" kp="100"/>
  <position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeZ" kp="100"/>
</actuator>

• 平移关节可以用较高的kp（比如200），旋转关节用稍低的数值（比如100），根据实际仿真效果微调。

4. 优化轨迹与仿真参数 #

• 确保B样条生成的轨迹是速度、加速度连续的，避免突变，否则控制器会产生过大力矩，导致运动不稳定；
• 如果出现轻微碰撞穿透，可以减小仿真步长model.opt.timestep（比如从默认的0.002改成0.001），提升碰撞检测的精度；
• 不要让参考轨迹的速度过快，超出控制器的跟踪能力，否则会出现明显的轨迹偏差。

5. 碰撞问题的根本解决 #

直接修改data.qpos的穿模问题是因为跳过了物理约束，使用控制器后，只要物理参数（摩擦、质量、关节限制）设置正确，MuJoCo会自动处理碰撞，不会出现穿模。如果仍然有穿模，检查：

• 几何模型的size是否正确，是否存在重叠；
• 仿真步长是否足够小；
• 控制器的kp是否足够大，能及时响应碰撞力。

内容的提问来源于stack exchange，提问作者HansDoe