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

src/iiwa_llm_plan_executor/config/motion_limits.yaml

@@ -0,0 +1,25 @@
+# Hard limits for the constrained MoveItPy adapter.  They are deliberately
+# separate from the reference executor: the graph executor never falls back to
+# an unconstrained MoveToPose goal.
+schema_version: 1
+
+joint7:
+  # The neutral work pose has joint_7 = 0 rad.  The first-stage hard corridor
+  # is +/-90 degrees around it; the -180 degree IK branch is forbidden.
+  work_reference_rad: 0.0
+  corridor_rad: 1.5707963267948966
+
+tcp:
+  # The working tip points down; a working motion may tilt at most 10 degrees.
+  max_tilt_deg: 10.0
+
+trajectory:
+  # A near-pi jump indicates an IK branch change.  Reject it before execution.
+  max_joint_jump_rad: 0.6
+  max_cartesian_jump_m: 0.15
+  # The planned first point must match the measured joints used as IK seed.
+  start_tolerance_rad: 0.35
+
+freshness:
+  joint_state_max_age_sec: 1.0
+  scene_max_age_sec: 2.0

src/iiwa_llm_plan_executor/docs/interfaces.md

@@ -88,3 +88,35 @@ Important pre-movement rules:
 * Screws are handed to the virtual operator at a hole; the hole stays empty
   and no insertion/tightening occurs.
 * No task is inferred from `experiment_id`; only the initial scene is selected.
+
+## Constrained motion adapter
+
+`MotionPolicy` (`config/motion_limits.yaml`) checks every point before the
+controller sees a plan:
+
+* `joint_7` must stay inside `work_reference_rad +/- corridor_rad`
+  (0 +/- 90 deg; boundaries inclusive).
+* The tcp local +Z (working direction) must point down within
+  `max_tilt_deg` (10 deg).
+* Consecutive points may not jump more than `max_joint_jump_rad` (0.6 rad) or
+  `max_cartesian_jump_m` (0.15 m); the first point must match the measured
+  joints used as IK seed.
+* `joint_states` and the planning scene freshness are checked before planning.
+
+`MoveItPyPlanner` uses the installed MoveItPy 2.12 API:
+
+```python
+arm.set_start_state_to_current_state()          # IK seed from current joints
+arm.set_path_constraints(constraints)           # joint_7 + orientation
+arm.set_goal_state(pose_stamped_msg=..., pose_link='tcp')
+result = arm.plan(single_plan_parameters=params)
+result.trajectory.get_robot_trajectory_msg()    # every point checked
+moveit.execute(result.trajectory, controllers=[])
+```
+
+The path constraints are a `JointConstraint` on `joint_7` with
+`tolerance_below/above = corridor_rad` and an `OrientationConstraint` on
+`tcp` with the goal quaternion and 10 deg tolerance on each axis.  The planner
+never falls back to an unconstrained `MoveToPose` goal: a rejected constrained
+plan, a limit violation or an execution failure stops the backend.
+

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py

