fix(llm-plan-executor): use PTP/LIN with checked camera collisions

src/iiwa_config/config/moveit/iiwa7.srdf

@@ -37,66 +37,16 @@
     <end_effector name="patron" parent_link="patron" group="iiwa_arm"/>
 
     <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
-    <disable_collisions link1="base_link" link2="link2" reason="Never"/>
-    <disable_collisions link1="base_link" link2="link3" reason="Never"/>
-    <disable_collisions link1="base_link" link2="link4" reason="Never"/>
     <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
-    <disable_collisions link1="link1" link2="link3" reason="Never"/>
-    <disable_collisions link1="link1" link2="link4" reason="Never"/>
-    <disable_collisions link1="link1" link2="link5" reason="Never"/>
-    <disable_collisions link1="link1" link2="link6" reason="Never"/>
-    <disable_collisions link1="link1" link2="link7" reason="Never"/>
     <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
-    <disable_collisions link1="link2" link2="link4" reason="Never"/>
-    <disable_collisions link1="link2" link2="link5" reason="Never"/>
-    <disable_collisions link1="link2" link2="link6" reason="Never"/>
-    <disable_collisions link1="link2" link2="link7" reason="Never"/>
     <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
-    <disable_collisions link1="link3" link2="link5" reason="Never"/>
-    <disable_collisions link1="link3" link2="link6" reason="Never"/>
-    <disable_collisions link1="link3" link2="link7" reason="Never"/>
     <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
-    <disable_collisions link1="link4" link2="link6" reason="Never"/>
-    <disable_collisions link1="link4" link2="link7" reason="Never"/>
     <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
-    <disable_collisions link1="link5" link2="link7" reason="Never"/>
     <disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
 
-    <disable_collisions link1="link1" link2="patron" reason="Never"/>
-    <disable_collisions link1="link2" link2="patron" reason="Never"/>
-    <disable_collisions link1="link3" link2="patron" reason="Never"/>
-    <disable_collisions link1="link4" link2="patron" reason="Never"/>
-    <disable_collisions link1="link5" link2="patron" reason="Never"/>
-    <disable_collisions link1="link6" link2="patron" reason="Never"/>
-    <disable_collisions link1="link7" link2="patron" reason="Adjacent"/>
     <disable_collisions link1="link7" link2="camera_holder" reason="Adjacent"/>
-    <disable_collisions link1="link6" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link5" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link4" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link3" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link2" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link1" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="base_link" link2="camera_holder" reason="Never"/>
     <disable_collisions link1="camera_holder" link2="patron" reason="Adjacent"/>
     <disable_collisions link1="camera_holder" link2="camera_corner" reason="Adjacent"/>
-    <disable_collisions link1="camera_corner" link2="patron" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link7" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link6" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link5" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link4" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link3" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link2" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link1" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="base_link" reason="Never"/>
     <disable_collisions link1="camera_corner" link2="camera_hand" reason="Adjacent"/>
-    <disable_collisions link1="camera_hand" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="camera_holder" link2="camera_hand" reason="Adjacent"/>
-    <disable_collisions link1="camera_hand" link2="patron" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link7" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link6" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link5" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link4" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link3" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link2" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link1" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="base_link" reason="Never"/>
 </robot>

src/iiwa_config/config/tools.yaml

@@ -24,40 +24,10 @@ tools:
     tip_link: "patron"
     tcp_link: "tcp"
     collisions:
-      - [link1, patron, Never]
+      # Only fixed-joint or constructive camera-assembly contact pairs are
-      - [link2, patron, Never]
+      # disabled.  Every camera-to-robot pair must be checked by MoveIt.
-      - [link3, patron, Never]
-      - [link4, patron, Never]
-      - [link5, patron, Never]
-      - [link6, patron, Never]
-      - [link7, patron, Adjacent]
       - [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_holder, camera_hand, Adjacent]
-      - [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]

src/iiwa_description/urdf/tools/patron.xacro

@@ -86,9 +86,9 @@
             </geometry>
         </visual>
         <collision>
-            <origin xyz="0.0 0.014 0.0125" rpy="0.0 0.0 0.0"/>
+            <origin xyz="0.002 0.0155 0.013" rpy="0.0 0.0 0.0"/>
             <geometry>
