这一章要解决什么问题：前四章你已经能让机器人动起来、看得见。
但要做真正的任务（抓取、放置、力控、强化学习），还缺四块拼图：

  1. 怎么读出机器人到底发生了什么（传感器）
  2. 怎么知道谁碰到了谁、用了多大力（接触）
  3. 怎么选对执行器（位置 / 力矩 / 速度）
  4. 怎么让物体粘在夹爪上（约束与焊接）

最后再把这些能力跑得够快。

配套代码：[code/ch14_mujoco_advanced.py]

"""
第 14 章配套代码：MuJoCo 进阶（传感器 / 接触 / 执行器 / 约束 / 性能）。

运行：
    D:\\Environment\\dm_control_env\\python.exe ch14_mujoco_advanced.py

内容：
  14.1 传感器：sensordata 扁平数组、sensor_adr/sensor_dim 切片、12 类传感器语义实测
       ⚠️ 实测：noise 与 cutoff 属性【不会】自动生效
  14.2 接触：ncon/contact 字段、frame 的【行】是轴（第 0 行是法线）、
       mj_contactForce 分量顺序是 [法向, 切向1, 切向2]、4 个接触点分摊 mg、
       冲击峰值 25.19 倍、摩擦锥
  14.3 执行器：position/motor/velocity/general 对比、重力下垂公式、
       ⚠️ 实测：degree 模式下 range="-3 3" 只有 ±3°
  14.4 约束与焊接：weld 语义、eq_data 布局 [3:6]/[6:10]/[10]、
       ⚠️ 实验 A：直接激活（relpose 由编译器按初始位姿自动推断）实测瞬移 0.000593 m
  14.5 性能：迭代次数几乎无效、步长才是关键、实时因子
"""
import os
import time
import numpy as np
import mujoco
from scipy.spatial.transform import Rotation as Rot

np.set_printoptions(precision=5, suppress=True)

HERE = os.path.dirname(os.path.abspath(__file__))
OUT_DIR = os.path.join(HERE, "..", "outputs")
os.makedirs(OUT_DIR, exist_ok=True)

MJCF = os.path.abspath(os.path.join(HERE, "..", "..", "models",
                                    "cx4_a601c_simulation.xml"))
model = mujoco.MjModel.from_xml_path(MJCF)
data = mujoco.MjData(model)

# 本书约定：Z-up，plane 的默认法线就是 +Z，euler="0 0 0" 即为水平地面
# （Y-up 项目才需要绕 X 转 -90° 把法线转到 +Y）
# ⚠️ euler 的数值单位由 compiler/angle 决定，本项目统一 radian
GROUND = '<geom name="floor" type="plane" size="2 2 0.1" euler="0 0 0"/>'
HEAD = '<compiler angle="radian"/>\n  <option timestep="0.001" gravity="0 0 -9.81"/>'

SENSOR_TYPE = {int(v): k for k, v in vars(mujoco.mjtSensor).items()
               if k.startswith("mjSENS_")}


def banner(title):
    print("\n" + "=" * 72)
    print(title)
    print("=" * 72)


# ============================================================
# 14.1  传感器
# ============================================================
banner("14.1  传感器系统：sensordata 是一个扁平数组")

print(f"项目模型: nsensor = {model.nsensor}, nsensordata = {model.nsensordata}")
print("\n读取三要素：model.sensor_adr（起始下标）/ sensor_dim（长度）/ sensor_type（类型）\n")
print(f"  {'i':>2} {'name':>12} {'type':>22} {'adr':>4} {'dim':>4}")
for i in range(model.nsensor):
    nm = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i)
    print(f"  {i:>2} {str(nm):>12} {SENSOR_TYPE.get(int(model.sensor_type[i]), '?'):>22} "
          f"{int(model.sensor_adr[i]):>4} {int(model.sensor_dim[i]):>4}")

data.qpos[:6] = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
mujoco.mj_forward(model, data)
ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
oc_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "object_center")
print("\n切片 = 真值 校验:")
print(f"  sensordata[0:6]  = {data.sensordata[0:6]}")
print(f"  qpos[:6]         = {data.qpos[:6]}      -> "
      f"{'一致' if np.allclose(data.sensordata[0:6], data.qpos[:6]) else '不一致'}")
print(f"  sensordata[6:9]  = {data.sensordata[6:9]}")
print(f"  ee site_xpos     = {data.site_xpos[ee_id]}  -> "
      f"{'一致' if np.allclose(data.sensordata[6:9], data.site_xpos[ee_id]) else '不一致'}")

# ---- 12 类传感器全家桶 ----
SENS_XML = f"""
<mujoco model="sensorlab">
  {HEAD}
  <worldbody>
    {GROUND}
    <body name="plate" pos="0 0 0.05">
      <geom name="plateg" type="box" size="0.15 0.15 0.05" mass="5.0"/>
      <site name="top_site" pos="0 0 0.05" size="0.03"/>
    </body>
    <body name="ball" pos="0 0 0.45">
      <freejoint/>
      <geom name="ballg" type="sphere" size="0.08" mass="0.5"/>
      <site name="ball_site" pos="0 0 0" size="0.03"/>
    </body>
    <body name="slider" pos="0.5 0 0.08">
      <joint name="jx" type="slide" axis="1 0 0" limited="false"/>
      <geom name="sliderg" type="box" size="0.08 0.08 0.08" mass="1.0"/>
      <site name="force_site" pos="0 0 0" size="0.02"/>
    </body>
  </worldbody>
  <actuator><motor name="m_x" joint="jx" gear="1" ctrllimited="true"
                   ctrlrange="-50 50"/></actuator>
  <sensor>
    <jointpos    name="s_jpos"  joint="jx" noise="0.01"/>
    <jointvel    name="s_jvel"  joint="jx" noise="0.05"/>
    <actuatorfrc name="s_afrc"  actuator="m_x"/>
    <framepos    name="s_ball"  objtype="site" objname="ball_site"/>
    <framelinvel name="s_ballv" objtype="site" objname="ball_site"/>
    <framequat   name="s_quat"  objtype="site" objname="ball_site"/>
    <accelerometer name="s_acc" site="ball_site"/>
    <gyro        name="s_gyro"  site="ball_site"/>
    <velocimeter name="s_vel"   site="ball_site"/>
    <touch       name="s_touch" site="top_site"/>
    <force       name="s_force" site="force_site"/>
    <torque      name="s_torque" site="force_site"/>
  </sensor>
</mujoco>
"""
ms = mujoco.MjModel.from_xml_string(SENS_XML)
ds = mujoco.MjData(ms)
ADR = {mujoco.mj_id2name(ms, mujoco.mjtObj.mjOBJ_SENSOR, i):
       (int(ms.sensor_adr[i]), int(ms.sensor_dim[i])) for i in range(ms.nsensor)}
rd = lambda n: ds.sensordata[ADR[n][0]:ADR[n][0] + ADR[n][1]]   # noqa: E731

print("\n12 类传感器布局（按 XML 顺序紧凑排列）:")
for nm, (a, n) in ADR.items():
    print(f"  sensordata[{a}:{a+n}]  <- {nm}")

print("\n球自由落体 -> 撞到托盘 -> 静止，观察各传感器:")
print(f"  {'t(s)':>5} {'球 z':>8} {'|加速度|':>9} {'|线速度|':>9} {'touch':>8}")
peak_acc = 0.0
for k in range(1500):
    ds.ctrl[0] = 3.0
    mujoco.mj_step(ms, ds)
    peak_acc = max(peak_acc, np.linalg.norm(rd("s_acc")))
    if k % 150 == 0:
        print(f"  {ds.time:>5.2f} {ds.qpos[2]:>8.4f} "
              f"{np.linalg.norm(rd('s_acc')):>9.4f} "
              f"{np.linalg.norm(rd('s_vel')):>9.4f} "
              f"{rd('s_touch')[0]:>8.4f}")
print(f"\n  球静止在 z = {ds.qpos[2]:.6f}（托盘顶面 0.10 + 球半径 0.08 = 0.18）")
print("  ⚠️ 加速度计读的是【比力 proper acceleration】，不是运动学加速度：")
print(f"     自由落体时 = 0（失重）  撞击峰值 = {peak_acc:.2f}  静止时 = 9.81（读的是 g）")
print(f"  touch 静止读数 = {rd('s_touch')[0]:.4f} N，球质量 0.5 kg -> mg = {0.5*9.81:.4f} N")

# 语义验证
SPIN = f"""
<mujoco model="spin">
  {HEAD}
  <worldbody>
    <body name="b" pos="0 0 0.3">
      <joint name="jz" type="hinge" axis="0 0 1" limited="false"/>
      <geom type="sphere" size="0.08" mass="0.5" pos="0.2 0 0"/>
      <site name="spin_site" pos="0.2 0 0" size="0.03"/>
    </body>
  </worldbody>
  <actuator><velocity name="v" joint="jz" kv="5"/></actuator>
  <sensor>
    <jointvel name="s_jv" joint="jz"/>
    <framelinvel name="s_flv" objtype="site" objname="spin_site"/>
    <velocimeter name="s_vel" site="spin_site"/>
    <gyro name="s_gyro" site="spin_site"/>
    <framequat name="s_fq" objtype="site" objname="spin_site"/>
  </sensor>
</mujoco>
"""
mp = mujoco.MjModel.from_xml_string(SPIN)
dp = mujoco.MjData(mp)
AP = {mujoco.mj_id2name(mp, mujoco.mjtObj.mjOBJ_SENSOR, i):
      (int(mp.sensor_adr[i]), int(mp.sensor_dim[i])) for i in range(mp.nsensor)}
rp = lambda n: dp.sensordata[AP[n][0]:AP[n][0] + AP[n][1]]     # noqa: E731
dp.ctrl[0] = 2.0
for _ in range(2000):
    mujoco.mj_step(mp, dp)
sidp = mujoco.mj_name2id(mp, mujoco.mjtObj.mjOBJ_SITE, "spin_site")
q_wxyz = np.roll(Rot.from_matrix(dp.site_xmat[sidp].reshape(3, 3)).as_quat(), 1)
print("\n传感器语义实测（球以 2 rad/s 绕 Z 轴自转，site 距轴 0.2 m）:")
print(f"  qvel[0]            = {dp.qvel[0]:+.6f}   jointvel = {rp('s_jv')[0]:+.6f}  "
      f"-> {'相等' if abs(rp('s_jv')[0]-dp.qvel[0]) < 1e-9 else '不等'}")
print(f"  gyro (局部角速度)  = {rp('s_gyro')}   -> 期望 [0, 0, 2]")
print(f"  framelinvel (世界) = {rp('s_flv')}   |v| = {np.linalg.norm(rp('s_flv')):.4f}")
print(f"  velocimeter (局部) = {rp('s_vel')}   |v| = {np.linalg.norm(rp('s_vel')):.4f}")
print(f"  -> 二者模长相等（都是 0.4 = ω×r），但坐标系不同：velocimeter 是局部系")
print(f"  framequat (wxyz)   = {rp('s_fq')}")
print(f"  由 site_xmat 换算   = {np.round(q_wxyz, 6)}  -> "
      f"{'一致' if np.allclose(rp('s_fq'), q_wxyz, atol=1e-6) else '不一致'}")