@@ -0,0 +1,258 @@
+"""Trajectory-level TCP/J7 limits applied before anything reaches the controller.
+
+The MoveToPose action cannot express a joint-7 corridor or a working tilt
+limit, so every plan produced by the planner adapter is checked here, point
+by point, before execution.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Any, Sequence
+
+import numpy as np
+import yaml
+
+from iiwa_experement.geometry import Transform
+
+
+class MotionPolicyError(ValueError):
+    """The limits file is missing or inconsistent."""
+
+
+@dataclass(frozen=True)
+class MotionLimits:
+    work_joint7_rad: float
+    joint7_corridor_rad: float
+    max_tilt_deg: float
+    max_joint_jump_rad: float
+    max_cartesian_jump_m: float
+    joint_state_max_age_sec: float
+    scene_max_age_sec: float
+    start_tolerance_rad: float = 0.35
+
+    @property
+    def joint7_min_rad(self) -> float:
+        return self.work_joint7_rad - self.joint7_corridor_rad
+
+    @property
+    def joint7_max_rad(self) -> float:
+        return self.work_joint7_rad + self.joint7_corridor_rad
+
+
+def _finite(value: Any, label: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise MotionPolicyError(f'{label} must be a finite number')
+    number = float(value)
+    if not math.isfinite(number):
+        raise MotionPolicyError(f'{label} must be a finite number')
+    return number
+
+
+def load_limits(path: Path | str) -> MotionLimits:
+    source = Path(path)
+    if not source.is_file():
+        raise FileNotFoundError(f'motion limits file does not exist: {source}')
+    raw = yaml.safe_load(source.read_text(encoding='utf-8'))
+    if not isinstance(raw, dict):
+        raise MotionPolicyError('motion limits must be a mapping')
+    if raw.get('schema_version') != 1:
+        raise MotionPolicyError('motion limits schema_version must be 1')
+    joint7 = raw.get('joint7') or {}
+    tcp = raw.get('tcp') or {}
+    trajectory = raw.get('trajectory') or {}
+    freshness = raw.get('freshness') or {}
+    corridor = _finite(joint7.get('corridor_rad'), 'joint7.corridor_rad')
+    tilt = _finite(tcp.get('max_tilt_deg'), 'tcp.max_tilt_deg')
+    jump = _finite(
+        trajectory.get('max_joint_jump_rad'), 'trajectory.max_joint_jump_rad',
+    )
+    cartesian = _finite(
+        trajectory.get('max_cartesian_jump_m'),
+        'trajectory.max_cartesian_jump_m',
+    )
+    joint_age = _finite(
+        freshness.get('joint_state_max_age_sec'),
+        'freshness.joint_state_max_age_sec',
+    )
+    scene_age = _finite(
+        freshness.get('scene_max_age_sec'), 'freshness.scene_max_age_sec',
+    )
+    if corridor <= 0 or tilt <= 0 or jump <= 0 or cartesian <= 0:
+        raise MotionPolicyError('motion limits must be positive')
+    if joint_age <= 0 or scene_age <= 0:
+        raise MotionPolicyError('freshness limits must be positive')
+    return MotionLimits(
+        work_joint7_rad=_finite(
+            joint7.get('work_reference_rad'), 'joint7.work_reference_rad',
+        ),
+        joint7_corridor_rad=corridor,
+        max_tilt_deg=tilt,
+        max_joint_jump_rad=jump,
+        max_cartesian_jump_m=cartesian,
+        joint_state_max_age_sec=joint_age,
+        scene_max_age_sec=scene_age,
+        start_tolerance_rad=_finite(
+            trajectory.get('start_tolerance_rad', 0.35),
+            'trajectory.start_tolerance_rad',
+        ),
+    )
+
+
+@dataclass(frozen=True)
+class MotionCheck:
+    ok: bool
+    reason: str = ''
+    details: dict[str, Any] = field(default_factory=dict)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {'ok': self.ok, 'reason': self.reason, 'details': dict(self.details)}
+
+
+def _tcp_tilt_deg(pose: Transform) -> float:
+    matrix = pose.matrix
+    # The working direction is the tcp local +Z axis; it must point down.
+    z_axis = matrix[:, 2]
+    dot = float(np.clip(-z_axis[2], -1.0, 1.0))
+    return math.degrees(math.acos(dot))
+
+
+class MotionPolicy:
+    def __init__(self, limits: MotionLimits) -> None:
+        self.limits = limits
+
+    @property
+    def work_joint7(self) -> float:
+        return self.limits.work_joint7_rad
+
+    def check_sources(
+        self,
+        *,
+        joint_state_age: float | None,
+        scene_age: float | None,
+    ) -> MotionCheck:
+        for name, age, limit in (
+            ('joint_states', joint_state_age, self.limits.joint_state_max_age_sec),
+            ('planning_scene', scene_age, self.limits.scene_max_age_sec),
+        ):
+            if age is None:
+                return MotionCheck(False, f'{name} freshness is unknown')
+            if not math.isfinite(float(age)) or float(age) < 0.0:
+                return MotionCheck(False, f'{name} freshness is invalid')
+            if float(age) > limit:
+                return MotionCheck(
+                    False,
+                    f'{name} is stale ({float(age):.2f} s > {limit:.2f} s)',
+                )
+        return MotionCheck(True, 'fresh')
+
+    def validate_trajectory(
+        self,
+        points: Sequence[Sequence[float]],
+        tcp_poses: Sequence[Transform],
+        current_joints: Sequence[float] | None = None,
+    ) -> MotionCheck:
+        if not points:
+            return MotionCheck(False, 'trajectory is empty')
+        if len(points) != len(tcp_poses):
+            return MotionCheck(
+                False,
+                'every trajectory point needs a matching tcp pose',
+            )
+        parsed: list[tuple[float, ...]] = []
+        for index, point in enumerate(points):
+            values = tuple(float(value) for value in point)
+            if len(values) != 7:
+                return MotionCheck(
+                    False, f'point {index} must contain 7 joint values',
+                )
+            if any(not math.isfinite(value) for value in values):
+                return MotionCheck(False, f'point {index} has a non-finite value')
+            parsed.append(values)
+
+        if current_joints is not None:
+            current = tuple(float(value) for value in current_joints)
+            if len(current) != 7:
+                return MotionCheck(False, 'current joint state must have 7 values')
+            if any(
+                abs(a - b) > self.limits.start_tolerance_rad
+                for a, b in zip(parsed[0], current)
+            ):
+                return MotionCheck(
+                    False,
+                    'trajectory does not start at the current joint state',
+                )
+
+        min_j7 = min(point[6] for point in parsed)
+        max_j7 = max(point[6] for point in parsed)
+        epsilon = 1e-9
+        if (
+            min_j7 < self.limits.joint7_min_rad - epsilon
+            or max_j7 > self.limits.joint7_max_rad + epsilon
+        ):
+            return MotionCheck(
+                False,
+                'joint7 leaves the hard corridor '
+                f'[{self.limits.joint7_min_rad:.3f}, '
+                f'{self.limits.joint7_max_rad:.3f}] rad '
+                f'(trajectory {min_j7:.3f}..{max_j7:.3f})',
+                {'min_j7': min_j7, 'max_j7': max_j7},
+            )
+
+        max_tilt = 0.0
+        for index, pose in enumerate(tcp_poses):
+            if not isinstance(pose, Transform):
+                return MotionCheck(False, f'tcp pose {index} is not a Transform')
+            tilt = _tcp_tilt_deg(pose)
+            max_tilt = max(max_tilt, tilt)
+            if tilt > self.limits.max_tilt_deg + epsilon:
+                return MotionCheck(
+                    False,
+                    f'tcp tilt at point {index} is {tilt:.2f} deg '
+                    f'(limit {self.limits.max_tilt_deg:.2f})',
+                    {'max_tilt_deg': tilt, 'point': index},
+                )
+
+        max_jump = 0.0
+        for index in range(1, len(parsed)):
+            jump = max(
+                abs(a - b) for a, b in zip(parsed[index], parsed[index - 1])
+            )
+            max_jump = max(max_jump, jump)
+            if jump > self.limits.max_joint_jump_rad + epsilon:
+                return MotionCheck(
+                    False,
+                    f'joint jump of {jump:.3f} rad before point {index} '
+                    f'(limit {self.limits.max_joint_jump_rad:.3f})',
+                    {'max_jump_rad': jump, 'point': index},
+                )
+
+        max_step = 0.0
+        for index in range(1, len(tcp_poses)):
+            step = float(
+                np.linalg.norm(
+                    tcp_poses[index].position - tcp_poses[index - 1].position
+                )
+            )
+            max_step = max(max_step, step)
+            if step > self.limits.max_cartesian_jump_m + epsilon:
+                return MotionCheck(
+                    False,
+                    f'cartesian jump of {step:.3f} m before point {index} '
+                    f'(limit {self.limits.max_cartesian_jump_m:.3f})',
+                    {'max_cartesian_step_m': step, 'point': index},
+                )
+
+        return MotionCheck(
+            True,
+            'trajectory accepted',
+            {
+                'points': len(parsed),
+                'min_j7': min_j7,
+                'max_j7': max_j7,
+                'max_tilt_deg': max_tilt,
+                'max_joint_jump_rad': max_jump,
+                'max_cartesian_step_m': max_step,
+            },
+        )

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py

@@ -0,0 +1,430 @@
+"""Constrained MoveItPy adapter used by every graph skill.
+
+The adapter differs from the existing ``MoveToPose`` action on purpose: it
+seeds IK from the measured current joints, sets a joint-7 corridor and a
+working-orientation path constraint, checks the *whole* planned trajectory
+with :class:`MotionPolicy` and only then asks the trajectory execution manager
+to run it.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import Any, Callable
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import GeometrySpec, Transform
+
+from .motion_policy import MotionCheck, MotionPolicy
+
+
+class PlannerError(RuntimeError):
+    """The MoveIt plan could not be produced or executed."""
+
+
+@dataclass
+class PlannedMotion:
+    points: list[tuple[float, ...]]
+    tcp_poses: list[Transform]
+    current_joints: tuple[float, ...] | None = None
+    handle: Any = None
+    details: dict[str, Any] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class MotionOutcome:
+    success: bool
+    code: str
+    message: str
+    details: dict[str, Any] = field(default_factory=dict)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'success': self.success,
+            'code': self.code,
+            'message': self.message,
+            'details': dict(self.details),
+        }
+
+
+_POSE_TO_MATRIX = None
+
+
+def _pose_message(transform: Transform, frame_id: str):
+    from geometry_msgs.msg import Pose, PoseStamped
+
+    quaternion = Rotation.from_matrix(transform.matrix).as_quat()
+    pose = Pose()
+    pose.position.x, pose.position.y, pose.position.z = (
+        float(transform.xyz[0]),
+        float(transform.xyz[1]),
+        float(transform.xyz[2]),
+    )
+    pose.orientation.x = float(quaternion[0])
+    pose.orientation.y = float(quaternion[1])
+    pose.orientation.z = float(quaternion[2])
+    pose.orientation.w = float(quaternion[3])
+    stamped = PoseStamped()
+    stamped.header.frame_id = frame_id
+    stamped.pose = pose
+    return stamped
+
+
+def _matrix_transform(matrix, parent: str, child: str) -> Transform:
+    values = np.asarray(matrix, dtype=float)
+    return Transform(
+        xyz=tuple(float(value) for value in values[:3, 3]),
+        rotation=tuple(
+            tuple(float(value) for value in row)
+            for row in values[:3, :3]
+        ),
+        parent=parent,
+        child=child,
+    )
+
+
+class MoveItPyPlanner:
+    """Thin wrapper around MoveItPy with the constrained plan request."""
+
+    PHASES = {
+        'transfer': ('pilz_industrial_motion_planner', 'PTP', 0.35, 2.0),
+        'approach': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
+        'retreat': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
+        'observe': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
+        'contact': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
+        'lower': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
+        'align': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
+        'dispense': ('pilz_industrial_motion_planner', 'LIN', 0.09, 2.0),
+        'escape': ('ompl', 'RRTConnectkConfigDefault', 0.20, 10.0),
+    }
+
+    def __init__(
+        self,
+        geometry: GeometrySpec,
+        policy: MotionPolicy,
+        *,
+        planning_group: str = 'iiwa_arm',
+        pose_link: str = 'tcp',
+        pose_frame: str = 'base_link',
+        moveit_factory: Callable[[], Any] | None = None,
+    ) -> None:
+        self.geometry = geometry
+        self.policy = policy
+        self.planning_group = planning_group
+        self.pose_link = pose_link
+        self.pose_frame = pose_frame
+        self._moveit_factory = moveit_factory
+        self._moveit = None
+        self._arm = None
+        self._model = None
+        self._group = None
+        self._stopped = False
+
+    # ------------------------------------------------------------------ setup
+    def _ensure(self) -> None:
+        if self._arm is not None:
+            return
+        try:
+            if self._moveit_factory is not None:
+                self._moveit = self._moveit_factory()
+            else:
+                from moveit.planning import MoveItPy
+
+                self._moveit = MoveItPy(node_name='iiwa_llm_plan_executor_moveit')
+            self._model = self._moveit.get_robot_model()
+            self._arm = self._moveit.get_planning_component(self.planning_group)
+            self._group = self._model.get_joint_model_group(self.planning_group)
+        except Exception as error:
+            raise PlannerError(f'MoveItPy is unavailable: {error}') from error
+
+    def _goal_in_pose_frame(self, goal: Transform) -> Transform:
+        if goal.parent == self.pose_frame:
+            return goal
+        if goal.parent == 'world':
+            return self.geometry.world_to_base.inverse().compose_relaxed(goal)
+        raise PlannerError(
+            f'goal frame {goal.parent!r} is not world or {self.pose_frame!r}'
+        )
+
+    def _constraints(self, goal: Transform):
+        from moveit_msgs.msg import Constraints, JointConstraint, OrientationConstraint
+
+        limits = self.policy.limits
+        constraints = Constraints()
+        joint = JointConstraint()
+        joint.joint_name = 'joint_7'
+        joint.position = float(limits.work_joint7_rad)
+        joint.tolerance_above = float(limits.joint7_corridor_rad)
+        joint.tolerance_below = float(limits.joint7_corridor_rad)
+        joint.weight = 1.0
+        constraints.joint_constraints.append(joint)
+
+        # The model uses the same joint order as the Webots controller.  With
+        # a hard joint-7 corridor, this constraint prevents the IK branch that
+        # would flip joint 7 by about -180 degrees.
+        orientation = OrientationConstraint()
+        orientation.header.frame_id = self.pose_frame
+        orientation.link_name = self.pose_link
+        quaternion = Rotation.from_matrix(goal.matrix).as_quat()
+        orientation.orientation.x = float(quaternion[0])
+        orientation.orientation.y = float(quaternion[1])
+        orientation.orientation.z = float(quaternion[2])
+        orientation.orientation.w = float(quaternion[3])
+        tilt = math.radians(float(limits.max_tilt_deg))
+        orientation.absolute_x_axis_tolerance = tilt
+        orientation.absolute_y_axis_tolerance = tilt
+        orientation.absolute_z_axis_tolerance = tilt
+        orientation.weight = 1.0
+        constraints.orientation_constraints.append(orientation)
+        return constraints
+
+    def _parameters(self, phase: str):
+        from moveit.planning import PlanRequestParameters
+
+        pipeline, planner_id, speed, plan_time = self.PHASES.get(
+            phase, self.PHASES['transfer'],
+        )
+        parameters = PlanRequestParameters(self._moveit, self.planning_group)
+        parameters.planning_pipeline = pipeline
+        parameters.planner_id = planner_id
+        parameters.planning_time = plan_time
+        parameters.planning_attempts = 3
+        parameters.max_velocity_scaling_factor = speed
+        parameters.max_acceleration_scaling_factor = speed
+        return parameters
+
+    # ------------------------------------------------------------------ plan
+    def plan(
+        self,
+        goal: Transform,
+        phase: str = 'transfer',
+        held_object: str | None = None,
+    ) -> PlannedMotion:
+        del held_object  # attached geometry is maintained by the scene bridge
+        self._ensure()
+        if self._stopped:
+            raise PlannerError('planner is stopped')
+        base_goal = self._goal_in_pose_frame(goal)
+        try:
+            self._arm.set_start_state_to_current_state()
+            start_state = self._arm.get_start_state()
+            current = tuple(
+                float(value)
+                for value in start_state.get_joint_group_positions(
+                    self.planning_group,
+                )
+            )
+            self._arm.set_path_constraints(self._constraints(base_goal))
+            self._arm.set_goal_state(
+                pose_stamped_msg=_pose_message(base_goal, self.pose_frame),
+                pose_link=self.pose_link,
+            )
+            result = self._arm.plan(
+                single_plan_parameters=self._parameters(phase),
+            )
+        except PlannerError:
+            raise
+        except Exception as error:
+            raise PlannerError(f'constrained planning failed: {error}') from error
+        if not result or result.trajectory is None:
+            raise PlannerError(
+                f'no constrained trajectory for phase {phase!r} '
+                f'(joint7 corridor {self.policy.limits.joint7_min_rad:.3f}..'
+                f'{self.policy.limits.joint7_max_rad:.3f} rad)'
+            )
+        message = result.trajectory.get_robot_trajectory_msg()
+        names = list(message.joint_trajectory.joint_names)
+        expected = list(self._group.active_joint_model_names)
+        if set(expected) - set(names):
+            raise PlannerError(
+                f'planned trajectory misses joints {sorted(set(expected) - set(names))}'
+            )
+        index = {name: position for position, name in enumerate(names)}
+        points: list[tuple[float, ...]] = []
+        poses: list[Transform] = []
+        from moveit.core.robot_state import RobotState
+
+        state = RobotState(self._model)
+        for waypoint in message.joint_trajectory.points:
+            ordered = tuple(
+                float(waypoint.positions[index[name]]) for name in expected
+            )
+            points.append(ordered)
+            state.set_joint_group_positions(self.planning_group, ordered)
+            state.update()
+            matrix = state.get_global_link_transform(self.pose_link)
+            poses.append(_matrix_transform(matrix, self.pose_frame, self.pose_link))
+        return PlannedMotion(
+            points=points,
+            tcp_poses=poses,
+            current_joints=current,
+            handle=result,
+            details={
+                'phase': phase,
+                'planner': self.PHASES.get(phase, self.PHASES['transfer'])[:2],
+            },
+        )
+
+    def execute(self, planned: PlannedMotion) -> bool:
+        self._ensure()
+        try:
+            execution = self._moveit.execute(
+                planned.handle.trajectory, controllers=[],
+            )
+            return bool(execution)
+        except Exception as error:
+            raise PlannerError(f'trajectory execution failed: {error}') from error
+
+    def stop(self) -> None:
+        self._stopped = True
+        if self._moveit is None:
+            return
+        try:
+            self._moveit.get_trajectory_execution_manager().stop_execution()
+        except Exception:
+            pass
+
+    def reset_stop(self) -> None:
+        self._stopped = False
+
+
+class MoveItBackend:
+    """The single execution path used by every skill."""
+
+    def __init__(
+        self,
+        geometry: GeometrySpec,
+        policy: MotionPolicy,
+        *,
+        planner: Any | None = None,
+        freshness_guard: Callable[[], tuple[bool, str]] | None = None,
+        scene_guard: Callable[[], tuple[bool, str]] | None = None,
+        current_joints: Callable[[], tuple[float, ...] | None] | None = None,
+        on_event: Callable[[dict[str, Any]], None] | None = None,
+    ) -> None:
+        self.geometry = geometry
+        self.policy = policy
+        self.planner = planner or MoveItPyPlanner(geometry, policy)
+        self._freshness_guard = freshness_guard
+        self._scene_guard = scene_guard
+        self._current_joints = current_joints
+        self._on_event = on_event
+        self.stopped = False
+        self.stop_reason: str | None = None
+
+    def _emit(self, **payload) -> None:
+        if self._on_event is not None:
+            self._on_event(payload)
+
+    def _guard(self) -> MotionOutcome | None:
+        if self.stopped:
+            return MotionOutcome(
+                False, 'stopped',
+                f'motion is blocked after stop: {self.stop_reason}',
+            )
+        for name, guard in (
+            ('joint_states', self._freshness_guard),
+            ('planning_scene', self._scene_guard),
+        ):
+            if guard is None:
+                continue
+            try:
+                ok, message = guard()
+            except Exception as error:
+                return self._fail(f'{name} guard failed: {error}', 'stale_state')
+            if not ok:
+                return self._fail(message, 'stale_state')
+        return None
+
+    def _fail(self, message: str, code: str = 'failed', **details) -> MotionOutcome:
+        self.stop(message)
+        self._emit(event='motion_failed', code=code, message=message, **details)
+        return MotionOutcome(False, code, message, details)
+
+    def plan_and_execute(
+        self,
+        goal: Transform,
+        phase: str = 'transfer',
+        held_object: str | None = None,
+    ) -> MotionOutcome:
+        blocked = self._guard()
+        if blocked is not None:
+            return blocked
+        try:
+            planned = self.planner.plan(goal, phase, held_object)
+        except Exception as error:
+            return self._fail(str(error), 'planning_failed')
+        current = None
+        if self._current_joints is not None:
+            try:
+                current = self._current_joints()
+            except Exception:
+                current = None
+        if current is None:
+            current = planned.current_joints
+        check: MotionCheck = self.policy.validate_trajectory(
+            planned.points, planned.tcp_poses, current,
+        )
+        if not check.ok:
+            try:
+                self.planner.stop()
+            except Exception:
+                pass
+            return self._fail(
+                f'motion policy rejected the trajectory: {check.reason}',
+                'motion_limit',
+                **check.details,
+            )
+        target = (
+            goal
+            if goal.parent == 'base_link'
+            else self.geometry.world_to_base.inverse().compose_relaxed(goal)
+        )
+        final = planned.tcp_poses[-1]
+        distance = float(np.linalg.norm(final.position - target.position))
+        cosine = (np.trace(final.matrix.T @ target.matrix) - 1.0) / 2.0
+        angle = float(math.acos(max(-1.0, min(1.0, cosine))))
+        if (
+            distance > self.geometry.position_tolerance
+            or angle > self.geometry.orientation_tolerance_rad
+        ):
+            return self._fail(
+                'planned final tcp is outside the goal tolerance '
+                f'({distance:.4f} m, {angle:.4f} rad)',
+                'goal_mismatch',
+            )
+        self._emit(
+            event='motion_planned', phase=phase, held_object=held_object,
+            **check.details,
+        )
+        try:
+            executed = self.planner.execute(planned)
+        except Exception as error:
+            return self._fail(str(error), 'execution_failed')
+        if not executed:
+            return self._fail('trajectory execution did not succeed', 'execution_failed')
+        self._emit(event='motion_executed', phase=phase, **check.details)
+        return MotionOutcome(
+            True,
+            'ok',
+            'motion executed',
+            {'phase': phase, **check.details},
+        )
+
+    def stop(self, reason: str = 'stop requested') -> None:
+        self.stopped = True
+        if self.stop_reason is None:
+            self.stop_reason = reason
+        try:
+            self.planner.stop()
+        except Exception:
+            pass
+
+    def reset_stop(self) -> None:
+        self.stopped = False
+        self.stop_reason = None
+        reset = getattr(self.planner, 'reset_stop', None)
+        if reset is not None:
+            reset()

src/iiwa_llm_plan_executor/test/test_motion_policy.py

@@ -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)

src/iiwa_llm_plan_executor/test/test_moveit_backend.py

@@ -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