feat(manipulation): validate connected grasp plans

CONTEXT.md

@@ -25,3 +25,37 @@ A Cartesian timing policy that resolves the requested path into joint space and
 
 **Custom Planner Components**:
 Backend-native solver tasks, constraints, and barriers injected as live objects. These are outside standard Cartesian planning and require a separate constrained-IK interface.
+
+# Grasp Planning
+
+This context defines the language at the boundary between upstream perception, grasp generation, and robot motion planning.
+
+## Language
+
+**Segmented Object Cloud**:
+A target-only 3D point cloud supplied by upstream perception in the manipulation planning frame.
+_Avoid_: Scene cloud, detection cloud, raw camera cloud
+
+**Feasible Grasp Sequence**:
+A connected, collision-free trajectory from the robot's current state through any required safety lift, pre-grasp, grasp, and retreat. Each segment begins at the preceding segment's endpoint. Validation is a no-motion dry run; execution replans each segment from fresh measured state.
+_Avoid_: Reachable grasp, feasible pose, independent IK success
+
+**Safety Lift**:
+An optional shared preparation segment planned before evaluating grasp candidates. If required and unplannable, the pick aborts once in `PREPARE`; the failure is not attributed to every candidate.
+_Avoid_: Pre-grasp failure, candidate rejection
+
+**Retreat Feasibility (MVP)**:
+A connected grasp-to-retreat plan that collision-checks the robot and gripper against the non-target scene. The selected target remains excluded; attached-object geometry and held-object clearance are not modeled.
+_Avoid_: Payload-safe retreat, attached-object validation
+
+**Live-Scene Validation (MVP)**:
+Each segment of a feasible grasp sequence is checked against the latest available planning scene. The MVP does not snapshot the scene or freeze non-target obstacle updates across the sequence. Execution replans against freshly measured state and scene data.
+_Avoid_: Atomic scene validation, frozen-scene guarantee
+
+**Gripper Geometry During Validation (MVP)**:
+Arm-path collision checks use the gripper configuration currently represented in the planning scene. Dry-run validation does not model the open-to-closed gripper transition or claim separate clearance guarantees for each finger configuration.
+_Avoid_: Coordinated arm-gripper plan, validated finger sweep
+
+**Candidate Rejection Reason (MVP)**:
+Candidate rejection is reported by failed sequence stage: `pre_grasp_infeasible`, `grasp_infeasible`, or `retreat_infeasible`. Detailed IK and planner outcomes remain diagnostic logs rather than public skill-result categories.
+_Avoid_: Backend-specific public failure codes

dimos/manipulation/manipulation_module.py

@@ -25,6 +25,7 @@
 from __future__ import annotations
 
 from collections.abc import Mapping, Sequence
+from dataclasses import dataclass
 from enum import Enum
 import math
 import threading
@@ -136,6 +137,15 @@ class ManipulationState(Enum):
     FAULT = 4
 
 
+@dataclass(frozen=True)
+class ConnectedPoseSequenceResult:
+    """Motion-free result for an ordered sequence of pose plans."""
+
+    failed_index: int | None
+    endpoint: JointState | None
+    paths: tuple[tuple[JointState, ...], ...]
+
+
 class ManipulationModuleConfig(ModuleConfig):
     """Configuration for ManipulationModule."""
 