print("\n⚠️ 实测踩坑：noise 与 cutoff 属性不会自动生效")
NOI = """
<mujoco model="n">
  <compiler angle="radian"/>
  <!-- 本项目统一使用 Z-up 重力约定 -->
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <body pos="0 0 0.1"><joint name="j" type="slide" axis="1 0 0" limited="false"/>
      <geom type="box" size="0.05 0.05 0.05" mass="1"/></body>
  </worldbody>
  <sensor>
    <jointpos name="noisy" joint="j" noise="0.05"/>
    <jointvel name="filt"  joint="j" cutoff="5"/>
  </sensor>
</mujoco>
"""
mn = mujoco.MjModel.from_xml_string(NOI)
dn = mujoco.MjData(mn)
print(f"  model.sensor_noise = {mn.sensor_noise}   sensor_cutoff = {mn.sensor_cutoff}")
vals = []
for _ in range(500):
    mujoco.mj_step(mn, dn)
    vals.append(dn.sensordata[0])
print(f"  静止关节 500 次采样: std = {np.std(vals):.8f}   (设定 noise = 0.05)")
print("  -> 标准差为 0，说明噪声没有被加入 sensordata")
print("  正确做法：自己加噪声")
print("      reading = data.sensordata[i] + np.random.normal(0, noise)")

# ============================================================
# 14.2  接触
# ============================================================
banner("14.2  接触力学：谁在碰谁、碰得多用力")

BOX = f"""
<mujoco model="box">
  {HEAD.replace('timestep="0.001"', 'timestep="0.002"')}
  <worldbody>
    {GROUND}
    <body name="obj" pos="0 0 0.06">
      <freejoint/>
      <geom name="objg" type="box" size="0.05 0.05 0.05" mass="1.0"/>
    </body>
  </worldbody>
</mujoco>
"""
mb = mujoco.MjModel.from_xml_string(BOX)
db = mujoco.MjData(mb)
for _ in range(1500):
    mujoco.mj_step(mb, db)

print(f"ncon = {db.ncon}   （一个立方体躺在地面上 -> 4 个角各 1 个接触点）\n")
tot = np.zeros(3)
for c in range(db.ncon):
    ct = db.contact[c]
    fr = np.array(ct.frame).reshape(3, 3)
    f = np.zeros(6)
    mujoco.mj_contactForce(mb, db, c, f)
    g1 = mujoco.mj_id2name(mb, mujoco.mjtObj.mjOBJ_GEOM, ct.geom1)
    g2 = mujoco.mj_id2name(mb, mujoco.mjtObj.mjOBJ_GEOM, ct.geom2)
    fw = fr.T @ f[:3]      # frame 的【行】才是轴：第0行=法线, 第1行=切向1, 第2行=切向2
    tot += fw
    print(f"  contact[{c}] {g1} <-> {g2}  dist = {ct.dist:+.7f}  dim = {ct.dim}")
    print(f"       pos(世界)      = {np.round(np.array(ct.pos), 5)}")
    print(f"       force(接触系)  = {np.round(f[:3], 5)}   <- [法向, 切向1, 切向2]")
    print(f"       force(世界系)  = {np.round(fw, 5)}")
    if c == 0:
        print(f"       frame 三行（世界系下的轴）:")
        for j, lab in enumerate(["第0行 = 法线", "第1行 = 切向1", "第2行 = 切向2"]):
            print(f"         {lab}: {np.round(fr[j, :], 5)}")
        print(f"       ⚠️ 错误写法 fr @ f = {np.round(fr @ f[:3], 5)}（行才是轴，列只是分量）")
        print(f"       ✅ 正确写法 fr.T @ f = {np.round(fr.T @ f[:3], 5)}")
print(f"\n  世界系合力 = {np.round(tot, 5)}   理论 mg = [0, 0, 9.81]  -> "
      f"{'吻合' if np.allclose(tot, [0, 0, 9.81], atol=1e-3) else '不吻合'}")
print("  ⚠️ 关键：法向力 1.0 kg 的物体被 4 个接触点平分，单点只有 mg/4")

print("\n冲击 vs 静止：接触力是【瞬时量】，不是平滑的")
mujoco.mj_resetData(mb, db)
db.qpos[2] = 0.30                 # 从 z=0.30 落下，静止位置 z=0.05，落差 0.25 m
peak, peak_t = 0.0, 0.0
for k in range(1500):
    mujoco.mj_step(mb, db)
    fn = 0.0
    for c in range(db.ncon):
        f = np.zeros(6)
        mujoco.mj_contactForce(mb, db, c, f)
        fn += f[0]
    if fn > peak:
        peak, peak_t = fn, db.time
print(f"  落差 0.25 m 撞地：峰值 = {peak:.4f} N @ t = {peak_t:.3f} s")
print(f"  静止值 = 9.8100 N   峰值/静止 = {peak/9.81:.2f} 倍")
print("  -> 别把单帧接触力当成「受力」，它本质上是 冲量/Δt")

print("\n摩擦锥：推力超过 μN 才滑动")
FRIC = f"""
<mujoco model="fric">
  {HEAD.replace('timestep="0.001"', 'timestep="0.002"')}
  <worldbody>
    <geom name="floor" type="plane" size="2 2 0.1" euler="0 0 0"
          friction="0.5 0.005 0.0001"/>
    <body name="box" pos="0 0 0.06">
      <freejoint/>
      <geom name="boxg" type="box" size="0.05 0.05 0.05" mass="1.0"
            friction="0.5 0.005 0.0001"/>
    </body>
  </worldbody>
</mujoco>
"""
mf = mujoco.MjModel.from_xml_string(FRIC)
df = mujoco.MjData(mf)
print(f"  质量 1 kg, μ = 0.5, N = 9.81 N  ->  理论滑动阈值 = {0.5*9.81:.4f} N\n")
print(f"  {'推力(N)':>8} {'位移(m)':>10}  状态")
for push in (3.0, 4.0, 4.5, 4.8, 5.0, 5.5, 7.0):
    mujoco.mj_resetData(mf, df)
    for _ in range(600):
        mujoco.mj_step(mf, df)
    x0 = df.qpos[0]
    for _ in range(1000):
        df.qfrc_applied[0] = push
        mujoco.mj_step(mf, df)
    dx = df.qpos[0] - x0
    print(f"  {push:>8.1f} {dx:>10.4f}  {'滑动' if abs(dx) > 0.05 else '静止(有微小蠕动)'}")
print("  -> 4.8 N 到 5.0 N 之间位移从 0.016 m 跳到 0.219 m，阈值 ≈ 4.9 N，与理论吻合")

print("\ntouch 传感器：直接读出某个 site 处的法向接触力")
TOUCH = """
<mujoco model="touch">
  {HEAD}
  <worldbody>
    {GROUND}
    <body name="plate" pos="0 0 0.05">
      <geom name="plateg" type="box" size="0.15 0.15 0.05" mass="5.0"/>
      <site name="top_site" pos="0 0 0.05" size="0.03"/>
      <site name="side_site" pos="0.15 0 0" size="0.03"/>
    </body>
    <body name="ball" pos="0 0 0.45"><freejoint/>
      <geom name="ballg" type="sphere" size="0.08" mass="{MASS}"/></body>
  </worldbody>
  <sensor>
    <touch name="s_top" site="top_site"/>
    <touch name="s_side" site="side_site"/>
  </sensor>
</mujoco>
"""
for MASS in (0.1, 0.5, 2.0):
    xml_t = (TOUCH.replace("{HEAD}", HEAD.replace('timestep="0.001"',
                                                  'timestep="0.002"'))
                  .replace("{GROUND}", GROUND)
                  .replace("{MASS}", str(MASS)))
    mt = mujoco.MjModel.from_xml_string(xml_t)
    dt = mujoco.MjData(mt)
    for _ in range(2500):
        mujoco.mj_step(mt, dt)
    print(f"  球质量 {MASS:>4} kg -> 顶面 site = {dt.sensordata[0]:>8.4f} N   "
          f"侧面 site = {dt.sensordata[1]:.4f} N   (理论 mg = {MASS*9.81:.4f} N)")
print("  -> touch 精确等于该处的法向接触力；没被压到的 site 读 0")

# ============================================================
# 14.3  执行器
# ============================================================
banner("14.3  执行器深入：位置 / 力矩 / 速度 / 手写 general")

ARM = """
<mujoco model="arm">
  <compiler angle="radian"/>
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <body name="link" pos="0 0 0">
      <joint name="j1" type="hinge" axis="0 1 0" limited="false"/>
      <geom name="g1" type="capsule" fromto="0 0 0 0 0 0.5" size="0.03" mass="2.0"/>
    </body>
  </worldbody>
  <actuator>{ACT}</actuator>
</mujoco>
"""
VARIANTS = [
    ("position  (内置PD)", '<position name="a" joint="j1" kp="200" kv="20"/>',
     0.8, "位置"),
    ("motor     (纯力矩)", '<motor name="a" joint="j1" gear="1"/>',
     0.0, "力矩"),
    ("velocity  (速度)  ", '<velocity name="a" joint="j1" kv="20"/>',
     0.0, "速度"),
    ("general   (affine)", '<general name="a" joint="j1" gaintype="affine" '
     'biastype="affine" gainprm="200 0 0" biasprm="0 -200 -20"/>', 0.8, "位置"),
    ("general   (NONE)  ", '<general name="a" joint="j1" gainprm="200 0 0" '
     'biasprm="0 -200 -20"/>', 0.8, "位置"),
]
print(f"  {'写法':<22} {'稳态 q':>12} {'说明':>10}")
for tag, act, ctrl, kind in VARIANTS:
    mm = mujoco.MjModel.from_xml_string(ARM.replace("{ACT}", act))
    dd = mujoco.MjData(mm)
    dd.qpos[0] = 0.05
    for _ in range(4000):
        dd.ctrl[0] = ctrl
        mujoco.mj_step(mm, dd)
    note = "发散！" if abs(dd.qpos[0]) > 3.2 else ("能定位" if kind == "位置" else "不能定位")
    print(f"  {tag:<22} {dd.qpos[0]:>12.4f} {note:>10}")
print("\n  ⚠️ 手写 <general> 默认是 biastype=NONE，PD 反馈被关掉 -> 直接发散")
print("  ⚠️ <velocity> 是【刹车】不是【定位】：gravity 会让它慢慢溜走")

print("\n重力下垂的定量公式：稳态误差 = |qfrc_bias| / kp")
mm = mujoco.MjModel.from_xml_string(
    ARM.replace("{ACT}", '<position name="a" joint="j1" kp="200" kv="20"/>'))
bias_pd = None
for tag, comp in [("纯 PD        ", False), ("PD + 重力补偿", True)]:
    dd = mujoco.MjData(mm)
    dd.ctrl[0] = 0.8
    for _ in range(4000):
        if comp:
            dd.qfrc_applied[0] = dd.qfrc_bias[0]
        mujoco.mj_step(mm, dd)
    print(f"  {tag}: q = {dd.qpos[0]:.6f}   误差 = {abs(dd.qpos[0]-0.8):.6f} rad")
    if not comp:
        bias_pd = dd.qfrc_bias[0]
