feat(experement): add guarded motion adapter with pose verification

src/iiwa_experement/iiwa_experement/motion.py

@@ -0,0 +1,377 @@
+"""Safe wrapper around the existing ``cobot/move_to_pose`` motion server.
+
+Every call performs the same guard sequence:
+fresh joint state -> synchronized planning scene -> available server ->
+accepted goal -> successful result -> measured tcp pose.  Any failure stops
+the robot and blocks all later motion.
+"""
+from __future__ import annotations
+
+import math
+import time
+from dataclasses import dataclass
+from typing import Any, Callable
+
+import numpy as np
+from geometry_msgs.msg import PoseStamped
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from scipy.spatial.transform import Rotation
+from sensor_msgs.msg import JointState
+from std_srvs.srv import Trigger
+import tf2_ros
+
+from iiwa_msgs.action import MoveToPose
+from iiwa_msgs.srv import MoveToNamedPose
+
+from .geometry import GeometrySpec, Transform
+
+
+MOVE_ACTION = '/cobot/move_to_pose'
+NAMED_SERVICE = '/cobot/move_to_named'
+STOP_SERVICE = '/cobot/stop'
+PLANNERS = ('ptp', 'lin', 'ompl')
+
+
+@dataclass(frozen=True)
+class MotionResult:
+    success: bool
+    message: str
+    label: str
+    sent: bool = False
+    goal: Transform | None = None
+    actual: Transform | None = None
+    duration_sec: float = 0.0
+
+
+def transform_from_pose(message, parent: str, child: str) -> Transform:
+    orientation = message.orientation
+    matrix = Rotation.from_quat([
+        orientation.x, orientation.y, orientation.z, orientation.w,
+    ]).as_matrix()
+    return Transform(
+        xyz=(
+            float(message.position.x),
+            float(message.position.y),
+            float(message.position.z),
+        ),
+        rotation=tuple(tuple(float(value) for value in row) for row in matrix),
+        parent=parent,
+        child=child,
+    )
+
+
+def transform_from_tf(message, parent: str, child: str) -> Transform:
+    translation = message.transform.translation
+    rotation = message.transform.rotation
+    matrix = Rotation.from_quat([
+        rotation.x, rotation.y, rotation.z, rotation.w,
+    ]).as_matrix()
+    return Transform(
+        xyz=(
+            float(translation.x),
+            float(translation.y),
+            float(translation.z),
+        ),
+        rotation=tuple(tuple(float(value) for value in row) for row in matrix),
+        parent=parent,
+        child=child,
+    )
+
+
+def _pose_distance(first: Transform, second: Transform) -> tuple[float, float]:
+    distance = float(np.linalg.norm(first.position - second.position))
+    cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
+    angle = float(np.arccos(max(-1.0, min(1.0, cosine))))
+    return distance, angle
+
+
+class MotionAdapter:
+    """Guarded motion client used by every reference operation."""
+
+    def __init__(
+        self,
+        node: Node,
+        geometry: GeometrySpec,
+        *,
+        action_client: Any | None = None,
+        named_client: Any | None = None,
+        stop_client: Any | None = None,
+        tf_buffer: Any | None = None,
+        scene_check: Callable[[], tuple[bool, str]] | None = None,
+        pose_provider: Callable[[], Transform | None] | None = None,
+        freshness_check: Callable[[], tuple[bool, str]] | None = None,
+        spin_until: Callable[[Any, float], bool] | None = None,
+        position_tolerance: float | None = None,
+        orientation_tolerance: float | None = None,
+        command_timeout_sec: float = 90.0,
+        settle_time_sec: float = 0.5,
+    ) -> None:
+        self.node = node
+        self.geometry = geometry
+        self.action_client = action_client or ActionClient(
+            node, MoveToPose, MOVE_ACTION,
+        )
+        self.named_client = named_client or node.create_client(
+            MoveToNamedPose, NAMED_SERVICE,
+        )
+        self.stop_client = stop_client or node.create_client(
+            Trigger, STOP_SERVICE,
+        )
+        self.tf_buffer = tf_buffer
+        self._tf_listener = None
+        if tf_buffer is None:
+            self.tf_buffer = tf2_ros.Buffer()
+            self._tf_listener = tf2_ros.TransformListener(self.tf_buffer, node)
+        self._scene_check = scene_check
+        self._pose_provider = pose_provider
+        self._freshness_check = freshness_check
+        self._spin_until = spin_until or self._default_spin_until
+        self.position_tolerance = (
+            geometry.position_tolerance
+            if position_tolerance is None else float(position_tolerance)
+        )
+        self.orientation_tolerance = (
+            geometry.orientation_tolerance_rad
+            if orientation_tolerance is None else float(orientation_tolerance)
+        )
+        self.command_timeout_sec = float(command_timeout_sec)
+        self.settle_time_sec = max(0.0, float(settle_time_sec))
+        self.stopped = False
+        self.stop_reason: str | None = None
+        self._last_joint_time = None
+        if freshness_check is None:
+            node.create_subscription(JointState, '/joint_states', self._on_joint_state, 10)
+
+    # ------------------------------------------------------------------ freshness
+    def _on_joint_state(self, message: JointState) -> None:
+        del message
+        self._last_joint_time = self.node.get_clock().now()
+
+    def _default_freshness(self) -> tuple[bool, str]:
+        if self._last_joint_time is None:
+            return False, 'no /joint_states received yet'
+        age = (self.node.get_clock().now() - self._last_joint_time).nanoseconds / 1e9
+        if age > 1.0:
+            return False, f'/joint_states is stale ({age:.2f} s old)'
+        if self.tf_buffer is not None:
+            try:
+                self.tf_buffer.lookup_transform('base_link', 'tcp', Time())
+            except Exception as error:
+                return False, f'tcp transform is unavailable: {error}'
+        return True, 'ok'
+
+    def _fresh(self) -> tuple[bool, str]:
+        if self._freshness_check is None:
+            return self._default_freshness()
+        return self._freshness_check()
+
+    def _scene(self) -> tuple[bool, str]:
+        if self._scene_check is None:
+            return False, 'planning scene bridge is not configured'
+        return self._scene_check()
+
+    # ------------------------------------------------------------------ transforms
+    def _default_spin_until(self, future, timeout_sec: float) -> bool:
+        import rclpy
+
+        rclpy.spin_until_future_complete(
+            self.node, future, timeout_sec=timeout_sec,
+        )
+        return future.done()
+
+    def world_to_base(self) -> Transform:
+        # The URDF ships a ``world_base_joint`` with an identity origin, so
+        # robot_state_publisher always provides a synthetic world->base_link
+        # transform at zero.  The physical Webots placement comes from the
+        # launch setting and is stored in GeometrySpec.world_to_base.
+        return self.geometry.world_to_base
+
+    def update_world_to_base(self, transform: Transform) -> None:
+        self.geometry = GeometrySpec(
+            **{**self.geometry.__dict__, 'world_to_base': transform}
+        )
+
+    def actual_tcp(self) -> Transform | None:
+        if self._pose_provider is not None:
+            return self._pose_provider()
+        if self.tf_buffer is None:
+            return None
+        try:
+            message = self.tf_buffer.lookup_transform('base_link', 'tcp', Time())
+            return transform_from_tf(message, 'base_link', 'tcp')
+        except Exception:
+            return None
+
+    def to_base_goal(self, goal: Transform) -> Transform:
+        return self.world_to_base().inverse().compose_relaxed(goal)
+
+    def _abc(self, goal: Transform) -> tuple[float, float, float]:
+        euler = Rotation.from_matrix(goal.matrix).as_euler('ZYX')
+        return float(euler[0]), float(euler[1]), float(euler[2])
+
+    # ------------------------------------------------------------------ guards
+    def _guard(self, label: str, speed: float, planner: str) -> MotionResult | None:
+        if self.stopped:
+            return MotionResult(
+                False,
+                f'motion is blocked after stop: {self.stop_reason}',
+                label,
+            )
+        if not math.isfinite(float(speed)) or not 0.0 < float(speed) <= 1.0:
+            return MotionResult(False, f'invalid speed {speed!r}', label)
+        if planner not in PLANNERS:
+            return MotionResult(False, f'unknown planner {planner!r}', label)
+        fresh, message = self._fresh()
+        if not fresh:
+            self.stop(message)
+            return MotionResult(False, message, label)
+        synchronized, message = self._scene()
+        if not synchronized:
+            self.stop(message)
+            return MotionResult(False, message, label)
+        return None
+
+    # ------------------------------------------------------------------ motion
+    def move_pose(
+        self,
+        goal: Transform,
+        planner: str = 'ptp',
+        speed: float = 0.2,
+        label: str = '',
+    ) -> MotionResult:
+        label = label or 'move_pose'
+        blocked = self._guard(label, speed, planner)
+        if blocked is not None:
+            return blocked
+        if not self.action_client.wait_for_server(timeout_sec=5.0):
+            return MotionResult(False, f'{MOVE_ACTION} is unavailable', label)
+
+        base_goal = self.to_base_goal(goal)
+        a, b, c = self._abc(base_goal)
+        request = MoveToPose.Goal()
+        request.x, request.y, request.z = (float(value) for value in base_goal.xyz)
+        request.a, request.b, request.c = a, b, c
+        request.speed = float(speed)
+        request.planner = planner
+        request.frame_id = 'base_link'
+
+        started = time.monotonic()
+        future = self.action_client.send_goal_async(request)
+        if not self._spin_until(future, self.command_timeout_sec):
+            return self._fail(label, 'goal response timed out', base_goal, started)
+        try:
+            goal_handle = future.result()
+        except Exception as error:
+            return self._fail(label, f'goal call failed: {error}', base_goal, started)
+        if goal_handle is None or not goal_handle.accepted:
+            return self._fail(label, 'motion goal was rejected', base_goal, started)
+
+        result_future = goal_handle.get_result_async()
+        if not self._spin_until(result_future, self.command_timeout_sec):
+            return self._fail(label, 'motion result timed out', base_goal, started)
+        try:
+            wrapped = result_future.result()
+            result = wrapped.result
+        except Exception as error:
+            return self._fail(label, f'motion result failed: {error}', base_goal, started)
+        if not result.success:
+            return self._fail(
+                label, result.message or 'motion server reported failure',
+                base_goal, started,
+            )
+
+        self._settle()
+        actual = self.actual_tcp()
+        if actual is None:
+            return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started)
+        distance, angle = _pose_distance(actual, base_goal)
+        if distance > self.position_tolerance or angle > self.orientation_tolerance:
+            return self._fail(
+                label,
+                f'actual tcp is {distance:.4f} m / {angle:.4f} rad from the goal',
+                base_goal,
+                started,
+                actual=actual,
+            )
+        return MotionResult(
+            True, 'motion completed', label, True, base_goal, actual,
+            time.monotonic() - started,
+        )
+
+    def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
+        label = label or f'move_named({name})'
+        blocked = self._guard(label, speed, 'ptp')
+        if blocked is not None:
+            return blocked
+        if not self.named_client.wait_for_service(timeout_sec=5.0):
+            return MotionResult(False, f'{NAMED_SERVICE} is unavailable', label)
+
+        request = MoveToNamedPose.Request()
+        request.name = str(name)
+        request.speed = float(speed)
+        request.accel_scale = float(speed)
+        started = time.monotonic()
+        future = self.named_client.call_async(request)
+        if not self._spin_until(future, self.command_timeout_sec):
+            return self._fail(label, 'named motion timed out', None, started)
+        try:
+            response = future.result()
+        except Exception as error:
+            return self._fail(label, f'named motion failed: {error}', None, started)
+        if not response.success:
+            return self._fail(
+                label, response.message or 'named motion failed', None, started,
+            )
+        self._settle()
+        return MotionResult(
+            True, response.message or 'motion completed', label,
+            True, None, self.actual_tcp(), time.monotonic() - started,
+        )
+
+    def _settle(self) -> None:
+        if self.settle_time_sec <= 0.0 or self.node is None:
+            return
+        import rclpy
+
+        end = time.monotonic() + self.settle_time_sec
+        while time.monotonic() < end:
+            rclpy.spin_once(
+                self.node,
+                timeout_sec=min(0.05, max(0.0, end - time.monotonic())),
+            )
+
+    def _fail(
+        self,
+        label: str,
+        message: str,
+        goal: Transform | None,
+        started: float,
+        *,
+        actual: Transform | None = None,
+    ) -> MotionResult:
+        self.stop(message)
+        return MotionResult(
+            False, message, label, True, goal, actual,
+            time.monotonic() - started,
+        )
+
+    def stop(self, reason: str) -> bool:
+        self.stopped = True
+        if self.stop_reason is None:
+            self.stop_reason = reason
+        try:
+            if self.stop_client.wait_for_service(timeout_sec=1.0):
+                future = self.stop_client.call_async(Trigger.Request())
+                self._spin_until(future, 5.0)
+                return bool(future.done() and future.result().success)
+        except Exception:
+            pass
+        return False
+
+    def reset_stop(self) -> None:
+        """Only for a new run; never used to resume after a failed motion."""
+        self.stopped = False
+        self.stop_reason = None

