Fix: Retain blocks and estimate grasp surfaces in dual_arm_sim

src/dual_arm_sim/CMakeLists.txt

@@ -34,12 +34,46 @@ if(BUILD_TESTING)
   target_link_libraries(test_sort_blocks tinyxml2::tinyxml2)
   target_compile_definitions(test_sort_blocks PRIVATE
     SORT_BLOCKS_XML="${CMAKE_CURRENT_SOURCE_DIR}/objectives/sort_blocks.xml")
+  find_package(moveit_pro_behavior_interface REQUIRED)
+  find_package(moveit_pro_base REQUIRED)
+  find_package(moveit_pro_task_constructor_core REQUIRED)
+  find_package(nlohmann_json REQUIRED)
+  find_package(pluginlib REQUIRED)
+  find_package(sensor_msgs REQUIRED)
+  find_package(geometry_msgs REQUIRED)
+  find_package(rclcpp REQUIRED)
+  find_package(pcl_conversions REQUIRED)
+  find_package(PCL REQUIRED COMPONENTS common io)
+  ament_add_gtest(test_block_surface test/test_block_surface.cpp)
+  ament_target_dependencies(test_block_surface
+    behaviortree_cpp_picknik moveit_pro_behavior_interface pluginlib
+    sensor_msgs geometry_msgs rclcpp pcl_conversions)
+  target_link_libraries(test_block_surface tinyxml2::tinyxml2 ${PCL_LIBRARIES})
+  target_compile_features(test_block_surface PRIVATE cxx_std_17)
+  target_compile_definitions(test_block_surface PRIVATE
+    FIND_WITH_PROMPT_XML="${CMAKE_CURRENT_SOURCE_DIR}/objectives/find_with_prompt.xml"
+    BLOCK_CLOUD_DATA="${CMAKE_CURRENT_SOURCE_DIR}/test/data")
+  add_executable(block_transfer_plan_probe test/block_transfer_plan_probe.cpp)
+  target_compile_features(block_transfer_plan_probe PRIVATE cxx_std_20)
+  target_link_libraries(block_transfer_plan_probe PRIVATE
+    moveit_pro_behavior_interface::behavior_context
+    moveit_pro_base::robot_model_loader
+    moveit_pro_base::moveit_pro_base_planning_scene
+    moveit_pro_task_constructor_core::moveit_pro_task_constructor_core
+    pluginlib::pluginlib nlohmann_json::nlohmann_json tinyxml2::tinyxml2)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
   ament_lint_auto_find_test_dependencies()
   ament_add_pytest_test(
           scene_test test/test_scene.py
           TIMEOUT 120)
+  ament_add_pytest_test(
+          grasp_contacts_test test/test_grasp_contacts.py
+          TIMEOUT 120)
+  ament_add_pytest_test(
+          block_transfer_test test/test_block_transfer.py
+          TIMEOUT 210
+          ENV BLOCK_TRANSFER_PROBE=$<TARGET_FILE:block_transfer_plan_probe>)
   ament_add_pytest_test(
           base_twist_to_planar_test test/test_base_twist_to_planar.py
           TIMEOUT 30)

src/dual_arm_sim/README.md

@@ -90,6 +90,18 @@ rather than overlooked.
 | "Whole Body Move to Pose", "Whole Body Move to Joint State" | the shared subtrees: IK on a `*_manipulator_with_base` group, then plan, switch, execute and switch back |
 | "Move Base to Ready", "Navigate to Clicked Point" | stow the arms, then drive with Nav2 to the Base Ready pose or to a clicked point |
 
