Support nested models in sdformat -> mjcf conversion (#119)

shameekganguly · scpeters · Addisu Z. Taddese · web-flow · commit 6e190a7ee9c7 · 2026-02-17T16:51:18.000-06:00
* Support nested models in sdformat -> mjcf conversion Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai> * Remove leftover comment Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai> * Fix typo Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai> * Handle nested parent link for fixed joint Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai> * Improve local var name Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai> * Simplify scoped name tracking (#121) * Simplify scoped name tracking Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> * Fix style Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> --------- Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> --------- Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai> Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Steve Peters <scpeters@openrobotics.org> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
diff --git a/sdformat_mjcf/README.md b/sdformat_mjcf/README.md
@@ -88,7 +88,6 @@ stated, SDFormat elements that affect the behavior of the simulator (i.e not the
 
 ## Currently unimplemented features
 
-  - Nested models
   - Links with multiple parents and kinematic loops
   - Revolute2 and Universal joints
   - `<scene>` element in SDFormat
diff --git a/sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/converters/link.py b/sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/converters/link.py
@@ -22,7 +22,7 @@
 import sdformat_mjcf.utils.sdf_utils as su
 
 
-def add_link(body, link, parent_name="world", link_pose=None):
+def add_link(body, link, parent_name="world", link_pose=None, link_name=None):
     """
     Converts a link from SDFormat to MJCF and add it to the given
     body/worldbody.
@@ -32,7 +32,8 @@ def add_link(body, link, parent_name="world", link_pose=None):
     :param str parent_name: Name of parent link.
     :param gz.math.Pose3d link_pose: Pose of link. This is optional and
     if set to None, the pose will be resolved from the link.
-    :return: The newly created MJCF body.
+    :param str link_name: The name to be assigned for the link body. This is
+    optional and if set to None, `link.name()` will be used.
     :rtype: mjcf.Element
     """
     # Currently the following SDFormat elements inside <link> are ignored
@@ -55,8 +56,9 @@ def add_link(body, link, parent_name="world", link_pose=None):
         else:
             pose = su.graph_resolver.resolve_pose(sem_pose, parent_name)
 
+    link_name = link_name if link_name else link.name()
     body = body.add("body",
-                    name=su.find_unique_name(body, "body", link.name()),
+                    name=su.find_unique_name(body, "body", link_name),
                     pos=su.vec3d_to_list(pose.pos()),
                     euler=su.quat_to_euler_list(pose.rot()))
 
diff --git a/sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/converters/model.py b/sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/converters/model.py
@@ -36,12 +36,17 @@ def add_model(mjcf_root, model):
     :rtype: mjcf.Element
     """
 
+    # The resulting kinematic hierarchy includes all links and joints from
+    # all child models.
     kin_hierarchy = KinematicHierarchy(model)
     model_pose = graph_resolver.resolve_pose(model.semantic_pose())
 
-    def convert_node(body, node, link_pose=None):
-        child_body = add_link(body, node.link, node.parent_node.link.name(),
-                              link_pose)
+    def convert_node(body, node, link_pose):
+        # A node may be assigned a scoped name if it belongs to a child model
+        # for disambiguation. In this case, use the scoped name for the link
+        # body.
+        child_body = add_link(body, node.link, node.parent_node.scoped_name,
+                              link_pose, node.scoped_name)
 
         add_joint(child_body, node.joint)
         # Geoms added to bodies attached to the worldbody without a
@@ -62,7 +67,7 @@ def convert_node(body, node, link_pose=None):
             should_add_exclusions = is_fixed_joint and is_body_world
 
         for cn in node.child_nodes:
-            grand_child_body = convert_node(child_body, cn)
+            grand_child_body = convert_node(child_body, cn, cn.resolved_pose)
             if should_add_exclusions:
                 body.root.contact.add(
                     "exclude",
@@ -74,8 +79,8 @@ def convert_node(body, node, link_pose=None):
 
     for cn in kin_hierarchy.world_node.child_nodes:
         # Adjust the poses of each of the nodes to account for the model
-        link_pose = graph_resolver.resolve_pose(cn.link.semantic_pose())
-        new_link_pose = model_pose * link_pose
+        new_link_pose = model_pose * cn.resolved_pose
+
         convert_node(mjcf_root.worldbody, cn, new_link_pose)
 
     return mjcf_root
diff --git a/sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/sdf_kinematics.py b/sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/sdf_kinematics.py
@@ -11,7 +11,8 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
+#
+from gz.math import Pose3d
 import sdformat as sdf
 import sdformat_mjcf.utils.sdf_utils as su
 
@@ -48,9 +49,18 @@ def __init__(self, link, parent=None, joint=FreeJoint()):
         self.joint = joint
         self.child_nodes = []
 
+        # Pose computed for all links. The value would be relative to the
+        # parent of the link. If the link has no joints, it is considered to be
+        # connected to the world link with a free joint.
+        self.resolved_pose = Pose3d()
+
+        # Scoped name assigned for child model links
+        self.scoped_name = None
+
     def __repr__(self):
         child_repr = " ".join(str(node) for node in self.child_nodes)
-        return f"{self.link.name()}->({child_repr})"
+        link_name = self.scoped_name if self.scoped_name else self.link.name()
+        return f"{link_name}->({child_repr})"
 
     def add_child(self, node, joint):
         """
@@ -89,14 +99,39 @@ def __init__(self, model):
         self.world_link = sdf.Link()
         self.world_link.set_name("world")
         self.world_node = LinkNode(self.world_link)
-
-        link_to_node_dict = {self.world_link: self.world_node}
+        self.world_node.scoped_name = "world"
+
+        self.link_to_node_dict = {self.world_link: self.world_node}
+
+        # Recursively create child model kin hierarchies and merge them up.
+        # When merging, all children of the child model kin hierarchy world
+        # node are initially listed under the current world node. If there is a
+        # joint with a child link in a child model, it will be re-parented when
+        # joints are processed below.
+        for mi in range(model.model_count()):
+            child_model = model.model_by_index(mi)
+            child_kh = KinematicHierarchy(child_model)
+            child_model_pose = su.graph_resolver.resolve_pose(
+                child_model.semantic_pose())
+            for cn in child_kh.world_node.child_nodes:
+                cn.resolved_pose *= child_model_pose
+                # Removing the child node from child_kh.world_node is
+                # unnecessary and dangerous since it affects the iterable.
+                self.world_node.add_child(cn, cn.joint)
+                cn.scoped_name = child_model.name() + '::' + cn.scoped_name
+
+            # Merge child model link to node map
+            self.link_to_node_dict.update(child_kh.link_to_node_dict)
 
         # Start with every link being a child of world link. Later on, we will
         # process joints to build the hierarchy.
         for li in range(model.link_count()):
             node = LinkNode(model.link_by_index(li), self.world_node)
-            link_to_node_dict[node.link] = node
+            node.scoped_name = node.link.name()
+            node.resolved_pose = su.graph_resolver.resolve_pose(
+                node.link.semantic_pose()
+            )
+            self.link_to_node_dict[node.link] = node
             joint = StaticFixedJoint() if model.static() else FreeJoint()
             self.world_node.add_child(node, joint)
 
@@ -109,10 +144,16 @@ def __init__(self, model):
                 parent = self.world_link
 
             child_link_name = su.graph_resolver.resolve_child_link_name(joint)
-            # TODO (azeey) We assume that the child link is in the current
-            # model, i.e., not nested. Remove this assumption when we support
-            # nesting.
-            child_node = link_to_node_dict[model.link_by_name(child_link_name)]
+            child_node = self.link_to_node_dict[
+                model.link_by_name(child_link_name)
+            ]
+
+            if parent_link_name != "world":
+                parent_node = self.link_to_node_dict[parent]
+                parent_pose_inv = parent_node.resolved_pose.inverse()
+                child_node.resolved_pose = (
+                    parent_pose_inv * child_node.resolved_pose
+                )
 
             self.world_node.remove_child(child_node)
-            link_to_node_dict[parent].add_child(child_node, joint)
+            self.link_to_node_dict[parent].add_child(child_node, joint)
diff --git a/sdformat_mjcf/tests/sdformat_to_mjcf/test_add_model.py b/sdformat_mjcf/tests/sdformat_to_mjcf/test_add_model.py
@@ -194,6 +194,150 @@ def test_static_model(self):
         self.assertFalse(mj_link1.get_children("joint"))
         self.assertFalse(mj_link1.get_children("freejoint"))
 
+    def test_nested_model_fixed_joint(self):
+        test_model_sdf = """
+        <sdf version="1.11">
+            <model name="modelA">
+                <pose>1 0 0 0 0 0</pose>
+                <link name="linkA">
+                    <pose>1 0 0 0 0 0</pose>
+                    <collision name="c1">
+                        <geometry>
+                            <sphere><radius>1</radius></sphere>
+                        </geometry>
+                    </collision>
+                </link>
+                <model name="modelB">
+                    <pose>0 1 0 0 0 0</pose>
+                    <link name="linkA">
+                        <pose>0 1 0 0 0 0</pose>
+                        <collision name="c1">
+                            <geometry>
+                                <sphere><radius>2</radius></sphere>
+                            </geometry>
+                        </collision>
+                    </link>
+                </model>
+                <joint name="modelB_joint" type="fixed">
+                    <parent>linkA</parent>
+                    <child>modelB::linkA</child>
+                </joint>
+            </model>
+        </sdf>
+        """
+        root = sdf.Root()
+        root.load_sdf_string(test_model_sdf)
+        mj_root = add_root(root)
+        self.assertIsNotNone(mj_root)
+        mj_modelA_linkA = mj_root.find("body", "linkA")
+        self.assertIsNotNone(mj_modelA_linkA)
+
+        # Check that modelB::linkA was added as a child of (modelA::)linkA
+        modelA_linkA_children = mj_modelA_linkA.get_children("body")
+        self.assertEqual(1, len(modelA_linkA_children))
+        mj_modelB_linkA = modelA_linkA_children[0]
+        self.assertEqual("modelB::linkA", mj_modelB_linkA.name)
+
+        # Verify that the right pos was set for modelB::linkA
+        sdf_linkA = root.model().link_by_name("linkA")
+        sdf_linkA_pose = sdf_linkA.semantic_pose().resolve(
+            _resolveTo="modelB::linkA")
+        modelB_linkA_expected_pos = su.vec3d_to_list(
+            sdf_linkA_pose.inverse().pos())
+        assert_allclose(modelB_linkA_expected_pos, mj_modelB_linkA.pos)
+
+    def test_nested_model_multi_level(self):
+        test_model_sdf = """
+        <sdf version="1.11">
+            <model name='top_nested'>
+                <model name='sub_nested'>
+                    <model name='simple_model'>
+                        <link name='link'/>
+                    </model>
+                    <model name='extra_model'>
+                        <link name='link'/>
+                    </model>
+                </model>
+            </model>
+        </sdf>
+        """
+        root = sdf.Root()
+        root.load_sdf_string(test_model_sdf)
+        mj_root = add_root(root)
+        self.assertIsNotNone(mj_root)
+        mj_simple_model_link = mj_root.find("body",
+                                            "sub_nested::simple_model::link")
+        self.assertIsNotNone(mj_simple_model_link)
+        self.assertIsNotNone(mj_simple_model_link.get_children("freejoint"))
+
+        mj_extra_model_link = mj_root.find("body",
+                                           "sub_nested::extra_model::link")
+        self.assertIsNotNone(mj_extra_model_link)
+        self.assertIsNotNone(mj_extra_model_link.get_children("freejoint"))
+
+    def test_nested_model_fixed_joint_with_nested_parent_link(self):
+        test_model_sdf = """
+        <sdf version="1.11">
+            <model name="modelA">
+                <pose>1 0 0 0 0 0</pose>
+                <link name="linkA">
+                    <pose>1 0 0 0 0 0</pose>
+                    <collision name="c1">
+                        <geometry>
+                            <sphere><radius>1</radius></sphere>
+                        </geometry>
+                    </collision>
+                </link>
+                <model name="modelB">
+                    <pose>0 1 0 0 0 0</pose>
+                    <link name="linkA">
+                        <pose>0 10 0 0 0 0</pose>
+                        <collision name="c2">
+                            <geometry>
+                                <sphere><radius>2</radius></sphere>
+                            </geometry>
+                        </collision>
+                    </link>
+                </model>
+
+                <joint name="modelB_joint" type="fixed">
+                    <parent>modelB::linkA</parent>
+                    <child>linkA</child>
+                </joint>
+            </model>
+        </sdf>
+        """
+        root = sdf.Root()
+        root.load_sdf_string(test_model_sdf)
+        mj_root = add_root(root)
+        self.assertIsNotNone(mj_root)
+
+        mj_modelB_linkA = mj_root.find("body", "modelB::linkA")
+        self.assertIsNotNone(mj_modelB_linkA)
+
+        # Check that (modelA::)linkA was added as a child of modelB::linkA
+        modelB_linkA_children = mj_modelB_linkA.get_children("body")
+        self.assertEqual(1, len(modelB_linkA_children))
+        mj_modelA_linkA = modelB_linkA_children[0]
+        self.assertEqual("linkA", mj_modelA_linkA.name)
+
+        sdf_modelA = root.model()
+
+        # Verify that the right pos was set for top level node modelB::linkA
+        sdf_modelB_linkA_pose = sdf_modelA.semantic_pose().resolve(
+            _resolveTo="modelA::modelB::linkA").inverse()
+        modelB_linkA_expected_pos = su.vec3d_to_list(
+            (sdf_modelA.raw_pose() * sdf_modelB_linkA_pose).pos()
+        )
+        assert_allclose(modelB_linkA_expected_pos, mj_modelB_linkA.pos)
+
+        # Verify that the right pos was set for (modelA::)linkA
+        sdf_linkA = sdf_modelA.link_by_name("linkA")
+        sdf_linkA_pose = sdf_linkA.semantic_pose().resolve(
+            _resolveTo="modelB::linkA")
+        modelA_linkA_expected_pos = su.vec3d_to_list(sdf_linkA_pose.pos())
+        assert_allclose(modelA_linkA_expected_pos, mj_modelA_linkA.pos)
+
 
 if __name__ == "__main__":
     unittest.main()
