Storing groceries 06 07

common/manipulation/lasr_manipulation_msgs/action/Place.action

@@ -1,6 +1,6 @@
 string object_id # name of object in the planning scene
 string surface_id # id of surface in planning scene 
-geometry_msgs/PoseStamped pose # pose of end-effector for placing.
+geometry_msgs/PoseStamped[] candidate_poses # pose of end-effector for placing.
 ---
 bool success
 ---

common/manipulation/lasr_manipulation_pipeline/config/motions.yaml

@@ -0,0 +1,82 @@
+# From https://github.com/pal-robotics/tiago_tutorials/blob/kinetic-devel/tiago_pick_demo/config/pick_motions.yaml
+play_motion:
+  motions:
+    pregrasp:
+      joints: [torso_lift_joint, arm_1_joint,
+      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+      arm_6_joint, arm_7_joint]
+      points:
+      - positions: [0.34, 0.07, -0.07, -3.0, 1.5, -1.57, 0.2, 0.0]
+        time_from_start: 17
+
+    post_grasp_pose:
+      joints: [torso_lift_joint, arm_1_joint,
+      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+      arm_6_joint, arm_7_joint]
+      points:  
+      - positions: [0.34, 0.21, -0.2, -2.2, 1.15, -1.57, 0.2, 0.0]
+        time_from_start: 10.
+
+    lift_torso:
+      joints: [torso_lift_joint, arm_1_joint,
+      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+      arm_6_joint, arm_7_joint]
+      points:  
+      - positions: [0.23, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0]
+        time_from_start: 2.0
+
+    home_to_pregrasp:
+      joints: [torso_lift_joint, arm_1_joint,
+      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+      arm_6_joint, arm_7_joint]
+      points:  
+      - positions: [0.23, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0] # 0.15
+        time_from_start: 0.0
+      - positions: [0.34, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0]
+        time_from_start: 2.0
+      - positions: [0.34, 0.21, -1.02, -0.20, 1.94, -1.57, 1.52, 0.0]
+        time_from_start: 3.5
+      - positions: [0.34, 0.21, 0.35, -0.2, 2.0, -1.57, 1.52, 0.0]
+        time_from_start: 6.5
+      - positions: [0.34, 0.21, 0.35, -0.2, 0.0, -1.57, 1.52, 0.0]
+        time_from_start: 10.0
+      - positions: [0.34, 0.21, 0.35, -3.0, 0.0, -1.57, 1.52, 0.0]
+        time_from_start: 12.0
+      - positions: [0.34, 0.07, -0.07, -3.0, 1.5, -1.57, 0.2, 0.0]
+        time_from_start: 17
+
+    grasp_to_home:
+      joints: [torso_lift_joint, arm_1_joint,
+      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+      arm_6_joint, arm_7_joint]
+      points:  
+      - positions: [0.34, 0.21, 0.35, -3.0, 0.0, -1.57, 1.37, 0.0]
+        time_from_start: 0.0
+      - positions: [0.34, 0.21, 0.35, -0.2, 0.0, -1.57, 1.37, 0.0]
+        time_from_start: 2.0
+      - positions: [0.34, 0.21, 0.35, -0.2, 2.0, -1.57, 1.37, 0.0]
+        time_from_start: 5.5
+      - positions: [0.34, 0.21, -1.02, -0.20, 1.94, -1.57, 1.37, 0.0]
+        time_from_start: 8.5
+      - positions: [0.34, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0]
+        time_from_start: 10.0
+      - positions: [0.15, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0]
+        time_from_start: 12
+
+    home_to_preplace:
+      joints: [torso_lift_joint, arm_1_joint,
+      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+      arm_6_joint, arm_7_joint]
+      points:  
+      - positions: [0.23, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0] # 0.15
+        time_from_start: 0.0
+      - positions: [0.34, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.0]
+        time_from_start: 2.0
+      - positions: [0.34, 0.21, -1.02, -0.20, 1.94, -1.57, 1.52, 0.0]
+        time_from_start: 3.5
+      - positions: [0.34, 0.21, 0.35, -0.2, 2.0, -1.57, 1.52, 0.0]
+        time_from_start: 6.5
+      - positions: [0.34, 0.21, 0.35, -0.2, 0.0, -1.57, 1.52, 0.0]
+        time_from_start: 10.0
+      - positions: [0.34, 0.21, 0.35, -1.5, 1.15, -1.57, 0.2, 0.0]
+        time_from_start: 13.0

common/manipulation/lasr_manipulation_pipeline/cfg/tiago.cfg → common/manipulation/lasr_manipulation_pipeline/config/tiago.cfg

@@ -64,7 +64,7 @@ deepen_hand = 1
 #   max_aperture: the maximum gripper width
 #   workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than <workspace>
 min_aperture = 0.0
-max_aperture = 0.75
+max_aperture = 0.75 
 workspace_grasps = -1.0 1.0 -1.0 1.0 -1.0 1.0
 
 # Filtering of candidates based on their approach direction
@@ -97,4 +97,4 @@ plot_candidates = 0
 plot_filtered_candidates = 0
 plot_valid_grasps = 0
 plot_clustered_grasps = 0
-plot_selected_grasps = 0
+plot_selected_grasps = 1

common/manipulation/lasr_manipulation_pipeline/nodes/pick_and_place_test.py

@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import rospy
 import rospkg
 import actionlib
@@ -21,7 +22,7 @@
 
 
 PACKAGE_PATH: str = rospkg.RosPack().get_path("lasr_manipulation_pipeline")
-MESH_NAME: str = "pringles.ply"
+MESH_NAME: str = "cereal.ply"
 
 
 class GraspingPipeline:
@@ -89,7 +90,7 @@ def run(self):
 
         pcl = sam.segment(pcl)
         # Register mesh with scene and apply the transformation
-        response = self._register_object(pcl, MESH_NAME.replace(".ply", ""), False, 1)
+        response = self._register_object(pcl, MESH_NAME.replace(".ply", ""), False, 30)
         ros_transform = response.transform
         ros_scale = response.scale
 
@@ -118,7 +119,9 @@ def run(self):
             rospy.loginfo("Pick failed. Will not place.")
             return
 
+        rospy.loginfo("Sending pregrasp motion...")
         self._play_motion("pregrasp")
+        rospy.loginfo("Pregrasp motion finished!")
 
         self._place_client.send_goal_and_wait(
             PlaceGoal(MESH_NAME.replace(".ply", ""), "table", pick_result.grasp_pose)