+### Sort Blocks grasp assumptions
+
+`Sort Blocks` uses the overhead scene camera to pick the 50 mm blocks from the front table. `Find Red Block` and `Find Green Block` fit the dominant plane before computing its centroid, rejecting mask-edge points on the sides, table or floor. The top must account for at least 60% of the segmented cloud within 1 mm. This covers the default scene and the measured displaced block resting on another face; it does not infer a safe grasp for arbitrary tilted or touching blocks.
+
+A Find Objective still returns failure when it has no pose. Its `detections_empty` output is true only after successful segmentation with no objects, and is cleared on every attempt. `Sort Blocks` checks both colors before reporting exhaustion and propagates perception, pose-conversion and motion errors instead of treating them as an empty table. Inspect the mask and point-cloud capture after a surface-fit error; separate or reset displaced blocks before retrying.
+
+The Simulator uses stiffer block/finger contacts (`solref="0.006 1"`, `solimp="0.95 0.99 0.001"`), finger sliding friction 4, and elliptic constraints with `impratio=10`. Lift and carry use `carry_speed=0.25` and its square for acceleration scaling; empty-hand moves keep their existing settings. Some within-grip movement remains; these are simulation settings, not hardware tuning guidance.
+
+`Plan Block Transfer` plans approach, descent, lift and carry together before pickup, evaluating up to 64 grasp IK solutions for a branch that can finish the transfer. It lifts 100 mm, keeps the grasp orientation while clearing the front table through a corner, then moves to the saved Place pose with 50 mm extra height for the destination stack. The solution splits at gripper closure: pickup ends after descent, carry starts with lift. A planning failure leaves the arm at its starting pose with the gripper open. Joint limits, table geometry and robot collision checks remain unchanged.
+
+Two consecutive default-scene Sort Blocks runs placed all six blocks on the correct tables, with no loss of hand contact or unintended block/table contact during the measured post-lift carries. This is not an object-aware planner: displaced obstacles or arbitrary stacks need a new plan and validation. `test/test_grasp_contacts.py` exercises both hands and replays near/far articulated carries; `test/test_block_transfer.py` plans recorded pickups through the real subtree and checks split continuity and a full-block clearance envelope including 25 mm within-grip displacement. [test/data/README.md](test/data/README.md) describes the regression inputs.
+
 ## Build and run
 
 ```bash
@@ -108,4 +120,4 @@ moveit_pro run -c dual_arm_sim
 | `maps/`, `params/` | the Nav2 map and parameters |
 | `objectives/`, `waypoints/` | Objectives, drawing paths and saved waypoints |
 | `scripts/` | the odometry and `/cmd_vel` bridges, the map generator, and the SVG drawing tools (see `scripts/README.md`) |
-| `test/` | the scene, bridge and Objectives integration tests |
+| `test/` | the scene, bridge and Objectives integration tests, and the block grasp, surface and transfer regressions with their recorded inputs in `test/data/` |

src/dual_arm_sim/description/mujoco/mobile_fr3_duo.xml

@@ -9,6 +9,16 @@
   <compiler angle="radian" meshdir="assets" />
   <size nuser_cam="4" />
 
+  <default>
+    <!-- Both sides of a grasp contact use this stiffness; mixing with a soft block lets the pads interpenetrate. -->
+    <default class="grasp_contact">
+      <geom solref="0.006 1" solimp="0.95 0.99 0.001" />
+      <default class="finger_contact">
+        <geom friction="4 0.01 0.0002" />
+      </default>
+    </default>
+  </default>
+
   <option integrator="implicitfast">
     <flag multiccd="enable" />
   </option>
@@ -1429,6 +1439,7 @@
                                           </body>
                                           <body
                                             name="left_fr3v2_leftfinger"
+                                            childclass="finger_contact"
                                             pos="0 0 0.0584"
                                             gravcomp="1"
                                           >
@@ -1493,6 +1504,7 @@
                                           </body>
                                           <body
                                             name="left_fr3v2_rightfinger"
+                                            childclass="finger_contact"
                                             pos="0 0 0.0584"
                                             quat="0 0 0 1"
                                             gravcomp="1"
@@ -2036,6 +2048,7 @@
                                           </body>
                                           <body
                                             name="right_fr3v2_leftfinger"
+                                            childclass="finger_contact"
                                             pos="0 0 0.0584"
                                             gravcomp="1"
                                           >
@@ -2100,6 +2113,7 @@
                                           </body>
                                           <body
                                             name="right_fr3v2_rightfinger"
+                                            childclass="finger_contact"
                                             pos="0 0 0.0584"
                                             quat="0 0 0 1"
                                             gravcomp="1"

src/dual_arm_sim/description/mujoco/scene.xml

@@ -1,7 +1,8 @@
 <mujoco model="mobile fr3 duo scene">
   <include file="mobile_fr3_duo.xml" />
 
-  <option timestep="0.003" />
+  <!-- Elliptic contacts with stronger friction constraints keep a carried block from rotating out of the pads. -->
+  <option timestep="0.003" cone="elliptic" impratio="10" />
   <statistic center="0.8 0 0.8" extent="2.5" />
   <size nkey="1" />
 
@@ -177,7 +178,7 @@
     </body>
 
     <!-- Blocks light enough for the simulated Franka Hand grip; the red row sits within the right arm's reach. -->
-    <body name="block_green_1" pos="0.65 0.25 0.8">
+    <body name="block_green_1" pos="0.65 0.25 0.8" childclass="grasp_contact">
       <freejoint name="block_green_1" />
       <geom
         type="box"
@@ -187,7 +188,7 @@
         friction="2 0.01 0.0002"
       />
     </body>
-    <body name="block_green_2" pos="0.75 -0.25 0.8">
+    <body name="block_green_2" pos="0.75 -0.25 0.8" childclass="grasp_contact">
       <freejoint name="block_green_2" />
       <geom
         type="box"
@@ -197,7 +198,7 @@
         friction="2 0.01 0.0002"
       />
     </body>
-    <body name="block_green_3" pos="0.85 0.25 0.8">
+    <body name="block_green_3" pos="0.85 0.25 0.8" childclass="grasp_contact">
       <freejoint name="block_green_3" />
       <geom
         type="box"
@@ -208,7 +209,7 @@
       />
     </body>
 
-    <body name="block_red_1" pos="0.65 -0.25 0.8">
+    <body name="block_red_1" pos="0.65 -0.25 0.8" childclass="grasp_contact">
       <freejoint name="block_red_1" />
       <geom
         type="box"
@@ -218,7 +219,7 @@
         friction="2 0.01 0.0002"
       />
     </body>
-    <body name="block_red_2" pos="0.75 0.25 0.8">
+    <body name="block_red_2" pos="0.75 0.25 0.8" childclass="grasp_contact">
       <freejoint name="block_red_2" />
       <geom
         type="box"
@@ -228,7 +229,7 @@
         friction="2 0.01 0.0002"
       />
     </body>
-    <body name="block_red_3" pos="0.85 -0.25 0.8">
+    <body name="block_red_3" pos="0.85 -0.25 0.8" childclass="grasp_contact">
       <freejoint name="block_red_3" />
       <geom
         type="box"

src/dual_arm_sim/objectives/find_green_block.xml

@@ -8,6 +8,7 @@
         _collapsed="true"
         prompt="a green block"
         object_centroid="{object_centroid}"
+        detections_empty="{detections_empty}"
       />
     </Control>
   </BehaviorTree>
@@ -17,6 +18,11 @@
         <Metadata runnable="true" />
         <Metadata subcategory="Perception - ML" />
       </MetadataFields>
+      <output_port
+        name="detections_empty"
+        default="{detections_empty}"
+        type="bool"
+      />
       <inout_port name="object_centroid" default="{object_centroid}" />
     </SubTree>
   </TreeNodesModel>

src/dual_arm_sim/objectives/find_red_block.xml

@@ -8,6 +8,7 @@
         _collapsed="false"
         prompt="a red block"
         object_centroid="{object_centroid}"
+        detections_empty="{detections_empty}"
       />
     </Control>
   </BehaviorTree>
@@ -17,6 +18,11 @@
         <Metadata runnable="true" />
         <Metadata subcategory="Perception - ML" />
       </MetadataFields>
+      <output_port
+        name="detections_empty"
+        default="{detections_empty}"
+        type="bool"
+      />
       <inout_port name="object_centroid" default="{object_centroid}" />
     </SubTree>
   </TreeNodesModel>

src/dual_arm_sim/objectives/find_with_prompt.xml

@@ -3,6 +3,7 @@
   <!--//////////-->
   <BehaviorTree ID="Find with prompt" _description="" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="Script" code="detections_empty := false" />
       <SubTree
         ID="Segment Point Cloud from Text Prompt Subtree"
         _collapsed="false"
@@ -18,16 +19,31 @@
         point_cloud_vector="{point_cloud_vector}"
         use_latest_transform="true"
       />
+      <Action
+        ID="GetSizeOfVector"
+        vector_in="{point_cloud_vector}"
+        vector_size="{detection_count}"
+      />
+      <Action ID="Script" code="detections_empty = detection_count == 0" />
+      <Condition ID="ScriptCondition" code="!detections_empty" />
       <Action
         ID="GetElementOfVector"
         index="0"
         vector_in="{point_cloud_vector}"
         element="{object_cloud}"
       />
+      <!-- The overhead view is dominated by the block top; mask edges can also contain sides, table or floor. -->
+      <Action
+        ID="ExtractDominantPlaneFromPointCloud"
+        point_cloud="{object_cloud}"
+        plane_point_cloud="{object_surface}"
+        inlier_tolerance="0.001"
+        min_inlier_fraction="0.6"
+      />
       <Action
         ID="GetCentroidFromPointCloud"
         output_pose="{object_centroid}"
-        point_cloud="{object_cloud}"
+        point_cloud="{object_surface}"
       />
       <Action
         ID="VisualizePose"
@@ -44,6 +60,14 @@
         <Metadata runnable="false" />
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
+      <output_port
+        name="detections_empty"
+        default="{detections_empty}"
+        type="bool"
+      >
+        True only when segmentation succeeded with no objects; failures leave
+        this false.
+      </output_port>
       <inout_port name="object_centroid" default="{object_centroid}" />
       <inout_port name="prompt" default="{prompt}" />
     </SubTree>