print(f"  纯 PD 稳态处的重力力矩 qfrc_bias = {bias_pd:.4f} Nm，kp = 200")
print(f"  理论稳态误差 = |{bias_pd:.4f}| / 200 = {abs(bias_pd)/200:.6f} rad  -> 与实测吻合")
print("  重力补偿的做法：data.qfrc_applied[:nu] = data.qfrc_bias[:nu]（第 12 章讲过）")

print("\n⚠️ degree / radian 陷阱（本章最隐蔽的坑）")
DEG = """
<mujoco model="d">
  <compiler angle="{ANG}"/>
  <option timestep="0.001" gravity="0 0 -9.81"/>
  <worldbody>
    {GROUND2}
    <body name="b" pos="0 0.06 0.06"><freejoint/>
      <geom name="bg" type="box" size="0.05 0.05 0.05" mass="1.0"/></body>
  </worldbody>
</mujoco>
"""
# Z-up 下的单位陷阱演示：-1.570796 rad = -90°（平面立起来变墙），
# 而 degree 模式把它当成 -1.57°（几乎是平地）
GROUND2 = '<geom name="floor" type="plane" size="2 2 0.1" euler="-1.570796 0 0"/>'
for ang in ("radian", "degree"):
    mm = mujoco.MjModel.from_xml_string(
        DEG.replace("{ANG}", ang).replace("{GROUND2}", GROUND2))
    dd = mujoco.MjData(mm)
    for _ in range(1000):
        mujoco.mj_step(mm, dd)
    nrm = dd.geom_xmat[0].reshape(3, 3)[:, 2]
    print(f"  angle='{ang}': euler=\"-1.570796 0 0\" -> 地面法线 {np.round(nrm,4)}  "
          f"物体最终 z = {dd.qpos[2]:+8.4f}  ncon = {dd.ncon}")
print("  -> radian 模式：-1.570796 = -90°，地面立起来变成一面墙，物体直接掉下去")
print("     degree 模式：被当成 -1.57°，地面几乎还是平的，物体稳稳停住")
print("     同一个数，单位不同，物理完全两样；同理 range=\"-3 3\" 在 degree 模式下只有 ±3°（±0.0524 rad）")

RANGE = """
<mujoco model="r">
  <compiler angle="{ANG}"/>
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <body name="link"><joint name="j1" type="hinge" axis="0 1 0" range="-3 3"/>
      <geom type="capsule" fromto="0 0 0 0 0 0.5" size="0.03" mass="2.0"/></body>
  </worldbody>
  <actuator><position name="a" joint="j1" kp="200" kv="20"/></actuator>
</mujoco>
"""
for ang in ("degree", "radian"):
    mm = mujoco.MjModel.from_xml_string(RANGE.replace("{ANG}", ang))
    dd = mujoco.MjData(mm)
    dd.ctrl[0] = 0.5
    for _ in range(3000):
        mujoco.mj_step(mm, dd)
    print(f"  angle='{ang}': range=\"-3 3\" 实际 = {np.round(mm.jnt_range[0],5)}  "
          f"命令 0.5 rad -> 实际 q = {dd.qpos[0]:.5f}  "
          f"qfrc_constraint = {dd.qfrc_constraint[0]:+.2f}")
print("  -> degree 模式下关节被卡在 ±3°（0.0524 rad），执行器再使劲也转不动")

print("\n执行器输出力：data.actuator_force")
mujoco.mj_resetData(model, data)
data.ctrl[:6] = [0.3, 0.1, -0.2, 0, 0.2, 0]
for _ in range(1500):
    mujoco.mj_step(model, data)
kp0 = model.actuator_gainprm[0][0]
print(f"  ctrl = {np.round(data.ctrl[:6], 4)}")
print(f"  qpos = {np.round(data.qpos[:6], 4)}")
print(f"  actuator_force = {np.round(data.actuator_force, 4)}")
print(f"  手算 j1: kp*(ctrl-q) = {kp0:.0f}*({data.ctrl[0]:.4f}-{data.qpos[0]:.4f}) "
      f"= {kp0*(data.ctrl[0]-data.qpos[0]):.4f}  -> 与 actuator_force[0] 一致")

# ============================================================
# 14.4  约束与焊接
# ============================================================
banner("14.4  约束与焊接：weld 的正确打开方式")

OBJ_B = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object")
GM_B = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "gripper_mount")
print("项目模型里的 weld:")
print(f"  body1 = {mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, model.eq_obj1id[0])}"
      f"   body2 = {mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, model.eq_obj2id[0])}")
print(f"  eq_data[0] = {model.eq_data[0]}   (11 个数)")
print("  ⚠️ 布局（实测）：[0:3] 未使用 / [3:6] 平移 / [6:10] 四元数(wxyz) / [10] torquescale")

print("\nweld 语义（实测判定）：T_body2 = T_body1 @ T_relpose")
print("  即  p_body2 = p_body1 + R_body1 @ rel_p")
print("      R_body2 = R_body1 @ R_rel")


def compute_relpose(m, d):
    """抓取瞬间调用：算出物体相对夹爪座的位姿。"""
    i1, i2 = m.eq_obj1id[0], m.eq_obj2id[0]
    R1 = d.xmat[i1].reshape(3, 3).copy()
    R2 = d.xmat[i2].reshape(3, 3).copy()
    p1 = d.xpos[i1].copy()          # ⚠️ 必须 .copy()，d.xpos[i] 是视图！
    p2 = d.xpos[i2].copy()
    rel_R = R1.T @ R2
    rel_p = R1.T @ (p2 - p1)
    q_xyzw = Rot.from_matrix(rel_R).as_quat()
    return rel_p, np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]])  # -> wxyz


def weld_violation(m, d):
    """约束违反量：body2 实际位置 与 约束要求位置 的距离。"""
    i1, i2 = m.eq_obj1id[0], m.eq_obj2id[0]
    R1 = d.xmat[i1].reshape(3, 3)
    p1, p2 = d.xpos[i1], d.xpos[i2]
    q = m.eq_data[0][6:10]
    Rr = Rot.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
    return np.linalg.norm(p2 - (p1 + R1 @ m.eq_data[0][3:6]))


print("\n实验 A：不做任何处理，直接激活（relpose 由编译器按初始位姿自动推断）")
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)
p0 = data.xpos[OBJ_B].copy()
print(f"  激活前 物体 = {p0}   夹爪座 = {data.xpos[GM_B]}")
data.eq_active[0] = 1
for _ in range(200):
    data.ctrl[:6] = model.qpos0[:6]
    mujoco.mj_step(model, data)
jump = np.linalg.norm(data.xpos[OBJ_B] - p0)
print(f"  激活后 物体 = {data.xpos[OBJ_B]}")
print(f"  激活瞬移 = {jump:.6f} m  <- 自动推断的 relpose 与初始位姿一致，物体几乎不动"
      f"（若 XML 里真写了 identity relpose，物体才会被拽到夹爪座原点）")

print("\n实验 B：正确菜谱——抓取瞬间算出 relpose 写回 eq_data")
mujoco.mj_resetData(model, data)
# 故意把物体转一个角度，这样 rel_q 不是单位四元数，能真正验证旋转部分
_q = Rot.from_euler("y", 40, degrees=True).as_quat()
data.qpos[11:15] = np.array([_q[3], _q[0], _q[1], _q[2]])     # MuJoCo 用 wxyz
mujoco.mj_forward(model, data)
p0 = data.xpos[OBJ_B].copy()
rel_p, rel_q = compute_relpose(model, data)
model.eq_data[0][3:6] = rel_p
model.eq_data[0][6:10] = rel_q
print(f"  写入 rel_p    = {rel_p}")
print(f"  写入 rel_q    = {rel_q}  (wxyz)")
data.eq_active[0] = 1
for _ in range(200):
    data.ctrl[:6] = model.qpos0[:6]
    mujoco.mj_step(model, data)
drift = np.linalg.norm(data.xpos[OBJ_B] - p0)
print(f"  激活后 物体 = {data.xpos[OBJ_B]}")
print(f"  ✅ 瞬移 = {drift:.6f} m  （对比实验 A 的 {jump:.4f} m）")

print("\n实验 C：悬空搬运，检查刚体跟随精度")
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)
rel_p, rel_q = compute_relpose(model, data)
model.eq_data[0][3:6] = rel_p
model.eq_data[0][6:10] = rel_q
data.eq_active[0] = 1
data.qpos[8:11] = [-0.30, 0.30, 0.18]      # 把物体抬到空中
data.qpos[11:15] = [1, 0, 0, 0]
mujoco.mj_forward(model, data)
rel_p, rel_q = compute_relpose(model, data)   # 换位置后要重算！
model.eq_data[0][3:6] = rel_p
model.eq_data[0][6:10] = rel_q
p_start = data.xpos[OBJ_B].copy()
q_home = model.qpos0[:6].copy()
tgt = np.array([0.6, -0.2, 0.2, 0.0, 0.0, 0.0])
n_step = int(1.5 / model.opt.timestep)
for k in range(n_step):
    a = k / n_step
    data.ctrl[:6] = q_home * (1 - a) + tgt * a
    mujoco.mj_step(model, data)
print(f"  起始物体 {p_start}")
print(f"  搬运后物体 {data.xpos[OBJ_B]}   位移 {np.linalg.norm(data.xpos[OBJ_B]-p_start):.4f} m")
print(f"  约束违反量 = {weld_violation(model, data):.8f} m  -> 刚体跟随正常")

print("\n实验 D：用约束违反量做诊断（物体被顶死在地面上时）")
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)
rel_p, rel_q = compute_relpose(model, data)
model.eq_data[0][3:6] = rel_p
model.eq_data[0][6:10] = rel_q
data.eq_active[0] = 1
worst = 0.0
for k in range(500):
    data.ctrl[:6] = [0.0, -0.3, 0.3, 0.0, 0.0, 0.0]   # 这个指令其实是把臂往下压
    mujoco.mj_step(model, data)
    worst = max(worst, weld_violation(model, data))
print(f"  最大约束违反 = {worst:.5f} m   ncon = {data.ncon}")
print("  ⚠️ 违反量大 = 夹爪在往物体里怼（或物体被地面卡住）。")
print("     把它当成「抓取是否成功」的诊断指标，比肉眼看画面可靠得多。")

print("\n实验 E：solref 调优（约束刚度）")
print(f"  {'solref':>12} {'最大违反(m)':>13}")
for sr in ([0.02, 1.0], [0.01, 1.0], [0.005, 1.0], [0.002, 1.0]):
    m = mujoco.MjModel.from_xml_path(MJCF)
    m.eq_solref[0] = sr
    d = mujoco.MjData(m)
    mujoco.mj_forward(m, d)
    rp_, rq_ = compute_relpose(m, d)
    m.eq_data[0][3:6] = rp_
    m.eq_data[0][6:10] = rq_
    d.eq_active[0] = 1
    w = 0.0
    for _ in range(500):
        d.ctrl[:6] = [0.0, -0.3, 0.3, 0.0, 0.0, 0.0]
        mujoco.mj_step(m, d)
        w = max(w, weld_violation(m, d))
    print(f"  {str(sr):>12} {w:>13.5f}")
