From 442f066da67686bf2ff3ef0e148017e2bf3cb75f Mon Sep 17 00:00:00 2001 From: DJL Date: Tue, 18 Aug 2026 14:39:32 +0800 Subject: [PATCH] Fix MJCF D6 collapse dropping distinct axes and leaving unused D6 axes free When the MJCF importer collapses an over-constrained group of single-axis joints (e.g. a humanoid hip authored as three hinges) into one D6 joint, the axis of each source joint was derived from physics:axis alone. mujoco-usd-converter always exports single-axis joints x-aligned and encodes the real MJCF axis direction in localRot0/localRot1, so physics:axis is X for every joint in such a group. Consequences: - Every joint after the first collided on the rotX token and was dropped, losing its DOF even when its physical axis was distinct (and exactly representable by a D6). - The D6 frame was left unrotated, so the surviving axes rotated about the wrong directions whenever the MJCF axes were not cardinal. - The five unused D6 axes were never locked, so the collapsed joint gained spurious free translational (and sometimes rotational) DOFs in PhysX. Recover the physical axis of each source joint from localRot0/localRot1, build a right-handed orthonormal D6 basis from the group's axes via Gram-Schmidt, and assign each joint to a basis column by dot product. Joints that cannot be represented (duplicate/non-orthogonal axes) are still dropped but with a physically accurate warning. All six D6 axes are now explicitly locked except the ones backed by a source joint, and each used axis records the source MJCF joint name in mjcf::name. Refs isaac-sim/IsaacLab#6854 --- .../impl/mjc_to_physx_conversion_utils.py | 339 +++++++++++++++--- .../standalone_tests/test_mjc_d6_collapse.py | 332 +++++++++++++++++ 2 files changed, 612 insertions(+), 59 deletions(-) create mode 100644 source/extensions/isaacsim.asset.importer.utils/standalone_tests/test_mjc_d6_collapse.py diff --git a/source/extensions/isaacsim.asset.importer.utils/python/impl/mjc_to_physx_conversion_utils.py b/source/extensions/isaacsim.asset.importer.utils/python/impl/mjc_to_physx_conversion_utils.py index 9bf15ee079..24e243eb2d 100644 --- a/source/extensions/isaacsim.asset.importer.utils/python/impl/mjc_to_physx_conversion_utils.py +++ b/source/extensions/isaacsim.asset.importer.utils/python/impl/mjc_to_physx_conversion_utils.py @@ -22,7 +22,7 @@ import os from collections import defaultdict -from pxr import Sdf, Usd, UsdPhysics +from pxr import Gf, Sdf, Usd, UsdPhysics from .physx_types import PhysxAttr, PhysxSchema @@ -223,29 +223,134 @@ def convert_mjc_joint_to_physx(joint: Usd.Prim, stage: Usd.Stage) -> None: drive_api.CreateTargetPositionAttr().Set(joint_target_position) -def _joint_axis_d6_token(joint_prim: Usd.Prim) -> str | None: - """Return the D6 axis token for a single-axis revolute or prismatic joint. +_AXIS_VECTORS = { + "X": Gf.Vec3d(1.0, 0.0, 0.0), + "Y": Gf.Vec3d(0.0, 1.0, 0.0), + "Z": Gf.Vec3d(0.0, 0.0, 1.0), +} + +_D6_ROTATION_AXES = (UsdPhysics.Tokens.rotX, UsdPhysics.Tokens.rotY, UsdPhysics.Tokens.rotZ) +_D6_TRANSLATION_AXES = (UsdPhysics.Tokens.transX, UsdPhysics.Tokens.transY, UsdPhysics.Tokens.transZ) +_D6_ALL_AXES = _D6_TRANSLATION_AXES + _D6_ROTATION_AXES + +# Dot-product tolerance when matching source joint axes against D6 basis columns. +_AXIS_DOT_TOLERANCE = 1e-4 +# Tolerance when checking that all joints of a group share the same joint frame. +_FRAME_POSITION_TOLERANCE = 1e-4 + + +def _normalized(vec: Gf.Vec3d) -> Gf.Vec3d | None: + """Return the unit vector for *vec*, or ``None`` when degenerate.""" + length = vec.GetLength() + if length < 1e-12: + return None + return vec / length + + +def _joint_axis_directions(joint_prim: Usd.Prim) -> tuple[str, Gf.Vec3d, Gf.Vec3d] | None: + """Return the physical axis of a single-axis joint in both body frames. + + mujoco-usd-converter always exports x-aligned joints and encodes the MJCF + axis direction in ``localRot0``/``localRot1``, so the D6 axis cannot be + derived from ``physics:axis`` alone. Args: joint_prim: The USD joint prim. Returns: - D6 axis token (e.g. ``"rotX"``, ``"transY"``) or ``None`` if the - joint has no recognizable ``physics:axis`` value or is not a - single-axis joint type. + ``(kind, axis_body0, axis_body1)`` where ``kind`` is ``"rotation"`` or + ``"translation"`` and the vectors are the normalized physical axis + expressed in body0/body1 local space, or ``None`` when the joint is not + a single-axis joint or has no recognizable ``physics:axis``. """ + if joint_prim.IsA(UsdPhysics.RevoluteJoint): + kind = "rotation" + elif joint_prim.IsA(UsdPhysics.PrismaticJoint): + kind = "translation" + else: + return None axis_attr = joint_prim.GetAttribute("physics:axis") if not axis_attr or not axis_attr.IsValid(): return None - axis_value = axis_attr.Get() - if not axis_value: + axis_value = str(axis_attr.Get()).upper() + if axis_value not in _AXIS_VECTORS: return None - axis_value = str(axis_value).upper() - if joint_prim.IsA(UsdPhysics.RevoluteJoint): - return _REVOLUTE_AXIS_TO_D6_TOKEN.get(axis_value) - if joint_prim.IsA(UsdPhysics.PrismaticJoint): - return _PRISMATIC_AXIS_TO_D6_TOKEN.get(axis_value) - return None + local_axis = _AXIS_VECTORS[axis_value] + + joint = UsdPhysics.Joint(joint_prim) + directions = [] + for getter in (joint.GetLocalRot0Attr, joint.GetLocalRot1Attr): + attr = getter() + if attr and attr.HasAuthoredValue(): + quat = attr.Get() + rotation = Gf.Rotation(Gf.Quatd(quat.GetReal(), Gf.Vec3d(quat.GetImaginary()))) + else: + rotation = Gf.Rotation() + direction = _normalized(rotation.TransformDir(local_axis)) + if direction is None: + return None + directions.append(direction) + return kind, directions[0], directions[1] + + +def _assign_axes_to_d6_basis( + directions: list[Gf.Vec3d], +) -> tuple[list[Gf.Vec3d], list[tuple[int, float] | None], list[int]]: + """Assign source axis directions to the columns of a right-handed D6 basis. + + Args: + directions: Physical joint axes in body1 local space, one per joint. + + Returns: + ``(columns, assignments, dropped)`` where ``columns`` is a right-handed + orthonormal basis of three vectors, ``assignments[i]`` is + ``(column_index, sign)`` for joints whose axis maps onto a basis column + (``sign`` is ``-1.0`` when the joint axis is anti-parallel to the + column, which can happen after handedness correction) or ``None`` when + the joint could not be represented, and ``dropped`` lists the indices + of unrepresentable joints. + """ + columns: list[Gf.Vec3d] = [] + assignments: list[tuple[int, float] | None] = [] + dropped: list[int] = [] + for direction in directions: + assignment = None + orthogonal = True + for column_index, column in enumerate(columns): + dot = direction * column + if abs(dot) > 1.0 - _AXIS_DOT_TOLERANCE: + # Same physical axis (parallel or anti-parallel): a D6 hosts a + # single DOF per axis, so a second joint here is a duplicate. + orthogonal = False + break + if abs(dot) > _AXIS_DOT_TOLERANCE: + orthogonal = False + if orthogonal and len(columns) < 3: + columns.append(direction) + assignment = (len(columns) - 1, 1.0) + assignments.append(assignment) + if assignment is None: + dropped.append(len(assignments) - 1) + + # Complete the basis so it is always right-handed and orthonormal. + if len(columns) == 1: + c0 = columns[0] + fallback = min(_AXIS_VECTORS.values(), key=lambda v: abs(c0 * v)) + c1 = _normalized(Gf.Cross(c0, fallback)) + columns = [c0, c1, Gf.Cross(c0, c1)] + elif len(columns) == 2: + c0, c1 = columns + columns = [c0, c1, Gf.Cross(c0, c1)] + else: + # Three source-owned columns: flip the last one if the source axes + # form a left-handed triple, which a D6 frame cannot represent. The + # owning joint's sign is flipped accordingly (limits are swapped). + if Gf.Cross(columns[0], columns[1]) * columns[2] < 0.0: + columns[2] = -columns[2] + for index, assignment in enumerate(assignments): + if assignment is not None and assignment[0] == 2: + assignments[index] = (2, -assignment[1]) + return columns, assignments, dropped def _group_joints_by_body_pair(stage: Usd.Stage) -> dict[tuple, list[Usd.Prim]]: @@ -318,6 +423,31 @@ def _restore_ancestor_specifiers(layer: Sdf.Layer, snapshot: list[tuple[Sdf.Path spec.typeName = type_name +def _quat_from_columns(columns: list[Gf.Vec3d]) -> Gf.Quatd: + """Build a quaternion for the rotation whose matrix columns are the given basis vectors.""" + c0, c1, c2 = columns + trace = c0[0] + c1[1] + c2[2] + if trace > 0.0: + s = math.sqrt(trace + 1.0) * 2.0 + quat = Gf.Quatd(0.25 * s, Gf.Vec3d((c1[2] - c2[1]) / s, (c2[0] - c0[2]) / s, (c0[1] - c1[0]) / s)) + elif c0[0] > c1[1] and c0[0] > c2[2]: + s = math.sqrt(1.0 + c0[0] - c1[1] - c2[2]) * 2.0 + quat = Gf.Quatd( + (c1[2] - c2[1]) / s, Gf.Vec3d(0.25 * s, (c1[0] + c0[1]) / s, (c2[0] + c0[2]) / s) + ) + elif c1[1] > c2[2]: + s = math.sqrt(1.0 + c1[1] - c0[0] - c2[2]) * 2.0 + quat = Gf.Quatd( + (c2[0] - c0[2]) / s, Gf.Vec3d((c1[0] + c0[1]) / s, 0.25 * s, (c2[1] + c1[2]) / s) + ) + else: + s = math.sqrt(1.0 + c2[2] - c0[0] - c1[1]) * 2.0 + quat = Gf.Quatd( + (c0[1] - c1[0]) / s, Gf.Vec3d((c2[0] + c0[2]) / s, (c2[1] + c1[2]) / s, 0.25 * s) + ) + return Gf.Quatd(quat.GetNormalized()) + + def _convert_overconstrained_group_to_d6( stage: Usd.Stage, joints: list[Usd.Prim], @@ -327,9 +457,14 @@ def _convert_overconstrained_group_to_d6( ) -> bool: """Combine one over-constrained joint group into a D6 joint. - The first joint becomes the D6 host (its path is reused so external - references stay valid); every other joint in the group is either - folded in as another D6 axis or deactivated. + The first representable joint becomes the D6 host (its path is reused so + external references stay valid); every other joint in the group is either + folded in as another D6 axis or deactivated. The D6 joint frame is rebuilt + from the physical source axes (recovered from each joint's ``localRot``), + per-axis limits and drive parameters are mapped onto the corresponding D6 + axes, every unused D6 axis is explicitly locked, and each used axis records + its source MJCF joint name in a ``mjcf::name`` attribute so consumers + can bind D6 axes back to the semantic joints. Args: stage: USD stage being edited. @@ -344,15 +479,17 @@ def _convert_overconstrained_group_to_d6( ``True`` if a D6 was constructed, ``False`` if no joint in the group had a recognizable axis. """ - # Defer picking the primary until axis assignment is done so the host - # never lands on a path that's about to be deactivated. group_paths = [j.GetPath() for j in joints] - axis_assignments: list[tuple[Usd.Prim, str]] = [] + + # Recover each joint's physical axis (in both body frames) from localRot. + kinds: list[str] = [] + directions0: list[Gf.Vec3d] = [] + directions1: list[Gf.Vec3d] = [] + recognized: list[int] = [] dropped_joints: list[Usd.Prim] = [] - used_axes: set[str] = set() - for joint in joints: - token = _joint_axis_d6_token(joint) - if token is None: + for index, joint in enumerate(joints): + result = _joint_axis_directions(joint) + if result is None: _logger.warning( f"Joint {joint.GetPath()} has no recognizable physics:axis " "and cannot be encoded as a D6 axis; its DOF will be lost in " @@ -360,35 +497,58 @@ def _convert_overconstrained_group_to_d6( ) dropped_joints.append(joint) continue - if token in used_axes: - _logger.warning( - f"Joint {joint.GetPath()} duplicates D6 axis '{token}' in over-constrained " - f"group {group_paths} (the MJCF axis direction is encoded in localRot, " - "which the D6 cannot represent uniquely); its DOF will be lost in the " - "PhysX variant" - ) - dropped_joints.append(joint) - continue - used_axes.add(token) - axis_assignments.append((joint, token)) + kind, axis_body0, axis_body1 = result + recognized.append(index) + kinds.append(kind) + directions0.append(axis_body0) + directions1.append(axis_body1) - if not axis_assignments: + if not recognized: return False - # Host the D6 on the first joint that actually contributed an axis. - primary, _ = axis_assignments[0] + columns, axis_assignments, unassignable = _assign_axes_to_d6_basis(directions1) + for index in unassignable: + joint = joints[recognized[index]] + _logger.warning( + f"Joint {joint.GetPath()} shares its physical axis with another joint " + f"or is non-orthogonal to the rest of over-constrained group {group_paths}; " + "a D6 joint cannot represent this DOF, so it will be lost in the PhysX variant" + ) + dropped_joints.append(joint) + + host_index = next(i for i, a in enumerate(axis_assignments) if a is not None) + primary = joints[recognized[host_index]] primary_path = primary.GetPath() _logger.warning( f"Over-constrained joint group with {len(joints)} joints ({group_paths}) " - f"between bodies {body0} and {body1} is being collapsed into single D6 joint at {primary_path}. " - "Only one DOF per axis will be preserved; duplicate or unrecognized axes will be dropped." + f"between bodies {body0} and {body1} is being collapsed into single D6 joint at {primary_path}." ) primary_joint_api = UsdPhysics.Joint(primary) - local_pos0 = primary_joint_api.GetLocalPos0Attr().Get() if primary_joint_api.GetLocalPos0Attr() else None - local_pos1 = primary_joint_api.GetLocalPos1Attr().Get() if primary_joint_api.GetLocalPos1Attr() else None - local_rot0 = primary_joint_api.GetLocalRot0Attr().Get() if primary_joint_api.GetLocalRot0Attr() else None - local_rot1 = primary_joint_api.GetLocalRot1Attr().Get() if primary_joint_api.GetLocalRot1Attr() else None + + # A single D6 has one joint frame: verify the group actually shares it. + pos0_attr = primary_joint_api.GetLocalPos0Attr() + pos1_attr = primary_joint_api.GetLocalPos1Attr() + local_pos0 = pos0_attr.Get() if pos0_attr else None + local_pos1 = pos1_attr.Get() if pos1_attr else None + for index, assignment in enumerate(axis_assignments): + if assignment is None: + continue + joint = UsdPhysics.Joint(joints[recognized[index]]) + for axis_name, attr, reference in ( + ("body0", joint.GetLocalPos0Attr(), local_pos0), + ("body1", joint.GetLocalPos1Attr(), local_pos1), + ): + position = attr.Get() if attr else None + if position is None or reference is None: + continue + if (Gf.Vec3d(position) - Gf.Vec3d(reference)).GetLength() > _FRAME_POSITION_TOLERANCE: + _logger.warning( + f"Joint {joint.GetPath()} has a different {axis_name}-space joint position than " + f"{primary_path}; a single D6 joint cannot represent distinct source frames, " + f"so the frame of {primary_path} is used and the articulation may change." + ) + primary_break_force = primary_joint_api.GetBreakForceAttr() if primary_joint_api.GetBreakForceAttr() else None primary_break_torque = primary_joint_api.GetBreakTorqueAttr() if primary_joint_api.GetBreakTorqueAttr() else None primary_collisions = ( @@ -408,8 +568,20 @@ def _convert_overconstrained_group_to_d6( primary_physx_attrs.append((attr_enum.name, attr_enum.type, src_attr.Get())) # Snapshot per-axis limits/drive params before retyping the primary. - axis_state: list[tuple[str, dict, dict]] = [] - for joint, token in axis_assignments: + # ``sign`` is -1 when the source axis is anti-parallel to the D6 basis + # column: limits swap and drive targets flip accordingly. + axis_state: list[tuple[str, dict, dict, float, Usd.Prim]] = [] + used_axes: set[str] = set() + for index, assignment in enumerate(axis_assignments): + if assignment is None: + continue + column_index, sign = assignment + joint = joints[recognized[index]] + token = ( + _D6_ROTATION_AXES[column_index] if kinds[index] == "rotation" else _D6_TRANSLATION_AXES[column_index] + ) + used_axes.add(token) + limit_state: dict = {} lower_attr = joint.GetAttribute("physics:lowerLimit") upper_attr = joint.GetAttribute("physics:upperLimit") @@ -433,7 +605,15 @@ def _convert_overconstrained_group_to_d6( src_attr = getattr(src_drive, getter_name)() if src_attr and src_attr.IsValid() and src_attr.HasAuthoredValue(): drive_state[key] = src_attr.Get() - axis_state.append((token, limit_state, drive_state)) + + # Limit spring gains authored through PhysxLimitAPI on the source axis. + source_axis = str(joint.GetAttribute("physics:axis").Get()) + for gain in ("stiffness", "damping"): + src_attr = joint.GetAttribute(f"physxLimit:{source_axis}:{gain}") + if src_attr and src_attr.IsValid() and src_attr.HasAuthoredValue(): + limit_state[f"physx_{gain}"] = src_attr.Get() + + axis_state.append((token, limit_state, drive_state, sign, joint)) edit_layer = stage.GetEditTarget().GetLayer() ancestor_snapshot = _snapshot_ancestor_specifiers(edit_layer, primary_path) @@ -447,10 +627,22 @@ def _convert_overconstrained_group_to_d6( d6_joint.CreateLocalPos0Attr().Set(local_pos0) if local_pos1 is not None: d6_joint.CreateLocalPos1Attr().Set(local_pos1) - if local_rot0 is not None: - d6_joint.CreateLocalRot0Attr().Set(local_rot0) - if local_rot1 is not None: - d6_joint.CreateLocalRot1Attr().Set(local_rot1) + + # Rebuild the joint frame from the physical source axes: the D6 frame in + # body1 space has the basis columns as its axes, and the body0 frame is the + # same physical frame expressed through the primary joint's relative pose. + d6_joint.CreateLocalRot1Attr().Set(Gf.Quatf(_quat_from_columns(columns))) + primary_rot0 = primary_joint_api.GetLocalRot0Attr() + primary_rot1 = primary_joint_api.GetLocalRot1Attr() + if primary_rot0 and primary_rot0.HasAuthoredValue() and primary_rot1 and primary_rot1.HasAuthoredValue(): + quat0 = primary_rot0.Get() + quat1 = primary_rot1.Get() + rotation0 = Gf.Rotation(Gf.Quatd(quat0.GetReal(), Gf.Vec3d(quat0.GetImaginary()))) + rotation1 = Gf.Rotation(Gf.Quatd(quat1.GetReal(), Gf.Vec3d(quat1.GetImaginary()))) + relative = rotation0 * rotation1.GetInverse() + columns0 = [_normalized(relative.TransformDir(c)) for c in columns] + d6_joint.CreateLocalRot0Attr().Set(Gf.Quatf(_quat_from_columns(columns0))) + if primary_break_force and primary_break_force.HasAuthoredValue(): d6_joint.CreateBreakForceAttr().Set(primary_break_force.Get()) if primary_break_torque and primary_break_torque.HasAuthoredValue(): @@ -463,14 +655,29 @@ def _convert_overconstrained_group_to_d6( d6_prim = d6_joint.GetPrim() - for token, limit_state, drive_state in axis_state: - limit = UsdPhysics.LimitAPI.Apply(d6_prim, token) - if "low" in limit_state: - limit.CreateLowAttr().Set(limit_state["low"]) - if "high" in limit_state: - limit.CreateHighAttr().Set(limit_state["high"]) + for token, limit_state, drive_state, sign, source_joint in axis_state: + if "low" in limit_state and "high" in limit_state and sign < 0: + limit_state = {**limit_state, "low": -limit_state["high"], "high": -limit_state["low"]} + # Only author a LimitAPI when the source joint actually has limits: + # an applied-but-unauthored LimitAPI still composes to the schema + # fallback, and an unlimited source axis must stay truly free. + if "low" in limit_state or "high" in limit_state: + limit = UsdPhysics.LimitAPI.Apply(d6_prim, token) + if "low" in limit_state: + limit.CreateLowAttr().Set(limit_state["low"]) + if "high" in limit_state: + limit.CreateHighAttr().Set(limit_state["high"]) + for gain in ("stiffness", "damping"): + if f"physx_{gain}" in limit_state: + d6_prim.CreateAttribute(f"physxLimit:{token}:{gain}", Sdf.ValueTypeNames.Float).Set( + limit_state[f"physx_{gain}"] + ) if drive_state: + if sign < 0: + for key in ("target_position", "target_velocity"): + if key in drive_state: + drive_state = {**drive_state, key: -drive_state[key]} dst_drive = UsdPhysics.DriveAPI.Apply(d6_prim, token) if "damping" in drive_state: dst_drive.CreateDampingAttr().Set(drive_state["damping"]) @@ -485,6 +692,20 @@ def _convert_overconstrained_group_to_d6( if "type" in drive_state: dst_drive.CreateTypeAttr().Set(drive_state["type"]) + # Bind the D6 axis back to the semantic MJCF joint name so consumers + # can recover the per-axis mapping after the collapse. + source_name = source_joint.GetPrim().GetDisplayName() or source_joint.GetName() + d6_prim.CreateAttribute(f"mjcf:{token}:name", Sdf.ValueTypeNames.Token).Set(source_name) + + # Lock every D6 axis that no source joint maps to; an unlimited D6 axis is + # free in PhysX, which would add DOFs the MJCF never had. + for token in _D6_ALL_AXES: + if token in used_axes: + continue + limit = UsdPhysics.LimitAPI.Apply(d6_prim, token) + limit.CreateLowAttr().Set(1.0) + limit.CreateHighAttr().Set(-1.0) + # PhysxJointAPI tuning is single-valued per joint: take the primary's # values and drop the rest (the warning at the end notes the loss). if primary_physx_attrs: @@ -504,7 +725,7 @@ def _convert_overconstrained_group_to_d6( # Deactivate every other joint; leaving any active re-triggers # over-constraining. Filter primary_path from both lists defensively. - converted_joints = [j for j, _ in axis_assignments] + converted_joints = [joint for _, _, _, _, joint in axis_state] joints_to_deactivate: list[Usd.Prim] = [ j for j in converted_joints + list(dropped_joints) if j.GetPath() != primary_path ] @@ -512,7 +733,7 @@ def _convert_overconstrained_group_to_d6( override = stage.OverridePrim(joint.GetPath()) override.SetActive(False) - for joint, _ in axis_assignments: + for joint in converted_joints: source_joint_remap[joint.GetPath()] = primary_path _restore_ancestor_specifiers(edit_layer, ancestor_snapshot) diff --git a/source/extensions/isaacsim.asset.importer.utils/standalone_tests/test_mjc_d6_collapse.py b/source/extensions/isaacsim.asset.importer.utils/standalone_tests/test_mjc_d6_collapse.py new file mode 100644 index 0000000000..4df2b7ea4c --- /dev/null +++ b/source/extensions/isaacsim.asset.importer.utils/standalone_tests/test_mjc_d6_collapse.py @@ -0,0 +1,332 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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. + +"""Regression tests for collapsing over-constrained MJCF joint groups into PhysX D6 joints. + +Covers isaac-sim/IsaacLab#6854: when several single-axis joints connect the +same body pair (e.g. a 3-DOF humanoid hip authored as three hinges), the +collapse must preserve every source DOF on a distinct D6 axis, keep per-axis +limits and drive parameters on the correct axis, lock every unused D6 axis, +and record which source MJCF joint feeds each used axis. + +Runs with plain ``pxr`` (no Omni modules) like test_smoke.py. The fixtures +mirror what mujoco-usd-converter produces: joints are exported x-aligned +(``physics:axis = X``) with the MJCF axis direction encoded in +``localRot0``/``localRot1``. +""" + +from __future__ import annotations + +import unittest + +from pxr import Gf, Sdf, Usd, UsdPhysics + +from isaacsim.asset.importer.utils.impl.mjc_to_physx_conversion_utils import ( + combine_overconstrained_joints_to_d6, +) + +_ROT_AXES = ("rotX", "rotY", "rotZ") +_TRANS_AXES = ("transX", "transY", "transZ") + + +def _x_aligned_quat(axis_dir: tuple[float, float, float]) -> Gf.Quatf: + """Shortest-arc rotation taking local +X to the given MJCF axis direction.""" + rotation = Gf.Rotation(Gf.Vec3d(1, 0, 0), Gf.Vec3d(*axis_dir)) + return Gf.Quatf(rotation.GetQuat()) + + +def _make_revolute_joint( + stage: Usd.Stage, + name: str, + body0: Usd.Prim, + body1: Usd.Prim, + axis_dir: tuple[float, float, float], + pos: tuple[float, float, float] = (0.0, 0.0, 0.0), + lower: float | None = None, + upper: float | None = None, + stiffness: float | None = None, + damping: float | None = None, + max_force: float | None = None, +) -> UsdPhysics.RevoluteJoint: + """Author a revolute joint the way mujoco-usd-converter does.""" + joint = UsdPhysics.RevoluteJoint.Define(stage, Sdf.Path(f"/Robot/Joints/{name}")) + joint.CreateBody0Rel().SetTargets([body0.GetPath()]) + joint.CreateBody1Rel().SetTargets([body1.GetPath()]) + joint.CreateAxisAttr().Set("X") + + quat = _x_aligned_quat(axis_dir) + joint.CreateLocalPos0Attr().Set(Gf.Vec3f(*pos)) + joint.CreateLocalRot0Attr().Set(quat) + joint.CreateLocalPos1Attr().Set(Gf.Vec3f(*pos)) + joint.CreateLocalRot1Attr().Set(quat) + + if lower is not None and upper is not None: + joint.CreateLowerLimitAttr().Set(lower) + joint.CreateUpperLimitAttr().Set(upper) + + if stiffness is not None or damping is not None or max_force is not None: + drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular") + if stiffness is not None: + drive.CreateStiffnessAttr().Set(stiffness) + if damping is not None: + drive.CreateDampingAttr().Set(damping) + if max_force is not None: + drive.CreateMaxForceAttr().Set(max_force) + return joint + + +def _build_three_hinge_stage() -> Usd.Stage: + """A humanoid-style hip: three orthogonal hinges between torso and thigh.""" + stage = Usd.Stage.CreateInMemory() + stage.DefinePrim("/Robot") + torso = stage.DefinePrim("/Robot/torso") + thigh = stage.DefinePrim("/Robot/thigh") + _make_revolute_joint( + stage, "hip_x", torso, thigh, (1, 0, 0), + lower=-30.0, upper=45.0, stiffness=100.0, damping=10.0, max_force=1000.0, + ) + _make_revolute_joint( + stage, "hip_y", torso, thigh, (0, 1, 0), + lower=-20.0, upper=60.0, stiffness=200.0, damping=20.0, max_force=2000.0, + ) + _make_revolute_joint( + stage, "hip_z", torso, thigh, (0, 0, 1), + lower=-90.0, upper=90.0, stiffness=300.0, damping=30.0, max_force=3000.0, + ) + return stage + + +def _build_two_hinge_stage() -> Usd.Stage: + """A 2-DOF joint group (e.g. a wrist): two orthogonal hinges.""" + stage = Usd.Stage.CreateInMemory() + stage.DefinePrim("/Robot") + upper = stage.DefinePrim("/Robot/upper_arm") + forearm = stage.DefinePrim("/Robot/forearm") + _make_revolute_joint( + stage, "wrist_x", upper, forearm, (1, 0, 0), + lower=-45.0, upper=45.0, stiffness=50.0, damping=5.0, max_force=500.0, + ) + _make_revolute_joint( + stage, "wrist_y", upper, forearm, (0, 1, 0), + lower=-70.0, upper=10.0, stiffness=80.0, damping=8.0, max_force=800.0, + ) + return stage + + +def _active_joints(stage: Usd.Stage) -> list[Usd.Prim]: + return [p for p in stage.TraverseAll() if p.IsA(UsdPhysics.Joint) and p.IsActive()] + + +def _limit(prim: Usd.Prim, axis: str) -> tuple[float | None, float | None] | None: + if not prim.HasAPI(UsdPhysics.LimitAPI, axis): + return None + api = UsdPhysics.LimitAPI(prim, axis) + low = api.GetLowAttr() + high = api.GetHighAttr() + return ( + low.Get() if low and low.HasAuthoredValue() else None, + high.Get() if high and high.HasAuthoredValue() else None, + ) + + +def _drive(prim: Usd.Prim, axis: str) -> dict: + if not prim.HasAPI(UsdPhysics.DriveAPI, axis): + return {} + api = UsdPhysics.DriveAPI(prim, axis) + out = {} + for key, getter_name in ( + ("stiffness", "GetStiffnessAttr"), + ("damping", "GetDampingAttr"), + ("max_force", "GetMaxForceAttr"), + ): + attr = getattr(api, getter_name)() + if attr and attr.HasAuthoredValue(): + out[key] = attr.Get() + return out + + +def _axis_source_name(prim: Usd.Prim, axis: str) -> str | None: + attr = prim.GetAttribute(f"mjcf:{axis}:name") + if attr and attr.IsValid() and attr.HasAuthoredValue(): + return str(attr.Get()) + return None + + +class TestThreeHingeCollapse(unittest.TestCase): + """Collapsing a 3-hinge group must preserve all three DOFs.""" + + def setUp(self) -> None: + self.stage = _build_three_hinge_stage() + combine_overconstrained_joints_to_d6(self.stage) + joints = _active_joints(self.stage) + self.assertEqual(len(joints), 1, f"expected a single D6 joint, got {joints}") + self.d6 = joints[0] + + def test_all_source_dofs_survive(self) -> None: + used = [ + axis + for axis in _ROT_AXES + if (limit := _limit(self.d6, axis)) is not None and limit[0] is not None and limit[0] <= limit[1] + ] + self.assertEqual( + len(used), 3, f"expected 3 usable rotation axes, got {used}; duplicate-axis joints were dropped" + ) + + def test_per_axis_limits_preserved(self) -> None: + authored = {_axis_source_name(self.d6, axis): _limit(self.d6, axis) for axis in _ROT_AXES} + self.assertEqual(authored["hip_x"], (-30.0, 45.0)) + self.assertEqual(authored["hip_y"], (-20.0, 60.0)) + self.assertEqual(authored["hip_z"], (-90.0, 90.0)) + + def test_per_axis_drives_preserved(self) -> None: + expected = { + "hip_x": {"stiffness": 100.0, "damping": 10.0, "max_force": 1000.0}, + "hip_y": {"stiffness": 200.0, "damping": 20.0, "max_force": 2000.0}, + "hip_z": {"stiffness": 300.0, "damping": 30.0, "max_force": 3000.0}, + } + for axis in _ROT_AXES: + name = _axis_source_name(self.d6, axis) + self.assertIn(name, expected, f"axis {axis} missing source-joint binding") + self.assertEqual(_drive(self.d6, axis), expected[name], f"axis {axis} ({name})") + + def test_unused_axes_locked(self) -> None: + for axis in _TRANS_AXES: + limit = _limit(self.d6, axis) + self.assertIsNotNone(limit, f"{axis} has no LimitAPI at all (axis is FREE)") + self.assertGreater(limit[0], limit[1], f"{axis} not locked: {limit}") + + def test_frame_matches_source_axes(self) -> None: + rotation = Gf.Rotation(UsdPhysics.Joint(self.d6).GetLocalRot1Attr().Get()) + expected = {"hip_x": Gf.Vec3d(1, 0, 0), "hip_y": Gf.Vec3d(0, 1, 0), "hip_z": Gf.Vec3d(0, 0, 1)} + d6_dirs = {"rotX": Gf.Vec3d(1, 0, 0), "rotY": Gf.Vec3d(0, 1, 0), "rotZ": Gf.Vec3d(0, 0, 1)} + for axis in _ROT_AXES: + name = _axis_source_name(self.d6, axis) + physical = rotation.TransformDir(d6_dirs[axis]) + self.assertGreater( + physical * expected[name], 0.999, + f"{axis} bound to {name} but points along {physical}, expected {expected[name]}", + ) + + def test_non_primary_joints_deactivated(self) -> None: + inactive = [p for p in self.stage.TraverseAll() if p.IsA(UsdPhysics.Joint) and not p.IsActive()] + self.assertEqual(len(inactive), 2) + + def test_rerun_is_idempotent(self) -> None: + self.assertEqual(combine_overconstrained_joints_to_d6(self.stage), 0) + self.assertEqual(len(_active_joints(self.stage)), 1) + + +class TestTwoHingeCollapse(unittest.TestCase): + """Collapsing a 2-hinge group must lock the remaining rotation axis.""" + + def setUp(self) -> None: + self.stage = _build_two_hinge_stage() + combine_overconstrained_joints_to_d6(self.stage) + self.d6 = _active_joints(self.stage)[0] + + def test_third_rotation_axis_locked(self) -> None: + locked = [ + axis + for axis in _ROT_AXES + if (limit := _limit(self.d6, axis)) is not None and limit[0] is not None and limit[0] > limit[1] + ] + used = [axis for axis in _ROT_AXES if axis not in locked] + self.assertEqual(len(used), 2, f"expected 2 used rotation axes, got {used}") + self.assertEqual(len(locked), 1, f"expected the unused rotation axis locked, got {locked}") + for axis in _TRANS_AXES: + limit = _limit(self.d6, axis) + self.assertIsNotNone(limit) + self.assertGreater(limit[0], limit[1], f"{axis} not locked: {limit}") + + def test_axis_binding_metadata(self) -> None: + bound = sorted(name for axis in _ROT_AXES if (name := _axis_source_name(self.d6, axis))) + self.assertEqual(bound, ["wrist_x", "wrist_y"]) + + +class TestEdgeCases(unittest.TestCase): + """Degenerate and inconsistent source groups.""" + + def test_left_handed_axis_triple_flips_limits(self) -> None: + stage = _build_three_hinge_stage() + stage.RemovePrim("/Robot/Joints/hip_z") + _make_revolute_joint( + stage, "hip_z", stage.GetPrimAtPath("/Robot/torso"), stage.GetPrimAtPath("/Robot/thigh"), + (0, 0, -1), lower=-90.0, upper=90.0, stiffness=300.0, damping=30.0, max_force=3000.0, + ) + combine_overconstrained_joints_to_d6(stage) + d6 = _active_joints(stage)[0] + + bound_axis = next(axis for axis in _ROT_AXES if _axis_source_name(d6, axis) == "hip_z") + self.assertEqual(_limit(d6, bound_axis), (-90.0, 90.0)) + + # The D6 frame must remain right-handed. + rotation = Gf.Rotation(UsdPhysics.Joint(d6).GetLocalRot1Attr().Get()) + x = rotation.TransformDir(Gf.Vec3d(1, 0, 0)) + y = rotation.TransformDir(Gf.Vec3d(0, 1, 0)) + z = rotation.TransformDir(Gf.Vec3d(0, 0, 1)) + self.assertGreater(Gf.Cross(x, y) * z, 0.999) + + # The bound axis must point along the source joint's physical axis (0, 0, -1). + d6_dirs = {"rotX": Gf.Vec3d(1, 0, 0), "rotY": Gf.Vec3d(0, 1, 0), "rotZ": Gf.Vec3d(0, 0, 1)} + physical = rotation.TransformDir(d6_dirs[bound_axis]) + self.assertGreater(abs(physical * Gf.Vec3d(0, 0, -1)), 0.999) + + def test_true_duplicate_axis_dropped_but_others_survive(self) -> None: + stage = _build_three_hinge_stage() + _make_revolute_joint( + stage, "hip_x_dup", stage.GetPrimAtPath("/Robot/torso"), stage.GetPrimAtPath("/Robot/thigh"), + (1, 0, 0), lower=-10.0, upper=10.0, stiffness=1.0, damping=1.0, + ) + combine_overconstrained_joints_to_d6(stage) + d6 = _active_joints(stage)[0] + + bound = sorted(name for axis in _ROT_AXES if (name := _axis_source_name(d6, axis))) + self.assertEqual(bound, ["hip_x", "hip_y", "hip_z"]) + self.assertFalse(stage.GetPrimAtPath("/Robot/Joints/hip_x_dup").IsActive()) + + def test_unlimited_hinge_axis_stays_free(self) -> None: + stage = _build_three_hinge_stage() + stage.RemovePrim("/Robot/Joints/hip_z") + _make_revolute_joint( + stage, "hip_z", stage.GetPrimAtPath("/Robot/torso"), stage.GetPrimAtPath("/Robot/thigh"), (0, 0, 1) + ) + combine_overconstrained_joints_to_d6(stage) + d6 = _active_joints(stage)[0] + + bound_axis = next(axis for axis in _ROT_AXES if _axis_source_name(d6, axis) == "hip_z") + limit = _limit(d6, bound_axis) + self.assertTrue(limit is None or limit == (None, None), f"unlimited hinge got limits {limit}") + for axis in _TRANS_AXES: + locked = _limit(d6, axis) + self.assertIsNotNone(locked) + self.assertGreater(locked[0], locked[1], f"{axis} not locked: {locked}") + + def test_mismatched_joint_positions_warn(self) -> None: + stage = _build_three_hinge_stage() + stage.RemovePrim("/Robot/Joints/hip_z") + _make_revolute_joint( + stage, "hip_z", stage.GetPrimAtPath("/Robot/torso"), stage.GetPrimAtPath("/Robot/thigh"), + (0, 0, 1), pos=(0.05, 0.0, 0.0), lower=-90.0, upper=90.0, + ) + with self.assertLogs("isaacsim.asset.importer.utils.impl.mjc_to_physx_conversion_utils", level="WARNING") as logs: + combine_overconstrained_joints_to_d6(stage) + self.assertTrue( + any("different" in message and "position" in message for message in logs.output), + f"no joint-frame-mismatch warning in {logs.output}", + ) + + +if __name__ == "__main__": + unittest.main()