Added option to convert fixed joints in sites

[davidegorbani]

This PR adds an option in the configuration file to enable the conversion of fixed joints in the urdf to sites in the mjcf.

This PR goes in the direction of what @Nicogene asked #18

[GiulioRomualdi]

cc @traversaro

[Copilot]

Pull request overview

This PR adds a fixed_joints option to URDFtoMuJoCoLoaderCfg that enables converting fixed joints from a URDF into sites in the generated MJCF. Fixed joint metadata (parent/child links, origin) is extracted from the original URDF before simplification, and sites are then added to the MJCF model.

Changes:

• Added fixed_joints field to URDFtoMuJoCoLoaderCfg dataclass to specify which fixed joints should become sites.
• Added get_fixed_joint_sites static method to extract fixed-joint metadata from the original URDF.
• Added add_sites_for_fixed_joints method to create MJCF site elements on the appropriate body (child if available, otherwise parent).

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[traversaro]

I think passing explicitly the list of fixed joints to convert in sites is quite cumbersome in real life. In iDynTree, historically for loading the URDFs and distinguish between "real links" and "additional frames" we used a simple set of rules, encoded in https://github.com/gbionics/idyntree/blob/2388e5ea0d87a29fd395a560532fcdff0e68263b/src/model/src/ModelTransformers.cpp#L110-L139 . A link is considered a "fake link" and hence converted in a "additional frame" in iDynTree (and I guess the direct equivalent is a "site" in mujoco/mjcf) if and only if these three conditions are true:

• C1: The URDF link has zero mass
• C2: The URDF link has only a parent and no child
• C3: The URDF link is attached to its parent with a fixed joint

If all three conditions are met, the URDF fake link is automatically converted to an additional frames, no questions (or additional configurations, that creates frictions and does not scale) asked. On the other hand, this ensures that useful fixed joints (such as the one representing FT sensors) are not removed, as conditions C1 and C2 are not satisfied. Could it make sense to do something like this also here? @davidegorbani @Nicogene would this solve your problems?

[traversaro]

By the way, I think something similar to this was done in the URDF --> SDF converter, even if there the condition was a bit different (any link with mass<1e-6 was considered as fake): gazebosim/sdformat#1238 . I wonder if we need something similar in our URDF --> USD converters, fyi @GiulioRomualdi .

[traversaro]

I think passing explicitly the list of fixed joints to convert in sites is quite cumbersome in real life. In iDynTree, historically for loading the URDFs and distinguish between "real links" and "additional frames" we used a simple set of rules, encoded in https://github.com/gbionics/idyntree/blob/2388e5ea0d87a29fd395a560532fcdff0e68263b/src/model/src/ModelTransformers.cpp#L110-L139 . A link is considered a "fake link" and hence converted in a "additional frame" in iDynTree (and I guess the direct equivalent is a "site" in mujoco/mjcf) if and only if these three conditions are true:

* C1: The URDF link has zero mass

* C2: The URDF link has only a parent and no child

* C3: The URDF link is attached to its parent with a fixed joint

If all three conditions are met, the URDF fake link is automatically converted to an additional frames, no questions (or additional configurations, that creates frictions and does not scale) asked. On the other hand, this ensures that useful fixed joints (such as the one representing FT sensors) are not removed, as conditions C1 and C2 are not satisfied. Could it make sense to do something like this also here? @davidegorbani @Nicogene would this solve your problems?

