From 112673f1f4199096eb82a21dc1a003be61e18c77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 11 Nov 2026 10:16:46 +1000 Subject: [PATCH] fix(llm-plan-executor): enforce safe motion and live scene state --- src/iiwa_llm_plan_executor/doc/webots-qa.md | 12 ++-- .../iiwa_llm_plan_executor/motion_policy.py | 6 +- .../iiwa_llm_plan_executor/node.py | 10 +-- .../iiwa_llm_plan_executor/skills.py | 35 +++++++---- .../test/test_motion_policy.py | 2 +- src/iiwa_llm_plan_executor/test/test_node.py | 22 +++++++ .../test/test_skills.py | 63 +++++++++++++++++++ 7 files changed, 125 insertions(+), 25 deletions(-) diff --git a/src/iiwa_llm_plan_executor/doc/webots-qa.md b/src/iiwa_llm_plan_executor/doc/webots-qa.md index 05eeb16..25fa742 100644 --- a/src/iiwa_llm_plan_executor/doc/webots-qa.md +++ b/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 - deg tilt against the 10 deg policy; the policy uses a 0.25 deg numerical - band and rejects larger values. Some approach paths reached 11–12 deg and - were rejected safely. A clean solution requires a planner/adapter that - honors the orientation constraint end-to-end. +3. **Tilt limit.** The original Webots runs returned some OMPL/LIN paths at + 10.03–10.16 deg tilt against the 10 deg limit; the previous 0.25 deg + allowance accepted them. The review on 2026-11-11 replaced that allowance + with floating-point roundoff only, so those paths are now rejected before + 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. diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py index 208c8c0..4c6de51 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py +++ b/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 - # planner is constrained to a stricter 8 deg band. - tilt_epsilon_deg = 0.25 + # Allow only floating-point roundoff; a planned path must obey the + # configured tilt limit rather than widening it by a visible angle. + 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): diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py index 58ca7e9..306b982 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py +++ b/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: - self._last_status = dict(envelope) + run_id = (self._last_status or {}).get('run_id') 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'), diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py index 792d42e..2785c0f 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py +++ b/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': - return world_target(context.spec, context.geometry, 'cap') - if object_id.startswith('screw_'): - 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}', + pose = _state_pose( + (state.get('tools') or {}).get(object_id, {}).get('pose'), + 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) - self._ensure_contact(context, node) + 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) - contact = self._ensure_contact(context, node) + 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: diff --git a/src/iiwa_llm_plan_executor/test/test_motion_policy.py b/src/iiwa_llm_plan_executor/test/test_motion_policy.py index 58fce87..f7ead66 100644 --- a/src/iiwa_llm_plan_executor/test/test_motion_policy.py +++ b/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(): diff --git a/src/iiwa_llm_plan_executor/test/test_node.py b/src/iiwa_llm_plan_executor/test/test_node.py index 74a4d1f..aea8017 100644 --- a/src/iiwa_llm_plan_executor/test/test_node.py +++ b/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') diff --git a/src/iiwa_llm_plan_executor/test/test_skills.py b/src/iiwa_llm_plan_executor/test/test_skills.py index c91da39..c100640 100644 --- a/src/iiwa_llm_plan_executor/test/test_skills.py +++ b/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'