Skip to content

fix: pin get_body_*_vel_b to analytical body-frame semantics across backends - #1254

Merged
TATP-233 merged 2 commits into
dev/issue-1042-manager-based-apifrom
fix/discussion-1246-body-vel-b-contract
Aug 23, 2026
Merged

fix: pin get_body_*_vel_b to analytical body-frame semantics across backends#1254
TATP-233 merged 2 commits into
dev/issue-1042-manager-based-apifrom
fix/discussion-1246-body-vel-b-contract

Conversation

@TATP-233

@TATP-233 TATP-233 commented Aug 22, 2026

Copy link
Copy Markdown
Collaborator

背景

按 discussion #1246 的决策(决策点 1/2):get_body_lin_vel_b / get_body_ang_vel_b 语义钉死为 mjlab/Isaac 风格解析式 —— 世界系速度 × 该 body 世界系姿态的逆旋转(quat_apply_inverse(quat_w, vel_w)),对任意 body(含 root)都有良好定义。

实证确认退化问题两个后端都存在(讨论只观测到 motrix):给 base 设世界系速度 [0.5, -0.2, 0.8] + 30° yaw 后,MuJoCo framelinvel(refname=baselink)与 MotrixSim frame sensor 对 root body 均读出 [0, 0, 0] —— 两者测的都是相对 base 的运动。

改动

  • base.py:在 SimBackend 契约 docstring 中钉死解析式语义,并明确禁止"相对 baselink 的运动"实现。
  • motrixget_body_*_vel_b 改用现成的 link 世界系速度 cache(_ensure_link_velocity_cache)+ get_body_quat_w 解析计算,不再读 base 相对 frame sensor。
  • mujocoget_body_*_vel_b 改用 _tracked_quat_w_all / _tracked_*_vel_w_all 世界系 sensor view 解析计算。
  • drake:原实现把速度表达到 base 系,改为表达到各 body 自身坐标系(root 时两者等价)。
  • mjwarp:本就 NotImplementedError,无改动。
  • 移除随之失效的 track_linvel_b_* / track_angvel_b_* 传感器(mujoco/xml.py、motrix/scene.py),避免误导性的相对运动读数被再次误用;track_pos_b / track_quat_b 保留(pos/quat 的 baselink 相对语义不变)。
  • rotation.py:新增 np_quat_apply_inverse_batchednp_quat_conjugate 推广到 (..., 4)(原实现对 3D 输入会沿错误轴取反)。

决策点 3:回退 sensor-bound reward 临时方案

契约修正后 Entity.data.root_link_*_vel_b 在所有后端恢复正确语义,临时方案(58ec02ea)随之回退:

  • 四足全部 joystick owner(appo/ppo/sac/td3/flashsac;go1/go2/go2w/a2/rough)的 tracking_lin_vel / tracking_ang_vel / lin_vel_z / ang_vel_xy 切回通用 entity-data 项(track_lin_vel_xy_exp / track_ang_vel_z_exp / lin_vel_z_l2 / ang_vel_xy_l2),恢复单份实现;flashsac go2 保留 std=sqrt(0.4) 调参。
  • 删除 locomotion common 中新增的 sensor-bound 版本 track_lin_vel / track_ang_vel / lin_vel_z / ang_vel_xy 及其测试(common/manager_terms.py 恢复至临时方案前内容)。
  • 保留 common/sensor_terms.pySensorTermBase 仍被 G1 家族使用,G1 的 sensor 绑定(pelvis_local_linvel / torso_gyro)是设计如此,不属于临时方案。

测试

  • tests/base/test_sim_backend.py:mujoco / motrix 各加 root body vel_b 语义测试(30° yaw + 非零速度下对照解析期望值,防退化恒零回归);TestCrossBackendBodySensors 加非零 base 运动下的跨后端 vel_b 一致性测试(含 root)。
  • tests/utils/test_math_utils.pynp_quat_apply_inverse_batched 对旋转矩阵转置的等价性与 round-trip。
  • tests/utils/test_xml_utils.py:移除已删除传感器的断言。
  • test_reward_injection.py / test_manager_based_cfg.py / test_manager_gait_terms.py:随临时方案回退恢复对通用 entity-data 项的断言。

Collector 吞吐量化(解析式 vs 传感器,A/B 交替 3 轮,8192 envs)

scripts/benchmark/rl/benchmark_offpolicy_collector_active.py --all 对比基线 6047408 与本分支 65b7b78

