Merge pull request #99 from ft-lab/ghost_link

Fixed a bug where all the body0 components of joints would become root prims when a Ghost Link was present

Author: andrewkaufman

tests/data/link_first_ghost_link_fixed.urdf

@@ -15,10 +15,27 @@
       </geometry>
     </collision>
   </link>
+  <link name="link_box">
+    <visual>
+      <geometry>
+        <box size="0.5 0.5 0.5"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.5 0.5 0.5"/>
+      </geometry>
+    </collision>
+  </link>
 
   <joint name="joint1" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 1"/>
     <parent link="BaseLink"/>
     <child link="link1"/>
   </joint>
+  <joint name="joint_box" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.8"/>
+    <parent link="link1"/>
+    <child link="link_box"/>
+  </joint>
 </robot>

tests/data/link_first_ghost_link_floating.urdf

@@ -15,10 +15,28 @@
       </geometry>
     </collision>
   </link>
+  <link name="link_box">
+    <visual>
+      <geometry>
+        <box size="0.5 0.5 0.5"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.5 0.5 0.5"/>
+      </geometry>
+    </collision>
+  </link>
 
   <joint name="joint1" type="floating">
     <origin rpy="0 0 0" xyz="0 0 1"/>
     <parent link="BaseLink"/>
     <child link="link1"/>
   </joint>
+  <joint name="joint_box" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.8"/>
+    <parent link="link1"/>
+    <child link="link_box"/>
+  </joint>
+
 </robot>

tests/testGhostLink.py

@@ -40,9 +40,16 @@ def test_link_first_ghost_link_fixed(self):
         self.assertTrue(link1_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
         self.assertTrue(link1_prim.HasAPI("NewtonArticulationRootAPI"))
 
+        link_box_prim = link1_prim.GetChild("link_box")
+        self.assertTrue(link_box_prim.IsValid())
+        self.assertTrue(link_box_prim.HasAPI(UsdPhysics.RigidBodyAPI))
+        self.assertFalse(link_box_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
+        self.assertFalse(link_box_prim.HasAPI("NewtonArticulationRootAPI"))
+
         # Check physics joint.
         physics_scope_prim = default_prim.GetChild("Physics")
         self.assertTrue(physics_scope_prim.IsValid())
+        self.assertEqual(len(physics_scope_prim.GetChildren()), 2)
 
         joint1_prim = physics_scope_prim.GetChild("joint1")
         self.assertTrue(joint1_prim.IsValid())
@@ -55,6 +62,17 @@ def test_link_first_ghost_link_fixed(self):
         self.assertRotationsAlmostEqual(joint.GetLocalRot0Attr().Get(), Gf.Quatf(1, 0, 0, 0))
         self.assertRotationsAlmostEqual(joint.GetLocalRot1Attr().Get(), Gf.Quatf(1, 0, 0, 0))
 
+        joint_box_prim = physics_scope_prim.GetChild("joint_box")
+        self.assertTrue(joint_box_prim.IsValid())
+        self.assertTrue(joint_box_prim.IsA(UsdPhysics.FixedJoint))
+        joint = UsdPhysics.FixedJoint(joint_box_prim)
+        self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_first_ghost_link_fixed/Geometry/BaseLink/link1"])
+        self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_first_ghost_link_fixed/Geometry/BaseLink/link1/link_box"])
+        self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.8), 1e-6))
+        self.assertTrue(Gf.IsClose(joint.GetLocalPos1Attr().Get(), Gf.Vec3f(0, 0, 0), 1e-6))
+        self.assertRotationsAlmostEqual(joint.GetLocalRot0Attr().Get(), Gf.Quatf(1, 0, 0, 0))
+        self.assertRotationsAlmostEqual(joint.GetLocalRot1Attr().Get(), Gf.Quatf(1, 0, 0, 0))
+
     def test_link_first_ghost_link_floating(self):
         input_path = "tests/data/link_first_ghost_link_floating.urdf"
         output_dir = self.tmpDir()
@@ -86,7 +104,25 @@ def test_link_first_ghost_link_floating(self):
         self.assertTrue(link1_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
         self.assertTrue(link1_prim.HasAPI("NewtonArticulationRootAPI"))
 
-        # The physics joint does not exist.
+        link_box_prim = link1_prim.GetChild("link_box")
+        self.assertTrue(link_box_prim.IsValid())
+        self.assertTrue(link_box_prim.HasAPI(UsdPhysics.RigidBodyAPI))
+        self.assertFalse(link_box_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
+        self.assertFalse(link_box_prim.HasAPI("NewtonArticulationRootAPI"))
+
+        # Check physics joint.
+        # There is only one physics joint, which is the joint between the link1 and the link_box.
         physics_scope_prim = default_prim.GetChild("Physics")
         self.assertTrue(physics_scope_prim.IsValid())
-        self.assertEqual(len(physics_scope_prim.GetChildren()), 0)
+        self.assertEqual(len(physics_scope_prim.GetChildren()), 1)
+
+        joint_box_prim = physics_scope_prim.GetChild("joint_box")
+        self.assertTrue(joint_box_prim.IsValid())
+        self.assertTrue(joint_box_prim.IsA(UsdPhysics.FixedJoint))
+        joint = UsdPhysics.FixedJoint(joint_box_prim)
+        self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_first_ghost_link_floating/Geometry/BaseLink/link1"])
+        self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_first_ghost_link_floating/Geometry/BaseLink/link1/link_box"])
+        self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.8), 1e-6))
+        self.assertTrue(Gf.IsClose(joint.GetLocalPos1Attr().Get(), Gf.Vec3f(0, 0, 0), 1e-6))
+        self.assertRotationsAlmostEqual(joint.GetLocalRot0Attr().Get(), Gf.Quatf(1, 0, 0, 0))
+        self.assertRotationsAlmostEqual(joint.GetLocalRot1Attr().Get(), Gf.Quatf(1, 0, 0, 0))

urdf_usd_converter/_impl/link.py

@@ -291,7 +291,8 @@ def physics_joints(parent: Usd.Prim, link: ElementLink, data: ConversionData):
         body0_link_name = joint.parent.get_with_default("link")
         body1_link_name = joint.child.get_with_default("link")
 
-        body0 = data.references[Tokens.Physics][body0_link_name] if not is_ghost_link else default_prim
+        has_ghost_link = is_ghost_link and root_link.name == body0_link_name
+        body0 = data.references[Tokens.Physics][body0_link_name] if not has_ghost_link else default_prim
         body1 = data.references[Tokens.Physics][body1_link_name]
 
         # Specifies that the origin position of Body1 (the "child" of the joint in the URDF) is the center.