mujoco: Add support for Universal and Screw Joints by azeey · Pull Request #980 · gazebosim/gz-physics

azeey · 2026-05-13T17:15:41Z

🎉 New feature

Part of #299

Summary

This pull request adds support for Universal and Screw joint types inside the MuJoCo physics engine plugin

Changes Made:

Add Universal joint support in MuJoCo by modeling two hinge joints in series on the child body.
Add Screw joint support in MuJoCo by modeling a rotational hinge joint and a translational slide joint in series on the child body, kinematically coupled by a joint equality constraint (mjEQ_JOINT).
Configure the rotational hinge joint as the primary joint stored in JointInfo. This aligns MuJoCo with DARTsim's internal architecture, ensuring both physics engines expose identical angular units (radians and rad/s) for screw joints across the gz-physics API, and maps linear force commands directly to motor torques.
Add out-of-bounds DOF access checks across the bullet-featherstone, dartsim, and mujoco plugins to safely reject invalid DOF queries without solver crashes.

Video of a universal joint

Screen.Recording.2026-05-12.at.15.31.28.mov

SDF file: https://gist.github.com/azeey/ec80bf54f326becd1869400bf1d7edf6#file-universal_joint_demo-sdf

Video of Screw Joint

Screen.Recording.2026-05-13.at.15.33.20.mov

SDF file: https://gist.github.com/azeey/ec80bf54f326becd1869400bf1d7edf6#file-screw_joint_demo-sdf

Test it

# Run MuJoCo unit & integration tests
ctest -R "COMMON_TEST_joint_features_mujoco|UNIT_mujoco_SDFFeatures_TEST"
# Run full joint basic features tests across all engines
ctest -R "COMMON_TEST_joint_basic_features"

Checklist

Generated-by: Gemini 3.0

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by and Generated-by messages.

Backports: If this is a backport, please use Rebase and Merge instead.

- Support universal joints in the MuJoCo plugin by creating two hinge joints in series and two actuators. - Add basic state integration tests and out-of-bounds DOF safety checks across MuJoCo, Dartsim, and Bullet-Featherstone wrappers. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

- Support screw joints in the MuJoCo physics engine plugin using coupled slide/hinge joints and equality constraints. - Make the rotational hinge joint the primary axis in the MuJoCo plugin, fully aligning its coordinate units (radians) with DARTsim. - Add a new anchored screw joint model with static damping in the test world. - Add a unified screw joint integration test suite inside joint_features, validating kinematic coupling and damping decay. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

…DF strings - Revert test.world back to its pristine state to ensure all other engine tests (dartsim, tpe) expecting exactly 9 models pass cleanly. - Migrate Universal joint checks in SDFFeatures_TEST to load a self-contained SDF string. - Migrate Screw joint coupling and damping checks in joint_features to load a self-contained SDF string. Generated-by: Gemini 3.0 Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

- Verify screw joint position limits enforcement across DARTsim and MuJoCo plugins. - Enforce position limits purely on the translational Slide joint to allow exact kinematic constraint coupling without solver damping. Generated-by: Gemini 3.0 Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

caguero

Nice videos! I left a few comments to consider.

caguero · 2026-05-15T20:20:34Z

    const Identity &_id, const std::size_t _dof) const
 {
  const auto *joint = this->ReferenceInterface<JointInfo>(_id);
+  if (_dof >= this->GetJointDegreesOfFreedom(_id))


I just noticed that this pattern is slightly different than the one in GetJointVelocity(), where you're checking the identifier before this block. Here, you're checking it after this block.

I think a fixed joint will return NAN pose and 0 velocity. Maybe that's the intention but just checking in.

Also, line 104-105 comment implies that a fixed joint will return 0. I think now it'll return NAN.

Okay, there seems to be a mismatch between what GetJointDegreesOfFreedom returns and what is an acceptable input value for _dof in bullet-featherstone. GetJointDegreesOfFreedom returns zero for RootJoint and FixedConstraintJoint joint types, but GetJointVelocity returns a valid value for RootJoint types and nan for FixedConstraintJoint while GetJointPosition returns 0 for both. In dartsim, the free joint at the root of the kinematic tree (model) is not exposed to the gz-physics API. Thus, there no way GetJointPosition or GetJointVelocity would be called on a free joint, but this doesn't appear to be the case in bullet-featherstone. I don't want to go down a rabbit hole in this PR so I'm inclined to revert all the changes in bullet-featherstone and skip it in the test.

caguero · 2026-05-15T20:32:04Z

+          joint->limited = false;
+        }
+
+        joint2 = mjs_addJoint(child, nullptr);


