这一章要解决什么问题：前四章你已经能让机器人动起来、看得见。 但要做真正的任务（抓取、放置、力控、强化学习），还缺四块拼图：
最后再把这些能力跑得够快。
配套代码：[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）。 你的机器数值会略有差异，但结论和量级关系应当一致。
🎯 本章学习目标
学完本章，你将能够：
📌 前置知识：本章需要第 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）：
| 0 | pos_j1 | mjSENS_JOINTPOS | 0 | 1 |
| 1–5 | pos_j2…pos_j6 | mjSENS_JOINTPOS | 1…5 | 1 |
| 6 | ee_pos | mjSENS_FRAMEPOS | 6 | 3 |
| 7 | object_pos | mjSENS_FRAMEPOS | 9 | 3 |
校验结果（把关节设成 [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 类实测）
| <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> | 力 | 3 | site 处传递的力 |
| <torque> | 力矩 | 3 | site 处传递的力矩 |
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）：
| 3.0 | 0.0027 | 静止（有微小蠕动） |
| 4.0 | 0.0081 | 静止（有微小蠕动） |
| 4.5 | 0.0117 | 静止（有微小蠕动） |
| 4.8 | 0.0156 | 静止（有微小蠕动） |
| 5.0 | 0.2187 | 滑动 |
| 5.5 | 1.1980 | 滑动 |
| 7.0 | 4.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 上）：
| 0.1 kg | 0.9810 N | 0.0000 N | 0.9810 N |
| 0.5 kg | 4.9050 N | 0.0000 N | 4.9050 N |
| 2.0 kg | 19.6200 N | 0.0000 N | 19.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 反馈被关掉！ │
│ │
└─────────────────────────────────────────────────────────────────────┘
四种执行器的力律对比
| <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：
| <position kp kv> | 0.8179 | ✅ |
| <general gaintype/biastype="affine"> | 0.8179 | ✅ |
| <motor gear>（ctrl=0） | 0.1637 | ❌ 被重力拖走 |
| <velocity kv>（ctrl=0） | 0.3503 | ❌ 慢慢溜走 |
| <general> 不写 biastype | 29425.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"/>
| radian（本书项目默认） | [0, 1, 0] ❌ 立成墙 | −4.8499 | 0 |
| degree | [0, 0.0274, 0.9996] ⚠️ 蒙对 | +0.0482 | 4 |
- 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"/>
| 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：
| 0.02 1（默认） | 0.00385 |
| 0.01 1 | 0.00199 |
| 0.005 1 | 0.00094 |
| 0.002 1 | 0.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] | 约束宽度 margin | 0.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 几乎没有收益
| 1 | 32.60 |
| 5 | 31.90 |
| 20 | 30.78 |
| 50 | 30.78 |
| 100（默认） | 30.85 |
| 200 | 32.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 才是主开关
| 0.0005 | 31.45 | 15.9x | 0.0081 mm |
| 0.001 | 31.34 | 31.9x | 0.0081 mm |
| 0.002（本项目） | 30.72 | 65.1x | 0.0081 mm |
| 0.005 | 31.70 | 157.7x | 0.0325 mm |
| 0.01 | 31.97 | 312.8x | 0.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 动手练
传感器切片：打印本项目模型的 sensor_adr / sensor_dim 表，写一个 read(name) 函数， 验证 read("ee_pos") 等于 data.site_xpos[ee_id]。
接触力分量：让 target_object 静置在地面，打印所有接触点的 dist 与 force，验证合力等于 m*g。再按 14.2.2 的公式换算到世界系。
执行器对比：用同一个单连杆模型，分别用 <position> / <motor> / <velocity> / <general>（带与不带 biastype），记录稳态角度，观察 general(NONE) 的发散。
weld 抓取：先不做任何处理直接激活（观察瞬移量），再用 14.4.5 的菜谱重试， 对比瞬移量。然后搬运一段距离并监控 weld_violation。
性能权衡：扫 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]。
步骤：
关键代码：
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 的公式换算到世界系。
步骤：
关键代码：
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 💥 发散！
关键结论：
练习 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 velocimeter | RL 策略的速度观测选择（世界系 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() | 所有需要保存位置快照的场景 |
扩展阅读方向
下一部分：dm_control —— 把「模型 + 任务 + 观测 + 奖励」打包成标准强化学习环境。
上一章：13 · 渲染、相机与录屏 ｜ 下一章：15 · dm_control 入门
