Add URDF model for RM75-B OmniPicker with detailed link and joint specifications

xr_rm_teleop/test/test_placo_transforms.py

@@ -1,37 +1,72 @@
-import math
+from pathlib import Path
+from xml.etree import ElementTree
 
 import numpy as np
 import pytest
 
 from xr_rm_teleop.placo_ik_solver import (
     PlacoIkSolver,
-    _arm_pose_to_transform,
-    _tool_pose_to_transform,
-    _transform_to_arm_pose,
+    _validated_transform,
 )
-from xr_rm_teleop.realman_adapter import ArmPose
 
 
-def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
-    flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
-    tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    root = ElementTree.parse(urdf_path).getroot()
+    moving_joint_names = [
+        joint.attrib["name"]
+        for joint in root.findall("joint")
+        if joint.attrib["type"] != "fixed"
+    ]
+    tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
+    mesh_filenames = [
+        mesh.attrib["filename"]
+        for mesh in root.findall(".//mesh")
+    ]
 
-    base_to_flange = _arm_pose_to_transform(flange_pose)
-    flange_to_tool = _tool_pose_to_transform(tool_pose)
-    base_to_tool = base_to_flange @ flange_to_tool
-    recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
-
-    assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
-    assert recovered_flange == pytest.approx(base_to_flange)
+    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
+    assert all(
+        filename.startswith(
+            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
+        )
+        for filename in mesh_filenames
+    )
+    assert tcp_joint is not None
+    assert tcp_joint.attrib["type"] == "fixed"
+    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
+    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
+    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
+    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
 
 
-def test_transform_to_arm_pose_roundtrip() -> None:
-    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
+def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
+    transform = np.eye(4)
+    transform[:3, 3] = [0.3, -0.1, 0.2]
 
-    actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
+    actual = _validated_transform(transform)
 
-    assert actual.xyz() == pytest.approx(expected.xyz())
-    assert actual.rpy() == pytest.approx(expected.rpy())
+    assert actual == pytest.approx(transform)
+    assert actual is not transform
+
+
+@pytest.mark.parametrize(
+    "transform",
+    [
+        np.eye(3),
+        np.full((4, 4), np.nan),
+        np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
+        np.diag([2.0, 1.0, 1.0, 1.0]),
+    ],
+)
+def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
+    with pytest.raises(ValueError):
+        _validated_transform(transform)
 
 
 def test_qp_result_rejects_nan_position_and_velocity_violations() -> None: