fix(llm-plan-executor): enforce safe motion and live scene state

src/iiwa_llm_plan_executor/doc/webots-qa.md

@@ -88,11 +88,13 @@ bounds.  Out-of-corridor and stale-state graphs stopped without execution.
    part transits/approaches/contact, and enforces all tilt/J7/jump limits after
    planning. This is a planning limitation, not a fallback to an unconstrained
    `MoveToPose` action.
-3. **Tilt numerical band.** A few OMPL/LIN paths were returned with 10.03–10.16
+3. **Tilt limit.** The original Webots runs returned some OMPL/LIN paths at
-   deg tilt against the 10 deg policy; the policy uses a 0.25 deg numerical
+   10.03–10.16 deg tilt against the 10 deg limit; the previous 0.25 deg
-   band and rejects larger values. Some approach paths reached 11–12 deg and
+   allowance accepted them. The review on 2026-11-11 replaced that allowance
-   were rejected safely. A clean solution requires a planner/adapter that
+   with floating-point roundoff only, so those paths are now rejected before
-   honors the orientation constraint end-to-end.
+   execution. This stricter policy passed package tests but has not yet been
+   rechecked in Webots. A planner/adapter that honors the orientation
+   constraint end-to-end is still needed for reliable execution.
 4. **Controller start overshoot.** Webots can settle a joint ~3e-5 rad outside
    MoveIt's soft limit; the start state is now clamped to the same
    `soft_min_position`/`soft_max_position` band before planning.

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py

@@ -218,9 +218,9 @@ class MotionPolicy:
             start_tilt > self.limits.max_tilt_deg + epsilon
             and held_object is None
         )
-        # 0.1 deg is the measurement/numerical band of the FK check; the
+        # Allow only floating-point roundoff; a planned path must obey the
-        # planner is constrained to a stricter 8 deg band.
+        # configured tilt limit rather than widening it by a visible angle.
-        tilt_epsilon_deg = 0.25
+        tilt_epsilon_deg = 1e-6
         tilt_limit_deg = self.limits.max_tilt_deg + tilt_epsilon_deg
         for index, pose in enumerate(tcp_poses):
             if not isinstance(pose, Transform):

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py

@@ -580,6 +580,9 @@ class PlannerExecutorNode(Node):
             else:
                 stable_since = None
             time.sleep(0.05)
+        raise RuntimeError(
+            f'robot did not become stationary within {timeout_sec:.2f} s'
+        )
 
     def _scene_guard(self) -> tuple[bool, str]:
         if not self._bridge_ready:
@@ -694,16 +697,15 @@ class PlannerExecutorNode(Node):
         self.log.write(payload)
 
     def _on_replan(self, envelope: dict[str, Any] | None) -> None:
-        if envelope is not None:
+        run_id = (self._last_status or {}).get('run_id')
-            self._last_status = dict(envelope)
         self.log.write({
-            'run_id': (self._last_status or {}).get('run_id'),
+            'run_id': run_id,
             'event': 'replan_required',
             'scene_revision': None if envelope is None else envelope.get('scene_revision'),
         })
         if envelope is not None:
             self._publish_status({
-                'run_id': (self._last_status or {}).get('run_id'),
+                'run_id': run_id,
                 'state': 'replan_scene',
                 'reason': 'fresh scene snapshot for the external planner',
                 'scene_revision': envelope.get('scene_revision'),

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py

@@ -149,7 +149,8 @@ class SkillRegistry:
         goal: Transform,
         phase: str,
     ) -> Any:
-        outcome = self.motion.plan_and_execute(goal, phase, context.held_object)
+        carried = context.held_object or context.mounted_tool
+        outcome = self.motion.plan_and_execute(goal, phase, carried)
         if not getattr(outcome, 'success', False):
             code = getattr(outcome, 'code', 'failed')
             event = 'collision' if code == 'collision' else 'failed'
@@ -205,16 +206,12 @@ class SkillRegistry:
             )
             if pose is not None:
                 return pose