-                <box size="0.12 0.025 0.025"/>
+                <box size="0.124 0.031 0.026"/>
             </geometry>
         </collision>
     </link>

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
+  # Preferred value is the neutral work pose (joint_7 = 0).  This is a
-  # is +/-90 degrees around it; the -180 degree IK branch is forbidden.
+  # 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

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°)
+The executor uses Pilz PTP for free-space working-point moves and Pilz LIN for
-on **every** planned point, seeds IK from current joints, enforces these as
+axial contact/retreat and the dispense contour.  Every plan is densely
-MoveIt path constraints, checks scene/joint freshness and verifies the final
+resampled and checked for real joint limits, self-collision (including the
-pose.  The only logged exception is the initial empty-gripper reorientation
+camera assembly), scene collision, camera clearance, downward TCP tilt (≤10°)
-from home to the downward `work` posture; J7 and the final orientation are
+and joint jumps before execution.  J7 may use its full safe joint range; the
-still enforced and a reorientation with a held object is rejected.  The
+`work` value is only a preference.  A lateral PTP from a contact pose is
-executor never falls back to an unconstrained `MoveToPose` goal.  This is a
+preceded by an axial LIN retreat.  The only logged orientation exception is
-Webots verification, not a physical-robot validation and not a Qwen3 quality
+the initial empty-gripper reorientation from home to the downward `work`
-evaluation.
+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.

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,
+Result: build of 10 packages succeeded; `365 tests, 0 errors, 0 failures,
-0 skipped`.
+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.

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
+`MotionPolicy` (`config/motion_limits.yaml`) checks every sampled point before
-controller sees a plan:
+the controller sees a plan:
 
