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()