fix: pin get_body_*_vel_b to analytical body-frame semantics across backends - #1254
Merged
TATP-233 merged 2 commits intoAug 23, 2026
Conversation
…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.
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 已在最终提交上通过。 |
This was referenced Aug 23, 2026
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
背景
按 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 后,MuJoCoframelinvel(refname=baselink)与 MotrixSim frame sensor 对 root body 均读出[0, 0, 0]—— 两者测的都是相对 base 的运动。改动
base.py:在SimBackend契约 docstring 中钉死解析式语义,并明确禁止"相对 baselink 的运动"实现。get_body_*_vel_b改用现成的 link 世界系速度 cache(_ensure_link_velocity_cache)+get_body_quat_w解析计算,不再读 base 相对 frame sensor。get_body_*_vel_b改用_tracked_quat_w_all/_tracked_*_vel_w_all世界系 sensor view 解析计算。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_batched;np_quat_conjugate推广到(..., 4)(原实现对 3D 输入会沿错误轴取反)。决策点 3:回退 sensor-bound reward 临时方案
契约修正后
Entity.data.root_link_*_vel_b在所有后端恢复正确语义,临时方案(58ec02ea)随之回退: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)调参。track_lin_vel/track_ang_vel/lin_vel_z/ang_vel_xy及其测试(common/manager_terms.py恢复至临时方案前内容)。common/sensor_terms.py:SensorTermBase仍被 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.py:np_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: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。TestMuJoCoBodySensors/TestMotrixBodySensors/TestCrossBackend/TestCrossBackendBodySensors46 passed。make test-all时test_removed_legacy_env_packages_stay_removed因本地src/unilab/envs/{locomotion,manipulation,motion_tracking}残留未跟踪的空目录(旧布局__pycache__)构成 namespace package 而失败;清理后通过,与本改动无关。