case sps(基线) sps(本分支) Δ
flashsac/g1_walk_flat/motrix 78743±6009 79321±3698 +0.7%
flashsac/g1_walk_flat/mujoco 55287±3207 58723±2686 +6.2%
sac/g1_motion_tracking/motrix 44489±4702 44381±4220 −0.2%
sac/g1_motion_tracking/mujoco 50333±893 51857±845 +3.0%

getter 级微基准(每次 env.step 后单次调用,200 步 × 3 轮):解析式 getter 比传感器读取每次调用贵约 0.15–0.16ms(mujoco lin 0.036→0.197ms、ang 0.033→0.177ms;motrix lin 0.103→0.266ms、ang 0.063→0.222ms),每 vector step 两个 getter 合计约 +0.3ms,占 env_step 的 ~0.2–0.3%。

结论:无可辨吞吐回归 —— 所有 case 的 Δ 都在轮间噪声(±~5%)内;getter 新增成本被旧路径 step 内 frame sensor 计算成本的移除所对冲。

Validation

  • make test-all 通过(format / mypy / pyright / pytest 非 slow + benchmark smoke),最终提交 bdba404
  • 相关 slow 测试单独跑过:TestMuJoCoBodySensors / TestMotrixBodySensors / TestCrossBackend / TestCrossBackendBodySensors 46 passed。
  • 注:首次 make test-alltest_removed_legacy_env_packages_stay_removed 因本地 src/unilab/envs/{locomotion,manipulation,motion_tracking} 残留未跟踪的空目录(旧布局 __pycache__)构成 namespace package 而失败;清理后通过,与本改动无关。

…ackends

Discussion #1246 decided the contract: get_body_lin_vel_b /
get_body_ang_vel_b return the world-frame velocity expressed in each body's
own frame (mjlab/Isaac-style quat_apply_inverse(quat_w, vel_w)), well-defined
for every body including the root.

Previously both the MuJoCo framelinvel/frameangvel sensors and the MotrixSim
frame sensors measured motion relative to the baselink reference frame, so
the root body read exactly zero (verified empirically on both backends).

- base.py: pin the analytical semantics in the SimBackend contract docstrings
- motrix: compute vel_b from the link velocity cache + world-frame quats
  instead of base-relative frame sensors
- mujoco: compute vel_b from the tracked world-frame sensor views
- drake: express velocities in each body's frame (was: baselink frame)
- drop the now-unused track_linvel_b_*/track_angvel_b_* sensors
- add np_quat_apply_inverse_batched and extend np_quat_conjugate to (..., 4)
- tests: root-body vel_b semantic checks per backend, cross-backend parity
  under non-zero base motion, batched-rotation helper coverage
…a terms

Discussion #1246 decision point 3: with get_body_*_vel_b pinned to
analytical body-frame semantics (previous commit), Entity
root_link_*_vel_b is well-defined on every backend, so the task-layer
stopgap from 58ec02e is no longer needed.

- Rewire all quadruped joystick owners (appo/ppo/sac/td3/flashsac;
  go1/go2/go2w/a2/rough) back to the generic track_lin_vel_xy_exp /
  track_ang_vel_z_exp / lin_vel_z_l2 / ang_vel_xy_l2 terms, restoring a
  single implementation; flashsac go2 keeps its std=sqrt(0.4) tuning.
- Drop the sensor-bound track_lin_vel / track_ang_vel / lin_vel_z /
  ang_vel_xy terms from locomotion common manager_terms (restored to
  the pre-stopgap content) and their tests.
- Keep common/sensor_terms.py: SensorTermBase is still used by the G1
  family, whose sensor binding (pelvis_local_linvel / torso_gyro) is by
  design, not part of the stopgap.

Validation: make test-all passed.
@TATP-233

Copy link
Copy Markdown
Collaborator Author

已按讨论决策点 3 在本 PR 直接回退 sensor-bound reward 临时方案(commit bdba404):四足全部 owner 的四个基座速度 reward 项切回通用 entity-data 项,删除临时新增的 sensor-bound 实现;G1 的 sensor 绑定是设计如此,保留 SensorTermBase。

另补充 collector 吞吐量化(详见更新后的 PR body):A/B 交替 3 轮基准下 4 个 case 的 sps 变化 −0.2% ~ +6.2%,均在轮间噪声(±~5%)内;解析式 getter 单次调用比传感器读取贵 ~0.15ms,但被旧路径 step 内 frame sensor 成本的移除对冲,无可辨吞吐回归

make test-all 已在最终提交上通过。

@TATP-233
TATP-233 merged commit 2a3bcea into dev/issue-1042-manager-based-api Aug 23, 2026
6 checks passed
@TATP-233
TATP-233 deleted the fix/discussion-1246-body-vel-b-contract branch August 23, 2026 09:56
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant