feat(llm-plan-executor): constrain planned trajectories by J7 and TCP tilt
This commit is contained in:
Даниил Грабарь
@@ -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
|
||||
@@ -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.
|
||||
|
||||
|
||||
@@ -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,
|
||||
},
|
||||
)
|
||||
@@ -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()
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user