fix(robot): index actuator qpos reads with jnt_qposadr, not jnt_dofadr#171
Merged
Merged
Conversation
Robot._resolve_data_ids caches each joint actuator's feedback address as data_id into sim.data.qpos, but computed it with jnt_dofadr (the qvel address). qpos and qvel addresses coincide only while all preceding joints are 1-DOF: any free/ball joint earlier in the model (e.g. a manipulable object defined before the arm) shifts every subsequent actuator's read by the qpos/qvel size difference (freejoint: 7 vs 6). The stale read feeds the velocity limiter in process_actuator(), which re-anchors each command to the wrong joint's position — in practice the arm freezes at whatever pose keeps the misread error small. Observed on anthrohive URDtriosPickPlace-v0 (box freejoint precedes the UR5e): the arm ignored all position targets until this fix.
vikashplus
approved these changes
Jul 15, 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
2 participants
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.
Bug
Robot._resolve_data_idscaches each joint actuator's feedback address (data_id) intosim.data.qpos, but computes it withjnt_dofadr— the qvel address space. The two address spaces coincide only while every preceding joint is 1-DOF: any free/ball joint earlier in the model shifts every subsequent actuator's qpos read by the qpos/qvel size difference (freejoint: 7 qpos vs 6 dofs).The stale read feeds the velocity limiter in
process_actuator(), which re-anchors each command to the wrong joint's position — in practice the arm freezes at whatever pose keeps the misread error small, silently ignoring position targets.Repro
Observed on anthrohive
URDtriosPickPlace-v0, where a manipulable box (freejoint) is defined before the UR5e: the arm ignored all position targets until this fix. Any model that places a freejoint body ahead of the actuated kinematic chain hits the same shift.Fix
One-line change: use
jnt_qposadr[actuator_trnid]for theqposdata_id. Models with only 1-DOF joints (or with the robot defined first) are byte-for-byte unaffected, since the two tables agree there.Note: this is independent of the
fix_act_id_indexingbranch (act_id vs sim_id remapping innormalize_actions) — different bug, no overlapping lines.