print("  -> solref 越小（时间常数越短）约束越硬，违反量越小")

# ============================================================
# 14.5  性能
# ============================================================
banner("14.5  性能调优：什么才真的有用")


def bench(m, d, n=20000, reps=5, settle=500, drive=False):
    best = 1e9
    for _ in range(reps):
        mujoco.mj_resetData(m, d)
        for _ in range(settle):
            mujoco.mj_step(m, d)
        t0 = time.perf_counter()
        for k in range(n):
            if drive:
                d.ctrl[:6] = 0.3 * np.sin(k * 0.02)
            mujoco.mj_step(m, d)
        best = min(best, (time.perf_counter() - t0) / n)
    return best * 1e6


print("\n  ⚠️ 实测：调大 solver iterations 几乎没有收益")
print(f"  {'iterations':>11} {'us/step':>9}")
for it in (1, 5, 20, 50, 100, 200):
    m = mujoco.MjModel.from_xml_path(MJCF)
    m.opt.iterations = it
    d = mujoco.MjData(m)
    print(f"  {it:>11} {bench(m, d, n=20000, reps=3, settle=600):>9.2f}")
print("  原因：MuJoCo 求解器残差够小就【提前退出】，默认 100 早就收敛了。")
print("  真正影响接触精度的是 solref / solimp / timestep，不是 iterations。")

print("\n  ✅ 实测：timestep 才是速度与精度的主开关")
print(f"  {'timestep':>9} {'us/step':>9} {'实时因子':>10} {'静止高度误差':>14}")
for ts in (0.0005, 0.001, 0.002, 0.005, 0.01):
    m = mujoco.MjModel.from_xml_path(MJCF)
    m.opt.timestep = ts
    d = mujoco.MjData(m)
    t = bench(m, d, n=20000, reps=3, settle=600)
    d2 = mujoco.MjData(m)
    d2.qpos[8:11] = [0.30, 0.20, 0.30]
    d2.qpos[11:15] = [1, 0, 0, 0]
    for _ in range(int(2.0 / ts)):
        mujoco.mj_step(m, d2)
    err = abs(d2.qpos[10] - 0.02) * 1000
    print(f"  {ts:>9} {t:>9.2f} {ts/t*1e6:>9.1f}x {err:>11.4f} mm")
print("  -> 每步耗时基本恒定，所以【大步长 = 更高实时因子】，代价是精度")
print("     本项目取 0.002 s（500 Hz）：误差 0.008 mm，实时因子见上方实测值（随机器性能变化）")

print("\n  mj_step vs mj_step1 + mj_step2")
m = mujoco.MjModel.from_xml_path(MJCF)
d = mujoco.MjData(m)
for _ in range(500):
    mujoco.mj_step(m, d)
t0 = time.perf_counter()
for _ in range(20000):
    mujoco.mj_step(m, d)
a = (time.perf_counter() - t0) / 20000 * 1e6
t0 = time.perf_counter()
for _ in range(20000):
    mujoco.mj_step1(m, d)
    mujoco.mj_step2(m, d)
b = (time.perf_counter() - t0) / 20000 * 1e6
print(f"    mj_step           = {a:6.2f} us")
print(f"    mj_step1+mj_step2 = {b:6.2f} us  (差 {b-a:+.2f} us，在噪声量级内)")
print("    -> 两者开销基本等价，拆开不亏。")
print("       拆开的唯一理由：在两步之间插入自定义逻辑（外部力、控制器、状态记录）")

print("\n  汇总（充分预热后测量，取多次最小值）")
for _ in range(3):                      # 预热
    bench(model, data, n=20000, reps=1, drive=True)
    bench(model, data, n=20000, reps=1)
us_d = bench(model, data, drive=True)
us_c = bench(model, data, settle=600)
print(f"    机械臂运动 + 无接触 : {us_d:6.2f} us/step   实时因子 "
      f"{model.opt.timestep/us_d*1e6:6.1f}x")
print(f"    物体静置接触        : {us_c:6.2f} us/step   实时因子 "
      f"{model.opt.timestep/us_c*1e6:6.1f}x")
print("    -> 接触对耗时几乎无影响；瓶颈在正向动力学（mj_step1）")

# ============================================================
# 14.7 动手练 参考答案
# ============================================================
banner("14.7 动手练 参考答案")
print("练习1 传感器切片 : 见 14.1 的 adr/dim 表与 12 类传感器布局")
print("练习2 接触力分量 : [法向, 切向1, 切向2]；世界系力见 14.2 的 4 点分摊 mg")
print("练习3 执行器对比 : position/general(affine) 能定位；general(NONE) 发散")
print("练习4 weld 抓取  : 直接激活(自动推断 relpose)瞬移 0.000593 m；抓取瞬间手算 relpose 0.009348 m")
print("练习5 性能权衡   : iterations 无效，timestep 才是主开关")

print("\n第 14 章示例代码运行完毕。")

📌 本章所有数字都在作者机器上实测得到（mujoco 3.11.0 / numpy 2.4.6 / Python 3.11.15）。
你的机器数值会略有差异，但结论和量级关系应当一致。

🎯 本章学习目标

学完本章，你将能够：

  1. 正确读取传感器：理解 sensordata 是扁平数组，会用 sensor_adr/sensor_dim 建名字映射表，知道 framelinvel（世界系）和 velocimeter（局部系）的区别，理解加速度计读的是"比力"不是运动学加速度。
  2. 分析接触力学：会遍历 data.contact，知道 frame 重塑 3×3 后行是轴向量（第 0 行是法线）、mj_contactForce 的顺序是 [法向, 切向1, 切向2]，会用 fr.T @ f[:3] 把接触力换算到世界系，知道一个箱子有 4 个接触点、接触力是瞬时量。
  3. 选对执行器类型：理解 <position>/<velocity>/<motor>/<general> 的区别，知道 <velocity> 是刹车不是定位，会避开 biastype 的坑，会用 actuator_force 读取实际输出。
  4. 用 weld 约束做抓取：理解 weld 的语义 T_body2 = T_body1 @ T_relpose，会在抓取瞬间计算相对位姿写入 eq_data，会用 weld_violation 做诊断，会调 solref 让约束更硬。
  5. 性能调优：知道 iterations 从 1 到 200 几乎没变化（求解器提前退出），timestep 才是实时因子的主开关，会在精度和速度之间做权衡。

📌 前置知识：本章需要第 10 章（核心三件套）、第 11 章（MJCF 建模，特别是执行器和约束）、第 12 章（仿真循环，特别是 qfrc_* 中间量）的基础。接触力学部分需要第 02 章（坐标系变换）的知识。

14.1 传感器：一个扁平数组

14.1.1 sensordata 不是字典

新手最容易犯的错：以为能写 data.sensor["ee_pos"]。不行。

MuJoCo 把所有传感器的输出紧挨着塞进一个一维数组 data.sensordata：

data.sensordata   # shape = (model.nsensordata,)，纯数字，没有名字

要取某个传感器，必须自己切：

adr = model.sensor_adr[i]     # 起始下标
dim = model.sensor_dim[i]     # 这个传感器占几个数
value = data.sensordata[adr:adr + dim]

推荐做法：启动时建一张名字到切片的映射表，之后就能按名字读了。

ADR = {}
for i in range(model.nsensor):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i)
    a, n = int(model.sensor_adr[i]), int(model.sensor_dim[i])
    ADR[name] = (a, n)

def read(name):
    a, n = ADR[name]
    return data.sensordata[a:a + n]

本项目模型实测（nsensor = 8，nsensordata = 12）：

inametypeadrdim
0pos_j1mjSENS_JOINTPOS01
1–5pos_j2…pos_j6mjSENS_JOINTPOS1…51
6ee_posmjSENS_FRAMEPOS63
7object_posmjSENS_FRAMEPOS93

校验结果（把关节设成 [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]）：

sensordata[0:6]  = [0.1 0.2 0.3 0.4 0.5 0.6]
qpos[:6]         = [0.1 0.2 0.3 0.4 0.5 0.6]      -> 一致
sensordata[6:9]  = [-0.30158 -0.05491  0.83329]
ee site_xpos     = [-0.30158 -0.05491  0.83329]  -> 一致

⚠️ 别猜类型编号。第 11 章踩过一次坑：我猜 type=9 是 JOINTVEL，实测是 JOINTPOS。
正确做法是用枚举反查表，见配套代码里的 SENSOR_TYPE 字典。

14.1.2 传感器类型速查（12 类实测）

MJCF 标签输出dim语义（实测）
<jointpos>关节位置1≡ qpos[joint]
<jointvel>关节速度1≡ qvel[joint]
<actuatorfrc>执行器输出1≡ data.actuator_force[i]
<framepos>点的位置3≡ site_xpos（世界系）
<framequat>点的朝向4≡ quat(site_xmat)，wxyz 顺序
<framelinvel>点的线速度3世界系线速度
<velocimeter>点的线速度3局部系线速度
<gyro>角速度3局部系角速度
<accelerometer>比力3见下方「加速度计的真相」
<touch>法向接触力1该 site 处的法向力，精确 = 载荷
<force>力3site 处传递的力
<torque>力矩3site 处传递的力矩

framelinvel vs velocimeter —— 最容易混的一对。实测（site 距转轴 0.2 m，以 2 rad/s 自转）：

framelinvel (世界系) = [ 0.30048 -0.26402  0.     ]   |v| = 0.4000
velocimeter (局部系) = [ 0.       0.4      0.     ]   |v| = 0.4000

模长都是 0.4（= ω×r = 2×0.2；Z-up 下绕 Z 轴自转，线速度在水平面内），但坐标系不同。做状态观测时选错坐标系，网络会学得很痛苦。

14.1.3 加速度计的真相

球从 0.45 m 落到托盘上，实测：

阶段|加速度|
自由落体0.0000
接触瞬间244.93
静止9.8100

🔥 加速度计读的是比力（proper acceleration），不是运动学加速度。

  • 自由落体 = 完全失重 → 读 0
  • 静止在地面 → 地面在往上推你 → 读 9.81

这不是 bug，这是真实 IMU 的物理行为。写观测归一化时别搞反。

14.1.4 ⚠️ 实测踩坑：noise 不会自动生效（但 cutoff 会）

MJCF 里写 noise="0.05" 看起来很美好：

<sensor>
  <jointpos name="jp" joint="j" noise="0.05"/>
  <jointvel name="jv" joint="j" cutoff="5"/>
</sensor>

实测结果：

model.sensor_noise  = [0.05 0.  ]    # 值确实被解析进来了
model.sensor_cutoff = [0.   5. ]
静止关节 500 次采样: std = 0.00000000   (设定 noise = 0.05)

标准差是 0。 sensordata 里是纯净值，噪声并没有被自动加进去。

cutoff 则相反——它真的生效：把关节速度设到 23994 rad/s，jointvel 传感器（cutoff=5）读出来是 5.0，超限读数被裁剪到了 ±cutoff。

