[BUG FIX] Fix rigid body Jacobian getter for compound joints.#2706
[BUG FIX] Fix rigid body Jacobian getter for compound joints.#2706vlordier wants to merge 1 commit intoGenesis-Embodied-AI:mainfrom
Conversation
…und joints
When a single body carries multiple joints (compound joints, e.g. a 3-axis
gimbal modelled as separate revolute joints in MJCF), _func_get_jacobian
accumulated a per-link dof_offset and added it to the column index:
i_d_jac = i_d + dof_offset - self._dof_start
joints_info.dof_start[I_j] is already the global scene-level DOF index, so
subtracting self._dof_start gives the correct entity-local column. Adding
dof_offset on top of that shifts subsequent joints' columns by +1, +2, … per
position within the link, causing later joints to overwrite earlier ones in
the Jacobian matrix and producing discontinuous / incorrect IK solutions
(issue Genesis-Embodied-AI#1608).
For the common case (one joint per link), dof_offset is always 0 so behaviour
is unchanged. For compound joints the fix gives correct, unique columns.
Changes:
- Remove dof_offset initialisation and increment from _func_get_jacobian
- Use i_d_jac = i_d - self._dof_start for REVOLUTE, PRISMATIC and FREE joints
- Add test fixture xml/compound_joint.xml (3-DOF arm with 2 compound revolutes
on the same body) and test_jacobian_compound_joints() that verifies the
analytical Jacobian at q=0 against known expected values
There was a problem hiding this comment.
Pull request overview
Fixes incorrect Jacobian column indexing for compound joints (multiple DOFs on the same body) by removing an erroneous per-link DOF offset, and adds a regression test + minimal MJCF reproducer for issue #1608.
Changes:
- Remove
dof_offsetfrom Jacobian column indexing in_func_get_jacobianfor REVOLUTE/PRISMATIC/FREE joints. - Add a regression test that validates the Jacobian for a compound-joint MJCF model.
- Add a new MJCF asset (
compound_joint.xml) used by the regression test.
Reviewed changes
Copilot reviewed 3 out of 3 changed files in this pull request and generated 2 comments.
| File | Description |
|---|---|
genesis/engine/entities/rigid_entity/rigid_entity.py |
Fix Jacobian column indexing by mapping global DOF indices to entity-local columns without an extra per-link offset. |
tests/test_rigid_physics.py |
Add regression test for compound-joint Jacobian behavior (issue #1608). |
genesis/assets/xml/compound_joint.xml |
Add minimal MJCF model with compound joints to reproduce/test the Jacobian issue. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| # joints_info.dof_start is the global scene-level DOF index; subtract entity | ||
| # offset to get the entity-local column index into _jacobian. | ||
| i_d_jac = i_d - self._dof_start | ||
| rotation = gu.qd_transform_by_quat(dofs_info.motion_ang[I_d], links_state.quat[i_l, i_b]) | ||
| translation = rotation.cross(tgt_link_pos - links_state.pos[i_l, i_b]) |
There was a problem hiding this comment.
For compound joints, computing the joint axis as transform_by_quat(dofs_info.motion_ang, links_state.quat[i_l]) uses the final link orientation (after all joints on that link). That will rotate earlier joints’ axes by later joints on the same body, yielding incorrect Jacobian columns for non-zero compound joint angles. Consider using per-joint world axes/anchors from joints_state (e.g., joints_state.xaxis/xanchor computed in FK) or computing the intermediate quat/pos within this loop so each joint’s axis is expressed using the pose before subsequent joints on the same link.
| # Zero configuration: seg2 origin is at (0, 0, 0.4) in world frame. | ||
| # Analytical Jacobian: | ||
| # j_x (col 0): rot=[1,0,0], r=[0,0,0.4] → J_pos = r×ω = [0,-0.4,0], J_rot=[1,0,0] | ||
| # j_y (col 1): rot=[0,1,0], r=[0,0,0.4] → J_pos = r×ω = [0.4,0,0], J_rot=[0,1,0] | ||
| # j_z (col 2): at seg2 origin; r=[0,0,0] → J_pos = [0,0,0], J_rot=[0,0,1] | ||
| qpos = np.zeros(robot.n_dofs, dtype=gs.np_float) | ||
| robot.set_qpos(qpos, zero_velocity=True) | ||
| scene.step() | ||
|
|
||
| end_link = robot.get_link("seg2") | ||
| J = tensor_to_array(robot.get_jacobian(end_link)) # shape (6, 3) |
There was a problem hiding this comment.
This regression test only validates the Jacobian at q=[0,0,0]. That configuration won’t catch issues where compound-joint axes/columns become incorrect only when earlier joints are affected by later joints (i.e., nonzero angles). Adding at least one additional nonzero configuration (and/or asserting robot.n_dofs == 3 to lock the model shape) would make the test robust against remaining compound-joint Jacobian bugs.
|
@claude review |
![claude[bot]](https://avatars.githubusercontent.com/in/1236702?s=60&v=4){kind=small}
There was a problem hiding this comment.
The index-offset fix is correct and addresses issue #1608, but the outstanding Copilot comment about compound-joint axis computation at non-zero angles is a valid remaining concern that warrants a human look before merging.
Extended reasoning...
Overview
This PR removes dof_offset from the Jacobian column index formula in _func_get_jacobian across all three joint-type branches (REVOLUTE, PRISMATIC, FREE), and adds a new MJCF fixture and regression test.
Security risks
None — this is pure physics math with no I/O, auth, or external data paths.
Level of scrutiny
High. _func_get_jacobian is on the critical path for IK. Even a subtle error here will silently produce wrong end-effector trajectories for any robot using compound joints.
Outstanding concerns
The Copilot inline comment on line 2488 raises a valid remaining issue: the joint axis is computed as transform_by_quat(dofs_info.motion_ang[I_d], links_state.quat[i_l, i_b]), which uses the link's final accumulated orientation after all compound joints on that link. At q=[0,0,0] all intermediate orientations coincide (identity), so the regression test passes — but at any non-trivial compound-joint configuration the axis directions would be wrong. This concern is unaddressed and the test only covers q=[0,0,0]. The nit about the cross-product comment label (r×ω vs ω×r) is minor.
| # j_x (col 0): rot=[1,0,0], r=[0,0,0.4] → J_pos = r×ω = [0,-0.4,0], J_rot=[1,0,0] | ||
| # j_y (col 1): rot=[0,1,0], r=[0,0,0.4] → J_pos = r×ω = [0.4,0,0], J_rot=[0,1,0] | ||
| # j_z (col 2): at seg2 origin; r=[0,0,0] → J_pos = [0,0,0], J_rot=[0,0,1] |
There was a problem hiding this comment.
🟡 The inline comment in test_jacobian_compound_joints (lines 2954–2956) uses r×ω notation but the values provided are actually ω×r (the correct Jacobian formula). For j_x: r=[0,0,0.4], ω=[1,0,0]: r×ω=[0,+0.4,0] but the comment claims [0,-0.4,0], which is ω×r. Similarly for j_y. The expected array values in the test are numerically correct; only the comment label is reversed. A reviewer hand-verifying the test using the stated formula would compute the wrong sign and incorrectly conclude the expected values are wrong.
Extended reasoning...
What the bug is and how it manifests
The comment block above the expected array in test_jacobian_compound_joints documents the analytical Jacobian by writing the formula as J_pos = r×ω. The cross-product operand order in that label is backwards: the correct linear Jacobian column for a revolute joint is J_pos = ω × r (angular velocity cross lever arm), not r × ω.
The specific code path that triggers it
Lines 2954–2956 of tests/test_rigid_physics.py (in the newly added test):
# j_x (col 0): rot=[1,0,0], r=[0,0,0.4] → J_pos = r×ω = [0,-0.4,0], J_rot=[1,0,0]
# j_y (col 1): rot=[0,1,0], r=[0,0,0.4] → J_pos = r×ω = [0.4,0,0], J_rot=[0,1,0]
Why existing code doesn't prevent it
This is a purely documentation error in a comment. No runtime check or linter catches incorrect cross-product notation in a string literal.
What the impact would be
Any developer reading this test to verify correctness would compute r × ω as the comment instructs and get the opposite sign, leading them to incorrectly believe the expected values in the test array are wrong.
Step-by-step proof
For j_x: ω=[1,0,0], r=[0,0,0.4]:
- r×ω = [0,0,0.4]×[1,0,0] = [0·0-0.4·0, 0.4·1-0·0, 0·0-0·1] = [0, +0.4, 0] — does NOT match the comment's claimed [0,-0.4,0]
- ω×r = [1,0,0]×[0,0,0.4] = [0·0.4-0·0, 0·0-1·0.4, 1·0-0·0] = [0, -0.4, 0] ✓ matches expected
For j_y: ω=[0,1,0], r=[0,0,0.4]:
- r×ω = [0,0,0.4]×[0,1,0] = [0·0-0.4·1, 0.4·0-0·0, 0·1-0·0] = [-0.4, 0, 0] — does NOT match the comment's claimed [0.4,0,0]
- ω×r = [0,1,0]×[0,0,0.4] = [1·0.4-0·0, 0·0-0·0.4, 0·0-1·0] = [0.4, 0, 0] ✓ matches expected
This is consistent with the implementation: rotation.cross(tgt_link_pos - links_state.pos[...]) computes ω×r. The fix is simply to change r×ω to ω×r in the two comment lines.
There was a problem hiding this comment.
Remove this and use procedural generation.
| """ | ||
| Regression test for issue #1608: compound joints (multiple revolute joints on the same | ||
| body) produced wrong Jacobian columns because `dof_offset` was erroneously added to | ||
| `joints_info.dof_start`, which already encodes the global DOF position. | ||
|
|
||
| Before the fix, j_y (col 1) wrote to column 2 (1 + dof_offset=1 = 2) and j_z (col 2) | ||
| also wrote to column 2, making columns 1 and 2 identical. The test checks against the | ||
| known analytical Jacobian at q=[0,0,0] for a 3-DOF arm where seg1 has two compound | ||
| revolute joints (j_x, j_y) and seg2 has one (j_z). | ||
| """ |
| scene = gs.Scene(show_viewer=False) | ||
| robot = scene.add_entity( | ||
| gs.morphs.MJCF(file="xml/compound_joint.xml", requires_jac_and_IK=True), | ||
| ) | ||
| scene.build() |
There was a problem hiding this comment.
One line per options in unit tests and examples.
| scene.step() | ||
|
|
||
| end_link = robot.get_link("seg2") | ||
| J = tensor_to_array(robot.get_jacobian(end_link)) # shape (6, 3) |
There was a problem hiding this comment.
Remove tensor_to_array. It is useless here.
| qpos = np.zeros(robot.n_dofs, dtype=gs.np_float) | ||
| robot.set_qpos(qpos, zero_velocity=True) |
There was a problem hiding this comment.
Remove this. It is already the default.
| qpos = np.zeros(robot.n_dofs, dtype=gs.np_float) | ||
| robot.set_qpos(qpos, zero_velocity=True) | ||
| scene.step() | ||
|
|
||
| end_link = robot.get_link("seg2") | ||
| J = tensor_to_array(robot.get_jacobian(end_link)) # shape (6, 3) | ||
|
|
||
| L = 0.4 # seg1 length | ||
| expected = np.array( | ||
| [ | ||
| [0.0, L, 0.0], # pos x | ||
| [-L, 0.0, 0.0], # pos y | ||
| [0.0, 0.0, 0.0], # pos z (j_z at seg2 origin, no translation) | ||
| [1.0, 0.0, 0.0], # rot x | ||
| [0.0, 1.0, 0.0], # rot y | ||
| [0.0, 0.0, 1.0], # rot z | ||
| ], | ||
| dtype=gs.np_float, | ||
| ) | ||
| assert_allclose(J, expected, tol=tol) |
There was a problem hiding this comment.
Remove np.array( wrapping. it is necessary.
| end_link = robot.get_link("seg2") | ||
| J = tensor_to_array(robot.get_jacobian(end_link)) # shape (6, 3) | ||
|
|
||
| L = 0.4 # seg1 length |
There was a problem hiding this comment.
Remove this constant, it is useless.
duburcqa
changed the title
Root cause
_func_get_jacobianaccumulated a per-linkdof_offsetand added it to the Jacobian column index:joints_info.dof_start[I_j]is the global scene-level DOF index, so subtractingself._dof_startalready gives the correct entity-local column. Addingdof_offseton top of that shifts each successive joint's column by +1, +2, … causing later compound joints to overwrite earlier ones and producing wrong / discontinuous IK solutions.For the common case (one joint per link),
dof_offsetis always 0 — existing robots are unaffected.The bug only triggers for compound joints (multiple revolute/prismatic joints on the same body), which is valid MJCF — a ball-socket joint is often modelled this way.
Example (2 revolute joints on
seg1, DOFs 0 and 1):i_ddof_offseti_d_jaci_d_jacFix
Remove
dof_offsetfrom thei_d_jacformula in all three joint-type branches (REVOLUTE, PRISMATIC, FREE):Test
New fixture
xml/compound_joint.xml: a 3-DOF arm whereseg1carries two compound revolutes (j_x,j_y) andseg2carries one (j_z).test_jacobian_compound_jointsverifies the analytical Jacobian atq=[0,0,0]against closed-form expected values. Max error = 0.0.Closes #1608