URDF import: merge_fixed_joints rotates origins with pitch −π/2 by 180° about x #880

Description

With merge_fixed_joints=True, merge_fixed_joints() (source/libraries/isaacsim/asset/importer/urdf/python/impl/urdf_utils.py) writes each composed origin back as RPY through _rotation_matrix_to_rpy. In the gimbal-lock branch, roll is always atan2(R[0, 1], R[0, 2]), which is only correct for pitch = +π/2. For roll r, pitch −π/2 and yaw y, R[0, 1] = −sin(r + y) and R[0, 2] = −cos(r + y), so roll comes out as r + y + π and the written rotation is the true rotation times Rx(π).

Every composed origin with pitch −π/2 is therefore written back rotated by 180° about its x axis: re-parented joint origins, which carry their child subtree, and merged visual and collision origins, for example behind a fixed joint with rpy="0 -1.5707963 0". The branch covers pitches within about 1.4e-6 rad of ±π/2, so -1.5707963 and xacro's ${-pi/2} are affected. Isaac Lab's URDF converter enables merge_fixed_joints by default.

Repro (6.2.0 standalone importer, isaacsim-asset-isolated==6.2.0.0; script below): mount is fixed to base_link with pitch −π/2, and arm hangs from mount on a revolute joint.

<robot name="rpy">
  <link name="base_link">
    <inertial><mass value="2"/><inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/></inertial>
    <collision><geometry><box size="0.2 0.2 0.1"/></geometry></collision>
  </link>
  <link name="mount"/>
  <link name="arm">
    <inertial><mass value="1"/><inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/></inertial>
    <collision><origin xyz="0 0 0.15"/><geometry><box size="0.04 0.04 0.3"/></geometry></collision>
  </link>
  <joint name="mount_joint" type="fixed">
    <parent link="base_link"/><child link="mount"/><origin xyz="0 0 0.1" rpy="0 -1.5707963 0"/>
  </joint>
  <joint name="arm_joint" type="revolute">
    <parent link="mount"/><child link="arm"/><origin xyz="0 0 0.05"/><axis xyz="0 1 0"/>
    <limit lower="-1.5" upper="1.5" effort="10" velocity="5"/>
  </joint>
</robot>

merge_fixed_joints=False: arm z axis (-1.0, 0.0, 0.0), arm collider center (-0.2, 0.0, 0.1)
merge_fixed_joints=True: arm z axis (1.0, 0.0, 0.0), arm collider center (0.1, 0.0, 0.1)

Expected: the same pose with and without merging.

Suggested fix in _rotation_matrix_to_rpy: when pitch is −π/2, negate both atan2 arguments. The +π/2 case and the non-degenerate branch stay as they are.

roll = math.atan2(R[0, 1], R[0, 2]) if sy > 0 else math.atan2(-R[0, 1], -R[0, 2])

#860 had this fix and two regression tests against the 6.1 location of the file. It can no longer land because the file moved to source/libraries in 6.2.0, which external pull requests cannot change, so I am closing it. Its tests still apply to test_urdf_utils.py, which stayed in the extension.

"""Import a URDF with and without merge_fixed_joints and print where the arm behind the merged joint ends up."""

import tempfile

from isaacsim.asset.importer.urdf import URDFImporter, URDFImporterConfig
from pxr import Gf, Usd, UsdGeom, UsdPhysics

for merge in (False, True):
    config = URDFImporterConfig(urdf_path="rpy.urdf", usd_path=tempfile.mkdtemp(), merge_fixed_joints=merge)
    stage = Usd.Stage.Open(URDFImporter(config).import_urdf())
    arm = next(p for p in stage.Traverse() if p.GetName() == "arm")
    xform = UsdGeom.Xformable(arm).ComputeLocalToWorldTransform(Usd.TimeCode.Default())
    collider = next(p for p in Usd.PrimRange(arm) if p.HasAPI(UsdPhysics.CollisionAPI))
    purposes = [UsdGeom.Tokens.default_, UsdGeom.Tokens.guide, UsdGeom.Tokens.proxy, UsdGeom.Tokens.render]
    bounds = UsdGeom.BBoxCache(Usd.TimeCode.Default(), purposes).ComputeWorldBound(collider).ComputeAlignedRange()
    print(
        f"merge_fixed_joints={merge}: arm z axis {tuple(round(v, 3) + 0.0 for v in xform.TransformDir(Gf.Vec3d(0, 0, 1)))}, "
        f"arm collider center {tuple(round(v, 3) + 0.0 for v in bounds.GetMidpoint())}"
    )

Isaac Sim version

6.2.0 (isaacsim-asset-isolated 6.2.0.0; the same code is on develop 74a9356, and 6.1.0 behaves the same)

Operating System (OS)

Ubuntu 24.04.2 LTS

GPU Name

NVIDIA RTX PRO 5000 72GB Blackwell

GPU Driver and CUDA versions

Driver 580.126.20, CUDA 13.0

Logs

No warning or error refers to the merged origins. The only warning, Stiffness and damping not available joint /rpy/Physics/arm_joint, actuator will be created without gain parameters, is logged with and without merging.

Additional information

No response