refactor(manipulation): replace the pick module with a composed one - #3732
refactor(manipulation): replace the pick module with a composed one#3732mustafab0 wants to merge 1 commit into
Conversation
Swaps main's 829-line PickAndPlaceModule, a ManipulationModule subclass that piled nine agent skills onto the primitive RPC module, for a plain module that reaches motion, reachability and the gripper through Specs. The primitives it needs move down onto ManipulationModule: the verified gripper pair that wraps #3701's await_gripper_settle, lift_if_low, the robot model's pre-grasp standoff, and the perception objects port with the obstacle-monitor RPCs that used to live on the subclass, so the planner keeps seeing detected objects. Grasp geometry now comes from HeuristicGraspModule reading the object cloud rather than the occlusion-inset and tall-object constants it replaces. Two bugs surfaced while verifying this on the xArm7 sim. Re-opening jaws that already rest at their measured 0.947 could only ever time out, because that is neither travel nor arrival at the commanded 1.0; await_gripper_settle takes an arrival_tolerance so a caller that already accepts a band around the target can say so. And make_xarm7_sim_robot_config placed the arm at the origin while xarm7.xml bolts it to a 12cm pedestal, so the planner solved every pose 12cm below the arm it was driving and every grasp closed on air. Dropped on purpose: look, get_scene_info, place_back, drop_on, pick_and_place, and the clear_perception_obstacles skill, whose documented use was to delete the world when planning hit COLLISION_AT_START. The obstacle RPCs remain on ManipulationModule. Also dropped: the multi-candidate retry loop and the far-reach pre-grasp shortening, both of which want the contact primitive rather than a reimplementation here.
Greptile SummaryThis change replaces the inherited pick-and-place flow with composed perception, grasp-generation, motion, and gripper operations. A failed object selection can leave a previously selected grasp active. The reproduced sequence selected object A, failed to select missing object B, and then successfully commanded motions for A when picking. Fresh scan detections also are not added to the planning world before collision-aware grasp filtering runs, so immediate pick planning can miss newly detected scene geometry. T-Rex validation blockedThe planning-world refresh check could not reach its scan and collision assertions because required runtime packages were missing: Confidence Score: 3/5Not safe to merge until failed selections clear executable grasp state and fresh detections are materialized for collision-aware planning. One failure was reproduced with a focused executable check and an empty-selection comparison. The planning-world issue is directly visible in the composed scan and collision-check flow, but its runtime comparison could not run because required packages are unavailable. Files Needing Attention:
 T-Rex LogsWhat T-Rex did
Reviews (1): Last reviewed commit: "refactor(manipulation): replace the pick..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| self._scene.set_prompts(prompts) | ||
| with self._objects_condition: | ||
| fresh = self._objects_condition.wait_for( | ||
| lambda: self._objects_version > objects_version, | ||
| timeout=self.config.scan_timeout, | ||
| ) | ||
| objects = self._objects | ||
| if not fresh: | ||
| return SkillResult.fail( | ||
| "OBJECT_NOT_DETECTED", | ||
| f"No detections published within {self.config.scan_timeout:.1f}s of the scan", | ||
| ) | ||
| return SkillResult.ok( |
There was a problem hiding this comment.
When scan_objects receives fresh detections, it returns without materializing the cached objects into the planning world. The subsequent collision-aware IK and motion planning therefore remain blind to detected scene objects, allowing approach paths to intersect surrounding geometry.
| obj = self.get_object(object_id) | ||
| if obj is None: | ||
| return SkillResult.fail("OBJECT_NOT_DETECTED", f"No object with ID {object_id!r}") | ||
| try: | ||
| candidates = self._grasp_generator.propose_grasps(obj.pointcloud) | ||
| except ValueError as exc: | ||
| return SkillResult.fail("GRASP_INPUT_INVALID", str(exc)) |
There was a problem hiding this comment.
After a grasp for object A has been selected, selecting a missing object B returns OBJECT_NOT_DETECTED without clearing the stored grasp and pregrasp poses. pick_selected only checks that those poses are present, so it proceeds to move to A even though the most recent selection failed. Clear the selection on every unsuccessful select_grasp path, or clear it before validating a new selection so a failure leaves picking in INVALID_STATE.
Artifacts
trex-artifacts/stale-grasp-selection-harness.py
- Executable focused harness that reproduces A selection, missing-B failure, and the ensuing pick motion calls.
trex-artifacts/stale-grasp-01-before.log
- Executed empty-selection baseline showing `INVALID_STATE` and `motion_calls=0`.
trex-artifacts/stale-grasp-02-after.log
- Executed stale-selection repro showing failed B selection retains A and `pick_selected` succeeds with three A motions.
1 participant
Main's PickAndPlaceModule was a ManipulationModule subclass that piled nine agent skills onto the primitive RPC module and reached into its privates to do it; this replaces it with a plain module that gets motion, reachability and the gripper through Specs. The primitives move down onto ManipulationModule, including the perception objects port and the obstacle-monitor RPCs that used to live on the subclass, so the planner keeps seeing detected objects instead of going collision-blind. Grasp geometry now comes from HeuristicGraspModule reading the object cloud rather than the occlusion-inset and tall-object constants it replaces, and #3701's grasp verification is wired through unchanged.
Verifying this on the xArm7 sim turned up two pre-existing bugs it also fixes. Re-opening jaws already resting at their measured 0.947 could only ever time out, since that is neither travel nor arrival at the commanded 1.0, so await_gripper_settle now takes an arrival_tolerance. And make_xarm7_sim_robot_config placed the arm at the origin while xarm7.xml bolts it to a 12cm pedestal, so the planner solved every pose 12cm below the arm it was driving and every grasp closed on air.
Deliberately dropped: look, get_scene_info, place_back, drop_on, pick_and_place, and clear_perception_obstacles, whose documented use was to delete the world when planning hit COLLISION_AT_START. The multi-candidate retry loop and far-reach pre-grasp shortening are dropped too; both want the contact primitive rather than a reimplementation here.