first commit

text-to-cad-main/tests/python/viewer/moveit2_server/__init__.py

@@ -0,0 +1 @@
+"""MoveIt2 server tests."""

text-to-cad-main/tests/python/viewer/moveit2_server/test_context.py

@@ -0,0 +1,281 @@
+from __future__ import annotations
+
+import asyncio
+import json
+import os
+import tempfile
+from pathlib import Path
+import unittest
+from unittest.mock import patch
+
+from tests.python.support.paths import add_repo_path
+
+add_repo_path("viewer/moveit2_server")
+
+from moveit2_server.context import build_moveit2_context
+from moveit2_server.protocol import MotionProtocolError
+from moveit2_server.server import handle_message
+
+
+def write_file(path: Path, text: str) -> None:
+    path.parent.mkdir(parents=True, exist_ok=True)
+    path.write_text(text, encoding="utf-8")
+
+
+def write_sample_robot(repo_root: Path, urdf_ref: str = "robot.urdf", srdf_ref: str = "robot.srdf") -> None:
+    urdf_path = repo_root / urdf_ref
+    srdf_path = repo_root / srdf_ref
+    write_file(
+        urdf_path,
+        """
+        <robot name="robot">
+          <link name="base_link"/>
+          <link name="wrist_link"/>
+          <link name="tool_link"/>
+          <joint name="shoulder" type="revolute">
+            <parent link="base_link"/>
+            <child link="wrist_link"/>
+            <limit lower="-1" upper="1" effort="1" velocity="1"/>
+          </joint>
+          <joint name="wrist_tool" type="fixed">
+            <parent link="wrist_link"/>
+            <child link="tool_link"/>
+          </joint>
+        </robot>
+        """,
+    )
+    relative_urdf_ref = Path(os.path.relpath(urdf_path, srdf_path.parent)).as_posix()
+    write_file(
+        srdf_path,
+        f"""
+        <robot name="robot" xmlns:tcad="https://text-to-cad.dev/srdf">
+          <tcad:urdf path="{relative_urdf_ref}"/>
+          <group name="arm">
+            <joint name="shoulder"/>
+          </group>
+          <group name="tool_group">
+            <link name="tool_link"/>
+          </group>
+          <end_effector name="tool" parent_link="wrist_link" group="tool_group" parent_group="arm"/>
+          <group_state name="home" group="arm">
+            <joint name="shoulder" value="0"/>
+          </group_state>
+        </robot>
+        """,
+    )
+
+
+def sample_payload() -> dict[str, object]:
+    return {
+        "dir": "",
+        "file": "robot.srdf",
+        "startJointValuesByNameDeg": {"shoulder": 10},
+        "target": {
+            "endEffector": "tool",
+            "frame": "base_link",
+            "xyz": [0.1, 0.0, 0.2],
+        },
+        "moveit2": {
+            "planningGroup": "arm",
+            "endEffector": "tool",
+            "targetFrame": "base_link",
+            "ik": {"timeout": 0.2, "attempts": 3, "tolerance": 0.01},
+            "planning": {
+                "pipeline": "ompl",
+                "plannerId": "RRTConnectkConfigDefault",
+                "planningTime": 2.0,
+                "maxVelocityScalingFactor": 0.5,
+                "maxAccelerationScalingFactor": 0.25,
+            },
+        },
+    }
+
+
+class MoveIt2ContextTests(unittest.TestCase):
+    def test_builds_context_from_srdf_file_and_request_settings(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_sample_robot(repo_root)
+
+            context = build_moveit2_context(
+                repo_root=repo_root,
+                dir="",
+                file="robot.srdf",
+                type="srdf.planToPose",
+                payload=sample_payload(),
+            )
+
+            self.assertEqual(context["file"], "robot.srdf")
+            self.assertEqual(context["command"]["planningGroup"], "arm")
+            self.assertEqual(context["command"]["endEffectors"][0]["link"], "tool_link")
+            self.assertTrue(context["command"]["ik"]["positionOnly"])
+            self.assertEqual(context["command"]["endEffectors"][0]["positionTolerance"], 0.01)
+            self.assertEqual(context["command"]["ik"]["attempts"], 3)
+            self.assertEqual(context["command"]["planner"]["planningTime"], 2.0)
+            self.assertIn("robot.srdf", context["modelAssetHash"])
+            self.assertEqual(Path(str(context["urdfPath"])).resolve(), (repo_root / "robot.urdf").resolve())
+
+    def test_chain_planning_group_uses_chain_joints_not_all_active_joints(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_file(
+                repo_root / "robot.urdf",
+                """
+                <robot name="robot">
+                  <link name="base_link"/>
+                  <link name="shoulder_link"/>
+                  <link name="wrist_link"/>
+                  <link name="tool_link"/>
+                  <link name="finger_link"/>
+                  <joint name="shoulder" type="revolute">
+                    <parent link="base_link"/>
+                    <child link="shoulder_link"/>
+                  </joint>
+                  <joint name="elbow" type="revolute">
+                    <parent link="shoulder_link"/>
+                    <child link="wrist_link"/>
+                  </joint>
+                  <joint name="wrist_tool" type="fixed">
+                    <parent link="wrist_link"/>
+                    <child link="tool_link"/>
+                  </joint>
+                  <joint name="gripper" type="revolute">
+                    <parent link="tool_link"/>
+                    <child link="finger_link"/>
+                  </joint>
+                </robot>
+                """,
+            )
+            write_file(
+                repo_root / "robot.srdf",
+                """
+                <robot name="robot" xmlns:tcad="https://text-to-cad.dev/srdf">
+                  <tcad:urdf path="robot.urdf"/>
+                  <group name="arm">
+                    <chain base_link="base_link" tip_link="tool_link"/>
+                  </group>
+                  <group name="gripper">
+                    <joint name="gripper"/>
+                    <link name="finger_link"/>
+                  </group>
+                  <end_effector name="tool" parent_link="tool_link" group="gripper" parent_group="arm"/>
+                </robot>
+                """,
+            )
+
+            context = build_moveit2_context(
+                repo_root=repo_root,
+                dir="",
+                file="robot.srdf",
+                type="srdf.solvePose",
+                payload=sample_payload(),
+            )
+
+            self.assertEqual(context["command"]["jointNames"], ["shoulder", "elbow"])
+            self.assertEqual(context["command"]["endEffectors"][0]["jointNames"], ["shoulder", "elbow"])
+
+    def test_accepts_orientation_and_explicit_target_link(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_sample_robot(repo_root)
+            payload = sample_payload()
+            payload["target"]["quat_xyzw"] = [0, 0, 0, 2]
+            payload["moveit2"]["targetLink"] = "tool_link"
+            payload["moveit2"]["ik"]["positionOnly"] = False
+
+            context = build_moveit2_context(
+                repo_root=repo_root,
+                dir="",
+                file="robot.srdf",
+                type="srdf.solvePose",
+                payload=payload,
+            )
+
+            self.assertFalse(context["command"]["ik"]["positionOnly"])
+            self.assertEqual(context["command"]["endEffectors"][0]["link"], "tool_link")
+
+    def test_rejects_group_state_joint_outside_group_or_limits(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_sample_robot(repo_root)
+            srdf_path = repo_root / "robot.srdf"
+            srdf_path.write_text(
+                srdf_path.read_text(encoding="utf-8").replace(
+                    '<joint name="shoulder" value="0"/>',
+                    '<joint name="shoulder" value="2"/>',
+                ),
+                encoding="utf-8",
+            )
+
+            with self.assertRaisesRegex(MotionProtocolError, "above its URDF upper limit"):
+                build_moveit2_context(
+                    repo_root=repo_root,
+                    dir="",
+                    file="robot.srdf",
+                    type="srdf.solvePose",
+                    payload=sample_payload(),
+                )
+
+    def test_accepts_repo_relative_file_ref_with_catalog_dir(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_sample_robot(repo_root, "workspace/robot.urdf", "workspace/robot.srdf")
+            payload = sample_payload()
+            payload["dir"] = "workspace"
+            payload["file"] = "workspace/robot.srdf"
+
+            context = build_moveit2_context(
+                repo_root=repo_root,
+                dir="workspace",
+                file="workspace/robot.srdf",
+                type="srdf.solvePose",
+                payload=payload,
+            )
+
+            self.assertEqual(context["dir"], "workspace")
+            self.assertEqual(context["file"], "robot.srdf")
+            self.assertEqual(Path(str(context["srdfPath"])).resolve(), (repo_root / "workspace/robot.srdf").resolve())
+
+    def test_handle_message_uses_moveit2_context(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_sample_robot(repo_root)
+            message = {"id": "req-1", "type": "srdf.solvePose", "payload": sample_payload()}
+
+            with patch("moveit2_server.server.dispatch", return_value={"jointValuesByNameDeg": {"shoulder": 11}}):
+                response = asyncio.run(handle_message(json.dumps(message), repo_root=repo_root))
+
+            payload = json.loads(response)
+            self.assertTrue(payload["ok"])
+            self.assertEqual(payload["result"]["jointValuesByNameDeg"]["shoulder"], 11)
+
+    def test_rejects_traversal(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            with self.assertRaisesRegex(MotionProtocolError, "file must stay inside"):
+                build_moveit2_context(
+                    repo_root=Path(tempdir),
+                    dir="",
+                    file="../robot.srdf",
+                    type="srdf.solvePose",
+                    payload=sample_payload(),
+                )
+
+    def test_rejects_unknown_planning_group_from_request(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            repo_root = Path(tempdir)
+            write_sample_robot(repo_root)
+            payload = sample_payload()
+            payload["moveit2"]["planningGroup"] = "missing"
+
+            with self.assertRaisesRegex(MotionProtocolError, "Selected planning group"):
+                build_moveit2_context(
+                    repo_root=repo_root,
+                    dir="",
+                    file="robot.srdf",
+                    type="srdf.planToPose",
+                    payload=payload,
+                )
+
+
+if __name__ == "__main__":
+    unittest.main()

text-to-cad-main/tests/python/viewer/moveit2_server/test_moveit_py_adapter.py

@@ -0,0 +1,171 @@
+from __future__ import annotations
+
+from pathlib import Path
+from types import SimpleNamespace
+import tempfile
+import unittest
+
+from tests.python.support.paths import add_repo_path
+
+add_repo_path("viewer/moveit2_server")
+
+from moveit2_server.moveit_py import (
+    MoveItPyAdapter,
+    _joint_state_seed,
+    _native_start_joint_values,
+    _robot_description_for_moveit,
+)
+
+
+class MoveItPyAdapterHelperTests(unittest.TestCase):
+    def test_robot_description_rewrites_relative_meshes_to_file_uris(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            root = Path(tempdir)
+            urdf_path = root / "robot.urdf"
+            urdf_path.write_text(
+                """
+                <robot name="robot">
+                  <link name="base">
+                    <visual>
+                      <geometry>
+                        <mesh filename="STL/base.stl" />
+                      </geometry>
+                    </visual>
+                  </link>
+                </robot>
+                """,
+                encoding="utf-8",
+            )
+
+            description = _robot_description_for_moveit(urdf_path)
+
+            self.assertIn((root / "STL/base.stl").resolve().as_uri(), description)
+            self.assertNotIn('filename="STL/base.stl"', description)
+
+    def test_config_dict_builds_moveit2_config_from_srdf_and_request_settings(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            root = Path(tempdir)
+            urdf_path = root / "robot.urdf"
+            urdf_path.write_text('<robot name="robot"><link name="base" /></robot>', encoding="utf-8")
+            srdf_path = root / "robot.srdf"
+            srdf_path.write_text('<robot name="robot" />', encoding="utf-8")
+            request = SimpleNamespace(
+                context={
+                    "urdfPath": str(urdf_path),
+                    "srdfPath": str(srdf_path),
+                    "srdf": {
+                        "planningGroups": [
+                            {"name": "arm", "jointNames": ["shoulder"]},
+                            {"name": "arm_with_gripper", "subgroups": ["arm", "gripper"]},
+                        ]
+                    },
+                },
+                command={
+                    "ik": {"timeout": 0.2, "attempts": 3},
+                    "planner": {
+                        "pipeline": "ompl",
+                        "plannerId": "RRTConnectkConfigDefault",
+                        "planningTime": 2.0,
+                        "maxVelocityScalingFactor": 0.5,
+                        "maxAccelerationScalingFactor": 0.25,
+                    },
+                },
+            )
+
+            config = MoveItPyAdapter()._config_dict(request)
+
+            self.assertEqual(config["robot_description_semantic"], '<robot name="robot" />')
+            self.assertEqual(config["robot_description_kinematics"]["arm"]["kinematics_solver_timeout"], 0.2)
+            self.assertTrue(config["robot_description_kinematics"]["arm"]["position_only_ik"])
+            self.assertNotIn("arm_with_gripper", config["robot_description_kinematics"])
+            self.assertEqual(config["planning_pipelines"]["pipeline_names"], ["ompl"])
+            self.assertEqual(config["plan_request_params"]["planning_attempts"], 3)
+            self.assertEqual(config["plan_request_params"]["max_velocity_scaling_factor"], 0.5)
+            self.assertIsNot(config["plan_request_params"], config["ompl_rrtc"]["plan_request_params"])
+            self.assertEqual(config["ompl_rrtc"]["plan_request_params"]["planning_attempts"], 3)
+            self.assertEqual(config["planning_scene_monitor_options"]["wait_for_initial_state_timeout"], 5.0)
+
+    def test_joint_state_seed_includes_all_active_urdf_joints(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            urdf_path = Path(tempdir) / "robot.urdf"
+            urdf_path.write_text(
+                """
+                <robot name="robot">
+                  <joint name="fixed_base" type="fixed" />
+                  <joint name="shoulder" type="revolute" />
+                  <joint name="slide" type="prismatic" />
+                </robot>
+                """,
+                encoding="utf-8",
+            )
+            request = SimpleNamespace(
+                context={"urdfPath": str(urdf_path)},
+                payload={"startJointValuesByNameDeg": {"shoulder": 90, "slide": 0.25}},
+                command={
+                    "planningGroup": "arm",
+                    "jointNames": ["shoulder"],
+                },
+            )
+
+            names, positions = _joint_state_seed(request)
+
+            self.assertEqual(names, ["shoulder", "slide"])
+            self.assertAlmostEqual(positions[0], 1.5707963267948966)
+            self.assertEqual(positions[1], 0.25)
+
+    def test_legacy_start_joint_values_only_convert_angular_joints(self) -> None:
+        with tempfile.TemporaryDirectory() as tempdir:
+            urdf_path = Path(tempdir) / "robot.urdf"
+            urdf_path.write_text(
+                """
+                <robot name="robot">
+                  <joint name="shoulder" type="revolute" />
+                  <joint name="slide" type="prismatic" />
+                </robot>
+                """,
+                encoding="utf-8",
+            )
+            request = SimpleNamespace(
+                context={"urdfPath": str(urdf_path)},
+                payload={"startJointValuesByNameDeg": {"shoulder": 90, "slide": 0.25}},
+                command={"jointNames": ["shoulder", "slide"]},
+            )
+
+            values = _native_start_joint_values(request)
+
+            self.assertAlmostEqual(values["shoulder"], 1.5707963267948966)
+            self.assertEqual(values["slide"], 0.25)
+
+    def test_serializes_robot_trajectory_messages_with_fallback_timing(self) -> None:
+        class Duration:
+            sec = 0
+            nanosec = 0
+
+        class Point:
+            def __init__(self, positions: list[float]) -> None:
+                self.time_from_start = Duration()
+                self.positions = positions
+
+        class JointTrajectory:
+            joint_names = ["shoulder", "elbow"]
+            points = [Point([0.0, 0.0]), Point([1.5707963267948966, 0.0])]
+
+        class RobotTrajectoryMsg:
+            joint_trajectory = JointTrajectory()
+
+        class RobotTrajectory:
+            def get_robot_trajectory_msg(self) -> RobotTrajectoryMsg:
+                return RobotTrajectoryMsg()
+
+        request = SimpleNamespace(context={}, payload={}, command={"jointNames": ["shoulder", "elbow"]})
+        serialized = MoveItPyAdapter()._serialize_trajectory(RobotTrajectory(), ["shoulder", "elbow"], request)
+
+        self.assertEqual(serialized["jointNames"], ["shoulder", "elbow"])
+        self.assertEqual(serialized["points"][0]["timeFromStartSec"], 0.0)
+        self.assertGreater(serialized["points"][1]["timeFromStartSec"], 0.0)
+        self.assertAlmostEqual(serialized["points"][1]["positions"][0], 1.5707963267948966)
+        self.assertAlmostEqual(serialized["points"][1]["positionsDeg"][0], 90.0)
+
+
+if __name__ == "__main__":
+    unittest.main()

text-to-cad-main/tests/python/viewer/moveit2_server/test_protocol.py

@@ -0,0 +1,133 @@
+from __future__ import annotations
+
+import unittest
+from unittest.mock import patch
+
+from tests.python.support.paths import add_repo_path
+
+add_repo_path("viewer/moveit2_server")
+
+from moveit2_server.dispatcher import dispatch
+from moveit2_server.protocol import (
+    MotionProtocolError,
+    normalize_joint_values,
+    normalize_motion_target,
+    normalize_request,
+)
+
+
+def moveit2_context() -> dict[str, object]:
+    return {
+        "command": {
+            "planningGroup": "arm",
+            "jointNames": ["shoulder", "elbow"],
+            "endEffectors": [
+                {
+                    "name": "tool",
+                    "link": "tool_link",
+                    "frame": "base_link",
+                    "planningGroup": "arm",
+                    "jointNames": ["shoulder", "elbow"],
+                    "positionTolerance": 0.002,
+                }
+            ],
+            "ik": {"timeout": 0.05, "attempts": 1, "tolerance": 0.002},
+            "planner": {
+                "pipeline": "ompl",
+                "plannerId": "RRTConnectkConfigDefault",
+                "planningTime": 1.0,
+                "maxVelocityScalingFactor": 1.0,
+                "maxAccelerationScalingFactor": 1.0,
+            },
+        },
+    }
+
+
+class FakeMoveIt2Adapter:
+    def solve_pose(self, request: object) -> dict[str, object]:
+        return {"jointValuesByNameDeg": {"shoulder": 11}}
+
+    def plan_to_pose(self, request: object) -> dict[str, object]:
+        return {
+            "jointValuesByNameDeg": {"shoulder": 11},
+            "trajectory": {"jointNames": ["shoulder", "elbow"], "points": []},
+        }
+
+
+class MoveIt2ProtocolTests(unittest.TestCase):
+    def test_normalizes_joint_values(self) -> None:
+        self.assertEqual(normalize_joint_values({"shoulder": "12.5"}), {"shoulder": 12.5})
+        with self.assertRaisesRegex(MotionProtocolError, "empty joint names"):
+            normalize_joint_values({"": 1})
+
+    def test_normalizes_request_with_moveit2_context(self) -> None:
+        request = normalize_request(
+            {
+                "id": "abc",
+                "type": "srdf.solvePose",
+                "payload": {
+                    "file": "robot.srdf",
+                    "target": {
+                        "endEffector": "tool",
+                        "frame": "base_link",
+                        "xyz": [0.1, 0.0, 0.2],
+                    },
+                },
+            },
+            context=moveit2_context(),
+        )
+
+        self.assertEqual(request.id, "abc")
+        self.assertEqual(request.protocol_version, 1)
+        self.assertEqual(request.command["planningGroup"], "arm")
+
+    def test_normalizes_target_orientation(self) -> None:
+        target = normalize_motion_target({
+            "target": {
+                "endEffector": "tool",
+                "frame": "base_link",
+                "xyz": [0, 0, 0],
+                "quat_xyzw": [0, 0, 0, 2],
+            }
+        })
+
+        self.assertEqual(target["orientationMode"], "quat_xyzw")
+        self.assertEqual(target["quat_xyzw"], (0.0, 0.0, 0.0, 1.0))
+
+        with self.assertRaisesRegex(MotionProtocolError, "exactly one"):
+            normalize_motion_target({
+                "target": {
+                    "endEffector": "tool",
+                    "frame": "base_link",
+                    "xyz": [0, 0, 0],
+                    "quat_xyzw": [0, 0, 0, 1],
+                    "rpy": [0, 0, 0],
+                }
+            })
+
+    def test_dispatch_uses_moveit2_adapter_for_solve_and_plan(self) -> None:
+        request = normalize_request(
+            {
+                "id": "abc",
+                "type": "srdf.planToPose",
+                "payload": {
+                    "startJointValuesByNameDeg": {"shoulder": 10},
+                    "target": {
+                        "endEffector": "tool",
+                        "frame": "base_link",
+                        "xyz": [0.1, 0.0, 0.2],
+                    },
+                },
+            },
+            context=moveit2_context(),
+        )
+
+        with patch("moveit2_server.dispatcher._adapter", return_value=FakeMoveIt2Adapter()):
+            result = dispatch(request)
+
+        self.assertEqual(result["jointValuesByNameDeg"]["shoulder"], 11)
+        self.assertEqual(result["trajectory"]["jointNames"], ["shoulder", "elbow"])
+
+
+if __name__ == "__main__":
+    unittest.main()