move_group.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Ioan Sucan, William Baker
34 
35 from geometry_msgs.msg import Pose, PoseStamped
36 from moveit_msgs.msg import RobotTrajectory, Grasp, PlaceLocation, Constraints, RobotState
37 from moveit_msgs.msg import MoveItErrorCodes, TrajectoryConstraints, PlannerInterfaceDescription
38 from sensor_msgs.msg import JointState
39 import rospy
40 import tf
41 from moveit_ros_planning_interface import _moveit_move_group_interface
42 from .exception import MoveItCommanderException
43 import moveit_commander.conversions as conversions
44 
45 
46 class MoveGroupCommander(object):
47  """
48  Execution of simple commands for a particular group
49  """
50 
51  def __init__(self, name, robot_description="robot_description", ns="", wait_for_servers=5.0):
52  """ Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """
53  self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description, ns, wait_for_servers)
54 
55  def get_name(self):
56  """ Get the name of the group this instance was initialized for """
57  return self._g.get_name()
58 
59  def stop(self):
60  """ Stop the current execution, if any """
61  self._g.stop()
62 
63  def get_active_joints(self):
64  """ Get the active joints of this group """
65  return self._g.get_active_joints()
66 
67  def get_joints(self):
68  """ Get the joints of this group """
69  return self._g.get_joints()
70 
71  def get_variable_count(self):
72  """ Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)"""
73  return self._g.get_variable_count()
74 
76  """ Check if this group has a link that is considered to be an end effector """
77  return len(self._g.get_end_effector_link()) > 0
78 
80  """ Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector. """
81  return self._g.get_end_effector_link()
82 
83  def set_end_effector_link(self, link_name):
84  """ Set the name of the link to be considered as an end effector """
85  if not self._g.set_end_effector_link(link_name):
86  raise MoveItCommanderException("Unable to set end effector link")
87 
89  """ Get the description of the planner interface (list of planner ids) """
90  desc = PlannerInterfaceDescription()
91  conversions.msg_from_string(desc, self._g.get_interface_description())
92  return desc
93 
95  """ Get the reference frame assumed for poses of end-effectors """
96  return self._g.get_pose_reference_frame()
97 
98  def set_pose_reference_frame(self, reference_frame):
99  """ Set the reference frame to assume for poses of end-effectors """
100  self._g.set_pose_reference_frame(reference_frame)
101 
103  """ Get the name of the frame where all planning is performed """
104  return self._g.get_planning_frame()
105 
107  """ Get the current configuration of the group as a list (these are values published on /joint_states) """
108  return self._g.get_current_joint_values()
109 
110  def get_current_pose(self, end_effector_link=""):
111  """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """
112  if len(end_effector_link) > 0 or self.has_end_effector_link():
113  return conversions.list_to_pose_stamped(self._g.get_current_pose(end_effector_link), self.get_planning_frame())
114  else:
115  raise MoveItCommanderException("There is no end effector to get the pose of")
116 
117  def get_current_rpy(self, end_effector_link=""):
118  """ Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector. """
119  if len(end_effector_link) > 0 or self.has_end_effector_link():
120  return self._g.get_current_rpy(end_effector_link)
121  else:
122  raise MoveItCommanderException("There is no end effector to get the rpy of")
123 
125  return self._g.get_random_joint_values()
126 
127  def get_random_pose(self, end_effector_link=""):
128  if len(end_effector_link) > 0 or self.has_end_effector_link():
129  return conversions.list_to_pose_stamped(self._g.get_random_pose(end_effector_link), self.get_planning_frame())
130  else:
131  raise MoveItCommanderException("There is no end effector to get the pose of")
132 
135 
136  def set_start_state(self, msg):
137  """
138  Specify a start state for the group.
139 
140  Parameters
141  ----------
142  msg : moveit_msgs/RobotState
143 
144  Examples
145  --------
146  >>> from moveit_msgs.msg import RobotState
147  >>> from sensor_msgs.msg import JointState
148  >>> joint_state = JointState()
149  >>> joint_state.header = Header()
150  >>> joint_state.header.stamp = rospy.Time.now()
151  >>> joint_state.name = ['joint_a', 'joint_b']
152  >>> joint_state.position = [0.17, 0.34]
153  >>> moveit_robot_state = RobotState()
154  >>> moveit_robot_state.joint_state = joint_state
155  >>> group.set_start_state(moveit_robot_state)
156  """
157  self._g.set_start_state(conversions.msg_to_string(msg))
158 
160  """ Get the current state of the robot bounded."""
161  s = RobotState()
162  c_str = self._g.get_current_state_bounded()
163  conversions.msg_from_string(s, c_str)
164  return s
165 
166  def get_current_state(self):
167  """ Get the current state of the robot."""
168  s = RobotState()
169  c_str = self._g.get_current_state()
170  conversions.msg_from_string(s, c_str)
171  return s
172 
174  return self._g.get_joint_value_target()
175 
176  def set_joint_value_target(self, arg1, arg2=None, arg3=None):
177  """
178  Specify a target joint configuration for the group.
179  - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided.
180  The dict should specify pairs of joint variable names and their target values, the list should specify all the variable values
181  for the group. The JointState message specifies the positions of some single-dof joints.
182  - if the type of arg1 is string, then arg2 is expected to be defined and be either a real value or a list of real values. This is
183  interpreted as setting a particular joint to a particular value.
184  - if the type of arg1 is Pose or PoseStamped, both arg2 and arg3 could be defined. If arg2 or arg3 are defined, their types must
185  be either string or bool. The string type argument is interpreted as the end-effector the pose is specified for (default is to use
186  the default end-effector), and the bool is used to decide whether the pose specified is approximate (default is false). This situation
187  allows setting the joint target of the group by calling IK. This does not send a pose to the planner and the planner will do no IK.
188  Instead, one IK solution will be computed first, and that will be sent to the planner.
189  """
190  if isinstance(arg1, RobotState):
191  if not self._g.set_state_value_target(conversions.msg_to_string(arg1)):
192  raise MoveItCommanderException("Error setting state target. Is the target state within bounds?")
193 
194  elif isinstance(arg1, JointState):
195  if (arg2 is not None or arg3 is not None):
196  raise MoveItCommanderException("Too many arguments specified")
197  if not self._g.set_joint_value_target_from_joint_state_message(conversions.msg_to_string(arg1)):
198  raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
199 
200  elif isinstance(arg1, str):
201  if (arg2 is None):
202  raise MoveItCommanderException("Joint value expected when joint name specified")
203  if (arg3 is not None):
204  raise MoveItCommanderException("Too many arguments specified")
205  if not self._g.set_joint_value_target(arg1, arg2):
206  raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
207 
208  elif isinstance(arg1, (Pose, PoseStamped)):
209  approx = False
210  eef = ""
211  if (arg2 is not None):
212  if type(arg2) is str:
213  eef = arg2
214  else:
215  if type(arg2) is bool:
216  approx = arg2
217  else:
218  raise MoveItCommanderException("Unexpected type")
219  if (arg3 is not None):
220  if type(arg3) is str:
221  eef = arg3
222  else:
223  if type(arg3) is bool:
224  approx = arg3
225  else:
226  raise MoveItCommanderException("Unexpected type")
227  r = False
228  if type(arg1) is PoseStamped:
229  r = self._g.set_joint_value_target_from_pose_stamped(conversions.msg_to_string(arg1), eef, approx)
230  else:
231  r = self._g.set_joint_value_target_from_pose(conversions.msg_to_string(arg1), eef, approx)
232  if not r:
233  if approx:
234  raise MoveItCommanderException("Error setting joint target. Does your IK solver support approximate IK?")
235  else:
236  raise MoveItCommanderException("Error setting joint target. Is the IK solver functional?")
237 
238  elif (hasattr(arg1, '__iter__')):
239  if (arg2 is not None or arg3 is not None):
240  raise MoveItCommanderException("Too many arguments specified")
241  if not self._g.set_joint_value_target(arg1):
242  raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
243 
244  else:
245  raise MoveItCommanderException("Unsupported argument of type %s" % type(arg1))
246 
247  def set_rpy_target(self, rpy, end_effector_link=""):
248  """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
249  if len(end_effector_link) > 0 or self.has_end_effector_link():
250  if len(rpy) == 3:
251  if not self._g.set_rpy_target(rpy[0], rpy[1], rpy[2], end_effector_link):
252  raise MoveItCommanderException("Unable to set orientation target")
253  else:
254  raise MoveItCommanderException("Expected [roll, pitch, yaw]")
255  else:
256  raise MoveItCommanderException("There is no end effector to set the pose for")
257 
258  def set_orientation_target(self, q, end_effector_link=""):
259  """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
260  if len(end_effector_link) > 0 or self.has_end_effector_link():
261  if len(q) == 4:
262  if not self._g.set_orientation_target(q[0], q[1], q[2], q[3], end_effector_link):
263  raise MoveItCommanderException("Unable to set orientation target")
264  else:
265  raise MoveItCommanderException("Expected [qx, qy, qz, qw]")
266  else:
267  raise MoveItCommanderException("There is no end effector to set the pose for")
268 
269  def set_position_target(self, xyz, end_effector_link=""):
270  """ Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
271  if len(end_effector_link) > 0 or self.has_end_effector_link():
272  if not self._g.set_position_target(xyz[0], xyz[1], xyz[2], end_effector_link):
273  raise MoveItCommanderException("Unable to set position target")
274  else:
275  raise MoveItCommanderException("There is no end effector to set the pose for")
276 
277  def set_pose_target(self, pose, end_effector_link=""):
278  """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
279  """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
280  if len(end_effector_link) > 0 or self.has_end_effector_link():
281  ok = False
282  if type(pose) is PoseStamped:
283  old = self.get_pose_reference_frame()
284  self.set_pose_reference_frame(pose.header.frame_id)
285  ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
286  self.set_pose_reference_frame(old)
287  elif type(pose) is Pose:
288  ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
289  else:
290  ok = self._g.set_pose_target(pose, end_effector_link)
291  if not ok:
292  raise MoveItCommanderException("Unable to set target pose")
293  else:
294  raise MoveItCommanderException("There is no end effector to set the pose for")
295 
296  def set_pose_targets(self, poses, end_effector_link=""):
297  """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
298  if len(end_effector_link) > 0 or self.has_end_effector_link():
299  if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
300  raise MoveItCommanderException("Unable to set target poses")
301  else:
302  raise MoveItCommanderException("There is no end effector to set poses for")
303 
304  def shift_pose_target(self, axis, value, end_effector_link=""):
305  """ Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target """
306  if len(end_effector_link) > 0 or self.has_end_effector_link():
307  pose = self._g.get_current_pose(end_effector_link)
308  # by default we get orientation as a quaternion list
309  # if we are updating a rotation axis however, we convert the orientation to RPY
310  if axis > 2:
311  (r, p, y) = tf.transformations.euler_from_quaternion(pose[3:])
312  pose = [pose[0], pose[1], pose[2], r, p, y]
313  if axis >= 0 and axis < 6:
314  pose[axis] = pose[axis] + value
315  self.set_pose_target(pose, end_effector_link)
316  else:
317  raise MoveItCommanderException("An axis value between 0 and 5 expected")
318  else:
319  raise MoveItCommanderException("There is no end effector to set poses for")
320 
321  def clear_pose_target(self, end_effector_link):
322  """ Clear the pose target for a particular end-effector """
323  self._g.clear_pose_target(end_effector_link)
324 
326  """ Clear all known pose targets """
327  self._g.clear_pose_targets()
328 
329  def set_random_target(self):
330  """ Set a random joint configuration target """
331  self._g.set_random_target()
332 
333  def get_named_targets(self):
334  """ Get a list of all the names of joint configurations."""
335  return self._g.get_named_targets()
336 
337  def set_named_target(self, name):
338  """ Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF. """
339  if not self._g.set_named_target(name):
340  raise MoveItCommanderException("Unable to set target %s. Is the target within bounds?" % name)
341 
342  def get_named_target_values(self, target):
343  """Get a dictionary of joint values of a named target"""
344  return self._g.get_named_target_values(target)
345 
346  def remember_joint_values(self, name, values=None):
347  """ Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded. """
348  if values is None:
349  values = self.get_current_joint_values()
350  self._g.remember_joint_values(name, values)
351 
353  """ Get a dictionary that maps names to joint configurations for the group """
354  return self._g.get_remembered_joint_values()
355 
356  def forget_joint_values(self, name):
357  """ Forget a stored joint configuration """
358  self._g.forget_joint_values(name)
359 
361  """ Return a tuple of goal tolerances: joint, position and orientation. """
363 
365  """ Get the tolerance for achieving a joint goal (distance for each joint variable) """
366  return self._g.get_goal_joint_tolerance()
367 
369  """ When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector """
370  return self._g.get_goal_position_tolerance()
371 
373  """ When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector """
374  return self._g.get_goal_orientation_tolerance()
375 
376  def set_goal_tolerance(self, value):
377  """ Set the joint, position and orientation goal tolerances simultaneously """
378  self._g.set_goal_tolerance(value)
379 
380  def set_goal_joint_tolerance(self, value):
381  """ Set the tolerance for a target joint configuration """
382  self._g.set_goal_joint_tolerance(value)
383 
384  def set_goal_position_tolerance(self, value):
385  """ Set the tolerance for a target end-effector position """
386  self._g.set_goal_position_tolerance(value)
387 
389  """ Set the tolerance for a target end-effector orientation """
391 
392  def allow_looking(self, value):
393  """ Enable/disable looking around for motion planning """
394  self._g.allow_looking(value)
395 
396  def allow_replanning(self, value):
397  """ Enable/disable replanning """
398  self._g.allow_replanning(value)
399 
401  """ Get a list of names for the constraints specific for this group, as read from the warehouse """
402  return self._g.get_known_constraints()
403 
405  """ Get the acutal path constraints in form of a moveit_msgs.msgs.Constraints """
406  c = Constraints()
407  c_str = self._g.get_path_constraints()
408  conversions.msg_from_string(c, c_str)
409  return c
410 
411  def set_path_constraints(self, value):
412  """ Specify the path constraints to be used (as read from the database) """
413  if value is None:
415  else:
416  if type(value) is Constraints:
417  self._g.set_path_constraints_from_msg(conversions.msg_to_string(value))
418  elif not self._g.set_path_constraints(value):
419  raise MoveItCommanderException("Unable to set path constraints " + value)
420 
422  """ Specify that no path constraints are to be used during motion planning """
424 
426  """ Get the actual trajectory constraints in form of a moveit_msgs.msgs.Constraints """
427  c = Constraints()
428  c_str = self._g.get_trajectory_constraints()
429  conversions.msg_from_string(c, c_str)
430  return c
431 
432  def set_trajectory_constraints(self, value):
433  """ Specify the trajectory constraints to be used """
434  if value is None:
436  else:
437  if type(value) is TrajectoryConstraints:
438  self._g.set_trajectory_constraints_from_msg(conversions.msg_to_string(value))
439  elif not self._g.set_trajectory_constraints(value):
440  raise MoveItCommanderException("Unable to set trajectory constraints " + value)
441 
443  """ Specify that no trajectory constraints are to be used during motion planning """
445 
446  def set_constraints_database(self, host, port):
447  """ Specify which database to connect to for loading possible path constraints """
448  self._g.set_constraints_database(host, port)
449 
450  def set_planning_time(self, seconds):
451  """ Specify the amount of time to be used for motion planning. """
452  self._g.set_planning_time(seconds)
453 
454  def get_planning_time(self):
455  """ Specify the amount of time to be used for motion planning. """
456  return self._g.get_planning_time()
457 
458  def set_planner_id(self, planner_id):
459  """ Specify which planner to use when motion planning """
460  self._g.set_planner_id(planner_id)
461 
462  def get_planner_id(self):
463  """ Get the current planner_id """
464  return self._g.get_planner_id()
465 
466  def set_num_planning_attempts(self, num_planning_attempts):
467  """ Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1. """
468  self._g.set_num_planning_attempts(num_planning_attempts)
469 
470  def set_workspace(self, ws):
471  """ Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ] """
472  if len(ws) == 0:
473  self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
474  else:
475  if len(ws) == 4:
476  self._g.set_workspace(ws[0], ws[1], 0.0, ws[2], ws[3], 0.0)
477  else:
478  if len(ws) == 6:
479  self._g.set_workspace(ws[0], ws[1], ws[2], ws[3], ws[4], ws[5])
480  else:
481  raise MoveItCommanderException("Expected 0, 4 or 6 values in list specifying workspace")
482 
484  """ Set a scaling factor to reduce the maximum joint velocities. Allowed values are in (0,1].
485  The default value is set in the joint_limits.yaml of the moveit_config package. """
486  if value > 0 and value <= 1:
488  else:
489  raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")
490 
492  """ Set a scaling factor to reduce the maximum joint accelerations. Allowed values are in (0,1].
493  The default value is set in the joint_limits.yaml of the moveit_config package. """
494  if value > 0 and value <= 1:
496  else:
497  raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")
498 
499  def go(self, joints=None, wait=True):
500  """ Set the target of the group and then move the group to the specified target """
501  if type(joints) is bool:
502  wait = joints
503  joints = None
504 
505  elif type(joints) is JointState:
506  self.set_joint_value_target(joints)
507 
508  elif type(joints) is Pose:
509  self.set_pose_target(joints)
510 
511  elif joints is not None:
512  try:
514  except TypeError:
515  self.set_joint_value_target(joints)
516  if wait:
517  return self._g.move()
518  else:
519  return self._g.async_move()
520 
521  def plan(self, joints=None):
522  """ Return a tuple of the motion planning results such as
523  (success flag : boolean, trajectory message : RobotTrajectory,
524  planning time : float, error code : MoveitErrorCodes) """
525  if type(joints) is JointState:
526  self.set_joint_value_target(joints)
527 
528  elif type(joints) is Pose:
529  self.set_pose_target(joints)
530 
531  elif joints is not None:
532  try:
534  except MoveItCommanderException:
535  self.set_joint_value_target(joints)
536 
537  (error_code_msg, trajectory_msg, planning_time) = self._g.plan()
538 
539  error_code = MoveItErrorCodes()
540  error_code.deserialize(error_code_msg)
541  plan = RobotTrajectory()
542  return (error_code.val == MoveItErrorCodes.SUCCESS,
543  plan.deserialize(trajectory_msg),
544  planning_time,
545  error_code)
546 
547  def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None):
548  """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
549  if path_constraints:
550  if type(path_constraints) is Constraints:
551  constraints_str = conversions.msg_to_string(path_constraints)
552  else:
553  raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
554  (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str)
555  else:
556  (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
557 
558  path = RobotTrajectory()
559  path.deserialize(ser_path)
560  return (path, fraction)
561 
562  def execute(self, plan_msg, wait=True):
563  """Execute a previously planned path"""
564  if wait:
565  return self._g.execute(conversions.msg_to_string(plan_msg))
566  else:
567  return self._g.async_execute(conversions.msg_to_string(plan_msg))
568 
569  def attach_object(self, object_name, link_name="", touch_links=[]):
570  """ Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was succesfully sent to the move_group node. This does not verify that the attach request also was successfuly applied by move_group."""
571  return self._g.attach_object(object_name, link_name, touch_links)
572 
573  def detach_object(self, name=""):
574  """ Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group."""
575  return self._g.detach_object(name)
576 
577  def pick(self, object_name, grasp=[], plan_only=False):
578  """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument."""
579  if type(grasp) is Grasp:
580  return self._g.pick(object_name, conversions.msg_to_string(grasp), plan_only)
581  else:
582  return self._g.pick(object_name, [conversions.msg_to_string(x) for x in grasp], plan_only)
583 
584  def place(self, object_name, location=None, plan_only=False):
585  """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
586  result = False
587  if not location:
588  result = self._g.place(object_name, plan_only)
589  elif type(location) is PoseStamped:
590  old = self.get_pose_reference_frame()
591  self.set_pose_reference_frame(location.header.frame_id)
592  result = self._g.place(object_name, conversions.pose_to_list(location.pose), plan_only)
593  self.set_pose_reference_frame(old)
594  elif type(location) is Pose:
595  result = self._g.place(object_name, conversions.pose_to_list(location), plan_only)
596  elif type(location) is PlaceLocation:
597  result = self._g.place(object_name, conversions.msg_to_string(location), plan_only)
598  elif type(location) is list:
599  if location:
600  if type(location[0]) is PlaceLocation:
601  result = self._g.place_locations_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only)
602  elif type(location[0]) is PoseStamped:
603  result = self._g.place_poses_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only)
604  else:
605  raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object")
606  else:
607  raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object")
608  return result
609 
610  def set_support_surface_name(self, value):
611  """ Set the support surface name for a place operation """
612  self._g.set_support_surface_name(value)
613 
614  def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization"):
615  ser_ref_state_in = conversions.msg_to_string(ref_state_in)
616  ser_traj_in = conversions.msg_to_string(traj_in)
617  ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor, algorithm)
618  traj_out = RobotTrajectory()
619  traj_out.deserialize(ser_traj_out)
620  return traj_out
621 
622  def get_jacobian_matrix(self, joint_values, reference_point=None):
623  """ Get the jacobian matrix of the group as a list"""
624  return self._g.get_jacobian_matrix(joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point)
625 
626  def enforce_bounds(self, robot_state_msg):
627  """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """
628  s = RobotState()
629  c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg))
630  conversions.msg_from_string(s, c_str)
631  return s
moveit_commander.move_group.MoveGroupCommander.set_constraints_database
def set_constraints_database(self, host, port)
Definition: move_group.py:446
moveit_commander.move_group.MoveGroupCommander.plan
def plan(self, joints=None)
Definition: move_group.py:521
moveit_commander.move_group.MoveGroupCommander.set_num_planning_attempts
def set_num_planning_attempts(self, num_planning_attempts)
Definition: move_group.py:466
moveit_commander.move_group.MoveGroupCommander.get_trajectory_constraints
def get_trajectory_constraints(self)
Definition: move_group.py:425
moveit_commander.move_group.MoveGroupCommander.pick
def pick(self, object_name, grasp=[], plan_only=False)
Definition: move_group.py:577
moveit_commander.move_group.MoveGroupCommander.get_joints
def get_joints(self)
Definition: move_group.py:67
moveit_commander.move_group.MoveGroupCommander.get_named_targets
def get_named_targets(self)
Definition: move_group.py:333
moveit_commander.move_group.MoveGroupCommander.clear_trajectory_constraints
def clear_trajectory_constraints(self)
Definition: move_group.py:442
moveit_commander.move_group.MoveGroupCommander.get_name
def get_name(self)
Definition: move_group.py:55
moveit_commander.move_group.MoveGroupCommander
Definition: move_group.py:46
moveit_commander.move_group.MoveGroupCommander.set_goal_orientation_tolerance
def set_goal_orientation_tolerance(self, value)
Definition: move_group.py:388
moveit_commander.move_group.MoveGroupCommander.set_pose_reference_frame
def set_pose_reference_frame(self, reference_frame)
Definition: move_group.py:98
moveit_commander.move_group.MoveGroupCommander.stop
def stop(self)
Definition: move_group.py:59
moveit_commander.move_group.MoveGroupCommander.place
def place(self, object_name, location=None, plan_only=False)
Definition: move_group.py:584
moveit_commander.move_group.MoveGroupCommander.set_planner_id
def set_planner_id(self, planner_id)
Definition: move_group.py:458
moveit_commander.move_group.MoveGroupCommander.get_goal_orientation_tolerance
def get_goal_orientation_tolerance(self)
Definition: move_group.py:372
moveit_commander.move_group.MoveGroupCommander.clear_path_constraints
def clear_path_constraints(self)
Definition: move_group.py:421
moveit_commander.move_group.MoveGroupCommander.set_pose_targets
def set_pose_targets(self, poses, end_effector_link="")
Definition: move_group.py:296
moveit_commander.move_group.MoveGroupCommander.set_start_state_to_current_state
def set_start_state_to_current_state(self)
Definition: move_group.py:133
moveit_commander.move_group.MoveGroupCommander.set_joint_value_target
def set_joint_value_target(self, arg1, arg2=None, arg3=None)
Definition: move_group.py:176
moveit_commander.move_group.MoveGroupCommander.get_goal_position_tolerance
def get_goal_position_tolerance(self)
Definition: move_group.py:368
moveit_commander.move_group.MoveGroupCommander.detach_object
def detach_object(self, name="")
Definition: move_group.py:573
moveit_commander.move_group.MoveGroupCommander.go
def go(self, joints=None, wait=True)
Definition: move_group.py:499
moveit_commander.move_group.MoveGroupCommander.get_remembered_joint_values
def get_remembered_joint_values(self)
Definition: move_group.py:352
moveit_commander.move_group.MoveGroupCommander.set_max_acceleration_scaling_factor
def set_max_acceleration_scaling_factor(self, value)
Definition: move_group.py:491
moveit_commander.move_group.MoveGroupCommander.get_path_constraints
def get_path_constraints(self)
Definition: move_group.py:404
moveit_commander.move_group.MoveGroupCommander.clear_pose_target
def clear_pose_target(self, end_effector_link)
Definition: move_group.py:321
moveit_commander.move_group.MoveGroupCommander.set_end_effector_link
def set_end_effector_link(self, link_name)
Definition: move_group.py:83
moveit_commander.move_group.MoveGroupCommander.get_goal_joint_tolerance
def get_goal_joint_tolerance(self)
Definition: move_group.py:364
moveit_commander.move_group.MoveGroupCommander.allow_looking
def allow_looking(self, value)
Definition: move_group.py:392
moveit_commander.move_group.MoveGroupCommander.get_planner_id
def get_planner_id(self)
Definition: move_group.py:462
moveit_commander.move_group.MoveGroupCommander.get_jacobian_matrix
def get_jacobian_matrix(self, joint_values, reference_point=None)
Definition: move_group.py:622
moveit_commander.exception.MoveItCommanderException
Definition: exception.py:35
moveit_commander.move_group.MoveGroupCommander.set_max_velocity_scaling_factor
def set_max_velocity_scaling_factor(self, value)
Definition: move_group.py:483
moveit_commander.move_group.MoveGroupCommander.get_current_pose
def get_current_pose(self, end_effector_link="")
Definition: move_group.py:110
moveit_commander.move_group.MoveGroupCommander.set_goal_joint_tolerance
def set_goal_joint_tolerance(self, value)
Definition: move_group.py:380
moveit_commander_cmdline.type
type
Definition: moveit_commander_cmdline.py:162
moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None)
Definition: move_group.py:547
moveit_commander.move_group.MoveGroupCommander.get_current_state
def get_current_state(self)
Definition: move_group.py:166
moveit_commander.move_group.MoveGroupCommander.get_random_pose
def get_random_pose(self, end_effector_link="")
Definition: move_group.py:127
moveit_commander.move_group.MoveGroupCommander.set_rpy_target
def set_rpy_target(self, rpy, end_effector_link="")
Definition: move_group.py:247
moveit_commander.move_group.MoveGroupCommander.set_trajectory_constraints
def set_trajectory_constraints(self, value)
Definition: move_group.py:432
moveit_commander.move_group.MoveGroupCommander.set_workspace
def set_workspace(self, ws)
Definition: move_group.py:470
plan
Definition: plan.py:1
moveit_commander.move_group.MoveGroupCommander.get_active_joints
def get_active_joints(self)
Definition: move_group.py:63
moveit_commander.move_group.MoveGroupCommander.get_planning_frame
def get_planning_frame(self)
Definition: move_group.py:102
moveit_commander.move_group.MoveGroupCommander.set_random_target
def set_random_target(self)
Definition: move_group.py:329
moveit_commander.move_group.MoveGroupCommander.set_start_state
def set_start_state(self, msg)
Definition: move_group.py:136
moveit_commander.move_group.MoveGroupCommander.enforce_bounds
def enforce_bounds(self, robot_state_msg)
Definition: move_group.py:626
moveit_commander.move_group.MoveGroupCommander.set_pose_target
def set_pose_target(self, pose, end_effector_link="")
Definition: move_group.py:277
moveit_commander.move_group.MoveGroupCommander.execute
def execute(self, plan_msg, wait=True)
Definition: move_group.py:562
moveit_commander.move_group.MoveGroupCommander._g
_g
Definition: move_group.py:53
moveit_commander.move_group.MoveGroupCommander.get_named_target_values
def get_named_target_values(self, target)
Definition: move_group.py:342
moveit_commander.move_group.MoveGroupCommander.set_planning_time
def set_planning_time(self, seconds)
Definition: move_group.py:450
moveit_commander.move_group.MoveGroupCommander.set_goal_tolerance
def set_goal_tolerance(self, value)
Definition: move_group.py:376
moveit_commander.move_group.MoveGroupCommander.set_support_surface_name
def set_support_surface_name(self, value)
Definition: move_group.py:610
moveit_commander.conversions
Definition: conversions.py:1
moveit_commander.move_group.MoveGroupCommander.set_goal_position_tolerance
def set_goal_position_tolerance(self, value)
Definition: move_group.py:384
moveit_commander.move_group.MoveGroupCommander.set_path_constraints
def set_path_constraints(self, value)
Definition: move_group.py:411
moveit_commander.move_group.MoveGroupCommander.get_known_constraints
def get_known_constraints(self)
Definition: move_group.py:400
moveit_commander.move_group.MoveGroupCommander.attach_object
def attach_object(self, object_name, link_name="", touch_links=[])
Definition: move_group.py:569
moveit_commander.move_group.MoveGroupCommander.get_planning_time
def get_planning_time(self)
Definition: move_group.py:454
moveit_commander.move_group.MoveGroupCommander.get_current_joint_values
def get_current_joint_values(self)
Definition: move_group.py:106
moveit_commander.move_group.MoveGroupCommander.get_interface_description
def get_interface_description(self)
Definition: move_group.py:88
moveit_commander.move_group.MoveGroupCommander.retime_trajectory
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization")
Definition: move_group.py:614
moveit_commander.move_group.MoveGroupCommander.get_joint_value_target
def get_joint_value_target(self)
Definition: move_group.py:173
moveit_commander.move_group.MoveGroupCommander.set_position_target
def set_position_target(self, xyz, end_effector_link="")
Definition: move_group.py:269
moveit_commander.move_group.MoveGroupCommander.remember_joint_values
def remember_joint_values(self, name, values=None)
Definition: move_group.py:346
pick
Definition: pick.py:1
moveit_commander.move_group.MoveGroupCommander.allow_replanning
def allow_replanning(self, value)
Definition: move_group.py:396
moveit_commander.move_group.MoveGroupCommander.get_current_state_bounded
def get_current_state_bounded(self)
Definition: move_group.py:159
moveit_commander.move_group.MoveGroupCommander.has_end_effector_link
def has_end_effector_link(self)
Definition: move_group.py:75
moveit_commander.move_group.MoveGroupCommander.set_named_target
def set_named_target(self, name)
Definition: move_group.py:337
moveit_commander.move_group.MoveGroupCommander.get_current_rpy
def get_current_rpy(self, end_effector_link="")
Definition: move_group.py:117
moveit_commander.move_group.MoveGroupCommander.forget_joint_values
def forget_joint_values(self, name)
Definition: move_group.py:356
moveit_commander.move_group.MoveGroupCommander.get_end_effector_link
def get_end_effector_link(self)
Definition: move_group.py:79
moveit_commander.move_group.MoveGroupCommander.shift_pose_target
def shift_pose_target(self, axis, value, end_effector_link="")
Definition: move_group.py:304
moveit_commander.move_group.MoveGroupCommander.clear_pose_targets
def clear_pose_targets(self)
Definition: move_group.py:325
moveit_commander.move_group.MoveGroupCommander.set_orientation_target
def set_orientation_target(self, q, end_effector_link="")
Definition: move_group.py:258
moveit_commander.move_group.MoveGroupCommander.__init__
def __init__(self, name, robot_description="robot_description", ns="", wait_for_servers=5.0)
Definition: move_group.py:51
moveit_commander.move_group.MoveGroupCommander.get_goal_tolerance
def get_goal_tolerance(self)
Definition: move_group.py:360
moveit_commander.move_group.MoveGroupCommander.get_variable_count
def get_variable_count(self)
Definition: move_group.py:71
moveit_commander.move_group.MoveGroupCommander.get_random_joint_values
def get_random_joint_values(self)
Definition: move_group.py:124
moveit_commander.move_group.MoveGroupCommander.get_pose_reference_frame
def get_pose_reference_frame(self)
Definition: move_group.py:94


moveit_commander
Author(s): Ioan Sucan
autogenerated on Sat Dec 12 2020 03:27:14