⚠️ 结论：在 MuJoCo 3.11 里，sensordata 是真值——noise 只是存了个参数，
要加噪声就自己加：

reading = data.sensordata[i] + np.random.normal(0, noise)

这对强化学习反而是好事——仿真里加噪声、真机上再加一遍，不会重复污染。
另外记住 cutoff 会裁剪：高速关节的读数会「贴着天花板」，排查异常时先想到它。

14.2 接触力学：谁在碰谁，碰得多用力

14.2.1 data.contact 的字段

n = data.ncon                    # 当前接触点数量
ct = data.contact[i]             # 第 i 个接触
字段含义
ct.geom1 / ct.geom2两个 geom 的 id（用 mj_id2name 转名字）
ct.dist穿透深度，负值表示重叠（−0.0001 = 陷进去 0.1 mm）
ct.pos接触点世界坐标（3 维）
ct.frame接触坐标系（9 个数，见下）
ct.dim接触维度（3 = 纯法向+2 切向，4/6 会多出滚动/扭转摩擦）
ct.includemargin是否计入 margin
g1 = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_GEOM, ct.geom1)

接触检测示意图

          物体 A (box)
        ┌─────────────┐
        │             │
        │    ●────────┼──── 接触点 1 (角1)
        │    │        │
        │    │ 穿透   │
   ─────┼────┼────────┼───── 地面平面
        │    ▼        │
        │  接触点 2   │
        │  (角2)      │
        └─────────────┘
          物体 B (ground)

    每个接触点有一个局部坐标系（fr 重塑成 3×3 后，【行】是轴向量）：
         ┌── 法线 (fr[0]) —— 垂直于接触面
         │
    ●────┼── 切向1 (fr[1]) —— 接触面内的一个方向
         │
         └── 切向2 (fr[2]) —— 接触面内的另一个方向

    接触力在这个局部坐标系下表示：
    f[0] = 法向力（压力，总是 ≥ 0）
    f[1] = 切向力1（摩擦力）
    f[2] = 切向力2（摩擦力）

一个箱子为什么有 4 个接触点？

箱子是立方体，底面是一个矩形。MuJoCo 的碰撞检测是逐顶点的——箱子底面的 4 个角各自与地面产生一个接触点。每个接触点承担 1/4 的重量。

箱子底面（俯视图）：
    ┌───●──────────●───┐
    │   接触点1    接触点2 │
    │                    │
    │   接触点3    接触点4 │
    └───●──────────●───┘
    
    每个接触点力 = mg / 4
    4 个接触点合力 = mg ✓

💡 所以判断"物体有没有被托住"，必须把所有接触点的力加起来，只看 contact[0] 会得出"受力只有 1/4"的错误结论。

14.2.2 🔥 两个极易搞错的顺序

① ct.frame 重塑 3×3 后，【行】是轴向量，第 0 行是法线

fr = np.array(ct.frame).reshape(3, 3)
# fr[0] = 法线     fr[1] = 切向1     fr[2] = 切向2

② mj_contactForce 的分量顺序是 [法向, 切向1, 切向2]，不是 [x, y, z]

f = np.zeros(6)
mujoco.mj_contactForce(model, data, i, f)
f[0]            # 法向力（压力大小）
f[1], f[2]      # 切向力（摩擦）

所以世界系力要这么算：

fr = np.array(ct.frame).reshape(3, 3)
f_world = fr.T @ f[:3]   # 每行是轴向量：f_world = f[0]*fr[0] + f[1]*fr[1] + f[2]*fr[2]

⚠️ 错误写法：fr @ f[:3] —— 它把【列】当成了轴（实际【行】才是轴，列只是分量），
法向力会被乘到错误的方向上，连正负号都是反的。
实测对比（1 kg 的箱子静置在地面）：

force(接触系)  = [ 2.4525  0  0 ]      <- [法向, 切向1, 切向2]
fr @ f[:3]    = [ -0  -0  -2.4525]     <- 错
fr.T @ f[:3]  = [  0  -0   2.4525]     <- 对（Z-up，地面法线朝 +Z）

实测 frame 内容（箱子落地）：第 0 行 = 法线 [0,0,1]、第 1 行 = 切向1 [0,1,0]、第 2 行 = 切向2 [-1,0,0]。

14.2.3 🔥 一个箱子躺着，会产生 4 个接触点

这是新手最常问的「为什么我的接触力只有 mg/4」。

实测（1 kg 立方体静置在地面）：

ncon = 4     # 箱子 4 个角，每个角 1 个接触点
contact[0] force = [2.4525, 0, 0]   dist = -0.0001078
contact[1] force = [2.4525, 0, 0]   dist = -0.0001078
contact[2] force = [2.4525, 0, 0]   dist = -0.0001078
contact[3] force = [2.4525, 0, 0]   dist = -0.0001078
世界系合力 = [0, 0, 9.81]           理论 mg = [0, 0, 9.81]  -> 吻合

每个角 2.4525 N，四个加起来 9.81 N = mg。

💡 所以判断「物体有没有被托住」，必须把所有接触点的力加起来，
只看 contact[0] 会得出「受力只有 1/4」的错误结论。

14.2.4 接触力是瞬时量，不是「受力」

让 1 kg 的箱子从 z=0.30 落到地面（静止位置 z=0.05，落差 0.25 m）：

峰值 = 247.16 N @ t = 0.228 s
静止值 = 9.81 N
峰值/静止 = 25.19 倍

🔥 撞击力本质上是 冲量 / Δt。步长越小，同样的动量变化分摊到更短的时间，峰值就越高。
不要把单帧接触力当作物体的"受力"来判断抓取是否成功——
正确做法是看一段时间内的平均，或者直接用 touch 传感器。

14.2.5 摩擦锥

接触力必须落在摩擦锥内：|f_t| ≤ μ · f_n。

摩擦锥的几何意义：

        法向力 f_n
           ▲
           │
           │   ╱╲
           │  ╱  ╲    ← 摩擦锥（半角 = arctan(μ)）
           │ ╱    ╲
           │╱      ╲
     ──────┼────────┼──► 切向力 f_t
           │        │
          -μ·f_n   +μ·f_n

    接触力 (f_t, f_n) 必须在这个锥内：|f_t| ≤ μ·f_n
    μ = 摩擦系数，μ 越大锥越宽，越不容易滑动

静摩擦 vs 动摩擦：

  • 静摩擦：|f_t| < μ·f_n 时，物体保持静止，切向力可以取任意值（只要在锥内）
  • 临界状态：|f_t| = μ·f_n 时，物体即将滑动
  • 动摩擦：|f_t| = μ·f_n 且物体滑动时，摩擦力方向与速度相反

MuJoCo 用的是正则化摩擦锥（pyramid approximation），把圆锥近似成棱锥，计算更快但略有误差。

实测（1 kg 箱子，μ = 0.5，N = 9.81 N，理论阈值 4.905 N）：

推力 (N)位移 (m)状态
3.00.0027静止（有微小蠕动）
4.00.0081静止（有微小蠕动）
4.50.0117静止（有微小蠕动）
4.80.0156静止（有微小蠕动）
5.00.2187滑动
5.51.1980滑动
7.04.1830滑动

阈值落在 4.8–5.0 N 之间，与理论 4.905 N 吻合 ✅

💡 阈值附近有缓慢「蠕动」（creep），这是软摩擦锥的正常现象。
判断滑动要用位移量的阶跃（0.016 → 0.219 m），而不是「是否严格为 0」。

14.2.6 touch 传感器：抓取判定首选

不想遍历接触点？用 <touch>，它直接告诉你某个 site 处压了多大力：

<site name="finger_pad" pos="-0.01 0 0.00" size="0.01"/>
<sensor><touch name="s_touch" site="finger_pad"/></sensor>

实测（球压在托盘顶面的 site 上）：

球质量顶面 site侧面 site理论 mg
0.1 kg0.9810 N0.0000 N0.9810 N
0.5 kg4.9050 N0.0000 N4.9050 N
2.0 kg19.6200 N0.0000 N19.6200 N

精确等于法向接触力；没被压到的 site 读 0。

✅ 这就是抓取判定的标准做法：夹爪指尖各放一个 touch site，
touch_left > 阈值 and touch_right > 阈值 → 抓稳了。
比数接触点、比看画面都可靠。

14.3 执行器：位置、力矩、速度到底选哪个

执行器类型对比图

┌─────────────────────────────────────────────────────────────────────┐
│                        执行器类型选择决策树                             │
├─────────────────────────────────────────────────────────────────────┤
│                                                                     │
│  你想控制什么？                                                      │
│  │                                                                  │
│  ├─ 位置（关节角）──→ <position kp kv>                             │
│  │   自带 PD，ctrl = 目标角度                                       │
│  │   ✅ 本书用这个，最省心                                           │
│  │                                                                  │
│  ├─ 速度（角速度）──→ <velocity kv>                                │
│  │   自带阻尼，ctrl = 目标速度                                       │
│  │   ⚠️ 是刹车不是定位！控不住位置                                    │
│  │                                                                  │
│  ├─ 力矩（Nm）─────→ <motor gear>                                 │
│  │   纯力控，ctrl = 力矩                                             │
│  │   ❌ 被重力拖走，需要自己做闭环                                    │
│  │                                                                  │
│  └─ 自定义力律──────→ <general gaintype biastype gainprm biasprm> │
│      最灵活，但最容易踩坑                                             │
│      ⚠️ 必须写 biastype="affine"，否则 PD 反馈被关掉！               │
│                                                                     │
└─────────────────────────────────────────────────────────────────────┘

四种执行器的力律对比

执行器力的公式ctrl 含义能定位吗适合场景
<position>kp·(ctrl-q) - kv·q̇目标角度✅ 能位置伺服（本书）
<velocity>-kv·(q̇-ctrl)目标速度❌ 不能速度控制、阻尼
<motor>gear·ctrl力矩❌ 不能纯力控、力矩限制
<general>自定义取决于配置取决于配置高级自定义

用生活类比理解：

  • <position> = 自动驾驶的"定速巡航+车道保持"——你告诉它去哪，它自己打方向盘、踩油门
  • <velocity> = 定速巡航——你告诉它开多快，它保持速度，但不会自动拐弯
  • <motor> = 手动挡——你直接踩油门，开多快全靠自己控制
  • <general> = 改装车——你可以自定义任何控制逻辑，但也最容易出问题

14.3.1 五种写法横向对比

用同一个单连杆模型（2 kg，长 0.5 m，重力 −Z，即本书统一的 Z-up），命令它到 0.8 rad：

写法稳态 q能定位吗
<position kp kv>0.8179✅
<general gaintype/biastype="affine">0.8179✅
<motor gear>（ctrl=0）0.1637❌ 被重力拖走
<velocity kv>（ctrl=0）0.3503❌ 慢慢溜走
<general> 不写 biastype29425.86💥 发散
<!-- ✅ 推荐：用现成标签 -->
<position name="a" joint="j1" kp="200" kv="20"/>

<!-- ✅ 手写等价形式：必须显式写 affine -->
<general name="a" joint="j1" gaintype="affine" biastype="affine"
         gainprm="200 0 0" biasprm="0 -200 -20"/>

