diff --git a/src/iiwa_llm_plan_executor/config/motion_limits.yaml b/src/iiwa_llm_plan_executor/config/motion_limits.yaml new file mode 100644 index 0000000..893e52e --- /dev/null +++ b/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 diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index 01aab13..8f2fdc2 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/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. + 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 new file mode 100644 index 0000000..fcc9cba --- /dev/null +++ b/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, + }, + ) diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py new file mode 100644 index 0000000..d852db7 --- /dev/null +++ b/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() diff --git a/src/iiwa_llm_plan_executor/test/test_motion_policy.py b/src/iiwa_llm_plan_executor/test/test_motion_policy.py new file mode 100644 index 0000000..3c5eabd --- /dev/null +++ b/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) diff --git a/src/iiwa_llm_plan_executor/test/test_moveit_backend.py b/src/iiwa_llm_plan_executor/test/test_moveit_backend.py new file mode 100644 index 0000000..d5dc3a9 --- /dev/null +++ b/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