Added mimic support (#84)

* Added mimic support

* Updated newton-usd-schemas 0.1.0rc2

Author: ft-lab

pyproject.toml

@@ -8,7 +8,7 @@ readme = "README.md"
 requires-python = ">=3.10,<3.13"
 dependencies = [
     "usd-exchange>=2.2.0",
-    "newton-usd-schemas>=0.1.0rc1",
+    "newton-usd-schemas>=0.1.0rc2",
     "numpy-stl>=3.2",
     "tinyobjloader>=2.0.0rc13",
     "pycollada>=0.9.2",

tests/testJoints.py

@@ -563,3 +563,40 @@ def test_fixed_floating_joints(self):
         arm_2_prim = stage.GetPrimAtPath(arm2_prim_path)
         self.assertTrue(arm_2_prim.IsValid())
         self.assertTrue(arm_2_prim.HasAPI(UsdPhysics.RigidBodyAPI))
+
+    def test_mimic_joints(self):
+        input_path = "tests/data/mimic_joint.urdf"
+        output_dir = self.tmpDir()
+
+        converter = urdf_usd_converter.Converter()
+        asset_path = converter.convert(input_path, output_dir)
+
+        self.assertIsNotNone(asset_path)
+        self.assertTrue(pathlib.Path(asset_path.path).exists())
+
+        stage: Usd.Stage = Usd.Stage.Open(asset_path.path)
+        self.assertIsValidUsd(stage)
+
+        default_prim = stage.GetDefaultPrim()
+        physics_scope_prim = stage.GetPrimAtPath(default_prim.GetPath().AppendChild("Physics"))
+        self.assertTrue(physics_scope_prim.IsValid())
+
+        joint_b_prim = physics_scope_prim.GetChild("jointB")
+        self.assertTrue(joint_b_prim.IsValid())
+        self.assertTrue(joint_b_prim.IsA(UsdPhysics.RevoluteJoint))
+
+        self.assertTrue(joint_b_prim.HasAPI("NewtonMimicAPI"))
+
+        # 'newton:mimicEnabled', 'newton:mimicCoef0' return False because they have default values.
+        self.assertFalse(joint_b_prim.GetAttribute("newton:mimicEnabled").HasAuthoredValue())
+        self.assertFalse(joint_b_prim.GetAttribute("newton:mimicCoef0").HasAuthoredValue())
+
+        self.assertTrue(joint_b_prim.GetAttribute("newton:mimicCoef1").HasAuthoredValue())
+        self.assertTrue(joint_b_prim.GetAttribute("newton:mimicEnabled").Get())
+        self.assertAlmostEqual(joint_b_prim.GetAttribute("newton:mimicCoef1").Get(), 2.0)
+
+        self.assertTrue(joint_b_prim.HasRelationship("newton:mimicJoint"))
+        rel = joint_b_prim.GetRelationship("newton:mimicJoint")
+        self.assertEqual(len(rel.GetTargets()), 1)
+        ref_joint = rel.GetTargets()[0]
+        self.assertEqual(ref_joint, "/mimic_joint/Physics/jointA")

tests/testUndefined.py

@@ -347,42 +347,3 @@ def test_check_custom_name(self):
         self.assertTrue(item_prim.GetAttribute("urdf:value").IsCustom())
         value = item_prim.GetAttribute("urdf:value").Get()
         self.assertEqual(value, "foo1")
-
-    def test_check_mimic_joint(self):
-        """
-        Test case where the joint has a mimic element.
-        Currently unsupported, it stores custom elements and custom attributes of mimic joints.
-        """
-        input_path = "tests/data/mimic_joint.urdf"
-        output_dir = self.tmpDir()
-
-        converter = urdf_usd_converter.Converter()
-        asset_path = converter.convert(input_path, output_dir)
-
-        self.assertIsNotNone(asset_path)
-        self.assertTrue(pathlib.Path(asset_path.path).exists())
-
-        stage: Usd.Stage = Usd.Stage.Open(asset_path.path)
-        self.assertIsValidUsd(stage)
-
-        default_prim = stage.GetDefaultPrim()
-        physics_scope_prim = stage.GetPrimAtPath(default_prim.GetPath().AppendChild("Physics"))
-        self.assertTrue(physics_scope_prim.IsValid())
-
-        joint_b_prim = physics_scope_prim.GetChild("jointB")
-        self.assertTrue(joint_b_prim.IsValid())
-        self.assertTrue(joint_b_prim.IsA(UsdPhysics.RevoluteJoint))
-
-        # Unsupported parameters: Custom attributes in "mimic".
-        self.assertTrue(joint_b_prim.GetAttribute("urdf:mimic:joint").HasAuthoredValue())
-        self.assertTrue(joint_b_prim.GetAttribute("urdf:mimic:joint").IsCustom())
-        joint = joint_b_prim.GetAttribute("urdf:mimic:joint").Get()
-        self.assertEqual(joint, "jointA")
-        self.assertTrue(joint_b_prim.GetAttribute("urdf:mimic:multiplier").HasAuthoredValue())
-        self.assertTrue(joint_b_prim.GetAttribute("urdf:mimic:multiplier").IsCustom())
-        multiplier = joint_b_prim.GetAttribute("urdf:mimic:multiplier").Get()
-        self.assertAlmostEqual(multiplier, 2.0)
-        self.assertTrue(joint_b_prim.GetAttribute("urdf:mimic:offset").HasAuthoredValue())
-        self.assertTrue(joint_b_prim.GetAttribute("urdf:mimic:offset").IsCustom())
-        offset = joint_b_prim.GetAttribute("urdf:mimic:offset").Get()
-        self.assertAlmostEqual(offset, 0.0)

urdf_usd_converter/_impl/link.py

@@ -22,6 +22,7 @@
 from .utils import (
     float3_to_quatf,
     get_geometry_name,
+    set_schema_attribute,
     set_transform,
 )
 
@@ -204,6 +205,11 @@ def physics_joints(parent: Usd.Prim, link: ElementLink, data: ConversionData):
     joint_names = [joint.name for joint in joints] if joints else []
     joint_safe_names = data.name_cache.getPrimNames(parent, joint_names)
 
+    # Usd.Prim dict for joint name.
+    # This is used to reference mimic joints.
+    physics_joints_dict: dict[str, Usd.Prim] = {}
+    mimic_joints_names: list[str] = []
+
     # Create physics joints.
     for joint, joint_safe_name in zip(joints, joint_safe_names):
         body0_link_name = joint.parent.get_with_default("link")
@@ -243,12 +249,30 @@ def physics_joints(parent: Usd.Prim, link: ElementLink, data: ConversionData):
 
         # Unsupported attributes and elements within the joint are stored as custom attributes of the PhysicsJoint.
         if physics_joint:
+            # Stores information used by the mimic joint.
+            physics_joints_dict[joint.name] = physics_joint.GetPrim()
+            if joint.mimic and joint.mimic.joint:
+                mimic_joints_names.append(joint.name)
+
             convert_unsupported_attributes_and_elements(joint, physics_joint.GetPrim(), data)
 
             # Store custom attributes and custom elements for the specified element.
             if joint.undefined_attributes or joint.undefined_elements or joint.undefined_text:
                 convert_undefined_elements(joint, physics_joint.GetPrim(), data)
 
+    # Set mimic joints.
+    for mimic_joint_name in mimic_joints_names:
+        # USD prim associated with mimic_joint_name.
+        physics_joint_prim = physics_joints_dict[mimic_joint_name]
+
+        # Search for the mimic_joint_name among the joints.
+        joint = next((joint for joint in joints if joint.name == mimic_joint_name), None)
+        if joint:
+            # USD prim associated with the joint name referenced by mimic.
+            ref_joint_prim = physics_joints_dict[joint.mimic.joint]
+
+            set_physics_mimic_joint(joint, physics_joint_prim, ref_joint_prim)
+
 
 def convert_unsupported_attributes_and_elements(element_joint: ElementJoint, prim: Usd.Prim, data: ConversionData):
     """
@@ -300,13 +324,26 @@ def convert_unsupported_attributes_and_elements(element_joint: ElementJoint, pri
         if soft_upper_limit is not None:
             prim.CreateAttribute("urdf:safety_controller:soft_upper_limit", Sdf.ValueTypeNames.Float, custom=True).Set(soft_upper_limit)
 
-    if element_joint.mimic:
-        joint = element_joint.mimic.get_with_default("joint")
-        if joint is not None:
-            prim.CreateAttribute("urdf:mimic:joint", Sdf.ValueTypeNames.String, custom=True).Set(joint)
-        multiplier = element_joint.mimic.get_with_default("multiplier")
-        if multiplier is not None:
-            prim.CreateAttribute("urdf:mimic:multiplier", Sdf.ValueTypeNames.Float, custom=True).Set(multiplier)
-        offset = element_joint.mimic.get_with_default("offset")
-        if offset is not None:
-            prim.CreateAttribute("urdf:mimic:offset", Sdf.ValueTypeNames.Float, custom=True).Set(offset)
+
+def set_physics_mimic_joint(element_joint: ElementJoint, prim: Usd.Prim, ref_prim: Usd.Prim):
+    """
+    Set the mimic joint.
+
+    Args:
+        element_joint: ElementJoint in URDF
+        prim: PhysicsJoint in USD
+        ref_prim: PhysicsJoint in USD referenced by mimic
+    """
+    # Both multiplier and offset have default values.
+    multiplier = element_joint.mimic.get_with_default("multiplier")
+    offset = element_joint.mimic.get_with_default("offset")
+
+    # URDF: joint0 = multiplier * joint1 + offset
+    # Newton USD Schemas: joint0 = coef1 * joint1 + coef0
+    prim.ApplyAPI("NewtonMimicAPI")
+    set_schema_attribute(prim, "newton:mimicEnabled", True)
+    set_schema_attribute(prim, "newton:mimicCoef0", offset)
+    set_schema_attribute(prim, "newton:mimicCoef1", multiplier)
+
+    rel = prim.CreateRelationship("newton:mimicJoint")
+    rel.AddTarget(ref_prim.GetPath())

urdf_usd_converter/_impl/utils.py

@@ -2,9 +2,10 @@
 # SPDX-License-Identifier: Apache-2.0
 import math
 import pathlib
+from typing import Any
 
 import usdex.core
-from pxr import Gf, UsdGeom
+from pxr import Gf, Tf, Usd, UsdGeom
 
 from .._version import __version__
 from .urdf_parser.elements import (
@@ -18,6 +19,7 @@
     "float3_to_quatf",
     "get_authoring_metadata",
     "get_geometry_name",
+    "set_schema_attribute",
     "set_transform",
 ]
 
@@ -122,3 +124,13 @@ def multiply_transforms_preserve_scale(transform1: Gf.Transform, transform2: Gf.
     result.SetScale(combined_scale)
 
     return result
+
+
+def set_schema_attribute(prim: Usd.Prim, name: str, value: Any):
+    attr: Usd.Attribute = prim.GetAttribute(name)
+    if not attr.IsValid():
+        Tf.RaiseCodingError(f'Attribute "{name}" is not valid for prim <{prim.GetPath()}> with schemas {prim.GetAppliedSchemas()}')
+    # Only set the value if it is different from the schema default value
+    default = attr.Get()
+    if default is None or value != default:
+        attr.Set(value)