<!-- 💥 坑：默认 biastype=NONE，PD 反馈被关掉 -->
<general name="a" joint="j1" gainprm="200 0 0" biasprm="0 -200 -20"/>

⚠️ 这是第 11 章那个「仿真跑飞到 q=2262」的根因，本章再次实测确认：
手写 <general> 不发愁，发愁的是你不写 biastype="affine"。

14.3.2 <velocity> 是刹车，不是定位

很多人以为 <velocity kv="20"> 能控位置。实测：命令 0，连杆从 0.05 慢慢溜到 0.3503。

因为速度执行器输出的是 τ = -kv·(qvel − ctrl)，本质是个阻尼器。
它能让你停下来，但不能让你停在指定位置。

14.3.3 重力下垂的定量公式

位置控制的稳态误差不是随机的，它是可预测的：

稳态误差 = |qfrc_bias| / kp

实测（kp = 200）：

纯 PD        : q = 0.817896   误差 = 0.017896 rad
PD + 重力补偿: q = 0.800000   误差 = 0.000000 rad
纯 PD 稳态处的重力力矩 qfrc_bias = -3.5792 Nm
理论稳态误差 = |-3.5792| / 200 = 0.017896 rad  -> 与实测完全吻合

补偿方法（第 12 章讲过，这里补上定量解释）：

data.qfrc_applied[:6] = data.qfrc_bias[:6]   # 前馈掉重力

💡 想减小下垂，两条路：加大 kp，或加重力补偿。
加 kp 会让系统变刚、容易抖；加前馈是更优雅的做法。

14.3.4 ⚠️ 本章最隐蔽的坑：degree vs radian

MJCF 里所有角度默认单位是「度」，不是弧度。

<mujoco>
  <compiler angle="radian"/>    <!-- 本书项目全部加了这一行 -->
  ...
</mujoco>

我在这章的探测脚本里用同一段 XML 把两种单位各跑了一遍（Z-up 下把地面写成
euler="-1.570796 0 0"——这是从 Y-up 教材照搬来的坏习惯）：

坑 1：地面平面立起来了

<geom name="floor" type="plane" size="2 2 0.1" euler="-1.570796 0 0"/>
compiler angle地面法线物体最终 zncon
radian（本书项目默认）[0, 1, 0] ❌ 立成墙−4.84990
degree[0, 0.0274, 0.9996] ⚠️ 蒙对+0.04824
  • radian 模式：-1.570796 = −90°，地面被转成一面竖直的墙（法线 [0,1,0]），物体直接掉到 −4.85 m，ncon=0。
  • degree 模式：同一个数被当成 −1.57 度，地面几乎还是水平的，物体稳稳停住——但这是「蒙对」，所有角度其实都错着单位。

🔥 正确写法：Z-up 下地面根本不需要 euler。<plane> 的默认法线就是 +Z，
照搬 Y-up 的 euler="-90 0 0" 反而会把地面立成墙。
同一个数，单位不同，物理完全两样。

坑 2：关节限位只剩 ±3°

<joint name="j1" type="hinge" axis="-1 0 0" range="-3 3"/>
compiler angle实际 range (rad)命令 0.5 rad → 实际 qqfrc_constraint
degree[-0.05236, 0.05236]0.06245−87.82
radian[-3, 3]0.51202+0.00

degree 模式下关节被卡在 ±3°，执行器使出 87.8 Nm 也转不动。

🔥 排错技巧：关节「推不动」时，打印 data.qfrc_constraint。
如果这一项很大，说明有约束在跟执行器对抗——99% 是限位设错了或者被 weld 卡住。

本书项目模型第一行就是 <compiler angle="radian"/>，请务必保持。

14.3.5 读取执行器输出

data.actuator_force   # shape = (nu,)，每个执行器实际输出的广义力

实测（本项目模型，pd 稳态）：

ctrl = [ 0.3  0.1 -0.2  0.  0.2  0. ]
qpos = [ 0.2999  0.0945 -0.2063 -0.0001  0.1997 -0. ]
actuator_force = [ 0.1  4.4198  5.0138  0.0795  0.2413  0.0032  0.0386 -0.3887]
手算 j1: kp*(ctrl-q) = 800*(0.3000-0.2999) = 0.1000  -> 与 actuator_force[0] 一致

💡 actuator_force 就是「电机实际出了多大力」。用它做力矩限幅报警、
或者判断「是不是在死顶着障碍物」。

执行器力调试实战

当机械臂行为异常时，actuator_force 是最重要的调试信号之一：

场景 1：关节不动但电机在使劲

print(data.actuator_force)
# 如果某个关节的 actuator_force 很大（如 >50Nm）但 qpos 不变，
# 说明被约束卡住了——检查限位、weld、或接触
print(data.qfrc_constraint)   # 约束力也会很大

场景 2：电机完全不使劲

print(data.actuator_force)
# 如果全是 0，说明执行器没工作
# 检查：ctrl 有没有设？执行器有没有绑对关节？biastype 是不是 none？
print(data.ctrl)              # 控制指令
print(model.actuator_biastype)  # 偏置类型

场景 3：力矩振荡/抖动

# 记录一段时间的 actuator_force
forces = []
for _ in range(100):
    mujoco.mj_step(model, data)
    forces.append(data.actuator_force.copy())
forces = np.array(forces)
print(f"力矩波动: {forces.std(axis=0)}")
# 如果波动很大，可能是 kp 太大、kv 太小、或接触不稳定
# 解决：降低 kp、增大 kv、增大 damping、调软 solref

14.4 约束与焊接：让物体粘在夹爪上

14.4.1 equality 的五种类型

类型作用
connect两点用球铰连起来（保留 3 个转动自由度）
weld两个刚体完全固连（6 个自由度全锁死） ← 抓取用这个
joint约束两个关节的值成比例
tendon约束腱的长度
flex柔性体相关

14.4.2 weld 的语义（实测判定）

<weld name="grasp_weld" body1="target_object" body2="gripper_mount" active="false"/>

注意：项目模型的 XML 里没有写 relpose——编译器会按初始位姿自动推断一份（见 14.4.4）。

实测得到的精确语义（用带已知旋转的最小模型逐条排除后确认）：

T_body2 = T_body1 @ T_relpose

展开成代码能用的形式：

p_body2 = p_body1 + R_body1 @ rel_p
R_body2 = R_body1 @ R_rel

也就是说：relpose 是 body2 在 body1 坐标系下的位姿。

14.4.3 🔥 eq_data 的 11 个数怎么排

要在运行时改 relpose，就得写 model.eq_data。它的布局不直观：

eq_data[i] 共 11 个数：
  [0:3]    body2 侧锚点槽位（weld 默认全 0，不通过 XML 暴露；约束雅可比会用到）
  [3:6]    relpose 平移 (3)
  [6:10]   relpose 四元数 (4)，顺序 wxyz
  [10]     torquescale

实测证据（改 relpose 看 eq_data 怎么变）：

relpose="1 2 3 1 0 0 0"                -> eq_data = [0,0,0, 1,2,3, 1,0,0,0, 1]
relpose="0 0 0 0.7071068 0 0 0.7071068" -> eq_data = [0,0,0, 0,0,0, 0.707,0,0,0.707, 1]
                                                                ^^^^^^^^^^^^^ 四元数 wxyz
                                                          ^^^^^ 平移

⚠️ 四元数顺序三连坑（呼应第 2 章）：

场合顺序
MJCF relpose 属性wxyz
model.eq_data[6:10]wxyz
scipy Rotation.as_quat()xyzw ← 不一样！
MuJoCo data.qpos 里的四元数wxyz

从 scipy 拿到的四元数写进 eq_data 前，一定要换位：

q = Rot.from_matrix(R).as_quat()          # xyzw
model.eq_data[i][6:10] = [q[3], q[0], q[1], q[2]]   # -> wxyz

14.4.4 ⚠️ 直接激活 weld：relpose 是编译器按初始位姿推断的

项目模型的 XML 里根本没有写 relpose（见 14.4.2 的 XML），不写也不报错——
编译器按两个 body 的初始位姿自动推断一份，写进 eq_data：

eq_data[3:7] = [-0.2325, 0.3, 0.59253, 1]   (wxyz)   <- 一份「出厂快照」

实验 A：不做任何处理，直接激活：

激活前 物体 = [-0.18    -0.3     0.02   ]   夹爪座 = [-0.4125   0.       0.61253]
激活后 物体 = [-0.17977 -0.30041  0.02036]
⚠️ 瞬移 = 0.000593 m

只有 0.6 mm？因为这次激活恰好发生在初始位姿——快照描述的正是「现在」的相对位姿，约束天然满足。
但这纯属巧合：只要激活前物体被挪动过（真实抓取必然如此），这份陈旧的 relpose
就会把物体硬拽回「初始相对位姿」，瞬移量等于物体偏离初始位姿的距离，没有上限。
所以 relpose 必须在抓取瞬间现场算（下一节）。

14.4.5 ✅ 正确的抓取菜谱

在抓取的那一瞬间计算真实相对位姿，写回 eq_data，再激活：

from scipy.spatial.transform import Rotation as Rot

def grasp(m, d, eq_i=0):
    """抓取瞬间调用：把物体相对夹爪座的位姿固化下来。"""
    i1, i2 = m.eq_obj1id[eq_i], m.eq_obj2id[eq_i]      # i1=物体, i2=夹爪座
    R1 = d.xmat[i1].reshape(3, 3).copy()
    R2 = d.xmat[i2].reshape(3, 3).copy()
    p1 = d.xpos[i1].copy()      # ⚠️ 必须 .copy()！
    p2 = d.xpos[i2].copy()      #    d.xpos[i] 是视图，会随仿真变化

    rel_p = R1.T @ (p2 - p1)                    # 物体系下的相对平移
    rel_R = R1.T @ R2                           # 物体系下的相对旋转
    q = Rot.from_matrix(rel_R).as_quat()        # xyzw

    m.eq_data[eq_i][3:6]  = rel_p
    m.eq_data[eq_i][6:10] = [q[3], q[0], q[1], q[2]]    # -> wxyz
    d.eq_active[eq_i] = 1

实测（实验 B：故意把物体绕 Y 轴转 40°，让四元数不是单位值）：

写入 rel_p = [-0.55898  0.3      0.30446]
写入 rel_q = [ 0.93969  0.      -0.34202  0. ]  (wxyz)
激活后物体 = [-0.17819 -0.30255  0.02881]
✅ 瞬移 = 0.009348 m   （对比实验 A 的 0.0006 m——那个 0.0006 只是初始位姿下的巧合）

物体只被调整了约 9 mm 就稳稳「焊」进夹爪。

⚠️ d.xpos[i] 是视图不是副本——这个坑本章我自己踩了一次：
存了 p = d.xpos[i]，跑完仿真再比较，发现"位移是 0"，
其实是比较的同一个对象。第 3 章讲过的视图陷阱，在这里换个马甲又出现了。

14.4.6 用「约束违反量」做诊断

weld 是软约束，被卡住时不会报错，只会悄悄失效。所以要主动监控：

