Segment and Pick Anything (#268)

* Add the code for a Segment Anything assisted pick object (aka generic  pick object)

* Added config with better grasps and click points

Co-authored-by: Paul Gesel <paulgesel@gmail.com>

* Remove sensor name port variable for SendPointCloudToUI now that were on 6.0

Co-authored-by: David Yackzan <dwyackzan@gmail.com>

---------

Co-authored-by: Paul Gesel <paulgesel@gmail.com>
Co-authored-by: David Yackzan <dwyackzan@gmail.com>

README.md

@@ -9,7 +9,7 @@ MoveIt Pro can be used with real robots and full simulators such as Gazebo and N
 For testing purposes, you can also use the [ROS 2 Control Mock Components](https://control.ros.org/master/doc/ros2_control/hardware_interface/doc/mock_components_userdoc.html), which is what this repo is configured to use.
 
 This workspace offers a reasonable starting point for those users looking to develop with MoveIt Pro using custom base and site configurations.
-For more information refer to the [online documentation](https://docs.picknik.ai/en/stable/).
+For more information refer to the [online documentation](https://docs.picknik.ai).
 
 ## Universal Robots Configuration Packages
 

src/picknik_ur_site_config/objectives/sam_pick_object.xml

@@ -0,0 +1,40 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="SAM Pick Object">
+  <!--//////////-->
+  <BehaviorTree ID="SAM Pick Object" _description="Uses SAM to segment out an arbitrary object and pick it.">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="LoadObjectiveParameters" parameters="{pick_parameters}" config_file_name="sam_pick_object_config.yaml"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Hinge View" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" _collapsed="true"/>
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
+      <!--Get the point cloud from the wrist camera-->
+      <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}" timeout_sec="5.000000"/>
+      <!--Segment out 2D masks and then convert to 3D masks and visualize in UI-->
+      <Action ID="GetPointsFromUser" point_prompts="Click on the object you want to pick." point_names="clicked_object" pixel_coords="{pixel_coords}" view_name="/wrist_mounted_camera/color/image_raw"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_superseg" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.0002" max_relative_area="0.25" timeout_sec="15.0" masks2d="{masks2d}" valid_points="{pixel_coords}" valid_classes=""/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
+      <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
+        <Control ID="Sequence">
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" pcd_topic="/pcd_pointcloud_captures"/>
+        </Control>
+      </Decorator>
+      <!--Convert 3D masks into graspable objects-->
+      <Action ID="GetGraspableObjectsFromMasks3D" graspable_objects="{objects}" plane_inlier_threshold="0.020000" base_frame="world" minimum_face_area="0" masks3d="{masks3d}" face_separation_threshold="0.025000" point_cloud="{point_cloud}"/>
+      <Decorator ID="ForEachGraspableObject" vector_in="{objects}" out="{object}">
+        <Control ID="Sequence">
+          <Action ID="InitializeMTCTask" task="{mtc_task}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task_id=""/>
+          <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+          <Action ID="SetupMTCApproachGrasp" parameters="{pick_parameters}" target_object="{object}" monitored_stage="{monitored_stage}" task="{mtc_task}"/>
+          <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{pick_parameters}" target_object="{object}" monitored_stage="{monitored_stage}" task="{mtc_task}"/>
+          <Action ID="SetupMTCRetractFromGrasp" parameters="{pick_parameters}" target_object="{object}" task="{mtc_task}"/>
+          <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
+          <SubTree ID="Wait for Trajectory Approval if User Available" _collapsed="false" solution="{mtc_solution}"/>
+          <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>

src/picknik_ur_site_config/objectives/sam_pick_object_config.yaml

@@ -0,0 +1,71 @@
+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: "grasp_link"
+
+  approach_distance: 0.15
+
+  # Set to true to make the robot keep the object within view of its camera while reaching to grasp it
+  enforce_visibility_constraint: true
+  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
+  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: "grasp_link"
+  end_effector_closed_pose_name: "close"
+  ui_grasp_link: "grasp_link"
+
+  # Grasp candidate configs
+  grasp_candidate_config:
+    generate_x_axis_grasps: true
+    generate_y_axis_grasps: true
+    generate_z_axis_grasps: true
+
+    # Number of samples to generate for each quadrant of the cuboid.
+    samples_per_quadrant: 3
+
+  # Grasp data configs
+  grasp_data:
+    # See https://ros-planning.github.io/moveit_tutorials/_images/finger_gripper_explanation.jpg for description of the following parameters
+    # Note: these parameters are copied from Robotiq gripper without tweaking
+    # min/max values of the grasp depth range in meter.
+    # This should fit to the distance from finger tip to inner palm or the suction cup stroke
+    grasp_min_depth: 0.025 # minimum amount fingers must overlap object
+    grasp_max_depth: 0.065 # Maximum distance from tip of end effector inwards that an object can be for a grasp
+
+    # Maximum allowed finger width for a grasp.
+    # This value should be considerably smaller than max_finger_width
+    # to allow padded collision checks
+    max_grasp_width: 0.4
+
+    # The transform from the arm IK link to the grasp point to align it with the following convention.
+    #
+    # z-axis pointing toward object to grasp
+    # x-axis perpendicular to movement of grippers
+    # y-axis parallel to movement of grippers
+    eef_mount_to_tcp_transform:
+      x: 0.0
+      y: 0.0
+      z: -0.02
+      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: "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