feat(llm-plan-executor): constrain planned trajectories by J7 and TCP tilt

2026-11-11 00:17:13 +10:00
parent 9ca08c01c2
commit 30c355ba6a
6 changed files with 1080 additions and 0 deletions
@@ -0,0 +1,136 @@
+"""Trajectory-level limits: J7 corridor, tilt, jumps and freshness."""
+from __future__ import annotations
+
+import math
+from pathlib import Path
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import Transform
+from iiwa_llm_plan_executor.motion_policy import MotionPolicy, load_limits
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+LIMITS = PACKAGE / 'config' / 'motion_limits.yaml'
+BASE = (0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0)
+
+
+def policy() -> MotionPolicy:
+    return MotionPolicy(load_limits(LIMITS))
+
+
+def point(j7: float, base=BASE):
+    values = list(base)
+    values[6] = j7
+    return tuple(values)
+
+
+def down_pose(tilt_deg: float = 0.0, xyz=(0.4, 0.0, 1.2)) -> Transform:
+    rotation = (
+        Rotation.from_euler('x', 180.0, degrees=True).as_matrix()
+        @ Rotation.from_euler('y', tilt_deg, degrees=True).as_matrix()
+    )
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+        parent='world',
+        child='tcp',
+    )
+
+
+def test_work_joint7_is_zero_and_corridor_is_inclusive():
+    limits = load_limits(LIMITS)
+    assert limits.work_joint7_rad == 0.0
+    assert limits.joint7_min_rad == -math.pi / 2
+    assert limits.joint7_max_rad == math.pi / 2
+    checker = MotionPolicy(limits)
+    for boundary in (-math.pi / 2, math.pi / 2):
+        result = checker.validate_trajectory(
+            [point(boundary)], [down_pose()],
+        )
+        assert result.ok, result.reason
+
+
+def test_point_outside_the_corridor_is_rejected():
+    result = policy().validate_trajectory(
+        [point(-math.pi / 2 - 0.01)], [down_pose()],
+    )
+    assert result.ok is False
+    assert 'joint7' in result.reason
+
+
+def test_intermediate_point_outside_the_corridor_rejects_the_whole_path():
+    result = policy().validate_trajectory(
+        [point(-1.2), point(-1.6), point(-1.2)],
+        [down_pose(), down_pose(), down_pose()],
+    )
+    assert result.ok is False
+    assert 'joint7' in result.reason
+
+
+def test_near_pi_joint_jump_is_rejected():
+    result = policy().validate_trajectory(
+        [point(-1.4), point(1.4)],
+        [down_pose(), down_pose()],
+    )
+    assert result.ok is False
+    assert 'jump' in result.reason
+
+
+def test_tcp_tilt_above_ten_degrees_is_rejected():
+    checker = policy()
+    assert checker.validate_trajectory(
+        [point(0.0)], [down_pose(9.9)],
+    ).ok
+    rejected = checker.validate_trajectory(
+        [point(0.0)], [down_pose(10.5)],
+    )
+    assert rejected.ok is False
+    assert 'tilt' in rejected.reason
+
+
+def test_stale_joint_state_and_scene_are_rejected_before_motion():
+    checker = policy()
+    assert checker.check_sources(
+        joint_state_age=0.1, scene_age=0.2,
+    ).ok
+    assert checker.check_sources(
+        joint_state_age=1.5, scene_age=0.2,
+    ).ok is False
+    assert checker.check_sources(
+        joint_state_age=0.1, scene_age=2.5,
+    ).ok is False
+    assert checker.check_sources(
+        joint_state_age=None, scene_age=0.2,
+    ).ok is False
+
+
+def test_malformed_or_jumping_trajectories_are_rejected():
+    checker = policy()
+    assert checker.validate_trajectory(
+        [point(0.0)[:6]], [down_pose()],
+    ).ok is False
+    assert checker.validate_trajectory(
+        [point(0.0)], [],
+    ).ok is False
+    assert checker.validate_trajectory(
+        [point(0.0), point(0.0)], [down_pose(), down_pose()],
+        current_joints=(0.5, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0),
+    ).ok is False
+    far = down_pose(xyz=(0.9, 0.0, 1.2))
+    assert checker.validate_trajectory(
+        [point(0.0), point(0.0)], [down_pose(), far],
+    ).ok is False
+
+
+def test_details_report_the_checked_extremes():
+    result = policy().validate_trajectory(
+        [point(-0.2), point(0.1)],
+        [down_pose(2.0), down_pose(4.0)],
+    )
+    assert result.ok
+    assert result.details['min_j7'] == pytest.approx(-0.2)
+    assert result.details['max_j7'] == pytest.approx(0.1)
+    assert result.details['max_tilt_deg'] == pytest.approx(4.0, abs=1e-6)
@@ -0,0 +1,199 @@
+"""The MoveIt backend rejects unsafe plans before asking for execution."""
+from __future__ import annotations
+
+import math
+from pathlib import Path
+
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import Transform, load_geometry
+from iiwa_llm_plan_executor.motion_policy import MotionPolicy, load_limits
+from iiwa_llm_plan_executor.moveit_backend import (
+    MotionOutcome,
+    MoveItBackend,
+    PlannedMotion,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+LIMITS = load_limits(PACKAGE / 'config' / 'motion_limits.yaml')
+GEOMETRY = load_geometry(ASSETS / 'geometry.yaml')
+BASE = (0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0)
+
+
+def point(j7: float):
+    values = list(BASE)
+    values[6] = j7
+    return tuple(values)
+
+
+def down_pose(tilt=0.0, xyz=(0.4, 0.0, 1.2)) -> Transform:
+    rotation = (
+        Rotation.from_euler('x', 180.0, degrees=True).as_matrix()
+        @ Rotation.from_euler('y', tilt, degrees=True).as_matrix()
+    )
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+        parent='world',
+        child='tcp',
+    )
+
+
+GOAL = down_pose()
+BASE_GOAL = GEOMETRY.world_to_base.inverse().compose_relaxed(GOAL)
+
+
+class FakePlanner:
+    def __init__(self, *, planned=None, execute_ok=True, plan_error=None,
+                 execute_error=None):
+        self.planned = planned
+        self.execute_ok = execute_ok
+        self.plan_error = plan_error
+        self.execute_error = execute_error
+        self.plan_calls = []
+        self.execute_calls = []
+        self.stopped = 0
+        self.reset_calls = 0
+
+    def plan(self, goal, phase, held_object):
+        self.plan_calls.append((goal, phase, held_object))
+        if self.plan_error is not None:
+            raise self.plan_error
+        return self.planned
+
+    def execute(self, planned):
+        self.execute_calls.append(planned)
+        if self.execute_error is not None:
+            raise self.execute_error
+        return self.execute_ok
+
+    def stop(self):
+        self.stopped += 1
+
+    def reset_stop(self):
+        self.reset_calls += 1
+
+
+def planned(points, poses, current=None):
+    if current is None:
+        current = tuple(points[0])
+    return PlannedMotion(
+        points=list(points),
+        tcp_poses=list(poses),
+        current_joints=current,
+        handle='handle',
+    )
+
+
+def backend(planner, **kwargs):
+    checker = MotionPolicy(LIMITS)
+    return MoveItBackend(
+        GEOMETRY,
+        checker,
+        planner=planner,
+        freshness_guard=kwargs.pop('freshness_guard', lambda: (True, 'ok')),
+        scene_guard=kwargs.pop('scene_guard', lambda: (True, 'ok')),
+        **kwargs,
+    )
+
+
+def test_safe_plan_is_executed_once_and_the_final_pose_is_verified():
+    planner = FakePlanner(planned=planned(
+        [point(0.0), point(0.1)],
+        [BASE_GOAL, BASE_GOAL],
+    ))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer', 'vacuum')
+    assert result.success is True
+    assert result.code == 'ok'
+    assert len(planner.plan_calls) == 1
+    assert planner.plan_calls[0][2] == 'vacuum'
+    assert len(planner.execute_calls) == 1
+
+
+def test_intermediate_out_of_corridor_point_never_reaches_execution():
+    planner = FakePlanner(planned=planned(
+        [point(-1.0), point(-1.8), point(-1.0)],
+        [BASE_GOAL, BASE_GOAL, BASE_GOAL],
+    ))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.success is False
+    assert result.code == 'motion_limit'
+    assert 'joint7' in result.message
+    assert planner.execute_calls == []
+    assert planner.stopped >= 1
+
+
+def test_excessive_tilt_and_jump_are_rejected_without_execution():
+    tilted_goal = GEOMETRY.world_to_base.inverse().compose_relaxed(down_pose(20.0))
+    for points, poses in (
+        ([point(0.0)], [tilted_goal]),
+        ([point(-1.4), point(1.4)], [BASE_GOAL, BASE_GOAL]),
+    ):
+        planner = FakePlanner(planned=planned(points, poses))
+        result = backend(planner).plan_and_execute(GOAL, 'transfer')
+        assert result.code == 'motion_limit'
+        assert planner.execute_calls == []
+
+
+def test_stale_sources_reject_before_planning():
+    planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL]))
+    result = backend(
+        planner, freshness_guard=lambda: (False, 'joint_states is stale'),
+    ).plan_and_execute(GOAL, 'transfer')
+    assert result.success is False
+    assert result.code == 'stale_state'
+    assert planner.plan_calls == []
+    assert planner.execute_calls == []
+
+
+def test_planning_and_execution_failures_are_reported_and_stop_motion():
+    planner = FakePlanner(plan_error=RuntimeError('no IK branch'))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'planning_failed'
+    assert planner.execute_calls == []
+    assert planner.stopped >= 1
+
+    planner = FakePlanner(
+        planned=planned([point(0.0)], [BASE_GOAL]), execute_ok=False,
+    )
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'execution_failed'
+
+    planner = FakePlanner(
+        planned=planned([point(0.0)], [BASE_GOAL]),
+        execute_error=RuntimeError('controller lost'),
+    )
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'execution_failed'
+
+
+def test_mismatched_final_pose_is_rejected():
+    wrong = down_pose(tilt=0.0, xyz=(0.42, 0.0, 1.2))
+    planner = FakePlanner(planned=planned([point(0.0)], [wrong]))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'goal_mismatch'
+    assert planner.execute_calls == []
+
+
+def test_stop_blocks_all_later_motion():
+    planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL]))
+    path = backend(planner)
+    path.stop('operator pressed stop')
+    result = path.plan_and_execute(GOAL, 'transfer')
+    assert result.success is False
+    assert result.code == 'stopped'
+    assert planner.plan_calls == []
+    path.reset_stop()
+    assert path.plan_and_execute(GOAL, 'transfer').success is True
+
+
+def test_source_freshness_is_checked_for_non_finite_values():
+    checker = MotionPolicy(LIMITS)
+    assert checker.check_sources(
+        joint_state_age=math.nan, scene_age=0.1,
+    ).ok is False
+    assert checker.check_sources(
+        joint_state_age=0.1, scene_age=-1.0,
+    ).ok is False