def weld_violation(m, d, eq_i=0):
    i1, i2 = m.eq_obj1id[eq_i], m.eq_obj2id[eq_i]
    R1 = d.xmat[i1].reshape(3, 3)
    p1, p2 = d.xpos[i1], d.xpos[i2]
    return np.linalg.norm(p2 - (p1 + R1 @ m.eq_data[eq_i][3:6]))
  • 悬空搬运：违反量 = 0.00021 m ✅ 刚体跟随正常
  • 物体被顶死在地面上（夹爪还在往下压）：违反量 = 0.00385 m ❌

💡 这个指标比肉眼看画面可靠得多。写抓取任务时，
每步打印一次 weld_violation：悬空搬运在 0.0002 m 量级，
一旦冲到毫米级以上（实测被顶死 0.00385 m），就说明「夹爪在往物体里怼」。

14.4.7 solref 调优：让约束更硬

同一个「被顶死」场景，只改 eq_solref：

solref最大违反 (m)
0.02 1（默认）0.00385
0.01 10.00199
0.005 10.00094
0.002 10.00072
<weld name="grasp_weld" body1="..." body2="..." solref="0.005 1"/>

solref 第一个数是时间常数，越小越硬：违反量从 0.00385 m 一路降到 0.00072 m
（[0.002, 1] 时已在 1 mm 以下）。抓取场景建议 0.002 ~ 0.01。

solref / solimp 参数详解

接触和约束的"软硬"由两个参数控制：

参数作用典型值
solref[0]时间常数（秒），越小约束越硬、响应越快0.002~0.02
solref[1]阻尼比，1=临界阻尼，小于1=欠阻尼（振荡）1.0
solimp[0]约束最小误差下限0.9
solimp[1]约束最大误差上限0.95
solimp[2]约束宽度 margin0.001

solref 的物理意义：约束被违反后，恢复到满足状态的时间常数。可以理解为一个弹簧-阻尼系统，solref[0] 越小恢复越快，solref[1] 控制阻尼。

调优建议：

  • 抓取/weld 约束：solref="0.005 1"（硬但稳定）
  • 普通接触：默认 solref="0.02 1" 即可
  • 高速碰撞：solref="0.002 1"（更硬，减少穿透）
  • 出现高频振荡：增大 solref[0]（变软）或增大 solref[1]（增加阻尼）

接触问题排查指南

现象 1：物体穿透地面

  • 原因：timestep 太大 / 碰撞体太薄 / solref 太软
  • 解决：减小 timestep（0.002到0.001）、加厚碰撞体、调小 solref[0]

现象 2：接触抖动/弹跳

  • 原因：约束太硬 / 阻尼不足 / 摩擦参数不对
  • 解决：增大 solref[0]、增大 solref[1]、检查 friction 参数、给关节加 damping

现象 3：物体粘在地面上（该滑不滑）

  • 原因：摩擦系数太大 / condim 设置不对
  • 解决：减小 friction[0]、检查 condim（3=无摩擦,4=有摩擦）、检查 weld 是否意外激活

现象 4：接触力为 0 但物体明显在接触

  • 原因：contype/conaffinity 碰撞过滤把接触关掉了
  • 解决：检查两个 geom 的 contype 和 conaffinity，确保双向都通过；视觉 mesh 通常 contype=0，要用碰撞体

现象 5：抓取时物体滑落

  • 原因：摩擦不够 / weld 没激活 / 夹爪力不够
  • 解决：增大夹爪指尖 friction、检查 eq_active、增大夹爪 kp、用 weld 约束代替纯摩擦抓取

14.5 性能调优：什么才真的有用

14.5.1 ⚠️ 实测：调大 iterations 几乎没有收益

opt.iterationsus/step
132.60
531.90
2030.78
5030.78
100（默认）30.85
20032.47

从 1 到 200，耗时几乎不变（都在 31 µs 上下，差异是测量噪声）。

🔥 原因：MuJoCo 的求解器残差够小就提前退出。
对本项目这种规模（nv=14, ngeom=19）的场景，默认 100 早就收敛了，
多设的迭代根本没跑。

别盲目调 iterations。 真正影响接触精度的是 solref / solimp / timestep。
（我另外测过 5 箱堆叠、200 kg 重物的硬接触场景，iterations 从 1 到 500，
穿透深度也只从 0.0352 mm 变到 0.0350 mm——同样几乎不变。）

14.5.2 ✅ 实测：timestep 才是主开关

timestepus/step实时因子静止高度误差
0.000531.4515.9x0.0081 mm
0.00131.3431.9x0.0081 mm
0.002（本项目）30.7265.1x0.0081 mm
0.00531.70157.7x0.0325 mm
0.0131.97312.8x0.1298 mm

📌 us/step 在不同次运行间有 ±2 µs 的波动（CPU 调度、温度），
但量级关系和实时因子的线性趋势是稳定的。

关键观察：每一步的耗时基本恒定（≈31 µs），与步长无关。
所以：

实时因子 ≈ timestep / 31µs

大步长 = 白送的实时性能，代价是精度。本项目取 0.002 s（500 Hz），
误差 0.0081 mm，实时因子约 65x，是很舒服的工作点。

💡 训练 RL 想要更快？先把 timestep 从 0.002 放到 0.005，
实时因子直接 2.5 倍，精度损失只有 0.024 mm。这比买 GPU 划算。

14.5.3 其他实测结论

项目结果
接触对耗时的影响几乎为零（机械臂运动 32.66 vs 物体静置接触 30.97 us，在噪声内）
瓶颈正向动力学 mj_step1
mj_step vs mj_step1+mj_step2基本等价（33.31 vs 33.77 us，差 +0.46 µs，在噪声量级内）
本项目整体~31 us/step，约 60–65x 实时

拆分写法的价值不在性能，而在于能在两步之间插自己的逻辑：

mujoco.mj_step1(model, data)          # 正向动力学 + 约束建立
data.qfrc_applied[:6] = my_controller(data)   # 你的自定义力
mujoco.mj_step2(model, data)          # 积分

14.6 常见错误速查

现象原因解决
sensordata 读到的值不对切片下标错了用 sensor_adr / sensor_dim 建映射表
猜传感器 type 编号出错MuJoCo 3.x 枚举位置变了用 mujoco.mjtSensor 反查
noise 加了没反应实测：noise 不会自动应用自己在 Python 里加（cutoff 会生效，注意读数被裁剪）
接触力只有 mg/4一个箱子有 4 个接触点把所有接触点力求和
世界系接触力方向不对fr @ f 把【列】当成了轴fr.T @ f[:3]（【行】是轴，第 0 行是法线）
单帧接触力大到离谱撞击是冲量/Δt取时间平均，或用 touch
仿真跑到 q=29425<general> 没写 biastype加 biastype="affine"，或直接用 <position>
<velocity> 控不住位置它是阻尼器不是定位器用 <position>，或外面套 P 控制器
关节「推不动」限位被当成角度制加 <compiler angle="radian"/>
地面平面立成墙radian 模式下照搬 Y-up 的 euler=-90°Z-up 地面别写 euler
激活 weld 后物体被硬拽relpose 是编译器按初始位姿推断的旧快照抓取瞬间算 relpose 写回 eq_data
weld 看起来没生效物体被地面/其他约束卡住打印 weld_violation 诊断
“位移是 0” 的假象d.xpos[i] 是视图用 .copy()
盲目加大 iterations 没变快求解器提前退出改 timestep

14.7 动手练

  1. 传感器切片：打印本项目模型的 sensor_adr / sensor_dim 表，写一个 read(name) 函数，
    验证 read("ee_pos") 等于 data.site_xpos[ee_id]。

  2. 接触力分量：让 target_object 静置在地面，打印所有接触点的
    dist 与 force，验证合力等于 m*g。再按 14.2.2 的公式换算到世界系。

  3. 执行器对比：用同一个单连杆模型，分别用 <position> / <motor> / <velocity> /
    <general>（带与不带 biastype），记录稳态角度，观察 general(NONE) 的发散。

  4. weld 抓取：先不做任何处理直接激活（观察瞬移量），再用 14.4.5 的菜谱重试，
    对比瞬移量。然后搬运一段距离并监控 weld_violation。

  5. 性能权衡：扫 opt.iterations（1→200）与 opt.timestep（0.0005→0.01），
    记录 us/step、实时因子与落体静止误差，验证「iterations 无效、timestep 有效」。

参考答案见 code/ch14_mujoco_advanced.py。

练习 1 详解：传感器切片

题目：打印本项目模型的 sensor_adr / sensor_dim 表，写一个 read(name) 函数，验证 read("ee_pos") 等于 data.site_xpos[ee_id]。

步骤：

  1. 遍历所有传感器，打印名字、类型、起始下标、维度
  2. 建一个名字到 (adr, dim) 的映射表
  3. 写 read(name) 函数
  4. 设测试关节角，调用 mj_forward，验证一致性

关键代码：

ADR = {}
for i in range(model.nsensor):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i)
    a, n = int(model.sensor_adr[i]), int(model.sensor_dim[i])
    ADR[name] = (a, n)
    print(f"  {i}: {name:<15} adr={a:<3} dim={n}")

def read(name):
    a, n = ADR[name]
    return data.sensordata[a:a+n]

# 验证
data.qpos[:6] = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
mujoco.mj_forward(model, data)
ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
print(f"read('ee_pos') = {read('ee_pos')}")
print(f"site_xpos      = {data.site_xpos[ee_id]}")
print(f"一致: {np.allclose(read('ee_pos'), data.site_xpos[ee_id])}")   # True

注意：sensordata 是派生量，改了 qpos 后必须调用 mj_forward 才会更新。

练习 2 详解：接触力分量

题目：让 target_object 静置在地面，打印所有接触点的 dist 与 force，验证合力等于 m*g。再按 14.2.2 的公式换算到世界系。

步骤：

  1. 让物体静置在地面（跑几百步让它稳定）
  2. 遍历 data.contact，打印每个接触点的 geom、dist、force
  3. 把所有接触点的力加起来，验证等于 mg
  4. 把接触系力换算到世界系

关键代码：

total_force_normal = 0.0
for i in range(data.ncon):
    ct = data.contact[i]
    g1 = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_GEOM, ct.geom1)
    g2 = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_GEOM, ct.geom2)
    f = np.zeros(6)
    mujoco.mj_contactForce(model, data, i, f)
    print(f"  接触{i}: {g1} ↔ {g2}  dist={ct.dist:.6f}  法向力={f[0]:.4f}N")
    total_force_normal += f[0]

print(f"  总法向力 = {total_force_normal:.4f} N")
print(f"  理论 mg  = {0.1 * 9.81:.4f} N")   # 物体质量 0.1kg

世界系换算：

fr = np.array(ct.frame).reshape(3, 3)
f_world = fr.T @ f[:3]
# fr 重塑 3×3 后【行】是轴向量：第 0 行是法线，第 1/2 行是切向

预期结果：4 个接触点，每个法向力约 0.245N，总和 0.981N = 0.1×9.81 ✓

练习 3 详解：执行器对比

