pick_place_interface.py
Go to the documentation of this file.
1 # Copyright 2015, Fetch Robotics Inc
2 # Copyright 2011-2014, Michael Ferguson
3 # All rights reserved.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions
7 # are met:
8 #
9 # * Redistributions of source code must retain the above copyright
10 # notice, this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above
12 # copyright notice, this list of conditions and the following
13 # disclaimer in the documentation and/or other materials provided
14 # with the distribution.
15 #
16 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
19 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
20 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
22 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
24 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
26 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 # POSSIBILITY OF SUCH DAMAGE.
28 
29 import rospy
30 import actionlib
31 from moveit_msgs.msg import MoveItErrorCodes
32 from moveit_msgs.msg import PickupAction, PickupGoal, PlaceAction, PlaceGoal
33 
34 
35 class PickPlaceInterface(object):
36 
37 
41  def __init__(self,
42  group="arm",
43  ee_group="gripper",
44  plan_only=False,
45  verbose=False):
46  self._verbose = verbose
47  self._group = group
48  self._effector = ee_group
49  if self._verbose:
50  rospy.loginfo("Connecting to pickup action...")
52  PickupAction)
53  self._pick_action.wait_for_server()
54  if self._verbose:
55  rospy.loginfo("...connected")
56  rospy.loginfo("Connecting to place action...")
58  PlaceAction)
59  self._place_action.wait_for_server()
60  if self._verbose:
61  rospy.loginfo("...connected")
62  self._plan_only = plan_only
63  self.planner_id = None
64  self.allowed_planning_time = 30.0
65 
66  def get_pick_action(self):
67  return self._pick_action
68 
69  def get_place_action(self):
70  return self._place_action
71 
72 
77  def pickup(self, name, grasps, wait=True, **kwargs):
78  # Check arguments
79  supported_args = ("allow_gripper_support_collision",
80  "attached_object_touch_links",
81  "allowed_touch_objects",
82  "plan_only",
83  "planner_id",
84  "planning_scene_diff",
85  "planning_time",
86  "support_name")
87  for arg in kwargs.keys():
88  if not arg in supported_args:
89  rospy.loginfo("pickup: unsupported argument: %s", arg)
90 
91  # Create goal
92  g = PickupGoal()
93 
94  # 1. Fill in target name
95  g.target_name = name
96 
97  # 2. Fill in group name
98  g.group_name = self._group
99 
100  # 3. Fill in end effector
101  g.end_effector = self._effector
102 
103  # 4. List of grasps
104  g.possible_grasps = grasps
105 
106  # 5. Fill in support surface
107  try:
108  g.support_surface_name = kwargs["support_name"]
109  except KeyError:
110  g.support_surface_name = ""
111 
112  # 6. Can gripper contact the support surface (named above)
113  try:
114  g.allow_gripper_support_collision = kwargs["allow_gripper_support_collision"]
115  except KeyError:
116  g.allow_gripper_support_collision = True
117 
118  # 7. What links are part of the "gripper" (and can contact the object/surface)
119  try:
120  g.attached_object_touch_links = kwargs["attached_object_touch_links"]
121  except KeyError:
122  g.attached_object_touch_links = list() # empty list = use all links of end-effector
123 
124  # 8. Fill in minimize_object_distance
125 
126  # 9. Fill in path_constraints
127 
128  # 10. Fill in planner id
129  try:
130  g.planner_id = kwargs["planner_id"]
131  except KeyError:
132  if self.planner_id:
133  g.planner_id = self.planner_id
134 
135  # 11. List of obstacles that we are allowed to touch
136  try:
137  g.allowed_touch_objects = kwargs["allowed_touch_objects"]
138  except KeyError:
139  g.allowed_touch_objects = list()
140 
141  # 12. Planning time
142  try:
143  g.allowed_planning_time = kwargs["planning_time"]
144  except KeyError:
145  g.allowed_planning_time = self.allowed_planning_time
146 
147  # 13. Planning options
148  try:
149  g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
150  except KeyError:
151  g.planning_options.planning_scene_diff.is_diff = True
152  g.planning_options.planning_scene_diff.robot_state.is_diff = True
153  g.planning_options.plan_only = self._plan_only
154 
155  self._pick_action.send_goal(g)
156  if wait:
157  self._pick_action.wait_for_result()
158  return self._pick_action.get_result()
159  else:
160  return None
161 
162 
169  def place(self, name, locations, wait=True, **kwargs):
170  # Check arguments
171  supported_args = ("allow_gripper_support_collision",
172  "allowed_touch_objects",
173  "goal_is_eef",
174  "plan_only",
175  "planner_id",
176  "planning_scene_diff",
177  "planning_time",
178  "support_name")
179  for arg in kwargs.keys():
180  if not arg in supported_args:
181  rospy.loginfo("place: unsupported argument: %s", arg)
182 
183  # Create goal
184  g = PlaceGoal()
185 
186  # 1. Name of ARM planning group
187  g.group_name = self._group
188 
189  # 2. Name of attached object to place
190  g.attached_object_name = name
191 
192  # 3. Possible locations to place
193  g.place_locations = locations
194 
195  # 4. If true, using eef pose (same as pick)
196  try:
197  g.place_eef = kwargs["goal_is_eef"]
198  except KeyError:
199  g.place_eef = False
200 
201  # 5. Fill in support surface
202  try:
203  g.support_surface_name = kwargs["support_name"]
204  except KeyError:
205  g.support_surface_name = ""
206 
207  # 6. Can gripper contact the support surface (named above)
208  try:
209  g.allow_gripper_support_collision = kwargs["allow_gripper_support_collision"]
210  except KeyError:
211  g.allow_gripper_support_collision = True
212 
213  # 8. Fill in path_constraints
214 
215  # 9. Fill in planner id
216  try:
217  g.planner_id = kwargs["planner_id"]
218  except KeyError:
219  if self.planner_id:
220  g.planner_id = self.planner_id
221 
222  # 10. List of obstacles that we are allowed to touch
223  try:
224  g.allowed_touch_objects = kwargs["allowed_touch_objects"]
225  except KeyError:
226  g.allowed_touch_objects = list()
227 
228  # 11. Planning time
229  try:
230  g.allowed_planning_time = kwargs["planning_time"]
231  except KeyError:
232  g.allowed_planning_time = self.allowed_planning_time
233 
234  # 12. Planning options
235  try:
236  g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
237  except KeyError:
238  g.planning_options.planning_scene_diff.is_diff = True
239  g.planning_options.planning_scene_diff.robot_state.is_diff = True
240  g.planning_options.plan_only = self._plan_only
241 
242  self._place_action.send_goal(g)
243  if wait:
244  self._place_action.wait_for_result()
245  return self._place_action.get_result()
246  else:
247  return None
248 
249 
251  def pick_with_retry(self, name, grasps, retries=5, scene=None, **kwargs):
252  if self._verbose:
253  rospy.loginfo("Beginning to pick.")
254  while retries > 0:
255  retries += -1
256  pick_result = self.pickup(name, grasps, **kwargs)
257  if pick_result.error_code.val == MoveItErrorCodes.SUCCESS:
258  rospy.loginfo("Pick succeeded")
259  return [True, pick_result]
260  elif pick_result.error_code.val == MoveItErrorCodes.PLANNING_FAILED:
261  rospy.logerr("Pick failed in the planning stage, try again...")
262  rospy.sleep(0.5) # short sleep to try and let state settle a bit?
263  continue
264  elif not scene is None and \
265  (pick_result.error_code.val == MoveItErrorCodes.CONTROL_FAILED or \
266  pick_result.error_code.val == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE or \
267  pick_result.error_code.val == MoveItErrorCodes.TIMED_OUT):
268  rospy.logerr("Pick failed during execution, attempting to cleanup.")
269  if name in scene.getKnownAttachedObjects():
270  rospy.loginfo("Pick managed to grab object, retreat must have failed, continuing anyways")
271  return [True, pick_result]
272  else:
273  rospy.loginfo("Pick did not grab object, try again...")
274  continue
275  else:
276  rospy.logerr("Pick failed with error code: %d. Will retry...",
277  pick_result.error_code.val)
278  continue
279  rospy.logerr("Pick failed, and all retries are used")
280  return [False, pick_result]
281 
282 
285  name,
286  locations,
287  retries=5,
288  scene=None,
289  **kwargs):
290  if self._verbose:
291  rospy.loginfo("Beginning to place.")
292  while retries > 0:
293  retries += -1
294  place_result = self.place(name, locations, **kwargs)
295  if place_result.error_code.val == MoveItErrorCodes.SUCCESS:
296  rospy.loginfo("Place succeeded")
297  return [True, place_result]
298  elif place_result.error_code.val == MoveItErrorCodes.PLANNING_FAILED:
299  rospy.logerr("Place failed in planning stage, try again...")
300  rospy.sleep(0.5) # short sleep to let state settle a bit?
301  continue
302  elif not scene is None and \
303  (place_result.error_code.val == MoveItErrorCodes.CONTROL_FAILED or \
304  place_result.error_code.val == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE or \
305  place_result.error_code.val == MoveItErrorCodes.TIMED_OUT):
306  rospy.logerr("Place failed during execution, attempting to cleanup.")
307  if name in scene.getKnownAttachedObjects():
308  rospy.loginfo("Place did not place object, approach must have failed, will retry...")
309  continue
310  else:
311  rospy.loginfo("Object no longer in gripper, must be placed, continuing...")
312  return [True, place_result]
313  else:
314  rospy.logerr("Place failed with error code: %d. Will retry...",
315  place_result.error_code.val)
316  continue
317  rospy.logerr("Place failed, and all retries are used")
318  return [False, place_result]
moveit_python.pick_place_interface.PickPlaceInterface._verbose
_verbose
Definition: pick_place_interface.py:42
moveit_python.pick_place_interface.PickPlaceInterface.pick_with_retry
def pick_with_retry(self, name, grasps, retries=5, scene=None, **kwargs)
Common usage pattern TODO document.
Definition: pick_place_interface.py:251
moveit_python.pick_place_interface.PickPlaceInterface._group
_group
Definition: pick_place_interface.py:43
moveit_python.pick_place_interface.PickPlaceInterface._effector
_effector
Definition: pick_place_interface.py:44
moveit_python.pick_place_interface.PickPlaceInterface.planner_id
planner_id
Definition: pick_place_interface.py:59
moveit_python.pick_place_interface.PickPlaceInterface.place
def place(self, name, locations, wait=True, **kwargs)
Plan and grasp something.
Definition: pick_place_interface.py:169
moveit_python.pick_place_interface.PickPlaceInterface.allowed_planning_time
allowed_planning_time
Definition: pick_place_interface.py:60
moveit_python.pick_place_interface.PickPlaceInterface.get_pick_action
def get_pick_action(self)
Definition: pick_place_interface.py:66
moveit_python.pick_place_interface.PickPlaceInterface.place_with_retry
def place_with_retry(self, name, locations, retries=5, scene=None, **kwargs)
Common usage pattern TODO document.
Definition: pick_place_interface.py:284
actionlib::SimpleActionClient
moveit_python.pick_place_interface.PickPlaceInterface.__init__
def __init__(self, group="arm", ee_group="gripper", plan_only=False, verbose=False)
Create a grasp manager, connect actions.
Definition: pick_place_interface.py:41
moveit_python.pick_place_interface.PickPlaceInterface.get_place_action
def get_place_action(self)
Definition: pick_place_interface.py:69
moveit_python.pick_place_interface.PickPlaceInterface._place_action
_place_action
Definition: pick_place_interface.py:53
moveit_python.pick_place_interface.PickPlaceInterface
Simple interface to pick and place actions.
Definition: pick_place_interface.py:35
moveit_python.pick_place_interface.PickPlaceInterface.pickup
def pickup(self, name, grasps, wait=True, **kwargs)
Plan and grasp something.
Definition: pick_place_interface.py:77
moveit_python.pick_place_interface.PickPlaceInterface._plan_only
_plan_only
Definition: pick_place_interface.py:58
moveit_python.pick_place_interface.PickPlaceInterface._pick_action
_pick_action
Definition: pick_place_interface.py:47


moveit_python
Author(s): Michael Ferguson
autogenerated on Sun Jan 8 2023 03:16:41