diff --git a/src/iiwa_config/config/moveit/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf index 5379f3c..1015988 100644 --- a/src/iiwa_config/config/moveit/iiwa7.srdf +++ b/src/iiwa_config/config/moveit/iiwa7.srdf @@ -37,66 +37,16 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/src/iiwa_config/config/tools.yaml b/src/iiwa_config/config/tools.yaml index c1d7a36..353a79e 100644 --- a/src/iiwa_config/config/tools.yaml +++ b/src/iiwa_config/config/tools.yaml @@ -24,40 +24,10 @@ tools: tip_link: "patron" tcp_link: "tcp" collisions: - - [link1, patron, Never] - - [link2, patron, Never] - - [link3, patron, Never] - - [link4, patron, Never] - - [link5, patron, Never] - - [link6, patron, Never] - - [link7, patron, Adjacent] + # Only fixed-joint or constructive camera-assembly contact pairs are + # disabled. Every camera-to-robot pair must be checked by MoveIt. - [link7, camera_holder, Adjacent] - - [link6, camera_holder, Never] - - [link5, camera_holder, Never] - - [link4, camera_holder, Never] - - [link3, camera_holder, Never] - - [link2, camera_holder, Never] - - [link1, camera_holder, Never] - - [base_link, camera_holder, Never] - [camera_holder, patron, Adjacent] - [camera_holder, camera_corner, Adjacent] - - [camera_corner, patron, Never] - - [camera_corner, link7, Never] - - [camera_corner, link6, Never] - - [camera_corner, link5, Never] - - [camera_corner, link4, Never] - - [camera_corner, link3, Never] - - [camera_corner, link2, Never] - - [camera_corner, link1, Never] - - [camera_corner, base_link, Never] - [camera_corner, camera_hand, Adjacent] - - [camera_hand, camera_holder, Never] - - [camera_hand, patron, Never] - - [camera_hand, link7, Never] - - [camera_hand, link6, Never] - - [camera_hand, link5, Never] - - [camera_hand, link4, Never] - - [camera_hand, link3, Never] - - [camera_hand, link2, Never] - - [camera_hand, link1, Never] - - [camera_hand, base_link, Never] + - [camera_holder, camera_hand, Adjacent] diff --git a/src/iiwa_description/urdf/tools/patron.xacro b/src/iiwa_description/urdf/tools/patron.xacro index 312f8a4..08a8c60 100644 --- a/src/iiwa_description/urdf/tools/patron.xacro +++ b/src/iiwa_description/urdf/tools/patron.xacro @@ -86,9 +86,9 @@ - + - + diff --git a/src/iiwa_llm_plan_executor/config/motion_limits.yaml b/src/iiwa_llm_plan_executor/config/motion_limits.yaml index 893e52e..fd6f194 100644 --- a/src/iiwa_llm_plan_executor/config/motion_limits.yaml +++ b/src/iiwa_llm_plan_executor/config/motion_limits.yaml @@ -4,8 +4,8 @@ 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. + # Preferred value is the neutral work pose (joint_7 = 0). This is a + # preference only; the real per-joint limits below remain authoritative. work_reference_rad: 0.0 corridor_rad: 1.5707963267948966 @@ -19,7 +19,14 @@ trajectory: max_cartesian_jump_m: 0.15 # The planned first point must match the measured joints used as IK seed. start_tolerance_rad: 0.35 + # Dense joint-space resampling used for collision/orientation validation. + max_sample_step_rad: 0.01 freshness: joint_state_max_age_sec: 1.0 scene_max_age_sec: 2.0 + +collision: + # Minimum accepted collision distance (metres) for every sampled state. + # Camera-vs-robot pairs are enabled in the generated SRDF. + min_clearance_m: 0.003 diff --git a/src/iiwa_llm_plan_executor/doc/README.md b/src/iiwa_llm_plan_executor/doc/README.md index 464bbbd..22500c8 100644 --- a/src/iiwa_llm_plan_executor/doc/README.md +++ b/src/iiwa_llm_plan_executor/doc/README.md @@ -94,12 +94,14 @@ The JSONL log with every event is written to ## Safety scope -The executor checks J7 (±90° around `work`) and the downward TCP tilt (≤10°) -on **every** planned point, seeds IK from current joints, enforces these as -MoveIt path constraints, checks scene/joint freshness and verifies the final -pose. The only logged exception is the initial empty-gripper reorientation -from home to the downward `work` posture; J7 and the final orientation are -still enforced and a reorientation with a held object is rejected. The -executor never falls back to an unconstrained `MoveToPose` goal. This is a -Webots verification, not a physical-robot validation and not a Qwen3 quality -evaluation. +The executor uses Pilz PTP for free-space working-point moves and Pilz LIN for +axial contact/retreat and the dispense contour. Every plan is densely +resampled and checked for real joint limits, self-collision (including the +camera assembly), scene collision, camera clearance, downward TCP tilt (≤10°) +and joint jumps before execution. J7 may use its full safe joint range; the +`work` value is only a preference. A lateral PTP from a contact pose is +preceded by an axial LIN retreat. The only logged orientation exception is +the initial empty-gripper reorientation from home to the downward `work` +posture. The executor never falls back to an unconstrained `MoveToPose` goal. +This is a Webots verification, not a physical-robot validation and not a +Qwen3 quality evaluation. diff --git a/src/iiwa_llm_plan_executor/doc/webots-qa.md b/src/iiwa_llm_plan_executor/doc/webots-qa.md index e9cef77..af6082a 100644 --- a/src/iiwa_llm_plan_executor/doc/webots-qa.md +++ b/src/iiwa_llm_plan_executor/doc/webots-qa.md @@ -17,8 +17,11 @@ colcon test --base-paths src --packages-select \ colcon test-result --verbose ``` -Result: build of 10 packages succeeded; `361 tests, 0 errors, 0 failures, -0 skipped`. +Result: build of 10 packages succeeded; `365 tests, 0 errors, 0 failures, +0 skipped` for the four runtime packages. The new camera/tool geometry tests +in `iiwa_utils` pass separately (`7 passed`); four pre-existing +`test_body_hatch_worlds.py` cases still fail because their `.wbt`/`.proto` +files are absent from this checkout. `git diff --check` is clean. @@ -166,3 +169,48 @@ recovery issue: `grasp_failed` leads through `inspect_state: holding` to validator therefore rejects that recovery branch with its own ownership reason (`a_retry_pick ... while holding 'cap'`); this does not repair the model graph. Completion must not be claimed for this graph. + +## Motion planning fix (PTP/LIN, camera collision model) + +### Reproduction before the change + +The failing `approach(cap)` request was captured with diagnostics: + +```text +phase=approach +effective_phase=transfer_lin +pipeline=pilz_industrial_motion_planner +planner_id=LIN +goal_xyz=(0.6, -0.22, 0.206667) # base_link +goal_quat=(-0.382683, 0.92388, 0, 0) +start=(~0, ~0, ~0, ~0, ~0, ~0, ~0) +MoveIt error: NO_IK_SOLUTION +``` + +All three attempts failed in IK. The old error text about the J7 corridor was +therefore misleading: the failing planner was LIN and the actual cause was +`NO_IK_SOLUTION`, not a joint-7 boundary. + +### After the change + +* `detect(cap) → approach(cap)` in Webots: both nodes succeeded. Transfers + and approaches used `pilz_industrial_motion_planner/PTP`; the measured + minimum sampled clearance was **0.0288 m** (required 0.003 m), maximum J7 + was 0.875 rad. +* A separate J7-rotation probe moved the arm to the `work` posture with + `joint7 = 2.0 rad` and then executed `approach(cap)`. The accepted sampled + trajectory covered `joint7 = -1.225 … 2.000 rad`, i.e. well beyond the old + ±90 deg boundary, with no joint-limit, self-collision or clearance failure; + measured clearance was again **0.0288 m**. +* Camera geometry: `camera_corner` and `camera_hand` keep at least **35 mm** + AABB clearance to `link6`/`link7` at `home` and `work`. The D455 collision + box now matches its visible bounds. Only constructive fixed contact pairs + are disabled in `tools.yaml` and the regenerated SRDF; all camera-to-robot + pairs remain checked. +* Tests: `pytest src/iiwa_llm_plan_executor/test` → **122 passed**; camera / + tool collision + TCP tests in `iiwa_utils` → **7 passed**. The existing + unrelated + `test_body_hatch_worlds.py` still fails because its world files are absent + from this checkout; it is not part of the motion change. + +Physical robot was not tested. diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index 2fa11a1..6844bfe 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/src/iiwa_llm_plan_executor/docs/interfaces.md @@ -160,22 +160,41 @@ is required; RGB/D is used for liveness only. ## Constrained motion adapter -`MotionPolicy` (`config/motion_limits.yaml`) checks every point before the -controller sees a plan: +`MotionPolicy` (`config/motion_limits.yaml`) checks every sampled point before +the controller sees a plan: -* `joint_7` must stay inside `work_reference_rad +/- corridor_rad` - (0 +/- 90 deg; boundaries inclusive). +* Every joint stays inside the real per-joint position limits + (`iiwa_config/config/moveit/joint_limits.yaml`, soft limits included). + Joint 7 keeps the neutral `work` value as a documented preference, but the + former +/-90 deg hard corridor is no longer a rejection boundary; safe paths + may use the full joint range. * 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. +* The planned trajectory is resampled in joint space at + `trajectory.max_sample_step_rad` (0.01 rad). Every sample is checked for + joint limits, self-collision, scene collision, camera clearance and tcp + orientation before execution. * `joint_states` and the planning scene freshness are checked before planning. +Motion selection: + +| Purpose | Motion | +| --- | --- | +| between `work`/`home`, to an object, tool or placement point, part transfer, safe alignment | Pilz PTP | +| axial contact, axial lift/retreat, dispense contour | Pilz LIN | + +If a lateral PTP would start in a contact pose, the skill first performs the +axial LIN retreat to the saved approach height. The `dispense` contour itself +stays a continuous LIN contour; only its approach and retreat use PTP/axial +rules. + One explicit exception is recorded: if the measured start posture is not yet the downward working posture (the calibrated home pose points the tip up) and the -gripper is empty, the first reorientation to `work` is allowed to pass through -larger tilt angles. Joint 7, joint jumps, Cartesian jumps and the final +gripper is empty, the first reorientation to `work` may pass through larger +tilt angles. Joint limits, joint jumps, Cartesian jumps and the final downward orientation are still checked, and the event records `reorientation: true`. A reorientation with a held object is always rejected. @@ -183,26 +202,32 @@ downward orientation are still checked, and the event records ```python arm.set_start_state(robot_state=clamped_robot_state) # measured joints as IK seed -arm.set_path_constraints(constraints) # hard joint_7 corridor +arm.set_path_constraints(empty_constraints) # no artificial J7 corridor 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 +result.trajectory.get_robot_trajectory_msg() +# resample + joint/self/world/camera checks, then: moveit.execute(result.trajectory, controllers=[]) ``` -The path constraint is a `JointConstraint` on `joint7` with -`tolerance_below/above = corridor_rad`. The planner applies it on OMPL -transfers. In this MoveIt/Pilz installation the orientation path constraint is -not honored reliably, so the 10 deg working-tilt limit is enforced **after** -planning by `MotionPolicy` on every returned point; the planner also gets a -wider internal orientation tolerance for its goal sampling. Tool transits use -OMPL with the J7 corridor, part transits and approaches use Cartesian LIN, and -short contact moves use LIN. A plan that cannot be produced or that violates -the point-wise checks is stopped; there is no unconstrained `MoveToPose` -fallback. +The planner never falls back to an unconstrained `MoveToPose` action. If a +plan is missing, the status names the phase, pipeline, planner id, target tcp +pose, start joints and MoveIt error code (for example `NO_IK_SOLUTION`). If +our validation rejects a plan, the status names the real cause: joint limit, +jump, orientation or the colliding link pair with the measured minimum +distance and required clearance. The measured start state is clamped to the same soft joint limits used by MoveIt (`soft_min_position`/`soft_max_position`) before it becomes the IK seed. Without that clamp a Webots-controller overshoot of ~0.00003 rad makes `CheckStartStateBounds` abort every following plan. +Camera collision model: the generated SRDF disables only constructive +contact pairs (`link7-camera_holder`, `camera_holder-patron`, +`camera_holder-camera_corner`, `camera_corner-camera_hand`, +`camera_holder-camera_hand`). Every camera-to-robot pair, including +`camera_hand-link6` and `camera_corner-link6`, remains checked. The D455 +collision box matches its visible bounds; the measured AABB clearance from the +camera corner/hand to link6/link7 is at least 35 mm at `home` and `work`, and +the planner requires a non-zero minimum sampled clearance of 3 mm. + diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py index 4c6de51..ffe64c1 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py @@ -1,8 +1,8 @@ -"""Trajectory-level TCP/J7 limits applied before anything reaches the controller. +"""Trajectory-level checks 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. +Every PTP/LIN plan is resampled and checked for joint limits, self/world +collision, working orientation and joint jumps. The old hard +/-90 deg joint-7 +corridor is only a preference; real joint limits remain authoritative. """ from __future__ import annotations @@ -31,9 +31,14 @@ class MotionLimits: joint_state_max_age_sec: float scene_max_age_sec: float start_tolerance_rad: float = 0.35 + min_clearance_m: float = 0.003 + max_sample_step_rad: float = 0.01 @property def joint7_min_rad(self) -> float: + # Kept for diagnostics/compatibility only. It is no longer a hard + # rejection boundary: safe paths may leave the preferred +/-90 deg + # band while remaining inside the real joint limits. return self.work_joint7_rad - self.joint7_corridor_rad @property @@ -79,10 +84,23 @@ def load_limits(path: Path | str) -> MotionLimits: scene_age = _finite( freshness.get('scene_max_age_sec'), 'freshness.scene_max_age_sec', ) + collision = raw.get('collision') or {} + min_clearance = _finite( + collision.get('min_clearance_m', 0.003), + 'collision.min_clearance_m', + ) 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') + if min_clearance < 0: + raise MotionPolicyError('collision.min_clearance_m must be non-negative') + sample_step = _finite( + trajectory.get('max_sample_step_rad', 0.01), + 'trajectory.max_sample_step_rad', + ) + if sample_step <= 0: + raise MotionPolicyError('trajectory.max_sample_step_rad must be positive') return MotionLimits( work_joint7_rad=_finite( joint7.get('work_reference_rad'), 'joint7.work_reference_rad', @@ -97,6 +115,8 @@ def load_limits(path: Path | str) -> MotionLimits: trajectory.get('start_tolerance_rad', 0.35), 'trajectory.start_tolerance_rad', ), + min_clearance_m=min_clearance, + max_sample_step_rad=sample_step, ) @@ -119,8 +139,16 @@ def tcp_tilt_deg(pose: Transform) -> float: class MotionPolicy: - def __init__(self, limits: MotionLimits) -> None: + def __init__( + self, + limits: MotionLimits, + joint_limits: dict[str, tuple[float, float]] | None = None, + ) -> None: self.limits = limits + self.joint_limits = dict(joint_limits or {}) + self.joint_names = tuple( + f'joint{index}' for index in range(1, 8) + ) @property def work_joint7(self) -> float: @@ -189,19 +217,29 @@ class MotionPolicy: min_j7 = min(point[6] for point in parsed) max_j7 = max(point[6] for point in parsed) + j7_deviation = max( + abs(value - self.limits.work_joint7_rad) for value in (min_j7, max_j7) + ) 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}, - ) + for index, values in enumerate(parsed): + for joint_index, value in enumerate(values): + name = self.joint_names[joint_index] + bounds = self.joint_limits.get(name) + if bounds is None: + continue + low, high = float(bounds[0]), float(bounds[1]) + if value < low - epsilon or value > high + epsilon: + return MotionCheck( + False, + f'{name} at point {index} is {value:.4f} rad, ' + f'outside the joint limit [{low:.4f}, {high:.4f}]', + { + 'joint': name, + 'value': value, + 'limit': [low, high], + 'point': index, + }, + ) max_tilt = 0.0 start_tilt = ( @@ -281,6 +319,8 @@ class MotionPolicy: 'points': len(parsed), 'min_j7': min_j7, 'max_j7': max_j7, + 'max_j7_deviation_rad': j7_deviation, + 'preferred_j7_rad': self.limits.work_joint7_rad, 'max_tilt_deg': max_tilt, 'reorientation': reorientation, 'max_joint_jump_rad': max_jump, 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 index 3a5aac1..a9ad9eb 100644 --- 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 @@ -86,26 +86,49 @@ def _matrix_transform(matrix, parent: str, child: str) -> Transform: ) +def _error_code_name(value: int | None) -> str: + if value is None: + return 'unknown' + from moveit_msgs.msg import MoveItErrorCodes + + for name in dir(MoveItErrorCodes): + if name.isupper() and getattr(MoveItErrorCodes, name) == value: + return f'{name}({value})' + return f'code {value}' + + +def _format_contacts(result: Any) -> str: + contacts = getattr(result, 'contacts', None) + pairs: list[str] = [] + try: + for key in contacts.keys(): + pairs.append(str(key)) + except Exception: + try: + for item in contacts: + pairs.append(str(item)) + except Exception: + pass + return ', '.join(sorted(set(pairs))) if pairs else 'unknown pair' + + class MoveItPyPlanner: """Thin wrapper around MoveItPy with the constrained plan request.""" PHASES = { - # Pilz does not honor MoveIt path constraints, while ValidateSolution - # rejects trajectories that leave the joint-7 corridor. Transfers and - # approaches therefore use the constraint-aware OMPL pipeline. - 'transfer': ('ompl', 'RRTConnectkConfigDefault', 0.20, 10.0), - 'approach': ('ompl', 'RRTConnectkConfigDefault', 0.18, 10.0), - 'retreat': ('pilz_industrial_motion_planner', 'LIN', 0.18, 2.0), - 'observe': ('ompl', 'RRTConnectkConfigDefault', 0.18, 10.0), - 'ptp_transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0), - 'transfer_lin': ('pilz_industrial_motion_planner', 'LIN', 0.15, 2.0), - # Short axial segments keep the working orientation; LIN remains the - # deterministic choice and is still checked point by point. + # PTP: free-space changes between working points, objects, tools, + # placement points and safe alignments. No J7 path constraint; the + # full sampled trajectory is checked afterwards. + 'transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0), + 'approach': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0), + 'observe': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0), + 'align': ('pilz_industrial_motion_planner', 'PTP', 0.10, 2.0), + 'escape': ('pilz_industrial_motion_planner', 'PTP', 0.15, 2.0), + # LIN: axial contact/retreat and the continuous dispense contour. '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), + 'retreat': ('pilz_industrial_motion_planner', 'LIN', 0.12, 2.0), 'dispense': ('pilz_industrial_motion_planner', 'LIN', 0.09, 2.0), - 'escape': ('ompl', 'RRTConnectkConfigDefault', 0.20, 10.0), } def __init__( @@ -130,6 +153,7 @@ class MoveItPyPlanner: self._arm = None self._model = None self._group = None + self._scene_monitor = None self._stopped = False # ------------------------------------------------------------------ setup @@ -149,6 +173,7 @@ class MoveItPyPlanner: 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) + self._scene_monitor = self._moveit.get_planning_scene_monitor() except Exception as error: raise PlannerError(f'MoveItPy is unavailable: {error}') from error @@ -162,41 +187,10 @@ class MoveItPyPlanner: ) def _constraints(self, goal: Transform, *, include_orientation: bool = True): - from moveit_msgs.msg import Constraints, JointConstraint, OrientationConstraint + del goal, include_orientation + from moveit_msgs.msg import Constraints - limits = self.policy.limits - constraints = Constraints() - joint = JointConstraint() - joint.joint_name = 'joint7' - 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. - if not include_orientation: - return constraints - 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]) - # Keep the planner inside a stricter band than the final policy check - # (10 deg). OMPL interpolation can otherwise drift a fraction of a - # degree above the policy boundary and reject an otherwise valid path. - tilt = math.radians(float(limits.max_tilt_deg) * 2.0) - 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 + return Constraints() def _parameters(self, phase: str): from moveit.planning import PlanRequestParameters @@ -224,6 +218,13 @@ class MoveItPyPlanner: if self._stopped: raise PlannerError('planner is stopped') base_goal = self._goal_in_pose_frame(goal) + effective_phase = phase + print( + '[iiwa_llm_plan_executor] plan request ' + f'phase={phase!r} goal_xyz={tuple(round(float(v), 6) for v in base_goal.xyz)} ' + f'goal_quat={tuple(round(float(v), 6) for v in Rotation.from_matrix(base_goal.matrix).as_quat())}', + flush=True, + ) try: self._arm.set_start_state_to_current_state() start_state = self._arm.get_start_state() @@ -266,6 +267,12 @@ class MoveItPyPlanner: self._arm.set_start_state(robot_state=clamped_state) start_state = self._arm.get_start_state() current = tuple(clamped) + print( + '[iiwa_llm_plan_executor] plan start ' + f'measured={tuple(round(float(v), 6) for v in measured)} ' + f'clamped={tuple(round(float(v), 6) for v in current)}', + flush=True, + ) start_matrix = start_state.get_global_link_transform(self.pose_link) start_tcp = _matrix_transform( start_matrix, self.pose_frame, self.pose_link, @@ -275,22 +282,20 @@ class MoveItPyPlanner: and held_object is None ) effective_phase = phase - if phase == 'approach': - # Short approach segments are Cartesian and keep the tip down. - effective_phase = 'transfer_lin' - # Tool transfers use OMPL with the J7 corridor; part transfers - # use the explicit transfer_lin phase from the skill. Working - # tilt is always checked point by point by MotionPolicy. - if effective_phase in ( - 'contact', 'lower', 'align', 'dispense', 'retreat', - 'transfer_lin', - ): - from moveit_msgs.msg import Constraints - self._arm.set_path_constraints(Constraints()) - else: - self._arm.set_path_constraints(self._constraints( - base_goal, include_orientation=False, - )) + # J7 has no artificial +/-90 deg corridor and PTP/LIN goal IK is + # allowed to use the whole joint range. Safety comes from the + # dense post-plan joint/self/world/orientation checks below. + from moveit_msgs.msg import Constraints + self._arm.set_path_constraints(Constraints()) + phase_params = self.PHASES.get( + effective_phase, self.PHASES['transfer'], + ) + print( + '[iiwa_llm_plan_executor] plan pipeline ' + f'phase={phase!r} pipeline={phase_params[0]!r} ' + f'planner_id={phase_params[1]!r} planning_time={phase_params[3]}', + flush=True, + ) self._arm.set_goal_state( pose_stamped_msg=_pose_message(base_goal, self.pose_frame), pose_link=self.pose_link, @@ -301,12 +306,30 @@ class MoveItPyPlanner: except PlannerError: raise except Exception as error: + print( + '[iiwa_llm_plan_executor] plan exception ' + f'phase={phase!r} effective_phase={effective_phase!r} ' + f'error={error!r}', + flush=True, + ) raise PlannerError(f'constrained planning failed: {error}') from error - if not result or result.trajectory is None: + if result is None or result.trajectory is None: + error_code = ( + None if result is None + else getattr(result.error_code, 'val', None) + ) + error_name = _error_code_name(error_code) + print( + '[iiwa_llm_plan_executor] plan empty ' + f'phase={phase!r} pipeline={effective_phase!r} ' + f'error_code={error_name} goal_xyz={base_goal.xyz}', + flush=True, + ) 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)' + f'planning failed for phase {phase!r} ' + f'({effective_phase}, error {error_name}); ' + f'goal_xyz={tuple(round(float(v), 6) for v in base_goal.xyz)} ' + f'start={tuple(round(float(v), 6) for v in current)}' ) message = result.trajectory.get_robot_trajectory_msg() names = list(message.joint_trajectory.joint_names) @@ -316,20 +339,24 @@ class MoveItPyPlanner: f'planned trajectory misses joints {sorted(set(expected) - set(names))}' ) index = {name: position for position, name in enumerate(names)} - points: list[tuple[float, ...]] = [] + raw_points: list[tuple[float, ...]] = [] + for waypoint in message.joint_trajectory.points: + raw_points.append(tuple( + float(waypoint.positions[index[name]]) for name in expected + )) + points = self._densify( + raw_points, self.policy.limits.max_sample_step_rad, + ) 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) + for ordered in points: 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)) + clearance = self._check_collisions(points) return PlannedMotion( points=points, tcp_poses=poses, @@ -343,9 +370,110 @@ class MoveItPyPlanner: effective_phase, self.PHASES['transfer'], )[:2], 'reorientation': reorientation, + 'raw_points': len(raw_points), + 'sampled_points': len(points), + 'min_clearance_m': self.policy.limits.min_clearance_m, + 'measured_clearance_m': clearance, }, ) + @staticmethod + def _densify( + points: list[tuple[float, ...]], + max_step: float, + ) -> list[tuple[float, ...]]: + if not points: + return [] + result = [points[0]] + for previous, current_point in zip(points, points[1:]): + delta = max( + abs(a - b) for a, b in zip(current_point, previous) + ) + steps = max(1, int(math.ceil(delta / max_step))) + for step in range(1, steps + 1): + fraction = step / steps + result.append(tuple( + a + (b - a) * fraction + for a, b in zip(previous, current_point) + )) + return result + + def _check_collisions(self, points: list[tuple[float, ...]]) -> float | None: + if self._scene_monitor is None: + raise PlannerError('planning scene monitor is unavailable') + from moveit.core.collision_detection import ( + CollisionRequest, + CollisionResult, + ) + from moveit.core.robot_state import RobotState + + requirement = float(self.policy.limits.min_clearance_m) + measured_min: float | None = None + state = RobotState(self._model) + with self._scene_monitor.read_only() as scene: + acm = getattr(scene, 'allowed_collision_matrix', None) + for index, sample in enumerate(points): + state.set_joint_group_positions(self.planning_group, sample) + state.update() + request = CollisionRequest() + request.distance = True + request.contacts = True + request.max_contacts = 20 + request.max_contacts_per_pair = 2 + collided = False + checked = False + last_error = None + distances: list[float] = [] + contacts: list[str] = [] + for function in ( + 'check_collision_unpadded', 'check_self_collision', + ): + for arguments in ( + (request, CollisionResult(), state, acm), + (request, CollisionResult(), state), + ): + if acm is None and len(arguments) == 4: + continue + try: + result = arguments[1] + hit = bool(getattr(scene, function)(*arguments)) + checked = True + collided = collided or hit + except TypeError as error: + last_error = error + continue + try: + distances.append(float(result.distance)) + except Exception: + pass + formatted = _format_contacts(result) + if formatted != 'unknown pair': + contacts.append(formatted) + if not checked: + raise PlannerError( + 'collision checking is unavailable: ' + f'{last_error}' + ) + distance = min(distances) if distances else None + if distance is not None: + measured_min = ( + distance if measured_min is None + else min(measured_min, distance) + ) + if collided or ( + distance is not None and distance < requirement + ): + pairs = ', '.join(sorted(set(contacts))) or 'unknown pair' + measured = ( + 'unknown' if distance is None else f'{distance:.4f} m' + ) + raise PlannerError( + f'collision/clearance failure at sample {index}: ' + f'{pairs}; minimum distance {measured}, ' + f'required {requirement:.4f} m' + ) + return measured_min + def execute(self, planned: PlannedMotion) -> bool: self._ensure() try: @@ -382,6 +510,7 @@ class MoveItBackend: scene_guard: Callable[[], tuple[bool, str]] | None = None, current_joints: Callable[[], tuple[float, ...] | None] | None = None, before_plan: Callable[[], None] | None = None, + collision_checker: Callable[[Any], str | None] | None = None, on_event: Callable[[dict[str, Any]], None] | None = None, ) -> None: self.geometry = geometry @@ -391,6 +520,7 @@ class MoveItBackend: self._scene_guard = scene_guard self._current_joints = current_joints self._before_plan = before_plan + self._collision_checker = collision_checker self._on_event = on_event self.stopped = False self.stop_reason: str | None = None @@ -482,9 +612,20 @@ class MoveItBackend: except Exception: pass return self._fail( - f'motion policy rejected every constrained trajectory: {last_error}', + f'motion policy rejected every planned trajectory: {last_error}', 'motion_limit', ) + if self._collision_checker is not None: + try: + collision_reason = self._collision_checker(planned) + except Exception as error: + collision_reason = f'collision check failed: {error}' + if collision_reason: + try: + self.planner.stop() + except Exception: + pass + return self._fail(collision_reason, 'collision') target = ( goal if goal.parent == 'base_link' @@ -505,7 +646,7 @@ class MoveItBackend: ) self._emit( event='motion_planned', phase=phase, held_object=held_object, - **check.details, + planner_details=dict(planned.details), **check.details, ) try: executed = self.planner.execute(planned) diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py index 306b982..cabb4a1 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py @@ -321,7 +321,9 @@ class PlannerExecutorNode(Node): self.limits = load_limits( str(self.get_parameter('motion_limits_path').value), ) - self.policy = MotionPolicy(self.limits) + self.policy = MotionPolicy( + self.limits, self._read_joint_limits(), + ) self.close_inspection = frozenset( str(value) for value in self.get_parameter( 'close_inspection_objects', @@ -458,11 +460,9 @@ class PlannerExecutorNode(Node): ) # ------------------------------------------------------------------ setup - def _make_planner(self): + def _read_joint_limits(self) -> dict[str, tuple[float, float]]: import yaml - from .moveit_backend import MoveItPyPlanner - limits_path = Path(str(self.get_parameter('joint_limits_path').value)) joint_limits = {} try: @@ -482,12 +482,17 @@ class PlannerExecutorNode(Node): self.get_logger().warning( f'falling back to model joint bounds: {error}' ) + return joint_limits + + def _make_planner(self): + from .moveit_backend import MoveItPyPlanner + return MoveItPyPlanner( self.geometry, self.policy, planning_group=str(self.get_parameter('planning_group').value), pose_link=str(self.get_parameter('pose_link').value), - joint_limits=joint_limits, + joint_limits=self._read_joint_limits(), ) def _now(self) -> float: diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py index 671e57c..a4a346a 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py @@ -166,6 +166,25 @@ class SkillRegistry: goal: Transform, phase: str, ) -> Any: + lateral = phase in ('transfer', 'approach', 'align', 'observe', 'escape') + if ( + lateral + and context.phase == 'contact' + and context.approach_pose is not None + ): + # A lateral PTP must never start at contact depth: first move + # straight up along the tool axis to the saved safe height. + retreat = self.motion.plan_and_execute( + context.approach_pose, 'retreat', context.held_object, + ) + if not getattr(retreat, 'success', False): + raise SkillError( + 'failed', + f'axial retreat before lateral motion failed: ' + f'{getattr(retreat, "message", "motion failed")}', + ) + context.last_verified_pose = context.approach_pose + context.mark_lifted() carried = context.held_object or context.mounted_tool outcome = self.motion.plan_and_execute(goal, phase, carried) if not getattr(outcome, 'success', False): @@ -362,8 +381,7 @@ class SkillRegistry: context.mark_lifted() work = context.geometry.safe_poses['work'] if not (context.at_work and context.phase == 'free'): - phase = 'transfer_lin' if context.phase != 'free' else 'transfer' - self._move(context, work, phase) + self._move(context, work, 'transfer') context.reset_phase() context.at_work = True if context.capture_frame('top') is None: @@ -601,8 +619,7 @@ class SkillRegistry: return SkillOutcome('failed', {'reason': f'{obj!r} cannot be transferred'}) goal = self._destination_goal(context, obj, target) safe = _above(goal, context.geometry.transfer_gap) - phase = 'transfer' if obj in context.spec.tool_names else 'transfer_lin' - self._move(context, safe, phase) + self._move(context, safe, 'transfer') context.transfer_target = target context.mark_approach(safe, (obj, target)) return SkillOutcome('success', { diff --git a/src/iiwa_llm_plan_executor/test/test_motion_policy.py b/src/iiwa_llm_plan_executor/test/test_motion_policy.py index f7ead66..0a5f2e8 100644 --- a/src/iiwa_llm_plan_executor/test/test_motion_policy.py +++ b/src/iiwa_llm_plan_executor/test/test_motion_policy.py @@ -40,34 +40,46 @@ def down_pose(tilt_deg: float = 0.0, xyz=(0.4, 0.0, 1.2)) -> Transform: ) -def test_work_joint7_is_zero_and_corridor_is_inclusive(): +def test_work_joint7_is_the_preference_not_a_hard_corridor(): 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): + + +def test_safe_joint7_beyond_the_old_ninety_degree_band_is_accepted(): + checker = policy() + for value in (-2.0, 2.0): result = checker.validate_trajectory( - [point(boundary)], [down_pose()], + [point(value)], [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)], + result = checker.validate_trajectory( + [point(-1.8), point(-2.0), point(-1.8)], [down_pose(), down_pose(), down_pose()], ) + assert result.ok, result.reason + assert result.details['max_j7_deviation_rad'] == pytest.approx(2.0) + + +def test_real_joint_limit_is_still_enforced(): + checker = MotionPolicy( + load_limits(LIMITS), + joint_limits={ + 'joint1': (-2.967, 2.967), + 'joint2': (-2.09, 2.09), + 'joint3': (-2.967, 2.967), + 'joint4': (-2.09, 2.09), + 'joint5': (-2.967, 2.967), + 'joint6': (-2.0027, 2.0027), + 'joint7': (-3.05, 3.05), + }, + ) + result = checker.validate_trajectory( + [point(3.2)], [down_pose()], + ) assert result.ok is False assert 'joint7' in result.reason + assert 'joint limit' in result.reason def test_near_pi_joint_jump_is_rejected(): @@ -170,3 +182,4 @@ def test_details_report_the_checked_extremes(): 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) + assert result.details['preferred_j7_rad'] == pytest.approx(0.0) diff --git a/src/iiwa_llm_plan_executor/test/test_moveit_backend.py b/src/iiwa_llm_plan_executor/test/test_moveit_backend.py index d5dc3a9..1b5b171 100644 --- a/src/iiwa_llm_plan_executor/test/test_moveit_backend.py +++ b/src/iiwa_llm_plan_executor/test/test_moveit_backend.py @@ -112,17 +112,56 @@ def test_safe_plan_is_executed_once_and_the_final_pose_is_verified(): assert len(planner.execute_calls) == 1 -def test_intermediate_out_of_corridor_point_never_reaches_execution(): +def test_safe_joint7_beyond_the_old_corridor_reaches_execution(): planner = FakePlanner(planned=planned( - [point(-1.0), point(-1.8), point(-1.0)], + [point(-1.5), point(-1.7), point(-1.5)], [BASE_GOAL, BASE_GOAL, BASE_GOAL], )) result = backend(planner).plan_and_execute(GOAL, 'transfer') + assert result.success is True + assert len(planner.execute_calls) == 1 + + +def test_motion_phase_selection_is_ptp_for_working_points_and_lin_for_axes(): + from iiwa_llm_plan_executor.moveit_backend import MoveItPyPlanner + + phases = MoveItPyPlanner.PHASES + for phase in ('transfer', 'approach', 'align', 'observe', 'escape'): + assert phases[phase][0] == 'pilz_industrial_motion_planner' + assert phases[phase][1] == 'PTP' + for phase in ('contact', 'lower', 'retreat', 'dispense'): + assert phases[phase][0] == 'pilz_industrial_motion_planner' + assert phases[phase][1] == 'LIN' + + +def test_dense_resampling_covers_steps_between_waypoints(): + from iiwa_llm_plan_executor.moveit_backend import MoveItPyPlanner + + samples = MoveItPyPlanner._densify( + [(0.0,) * 7, (0.05,) * 7], 0.01, + ) + assert len(samples) == 6 + assert samples[0] == (0.0,) * 7 + assert samples[-1] == (0.05,) * 7 + assert max( + abs(a - b) for a, b in zip(samples[1], samples[0]) + ) <= 0.01 + 1e-12 + + +def test_camera_self_collision_is_rejected_before_execution(): + planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL])) + result = backend( + planner, + collision_checker=lambda planned_motion: ( + 'collision/clearance failure: camera_hand <-> link6; ' + 'minimum distance 0.0005 m, required 0.0030 m' + ), + ).plan_and_execute(GOAL, 'transfer') assert result.success is False - assert result.code == 'motion_limit' - assert 'joint7' in result.message + assert result.code == 'collision' + assert 'camera_hand' in result.message + assert 'link6' in result.message assert planner.execute_calls == [] - assert planner.stopped >= 1 def test_excessive_tilt_and_jump_are_rejected_without_execution(): diff --git a/src/iiwa_llm_plan_executor/test/test_skills.py b/src/iiwa_llm_plan_executor/test/test_skills.py index c61cbfc..336f369 100644 --- a/src/iiwa_llm_plan_executor/test/test_skills.py +++ b/src/iiwa_llm_plan_executor/test/test_skills.py @@ -440,6 +440,28 @@ def test_release_lifts_axially_before_any_lateral_motion(): assert motion.calls[-1][0].xyz[2] > contact.xyz[2] +def test_transfer_from_contact_lifts_axially_before_lateral_ptp(): + registry, context, motion, supervisor = make_registry() + context.mounted_tool = 'vacuum' + context.held_object = 'cap' + supervisor.state['mounted_tool'] = 'vacuum' + supervisor.state['held_object'] = 'cap' + supervisor.state['cap']['state'] = 'held' + seat = world_target(context.spec, context.geometry, 'cap_seat') + from iiwa_experement.geometry import tcp_goal + from iiwa_llm_plan_executor.skills import _above + contact = tcp_goal(seat, context.geometry.cap_tcp_to_node) + safe = _above(contact, context.geometry.approach_gap) + context.mark_approach(safe, ('cap', 'hatch_seat')) + context.mark_contact(contact) + result = registry.execute( + node('t', 'transfer', 'cap', 'hatch_seat'), context, + ) + assert result.event == 'success' + assert phases(motion) == ['retreat', 'transfer'] + assert motion.calls[0][1] == 'retreat' + + def test_dispense_moves_along_the_segment_without_changing_ownership(): registry, context, motion, supervisor = make_registry() context.mounted_tool = 'sealant_tool' @@ -490,7 +512,7 @@ def test_detect_withdraws_axially_then_uses_fresh_frames(): result = registry.execute(node('d', 'detect', 'cap'), context) assert result.event == 'found' assert phases(motion)[0] == 'retreat' - assert phases(motion)[1] == 'transfer_lin' + assert phases(motion)[1] == 'transfer' assert context.phase == 'free' context.frame_provider = lambda name: None diff --git a/src/iiwa_utils/iiwa_utils/tool_manager.py b/src/iiwa_utils/iiwa_utils/tool_manager.py index 4a99c19..b6d0496 100644 --- a/src/iiwa_utils/iiwa_utils/tool_manager.py +++ b/src/iiwa_utils/iiwa_utils/tool_manager.py @@ -19,29 +19,11 @@ from typing import Any # --------------------------------------------------------------------------- _BASE_COLLISIONS: list[tuple[str, str, str]] = [ ("base_link", "link1", "Adjacent"), - ("base_link", "link2", "Never"), - ("base_link", "link3", "Never"), - ("base_link", "link4", "Never"), ("link1", "link2", "Adjacent"), - ("link1", "link3", "Never"), - ("link1", "link4", "Never"), - ("link1", "link5", "Never"), - ("link1", "link6", "Never"), - ("link1", "link7", "Never"), ("link2", "link3", "Adjacent"), - ("link2", "link4", "Never"), - ("link2", "link5", "Never"), - ("link2", "link6", "Never"), - ("link2", "link7", "Never"), ("link3", "link4", "Adjacent"), - ("link3", "link5", "Never"), - ("link3", "link6", "Never"), - ("link3", "link7", "Never"), ("link4", "link5", "Adjacent"), - ("link4", "link6", "Never"), - ("link4", "link7", "Never"), ("link5", "link6", "Adjacent"), - ("link5", "link7", "Never"), ("link6", "link7", "Adjacent"), ] diff --git a/src/iiwa_utils/test/test_camera_clearance.py b/src/iiwa_utils/test/test_camera_clearance.py new file mode 100644 index 0000000..00ee97e --- /dev/null +++ b/src/iiwa_utils/test/test_camera_clearance.py @@ -0,0 +1,150 @@ +"""Visible camera geometry has a positive gap to the robot links.""" +from __future__ import annotations + +from pathlib import Path + +import numpy as np +from scipy.spatial.transform import Rotation +from urdf_parser_py.urdf import URDF + +from ament_index_python.packages import get_package_share_directory +from iiwa_utils import converter, setting_loader + + +ROOT = Path(__file__).resolve().parents[2] +MIN_GAP_M = 0.005 + + +def _transform(xyz, rpy): + matrix = np.eye(4) + matrix[:3, :3] = Rotation.from_euler('xyz', rpy).as_matrix() + matrix[:3, 3] = xyz + return matrix + + +def _axis_rotation(axis, angle): + vector = np.asarray(axis, dtype=float) + vector /= np.linalg.norm(vector) + matrix = np.eye(4) + matrix[:3, :3] = Rotation.from_rotvec(vector * angle).as_matrix() + return matrix + + +def _robot_description(): + settings = setting_loader.build_settings( + settings_path=str( + Path(get_package_share_directory('iiwa_config')) + / 'config' / 'setting.yaml' + ), + check_files=True, + ) + return URDF.from_xml_string(converter.load_robot_description( + settings.robot.description, + settings.robot.name, + { + 'simulate': 'true', + 'initial_positions_file': settings.controller.moveit.initial_positions, + 'joint_limits_file': settings.controller.moveit.joint_limits, + }, + )) + + +def _fk(robot, target, joints): + by_child = {joint.child: joint for joint in robot.joints} + chain = [] + node = target + while node != 'base_link': + joint = by_child[node] + chain.append(joint) + node = joint.parent + result = np.eye(4) + for joint in reversed(chain): + result = result @ _transform(joint.origin.xyz, joint.origin.rpy) + if joint.type == 'revolute': + result = result @ _axis_rotation( + joint.axis, joints.get(joint.name, 0.0), + ) + return result + + +def _stl_box(path): + raw = Path(path).read_bytes() + count = int(np.frombuffer(raw, dtype='= MIN_GAP_M, (camera, link, gaps) + # The holder is bolted to the flange; only link7 contact is + # constructive and disabled in the SRDF. + assert gaps[('camera_holder', 'link7')] == 0.0 + assert gaps[('camera_holder', 'link6')] >= MIN_GAP_M diff --git a/src/iiwa_utils/test/test_tool_collisions.py b/src/iiwa_utils/test/test_tool_collisions.py new file mode 100644 index 0000000..e471c9d --- /dev/null +++ b/src/iiwa_utils/test/test_tool_collisions.py @@ -0,0 +1,85 @@ +"""The camera/tool collision model only disables constructive contact pairs.""" +from __future__ import annotations + +import xml.etree.ElementTree as ET +from pathlib import Path + +from iiwa_utils.tool_manager import _BASE_COLLISIONS, load_registry + + +ROOT = Path(__file__).resolve().parents[2] +TOOLS = ROOT / 'iiwa_config' / 'config' / 'tools.yaml' +SRDF = ROOT / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf' +PATRON = ROOT / 'iiwa_description' / 'urdf' / 'tools' / 'patron.xacro' + +FIXED_PAIRS = { + tuple(sorted(pair)) for pair in ( + ('link7', 'camera_holder'), + ('camera_holder', 'patron'), + ('camera_holder', 'camera_corner'), + ('camera_corner', 'camera_hand'), + ('camera_holder', 'camera_hand'), + ) +} + + +def pairs(entries): + return { + tuple(sorted((str(first), str(second)))) + for first, second, *_ in entries + } + + +def test_only_constructive_camera_pairs_are_disabled_in_the_source(): + registry = load_registry(TOOLS) + collisions = registry['patron']['collisions'] + assert all(reason == 'Adjacent' for _, _, reason in collisions) + assert pairs(collisions) == FIXED_PAIRS + + +def test_only_adjacent_robot_links_are_disabled(): + assert all(reason == 'Adjacent' for _, _, reason in _BASE_COLLISIONS) + assert pairs(_BASE_COLLISIONS) == { + ('base_link', 'link1'), + ('link1', 'link2'), + ('link2', 'link3'), + ('link3', 'link4'), + ('link4', 'link5'), + ('link5', 'link6'), + ('link6', 'link7'), + } + + +def test_generated_srdf_keeps_camera_robot_collisions_enabled(): + root = ET.parse(SRDF).getroot() + entries = [ + ( + element.get('link1'), + element.get('link2'), + element.get('reason'), + ) + for element in root.findall('disable_collisions') + ] + assert all(reason == 'Adjacent' for _, _, reason in entries) + disabled = pairs(entries) + for camera in ('camera_holder', 'camera_corner', 'camera_hand'): + for robot_link in ( + 'base_link', 'link1', 'link2', 'link3', 'link4', + 'link5', 'link6', 'link7', + ): + pair = tuple(sorted((camera, robot_link))) + if pair in FIXED_PAIRS: + assert pair in disabled + else: + assert pair not in disabled, pair + +def test_camera_hand_collision_box_matches_the_visible_d455_bounds(): + root = ET.parse(PATRON).getroot() + link = root.find("link[@name='camera_hand']") + collision = link.find('collision') + origin = collision.find('origin') + box = collision.find('geometry/box') + values = [float(value) for value in box.get('size').split()] + centre = [float(value) for value in origin.get('xyz').split()] + assert values == [0.124, 0.031, 0.026] + assert all(abs(a - b) < 1e-9 for a, b in zip(centre, [0.002, 0.0155, 0.013]))