Actually I think I misunderstood the logic here. The URDF --> MJCF conversion is happening in MuJoCo if I am not wrong, and there the fixed joints are always lumped (i.e. there is no equivalent of Gazebo's disableFixedJointLumping), so the list here just gave us the list of fixed joints that are stored as sites (correct me if I am wrong). In that case, can't we just have an option to convert all the lumped fixed joints as sites? What is the downside of having that?

[davidegorbani]

In that case, can't we just have an option to convert all the lumped fixed joints as sites? What is the downside of having that?

Makes sense to me, it shouldn't be harmful to have all the fixed joints in the xml.

[davidegorbani]

As @vpunithreddy pointed out, a potential bug of the logic that I implemented is that it would fail to find a fixed link that is a child of another fixed link, as mujoco lumps all the fixed links. After a f2f discussion with @traversaro, we decided to implement a logic that creates a site for each missing link in the mjcf by retrieving the transform between the first surviving ancestor link and the missing link.

[Copilot]

Pull request overview

This PR adds support for converting fixed joints from URDF into MuJoCo sites. When MuJoCo compiles a URDF, it lumps fixed joints and removes intermediate bodies. This feature preserves the fixed joint frames as MJCF <site> elements, which is useful for defining sensor attachment points (e.g., force-torque sensors).

Changes:

• Added all_missing_joints_as_sites config option and logic to detect URDF joints missing from the compiled MJCF, compute their composed transforms through ancestor chains, and insert them as sites
• Implemented hand-rolled 3D math utilities (RPY→rotation matrix, rotation→quaternion, matrix/vector operations) as static methods on the loader class
• Added pretty option to get_mjcf_string and unit tests for the new site-generation feature

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 3 comments.

File	Description
mujoco_urdf_loader/loader.py	Adds config flag, get_missing_joint_sites and add_sites_for_missing_joints methods, math utilities for transform composition, and pretty formatting option
tests/test_missing_joint_sites.py	Tests for detecting missing joints and placing sites with correct parent body and composed position

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

You can also share your feedback on Copilot code review. Take the survey.

[traversaro]

I am a bit afraid we are almost not testing at all the kinematics of the fake link conversions. I would check that for a non trivial model (like an ergocub) for a few random joint configutation the forward kinematics of the fake links in the urdf (computed via idyntree or similar, as it is already a dep) and the one of the sites in mjcf (computed via mujoco) are the same. I would also check that the total mass in input is the same of the total mass in output, to avoid bugs such as the one discussed yesterday.

[traversaro]

I am a bit afraid we are almost not testing at all the kinematics of the fake link conversions. I would check that for a non trivial model (like an ergocub) for a few random joint configutation the forward kinematics of the fake links in the urdf (computed via idyntree or similar, as it is already a dep) and the one of the sites in mjcf (computed via mujoco) are the same. I would also check that the total mass in input is the same of the total mass in output, to avoid bugs such as the one discussed yesterday.

Actually the total mass test is there, I would just modify it to enable the site export feature to test it as well.

[davidegorbani]

I am a bit afraid we are almost not testing at all the kinematics of the fake link conversions. I would check that for a non trivial model (like an ergocub) for a few random joint configutation the forward kinematics of the fake links in the urdf (computed via idyntree or similar, as it is already a dep) and the one of the sites in mjcf (computed via mujoco) are the same. I would also check that the total mass in input is the same of the total mass in output, to avoid bugs such as the one discussed yesterday.

I added a test that computes the world transform for all the bodies and sites in iDynTree and Mujoco with different random joint position (commit 3192f4d). I also added the test for the total mass with commit 2a6b9dd.

[GiulioRomualdi]

Discussing with @traversaro we decided to keep this PR open but at the same time we created a new branch called https://github.com/gbionics/mujoco-urdf-loader/tree/devel-team-hermes where we will do integration test

[Nicogene]

I think passing explicitly the list of fixed joints to convert in sites is quite cumbersome in real life. In iDynTree, historically for loading the URDFs and distinguish between "real links" and "additional frames" we used a simple set of rules, encoded in gbionics/idyntree@2388e5e/src/model/src/ModelTransformers.cpp#L110-L139 . A link is considered a "fake link" and hence converted in a "additional frame" in iDynTree (and I guess the direct equivalent is a "site" in mujoco/mjcf) if and only if these three conditions are true:

* C1: The URDF link has zero mass

* C2: The URDF link has only a parent and no child

* C3: The URDF link is attached to its parent with a fixed joint

If all three conditions are met, the URDF fake link is automatically converted to an additional frames, no questions (or additional configurations, that creates frictions and does not scale) asked. On the other hand, this ensures that useful fixed joints (such as the one representing FT sensors) are not removed, as conditions C1 and C2 are not satisfied. Could it make sense to do something like this also here? @davidegorbani @Nicogene would this solve your problems?

Yes I would prefer that the behaviour is similar to the one in ModelTransformers

[traversaro]

Yes I would prefer that the behaviour is similar to the one in ModelTransformers

Yes, the tricky part is that the fixed joint lumping part is done by mujoco itself, so probably we can't control that part. Additionally, I do not think that mjcf support fixed joint at all, making it difficult to simulate stuff as FT sensors, you basically need to encode the "half of link" with additional metadata.