src/iiwa_experement/test/test_motion.py

@@ -0,0 +1,235 @@
+"""Motion guards: stale state, unsynchronized scene, failures and stop."""
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import Transform, load_geometry
+from iiwa_experement.motion import MotionAdapter
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+class FakeFuture:
+    def __init__(self, value=None, done=True):
+        self._value = value
+        self._done = done
+
+    def done(self):
+        return self._done
+
+    def result(self):
+        return self._value
+
+
+class FakeResult:
+    def __init__(self, success=True, message='ok'):
+        self.success = success
+        self.message = message
+
+
+class FakeWrapped:
+    def __init__(self, result):
+        self.result = result
+
+
+class FakeGoalHandle:
+    def __init__(self, accepted=True, result=None):
+        self.accepted = accepted
+        self._result = result or FakeResult()
+
+    def get_result_async(self):
+        return FakeFuture(FakeWrapped(self._result))
+
+
+class FakeActionClient:
+    def __init__(self, *, available=True, accepted=True, result=None, timeout=False):
+        self.available = available
+        self.accepted = accepted
+        self.result = result
+        self.timeout = timeout
+        self.requests = []
+
+    def wait_for_server(self, timeout_sec=0.0):
+        return self.available
+
+    def send_goal_async(self, request):
+        self.requests.append(request)
+        if self.timeout:
+            return FakeFuture(None, done=False)
+        return FakeFuture(FakeGoalHandle(self.accepted, self.result))
+
+
+class FakeService:
+    def __init__(self, *, available=True, success=True, message='ok', timeout=False):
+        self.available = available
+        self.success = success
+        self.message = message
+        self.timeout = timeout
+        self.requests = []
+
+    def wait_for_service(self, timeout_sec=0.0):
+        return self.available
+
+    def call_async(self, request):
+        self.requests.append(request)
+        if self.timeout:
+            return FakeFuture(None, done=False)
+        return FakeFuture(FakeResult(self.success, self.message))
+
+
+class StopService(FakeService):
+    def __init__(self):
+        super().__init__(success=True, message='stopped')
+        self.calls = 0
+
+    def call_async(self, request):
+        self.calls += 1
+        return super().call_async(request)
+
+
+def geometry():
+    return load_geometry(GEOMETRY)
+
+
+def goal():
+    return Transform.from_xyz_rpy(
+        [0.20, 0.10, 0.90], [180.0, 0.0, 45.0], parent='world', child='tcp',
+    )
+
+
+def make_adapter(
+    *,
+    action=None,
+    named=None,
+    stop=None,
+    fresh=True,
+    scene=True,
+    actual=None,
+    spin=None,
+):
+    g = geometry()
+    action = action or FakeActionClient()
+    named = named or FakeService()
+    stop = stop or StopService()
+    adapter = MotionAdapter(
+        node=None,
+        geometry=g,
+        action_client=action,
+        named_client=named,
+        stop_client=stop,
+        tf_buffer=object(),
+        scene_check=lambda: (scene, 'scene is stale' if not scene else 'ok'),
+        freshness_check=lambda: (fresh, 'joint state is stale' if not fresh else 'ok'),
+        pose_provider=lambda: actual if actual is not None else adapter.to_base_goal(goal()),
+        spin_until=spin or (lambda future, timeout: future.done()),
+    )
+    return adapter, action, named, stop
+
+
+def test_successful_move_populates_abc_and_verifies_actual_pose():
+    adapter, action, _, _ = make_adapter()
+    result = adapter.move_pose(goal(), planner='ptp', speed=0.2, label='test')
+    assert result.success, result.message
+    assert len(action.requests) == 1
+    request = action.requests[0]
+    expected = adapter.to_base_goal(goal())
+    assert (request.x, request.y, request.z) == pytest.approx(expected.xyz)
+    assert request.speed == pytest.approx(0.2)
+    assert request.planner == 'ptp'
+    assert request.frame_id == 'base_link'
+    assert result.goal.xyz == pytest.approx(expected.xyz)
+
+
+def test_stale_joint_state_blocks_before_sending_and_stops():
+    stop = StopService()
+    adapter, action, _, _ = make_adapter(fresh=False, stop=stop)
+    result = adapter.move_pose(goal(), label='stale')
+    assert not result.success
+    assert 'stale' in result.message
+    assert action.requests == []
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_unsynchronized_scene_blocks_before_sending_and_stops():
+    adapter, action, _, _ = make_adapter(scene=False)
+    result = adapter.move_pose(goal(), label='scene')
+    assert not result.success
+    assert 'scene' in result.message
+    assert action.requests == []
+    assert adapter.stopped is True
+
+
+def test_invalid_speed_and_planner_are_rejected_without_stopping():
+    adapter, action, _, _ = make_adapter()
+    assert not adapter.move_pose(goal(), speed=0.0).success
+    assert not adapter.move_pose(goal(), speed=2.0).success
+    assert not adapter.move_pose(goal(), planner='teleport').success
+    assert action.requests == []
+    assert adapter.stopped is False
+
+
+def test_rejected_goal_stops_and_blocks_the_next_move():
+    action = FakeActionClient(accepted=False)
+    adapter, action, _, stop = make_adapter(action=action)
+    result = adapter.move_pose(goal(), label='rejected')
+    assert not result.success
+    assert 'rejected' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+    count = len(action.requests)
+    blocked = adapter.move_pose(goal(), label='again')
+    assert not blocked.success
+    assert 'blocked' in blocked.message
+    assert len(action.requests) == count
+
+
+def test_timeout_stops_the_robot():
+    adapter, _, _, stop = make_adapter(action=FakeActionClient(timeout=True))
+    result = adapter.move_pose(goal(), label='timeout')
+    assert not result.success
+    assert 'timed out' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_actual_pose_out_of_tolerance_stops_the_robot():
+    actual = goal().compose_relaxed(Transform.from_xyz_rpy(
+        [0.0, 0.0, 0.05], [0, 0, 0], parent='world', child='tcp',
+    ))
+    adapter, _, _, stop = make_adapter(actual=actual)
+    result = adapter.move_pose(goal(), label='mismatch')
+    assert not result.success
+    assert 'from the goal' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_named_motion_uses_the_existing_service():
+    adapter, _, named, _ = make_adapter()
+    result = adapter.move_named('home', speed=0.2)
+    assert result.success
+    assert len(named.requests) == 1
+    assert named.requests[0].name == 'home'
+    assert named.requests[0].speed == pytest.approx(0.2)
+
+
+def test_failed_named_motion_stops():
+    named = FakeService(success=False, message='unknown state')
+    adapter, _, _, stop = make_adapter(named=named)
+    result = adapter.move_named('missing')
+    assert not result.success
+    assert 'unknown state' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_unavailable_server_is_reported_without_motion():
+    adapter, action, _, _ = make_adapter(action=FakeActionClient(available=False))
+    result = adapter.move_pose(goal(), label='no server')
+    assert not result.success
+    assert 'unavailable' in result.message
+    assert action.requests == []
+    assert adapter.stopped is False