-* `joint_7` must stay inside `work_reference_rad +/- corridor_rad`
+* Every joint stays inside the real per-joint position limits
-  (0 +/- 90 deg; boundaries inclusive).
+  (`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
+gripper is empty, the first reorientation to `work` may pass through larger
-larger tilt angles.  Joint 7, joint jumps, Cartesian jumps and the final
+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
+The planner never falls back to an unconstrained `MoveToPose` action.  If a
-`tolerance_below/above = corridor_rad`.  The planner applies it on OMPL
+plan is missing, the status names the phase, pipeline, planner id, target tcp
-transfers.  In this MoveIt/Pilz installation the orientation path constraint is
+pose, start joints and MoveIt error code (for example `NO_IK_SOLUTION`).  If
-not honored reliably, so the 10 deg working-tilt limit is enforced **after**
+our validation rejects a plan, the status names the real cause: joint limit,
-planning by `MotionPolicy` on every returned point; the planner also gets a
+jump, orientation or the colliding link pair with the measured minimum
-wider internal orientation tolerance for its goal sampling.  Tool transits use
+distance and required clearance.
-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 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.
+

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
+Every PTP/LIN plan is resampled and checked for joint limits, self/world
-limit, so every plan produced by the planner adapter is checked here, point
+collision, working orientation and joint jumps.  The old hard +/-90 deg joint-7
-by point, before execution.
+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,18 +217,28 @@ 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 (
+        for index, values in enumerate(parsed):
-            min_j7 < self.limits.joint7_min_rad - epsilon
+            for joint_index, value in enumerate(values):
-            or max_j7 > self.limits.joint7_max_rad + epsilon
+                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,
-                'joint7 leaves the hard corridor '
+                        f'{name} at point {index} is {value:.4f} rad, '
-                f'[{self.limits.joint7_min_rad:.3f}, '
+                        f'outside the joint limit [{low:.4f}, {high:.4f}]',
-                f'{self.limits.joint7_max_rad:.3f}] rad '
+                        {
-                f'(trajectory {min_j7:.3f}..{max_j7:.3f})',
+                            'joint': name,
-                {'min_j7': min_j7, 'max_j7': max_j7},
+                            'value': value,
+                            'limit': [low, high],
+                            'point': index,
+                        },
                     )
 
         max_tilt = 0.0
@@ -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,

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
+        # PTP: free-space changes between working points, objects, tools,
-        # rejects trajectories that leave the joint-7 corridor.  Transfers and
+        # placement points and safe alignments.  No J7 path constraint; the
-        # approaches therefore use the constraint-aware OMPL pipeline.
+        # full sampled trajectory is checked afterwards.
-        'transfer': ('ompl', 'RRTConnectkConfigDefault', 0.20, 10.0),
+        'transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0),
-        'approach': ('ompl', 'RRTConnectkConfigDefault', 0.18, 10.0),
+        'approach': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
-        'retreat': ('pilz_industrial_motion_planner', 'LIN', 0.18, 2.0),
+        'observe': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
-        'observe': ('ompl', 'RRTConnectkConfigDefault', 0.18, 10.0),
+        'align': ('pilz_industrial_motion_planner', 'PTP', 0.10, 2.0),
-        'ptp_transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0),
+        'escape': ('pilz_industrial_motion_planner', 'PTP', 0.15, 2.0),
-        'transfer_lin': ('pilz_industrial_motion_planner', 'LIN', 0.15, 2.0),
+        # LIN: axial contact/retreat and the continuous dispense contour.
-        # Short axial segments keep the working orientation; LIN remains the
-        # deterministic choice and is still checked point by point.
         '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
+        return Constraints()
-        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
 
     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':
+            # J7 has no artificial +/-90 deg corridor and PTP/LIN goal IK is
-                # Short approach segments are Cartesian and keep the tip down.
+            # allowed to use the whole joint range.  Safety comes from the
-                effective_phase = 'transfer_lin'
+            # dense post-plan joint/self/world/orientation checks below.
-            # 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:
+            phase_params = self.PHASES.get(
-                self._arm.set_path_constraints(self._constraints(
+                effective_phase, self.PHASES['transfer'],
-                    base_goal, include_orientation=False,
+            )
-                ))
+            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'planning failed for phase {phase!r} '
-                f'(joint7 corridor {self.policy.limits.joint7_min_rad:.3f}..'
+                f'({effective_phase}, error {error_name}); '
-                f'{self.policy.limits.joint7_max_rad:.3f} rad)'
+                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:
+        for ordered in 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))
+        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)

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:

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, 'transfer')
-            self._move(context, work, phase)
         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, 'transfer')
-        self._move(context, safe, phase)
         context.transfer_target = target
         context.mark_approach(safe, (obj, target))
         return SkillOutcome('success', {

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
-
+    result = checker.validate_trajectory(
-
+        [point(-1.8), point(-2.0), point(-1.8)],
-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, 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)

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 result.code == 'collision'
-    assert 'joint7' in result.message
+    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():

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

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"),
 ]
 

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='<u4', count=1, offset=80)[0])
+    dtype = np.dtype([('n', '<f4', 3), ('v', '<f4', (3, 3)), ('a', '<u2')])
+    vertices = np.frombuffer(
+        raw, dtype=dtype, count=count, offset=84,
+    )['v'].reshape(-1, 3)
+    return vertices.min(axis=0), vertices.max(axis=0)
+
+
+def _corners(centre, half, rotation):
+    signs = np.array([
+        [x, y, z]
+        for x in (-1, 1) for y in (-1, 1) for z in (-1, 1)
+    ], dtype=float)
+    return centre + (signs @ np.diag(half)) @ rotation.T
+
+
+def _aabb_gap(first, second):
+    delta = np.maximum(
+        0.0,
+        np.maximum(first.min(axis=0) - second.max(axis=0),
+                   second.min(axis=0) - first.max(axis=0)),
+    )
+    return float(np.linalg.norm(delta))
+
+
+CAMERA_BOXES = {
+    'camera_holder': [
+        ([-0.035, -0.035, 0.0], [0.035, 0.035, 0.03]),
+        ([-0.015, 0.03, 0.0], [0.015, 0.13, 0.03]),
+    ],
+    'camera_corner': [
+        ([-0.062, 0.1015, 0.0135], [0.062, 0.1305, 0.0305]),
+    ],
+    'camera_hand': [
+        ([-0.06, 0.0, 0.0], [0.064, 0.031, 0.026]),
+    ],
+}
+
+
+def _gaps(joints):
+    robot = _robot_description()
+    result = {}
+    links = {}
+    for name in ('link6', 'link7'):
+        transform = _fk(robot, name, joints)
+        minimum, maximum = _stl_box(
+            ROOT / 'iiwa_description' / 'resource' / 'meshes'
+            / 'iiwa' / 'collision' / f'{name}.STL'
+        )
+        centre = transform[:3, :3] @ ((minimum + maximum) / 2.0) + transform[:3, 3]
+        corners = _corners(
+            centre, (maximum - minimum) / 2.0, transform[:3, :3],
+        )
+        links[name] = corners
+    for camera, boxes in CAMERA_BOXES.items():
+        transform = _fk(robot, camera, joints)
+        for link_name, link_corners in links.items():
+            for minimum, maximum in boxes:
+                centre_local = (np.array(minimum) + np.array(maximum)) / 2.0
+                half = (np.array(maximum) - np.array(minimum)) / 2.0
+                centre = transform[:3, :3] @ centre_local + transform[:3, 3]
+                corners = _corners(centre, half, transform[:3, :3])
+                result[(camera, link_name)] = min(
+                    result.get((camera, link_name), float('inf')),
+                    _aabb_gap(corners, link_corners),
+                )
+    return result
+
+
+def test_camera_corner_and_hand_clear_the_robot_at_work_and_home():
+    for joints in ({}, {'joint4': -1.57, 'joint6': 1.57}):
+        gaps = _gaps(joints)
+        for camera in ('camera_corner', 'camera_hand'):
+            for link in ('link6', 'link7'):
+                assert gaps[(camera, link)] >= 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

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