From 0484a62afd06b134bd79a8072f209aeee7eec014 Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Fri, 19 Jun 2026 22:38:10 -0400
Subject: [PATCH] feat(hangar_sim): persist placed boxes as planning-scene
 obstacles

Picks add each box to the planning scene under a unique id (placed_box_N)
and detach it into the world on release, so the whole-body base+arm planner
avoids every already-placed box on later picks instead of driving over them
(#19973). ResetPlanningSceneObjects at objective start clears boxes left by a
previous run, and a placed_count counter ported across perception passes keeps
the ids unique. Also reframes the scene camera, widens the suction-cup
collision margin, and clears stale grasp markers each pick.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
---
 src/hangar_sim/description/hangar_scene.xml   |  4 +-
 src/hangar_sim/description/vacuum.xml         |  4 +-
 .../ml_move_boxes_to_loading_zone.xml         | 11 +++
 ...ve_boxes_to_loading_zone_from_waypoint.xml | 74 ++++++++++++++++++-
 4 files changed, 88 insertions(+), 5 deletions(-)

diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index 405962ca5..7d6a86296 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -42,12 +42,12 @@
     <!-- Scene Camera -->
     <site
       name="scene_camera_optical_frame"
-      pos="-14.4207 -4.39686 9.04094"
+      pos="-12.9207 -2.89686 9.04094"
       quat="0.415847 -0.883719 0.194298 -0.09143"
     />
     <camera
       name="scene_camera"
-      pos="-14.4207 -4.39686 9.04094"
+      pos="-12.9207 -2.89686 9.04094"
       quat="0.883719 0.415847 -0.09143 -0.194298"
       fovy="45"
       mode="fixed"
diff --git a/src/hangar_sim/description/vacuum.xml b/src/hangar_sim/description/vacuum.xml
index 002a60d6b..38701dfbb 100644
--- a/src/hangar_sim/description/vacuum.xml
+++ b/src/hangar_sim/description/vacuum.xml
@@ -36,8 +36,8 @@
         type="mesh"
         name="collision_vacuum_suction_cups geom"
         mesh="collision_vacuum_suction_cups"
-        margin="0.02"
-        gap="0.02"
+        margin="0.04"
+        gap="0.04"
       />
       <site name="suction_cup" type="box" size="0.1 0.1 0.1" />
     </body>
diff --git a/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml b/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml
index cbcf82c12..8ff4f1ce6 100644
--- a/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml
+++ b/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml
@@ -7,6 +7,10 @@
     _favorite="true"
   >
     <Control ID="Sequence">
+      <!--Clear any collision objects left in the planning scene by a previous run of this
+          Objective (the per-box "placed_box_N" obstacles added below persist until removed).
+          Without this, stale boxes from the last run would block planning at their old poses.-->
+      <Action ID="ResetPlanningSceneObjects" />
       <Action
         ID="CreatePoseStamped"
         reference_frame="world"
@@ -14,6 +18,11 @@
         orientation_xyzw="0;1.0;0;0"
         position_xyz="-4.5;8.4;0.5"
       />
+      <!--Monotonic counter that gives every placed box a unique collision-object id
+          ("placed_box_0", "placed_box_1", ...) so each one persists as an obstacle the base
+          plans around instead of overwriting the previous box. Carried across perception
+          passes via the placed_count port on the subtree below.-->
+      <Action ID="Script" code="placed_count := 0" />
       <Decorator ID="RetryUntilSuccessful" num_attempts="1000">
         <Decorator ID="ForceFailure">
           <Control ID="Sequence">
@@ -27,12 +36,14 @@
                 negative_prompts="yellow ladder;"
                 prompts="small boxes"
                 place_pose="{place_pose}"
+                placed_count="{placed_count}"
                 erosion_size="10"
               />
               <SubTree
                 ID="Move Boxes to Loading Zone Start from Waypoint"
                 waypoint_name="View Boxes 2"
                 place_pose="{place_pose}"
+                placed_count="{placed_count}"
                 threshold="0.4"
                 _collapsed="false"
                 prompts="small boxes"
diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
index 0ce8d91ff..8717a37fa 100644
--- a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
+++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
@@ -9,6 +9,7 @@
     _description="Move boxes to loading zone from waypoint"
     waypoint_name="View Boxes"
     place_pose="{place_pose}"
+    placed_count="{placed_count}"
     threshold="{threshold}"
     prompt="{prompt}"
     negative_prompts="{negative_prompts}"
@@ -209,6 +210,36 @@
                       target_object="{graspable_object}"
                       ui_grasp_link="grasp_link"
                     />
+                    <!--Clear the previous pick's grasp markers so only the current pick's
+                        candidate poses are shown (markers use lifetime 0 = persist forever).-->
+                    <Action ID="ClearAllVisualMarkers" />
+                    <!--Visualize all candidate grasp poses in the 3D viewer (debug perception/grasp generation).-->
+                    <Control
+                      ID="Sequence"
+                      name="Visualize candidate grasp poses"
+                    >
+                      <Decorator
+                        ID="ForEach"
+                        vector_in="{grasp_poses}"
+                        out="{grasp_pose_viz}"
+                        index="{grasp_viz_index}"
+                      >
+                        <Control ID="Sequence">
+                          <!--Build a readable, unique marker name (grasp_0, grasp_1, ...) so all poses render simultaneously.-->
+                          <Action
+                            ID="Script"
+                            code="grasp_marker_name := &quot;grasp_&quot; .. grasp_viz_index"
+                          />
+                          <Action
+                            ID="VisualizePose"
+                            marker_name="{grasp_marker_name}"
+                            pose="{grasp_pose_viz}"
+                            marker_lifetime="0.000000"
+                            marker_size="0.100000"
+                          />
+                        </Control>
+                      </Decorator>
+                    </Control>
                     <Action
                       ID="InitializeMTCTask"
                       controller_names="joint_trajectory_controller"
@@ -275,7 +306,7 @@
                         <SubTree
                           ID="Execute MTC Solution (JTC)"
                           solution="{solution}"
-                          goal_duration_tolerance="-1.0"
+                          goal_duration_tolerance="30.0"
                         />
                       </Control>
                       <Control ID="Sequence" name="TopLevelSequence">
@@ -332,6 +363,37 @@
                       gripper_command_action_name="/vacuum_gripper/gripper_cmd"
                       position="1.0"
                     />
+                    <!--Box is now grasped. Add it to the planning scene at its ICP-refined pick pose
+                        under a unique id ("placed_box_N") and attach it to the gripper so the drive to
+                        the loading zone plans around the carried box. On release below it is detached
+                        and left in the world as a persistent obstacle, so later picks plan around every
+                        already-placed box instead of driving over them (#19973).-->
+                    <Action
+                      ID="Script"
+                      code="placed_box_id := 'placed_box_' .. placed_count"
+                    />
+                    <!--Idempotent cleanup: if a previous attempt for this id failed mid-carry (after
+                        AttachObject but before DetachObject), the box is still attached to grasp_link and
+                        placed_count was never advanced, so this retry reuses the same id. Detach it first
+                        (no-op if nothing is attached) so AddCollisionMesh below is not blocked by a
+                        still-attached object of the same id.-->
+                    <Decorator ID="ForceSuccess">
+                      <Action ID="DetachObject" object_id="{placed_box_id}" />
+                    </Decorator>
+                    <Action
+                      ID="AddCollisionMesh"
+                      object_id="{placed_box_id}"
+                      mesh_file_path="package://hangar_sim/objectives/box.stl"
+                      pose="{target_pose}"
+                      scale="1;1;1"
+                      overwrite="true"
+                    />
+                    <Action
+                      ID="AttachObject"
+                      object_id="{placed_box_id}"
+                      link_name="grasp_link"
+                      allowed_collision_links="vacuum_suction_cups;collision_vacuum_suction_cups"
+                    />
                     <SubTree
                       ID="Move to Pose No Preview"
                       _collapsed="true"
@@ -343,6 +405,15 @@
                       timeout="10.000000"
                       gripper_command_action_name="/vacuum_gripper/gripper_cmd"
                     />
+                    <!--Box released in the loading zone: detach it from the gripper so it stays in the
+                        world planning scene at its placed pose, then advance the counter so the next box
+                        gets its own unique id. These persistent obstacles are what stop the base from
+                        driving over already-placed boxes on later picks.-->
+                    <Action ID="DetachObject" object_id="{placed_box_id}" />
+                    <Action
+                      ID="Script"
+                      code="placed_count := placed_count + 1"
+                    />
                   </Control>
                 </Decorator>
               </Control>
@@ -361,6 +432,7 @@
       <inout_port name="erosion_size" default="0" />
       <inout_port name="negative_prompts" default="{negative_prompts}" />
       <inout_port name="place_pose" default="{place_pose}" />
+      <inout_port name="placed_count" default="0" />
       <inout_port name="prompt" default="{prompt}" />
       <inout_port name="prompts" default="{prompts}" />
       <inout_port name="threshold" default="{threshold}" />