@@ -1122,6 +1132,94 @@ def generate_plan_to_pose_targets(
         logger.info(f"IK solved, error: {ik.position_error:.4f}m")
         return self._plan_selected_path(group_ids, start, ik.joint_state, planning_epoch)
 
+    def _check_connected_pose_sequence(
+        self,
+        poses: Sequence[Pose],
+        robot_name: RobotName,
+        start: JointState | None = None,
+    ) -> tuple[int | None, JointState | None]:
+        """Dry-run a connected pose sequence and return failure index and endpoint.
+
+        The check does not store a plan or change module state. Each IK solve and
+        path plan starts at the preceding path's endpoint. When ``start`` is
+        omitted, the sequence begins at the robot's current authoritative state.
+        """
+        result = self._plan_connected_pose_sequence(poses, robot_name, start)
+        return result.failed_index, result.endpoint
+
+    def _plan_connected_pose_sequence(
+        self,
+        poses: Sequence[Pose],
+        robot_name: RobotName,
+        start: JointState | None = None,
+    ) -> ConnectedPoseSequenceResult:
+        """Dry-run connected pose plans and retain each successful segment path."""
+        if not poses:
+            return ConnectedPoseSequenceResult(None, start, ())
+        if self._world_monitor is None or self._kinematics is None or self._planner is None:
+            logger.warning("Connected pose planning is unavailable")
+            return ConnectedPoseSequenceResult(0, None, ())
+        try:
+            group_id = self._require_unique_pose_group_id_for_robot(robot_name)
+            selection = self._world_monitor.planning_groups.select((group_id,))
+            if start is None:
+                current = self._world_monitor.current_global_joint_state()
+                start = filter_joint_state_to_selected_joints(current, selection.joint_names)
+            else:
+                start = filter_joint_state_to_selected_joints(start, selection.joint_names)
+        except (KeyError, ValueError) as exc:
+            logger.warning("Failed to initialize connected pose planning: %s", exc)
+            return ConnectedPoseSequenceResult(0, None, ())
+
+        paths: list[tuple[JointState, ...]] = []
+        for index, pose in enumerate(poses):
+            target = PoseStamped(
+                frame_id="world",
+                position=pose.position,
+                orientation=pose.orientation,
+            )
+            ik = self.inverse_kinematics(
+                pose_targets={group_id: target},
+                seed=start,
+                check_collision=True,
+            )
+            if not ik.is_success() or ik.joint_state is None:
+                logger.info(
+                    "Connected pose planning failed IK at index %d: %s%s",
+                    index,
+                    ik.status.name,
+                    f": {ik.message}" if ik.message else "",
+                )
+                return ConnectedPoseSequenceResult(index, None, tuple(paths))
+            result = self._planner.plan_selected_joint_path(
+                world=self._world_monitor.world,
+                selection=selection,
+                start=start,
+                goal=ik.joint_state,
+                timeout=self.config.planning_timeout,
+            )
+            if not result.is_success() or not result.path:
+                logger.info(
+                    "Connected pose planning failed path at index %d: %s%s",
+                    index,
+                    result.status.name,
+                    f": {result.message}" if result.message else "",
+                )
+                return ConnectedPoseSequenceResult(index, None, tuple(paths))
+            try:
+                start = filter_joint_state_to_selected_joints(
+                    result.path[-1], selection.joint_names
+                )
+            except ValueError as exc:
+                logger.info(
+                    "Connected pose planning returned an invalid endpoint at index %d: %s",
+                    index,
+                    exc,
+                )
+                return ConnectedPoseSequenceResult(index, None, tuple(paths))
+            paths.append(tuple(result.path))
+        return ConnectedPoseSequenceResult(None, start, tuple(paths))
+
     @rpc
     def plan_cartesian_targets(
         self,
@@ -1710,21 +1808,29 @@ def _wait_for_trajectory_completion(self, timeout: float = 60.0) -> bool:
         time.sleep(wait_time)
         return True
 
-    def _lift_if_low(
+    def _safety_lift_pose(
         self, robot_name: RobotName | None = None, min_z: float = 0.05
-    ) -> SkillResult[ManipulationSkillError]:
-        """If the end-effector is below *min_z*, plan and execute a short lift."""
+    ) -> Pose | None:
+        """Return the required safety-lift target, if the end effector is low."""
         ee = self.get_ee_pose(robot_name)
         if ee is None or ee.position.z >= min_z:
-            return SkillResult.ok()
+            return None
 
         lift_z = min_z + 0.05
         logger.info(f"EE z={ee.position.z:.3f} < {min_z}, lifting to z={lift_z:.3f}")
-        lift_pose = Pose(Vector3(ee.position.x, ee.position.y, lift_z), ee.orientation)
+        return Pose(Vector3(ee.position.x, ee.position.y, lift_z), ee.orientation)
+
+    def _lift_if_low(
+        self, robot_name: RobotName | None = None, min_z: float = 0.05
+    ) -> SkillResult[ManipulationSkillError]:
+        """If the end-effector is below *min_z*, plan and execute a short lift."""
+        lift_pose = self._safety_lift_pose(robot_name, min_z)
+        if lift_pose is None:
+            return SkillResult.ok()
         if not self.plan_to_pose(lift_pose, robot_name):
             return SkillResult.fail(
                 "PLANNING_FAILED",
-                f"Failed to plan lift from z={ee.position.z:.3f}",
+                f"Failed to plan safety lift to z={lift_pose.position.z:.3f}",
             )
         return self._preview_execute_wait(robot_name)
 

dimos/manipulation/pick_and_place_module.py

@@ -59,6 +59,7 @@
 
 if TYPE_CHECKING:
     from dimos.msgs.geometry_msgs.PoseArray import PoseArray
+    from dimos.msgs.sensor_msgs.JointState import JointState
     from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 
 logger = setup_logger()
@@ -696,6 +697,7 @@ def _select_feasible_grasp(
         robot_name: str,
         robot_pre_grasp_offset: float,
         transaction: _PickTransaction,
+        sequence_start: JointState | None = None,
     ) -> _FeasibleGrasp:
         vector = Vector3(self.config.grasp_approach_vector)
         pre_offset = self.config.grasp_pre_grasp_offset or robot_pre_grasp_offset
@@ -708,21 +710,20 @@ def _select_feasible_grasp(
                 continue
             pre_grasp = self._compute_pre_grasp_pose(candidate.pose, pre_offset, vector)
             retreat = self._compute_pre_grasp_pose(candidate.pose, retreat_offset, vector)
-            targets = (
-                (_CandidateRejection.PRE_GRASP_INFEASIBLE, pre_grasp),
-                (_CandidateRejection.GRASP_INFEASIBLE, candidate.pose),
-                (_CandidateRejection.RETREAT_INFEASIBLE, retreat),
+            rejections = (
+                _CandidateRejection.PRE_GRASP_INFEASIBLE,
+                _CandidateRejection.GRASP_INFEASIBLE,
+                _CandidateRejection.RETREAT_INFEASIBLE,
             )
-            feasible = True
-            for rejection, target in targets:
-                if not self.inverse_kinematics_single(
-                    target, robot_name=robot_name, check_collision=True
-                ).is_success():
-                    transaction.rejections[rejection.value] += 1
-                    feasible = False
-                    break
-            if feasible:
-                return _FeasibleGrasp(candidate, rank, pre_grasp, retreat)
+            failed_index, _ = self._check_connected_pose_sequence(
+                (pre_grasp, candidate.pose, retreat),
+                robot_name,
+                start=sequence_start,
+            )
+            if failed_index is not None:
+                transaction.rejections[rejections[failed_index].value] += 1
+                continue
+            return _FeasibleGrasp(candidate, rank, pre_grasp, retreat)
 
         summary = ", ".join(
             f"{reason}={count}" for reason, count in sorted(transaction.rejections.items())
@@ -906,9 +907,25 @@ def pick(
                 )
 
             with self._world_monitor.suppress_object_obstacle(detection.object_id) as suppression:
+                sequence_start = None
+                lift_pose = self._safety_lift_pose(rname)
+                if lift_pose is not None:
+                    transaction.phase = _PickPhase.PREPARE
+                    failed_index, sequence_start = self._check_connected_pose_sequence(
+                        (lift_pose,), rname
+                    )
+                    if failed_index is not None:
+                        raise _PickPipelineError(
+                            "PLANNING_FAILED",
+                            "Required safety-lift planning failed",
+                        )
                 transaction.phase = _PickPhase.SELECT
                 transaction.selected = self._select_feasible_grasp(
-                    candidates, rname, robot_config.pre_grasp_offset, transaction
+                    candidates,
+                    rname,
+                    robot_config.pre_grasp_offset,
+                    transaction,
+                    sequence_start,
                 )
                 result = self._execute_selected_pick(transaction, rname)
             if suppression.cleanup_error is not None:

dimos/manipulation/planning/world/roboplan_world.py

@@ -450,7 +450,7 @@ def plan_joint_path(
         goal: JointState,
         timeout: float = 10.0,
     ) -> PlanningResult:
-        """Plan using the legacy robot-scoped local-name contract."""
+        """Plan between explicit states using the legacy robot-scoped contract."""
         if world is not self:
             return PlanningResult(
                 status=PlanningStatus.NO_SOLUTION,
@@ -470,12 +470,6 @@ def plan_joint_path(
                 message="RoboPlan planning scene is not ready: authoritative state is incomplete",
             )
         robot = self._get_robot(robot_id)
-        current = self._live_context.q_by_robot[robot_id]
-        if not np.allclose(q_start, current, atol=1e-6, rtol=0.0):
-            return PlanningResult(
-                status=PlanningStatus.INVALID_START,
-                message="Requested start state does not match current scene state",
-            )
         group = self._legacy_group(robot.config.name)
         return self._plan_group(
             group,
@@ -494,7 +488,7 @@ def plan_selected_joint_path(
         timeout: float = 10.0,
         max_iterations: int = 5000,
     ) -> PlanningResult:
-        """Plan one or more non-overlapping groups through RoboPlan RRT."""
+        """Plan selected groups between explicit states through RoboPlan RRT."""
         if world is not self:
             return PlanningResult(
                 status=PlanningStatus.UNSUPPORTED,
@@ -516,7 +510,7 @@ def plan_selected_joint_path(
         except ValueError as exc:
             return PlanningResult(status=PlanningStatus.INVALID_GOAL, message=str(exc))
         try:
-            normalized_start = self._validated_selection_start(selection, start)
+            normalized_start = self._normalized_selection_start(selection, start)
         except ValueError as exc:
             return PlanningResult(status=PlanningStatus.INVALID_START, message=str(exc))
         start_by_name = dict(zip(normalized_start.name, normalized_start.position, strict=True))
@@ -615,11 +609,7 @@ def _validated_selection_start(
         start: JointState,
     ) -> JointState:
         """Return a normalized start matching the authoritative scene state."""
-        if not self._is_ready():
-            raise ValueError(
-                "RoboPlan planning scene is not ready: authoritative state is incomplete"
-            )
-        normalized = normalize_selection_target(selection, start, "start")
+        normalized = self._normalized_selection_start(selection, start)
         start_by_name = dict(zip(normalized.name, normalized.position, strict=True))
         current_by_name = self._current_global_positions()
         if any(
@@ -629,6 +619,18 @@ def _validated_selection_start(
             raise ValueError("Requested start state does not match current scene state")
         return normalized
 
+    def _normalized_selection_start(
+        self,
+        selection: PlanningGroupSelection,
+        start: JointState,
+    ) -> JointState:
+        """Return a normalized explicit start when the planning scene is ready."""
+        if not self._is_ready():
+            raise ValueError(
+                "RoboPlan planning scene is not ready: authoritative state is incomplete"
+            )
+        return normalize_selection_target(selection, start, "start")
+
     def _validate_cartesian_request(
         self,
         selection: PlanningGroupSelection,