解读 13 · kinematics/math.rs:变换与向量工具
解读对象:
kinematics/src/math.rs(180 行)
需要的前置:知道四元数是什么即可;第一辑《解读 07》有助于理解用途,非必须。
为什么不引 nalgebra
机器人代码几乎总要用线性代数库,这个 crate 偏偏手写了全部数学。文件开头的注释(math.rs:1-7)把理由说尽:前向运动学只需要"复合十几个刚体变换、旋转几个向量",约一百行;tests/ 里的 MuJoCo fixture 把每个运算钉到 1e-6 精度——这比依赖的名字是更强的正确性论证,nalgebra 增加的是编译时间,不是信心。
于是全部内容就是两个类型:
Quat(math.rs:19):一个旋转。约定 Hamilton、标量在前[w, x, y, z](math.rs:9-12),与 MJCF 及所有线上格式一致(wxyz,math.rs:76)。Pose(math.rs:105):一个刚体变换 = 先按quat旋转、再平移pos。transform_point(math.rs:121)变换点,Mul(math.rs:127)复合两个位姿。
两个必须钉死的约定
乘法顺序。 a * b 表示"先 b 后 a"——父系乘子系,链式写法从根到尖自左向右读(math.rs:10-12)。顺序反了,整条运动链就镜像了。测试 pose_composition_applies_the_right_operand_first(math.rs:162)用一个"转 90° 的父系里走一步"的例子钉死语义:先走后转落在 +y,先转后走停在 +x。
退化输入。 normalized(math.rs:42-48)遇到近零四元数返回 identity 而不是 NaN。注释很直白:这种输入只可能来自手写的 XML 属性,忽略坏四元数的模型可以诊断,充满 NaN 的模型不行。mjcf.rs:219-227 里零关节轴回退 +z 是同一哲学。
其余是标准实现:from_axis_angle(math.rs:51)、叉积形式的 rotate(math.rs:58,省掉完整的 q v q⁻¹ 三明治)、单位四元数下共轭即逆(math.rs:71)、从旋转矩阵元素展开的 yaw(math.rs:81)。impl Mul for Quat(math.rs:89)是 Rust 的运算符重载——给自定义类型实现 Mul trait 后,* 就能作用于它。
测试即规格
三个测试各自守住一条人脑可验的性质(math.rs:149-179):绕 +z 转 90° 把 +x 送到 +y;复合顺序如上;零四元数归一化得 identity 而非 NaN。加上 tests/fk_against_mujoco.rs 用 64 个随机位姿对照 MuJoCo 本尊(lib.rs:17-19),手写数学有两层证据:先验性质,后精度。
你带走的收获
- 「最小集 + fixture 钉精度」是引入重型依赖前的合法选项:先写清需求规模,再决定要不要库。
- 约定(乘法顺序、标量在前)必须用测试钉住,否则会在某次重构里静默翻转。
- 对来自配置文件的坏数据,返回可诊断的默认值好过传播 NaN。
- 叉积形式旋转、展开式 yaw,这些省一次三角函数或一次完整乘法的小技巧值得收藏。