题目：用同一个单连杆模型，分别用 <position> / <motor> / <velocity> / <general>（带与不带 biastype），记录稳态角度，观察 general(NONE) 的发散。

实验设计：

  • 单连杆：2kg，长 0.5m，重力 -Z（Z-up）
  • 命令关节到 0.8 rad
  • 跑 1500 步（3秒），记录最终角度

实测结果（来自 14.3.1 节）：

<position kp=200 kv=20>:     稳态 q=0.8179  ✅ 能定位
<general + biastype=affine>: 稳态 q=0.8179  ✅ 与 position 等价
<motor gear=1> (ctrl=0):     稳态 q=0.1637  ❌ 被重力拖走
<velocity kv=20> (ctrl=0):   稳态 q=0.3503  ❌ 慢慢溜走
<general> 不写 biastype:      q=29425.86    💥 发散！

关键结论：

  1. <position> 最省心，自带 PD
  2. <motor> 是纯力控，需要自己做闭环
  3. <velocity> 是阻尼器，控不住位置
  4. <general> 不写 biastype="affine" 会直接发散——这是最危险的坑

练习 4 详解：weld 抓取

题目：先不做任何处理直接激活（观察瞬移量），再用 14.4.5 的菜谱重试，对比瞬移量。然后搬运一段距离并监控 weld_violation。

步骤 1：直接激活（不写 eq_data）

data.eq_active[weld_id] = 1    # 直接激活，不写 eq_data
# relpose 是编译器按初始位姿推断的快照：物体还在初始位姿附近时瞬移很小（实测 0.000593 m），
# 但物体一旦被挪远就会被硬拽回初始相对位姿——不能依赖

步骤 2：正确菜谱

# 抓取瞬间计算相对位姿
i1, i2 = model.eq_obj1id[0], model.eq_obj2id[0]
R1 = data.xmat[i1].reshape(3,3).copy()
R2 = data.xmat[i2].reshape(3,3).copy()
p1 = data.xpos[i1].copy()    # ⚠️ 必须 .copy()
p2 = data.xpos[i2].copy()

rel_p = R1.T @ (p2 - p1)
rel_R = R1.T @ R2
q = Rot.from_matrix(rel_R).as_quat()    # xyzw

model.eq_data[0][3:6] = rel_p
model.eq_data[0][6:10] = [q[3], q[0], q[1], q[2]]    # → wxyz
data.eq_active[0] = 1
# 瞬移量约 0.009 m（物体被转过 40° 后再抓），物体被稳稳焊进夹爪

步骤 3：监控 weld_violation

def weld_violation():
    i1, i2 = model.eq_obj1id[0], model.eq_obj2id[0]
    R1 = data.xmat[i1].reshape(3,3)
    p1, p2 = data.xpos[i1], data.xpos[i2]
    return np.linalg.norm(p2 - (p1 + R1 @ model.eq_data[0][3:6]))

# 搬运过程中每步打印
for _ in range(100):
    mujoco.mj_step(model, data)
    print(f"weld_violation = {weld_violation():.6f} m")

预期结果：

  • 直接激活（物体在初始位姿）：瞬移 0.000593 m——但物体被挪远后会大瞬移，不能依赖
  • 正确菜谱（物体先转 40° 再抓）：瞬移 0.009348 m
  • 正常搬运：violation ≈ 0.0002m
  • 被顶死在地面：violation ≈ 0.00385m

练习 5 详解：性能权衡

题目：扫 opt.iterations（1→200）与 opt.timestep（0.0005→0.01），记录 us/step、实时因子与落体静止误差，验证「iterations 无效、timestep 有效」。

iterations 扫描：

for it in [1, 5, 20, 50, 100, 200]:
    model.opt.iterations = it
    t0 = time.perf_counter()
    for _ in range(10000):
        mujoco.mj_step(model, data)
    us_per_step = (time.perf_counter() - t0) / 10000 * 1e6
    print(f"iterations={it:3d}: {us_per_step:.2f} us/step")

预期结果：所有 iterations 的耗时都在 31µs 左右——求解器提前退出，多设的迭代根本没跑。

timestep 扫描：

for dt in [0.0005, 0.001, 0.002, 0.005, 0.01]:
    model.opt.timestep = dt
    t0 = time.perf_counter()
    for _ in range(10000):
        mujoco.mj_step(model, data)
    wall = time.perf_counter() - t0
    sim_time = 10000 * dt
    realtime_factor = sim_time / wall
    print(f"dt={dt:.4f}: {us_per_step:.2f} us/step, 实时因子={realtime_factor:.1f}x")

预期结果：

  • 每步耗时恒定 ≈31µs（与 dt 无关）
  • 实时因子 = dt / 31µs，线性增长
  • dt=0.002: 65x, dt=0.01: 313x

结论：

  • 🔥 iterations 从 1 到 200 几乎没有变化——别调它
  • ✅ timestep 才是主开关——步长越大越快，代价是精度
  • 本项目工作点：0.002s / 65x 实时 / 0.008mm 误差

14.8 小结

传感器

  • sensordata 是扁平数组，靠 sensor_adr / sensor_dim 切片，建议建名字映射表。
  • framelinvel 是世界系，velocimeter 是局部系；framequat 是 wxyz。
  • 加速度计读比力：自由落体 0，静止 9.81。
  • 🔥 实测：noise 不生效（要自己加）；cutoff 会生效（超限读数被裁剪到 ±cutoff，实测 23994 被裁到 5.0）。

接触

  • 🔥 frame 重塑 3×3 后行是轴（第 0 行是法线）；mj_contactForce 顺序是 [法向, 切向1, 切向2]；世界系力 = fr.T @ f[:3]。
  • 一个箱子静置产生 4 个接触点，单点力是 mg/4。
  • 接触力是瞬时量（撞击峰值可达静止值 25 倍），别当"受力"用。
  • 摩擦锥阈值实测与 μN 吻合；判定滑动要看位移阶跃。
  • touch 传感器 = 该点的法向力，是抓取判定的首选。

执行器

  • <position> ≡ <general gaintype/biastype="affine">；手写不写 biastype 会直接发散。
  • <velocity> 是刹车不是定位。
  • 稳态误差 = |qfrc_bias| / kp；加重力前馈可归零。
  • 🔥 <compiler angle="radian"/> 必须有——range="-3 3" 在 degree 模式下只有 ±3°。
  • 关节推不动时看 data.qfrc_constraint。

约束与焊接

  • weld 语义：T_body2 = T_body1 @ T_relpose。
  • eq_data 布局：[3:6] 平移、[6:10] 四元数 wxyz、[10] torquescale。
  • ⚠️ XML 未写 relpose 时，编译器按初始位姿推断一份快照（eq_data[3:7]=[-0.2325, 0.3, 0.59253, 1]）；直接激活在物体被挪远后会大瞬移。
  • ✅ 抓取瞬间算 relpose 写回 eq_data → 瞬移 ~0.009 m。
  • 用 weld_violation 做诊断；调 solref 让约束更硬。
  • ⚠️ d.xpos[i] 是视图，要 .copy()。

性能

  • 🔥 iterations 从 1 到 200 几乎没有变化（求解器提前退出）——别调它。
  • ✅ timestep 才是主开关：每步耗时恒定 ≈31 µs，步长越大实时因子越高。
  • 本项目工作点：0.002 s / 65x 实时 / 0.008 mm 误差。

本章知识点在本项目中的应用

知识点用在项目哪里
sensordata 名字映射表第 20 章 dm_control 观测空间、第 21 章状态读取
framelinvel vs velocimeterRL 策略的速度观测选择（世界系 vs 局部系）
加速度计比力模拟真实 IMU 数据、域随机化
接触力遍历 + 世界系换算第 14 章接触分析、力控任务
touch 传感器抓取判定（touch_left > 阈值 and touch_right > 阈值）
<position> PD 执行器所有 8 个执行器，第 12 章控制
actuator_force 读取力矩限幅报警、死顶障碍物检测
weld 抓取菜谱scripts/pick_and_place.py、第 21 章抓取任务
weld_violation 诊断抓取质量监控、异常检测
solref 调优高精度抓取场景（0.005~0.01）
timestep 性能权衡RL 训练加速（0.005s → 2.5x 速度）
d.xpos[i].copy()所有需要保存位置快照的场景

扩展阅读方向

  1. 接触动力学深入：了解 MuJoCo 接触求解器的凸优化原理（primal-dual 方法），以及 solref/solimp 参数如何影响接触刚度和阻尼。
  2. 力控与阻抗控制：在 <motor> 执行器基础上实现阻抗控制（impedance control）——通过调节虚拟刚度和阻尼来控制交互力，这是协作机器人的核心技术。
  3. 域随机化（Domain Randomization）：在仿真中随机化质量、摩擦、传感器噪声等参数，训练出的策略能更好地迁移到真实机器人（sim-to-real）。
  4. 柔性体与软体机器人：MuJoCo 的 <flex> 元素支持有限元柔性体仿真，适合模拟软体抓手、可变形物体。
  5. GPU 加速仿真：了解 MuJoCo 的 GPU 后端和 brax（JAX 编写的可微物理引擎），可以在 GPU 上同时跑几千个仿真，RL 训练速度再提升几个数量级。

下一部分：dm_control —— 把「模型 + 任务 + 观测 + 奖励」打包成标准强化学习环境。

上一章：13 · 渲染、相机与录屏 ｜ 下一章：15 · dm_control 入门

# python
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

AgentScope 2.0 学习笔记：双剑合璧——工具 + RAG，做一个会查会答的客服（从会说到会查会答）

你有没有见过这种「半吊子」客服机器人：你问它「积雪草精华多少钱」，它答得飞快、价格库存全对；你接着问「我泛红用什么好」，它就卡壳了——因为它只会查价格，不会给建议。反过来也有一种：护肤知识背得滚瓜烂熟，你一问「有货吗」，它只能含含糊糊说「应该有的吧」。

avatar DAMO开发者矩阵

全自动 arXiv 论文监控 + Gemini 多模态解读 + 邮箱推送机器人

本文介绍了一款基于Python的自动化科研论文监控与AI解读工具，针对图像超分辨率等领域的文献追踪难题，实现“追新+补旧”推送机制。系统定时抓取arXiv最新论文，利用PyMuPDF提取首页图像，结合Google Gemini 2.0多模态模型进行图文联合分析，生成带星级评分与核心创新点的解读报告，并通过SMTP邮件精准推送。具备自动降级、轻量缓存与零数据库依赖等特性，支持GitHub Actio

avatar DAMO开发者矩阵

基于蚁群优化算法的直流电机模糊PID控制（Matlab实现）

直流电机因其调速性能优异、响应速度快等特点，在工业控制、电动汽车、机器人等领域有着广泛应用。然而，传统PID控制存在参数固定、难以应对非线性及环境扰动等问题。模糊PID控制虽通过引入模糊逻辑实现了参数自适应调整，但在复杂场景下仍面临规则库构建与参数优化的难题。本文将蚁群优化算法（Ant Colony Optimization, ACO）引入直流电机模糊PID控制系统中，利用其全局寻优能力对模糊PI

avatar DAMO开发者矩阵