Skip to content
Merged
Show file tree
Hide file tree
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 59 additions & 0 deletions tests/data/link_ghost_link_inertial_zero.urdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
<?xml version="1.0"?>
<robot name="link_ghost_link_inertial_zero">
<link name="BaseLink">
<visual>
<geometry>
<box size="1.0 1.0 0.2"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="1.0 1.0 0.2"/>
</geometry>
</collision>
</link>
<link name="boxLink">
<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>

<link name="ghost_link_zero_mass">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<link name="ghost_link_no_inertia">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.0"/>
</inertial>
</link>

<joint name="joint1" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<parent link="BaseLink"/>
<child link="boxLink"/>
</joint>

<joint name="joint_zero_mass" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<parent link="boxLink"/>
<child link="ghost_link_zero_mass"/>
</joint>
<joint name="joint_no_inertia" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<parent link="boxLink"/>
<child link="ghost_link_no_inertia"/>
</joint>

</robot>
185 changes: 114 additions & 71 deletions tests/testGhostLink.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@
# SPDX-License-Identifier: Apache-2.0
import pathlib

from pxr import Gf, Usd, UsdPhysics
import omni.asset_validator
import usdex.core
from pxr import Gf, Tf, Usd, UsdPhysics

import urdf_usd_converter
from tests.util.ConverterTestCase import ConverterTestCase
Expand Down Expand Up @@ -205,24 +207,24 @@ def test_link_skip_ghost_link_chain(self):
self.assertTrue(base_link_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertTrue(base_link_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link has a rigid body.
# This ghost link does not have a rigid body.
ghost_link_prim = base_link_prim.GetChild("ghost_link")
self.assertTrue(ghost_link_prim.IsValid())
self.assertTrue(ghost_link_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link has a rigid body.
# This ghost link does not have a rigid body.
ghost_link_2_prim = ghost_link_prim.GetChild("ghost_link_2")
self.assertTrue(ghost_link_2_prim.IsValid())
self.assertTrue(ghost_link_2_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_2_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_2_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_2_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link has a rigid body.
# This ghost link does not have a rigid body.
ghost_link_3_prim = ghost_link_2_prim.GetChild("ghost_link_3")
self.assertTrue(ghost_link_3_prim.IsValid())
self.assertTrue(ghost_link_3_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_3_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_3_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_3_prim.HasAPI("NewtonArticulationRootAPI"))

Expand All @@ -236,7 +238,7 @@ def test_link_skip_ghost_link_chain(self):
# Check physics joint.
physics_scope_prim = default_prim.GetChild("Physics")
self.assertTrue(physics_scope_prim.IsValid())
self.assertEqual(len(physics_scope_prim.GetChildren()), 5)
self.assertEqual(len(physics_scope_prim.GetChildren()), 2)

root_joint_prim = physics_scope_prim.GetChild("root_joint")
self.assertTrue(root_joint_prim.IsValid())
Expand All @@ -250,37 +252,13 @@ def test_link_skip_ghost_link_chain(self):
self.assertRotationsAlmostEqual(joint.GetLocalRot1Attr().Get(), Gf.Quatf(1, 0, 0, 0))

joint1_prim = physics_scope_prim.GetChild("joint1")
self.assertTrue(joint1_prim.IsValid())
self.assertTrue(joint1_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint1_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_skip_ghost_link_chain/Geometry/BaseLink"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_skip_ghost_link_chain/Geometry/BaseLink/ghost_link"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))
self.assertFalse(joint1_prim.IsValid())

joint2_prim = physics_scope_prim.GetChild("joint2")
self.assertTrue(joint2_prim.IsValid())
self.assertTrue(joint2_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint2_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_skip_ghost_link_chain/Geometry/BaseLink/ghost_link"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_skip_ghost_link_chain/Geometry/BaseLink/ghost_link/ghost_link_2"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))
self.assertFalse(joint2_prim.IsValid())

joint3_prim = physics_scope_prim.GetChild("joint3")
self.assertTrue(joint3_prim.IsValid())
self.assertTrue(joint3_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint3_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_skip_ghost_link_chain/Geometry/BaseLink/ghost_link/ghost_link_2"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_skip_ghost_link_chain/Geometry/BaseLink/ghost_link/ghost_link_2/ghost_link_3"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))
self.assertFalse(joint3_prim.IsValid())

joint_box_prim = physics_scope_prim.GetChild("joint_box")
self.assertTrue(joint_box_prim.IsValid())
Expand Down Expand Up @@ -384,24 +362,24 @@ def test_link_skip_ghost_link_chain_branch(self):
self.assertTrue(base_link_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertTrue(base_link_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link has a rigid body.
# This ghost link does not have a rigid body.
ghost_link_prim = base_link_prim.GetChild("ghost_link")
self.assertTrue(ghost_link_prim.IsValid())
self.assertTrue(ghost_link_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link has a rigid body.
# This ghost link does not have a rigid body.
ghost_link_2_prim = ghost_link_prim.GetChild("ghost_link_2")
self.assertTrue(ghost_link_2_prim.IsValid())
self.assertTrue(ghost_link_2_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_2_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_2_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_2_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link has a rigid body.
# This ghost link does not have a rigid body.
ghost_link_3_prim = ghost_link_2_prim.GetChild("ghost_link_3")
self.assertTrue(ghost_link_3_prim.IsValid())
self.assertTrue(ghost_link_3_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_3_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_3_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_3_prim.HasAPI("NewtonArticulationRootAPI"))

Expand Down Expand Up @@ -431,7 +409,7 @@ def test_link_skip_ghost_link_chain_branch(self):
# "ghost_link_4", and "ghost_link_5" are not created because they are fixed joints and body1 is a ghost link without a rigid body.
physics_scope_prim = default_prim.GetChild("Physics")
self.assertTrue(physics_scope_prim.IsValid())
self.assertEqual(len(physics_scope_prim.GetChildren()), 5)
self.assertEqual(len(physics_scope_prim.GetChildren()), 2)

root_joint_prim = physics_scope_prim.GetChild("root_joint")
self.assertTrue(root_joint_prim.IsValid())
Expand All @@ -444,39 +422,13 @@ def test_link_skip_ghost_link_chain_branch(self):
self.assertRotationsAlmostEqual(joint.GetLocalRot0Attr().Get(), Gf.Quatf(1, 0, 0, 0))

joint1_prim = physics_scope_prim.GetChild("joint1")
self.assertTrue(joint1_prim.IsValid())
self.assertTrue(joint1_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint1_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_skip_ghost_link_chain_branch/Geometry/BaseLink"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_skip_ghost_link_chain_branch/Geometry/BaseLink/ghost_link"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))
self.assertFalse(joint1_prim.IsValid())

joint2_prim = physics_scope_prim.GetChild("joint2")
self.assertTrue(joint2_prim.IsValid())
self.assertTrue(joint2_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint2_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_skip_ghost_link_chain_branch/Geometry/BaseLink/ghost_link"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_skip_ghost_link_chain_branch/Geometry/BaseLink/ghost_link/ghost_link_2"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))
self.assertFalse(joint2_prim.IsValid())

joint3_prim = physics_scope_prim.GetChild("joint3")
self.assertTrue(joint3_prim.IsValid())
self.assertTrue(joint3_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint3_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_skip_ghost_link_chain_branch/Geometry/BaseLink/ghost_link/ghost_link_2"])
self.assertEqual(
joint.GetBody1Rel().GetTargets(), ["/link_skip_ghost_link_chain_branch/Geometry/BaseLink/ghost_link/ghost_link_2/ghost_link_3"]
)
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))
self.assertFalse(joint3_prim.IsValid())

joint_box_prim = physics_scope_prim.GetChild("joint_box")
self.assertTrue(joint_box_prim.IsValid())
Expand Down Expand Up @@ -600,3 +552,94 @@ def test_link_skip_ghost_link_with_mimic(self):
self.assertEqual(len(rel.GetTargets()), 1)
ref_joint = rel.GetTargets()[0]
self.assertEqual(ref_joint, "/link_skip_ghost_link_with_mimic/Physics/jointA")

def test_link_ghost_link_inertial_zero(self):
input_path = "tests/data/link_ghost_link_inertial_zero.urdf"
output_dir = self.tmpDir()

converter = urdf_usd_converter.Converter()
with usdex.test.ScopedDiagnosticChecker(
self,
[
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, "zero-mass:.*"),
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, "no-inertia:.*"),
],
level=usdex.core.DiagnosticsLevel.eWarning,
):
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,
issuePredicates=[
omni.asset_validator.IssuePredicates.ContainsMessage("MassAPI can only be applied to a rigid body or collision prim"),
omni.asset_validator.IssuePredicates.ContainsMessage("If principalAxes or diagonalInertia is authored on rigid body"),
],
Comment thread
ft-lab marked this conversation as resolved.
Outdated
)

