Skip to content

Add persistent collision objects for placed boxes in hangar_sim ML picking #727

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
davetcoleman wants to merge 1 commit into from
Closed

Add persistent collision objects for placed boxes in hangar_sim ML picking #727

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/hangar_sim/description/hangar_scene.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
4 changes: 2 additions & 2 deletions src/hangar_sim/description/vacuum.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
11 changes: 11 additions & 0 deletions src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,22 @@
_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"
pose_stamped="{place_pose}"
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">
Expand All @@ -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"
Expand Down
74 changes: 73 additions & 1 deletion src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}"
Expand Down Expand Up @@ -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"
Expand Down Expand Up @@ -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">
Expand Down Expand Up @@ -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"
Expand All @@ -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>
Expand All @@ -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}" />
Expand Down
Loading