mujoco: support joint mimic constraints by azeey · Pull Request #990 · gazebosim/gz-physics

azeey · 2026-05-27T04:57:58Z

🎉 New feature

Part of #299

Summary

Implement joint mimic features in the MuJoCo physics engine plugin, supporting coupling for standard 1-DOF and multi-DOF joints. Because MuJoCo solves joint equality constraints as soft force constraints, dynamic keyframe state-teleportation (e.g. SetJointPosition) without coordinate alignment generates severe physical spring-force solver spikes. We resolve this by performing automated C++ coordinate state propagation to pre-align downstream follower states in the active state arrays at the keyframing boundary.

Test it

ctest -R mimic

Screen capture

Screen.Recording.2026-05-27.at.16.54.47.mp4

A dynamic spring-suspended scissor lift. It showcases multi-stage joint mimic constraints using an open-tree topology, keeping the cargo platform perfectly flat and stable as a heavy rolling payload shifts the suspension's gravity center under real-time spring-damper physics!

File: https://gist.github.com/azeey/acf33ed620084a4e82e032edc0bd4bce#file-scissor_lift_demo-sdf

Screen.Recording.2026-05-27.at.16.36.10.mp4

A concentric, stacked planetary Orrery. Driven by a single master PID force controller, it showcases 9 high-precision mimic joint constraints—simulating accurate Keplerian orbital speed ratios and the perfect synchronous tidal locking of the Moon as they spin in harmony.

File: https://gist.github.com/azeey/acf33ed620084a4e82e032edc0bd4bce#file-orrery_demo-sdf

Checklist

Generated-by: Gemini 3.0 Pro

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>

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>

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>

Implement joint mimic features in the MuJoCo physics engine plugin, supporting coupling for standard 1-DOF and multi-DOF joints. Because MuJoCo solves joint equality constraints as soft force constraints, dynamic keyframe state-teleportation (e.g. SetJointPosition) without coordinate alignment generates severe physical spring-force solver spikes. We resolve this by performing automated C++ coordinate state propagation to pre-align downstream follower states in the active state arrays at the keyframing boundary. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

Sdf Joint degree-of-freedom queries are fired by Gazebo Sim's model parser before the active world spec has undergone its first dynamic MuJoCo compilation pass. Querying standard compiled tables during this phase results in false-positive safety violations and startup failures. We resolve this by implementing a static spec DOM tree fallback path inside GetJointDegreesOfFreedom when query indices are uncompiled (out-of-bounds). We also add a virtual destructor to the Base class to make it polymorphic for safe casting, and add the GetJointDegreesOfFreedomPreCompiled unit test to assert this fallback path using a dummy model-swap trick. Generated-by: Gemini 3.0 Pro Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

iche033 · 2026-05-29T00:04:20Z

cool demos! we need to add those to gz-sim example worlds once this gets in

iche033 · 2026-05-28T02:00:34Z

+    if(this->PhysicsEngineName(name) != "bullet-featherstone" &&
+       this->PhysicsEngineName(name) != "mujoco")
    {
      GTEST_SKIP();


based on #930 (comment), I think we need to be using continue instead of GTEST_SKIP?

I think this is actually okay. The different tests are ran as different executables. The for loop is iterating through potential plugins found in each shared library. In theory, a shared library can have multiple plugins, but none of our physics engines do that.

iche033 · 2026-05-28T02:01:23Z

+  for (const std::string &name : this->pluginNames)
+  {
+    CHECK_SUPPORTED_ENGINE(name, "mujoco")
+    CHECK_SUPPORTED_ENGINE(name, "bullet-featherstone")


Just realized these macros call GTEST_SKIP which may not be what we want?

see #990 (comment)

iche033 · 2026-06-02T20:56:05Z

+        if (c.followerJointInfo.lock() == _followerJoint &&
+            c.followerDof == _followerDof)


minor optimization to short circuit the check if DOF does not match so avoid doing the lock()

Suggested change

if (c.followerJointInfo.lock() == _followerJoint &&

c.followerDof == _followerDof)

if (c.followerDof == _followerDoft &&

c.followerJointInfo.lock() == _followerJoin)

Thanks 4b117e3

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

Implement joint mimic features in the MuJoCo physics engine plugin, supporting coupling for standard 1-DOF and multi-DOF joints. Because MuJoCo solves joint equality constraints as soft force constraints, dynamic keyframe state-teleportation (e.g. SetJointPosition) without coordinate alignment generates severe physical spring-force solver spikes. We resolve this by performing automated C++ coordinate state propagation to pre-align downstream follower states in the active state arrays at the keyframing boundary. Generated-by: Gemini 3.0 Pro --------- Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai>

azeey added 15 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>

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>

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 scpeters as a code owner May 27, 2026 04:57

azeey added this to Core development May 27, 2026

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

azeey requested a review from iche033 May 27, 2026 04:58

iche033 reviewed Jun 2, 2026

View reviewed changes

Address reviewer feedback

4b117e3

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

Base automatically changed from azeey/mujoco_more_joint_types to main June 3, 2026 19:45

Merge branch 'main' into azeey/mujoco_mimic_constraint

263553f

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

iche033 approved these changes Jun 4, 2026

View reviewed changes

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

azeey merged commit 8aac3a7 into main Jun 4, 2026
13 checks passed

azeey deleted the azeey/mujoco_mimic_constraint branch June 4, 2026 01:27

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

mujoco: support joint mimic constraints#990

mujoco: support joint mimic constraints#990
azeey merged 18 commits into
mainfrom
azeey/mujoco_mimic_constraint

azeey commented May 27, 2026 •

edited

Loading

Uh oh!

iche033 commented May 29, 2026

Uh oh!

Uh oh!

iche033 May 28, 2026

Uh oh!

azeey Jun 3, 2026

Uh oh!

iche033 May 28, 2026

Uh oh!

azeey Jun 3, 2026

Uh oh!

Uh oh!

Uh oh!

iche033 Jun 2, 2026

Uh oh!

azeey Jun 3, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

		if (c.followerJointInfo.lock() == _followerJoint &&
		c.followerDof == _followerDof)

Uh oh!

Conversation

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

Uh oh!

🎉 New feature

Summary

Test it

Screen capture

Checklist

Uh oh!

iche033 commented May 29, 2026

Uh oh!

Uh oh!

iche033 May 28, 2026

Choose a reason for hiding this comment

Uh oh!

azeey Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

iche033 May 28, 2026

Choose a reason for hiding this comment

Uh oh!

azeey Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

iche033 Jun 2, 2026

Choose a reason for hiding this comment

Uh oh!

azeey Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

azeey commented May 27, 2026 •

edited

Loading