New Pick Cuboids Objectives (#73)

src/picknik_ur_gazebo_config/objectives/looping_pick_and_place_object_config.yaml

@@ -22,18 +22,14 @@ FindSingularCuboids:
   crop_box_size_y: 1.5
   crop_box_size_z: 0.5
 
-SetupMTCPickCuboid:
-  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+SetupMTCApproachGrasp:
   world_frame_name: "world"
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
   hand_frame_name: "manual_grasp_link"
-  end_effector_closed_pose_name: "close"
-  ui_grasp_link: "manual_grasp_link"
 
   approach_distance: 0.15
-  lift_distance: 0.1
 
   # Set to true to make the robot keep the object within view of its camera while reaching to grasp it
   enforce_visibility_constraint: true
@@ -42,6 +38,13 @@ SetupMTCPickCuboid:
   sensor_z_offset: 0.02
   target_diameter: 0.0  # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.
 
+SetupMTCGenerateCuboidGrasps:
+  end_effector_group_name: "gripper"
+  end_effector_name: "moveit_ee"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+  ui_grasp_link: "manual_grasp_link"
+
   # Grasp candidate configs
   grasp_candidate_config:
     generate_x_axis_grasps: true
@@ -77,3 +80,14 @@ SetupMTCPickCuboid:
       roll: 0.0
       pitch: 0.0
       yaw: 1.57
+
+SetupMTCRetractFromGrasp:
+  world_frame_name: "world"
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+
+  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+  approach_distance: 0.15
+  lift_distance: 0.1

src/picknik_ur_gazebo_config/objectives/pick_place_object_config.yaml

@@ -22,18 +22,14 @@ FindSingularCuboids:
   crop_box_size_y: 0.75
   crop_box_size_z: 0.5
 
-SetupMTCPickCuboid:
-  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+SetupMTCPickApproachGrasp:
   world_frame_name: "world"
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
   hand_frame_name: "manual_grasp_link"
-  end_effector_closed_pose_name: "close"
-  ui_grasp_link: "manual_grasp_link"
 
   approach_distance: 0.15
-  lift_distance: 0.1
 
   # Set to true to make the robot keep the object within view of its camera while reaching to grasp it
   enforce_visibility_constraint: true
@@ -42,6 +38,13 @@ SetupMTCPickCuboid:
   sensor_z_offset: 0.02
   target_diameter: 0.0  # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.
 
+SetupMTCGenerateCuboidGrasps:
+  end_effector_group_name: "gripper"
+  end_effector_name: "moveit_ee"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+  ui_grasp_link: "manual_grasp_link"
+
   # Grasp candidate configs
   grasp_candidate_config:
     generate_x_axis_grasps: true
@@ -78,6 +81,17 @@ SetupMTCPickCuboid:
       pitch: 0.0
       yaw: 1.57
 
+SetupMTCRetractFromGrasp:
+  world_frame_name: "world"
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+
+  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+  approach_distance: 0.15
+  lift_distance: 0.1
+
 IsConstraintSatisfied:
   camera_optical_frame_id: "wrist_mounted_camera_color_optical_frame"
   camera_field_of_view_angle: 1.0472  # radians: approx. 60 degrees, which is narrower than the 70 degree diagonal FOV of the D415 camera

src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml

@@ -54,7 +54,9 @@
                                             <!-- Set up an MTC task to pick the cuboid and then plan the task. -->
                                             <Action ID="InitializeMTCTask" task_id="looping_pick_and_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
                                             <Action ID="SetupMTCCurrentState" task="{task}"/>
-                                            <Action ID="SetupMTCPickCuboid" cuboid_object="{cuboid_object}" task="{task}" parameters="{parameters}"/>
+                                            <Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                                            <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                                            <Action ID="SetupMTCRetractFromGrasp" parameters="{parameters}" target_object="{cuboid_object}" task="{task}"/>
                                             <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>
 
                                             <!-- Execute the planned motion. -->

src/picknik_ur_site_config/objectives/looping_pick_and_place_object_config.yaml

@@ -22,18 +22,15 @@ FindSingularCuboids:
   crop_box_size_y: 0.75
   crop_box_size_z: 0.5
 
-SetupMTCPickCuboid:
+SetupMTCApproachGrasp:
   # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
   world_frame_name: "world"
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
   hand_frame_name: "manual_grasp_link"
-  end_effector_closed_pose_name: "close"
-  ui_grasp_link: "manual_grasp_link"
 
   approach_distance: 0.15
-  lift_distance: 0.1
 
   # Set to true to make the robot keep the object within view of its camera while reaching to grasp it
   enforce_visibility_constraint: true
@@ -42,6 +39,14 @@ SetupMTCPickCuboid:
   sensor_z_offset: 0.02
   target_diameter: 0.0  # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.
 
+
+SetupMTCGenerateCuboidGrasps:
+  end_effector_group_name: "gripper"
+  end_effector_name: "moveit_ee"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+  ui_grasp_link: "manual_grasp_link"
+
   # Grasp candidate configs
   grasp_candidate_config:
     generate_x_axis_grasps: true
@@ -77,3 +82,14 @@ SetupMTCPickCuboid:
       roll: 0.0
       pitch: 0.0
       yaw: 1.57
+
+SetupMTCRetractFromGrasp:
+  world_frame_name: "world"
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+
+  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+  approach_distance: 0.15
+  lift_distance: 0.1

src/picknik_ur_site_config/objectives/pick_place_object.xml

@@ -34,7 +34,9 @@
                             <!-- Set up an MTC task to pick the cuboid and then plan the task. -->
                             <Action ID="InitializeMTCTask" task_id="pick_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
                             <Action ID="SetupMTCCurrentState" task="{task}"/>
-                            <Action ID="SetupMTCPickCuboid" cuboid_object="{cuboid_object}" task="{task}" parameters="{parameters}"/>
+                            <Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                            <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                            <Action ID="SetupMTCRetractFromGrasp" parameters="{parameters}" target_object="{cuboid_object}" task="{task}"/>
                             <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>
 
                             <!-- Send trajectory preview to user only if this is the first time trying to pick the object. -->

src/picknik_ur_site_config/objectives/pick_place_object_config.yaml

@@ -22,8 +22,7 @@ FindSingularCuboids:
   crop_box_size_y: 0.75
   crop_box_size_z: 0.5
 
-SetupMTCPickCuboid:
-  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+SetupMTCApproachGrasp:
   world_frame_name: "world"
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
@@ -33,7 +32,6 @@ SetupMTCPickCuboid:
   ui_grasp_link: "manual_grasp_link"
 
   approach_distance: 0.15
-  lift_distance: 0.1
 
   # Set to true to make the robot keep the object within view of its camera while reaching to grasp it
   enforce_visibility_constraint: true
@@ -51,6 +49,13 @@ SetupMTCPickCuboid:
     # Number of samples to generate for each quadrant of the cuboid.
     samples_per_quadrant: 3
 
+SetupMTCGenerateCuboidGrasps:
+  end_effector_group_name: "gripper"
+  end_effector_name: "moveit_ee"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+  ui_grasp_link: "manual_grasp_link"
+
   # Grasp data configs
   grasp_data:
     # See https://ros-planning.github.io/moveit_tutorials/_images/finger_gripper_explanation.jpg for description of the following parameters
@@ -78,6 +83,17 @@ SetupMTCPickCuboid:
       pitch: 0.0
       yaw: 1.57
 
+SetupMTCRetractFromGrasp:
+  world_frame_name: "world"
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  hand_frame_name: "manual_grasp_link"
+  end_effector_closed_pose_name: "close"
+
+  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
+  approach_distance: 0.15
+  lift_distance: 0.1
+
 IsConstraintSatisfied:
   camera_optical_frame_id: "wrist_mounted_camera_color_optical_frame"
   camera_field_of_view_angle: 1.0472  # radians: approx. 60 degrees, which is narrower than the 70 degree diagonal FOV of the D415 camera