Just checking: Adding the two joints together will make GetDegreesOfFreedom() return 2 instead of 1?

Good catch! I added a test in ca30b39 and it failed. I ended up storing the number of equality constraints in JointInfo as you suggested in your review below and used that to subtract from the DOF computed by MuJoCo.

caguero · 2026-05-15T20:47:10Z

+  {
+    auto *m = jointInfo->worldInfo->mjModelObj;
+    int jntId = m->dof_jntid[jointInfo->nv_index];
+    for (int i = 0; i < m->neq; ++i)


I wonder if we could get rid of this loop by catching the information that you need in JointInfo and reading it from here directly instead of iterating. Besides the performance gain, we'd get rid of almost the same block in SetJointVelocity().

Thanks for the suggestion ca30b39

caguero · 2026-05-15T20:49:36Z

  const auto *identifier = std::get_if<InternalJoint>(&joint->identifier);
  if (identifier)
  {
+    if (_dof >= this->GetJointDegreesOfFreedom(_id))


This block is repeated many times. Could we perhaps move it to its own function similar to Mujoco's ValidateDofParam()? Same for DART.

Removed in 40ff6fa

Addressed for DART in 3a00189

caguero · 2026-05-15T21:01:29Z

+            // data[1] * (hinge_pos - hinge_ref)
+            // where data[1] = pitch (meters/rad) = ScrewThreadPitch/2pi
+            eq->data[0] = 0.0;
+            eq->data[1] = sdfJoint->ScrewThreadPitch() / (2.0 * GZ_PI);


This expression appears in the same function above. I wonder if you could declare a single const and use it in both places.

Since the function is valid only when the joint is a screw joint, I've instead created a converter function that deduplicates the unit conversion. 8f4e576

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

- Add an eqIndices vector to JointInfo to track equality constraint indices compiled in the mjModel. - Resolve and store associated equality constraint indices during model compilation in resolveJointIndices. - Dynamically resolve active degrees of freedom by subtracting eqIndices.size() from the physical joints compiled on the child body in GetJointDegreesOfFreedom. - Refactor simultaneous position and velocity updates to utilize eqIndices and updateCoupledSecondaryJoint helper to avoid code duplication. - Safely ensure Gazebo API queries are always backed by the actual compiled MuJoCo solver state, preventing out-of-bounds coordinate access. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

- Add convertScrewThreadPitch helper function to convert SDFormat screw thread pitch (m/rev) to MuJoCo coordinate ratio (m/rad). - Document screw thread pitch conversion helper along with physical units. - Deduplicate pitch conversion logic in Slide limits and equality constraint ratio calculations. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

- Replace manual index assignments with robust C++ std::fill zero-initialization. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

caguero

Minor comment, ready to approve after it.

caguero · 2026-05-21T16:10:59Z

+  for (int eqId : jointInfo->eqIndices)
+  {
+    double pitch = m->eq_data[eqId * mjNEQDATA + 1];
+    _mjDataArray[_baseIndex + 1] = _value * pitch;


Just checking: Is it OK to update the same _mjDataArray index at every pass of the loop?

No. Honestly, I missed this in my review of AI generated code. The original idea was, since we potentially have multiple equality constraints for implementing additional joint types (e.g. mimic joints), we'd put the constraints in a vector. But for that, the baseindex would need to be updated for each time in the loop.

After going ahead and implementing mimic joints, it actually seems better to track the equality constraints separately. So in 8c9388a, a couple of variables are added to JointInfo for this purpose. I've also renamed this function to updateScrewJointFollower to be more explicit that this is just for screw joints only.

iche033 · 2026-05-23T01:38:12Z

Add out-of-bounds DOF access checks across the bullet-featherstone, dartsim, and mujoco plugins to safely reject invalid DOF queries without solver crashes.

looks like the DOF checks were reverted in bullet-featherstone and handled differently in test. Curious, what's the reason?

iche033 · 2026-05-23T01:40:35Z

Add out-of-bounds DOF access checks across the bullet-featherstone, dartsim, and mujoco plugins to safely reject invalid DOF queries without solver crashes.

looks like the DOF checks were reverted in bullet-featherstone and handled differently in test. Curious, what's the reason?

ah just saw the reason in #980 (comment)

Replace implicit body ID comparisons and search loops with a high-performance pointer-based constraint tracking scheme that stores direct mjsEquality* pointers and caches the resolved screwEqIndex in O(1) time. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

…nt_types

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

iche033

looks good to me, just minor suggestions

…nt_types

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

azeey added 4 commits May 13, 2026 11:13

fix style errors

590534a

Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

azeey requested a review from scpeters as a code owner May 13, 2026 17:15

azeey added this to Core development May 13, 2026

github-actions Bot added the 🎡 rotary label May 13, 2026

github-project-automation Bot moved this to Inbox in Core development May 13, 2026

caguero reviewed May 15, 2026

View reviewed changes

azeey added 6 commits May 20, 2026 20:34

Revert changes to bullet-featherstone and skip DOF checks for it

40ff6fa

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

Address reviewer feedback

4803351

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

Use ValidateDofParam in dartsim

3a00189

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

refactor: Zero-initialize equality constraint data using std::fill

0bce453

- Replace manual index assignments with robust C++ std::fill zero-initialization. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

azeey requested a review from caguero May 21, 2026 02:24

azeey moved this from Inbox to In review in Core development May 21, 2026

caguero reviewed May 21, 2026

View reviewed changes

iche033 reviewed May 23, 2026

View reviewed changes

Comment thread dartsim/src/JointFeatures.cc Outdated

Comment thread mujoco/src/SDFFeatures_TEST.cc Outdated

azeey linked an issue May 26, 2026 that may be closed by this pull request

MuJoCo physics engine plugin #299

Closed

azeey added 3 commits May 26, 2026 16:51

Merge remote-tracking branch 'origin/main' into azeey/mujoco_more_joi…

451a971

…nt_types

Address more reviewer feedback

901f576

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

azeey requested a review from caguero May 26, 2026 22:00

iche033 approved these changes Jun 2, 2026

View reviewed changes

Comment thread mujoco/src/JointFeatures.cc Outdated

Comment thread test/common_test/joint_features.cc Outdated

azeey added 2 commits June 3, 2026 14:19

Merge remote-tracking branch 'origin/main' into azeey/mujoco_more_joi…

4369df6

…nt_types

Address reviewer feedback

55402fa

Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

azeey enabled auto-merge (squash) June 3, 2026 19:21

azeey merged commit 2ab68b8 into main Jun 3, 2026
13 checks passed

azeey deleted the azeey/mujoco_more_joint_types branch June 3, 2026 19:45

github-project-automation Bot moved this from In review to Done in Core development Jun 3, 2026

Conversation

azeey commented May 13, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🎉 New feature

Summary

Changes Made:

Test it

Checklist

Uh oh!

caguero left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

caguero left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

iche033 commented May 23, 2026

Uh oh!

iche033 commented May 23, 2026

Uh oh!

iche033 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

azeey commented May 13, 2026 •

edited

Loading