第 4 章 装配与坐标变换¶
装配是最贴近实际工作的场景：将一个夹爪子树挂接到某个 link 之下，并使其在该 link 的坐标系中落于指定位姿。有三件事需要先说明：<freejoint> 的边界、weld 的实际语义，以及"依赖软约束缓慢拉到位"为何不可行。后半章转向朝向与坐标系的存储约定，以及如何校验自身的约定表未写反。
1. freejoint 与 joint type="free"¶
XML reference 给的定义是一行等价式：<freejoint/> 只有 name 与 group 两个属性，它等价于 <joint type="free" stiffness="0" damping="0" frictionloss="0" armature="0"/>，目的是确保关节的默认设置不被继承。
因此两者的差别仅在于是否继承 default class：所谓 <freejoint> 不继承 bias= 的根据正在此处——它将 stiffness/damping/frictionloss/armature 显式置为 0。在同一份 default（stiffness=5 damping=2 frictionloss=1 armature=0.3）套在同一个 body 上，只换关节写法：
| 写法 | dof_damping |
dof_armature |
|---|---|---|
<freejoint name="fj"/> |
六个 0 | 六个 0 |
<joint type="free"/> |
全 2.0 | 全 0.3 |
freejoint 的六个自由度全被钉回 0，default 一个字都不进——这就是那条机制的确切形态。
位置限制是硬性的。joint type 条目对 free 的规定是：只允许用在世界刚体的子刚体上；一个刚体里定义了自由关节就不能再定义别的关节；自由关节不能设限位（XML reference）。
四句各对应一种结果：
<!-- ① 放在二级 body 上：违反"自由关节只允许在世界刚体的直接子刚体上"这条限制 -->
<body name="shell"><body name="payload"><freejoint/> …
<!-- ② 放在 <worldbody> 的直接子 body 上 → 通过，nq=7 nv=6 -->
<!-- ③ 放在 worldbody 自己身上 → 违反限制，载入被拒 -->
<!-- ④ 同一个 body 上再加一个 hinge → 自由度超出自由关节的 6 个，载入被拒 -->
"不能设置限位"则是 schema 层面的：<freejoint> 只认 name 与 group 两个属性，写 limited="true" 会因属性名无法识别而被拒绝载入。
装配守则：外部资产包里带 <freejoint> 的子树，装到机器人上时必须删掉那个根关节。它的前提是"该零件单独仿真时自由落体"，而非"装到任何父体上都保持自由"。第 3 章「attach 与 asset 子模型声明」中"attach 带根关节的子树会被拒绝"指的就是这条限制。
2. weld 的 relpose 语义¶
weld 的语义有三处容易误解（XML reference 的 equality/weld 条目）。第一，约束强制执行的是"建模时"的相对位姿——定义模型那一刻两个 body 的相对位置和方向就是目标。第二，relpose（real(7)，默认 "0 1 0 0 0 0 0"）的前三个分量是 body1 局部坐标系里的锚点坐标，后四个分量是 body2 相对 body1 的朝向；但只要四元数部分全零（正是默认值），整个属性就被忽略，目标退回 qpos0 里的参考位姿。第三，torquescale（默认 1）设成 0 会让 weld 退化成 connect，即只管位置不管朝向。
两条结论：前三个分量是锚点坐标（body1 局部系里的一个点），不是"要移动多远"；四元数部分全零时整个 relpose 被忽略，weld 的目标退回建模位姿——没有误差可拉。
下面这份模型是本章后半段的共用实验台：mount 在原点，part 带 freejoint 悬在 z=0.719，part 里嵌一个 range="0 0.046" 的 slide 关节：
<mujoco model="weld_range">
<!-- 关闭接触：本章只考察 weld 与 range，需排除碰撞对读数的干扰 -->
<option timestep="0.002"><flag contact="disable"/></option>
<worldbody>
<!-- mount 固定在世界里（没有 joint），充当焊枪的另一端；mass=1 保证它不会被拖着走 -->
<body name="mount"><geom name="mg" type="sphere" size="0.01" mass="1"
contype="0" conaffinity="0"/></body>
<!-- part 带 freejoint → 6 dof 悬在空中，z=0.719 是建模时的相对位姿来源 -->
<body name="part" pos="0 0 0.719">
<freejoint name="fj"/>
<geom name="pg" type="sphere" size="0.01" mass="0.05" contype="0" conaffinity="0"/>
<!-- 被测件：0 ≤ q ≤ 46 mm 的限位slide，本章后半段所有越界数字都出自它 -->
<body name="slider" pos="0 0 -0.03">
<joint name="slide" type="slide" range="0 0.046" limited="true" damping="0.01"/>
<geom name="sg" type="box" size="0.005 0.005 0.005" mass="0.01"
contype="0" conaffinity="0"/>
</body>
</body>
</worldbody>
<!-- 不写 relpose → 四元数全零 → 目标退回 qpos0 的参考位姿（本节的核心陷阱） -->
<equality><weld name="w" body1="mount" body2="part"/></equality>
</mujoco>
步进 5000 步（每步记录一次 qpos，观察 slide 关节是否越界、part 的最终位置），四种写法的结果：
<weld> 写法 |
slide 最大 qpos |
越界 | 首次越界 | part 的最终 z |
|---|---|---|---|---|
relpose 留空（默认） |
0.000000 | 未越界 | — | 0.718590 |
relpose="0 0 -0.719 1 0 0 0" |
0.175210 | +0.129210（+280.9%） | 第 2 步 | −0.719410 |
同上 + <flag autoreset="disable"/> |
0.175210 | 与上一行逐位相同 | 第 2 步 | −0.719410 |
relpose="0 0 -0.719 0 0 0 0"（四元数全零） |
0.000000 | 未越界 | — | 0.718590 |
第一行的"未见任何变化"正是正确行为：约束在 t=0 即已满足，求解器无需作用。第三行说明这件事与 autoreset 无关（第 5 章「事故复盘：NaN 命令下的停摆」用另一条不含 weld 的模型测 NaN，两条路径别混）。第四行是第一行的孪生：位移部分明明写了 719 mm，却因为四元数全零被整个忽略——relpose 的前三个分量单独存在是没有意义的。这个反例值得反复提醒：先确认自己的场景真的触发了要测的机制。
3. 软约束与 range 越界¶
第二行那 129 mm 的越界不是数值事故，而是设计如此。computation 章的约束模型一节说限位时只有一句：
文档对限位只有一句：限制被求解器视为无摩擦接触来处理。
即 range 属于软约束：限位仅施加速度相关的恢复力，可被更大的力克服。越界幅度随参数的变化在该模型上可直接测出（每行仅改动一个量，步进 5000 步后取 qpos 最大值）：
| 改动 | 越界幅度 | 相对默认 |
|---|---|---|
默认 solreflimit="0.02 1" solimplimit="0.9 0.95 …" |
+0.129210 m | +280.9% |
solreflimit="0.005 1"（限位变硬） |
+0.031369 m | +68.2% |
solimplimit="0 0.1 …"（阻抗更软） |
+0.208715 m | +453.7% |
solreflimit="-2000 -20"（直接刚度格式） |
+0.290841 m | +632.3% |
iterations 20 / 100 / 200 |
+0.129210 m | 不变 |
integrator Euler / implicitfast |
+0.129210 m | 不变 |
三点读法：调参只能减轻症状不能消除它——越界发生在第 2 步的瞬态里，与求解器迭代数、积分器都无关；限位变硬（时间常数 0.02→0.005）确实能把幅度压到 1/4，但 0.002 那一档已低于文档建议下限（"timeconst 参数应至少比仿真时间步长高出两倍"，timestep=0.002 ⇒ 下限 0.004），只能看趋势不能当推荐值抄；阻抗参数 solimplimit 与越界幅度同向——把 d0/dwidth 从 0.9/0.95 改软到 0/0.1，越界反而涨到 +453.7%。
⇒ range 不能当硬限位用。
4. 装配的几种做法¶
按代价从低到高：
- 零 DOF 嵌套（首选）：把子树直接放进目标 body 下，
pos/quat一次写对，不产生任何约束行（nefc不涨）。 - 资产复用走
<attach>（第 3 章）：位姿由包裹它的<frame>或父 body 决定，一次声明可以装左右两份。 - 需要可拆卸表达时用 weld + t=0 精确对齐：先按目标位姿摆放 body 初值（或用
keyframe给 qpos0），让约束在初始就成立。 - 只在"确实想让它被拽过去"的任务里（如抓取后对齐）用软约束，并给足时间常数（≥ 2×
timestep）与合理的solimplimit。 - 校验靠数字，不靠画面：加载后看
d.nefc（约束行数）、d.efc_state（哪些约束在激活）、以及每个关节qpos是否还在range内。上面那张表就是这套校验跑出来的——nefc=7、越界 0.129210 m，都是肉眼在画面里看不出来的。
5. 引擎的存储约定¶
这一章后半段的主题是朝向：引擎会告诉你它按右手系、单位四元数存一切，但它不会告诉你你的约定表是不是镜像。
modeling 章的 frame orientations 一节：
"若干模型元素具有与其相关联的右手空间坐标系（spatial frame）。" "围绕由单位向量 (x, y, z) 给出的轴旋转角度 a 的 3D 旋转对应于四元数 (cos(a/2), sin(a/2)·(x, y, z))" "请记住旋转是右手的；如果向量 (x, y, z) 的方向反转，将导致相反的旋转。" "无论用户选择哪种机制，坐标系朝向在内部始终会转换为单位四元数。"
programming 章的数据布局一节：
"MuJoCo 使用单位四元数——即排列为 q = (w, x, y, z) 的 4D 单位向量……对应于零旋转的四元数是 (1, 0, 0, 0)。"
工程后果是切片位置。qpos 里 freejoint 的分量顺序是"三个平移自由度，后跟三个旋转自由度"（joint type 条目），四元数落在这段旋转分量的前四个位置、第 0 个是 w。一个只含 <freejoint> 的 body 编译出来是 nq=7 nv=6：位置层 7 个、速度层 6 个——速度层存的是角速度（3 维）而不是四元数增量（4 维）。任何"把 qvel 当四元数速度用"的代码，从维度上就已经错了。"切片取错（qpos[0:4] 还是 qpos[3:7]）"这个经典 bug，本质就是把 (w,x,y,z) 当成了 (x,y,z,w)。两种层次的分量摆位是：
flowchart LR
fj["freejoint<br/>6 个自由度"] --> qp["位置层 qpos<br/>7 个数"]
fj --> qv["速度层 qvel<br/>6 个数"]
qp --> tq["qpos[0:3]<br/>平移"]
qp --> rq["qpos[3:7]<br/>单位四元数 (w, x, y, z)"]
qv --> lv["qvel[0:3]<br/>线速度"]
qv --> av["qvel[3:6]<br/>角速度（非四元数增量）"]坑：qvel 不是四元数速度
freejoint 的 qpos[3:7] 才是 (w,x,y,z)，qvel 的旋转段是 3 维角速度——把 qvel 当四元数增量用，从维度上就已经错了。
6. 换基检查与转身不变量¶
6.1 换基矩阵与 det 检查¶
事故经过：代码里声明了一张 CAM_TO_BASE_AXES——"相机 x 右 → 机体左、相机 y 上 → 机体上、相机 z 出屏 → 机体后"。三条合起来构成的矩阵 det = −1，是镜像。而相机系（x 右 y 上 z 出屏，由 OpenCV 系绕 X 转 180° 得来）与机体系（x 前 y 左 z 上）都是右手系，两个右手系之间的换基只能是旋转，det 必为 +1。
把那张表照抄成矩阵，问题一行就能看出来：
import numpy as np
## 每行是相机某根轴在机体系 (x 前, y 左, z 上) 里的像
BAD = np.array([[0, 1, 0], # 相机 x 右 → 机体左
[0, 0, 1], # 相机 y 上 → 机体上
[-1, 0, 0]]) # 相机 z 出屏 → 机体后（三条全写）
np.linalg.det(BAD) # → -1.0
np.cross(BAD[0], BAD[1]) # → [1, 0, 0]，与第三行 (-1,0,0) 反号
后果不止于"某个通道符号相反"：Rz·Ry·Rx · A 无论三个角取什么值都是镜像矩阵，因此没有任何一组安装角能表示真实的相机；而标定解算器（Kabsch/Procrustes）不报错——它强制输出 det=+1，于是给出的欧拉角是一副任意角。同一批完美数据下，把 det 修对之后残差从 905 mm 回到 0.0 mm：反过来看，残差爆掉只是 det 检查缺位的下游症状。
三条守则：
- 三条"轴对应"只能取两条，第三条被叉积强制（机体系里
x_b × y_b = z_b）。写表时故意留一条不写，让它由叉积推出来。 - 写完任何"帧 A 三轴 → 帧 B 三轴"的表，第一件事算
det，第二件事把它钉进代码：assert np.linalg.det(A) > 0。 - 换轴表与换序表分开放、各测一次。下游若还要把分量重排（例如返回
(侧移, 前后, 升降)而不是(x,y,z)），那个换序本身也是 det=−1，会把第 2 条检查掩盖掉。
6.2 独立对照与转身不变量¶
自己写的四元数→矩阵，不能拿自己写的反函数来验。两条独立对照：
mju_quat2Mat/mju_mat2Quat是引擎现成实现，自己写的必须与它逐位一致，往返还应回到原值（[0.939693, 0.091409, 0.182817, 0.274226]走一圈不变）。- 这两个函数的矩阵参数都必须是 flat (9,) 的连续数组——传 3×3 会直接报类型错，reshape 出来的二维视图不兼容。
更狠的行为级判据是转身不变量：把自由基座绕任意轴转一个角度，让引擎重算 site_xpos，再把观测点变换回机体系——刚附在机体上的点在机体系里必须原地不动。它能同时抓住三种典型错法：
def quat2mat(q): # 用引擎实现做独立对照
r = np.zeros(9); mujoco.mju_quat2Mat(r, q); return r.reshape(3, 3)
def to_body(site_xpos, body_pos, body_quat):
"""全局点 → 机体系：先减原点，再用机体姿态阵转回去。"""
# 顺序不能颠倒：R 是线性的，但对平移不线性——先减后转才对
return quat2mat(body_quat).T @ (site_xpos - body_pos)
机体绕 (1,2,3) 转 40°、同时平移 0.05 m 后，正确实现给出残差 1.39e-17 m（浮点噪声），而三种错法立刻现形：
| 错法 | 与正确结果的偏差 |
|---|---|
| 只减原点、不转向 | 0.0494 m |
旋转乘反（用 R 而不是 Rᵀ） |
0.0817 m |
| 四元数顺序按 (x,y,z,w) 取 | 0.1538 m |
想让某条错法逃过去也容易：把平移去掉再跑，"只减原点"这一路就恰好也是零残差——所以判据必须配一个会动的机体。"拿自己的函数验自己的函数"三种错法一个都抓不住。第 6 章给出与本节配套的位移对照表（雅可比列范数 vs 实际位移，同样是两条独立路径）。
7. 参考系 site 的可视化分组¶
在 site/geom 上挂参考系时注意 group（第 7 章）：默认只画 0/1/2 组，group=4 的坐标系 site 会静默不显示，表现成"viewer 里没有查看法兰坐标系的选项"。朝向排查时这是最常见的"看不到 ≠ 不存在"。
8. 要点¶
<freejoint/>=<joint type="free">且强制不继承（四个偏置参数被钉成 0）；只能挂在世界刚体的直接子 body 上，放二级 body 报free joint can only be used on top level，同 body 再加别的关节报more than 6 dofs。- weld 的默认目标就是建模位姿 ⇒ 不设
relpose就没有拉力；relpose前三分量是 body1 局部锚点而非位移，且四元数部分全零时整个属性被忽略。 - 关节 range 是软约束（"视为无摩擦接触处理"），被顶穿是正常行为；幅度由
solreflimit/solimplimit决定，与迭代数、积分器无关。 - 装配靠 t=0 对齐、零 DOF 嵌套或 attach，而不是靠约束瞬态；校验用
nefc/efc_state/qpos越界量，不以画面目视为准。 - 内部一律单位四元数 (w,x,y,z)、零旋转 (1,0,0,0)；
qpos里 freejoint 是 3 平移 + 4 四元数（nq=7），qvel只有 3 个角速度（nv=6）。 - 右手系 ↔ 右手系的换基只能是旋转：
det必须 +1，而标定解算器不会替你检查。轴对应表只写两条，第三条由叉积定；换轴表与换序表分开测。 - 独立对照用
mju_quat2Mat/mju_mat2Quat（矩阵参数要 flat (9,)）；行为级判据用转身不变量，并确认机体真的动了、测试伤得到被测对象。 - 参考系 site 的
group落在可视化默认显示组之外会静默消失。