Skip to content

[BUG FIX] Fix rigid body Jacobian getter for compound joints. #2706

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
vlordier wants to merge 1 commit into
base: main
Choose a base branch
from
+72 −7
Open

[BUG FIX] Fix rigid body Jacobian getter for compound joints. #2706

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions genesis/assets/xml/compound_joint.xml
View file
Open in desktop
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove this and use procedural generation.

Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<mujoco model="compound_joint">
<!-- Two-link arm where the first link has TWO compound revolute joints on the same body.
This configuration reproduces issue #1608: before the dof_offset fix, the second joint
writes to the wrong Jacobian column, overwriting the first joint's entry. -->
<compiler angle="radian"/>
<option gravity="0 0 0"/>
<worldbody>
<body name="base" pos="0 0 0">
<!-- seg1 carries TWO revolute joints (compound joints on the same body) -->
<body name="seg1" pos="0 0 0">
<joint name="j_x" axis="1 0 0" type="hinge"/>
<joint name="j_y" axis="0 1 0" type="hinge"/>
<geom type="capsule" fromto="0 0 0 0 0 0.4" size="0.02" rgba="1 0 0 1"/>
<body name="seg2" pos="0 0 0.4">
<joint name="j_z" axis="0 0 1" type="hinge"/>
<geom type="capsule" fromto="0 0 0 0 0 0.4" size="0.02" rgba="0 1 0 1"/>
</body>
</body>
</body>
</worldbody>
</mujoco>
13 changes: 6 additions & 7 deletions genesis/engine/entities/rigid_entity/rigid_entity.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2472,7 +2472,6 @@ def _func_get_jacobian(
while i_l > -1:
I_l = [i_l, i_b] if qd.static(self.solver._options.batch_links_info) else i_l

dof_offset = 0
for i_j in range(links_info.joint_start[I_l], links_info.joint_end[I_l]):
I_j = [i_j, i_b] if qd.static(self.solver._options.batch_joints_info) else i_j

Expand All @@ -2482,7 +2481,9 @@ def _func_get_jacobian(
elif joints_info.type[I_j] == gs.JOINT_TYPE.REVOLUTE:
i_d = joints_info.dof_start[I_j]
I_d = [i_d, i_b] if qd.static(self.solver._options.batch_dofs_info) else i_d
i_d_jac = i_d + dof_offset - self._dof_start
# 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])
Comment on lines +2484 to 2488
Copy link

Copilot AI Apr 13, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copilot uses AI. Check for mistakes.

Expand All @@ -2496,7 +2497,7 @@ def _func_get_jacobian(
elif joints_info.type[I_j] == gs.JOINT_TYPE.PRISMATIC:
i_d = joints_info.dof_start[I_j]
I_d = [i_d, i_b] if qd.static(self.solver._options.batch_dofs_info) else i_d
i_d_jac = i_d + dof_offset - self._dof_start
i_d_jac = i_d - self._dof_start
translation = gu.qd_transform_by_quat(dofs_info.motion_vel[I_d], links_state.quat[i_l, i_b])

self._jacobian[0, i_d_jac, i_b] = translation[0] * pos_mask[0]
Expand All @@ -2507,14 +2508,14 @@ def _func_get_jacobian(
# translation
for i_d_ in qd.static(range(3)):
i_d = joints_info.dof_start[I_j] + i_d_
i_d_jac = i_d + dof_offset - self._dof_start
i_d_jac = i_d - self._dof_start

self._jacobian[i_d_, i_d_jac, i_b] = 1.0 * pos_mask[i_d_]

# rotation
for i_d_ in qd.static(range(3)):
i_d = joints_info.dof_start[I_j] + i_d_ + 3
i_d_jac = i_d + dof_offset - self._dof_start
i_d_jac = i_d - self._dof_start
I_d = [i_d, i_b] if qd.static(self.solver._options.batch_dofs_info) else i_d
rotation = dofs_info.motion_ang[I_d]
translation = rotation.cross(tgt_link_pos - links_state.pos[i_l, i_b])
Expand All @@ -2526,8 +2527,6 @@ def _func_get_jacobian(
self._jacobian[4, i_d_jac, i_b] = rotation[1] * rot_mask[1]
self._jacobian[5, i_d_jac, i_b] = rotation[2] * rot_mask[2]

dof_offset = dof_offset + joints_info.n_dofs[I_j]

i_l = links_info.parent_idx[I_l]

@gs.assert_built
Expand Down
45 changes: 45 additions & 0 deletions tests/test_rigid_physics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2931,6 +2931,51 @@
assert_allclose(J_p[:3, 0], lin_expected, tol=tol)


@pytest.mark.required
def test_jacobian_compound_joints(tol):
"""
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).
"""
Comment on lines +2936 to +2945
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove docstring.

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()
Comment on lines +2946 to +2950
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

One line per options in unit tests and examples.


# 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]

Check warning on line 2956 in tests/test_rigid_physics.py

View check run for this annotation

Claude / Claude Code Review

Wrong cross-product notation in test comment (r×ω vs ω×r) 

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 expe
Comment on lines +2954 to +2956
Copy link
Copy Markdown

@claude claude Bot Apr 14, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 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.

qpos = np.zeros(robot.n_dofs, dtype=gs.np_float)
robot.set_qpos(qpos, zero_velocity=True)
Comment on lines +2957 to +2958
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove this. It is already the default.

scene.step()

end_link = robot.get_link("seg2")
J = tensor_to_array(robot.get_jacobian(end_link)) # shape (6, 3)
Comment on lines +2952 to +2962
Copy link

Copilot AI Apr 13, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copilot uses AI. Check for mistakes.
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove tensor_to_array. It is useless here.


L = 0.4 # seg1 length
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove this constant, it is useless.

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)
Comment on lines +2957 to +2976
Copy link
Copy Markdown
Collaborator

@duburcqa duburcqa Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove np.array( wrapping. it is necessary.



@pytest.mark.required
def test_mjcf_parsing_with_include():
scene = gs.Scene()
Expand Down
Loading