diff --git a/CONTEXT.md b/CONTEXT.md index d97571b990..16dae44bcb 100644 --- a/CONTEXT.md +++ b/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 diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 137d8af72c..f131c42ae3 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/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) diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 6977e306a0..caa04babb5 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/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: diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 3f1580c7af..4fe2cc3659 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/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, diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 8c886aeafa..c9eeebb76d 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -17,6 +17,7 @@ from __future__ import annotations from pathlib import Path +from types import SimpleNamespace from unittest.mock import MagicMock import pytest @@ -29,6 +30,7 @@ TrajectoryExecutionResult, TrajectoryExecutionStatus, ) +from dimos.manipulation.conftest import ModuleFactory from dimos.manipulation.manipulation_module import ( ManipulationModule, ManipulationModuleConfig, @@ -185,6 +187,23 @@ def _generated_plan_trajectory(joint_names: list[str], *points: list[float]) -> ) +def _connected_sequence_module( + robot_config: RobotModelConfig, + module_factory: ModuleFactory, +) -> tuple[ManipulationModule, list[str], JointState]: + module = module_factory() + module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._world_monitor = MagicMock() + module._world_monitor.world = MagicMock() + module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) + names = ["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"] + live = JointState(name=names, position=[0.0, 0.0, 0.0]) + module._world_monitor.current_global_joint_state.return_value = live + module._kinematics = MagicMock() + module._planner = MagicMock() + return module, names, live + + def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: joint_names = [f"j{i}" for i in range(len(points[0][1]))] if points else [] return JointTrajectory( @@ -196,6 +215,43 @@ def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: ) +class TestSafetyLift: + @pytest.mark.parametrize( + ("current_z", "expected_z"), + [(0.01, 0.10), (0.05, None), (0.20, None)], + ) + def test_safety_lift_target_is_derived_without_motion( + self, + mocker: MockerFixture, + module_factory: ModuleFactory, + current_z: float, + expected_z: float | None, + ) -> None: + module = module_factory() + orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + mocker.patch.object( + module, + "get_ee_pose", + return_value=Pose(Vector3(0.2, -0.1, current_z), orientation), + ) + plan = mocker.patch.object(module, "plan_to_pose") + + target = module._safety_lift_pose("test_arm") + + if expected_z is None: + assert target is None + else: + assert target is not None + assert target.position.as_tuple == pytest.approx((0.2, -0.1, expected_z)) + assert ( + target.orientation.x, + target.orientation.y, + target.orientation.z, + target.orientation.w, + ) == pytest.approx((0.0, 0.0, 0.0, 1.0)) + plan.assert_not_called() + + class TestObstacleUpdates: def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> None: module = module_factory() @@ -761,6 +817,312 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( _, planner_kwargs = module._planner.plan_selected_joint_path.call_args assert planner_kwargs["goal"] is ik_goal + def test_connected_pose_check_chains_each_plan_endpoint_into_the_next_start( + self, robot_config, mocker: MockerFixture, module_factory: ModuleFactory + ): + module, names, live = _connected_sequence_module(robot_config, module_factory) + goals = [ + JointState(name=names, position=[0.1, 0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1, 0.0]), + JointState(name=names, position=[0.1, 0.2, 0.1]), + ] + solve = mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + module._planner.plan_selected_joint_path.side_effect = lambda **kwargs: PlanningResult( + status=PlanningStatus.SUCCESS, + path=[kwargs["start"], kwargs["goal"]], + ) + poses = [ + Pose(position=Vector3(x=0.4 + index * 0.1), orientation=Quaternion()) + for index in range(3) + ] + + failed_index, endpoint = module._check_connected_pose_sequence(poses, "test_arm") + + assert failed_index is None + assert endpoint is not None + assert endpoint.position == goals[-1].position + ik_seeds = [call.kwargs["seed"] for call in solve.call_args_list] + planner_starts = [ + call.kwargs["start"] for call in module._planner.plan_selected_joint_path.call_args_list + ] + assert [seed.position for seed in ik_seeds] == [ + live.position, + goals[0].position, + goals[1].position, + ] + assert [start.position for start in planner_starts] == [ + live.position, + goals[0].position, + goals[1].position, + ] + assert module._state == ManipulationState.IDLE + assert module._last_plan is None + + def test_connected_pose_plan_exposes_paths_without_storing_them( + self, robot_config, mocker: MockerFixture, module_factory: ModuleFactory + ) -> None: + module, names, live = _connected_sequence_module(robot_config, module_factory) + goals = [ + JointState(name=names, position=[0.1, 0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1, 0.0]), + ] + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + module._planner.plan_selected_joint_path.side_effect = lambda **kwargs: PlanningResult( + status=PlanningStatus.SUCCESS, + path=[kwargs["start"], kwargs["goal"]], + ) + + result = module._plan_connected_pose_sequence( + [ + Pose(position=Vector3(x=0.4), orientation=Quaternion()), + Pose(position=Vector3(x=0.5), orientation=Quaternion()), + ], + "test_arm", + ) + + assert result.failed_index is None + assert result.endpoint is not None + assert result.endpoint.position == goals[-1].position + assert [[state.position for state in path] for path in result.paths] == [ + [live.position, goals[0].position], + [goals[0].position, goals[1].position], + ] + assert module._state == ManipulationState.IDLE + assert module._last_plan is None + + def test_connected_pose_check_accepts_explicit_start( + self, robot_config, mocker: MockerFixture, module_factory: ModuleFactory + ): + module, names, _ = _connected_sequence_module(robot_config, module_factory) + explicit_start = JointState(name=names, position=[0.3, 0.2, 0.1]) + goal = JointState(name=names, position=[0.4, 0.2, 0.1]) + solve = mocker.patch.object( + module, + "inverse_kinematics", + return_value=IKResult(status=IKStatus.SUCCESS, joint_state=goal), + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.SUCCESS, + path=[explicit_start, goal], + ) + + failed_index, endpoint = module._check_connected_pose_sequence( + [Pose(position=Vector3(x=0.5), orientation=Quaternion())], + "test_arm", + explicit_start, + ) + + assert failed_index is None + assert endpoint is not None + assert endpoint.position == goal.position + assert solve.call_args.kwargs["seed"].position == explicit_start.position + assert ( + module._planner.plan_selected_joint_path.call_args.kwargs["start"].position + == explicit_start.position + ) + module._world_monitor.current_global_joint_state.assert_not_called() + + def test_connected_pose_check_returns_first_ik_failure_with_diagnostics( + self, + robot_config, + mocker: MockerFixture, + module_factory: ModuleFactory, + ): + module, names, live = _connected_sequence_module(robot_config, module_factory) + first_goal = JointState(name=names, position=[0.1, 0.0, 0.0]) + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[ + IKResult(status=IKStatus.SUCCESS, joint_state=first_goal), + IKResult(status=IKStatus.NO_SOLUTION, message="blocked IK"), + ], + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.SUCCESS, + path=[live, first_goal], + ) + poses = [ + Pose(position=Vector3(x=0.4), orientation=Quaternion()), + Pose(position=Vector3(x=0.5), orientation=Quaternion()), + ] + log_info = mocker.patch("dimos.manipulation.manipulation_module.logger.info") + + failed_index, endpoint = module._check_connected_pose_sequence(poses, "test_arm") + + assert (failed_index, endpoint) == (1, None) + log_info.assert_called_with( + "Connected pose planning failed IK at index %d: %s%s", + 1, + "NO_SOLUTION", + ": blocked IK", + ) + assert module._planner.plan_selected_joint_path.call_count == 1 + + def test_connected_pose_check_returns_path_failure_with_diagnostics( + self, + robot_config, + mocker: MockerFixture, + module_factory: ModuleFactory, + ): + module, names, _ = _connected_sequence_module(robot_config, module_factory) + goal = JointState(name=names, position=[0.1, 0.0, 0.0]) + mocker.patch.object( + module, + "inverse_kinematics", + return_value=IKResult(status=IKStatus.SUCCESS, joint_state=goal), + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.NO_SOLUTION, + message="blocked path", + ) + log_info = mocker.patch("dimos.manipulation.manipulation_module.logger.info") + + failed_index, endpoint = module._check_connected_pose_sequence( + [Pose(position=Vector3(x=0.4), orientation=Quaternion())], + "test_arm", + ) + + assert (failed_index, endpoint) == (0, None) + log_info.assert_called_with( + "Connected pose planning failed path at index %d: %s%s", + 0, + "NO_SOLUTION", + ": blocked path", + ) + + @pytest.mark.parametrize( + "path", + [ + [], + [ + JointState( + name=["wrong/joint1", "wrong/joint2", "wrong/joint3"], + position=[0.1, 0.0, 0.0], + ) + ], + ], + ids=["empty", "malformed-endpoint"], + ) + def test_connected_pose_check_rejects_invalid_success_path( + self, + robot_config, + mocker: MockerFixture, + module_factory: ModuleFactory, + path: list[JointState], + ): + module, names, _ = _connected_sequence_module(robot_config, module_factory) + goal = JointState(name=names, position=[0.1, 0.0, 0.0]) + mocker.patch.object( + module, + "inverse_kinematics", + return_value=IKResult(status=IKStatus.SUCCESS, joint_state=goal), + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + ) + + result = module._check_connected_pose_sequence( + [Pose(position=Vector3(x=0.4), orientation=Quaternion())], + "test_arm", + ) + + assert result == (0, None) + + @pytest.mark.parametrize( + "blocked_index", + [None, 0, 1, 2], + ids=["open-scene", "pre-grasp-blocked", "grasp-blocked", "retreat-blocked"], + ) + def test_connected_pose_check_uses_one_reusable_scene_with_optional_blocker( + self, + robot_config, + mocker: MockerFixture, + module_factory: ModuleFactory, + blocked_index: int | None, + ) -> None: + module, names, live = _connected_sequence_module(robot_config, module_factory) + goals = [ + JointState(name=names, position=[0.1 + index * 0.1, 0.0, 0.0]) for index in range(3) + ] + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + starts = [live, goals[0], goals[1]] + module._planner.plan_selected_joint_path.side_effect = [ + ( + PlanningResult(status=PlanningStatus.NO_SOLUTION, message="scene blocker") + if index == blocked_index + else PlanningResult( + status=PlanningStatus.SUCCESS, + path=[starts[index], goals[index]], + ) + ) + for index in range(3) + ] + poses = [ + Pose(position=Vector3(x=0.4 + index * 0.1), orientation=Quaternion()) + for index in range(3) + ] + + failed_index, endpoint = module._check_connected_pose_sequence(poses, "test_arm") + + assert failed_index == blocked_index + if blocked_index is None: + assert endpoint is not None + assert endpoint.position == goals[-1].position + else: + assert endpoint is None + assert module._planner.plan_selected_joint_path.call_count == blocked_index + 1 + + def test_connected_pose_check_observes_live_scene_on_each_segment( + self, robot_config, mocker: MockerFixture, module_factory: ModuleFactory + ) -> None: + module, names, live = _connected_sequence_module(robot_config, module_factory) + world = SimpleNamespace(revision=0) + module._world_monitor.world = world + goals = [ + JointState(name=names, position=[0.1 + index * 0.1, 0.0, 0.0]) for index in range(3) + ] + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + starts = [live, goals[0], goals[1]] + observed_revisions: list[int] = [] + + def plan_with_live_scene(**kwargs) -> PlanningResult: + observed_revisions.append(kwargs["world"].revision) + index = len(observed_revisions) - 1 + world.revision += 1 + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=[starts[index], goals[index]], + ) + + module._planner.plan_selected_joint_path.side_effect = plan_with_live_scene + poses = [ + Pose(position=Vector3(x=0.4 + index * 0.1), orientation=Quaternion()) + for index in range(3) + ] + + failed_index, _ = module._check_connected_pose_sequence(poses, "test_arm") + + assert failed_index is None + assert observed_revisions == [0, 1, 2] + def test_failed_plan_materialization_clears_generated_plan(self, robot_config, module_factory): module = module_factory() registry = PlanningGroupRegistry([robot_config]) diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index d15a9598fa..ae61f22026 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -47,6 +47,7 @@ from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.std_msgs.Header import Header from dimos.perception.experimental.object import Object as DetObject @@ -406,13 +407,14 @@ def test_provider_is_required_when_fallback_is_disabled( def test_selection_skips_higher_scored_infeasible_candidate( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: - failed = SimpleNamespace(is_success=lambda: False) - succeeded = SimpleNamespace(is_success=lambda: True) - solve = mocker.patch.object( + endpoint = JointState(name=["arm/joint1"], position=[0.1]) + plan_sequence = mocker.patch.object( module, - "inverse_kinematics_single", - side_effect=[failed, succeeded, succeeded, succeeded], + "_check_connected_pose_sequence", + side_effect=[(0, None), (None, endpoint)], ) + plan_motion = mocker.patch.object(module, "plan_to_pose") + command_gripper = mocker.patch.object(module, "_set_gripper_position") transaction = SimpleNamespace(rejections=Counter()) selected = module._select_feasible_grasp( @@ -424,8 +426,37 @@ def test_selection_skips_higher_scored_infeasible_candidate( assert selected.rank == 2 assert selected.candidate.score == 0.8 - assert solve.call_count == 4 + assert plan_sequence.call_count == 2 assert transaction.rejections == {"pre_grasp_infeasible": 1} + plan_motion.assert_not_called() + command_gripper.assert_not_called() + + @pytest.mark.parametrize( + ("failed_index", "expected_rejection"), + [ + (0, "pre_grasp_infeasible"), + (1, "grasp_infeasible"), + (2, "retreat_infeasible"), + ], + ) + def test_selection_reports_failed_connected_segment( + self, + module: PickAndPlaceModule, + mocker: MockerFixture, + failed_index: int, + expected_rejection: str, + ) -> None: + mocker.patch.object( + module, + "_check_connected_pose_sequence", + return_value=(failed_index, None), + ) + transaction = SimpleNamespace(rejections=Counter()) + + with pytest.raises(RuntimeError, match="No feasible grasp among 1"): + module._select_feasible_grasp([_candidate(0.4, 0.9)], "arm", 0.1, transaction) + + assert transaction.rejections == {expected_rejection: 1} def test_selection_rejects_malformed_candidate_and_honors_limit( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -433,13 +464,13 @@ def test_selection_rejects_malformed_candidate_and_honors_limit( module.config.max_grasp_candidates_to_check = 1 invalid = _candidate(0.4, 0.9) invalid.pose.orientation.w = 0.0 - solve = mocker.patch.object(module, "inverse_kinematics_single") + plan_sequence = mocker.patch.object(module, "_check_connected_pose_sequence") transaction = SimpleNamespace(rejections=Counter()) with pytest.raises(RuntimeError, match="No feasible grasp among 1"): module._select_feasible_grasp([invalid, _candidate(0.5, 0.8)], "arm", 0.1, transaction) - solve.assert_not_called() + plan_sequence.assert_not_called() assert transaction.rejections == {"invalid": 1} @@ -457,6 +488,7 @@ def _arrange_success( mocker.patch.object(module, "_require_pick_object", return_value=detection) mocker.patch.object(module, "_provider_candidates", return_value=[candidate]) mocker.patch.object(module, "_select_feasible_grasp", return_value=selected) + mocker.patch.object(module, "_safety_lift_pose", return_value=None) mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) mocker.patch.object(module, "plan_to_pose", return_value=True) mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) @@ -487,7 +519,65 @@ def test_success_executes_ordered_pick_and_records_metadata( mocker.call(0.85, "arm"), mocker.call(0.0, "arm"), ] - assert module.plan_to_pose.call_count == 3 + assert module.plan_to_pose.call_args_list == [ + mocker.call(Pose(0.4, 0.0, 0.3), "arm"), + mocker.call(candidate.pose, "arm"), + mocker.call(Pose(0.4, 0.0, 0.3), "arm"), + ] + + def test_no_safety_lift_validates_candidates_from_current_state( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + check_sequence = mocker.patch.object(module, "_check_connected_pose_sequence") + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + check_sequence.assert_not_called() + assert module._select_feasible_grasp.call_args.args[4] is None + + def test_safety_lift_endpoint_is_shared_with_candidate_validation( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + lift_pose = Pose(0.2, 0.0, 0.1) + lift_endpoint = JointState(name=["arm/joint1"], position=[0.2]) + module._safety_lift_pose.return_value = lift_pose + check_sequence = mocker.patch.object( + module, + "_check_connected_pose_sequence", + return_value=(None, lift_endpoint), + ) + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + check_sequence.assert_called_once_with((lift_pose,), "arm") + assert module._select_feasible_grasp.call_args.args[4] is lift_endpoint + + def test_safety_lift_planning_failure_aborts_prepare_without_candidate_rejection( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + lift_pose = Pose(0.2, 0.0, 0.1) + module._safety_lift_pose.return_value = lift_pose + check_sequence = mocker.patch.object( + module, + "_check_connected_pose_sequence", + return_value=(0, None), + ) + + result = module.pick("cup", object_id="abc12345") + + assert result.error_code == "PLANNING_FAILED" + assert result.metadata["phase"] == "PREPARE" + assert result.metadata["rejections"] == {} + check_sequence.assert_called_once_with((lift_pose,), "arm") + module._select_feasible_grasp.assert_not_called() + module._lift_if_low.assert_not_called() + module.plan_to_pose.assert_not_called() + module._set_gripper_position.assert_not_called() def test_retreat_failure_keeps_gripper_closed( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -607,6 +697,7 @@ def test_phase_failures_stop_the_pipeline( assert result.error_code == expected_code assert result.metadata["phase"] == expected_phase + module._select_feasible_grasp.assert_called_once() def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( @@ -626,13 +717,12 @@ def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( module._grasp_generator = generator robot_config = SimpleNamespace(pre_grasp_offset=0.1) mocker.patch.object(module, "_get_robot", return_value=("arm", "robot-id", robot_config, None)) - failed = SimpleNamespace(is_success=lambda: False) - feasible = SimpleNamespace(is_success=lambda: True) - ik = mocker.patch.object( + plan_sequence = mocker.patch.object( module, - "inverse_kinematics_single", - side_effect=[failed, feasible, feasible, feasible], + "_check_connected_pose_sequence", + side_effect=[(0, None), (None, JointState())], ) + mocker.patch.object(module, "_safety_lift_pose", return_value=None) mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) plan = mocker.patch.object(module, "plan_to_pose", return_value=True) execute = mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) @@ -653,7 +743,8 @@ def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( scene.get_object_pointcloud_by_object_id.assert_called_once_with("abc12345") generator.propose_grasps.assert_called_once_with(scene.get_object_pointcloud_by_object_id()) world.suppress_object_obstacle.assert_called_once_with("abc12345") - assert ik.call_count == 4 + assert world.method_calls == [mocker.call.suppress_object_obstacle("abc12345")] + assert plan_sequence.call_count == 2 assert plan.call_count == 3 assert execute.call_count == 3 assert gripper.call_args_list == [mocker.call(0.85, "arm"), mocker.call(0.0, "arm")] diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..85a5f49fa2 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -1357,6 +1357,26 @@ def test_native_planner_names_path_from_robot_config_when_start_is_unnamed( assert [state.name for state in result.path] == [["joint1", "joint2"]] * 3 +def test_native_planner_accepts_hypothetical_start_without_mutating_live_state( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + world, robot_id = _make_world(fake_roboplan, robot_config) + + result = world.plan_joint_path( + world, + robot_id, + JointState(name=["joint1", "joint2"], position=[0.2, -0.1]), + JointState(name=["joint1", "joint2"], position=[0.4, 0.3]), + timeout=1.0, + ) + + assert result.status == PlanningStatus.SUCCESS + assert result.path[0].position == pytest.approx([0.2, -0.1]) + assert result.path[-1].position == pytest.approx([0.4, 0.3]) + live_state = world.get_joint_state(world.get_live_context(), robot_id) + assert live_state.position == pytest.approx([0.0, 0.0]) + + def test_native_selected_planner_returns_global_selected_joint_names( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: @@ -1798,12 +1818,12 @@ def collides_only_mid_edge(scene: FakeScene, q: np.ndarray) -> bool: assert "collision post-validation" in result.message -def test_native_planner_preserves_other_robot_and_auxiliary_joint_state( +def test_native_planner_uses_hypothetical_start_and_preserves_live_scene_state( fake_roboplan: None, robot_config: RobotModelConfig, mocker: MockerFixture, ) -> None: - world, _, second_id, second_config = _make_two_robot_world(fake_roboplan, robot_config) + world, first_id, second_id, second_config = _make_two_robot_world(fake_roboplan, robot_config) world.sync_from_joint_state( second_id, JointState(name=["joint1", "joint2"], position=[0.3, 0.1]), @@ -1831,15 +1851,20 @@ def capture_scene_state( result = world.plan_selected_joint_path( world, selection, - JointState(name=list(selection.joint_names), position=[0.0, 0.0]), + JointState(name=list(selection.joint_names), position=[0.15, -0.1]), JointState(name=list(selection.joint_names), position=[0.2, 0.1]), ) assert result.status == PlanningStatus.SUCCESS + assert result.path[0].position == pytest.approx([0.15, -0.1]) + assert observed_positions["arm__joint1"] == pytest.approx(0.0) + assert observed_positions["arm__joint2"] == pytest.approx(0.0) assert observed_positions["right__joint1"] == pytest.approx(0.3) assert observed_positions["right__joint2"] == pytest.approx(0.1) assert observed_positions["arm__joint3"] == pytest.approx(0.0) assert observed_positions["right__joint3"] == pytest.approx(0.0) + live_state = world.get_joint_state(world.get_live_context(), first_id) + assert live_state.position == pytest.approx([0.0, 0.0]) def test_native_planner_waits_for_every_robot_state( diff --git a/openspec/changes/validate-connected-grasp-sequences/.openspec.yaml b/openspec/changes/validate-connected-grasp-sequences/.openspec.yaml new file mode 100644 index 0000000000..f205fc727f --- /dev/null +++ b/openspec/changes/validate-connected-grasp-sequences/.openspec.yaml @@ -0,0 +1,2 @@ +schema: spec-driven +created: 2026-07-29 diff --git a/openspec/changes/validate-connected-grasp-sequences/design.md b/openspec/changes/validate-connected-grasp-sequences/design.md new file mode 100644 index 0000000000..bdcee2e7b7 --- /dev/null +++ b/openspec/changes/validate-connected-grasp-sequences/design.md @@ -0,0 +1,114 @@ +## Context + +`PickAndPlaceModule` currently gates ranked grasp candidates by solving collision-aware IK independently for pre-grasp, grasp, and retreat poses. This proves that each pose is individually reachable, but not that collision-free paths connect the robot's current state to those poses in order. + +The generic planner contract already accepts explicit `start` and `goal` joint states. The RoboPlan adapter is the exception: it rejects a valid explicit start when it differs from the authoritative live state, even though RoboPlan's native RRT accepts arbitrary start and goal configurations. The pick pipeline also performs an optional low-height safety lift immediately before approach, so candidate validation must account for that shared segment. + +Upstream perception remains responsible for supplying a target-only segmented cloud in the manipulation planning frame. This change starts at grasp proposals and does not add detection or segmentation behavior. + +## Goals / Non-Goals + +**Goals:** + +- Require a connected, collision-free sequence before accepting a grasp candidate. +- Align RoboPlan with the explicit-start planner contract without mutating authoritative live state. +- Include a required safety lift as one shared prerequisite rather than repeating it for every candidate. +- Keep validation motion-free and retain execution-time replanning from measured state. +- Validate the behavior with compact, deterministic fixtures. + +**Non-Goals:** + +- Snapshotting or freezing the planning scene across validation calls. +- Attaching generated object geometry to the gripper for retreat planning. +- Coordinated arm-and-gripper planning or validation of finger sweeps. +- Reusing dry-run paths for physical execution. +- Adding perception, point-cloud segmentation, a new timeout, or backend-specific public error codes. + +## Decisions + +### 1. RoboPlan will honor an explicit planning start + +Both robot-scoped and selected-group RoboPlan planning entry points will pass the supplied normalized start and goal to native RRT. The live planning context remains authoritative for unselected joints, other robots, and the rest of the scene. Planning from a hypothetical selected-joint start MUST NOT update the live context. + +Alternative: clone and overwrite the whole live scene for every request. Rejected because RoboPlan already represents the selected planning group's start explicitly, while the live scene is still needed for unselected state. + +### 2. Connected validation will be a side-effect-free planning helper + +A manipulation planning helper will accept an ordered pose sequence and an optional explicit selected-joint start. For each pose it will: + +1. solve collision-aware IK seeded by the current sequence endpoint; +2. plan from that endpoint to the IK solution; +3. use the returned path endpoint as the next segment's IK seed and planning start. + +It will return the first failed pose index and the final endpoint on success. It will not store a preview/execution plan, change manipulation state, or dispatch motion. + +Alternative: call the existing stateful `plan_to_pose` API three times. Rejected because failed screening would fault manipulation state, overwrite the execution plan, and prevent clean evaluation of lower-ranked candidates. + +### 3. The safety lift is planned once as shared preparation + +The current low-height check will be factored so the required lift target can be computed without moving. If no lift is required, candidate validation begins at authoritative current state. If a lift is required, it is dry-run planned once: + +- failure aborts the pick in `PREPARE` with `PLANNING_FAILED`; +- success supplies a common hypothetical endpoint for every candidate. + +The lift failure is not a candidate rejection because changing candidates cannot make the shared preparation segment feasible. + +### 4. Candidate validation is connected and ordered + +For each candidate in generator-score order, the pipeline derives pre-grasp and retreat poses and validates: + +```text +shared start -> pre-grasp -> grasp -> retreat +``` + +The first failed segment increments the existing stage-level rejection counter. A candidate is selected only after all three paths succeed. No motion or gripper command occurs while candidates are screened. + +Alternative: retain independent collision-aware IK checks. Rejected because disconnected feasible configurations do not establish an executable grasp sequence. + +### 5. Execution replans rather than reusing validation paths + +The selected candidate retains its poses, not the dry-run paths. Physical execution continues to plan each phase from fresh measured state immediately before dispatch. If an execution-time plan fails after motion begins, the transaction stops and does not try a different candidate. + +Alternative: execute the exact dry-run paths. Rejected because controller error, settling, preparation motion, and live scene changes can make stored paths stale. + +### 6. MVP validation uses the latest live scene and bounded collision fidelity + +Every segment uses the latest available non-target scene. The target remains suppressed during validation and execution, but non-target updates are not frozen. Retreat collision checking covers the robot and gripper state represented in the planning scene; it does not include attached-object geometry. The dry run does not model separate open and closed finger configurations. + +These limits are explicit so a successful result is not described as payload-safe or as a validated finger sweep. + +### 7. Reporting remains stable and stage-oriented + +Public rejection counts remain `pre_grasp_infeasible`, `grasp_infeasible`, and `retreat_infeasible`. IK status, planner status, timeout, and no-path details are logged for diagnosis rather than added to the skill-result contract. + +### 8. Tests use a small reusable fixture set + +Tests will be layered: + +- orchestration unit tests with deterministic IK and planner results; +- RoboPlan contract tests proving arbitrary starts and live-state preservation; +- an open scene plus one parameterized blocker for segment failures; + +This avoids maintaining a separate fixture file for every rejection case. + +## Risks / Trade-offs + +- [A scene update can occur between connected segment checks] → Use the latest scene for every segment and replan again before execution. +- [Execution-time IK can choose a different configuration than validation] → Seed from fresh measured state and require a complete collision-free execution plan before each dispatch. +- [Retreat can be safe for the robot but unsafe for the held object's volume] → State the MVP limitation explicitly and defer attached-object planning. +- [Current gripper geometry may differ from approach or retreat geometry] → Do not claim finger-sweep validation; add coordinated gripper planning only as a later capability. +- [Ranked screening multiplies planner calls] → Retain the configured candidate cap and existing planner timeout; planning is expected to be cheap for the MVP. + +## Migration Plan + +1. Land explicit-start RoboPlan behavior with backend contract tests. +2. Add the side-effect-free connected-sequence planning helper. +3. Integrate shared safety-lift and candidate-sequence validation. +4. Add deterministic integration coverage. +5. Enable the behavior through the existing `pick` pipeline with no caller migration. + +Rollback restores independent IK screening and the RoboPlan live-start guard. The public `pick` signature and configuration remain unchanged. + +## Open Questions + +No material MVP design questions remain. Payload attachment, coordinated gripper geometry, and physical gripper-threshold tuning are explicitly deferred follow-up work. diff --git a/openspec/changes/validate-connected-grasp-sequences/proposal.md b/openspec/changes/validate-connected-grasp-sequences/proposal.md new file mode 100644 index 0000000000..cdde9c1228 --- /dev/null +++ b/openspec/changes/validate-connected-grasp-sequences/proposal.md @@ -0,0 +1,29 @@ +## Why + +The grasp pipeline currently treats independently collision-free IK targets as sufficient evidence that a candidate is feasible. A candidate can therefore pass selection even when no connected motion exists from the current robot state through pre-grasp, grasp, and retreat. + +## What Changes + +- Make RoboPlan honor the planner contract's explicit start state instead of requiring it to match the live scene state. +- Validate an optional shared safety lift before evaluating candidate-specific motion. +- Accept a grasp candidate only when connected plans succeed from the shared start through pre-grasp, grasp, and retreat, with each segment starting at the preceding segment's endpoint. +- Keep validation motion-free and replan every physical execution segment from fresh measured state. +- Preserve stage-level candidate rejection reasons while logging detailed IK and planner outcomes. +- Add deterministic unit and backend-contract coverage for connected planning behavior. + +## Capabilities + +### New Capabilities + +- `connected-grasp-sequence-validation`: Defines explicit-start planning, shared preparation, connected candidate validation, execution replanning, failure reporting, and MVP collision-model boundaries. + +### Modified Capabilities + +None. + +## Impact + +- Affects RoboPlan planner adaptation, manipulation planning helpers, and `PickAndPlaceModule` candidate selection. +- Extends the pending grasp-pipeline behavior without changing the public `pick` signature or adding configuration. +- Uses the existing planning timeout and planning-scene update behavior. +- Adds no new runtime dependency. diff --git a/openspec/changes/validate-connected-grasp-sequences/specs/connected-grasp-sequence-validation/spec.md b/openspec/changes/validate-connected-grasp-sequences/specs/connected-grasp-sequence-validation/spec.md new file mode 100644 index 0000000000..e9baca6f5e --- /dev/null +++ b/openspec/changes/validate-connected-grasp-sequences/specs/connected-grasp-sequence-validation/spec.md @@ -0,0 +1,67 @@ +## ADDED Requirements + +### Requirement: Explicit-start RoboPlan planning +RoboPlan planning SHALL accept a valid explicit selected-joint start that differs from the authoritative live robot state. The requested start SHALL seed native planning, while unselected joints and other robots SHALL retain their latest live scene state. Planning MUST NOT mutate authoritative live state. + +#### Scenario: Hypothetical start differs from live state +- **WHEN** a caller requests a path from a valid selected-joint start that differs from the live selected-joint state +- **THEN** RoboPlan attempts the path from the requested start and leaves the live state unchanged + +#### Scenario: Unselected scene state is retained +- **WHEN** a selected planning group is planned from a hypothetical start +- **THEN** unselected joints, other robots, and non-target scene geometry retain their latest live values during collision checking + +### Requirement: Shared safety-lift validation +The pick pipeline SHALL determine whether its existing safety lift is required before candidate evaluation. It SHALL dry-run a required lift exactly once and SHALL use the successful lift endpoint as the common start for candidate validation. + +#### Scenario: Required safety lift is feasible +- **WHEN** the end effector requires a safety lift and a connected lift plan succeeds +- **THEN** every grasp candidate is evaluated from the planned lift endpoint without executing the lift during validation + +#### Scenario: Required safety lift is infeasible +- **WHEN** the end effector requires a safety lift and no lift plan succeeds +- **THEN** the pick aborts in `PREPARE` with `PLANNING_FAILED` and does not attribute the failure to any candidate + +### Requirement: Connected candidate sequence +The pipeline SHALL accept a grasp candidate only when connected plans succeed in order from the shared start to pre-grasp, from the pre-grasp path endpoint to grasp, and from the grasp path endpoint to retreat. Each IK solve SHALL be seeded from the preceding endpoint, and each plan SHALL begin at that same endpoint. + +#### Scenario: All candidate segments connect +- **WHEN** pre-grasp, grasp, and retreat each have a collision-free path beginning at the preceding path endpoint +- **THEN** the candidate passes motion-feasibility validation + +#### Scenario: Individually reachable poses are disconnected +- **WHEN** all three target poses have collision-free IK solutions but any required connecting path fails +- **THEN** the candidate is rejected without robot motion + +#### Scenario: Higher-ranked candidate has a blocked segment +- **WHEN** a higher-ranked candidate has a failed connected segment and a lower-ranked candidate has a complete connected sequence +- **THEN** the pipeline selects the lower-ranked candidate without moving for the rejected candidate + +### Requirement: Motion-free validation and fresh execution planning +Candidate validation SHALL NOT store an execution plan, dispatch robot motion, or issue gripper commands. After selection, every physical motion segment SHALL be replanned from freshly measured state before execution. + +#### Scenario: Candidate is selected +- **WHEN** a candidate passes connected dry-run validation +- **THEN** no validation path is executed and approach planning begins from fresh measured state + +#### Scenario: Execution replan fails after motion begins +- **WHEN** any execution-time replan fails after the robot has moved +- **THEN** the transaction stops at that phase and does not attempt a different grasp candidate + +### Requirement: Live-scene MVP collision contract +Each validation segment SHALL use the latest available planning scene with the selected target suppressed and non-target obstacles active. The MVP SHALL NOT claim atomic scene validation, attached-object clearance, or separate open-versus-closed finger-sweep validation. + +#### Scenario: Non-target scene changes during validation +- **WHEN** a non-target obstacle update arrives between two segment planning calls +- **THEN** the later segment uses the updated scene without restarting or freezing the complete validation sequence + +#### Scenario: Retreat validation succeeds +- **WHEN** the grasp-to-retreat path is collision-free for the robot and gripper state represented in the planning scene +- **THEN** retreat passes the MVP gate without asserting clearance for attached target geometry + +### Requirement: Stage-level rejection reporting +The pipeline SHALL report candidate rejection by the first failed sequence stage using `pre_grasp_infeasible`, `grasp_infeasible`, or `retreat_infeasible`. Backend-specific IK and planner outcomes SHALL remain diagnostic details and SHALL NOT become new public rejection categories in this change. + +#### Scenario: Grasp segment planning fails +- **WHEN** pre-grasp succeeds and the pre-grasp-to-grasp segment fails through IK, timeout, collision, or no path +- **THEN** the pipeline increments `grasp_infeasible` and records the detailed cause in diagnostic logs diff --git a/openspec/changes/validate-connected-grasp-sequences/tasks.md b/openspec/changes/validate-connected-grasp-sequences/tasks.md new file mode 100644 index 0000000000..ee748841bf --- /dev/null +++ b/openspec/changes/validate-connected-grasp-sequences/tasks.md @@ -0,0 +1,29 @@ +## 1. Align RoboPlan Explicit-Start Planning + +- [x] 1.1 Update robot-scoped and selected-group RoboPlan planning to pass any valid normalized explicit start to native RRT while retaining live unselected scene state. +- [x] 1.2 Add backend contract tests proving hypothetical starts are honored, paths begin at the requested start, and authoritative live robot state is not mutated. +- [x] 1.3 Extend multi-robot/auxiliary-joint coverage to prove hypothetical selected-group planning preserves other live scene state. + +## 2. Add Connected Dry-Run Planning + +- [x] 2.1 Add a side-effect-free manipulation helper that checks an ordered pose sequence from current state or an explicit selected-joint start and returns the first failed index plus the final endpoint. +- [x] 2.2 Chain each successful path endpoint into both the next IK seed and the next planner start, using collision-aware IK and the existing planning timeout. +- [x] 2.3 Log stage-specific IK and planner failure details without storing a generated execution plan or changing manipulation state. +- [x] 2.4 Add unit tests for complete success, IK failure, path failure, malformed/empty planner paths, explicit sequence starts, endpoint chaining, and no state or plan mutation. + +## 3. Integrate Shared Preparation and Candidate Validation + +- [x] 3.1 Factor the existing low-height safety-lift target calculation from its physical execution so it can be dry-run planned. +- [x] 3.2 Plan a required safety lift once in `PREPARE`, abort with `PLANNING_FAILED` on failure, and pass its endpoint as the shared start for all candidates. +- [x] 3.3 Replace independent pre-grasp/grasp/retreat IK screening with connected sequence checks in ranked candidate order. +- [x] 3.4 Preserve `pre_grasp_infeasible`, `grasp_infeasible`, and `retreat_infeasible` counters based on the first failed segment while keeping backend details diagnostic-only. +- [x] 3.5 Verify candidate screening issues no motion or gripper commands and that physical execution still replans every segment from fresh measured state. +- [x] 3.6 Add pick-pipeline tests for no-lift and shared-lift starts, lift failure, each failed candidate segment, lower-ranked fallback, exhausted candidates, and execution-time replan failure. + +## 4. Validate MVP Scene and Integration Boundaries + +- [x] 4.1 Add compact open-scene and parameterized-blocker coverage for successful sequences and blocked pre-grasp, grasp, and retreat paths without creating separate scene fixtures per case. +- [x] 4.2 Verify connected validation keeps the target suppressed, retains non-target obstacles, and allows later segments to observe live scene updates. +- [x] 4.3 Document and test the MVP boundary that retreat excludes attached-object geometry and uses the gripper state currently represented in the planning scene. +- [x] 4.4 Expose connected dry-run segment paths without storing an execution plan or changing manipulation state. +- [x] 4.5 Run focused manipulation tests, RoboPlan tests, static typing, formatting, and lint checks.