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

2026-11-08 14:02:55 +10:00
parent 5a63420aa2
commit c58aa56757
2 changed files with 612 additions and 0 deletions
@@ -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