[wlgys8]

摘要

为任务侧提供 learner 可用的纯 torch 可微正向运动学：给定 root 状态与关节状态，直接求 link 位姿/速度，梯度贯通到全部输入。同时提供从 MotrixSim 编译后模型抽取运动树的能力，并以 backend 无关的查询接口暴露。

改动

motrix_env_core/math — 可微 FK 引擎

• KinematicTree（math/kinematics.py）：纯 torch 数据类（parent、关节轴、固定偏移；偏移四元数用仓库统一 xyzw 惯例）；positions() 只算 link 原点（含叶节点旋转剪枝），forward() 全套位姿/速度，梯度流向 root 状态与关节状态
• axis_angle_to_mat 放在 torch-native 的 math/rotation.py（与双命名空间的 quaternion.py 分工）

motrix_env_motrixsim — 编译模型 → 树 loader

• kinematic_tree(model, base_link) 从编译后引擎模型抽树（单一事实来源，不重新解析模型文件）：fixed link 折叠进最近 moving 后代的 offset，moving link 必须恰好一个 1-DoF hinge，q 向量顺序以 tree.joint_names 暴露

公共暴露

• KinematicTreeQuery(body, device)（core ModelQuery）：经 SimModel.others 返回 backend 无关的树，消费方不接触任何 simulator 模型

验收

• 平面三连杆臂闭式解对拍（位置/旋转/角速度/线速度，含非零 root 线速度）+ float64 有限差分梯度对拍 + xyzw 惯例 pin 测试（x 轴旋转，两种顺序数值不同处）
• G1 对拍：随机 root 位姿/速度 + 随机关节状态写入 sim，29 个 moving link 的位置、旋转、link 原点线速度、角速度逐 link 与 tree.forward() 吻合（atol 1e-4）
• query dispatch 契约测试覆盖新钩子；query 路径与 loader 产物一致性测试

测试

python -m pytest motrix_env_core/tests motrix_env_motrixsim/tests -q
# 327 passed, 3 skipped

prek（ruff check/format、copyright、license）全部通过。

备注

• 调试中发现并修正两个真问题：offset 在 moving 父级上的重复组合（第二个关节起误差累积）；Link.joints 文档与运行时不符（实际为方法）