-        if object_id == 'cap':
+            pose = _state_pose(
-            return world_target(context.spec, context.geometry, 'cap')
+                (state.get('tools') or {}).get(object_id, {}).get('pose'),
-        if object_id.startswith('screw_'):
+                object_id,
-            return world_target(
-                context.spec, context.geometry, f'screw:{object_id}',
-            )
-        if object_id in context.spec.tool_names:
-            return world_target(
-                context.spec, context.geometry, f'tool:{object_id}',
             )
+            if pose is not None:
+                return pose
         raise SkillError('failed', f'no live pose for {object_id!r}')
 
     def _object_contact_goal(
@@ -457,9 +454,16 @@ class SkillRegistry:
             return SkillOutcome('success', {'reused': True, 'tool': tool})
         adapter = self._adapter(context)
         prepare = getattr(adapter, 'prepare_grab_tool', None)
+        self._ensure_approach(context, node)
         if prepare is not None:
             prepare(tool)
+        try:
             self._ensure_contact(context, node)
+        except Exception:
+            restore = getattr(adapter, 'restore_tool', None)
+            if prepare is not None and restore is not None:
+                restore(tool)
+            raise
         try:
             self._command(
                 context, 'attach_tool', tool, event='grasp_failed',
@@ -500,9 +504,16 @@ class SkillRegistry:
             prepare = getattr(adapter, 'prepare_grab_cap', None)
         else:
             prepare = getattr(adapter, 'prepare_grab_screw', None)
+        self._ensure_approach(context, node)
         if prepare is not None:
             prepare(node.object)
+        try:
             contact = self._ensure_contact(context, node)
+        except Exception:
+            restore = getattr(adapter, 'restore_part', None)
+            if prepare is not None and restore is not None:
+                restore(node.object)
+            raise
         try:
             self._command(context, command, node.object, event='grasp_failed')
         except SkillError as error:

src/iiwa_llm_plan_executor/test/test_motion_policy.py

@@ -91,7 +91,7 @@ def test_tcp_tilt_above_ten_degrees_is_rejected():
     assert 'tilt' in rejected.reason
     assert checker.validate_trajectory(
         [point(0.0)], [down_pose(10.05)],
-    ).ok
+    ).ok is False
 
 
 def test_an_empty_gripper_may_reorient_to_the_working_posture():

src/iiwa_llm_plan_executor/test/test_node.py

@@ -5,6 +5,8 @@ import json
 from pathlib import Path
 from types import SimpleNamespace
 
+import pytest
+
 from iiwa_experement.geometry import load_geometry
 from iiwa_experement.scenario import load_scenario
 from iiwa_llm_plan_executor.engine import AdmissionResult, RunStatus
@@ -78,6 +80,15 @@ def test_plan_qos_is_transient_local_depth_one():
     assert qos.depth == 1
 
 
+def test_stationary_wait_rejects_a_robot_that_keeps_moving():
+    node = object.__new__(PlannerExecutorNode)
+    node._last_joint_velocity = (0.2,) * 7
+    with pytest.raises(RuntimeError, match='did not become stationary'):
+        PlannerExecutorNode._wait_stationary(
+            node, timeout_sec=0.02, tolerance=0.01,
+        )
+
+
 def test_safe_name_and_run_log_are_machine_readable(tmp_path):
     assert _safe_name('run/one 1') == 'run_one_1'
     log = RunLog(str(tmp_path))
@@ -135,6 +146,17 @@ def test_rejected_submit_returns_false_and_publishes_the_reason():
     assert 'replan_required' in published[-1]['reason']
 
 
+def test_replan_scene_status_keeps_the_run_id():
+    node = fake_node()
+    node._last_status = {'run_id': 'run-42', 'state': 'running'}
+    envelope = {'scene_revision': 7, 'scene_json': {'components': []}}
+    PlannerExecutorNode._on_replan(node, envelope)
+    payload = json.loads(node._status_publisher.messages[-1])
+    assert payload['run_id'] == 'run-42'
+    assert payload['scene_revision'] == 7
+    assert payload['scene'] == envelope
+
+
 def test_invalid_submit_json_is_rejected_without_calling_the_engine():
     node = fake_node()
     request = SimpleNamespace(value='{broken')

src/iiwa_llm_plan_executor/test/test_skills.py

@@ -203,6 +203,69 @@ def test_approach_goes_above_the_target_and_saves_the_height():
     assert phases(motion) == ['approach']
 
 
+def test_mounted_tool_is_reported_as_carried_during_motion():
+    registry, context, motion, _ = make_registry()
+    context.mounted_tool = 'vacuum'
+    result = registry.execute(node('a', 'approach', 'cap'), context)
+    assert result.event == 'success'
+    assert motion.calls[0][2] == 'vacuum'
+
+
+def test_approach_refuses_to_use_manifest_pose_when_supervisor_pose_is_missing():
+    registry, context, motion, supervisor = make_registry()
+    supervisor.state['cap']['pose'] = None
+    result = registry.execute(node('a', 'approach', 'cap'), context)
+    assert result.event == 'failed'
+    assert motion.calls == []
+
+
+def test_grab_keeps_target_in_collision_scene_during_approach():
+    class RecordingScene(FakeScene):
+        def prepare_grab_cap(self, object_id):
+            self.events.append(('prepare_grab_cap', object_id))
+
+    adapter = RecordingScene()
+    registry, context, motion, _ = make_registry(
+        motion=FakeMotion(success=False), adapter=adapter,
+    )
+    context.mounted_tool = 'vacuum'
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'failed'
+    assert phases(motion) == ['approach']
+    assert adapter.events == []
+
+
+def test_grab_restores_target_when_lowering_fails():
+    class RecordingScene(FakeScene):
+        def prepare_grab_cap(self, object_id):
+            self.events.append(('prepare_grab_cap', object_id))
+
+        def restore_part(self, object_id):
+            self.events.append(('restore_part', object_id))
+
+    class FailOnLower(FakeMotion):
+        def plan_and_execute(self, goal, phase, held_object):
+            self.calls.append((goal, phase, held_object))
+            return MotionOutcome(
+                phase != 'lower',
+                'ok' if phase != 'lower' else 'motion_limit',
+                'ok' if phase != 'lower' else 'rejected by policy',
+            )
+
+    adapter = RecordingScene()
+    registry, context, motion, _ = make_registry(
+        motion=FailOnLower(), adapter=adapter,
+    )
+    context.mounted_tool = 'vacuum'
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'failed'
+    assert phases(motion) == ['approach', 'lower']
+    assert adapter.events == [
+        ('prepare_grab_cap', 'cap'),
+        ('restore_part', 'cap'),
+    ]
+
+
 def test_grab_cap_runs_missing_phases_and_lifts_to_saved_height():
     registry, context, motion, supervisor = make_registry()
     context.mounted_tool = 'vacuum'