Add rotation in nested model test cases

Signed-off-by: Shameek Ganguly <shameek@intrinsic.ai>

Author: shameekganguly

sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/converters/model.py

@@ -42,9 +42,6 @@ def add_model(mjcf_root, model):
     model_pose = graph_resolver.resolve_pose(model.semantic_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)
 

sdformat_mjcf/src/sdformat_mjcf/sdformat_to_mjcf/sdf_kinematics.py

@@ -54,7 +54,7 @@ def __init__(self, link, parent=None, joint=FreeJoint()):
         # connected to the world link with a free joint.
         self.resolved_pose = Pose3d()
 
-        # Scoped name assigned for child model links
+        # The scoped name of the link relative to the kinematic hierarchy root.
         self.scoped_name = None
 
     def __repr__(self):
@@ -114,7 +114,7 @@ def __init__(self, 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
+                cn.resolved_pose = child_model_pose * cn.resolved_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)

sdformat_mjcf/tests/sdformat_to_mjcf/test_add_model.py

@@ -208,9 +208,9 @@ def test_nested_model_fixed_joint(self):
                     </collision>
                 </link>
                 <model name="modelB">
-                    <pose>0 1 0 0 0 0</pose>
+                    <pose>0 1 0 0 0 1.5707</pose>
                     <link name="linkA">
-                        <pose>0 1 0 0 0 0</pose>
+                        <pose>0 1 0 1.5707 0 0</pose>
                         <collision name="c1">
                             <geometry>
                                 <sphere><radius>2</radius></sphere>
@@ -244,7 +244,7 @@ def test_nested_model_fixed_joint(self):
             _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)
+        assert_allclose(modelB_linkA_expected_pos, mj_modelB_linkA.pos, atol=1e-10)
 
     def test_nested_model_multi_level(self):
         test_model_sdf = """
@@ -289,9 +289,9 @@ def test_nested_model_fixed_joint_with_nested_parent_link(self):
                     </collision>
                 </link>
                 <model name="modelB">
-                    <pose>0 1 0 0 0 0</pose>
+                    <pose>0 1 0 0 0 1.5707</pose>
                     <link name="linkA">
-                        <pose>0 10 0 0 0 0</pose>
+                        <pose>0 10 0 1.5707 0 0</pose>
                         <collision name="c2">
                             <geometry>
                                 <sphere><radius>2</radius></sphere>
@@ -329,14 +329,14 @@ def test_nested_model_fixed_joint_with_nested_parent_link(self):
         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)
+        assert_allclose(modelB_linkA_expected_pos, mj_modelB_linkA.pos, atol=1e-10)
 
         # 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)
+        assert_allclose(modelA_linkA_expected_pos, mj_modelA_linkA.pos, atol=1e-10)
 
 
 if __name__ == "__main__":