pick_place_interface.py
Go to the documentation of this file.
00001 # Copyright 2015, Fetch Robotics Inc
00002 # Copyright 2011-2014, Michael Ferguson
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions
00007 # are met:
00008 #
00009 #  * Redistributions of source code must retain the above copyright
00010 #    notice, this list of conditions and the following disclaimer.
00011 #  * Redistributions in binary form must reproduce the above
00012 #    copyright notice, this list of conditions and the following
00013 #    disclaimer in the documentation and/or other materials provided
00014 #    with the distribution.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00017 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00018 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00019 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00020 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00022 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00024 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00025 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00026 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 
00029 import rospy
00030 import actionlib
00031 from moveit_msgs.msg import MoveItErrorCodes
00032 from moveit_msgs.msg import PickupAction, PickupGoal, PlaceAction, PlaceGoal
00033 
00034 ## @brief Simple interface to pick and place actions
00035 class PickPlaceInterface(object):
00036 
00037     ## @brief Create a grasp manager, connect actions
00038     ## @param group Name of arm planning group
00039     ## @param ee_group Name of end effector planning group
00040     ## @param plan_only Should we only plan, but not execute?
00041     def __init__(self,
00042                  group="arm",
00043                  ee_group="gripper",
00044                  plan_only=False,
00045                  verbose=False):
00046         self._verbose = verbose
00047         self._group = group
00048         self._effector = ee_group
00049         if self._verbose:
00050             rospy.loginfo("Connecting to pickup action...")
00051         self._pick_action = actionlib.SimpleActionClient("pickup",
00052                                                          PickupAction)
00053         self._pick_action.wait_for_server()
00054         if self._verbose:
00055             rospy.loginfo("...connected")
00056             rospy.loginfo("Connecting to place action...")
00057         self._place_action = actionlib.SimpleActionClient("place",
00058                                                           PlaceAction)
00059         self._place_action.wait_for_server()
00060         if self._verbose:
00061             rospy.loginfo("...connected")
00062         self._plan_only = plan_only
00063         self.planner_id = None
00064         self.allowed_planning_time = 30.0
00065 
00066     def get_pick_action(self):
00067         return self._pick_action
00068 
00069     def get_place_action(self):
00070         return self._place_action
00071 
00072     ## @brief Plan and grasp something
00073     ## @param name Name of the object to grasp
00074     ## @param grasps Grasps to try (moveit_msgs/Grasp)
00075     ## @param support_name Name of the support surface
00076     ## @returns moveit_msgs/PickupResult
00077     def pickup(self, name, grasps, wait=True, **kwargs):
00078         # Check arguments
00079         supported_args = ("allow_gripper_support_collision",
00080                           "allowed_touch_objects",
00081                           "plan_only",
00082                           "planner_id",
00083                           "planning_scene_diff",
00084                           "planning_time",
00085                           "support_name")
00086         for arg in kwargs.keys():
00087             if not arg in supported_args:
00088                 rospy.loginfo("pickup: unsupported argument: %s", arg)
00089 
00090         # Create goal
00091         g = PickupGoal()
00092 
00093         # 1. Fill in target name
00094         g.target_name = name
00095 
00096         # 2. Fill in group name
00097         g.group_name = self._group
00098 
00099         # 3. Fill in end effector
00100         g.end_effector = self._effector
00101 
00102         # 4. List of grasps
00103         g.possible_grasps = grasps
00104 
00105         # 5. Fill in support surface
00106         try:
00107             g.support_surface_name = kwargs["support_name"]
00108         except KeyError:
00109             g.support_surface_name = ""
00110 
00111         # 6. Can gripper contact the support surface (named above)
00112         try:
00113             g.allow_gripper_support_collision = kwargs["allow_gripper_support_collision"]
00114         except KeyError:
00115             g.allow_gripper_support_collision = True
00116 
00117         # 7. What links are part of the "gripper" (and can contact the object/surface)
00118         try:
00119             g.attached_object_touch_links = kwargs["attached_object_touch_links"]
00120         except KeyError:
00121             g.attached_object_touch_links = list() # empty list = use all links of end-effector
00122 
00123         # 8. Fill in minimize_object_distance
00124 
00125         # 9. Fill in path_constraints
00126 
00127         # 10. Fill in planner id
00128         try:
00129             g.planner_id = kwargs["planner_id"]
00130         except KeyError:
00131             if self.planner_id:
00132                 g.planner_id = self.planner_id
00133 
00134         # 11. List of obstacles that we are allowed to touch
00135         try:
00136             g.allowed_touch_objects = kwargs["allowed_touch_objects"]
00137         except KeyError:
00138             g.allowed_touch_objects = list()
00139 
00140         # 12. Planning time
00141         try:
00142             g.allowed_planning_time = kwargs["planning_time"]
00143         except KeyError:
00144             g.allowed_planning_time = self.allowed_planning_time
00145 
00146         # 13. Planning options
00147         try:
00148             g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
00149         except KeyError:
00150             g.planning_options.planning_scene_diff.is_diff = True
00151             g.planning_options.planning_scene_diff.robot_state.is_diff = True
00152         g.planning_options.plan_only = self._plan_only
00153 
00154         self._pick_action.send_goal(g)
00155         if wait:
00156             self._pick_action.wait_for_result()
00157             return self._pick_action.get_result()
00158         else:
00159             return None
00160 
00161     ## @brief Plan and grasp something
00162     ## @param name Name of the object to grasp
00163     ## @param grasps Grasps to try (moveit_msgs/Grasp)
00164     ## @param support_name Name of the support surface
00165     ## @param goal_is_eef Set to true if the place goal is for the
00166     ##        end effector frame, default is object frame.
00167     ## @returns moveit_msgs/PlaceResult
00168     def place(self, name, locations, wait=True, **kwargs):
00169         # Check arguments
00170         supported_args = ("allow_gripper_support_collision",
00171                           "allowed_touch_objects",
00172                           "goal_is_eef",
00173                           "plan_only",
00174                           "planner_id",
00175                           "planning_scene_diff",
00176                           "planning_time",
00177                           "support_name")
00178         for arg in kwargs.keys():
00179             if not arg in supported_args:
00180                 rospy.loginfo("place: unsupported argument: %s", arg)
00181 
00182         # Create goal
00183         g = PlaceGoal()
00184 
00185         # 1. Name of ARM planning group
00186         g.group_name = self._group
00187 
00188         # 2. Name of attached object to place
00189         g.attached_object_name = name
00190 
00191         # 3. Possible locations to place
00192         g.place_locations = locations
00193 
00194         # 4. If true, using eef pose (same as pick)
00195         try:
00196             g.place_eef = kwargs["goal_is_eef"]
00197         except KeyError:
00198             g.place_eef = False
00199 
00200         # 5. Fill in support surface
00201         try:
00202             g.support_surface_name = kwargs["support_name"]
00203         except KeyError:
00204             g.support_surface_name = ""
00205 
00206         # 6. Can gripper contact the support surface (named above)
00207         try:
00208             g.allow_gripper_support_collision = kwargs["allow_gripper_support_collision"]
00209         except KeyError:
00210             g.allow_gripper_support_collision = True
00211 
00212         # 8. Fill in path_constraints
00213 
00214         # 9. Fill in planner id
00215         try:
00216             g.planner_id = kwargs["planner_id"]
00217         except KeyError:
00218             if self.planner_id:
00219                 g.planner_id = self.planner_id
00220 
00221         # 10. List of obstacles that we are allowed to touch
00222         try:
00223             g.allowed_touch_objects = kwargs["allowed_touch_objects"]
00224         except KeyError:
00225             g.allowed_touch_objects = list()
00226 
00227         # 11. Planning time
00228         try:
00229             g.allowed_planning_time = kwargs["planning_time"]
00230         except KeyError:
00231             g.allowed_planning_time = self.allowed_planning_time
00232 
00233         # 12. Planning options
00234         try:
00235             g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
00236         except KeyError:
00237             g.planning_options.planning_scene_diff.is_diff = True
00238             g.planning_options.planning_scene_diff.robot_state.is_diff = True
00239         g.planning_options.plan_only = self._plan_only
00240 
00241         self._place_action.send_goal(g)
00242         if wait:
00243             self._place_action.wait_for_result()
00244             return self._place_action.get_result()
00245         else:
00246             return None
00247 
00248     ## Common usage pattern
00249     ## TODO document
00250     def pick_with_retry(self, name, grasps, retries=5, scene=None, **kwargs):
00251         if self._verbose:
00252             rospy.loginfo("Beginning to pick.")
00253         while retries > 0:
00254             retries += -1
00255             pick_result = self.pickup(name, grasps, **kwargs)
00256             if pick_result.error_code.val == MoveItErrorCodes.SUCCESS:
00257                 rospy.loginfo("Pick succeeded")
00258                 return [True, pick_result]
00259             elif pick_result.error_code.val == MoveItErrorCodes.PLANNING_FAILED:
00260                 rospy.logerr("Pick failed in the planning stage, try again...")
00261                 rospy.sleep(0.5)  # short sleep to try and let state settle a bit?
00262                 continue
00263             elif scene and \
00264                 pick_result.error_code.val == MoveItErrorCodes.CONTROL_FAILED or \
00265                 pick_result.error_code.val == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE or \
00266                 pick_result.error_code.val == MoveItErrorCodes.TIMED_OUT:
00267                 rospy.logerr("Pick failed during execution, attempting to cleanup.")
00268                 if name in scene.getKnownAttachedObjects():
00269                     rospy.loginfo("Pick managed to grab object, retreat must have failed, continuing anyways")
00270                     return [True, pick_result]
00271                 else:
00272                     rospy.loginfo("Pick did not grab object, try again...")
00273                     continue
00274             else:
00275                 rospy.logerr("Pick failed with error code: %d. Will retry...",
00276                              pick_result.error_code.val)
00277                 continue
00278         rospy.logerr("Pick failed, and all retries are used")
00279         return [False, pick_result]
00280 
00281     ## Common usage pattern
00282     ## TODO document
00283     def place_with_retry(self,
00284                          name,
00285                          locations,
00286                          retries=5,
00287                          scene=None,
00288                          **kwargs):
00289         if self._verbose:
00290             rospy.loginfo("Beginning to place.")
00291         while retries > 0:
00292             retries += -1
00293             place_result = self.place(name, locations, **kwargs)
00294             if place_result.error_code.val == MoveItErrorCodes.SUCCESS:
00295                 rospy.loginfo("Place succeeded")
00296                 return [True, place_result]
00297             elif place_result.error_code.val == MoveItErrorCodes.PLANNING_FAILED:
00298                 rospy.logerr("Place failed in planning stage, try again...")
00299                 rospy.sleep(0.5)  # short sleep to let state settle a bit?
00300                 continue
00301             elif scene and \
00302                  place_result.error_code.val == MoveItErrorCodes.CONTROL_FAILED or \
00303                  place_result.error_code.val == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE or \
00304                  place_result.error_code.val == MoveItErrorCodes.TIMED_OUT:
00305                 rospy.logerr("Place failed during execution, attempting to cleanup.")
00306                 if name in scene.getKnownAttachedObjects():
00307                     rospy.loginfo("Place did not place object, approach must have failed, will retry...")
00308                     continue
00309                 else:
00310                     rospy.loginfo("Object no longer in gripper, must be placed, continuing...")
00311                     return [True, place_result]
00312             else:
00313                 rospy.logerr("Place failed with error code: %d. Will retry...",
00314                              place_result.error_code.val)
00315                 continue
00316         rospy.logerr("Place failed, and all retries are used")
00317         return [False, place_result]


moveit_python
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:12:36