v1.1.0: Changes that allow replanning and greatly improve waiting times

README.md

@@ -50,7 +50,7 @@ With everything up and running ez_pick_and_place provides two services:
 
     * `string gripper_move_group`: Provide the name of the gripper move group.
 
-    * `bool allow_replanning`: Not used (yet).
+    * `int32 max_replanning`: Provide the number of maximum planning __retries__. This means that with `max_replanning=0` you will only get 1 try. GraspIt fails quite often, so it is advised to allow for a maximum of two or more retries.
 
 For further info on how to use the package, you can refer to the `test2_ez_pnp2.py` script under the `test` directory of this repository, which was used to create the the first animation of this doc.
 
@@ -99,7 +99,7 @@ Using Blender you can create and modify 3D models in order to make them useable
 
 ## Recommendations, reminders and extra info
 
-* In order for GraspIt to provide the algorithm with enough candidate grasp poses, we are using this [configuration file](https://github.com/Roboskel-Manipulation/manos_graspit_config/blob/master/config/graspit_planner_opt.yaml). You can experiment with your own configuration based on your needs, but always refer to the one provided.
+* In order for GraspIt to provide the algorithm with enough candidate grasp poses, we are using this [configuration file](https://github.com/Roboskel-Manipulation/manos_graspit_config/blob/master/config/graspit_planner_opt.yaml), when `max_planning` is set to 0. Normally, we set `max_planning` to 5, so we use this [configuration file](https://github.com/Roboskel-Manipulation/manos_graspit_config/blob/master/config/graspit_planner_opt_but_once.yaml) You can experiment with your own configuration based on your needs, but always refer to the ones provided.
 
 * Under normal conditions, do not use the `pose_factor` field of the `EzSceneSetup` service.
 
@@ -123,3 +123,4 @@ Awesome projects that without them, ez_pick_and_place would not be possible:
 * [GraspIt](https://graspit-simulator.github.io/): The feature-rich grasp pose generator and simulator.
 * [graspit-pkgs](https://github.com/JenniferBuehler/graspit-pkgs): The ultimate package for GraspIt/ROS integration.
 * [ivcon](https://github.com/ros/ivcon): The small-but-effective tool to convert your 3D models to all the required formats for MoveIt, GraspIt and beyond.
+* [Blender](https://www.blender.org/): The all-in-one (and open source) 3D modeling software.

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ez_pick_and_place</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>The ez_pick_and_place package</description>
 
   <maintainer email="stavrinosgeo@gmail.com">George Stavrinos</maintainer>

src/ez_tools.py

@@ -53,6 +53,10 @@ class EZToolSet():
 
     error_info = ""
 
+    replanning = 0
+
+    already_picked = False
+
     # Move the whole arm to the specified pose
     def move(self, pose):
         self.arm_move_group.set_pose_target(pose)
@@ -134,29 +138,33 @@ def uberPlan(self):
     # Open the gripper, move the arm to the grasping pose
     # and grab the object
     def pick(self):
-        # GraspIt assumes maxed out joints, so that's what we do here
-        self.openGripper()
-        time.sleep(1)
-        valid_g = self.discard(self.grasp_poses)
-
-        if len(valid_g) > 0:
-            for j in xrange(len(valid_g[0])):
-                self.arm_move_group.set_start_state_to_current_state()
-                if self.move(valid_g[0][j].pose):
-                    time.sleep(1)
-                    return self.grab(self.pose_n_joint[valid_g[0][j]])
-            self.error_info = "Error while trying to pick the object!"
-        else:
-            self.error_info = "No valid grasps were found!"
-        return False
+        if not self.already_picked:
+            # GraspIt assumes maxed out joints, so that's what we do here
+            self.openGripper()
+            time.sleep(1)
+            valid_g = self.discard(self.grasp_poses)
+
+            if len(valid_g) > 0:
+                for j in xrange(len(valid_g[0])):
+                    self.arm_move_group.set_start_state_to_current_state()
+                    if self.move(valid_g[0][j].pose):
+                        time.sleep(1)
+                        return self.grab(self.pose_n_joint[valid_g[0][j]])
+                self.error_info = "Error while trying to pick the object!"
+            else:
+                self.error_info = "No valid grasps were found!"
+            return False
+        return True
 
     # Move the arm to the place pose and open the gripper
     # to release the object
     def place(self):
         # Attached objects are removed from the MoveIt scene
         # so we have to query before we attach it
         obj_trans = self.moveit_scene.get_object_poses([self.object_to_grasp])
-        self.attachThis(self.object_to_grasp)
+        if not self.already_picked:
+            self.attachThis(self.object_to_grasp)
+            self.already_picked = True
         time.sleep(1)
         t, sol = self.calcTargetPose(obj_trans)
         if t and sol:
@@ -206,17 +214,27 @@ def startPlanning(self, req):
         self.object_to_grasp = req.graspit_target_object
         self.gripper_move_group_name = req.gripper_move_group
         self.target_place = req.target_place
+        self.replanning = req.max_replanning if req.max_replanning > 0 else 0
 
         # Get bounds for each gipper joint, so we can later use the graspit values
         self.getGripperBounds()
 
-        # Call graspit
-        graspit_grasps = self.graspThis(req.graspit_target_object)
+        res = False
+        while(self.replanning >= 0):
+            self.error_info = ""
+            if not self.already_picked:
+                # Call graspit
+                graspit_grasps = self.graspThis(req.graspit_target_object)
+
+                # Generate grasp poses
+                self.translateGraspIt2MoveIt(graspit_grasps, req.graspit_target_object)
 
-        # Generate grasp poses
-        self.translateGraspIt2MoveIt(graspit_grasps, req.graspit_target_object)
+            res = self.uberPlan()
+            self.replanning -= 1
+            if res:
+                break
 
-        return self.uberPlan(), self.error_info
+        return res, self.error_info
 
     # Graspit bodies are always referenced relatively to the "world" frame,
     # and units are not expressed in meters so translate the user's input

srv/EzStartPlanning.srv

@@ -2,7 +2,7 @@ string graspit_target_object
 geometry_msgs/PoseStamped target_place
 string arm_move_group
 string gripper_move_group
-bool allow_replanning
+int32 max_replanning
 ---
 bool success
 string info

test/test2_ez_pnp2.py

@@ -82,6 +82,7 @@ def main():
     doit.target_place = target_place
     doit.arm_move_group = "arm"
     doit.gripper_move_group = "gripper"
+    doit.max_replanning = 5
 
     response = start_planning_srv(doit)
 

test/test_ez_pnp2.py

@@ -83,7 +83,7 @@ def main():
     doit.target_place = target_place
     doit.arm_move_group = "arm"
     doit.gripper_move_group = "gripper"
-    # doit.allow_replanning = True
+    doit.max_replanning = 5
 
     response = start_planning_srv(doit)