From 1388789af8d2402265ccfc304b8c69e63b58a188 Mon Sep 17 00:00:00 2001 From: Ouassim Aouattah Date: Thu, 9 Jul 2026 13:43:53 +0200 Subject: [PATCH] fix(robot): index actuator qpos reads with jnt_qposadr, not jnt_dofadr MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- robohive/robot/robot.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 34279954..12e5338e 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -365,7 +365,10 @@ def configure_robot(self, sim, config_path): actuator_trnid = sim.model.actuator_trnid[actuator['sim_id'], 0] if actuator_trntype == mujoco.mjtTrn.mjTRN_JOINT: # // force on joint actuator['data_type'] = 'qpos' - actuator['data_id'] = sim.model.jnt_dofadr[actuator_trnid] + # qpos is indexed by jnt_qposadr, not jnt_dofadr; the two + # diverge when a free/ball joint precedes this joint + # (freejoint: 7 qpos vs 6 dofs), shifting every read. + actuator['data_id'] = sim.model.jnt_qposadr[actuator_trnid] elif actuator_trntype == mujoco.mjtTrn.mjTRN_TENDON: # force on tendon actuator['data_type'] = 'ten_length' actuator['data_id'] = actuator_trnid