default_prim = stage.GetDefaultPrim()
self.assertTrue(default_prim.IsValid())

geometry_scope_prim = default_prim.GetChild("Geometry")
self.assertTrue(geometry_scope_prim.IsValid())

# Check physics rigid bodies.
base_link_prim = geometry_scope_prim.GetChild("BaseLink")
self.assertTrue(base_link_prim.IsValid())
self.assertTrue(base_link_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertTrue(base_link_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertTrue(base_link_prim.HasAPI("NewtonArticulationRootAPI"))

box_link_prim = base_link_prim.GetChild("boxLink")
self.assertTrue(box_link_prim.IsValid())
self.assertTrue(box_link_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(box_link_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(box_link_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link does not have a rigid body.
ghost_link_zero_mass_prim = box_link_prim.GetChild("ghost_link_zero_mass")
self.assertTrue(ghost_link_zero_mass_prim.IsValid())
self.assertFalse(ghost_link_zero_mass_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_zero_mass_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_zero_mass_prim.HasAPI("NewtonArticulationRootAPI"))

# This ghost link does not have a rigid body.
ghost_link_no_inertia_prim = box_link_prim.GetChild("ghost_link_no_inertia")
self.assertTrue(ghost_link_no_inertia_prim.IsValid())
self.assertFalse(ghost_link_no_inertia_prim.HasAPI(UsdPhysics.RigidBodyAPI))
self.assertFalse(ghost_link_no_inertia_prim.HasAPI(UsdPhysics.ArticulationRootAPI))
self.assertFalse(ghost_link_no_inertia_prim.HasAPI("NewtonArticulationRootAPI"))

# Check physics joint.
physics_scope_prim = default_prim.GetChild("Physics")
self.assertTrue(physics_scope_prim.IsValid())

root_joint_prim = physics_scope_prim.GetChild("root_joint")
self.assertTrue(root_joint_prim.IsValid())
self.assertTrue(root_joint_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(root_joint_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_ghost_link_inertial_zero"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_ghost_link_inertial_zero/Geometry/BaseLink"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0), 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))

joint1_prim = physics_scope_prim.GetChild("joint1")
self.assertTrue(joint1_prim.IsValid())
self.assertTrue(joint1_prim.IsA(UsdPhysics.FixedJoint))
joint = UsdPhysics.FixedJoint(joint1_prim)
self.assertEqual(joint.GetBody0Rel().GetTargets(), ["/link_ghost_link_inertial_zero/Geometry/BaseLink"])
self.assertEqual(joint.GetBody1Rel().GetTargets(), ["/link_ghost_link_inertial_zero/Geometry/BaseLink/boxLink"])
self.assertTrue(Gf.IsClose(joint.GetLocalPos0Attr().Get(), Gf.Vec3f(0, 0, 0.1), 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))

joint_zero_mass_prim = physics_scope_prim.GetChild("joint_zero_mass")
self.assertFalse(joint_zero_mass_prim.IsValid())

joint_no_inertia_prim = physics_scope_prim.GetChild("joint_no_inertia")
self.assertFalse(joint_no_inertia_prim.IsValid())
5 changes: 5 additions & 0 deletions urdf_usd_converter/_impl/convert.py
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ def convert(self, input_file: str, output_dir: str) -> Sdf.AssetPath:
material_data_list=[],
mesh_material_references={},
undefined_elements=parser.get_undefined_elements(),
warnings={},
)

# setup the main output layer (which will become an asset interface later)
Expand Down Expand Up @@ -155,6 +156,10 @@ def convert(self, input_file: str, output_dir: str) -> Sdf.AssetPath:
# create the asset interface
usdex.core.addAssetInterface(asset_stage, source=data.content[Tokens.Contents])

# Print warnings.
for warning_type, message in data.warnings.items():
Tf.Warn(f"{warning_type}: {message}")

# optionally flatten the asset
if not self.params.layer_structure:
asset_identifier = export_flattened(asset_stage, output_dir, asset_dir, asset_stem, asset_format, self.params.comment)
Expand Down
1 change: 1 addition & 0 deletions urdf_usd_converter/_impl/data.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,3 +42,4 @@ class ConversionData:
material_data_list: list[MaterialData] # Store all material parameters.
mesh_material_references: dict[pathlib.Path, dict[str, list[str]]] # [mesh_file_path, [mesh_safe_name, material_name_list]]
undefined_elements: list[UndefinedData] # Store all undefined elements.
warnings: list[str, str] # Store all warnings. [warning_type, warning_message]
Loading
Loading