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 (
37  RobotTrajectory,
38  Grasp,
39  PlaceLocation,
40  Constraints,
41  RobotState,
42 )
43 from moveit_msgs.msg import (
44  MoveItErrorCodes,
45  TrajectoryConstraints,
46  PlannerInterfaceDescription,
47  MotionPlanRequest,
48 )
49 from sensor_msgs.msg import JointState
50 import rospy
51 import tf
52 from moveit_ros_planning_interface import _moveit_move_group_interface
53 from .exception import MoveItCommanderException
54 import moveit_commander.conversions as conversions
55 
56 
57 class MoveGroupCommander(object):
58  """
59  Execution of simple commands for a particular group
60  """
61 
62  def __init__(
63  self, name, robot_description="robot_description", ns="", wait_for_servers=5.0
64  ):
65  """Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error."""
66  self._g = _moveit_move_group_interface.MoveGroupInterface(
67  name, robot_description, ns, wait_for_servers
68  )
69 
70  def get_name(self):
71  """Get the name of the group this instance was initialized for"""
72  return self._g.get_name()
73 
74  def stop(self):
75  """Stop the current execution, if any"""
76  self._g.stop()
77 
78  def get_active_joints(self):
79  """Get the active joints of this group"""
80  return self._g.get_active_joints()
81 
82  def get_joints(self):
83  """Get the joints of this group"""
84  return self._g.get_joints()
85 
86  def get_variable_count(self):
87  """Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)"""
88  return self._g.get_variable_count()
89 
91  """Check if this group has a link that is considered to be an end effector"""
92  return len(self._g.get_end_effector_link()) > 0
93 
95  """Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector."""
96  return self._g.get_end_effector_link()
97 
98  def set_end_effector_link(self, link_name):
99  """Set the name of the link to be considered as an end effector"""
100  if not self._g.set_end_effector_link(link_name):
101  raise MoveItCommanderException("Unable to set end effector link")
102 
104  """Get the description of the planner interface (list of planner ids)"""
105  desc = PlannerInterfaceDescription()
106  conversions.msg_from_string(desc, self._g.get_interface_description())
107  return desc
108 
110  """Get the reference frame assumed for poses of end-effectors"""
111  return self._g.get_pose_reference_frame()
112 
113  def set_pose_reference_frame(self, reference_frame):
114  """Set the reference frame to assume for poses of end-effectors"""
115  self._g.set_pose_reference_frame(reference_frame)
116 
118  """Get the name of the frame where all planning is performed"""
119  return self._g.get_planning_frame()
120 
122  """Get the current configuration of the group as a list (these are values published on /joint_states)"""
123  return self._g.get_current_joint_values()
124 
125  def get_current_pose(self, end_effector_link=""):
126  """Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector."""
127  if len(end_effector_link) > 0 or self.has_end_effector_link():
128  return conversions.list_to_pose_stamped(
129  self._g.get_current_pose(end_effector_link), self.get_planning_frame()
130  )
131  else:
133  "There is no end effector to get the pose of"
134  )
135 
136  def get_current_rpy(self, end_effector_link=""):
137  """Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector."""
138  if len(end_effector_link) > 0 or self.has_end_effector_link():
139  return self._g.get_current_rpy(end_effector_link)
140  else:
141  raise MoveItCommanderException("There is no end effector to get the rpy of")
142 
144  return self._g.get_random_joint_values()
145 
146  def get_random_pose(self, end_effector_link=""):
147  if len(end_effector_link) > 0 or self.has_end_effector_link():
148  return conversions.list_to_pose_stamped(
149  self._g.get_random_pose(end_effector_link), self.get_planning_frame()
150  )
151  else:
153  "There is no end effector to get the pose of"
154  )
155 
158 
159  def set_start_state(self, msg):
160  """
161  Specify a start state for the group.
162 
163  Parameters
164  ----------
165  msg : moveit_msgs/RobotState
166 
167  Examples
168  --------
169  >>> from moveit_msgs.msg import RobotState
170  >>> from sensor_msgs.msg import JointState
171  >>> joint_state = JointState()
172  >>> joint_state.header = Header()
173  >>> joint_state.header.stamp = rospy.Time.now()
174  >>> joint_state.name = ['joint_a', 'joint_b']
175  >>> joint_state.position = [0.17, 0.34]
176  >>> moveit_robot_state = RobotState()
177  >>> moveit_robot_state.joint_state = joint_state
178  >>> group.set_start_state(moveit_robot_state)
179  """
180  self._g.set_start_state(conversions.msg_to_string(msg))
181 
183  """Get the current state of the robot bounded."""
184  s = RobotState()
185  c_str = self._g.get_current_state_bounded()
186  conversions.msg_from_string(s, c_str)
187  return s
188 
189  def get_current_state(self):
190  """Get the current state of the robot."""
191  s = RobotState()
192  c_str = self._g.get_current_state()
193  conversions.msg_from_string(s, c_str)
194  return s
195 
197  return self._g.get_joint_value_target()
198 
199  def set_joint_value_target(self, arg1, arg2=None, arg3=None):
200  """
201  Specify a target joint configuration for the group.
202  - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided.
203  The dict should specify pairs of joint variable names and their target values, the list should specify all the variable values
204  for the group. The JointState message specifies the positions of some single-dof joints.
205  - 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
206  interpreted as setting a particular joint to a particular value.
207  - 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
208  be either string or bool. The string type argument is interpreted as the end-effector the pose is specified for (default is to use
209  the default end-effector), and the bool is used to decide whether the pose specified is approximate (default is false). This situation
210  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.
211  Instead, one IK solution will be computed first, and that will be sent to the planner.
212  """
213  if isinstance(arg1, JointState):
214  if arg2 is not None or arg3 is not None:
215  raise MoveItCommanderException("Too many arguments specified")
216  if not self._g.set_joint_value_target_from_joint_state_message(
217  conversions.msg_to_string(arg1)
218  ):
220  "Error setting joint target. Is the target within bounds?"
221  )
222 
223  elif isinstance(arg1, str):
224  if arg2 is None:
226  "Joint value expected when joint name specified"
227  )
228  if arg3 is not None:
229  raise MoveItCommanderException("Too many arguments specified")
230  if not self._g.set_joint_value_target(arg1, arg2):
232  "Error setting joint target. Is the target within bounds?"
233  )
234 
235  elif isinstance(arg1, (Pose, PoseStamped)):
236  approx = False
237  eef = ""
238  if arg2 is not None:
239  if type(arg2) is str:
240  eef = arg2
241  else:
242  if type(arg2) is bool:
243  approx = arg2
244  else:
245  raise MoveItCommanderException("Unexpected type")
246  if arg3 is not None:
247  if type(arg3) is str:
248  eef = arg3
249  else:
250  if type(arg3) is bool:
251  approx = arg3
252  else:
253  raise MoveItCommanderException("Unexpected type")
254  r = False
255  if type(arg1) is PoseStamped:
256  r = self._g.set_joint_value_target_from_pose_stamped(
257  conversions.msg_to_string(arg1), eef, approx
258  )
259  else:
260  r = self._g.set_joint_value_target_from_pose(
261  conversions.msg_to_string(arg1), eef, approx
262  )
263  if not r:
264  if approx:
266  "Error setting joint target. Does your IK solver support approximate IK?"
267  )
268  else:
270  "Error setting joint target. Is the IK solver functional?"
271  )
272 
273  elif hasattr(arg1, "__iter__"):
274  if arg2 is not None or arg3 is not None:
275  raise MoveItCommanderException("Too many arguments specified")
276  if not self._g.set_joint_value_target(arg1):
278  "Error setting joint target. Is the target within bounds?"
279  )
280 
281  else:
283  "Unsupported argument of type %s" % type(arg1)
284  )
285 
286  def set_rpy_target(self, rpy, end_effector_link=""):
287  """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
288  if len(end_effector_link) > 0 or self.has_end_effector_link():
289  if len(rpy) == 3:
290  if not self._g.set_rpy_target(
291  rpy[0], rpy[1], rpy[2], end_effector_link
292  ):
293  raise MoveItCommanderException("Unable to set orientation target")
294  else:
295  raise MoveItCommanderException("Expected [roll, pitch, yaw]")
296  else:
298  "There is no end effector to set the pose for"
299  )
300 
301  def set_orientation_target(self, q, end_effector_link=""):
302  """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
303  if len(end_effector_link) > 0 or self.has_end_effector_link():
304  if len(q) == 4:
305  if not self._g.set_orientation_target(
306  q[0], q[1], q[2], q[3], end_effector_link
307  ):
308  raise MoveItCommanderException("Unable to set orientation target")
309  else:
310  raise MoveItCommanderException("Expected [qx, qy, qz, qw]")
311  else:
313  "There is no end effector to set the pose for"
314  )
315 
316  def set_position_target(self, xyz, end_effector_link=""):
317  """Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
318  if len(end_effector_link) > 0 or self.has_end_effector_link():
319  if not self._g.set_position_target(
320  xyz[0], xyz[1], xyz[2], end_effector_link
321  ):
322  raise MoveItCommanderException("Unable to set position target")
323  else:
325  "There is no end effector to set the pose for"
326  )
327 
328  def set_pose_target(self, pose, end_effector_link=""):
329  """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:"""
330  """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
331  if len(end_effector_link) > 0 or self.has_end_effector_link():
332  ok = False
333  if type(pose) is PoseStamped:
334  old = self.get_pose_reference_frame()
335  self.set_pose_reference_frame(pose.header.frame_id)
336  ok = self._g.set_pose_target(
337  conversions.pose_to_list(pose.pose), end_effector_link
338  )
339  self.set_pose_reference_frame(old)
340  elif type(pose) is Pose:
341  ok = self._g.set_pose_target(
342  conversions.pose_to_list(pose), end_effector_link
343  )
344  else:
345  ok = self._g.set_pose_target(pose, end_effector_link)
346  if not ok:
347  raise MoveItCommanderException("Unable to set target pose")
348  else:
350  "There is no end effector to set the pose for"
351  )
352 
353  def set_pose_targets(self, poses, end_effector_link=""):
354  """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]"""
355  if len(end_effector_link) > 0 or self.has_end_effector_link():
356  if not self._g.set_pose_targets(
357  [conversions.pose_to_list(p) if type(p) is Pose else p for p in poses],
358  end_effector_link,
359  ):
360  raise MoveItCommanderException("Unable to set target poses")
361  else:
362  raise MoveItCommanderException("There is no end effector to set poses for")
363 
364  def shift_pose_target(self, axis, value, end_effector_link=""):
365  """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"""
366  if len(end_effector_link) > 0 or self.has_end_effector_link():
367  pose = self._g.get_current_pose(end_effector_link)
368  # by default we get orientation as a quaternion list
369  # if we are updating a rotation axis however, we convert the orientation to RPY
370  if axis > 2:
371  (r, p, y) = tf.transformations.euler_from_quaternion(pose[3:])
372  pose = [pose[0], pose[1], pose[2], r, p, y]
373  if axis >= 0 and axis < 6:
374  pose[axis] = pose[axis] + value
375  self.set_pose_target(pose, end_effector_link)
376  else:
377  raise MoveItCommanderException("An axis value between 0 and 5 expected")
378  else:
379  raise MoveItCommanderException("There is no end effector to set poses for")
380 
381  def clear_pose_target(self, end_effector_link):
382  """Clear the pose target for a particular end-effector"""
383  self._g.clear_pose_target(end_effector_link)
384 
386  """Clear all known pose targets"""
387  self._g.clear_pose_targets()
388 
389  def set_random_target(self):
390  """Set a random joint configuration target"""
391  self._g.set_random_target()
392 
393  def get_named_targets(self):
394  """Get a list of all the names of joint configurations."""
395  return self._g.get_named_targets()
396 
397  def set_named_target(self, name):
398  """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."""
399  if not self._g.set_named_target(name):
401  "Unable to set target %s. Is the target within bounds?" % name
402  )
403 
404  def get_named_target_values(self, target):
405  """Get a dictionary of joint values of a named target"""
406  return self._g.get_named_target_values(target)
407 
408  def remember_joint_values(self, name, values=None):
409  """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."""
410  if values is None:
411  values = self.get_current_joint_values()
412  self._g.remember_joint_values(name, values)
413 
415  """Get a dictionary that maps names to joint configurations for the group"""
416  return self._g.get_remembered_joint_values()
417 
418  def forget_joint_values(self, name):
419  """Forget a stored joint configuration"""
420  self._g.forget_joint_values(name)
421 
423  """Return a tuple of goal tolerances: joint, position and orientation."""
424  return (
428  )
429 
431  """Get the tolerance for achieving a joint goal (distance for each joint variable)"""
432  return self._g.get_goal_joint_tolerance()
433 
435  """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"""
436  return self._g.get_goal_position_tolerance()
437 
439  """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"""
440  return self._g.get_goal_orientation_tolerance()
441 
442  def set_goal_tolerance(self, value):
443  """Set the joint, position and orientation goal tolerances simultaneously"""
444  self._g.set_goal_tolerance(value)
445 
446  def set_goal_joint_tolerance(self, value):
447  """Set the tolerance for a target joint configuration"""
448  self._g.set_goal_joint_tolerance(value)
449 
450  def set_goal_position_tolerance(self, value):
451  """Set the tolerance for a target end-effector position"""
452  self._g.set_goal_position_tolerance(value)
453 
455  """Set the tolerance for a target end-effector orientation"""
457 
458  def allow_looking(self, value):
459  """Enable/disable looking around for motion planning"""
460  self._g.allow_looking(value)
461 
462  def allow_replanning(self, value):
463  """Enable/disable replanning"""
464  self._g.allow_replanning(value)
465 
467  """Get a list of names for the constraints specific for this group, as read from the warehouse"""
468  return self._g.get_known_constraints()
469 
471  """Get the acutal path constraints in form of a moveit_msgs.msgs.Constraints"""
472  c = Constraints()
473  c_str = self._g.get_path_constraints()
474  conversions.msg_from_string(c, c_str)
475  return c
476 
477  def set_path_constraints(self, value):
478  """Specify the path constraints to be used (as read from the database)"""
479  if value is None:
481  else:
482  if type(value) is Constraints:
483  self._g.set_path_constraints_from_msg(conversions.msg_to_string(value))
484  elif not self._g.set_path_constraints(value):
486  "Unable to set path constraints " + value
487  )
488 
490  """Specify that no path constraints are to be used during motion planning"""
492 
494  """Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints"""
495  c = TrajectoryConstraints()
496  c_str = self._g.get_trajectory_constraints()
497  conversions.msg_from_string(c, c_str)
498  return c
499 
500  def set_trajectory_constraints(self, value):
501  """Specify the trajectory constraints to be used (setting from database is not implemented yet)"""
502  if value is None:
504  else:
505  if type(value) is TrajectoryConstraints:
506  self._g.set_trajectory_constraints_from_msg(
507  conversions.msg_to_string(value)
508  )
509  else:
511  "Unable to set trajectory constraints " + value
512  )
513 
515  """Specify that no trajectory constraints are to be used during motion planning"""
517 
518  def set_constraints_database(self, host, port):
519  """Specify which database to connect to for loading possible path constraints"""
520  self._g.set_constraints_database(host, port)
521 
522  def set_planning_time(self, seconds):
523  """Specify the amount of time to be used for motion planning."""
524  self._g.set_planning_time(seconds)
525 
526  def get_planning_time(self):
527  """Specify the amount of time to be used for motion planning."""
528  return self._g.get_planning_time()
529 
530  def set_planning_pipeline_id(self, planning_pipeline):
531  """Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner)"""
532  self._g.set_planning_pipeline_id(planning_pipeline)
533 
535  """Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner)"""
536  return self._g.get_planning_pipeline_id()
537 
538  def set_planner_id(self, planner_id):
539  """Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN)"""
540  self._g.set_planner_id(planner_id)
541 
542  def get_planner_id(self):
543  """Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline"""
544  return self._g.get_planner_id()
545 
546  def set_num_planning_attempts(self, num_planning_attempts):
547  """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."""
548  self._g.set_num_planning_attempts(num_planning_attempts)
549 
550  def set_workspace(self, ws):
551  """Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ]"""
552  if len(ws) == 0:
553  self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
554  else:
555  if len(ws) == 4:
556  self._g.set_workspace(ws[0], ws[1], 0.0, ws[2], ws[3], 0.0)
557  else:
558  if len(ws) == 6:
559  self._g.set_workspace(ws[0], ws[1], ws[2], ws[3], ws[4], ws[5])
560  else:
562  "Expected 0, 4 or 6 values in list specifying workspace"
563  )
564 
566  """Set a scaling factor to reduce the maximum joint velocities. Allowed values are in (0,1].
567  The default value is set in the joint_limits.yaml of the moveit_config package."""
568  if value > 0 and value <= 1:
570  else:
572  "Expected value in the range from 0 to 1 for scaling factor"
573  )
574 
576  """Set a scaling factor to reduce the maximum joint accelerations. Allowed values are in (0,1].
577  The default value is set in the joint_limits.yaml of the moveit_config package."""
578  if value > 0 and value <= 1:
580  else:
582  "Expected value in the range from 0 to 1 for scaling factor"
583  )
584 
585  def limit_max_cartesian_link_speed(self, speed, link_name=""):
586  """Set the maximum Cartesian link speed. Only positive real values are allowed.
587  The unit is meter per second."""
588  if speed > 0.0:
589  self._g.limit_max_cartesian_link_speed(speed, link_name)
590  else:
591  raise MoveItCommanderException("Expected speed value to be greater than 0")
592 
594  """Clear the maximum cartesian link speed."""
596 
597  def go(self, joints=None, wait=True):
598  """Set the target of the group and then move the group to the specified target"""
599  if type(joints) is bool:
600  wait = joints
601  joints = None
602 
603  elif type(joints) is JointState:
604  self.set_joint_value_target(joints)
605 
606  elif type(joints) is Pose:
607  self.set_pose_target(joints)
608 
609  elif joints is not None:
610  try:
612  except (KeyError, TypeError):
613  self.set_joint_value_target(joints)
614  if wait:
615  return self._g.move()
616  else:
617  return self._g.async_move()
618 
619  def plan(self, joints=None):
620  """Return a tuple of the motion planning results such as
621  (success flag : boolean, trajectory message : RobotTrajectory,
622  planning time : float, error code : MoveitErrorCodes)"""
623  if type(joints) is str:
625  elif type(joints) is Pose:
626  self.set_pose_target(joints)
627  elif joints is not None:
628  self.set_joint_value_target(joints)
629 
630  (error_code_msg, trajectory_msg, planning_time) = self._g.plan()
631 
632  error_code = MoveItErrorCodes()
633  error_code.deserialize(error_code_msg)
634  plan = RobotTrajectory()
635  return (
636  error_code.val == MoveItErrorCodes.SUCCESS,
637  plan.deserialize(trajectory_msg),
638  planning_time,
639  error_code,
640  )
641 
643  """Returns a MotionPlanRequest filled with the current goals of the move_group_interface"""
644  mpr = MotionPlanRequest()
645  return mpr.deserialize(self._g.construct_motion_plan_request())
646 
648  self,
649  waypoints,
650  eef_step,
651  jump_threshold,
652  avoid_collisions=True,
653  path_constraints=None,
654  ):
655  """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."""
656  if path_constraints:
657  if type(path_constraints) is Constraints:
658  constraints_str = conversions.msg_to_string(path_constraints)
659  else:
661  "Unable to set path constraints, unknown constraint type "
662  + type(path_constraints)
663  )
664  (ser_path, fraction) = self._g.compute_cartesian_path(
665  [conversions.pose_to_list(p) for p in waypoints],
666  eef_step,
667  jump_threshold,
668  avoid_collisions,
669  constraints_str,
670  )
671  else:
672  (ser_path, fraction) = self._g.compute_cartesian_path(
673  [conversions.pose_to_list(p) for p in waypoints],
674  eef_step,
675  jump_threshold,
676  avoid_collisions,
677  )
678 
679  path = RobotTrajectory()
680  path.deserialize(ser_path)
681  return (path, fraction)
682 
683  def execute(self, trajectory, wait=True):
684  """Execute a previously planned path"""
685  if not hasattr(trajectory, "joint_trajectory"):
686  trajectory = RobotTrajectory(joint_trajectory=trajectory)
687 
688  if wait:
689  return self._g.execute(conversions.msg_to_string(trajectory))
690  else:
691  return self._g.async_execute(conversions.msg_to_string(trajectory))
692 
693  def attach_object(self, object_name, link_name="", touch_links=[]):
694  """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."""
695  return self._g.attach_object(object_name, link_name, touch_links)
696 
697  def detach_object(self, name=""):
698  """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."""
699  return self._g.detach_object(name)
700 
701  def pick(self, object_name, grasp=[], plan_only=False):
702  """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument."""
703  if type(grasp) is Grasp:
704  return self._g.pick(
705  object_name, conversions.msg_to_string(grasp), plan_only
706  )
707  else:
708  return self._g.pick(
709  object_name, [conversions.msg_to_string(x) for x in grasp], plan_only
710  )
711 
712  def place(self, object_name, location=None, plan_only=False):
713  """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
714  result = False
715  if not location:
716  result = self._g.place(object_name, plan_only)
717  elif type(location) is PoseStamped:
718  old = self.get_pose_reference_frame()
719  self.set_pose_reference_frame(location.header.frame_id)
720  result = self._g.place(
721  object_name, conversions.pose_to_list(location.pose), plan_only
722  )
723  self.set_pose_reference_frame(old)
724  elif type(location) is Pose:
725  result = self._g.place(
726  object_name, conversions.pose_to_list(location), plan_only
727  )
728  elif type(location) is PlaceLocation:
729  result = self._g.place(
730  object_name, conversions.msg_to_string(location), plan_only
731  )
732  elif type(location) is list:
733  if location:
734  if type(location[0]) is PlaceLocation:
735  result = self._g.place_locations_list(
736  object_name,
737  [conversions.msg_to_string(x) for x in location],
738  plan_only,
739  )
740  elif type(location[0]) is PoseStamped:
741  result = self._g.place_poses_list(
742  object_name,
743  [conversions.msg_to_string(x) for x in location],
744  plan_only,
745  )
746  else:
748  "Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object"
749  )
750  else:
752  "Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object"
753  )
754  return result
755 
756  def set_support_surface_name(self, value):
757  """Set the support surface name for a place operation"""
758  self._g.set_support_surface_name(value)
759 
761  self,
762  ref_state_in,
763  traj_in,
764  velocity_scaling_factor=1.0,
765  acceleration_scaling_factor=1.0,
766  algorithm="iterative_time_parameterization",
767  ):
768  ser_ref_state_in = conversions.msg_to_string(ref_state_in)
769  ser_traj_in = conversions.msg_to_string(traj_in)
770  ser_traj_out = self._g.retime_trajectory(
771  ser_ref_state_in,
772  ser_traj_in,
773  velocity_scaling_factor,
774  acceleration_scaling_factor,
775  algorithm,
776  )
777  traj_out = RobotTrajectory()
778  traj_out.deserialize(ser_traj_out)
779  return traj_out
780 
781  def get_jacobian_matrix(self, joint_values, reference_point=None):
782  """Get the jacobian matrix of the group as a list"""
783  return self._g.get_jacobian_matrix(
784  joint_values,
785  [0.0, 0.0, 0.0] if reference_point is None else reference_point,
786  )
787 
788  def enforce_bounds(self, robot_state_msg):
789  """Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds()"""
790  s = RobotState()
791  c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg))
792  conversions.msg_from_string(s, c_str)
793  return s
moveit_commander.move_group.MoveGroupCommander.set_constraints_database
def set_constraints_database(self, host, port)
Definition: move_group.py:518
moveit_commander.move_group.MoveGroupCommander.plan
def plan(self, joints=None)
Definition: move_group.py:619
moveit_commander.move_group.MoveGroupCommander.set_num_planning_attempts
def set_num_planning_attempts(self, num_planning_attempts)
Definition: move_group.py:546
moveit_commander.move_group.MoveGroupCommander.get_trajectory_constraints
def get_trajectory_constraints(self)
Definition: move_group.py:493
moveit_commander.move_group.MoveGroupCommander.pick
def pick(self, object_name, grasp=[], plan_only=False)
Definition: move_group.py:701
moveit_commander.move_group.MoveGroupCommander.set_planning_pipeline_id
def set_planning_pipeline_id(self, planning_pipeline)
Definition: move_group.py:530
moveit_commander.move_group.MoveGroupCommander.get_joints
def get_joints(self)
Definition: move_group.py:82
moveit_commander.move_group.MoveGroupCommander.get_named_targets
def get_named_targets(self)
Definition: move_group.py:393
moveit_commander.move_group.MoveGroupCommander.clear_trajectory_constraints
def clear_trajectory_constraints(self)
Definition: move_group.py:514
moveit_commander.move_group.MoveGroupCommander.get_name
def get_name(self)
Definition: move_group.py:70
moveit_commander.move_group.MoveGroupCommander
Definition: move_group.py:57
moveit_commander.move_group.MoveGroupCommander.set_goal_orientation_tolerance
def set_goal_orientation_tolerance(self, value)
Definition: move_group.py:454
moveit_commander.move_group.MoveGroupCommander.set_pose_reference_frame
def set_pose_reference_frame(self, reference_frame)
Definition: move_group.py:113
moveit_commander.move_group.MoveGroupCommander.construct_motion_plan_request
def construct_motion_plan_request(self)
Definition: move_group.py:642
moveit_commander.move_group.MoveGroupCommander.stop
def stop(self)
Definition: move_group.py:74
moveit_commander.move_group.MoveGroupCommander.place
def place(self, object_name, location=None, plan_only=False)
Definition: move_group.py:712
moveit_commander.move_group.MoveGroupCommander.clear_max_cartesian_link_speed
def clear_max_cartesian_link_speed(self)
Definition: move_group.py:593
moveit_commander.move_group.MoveGroupCommander.set_planner_id
def set_planner_id(self, planner_id)
Definition: move_group.py:538
moveit_commander.move_group.MoveGroupCommander.get_goal_orientation_tolerance
def get_goal_orientation_tolerance(self)
Definition: move_group.py:438
moveit_commander.move_group.MoveGroupCommander.clear_path_constraints
def clear_path_constraints(self)
Definition: move_group.py:489
moveit_commander.move_group.MoveGroupCommander.set_pose_targets
def set_pose_targets(self, poses, end_effector_link="")
Definition: move_group.py:353
moveit_commander.move_group.MoveGroupCommander.set_start_state_to_current_state
def set_start_state_to_current_state(self)
Definition: move_group.py:156
moveit_commander.move_group.MoveGroupCommander.set_joint_value_target
def set_joint_value_target(self, arg1, arg2=None, arg3=None)
Definition: move_group.py:199
moveit_commander.move_group.MoveGroupCommander.get_goal_position_tolerance
def get_goal_position_tolerance(self)
Definition: move_group.py:434
moveit_commander.move_group.MoveGroupCommander.detach_object
def detach_object(self, name="")
Definition: move_group.py:697
moveit_commander.move_group.MoveGroupCommander.go
def go(self, joints=None, wait=True)
Definition: move_group.py:597
moveit_commander.move_group.MoveGroupCommander.get_remembered_joint_values
def get_remembered_joint_values(self)
Definition: move_group.py:414
moveit_commander.move_group.MoveGroupCommander.set_max_acceleration_scaling_factor
def set_max_acceleration_scaling_factor(self, value)
Definition: move_group.py:575
moveit_commander.move_group.MoveGroupCommander.get_planning_pipeline_id
def get_planning_pipeline_id(self)
Definition: move_group.py:534
moveit_commander.move_group.MoveGroupCommander.get_path_constraints
def get_path_constraints(self)
Definition: move_group.py:470
moveit_commander.move_group.MoveGroupCommander.clear_pose_target
def clear_pose_target(self, end_effector_link)
Definition: move_group.py:381
moveit_commander.move_group.MoveGroupCommander.set_end_effector_link
def set_end_effector_link(self, link_name)
Definition: move_group.py:98
moveit_commander.move_group.MoveGroupCommander.get_goal_joint_tolerance
def get_goal_joint_tolerance(self)
Definition: move_group.py:430
moveit_commander.move_group.MoveGroupCommander.allow_looking
def allow_looking(self, value)
Definition: move_group.py:458
moveit_commander.move_group.MoveGroupCommander.get_planner_id
def get_planner_id(self)
Definition: move_group.py:542
moveit_commander.move_group.MoveGroupCommander.get_jacobian_matrix
def get_jacobian_matrix(self, joint_values, reference_point=None)
Definition: move_group.py:781
moveit_commander.exception.MoveItCommanderException
Definition: exception.py:36
moveit_commander.move_group.MoveGroupCommander.set_max_velocity_scaling_factor
def set_max_velocity_scaling_factor(self, value)
Definition: move_group.py:565
moveit_commander.move_group.MoveGroupCommander.get_current_pose
def get_current_pose(self, end_effector_link="")
Definition: move_group.py:125
moveit_commander.move_group.MoveGroupCommander.set_goal_joint_tolerance
def set_goal_joint_tolerance(self, value)
Definition: move_group.py:446
moveit_commander_cmdline.type
type
Definition: moveit_commander_cmdline.py:202
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:647
moveit_commander.move_group.MoveGroupCommander.get_current_state
def get_current_state(self)
Definition: move_group.py:189
moveit_commander.move_group.MoveGroupCommander.get_random_pose
def get_random_pose(self, end_effector_link="")
Definition: move_group.py:146
moveit_commander.move_group.MoveGroupCommander.set_rpy_target
def set_rpy_target(self, rpy, end_effector_link="")
Definition: move_group.py:286
moveit_commander.move_group.MoveGroupCommander.set_trajectory_constraints
def set_trajectory_constraints(self, value)
Definition: move_group.py:500
moveit_commander.move_group.MoveGroupCommander.set_workspace
def set_workspace(self, ws)
Definition: move_group.py:550
plan
Definition: plan.py:1
moveit_commander.move_group.MoveGroupCommander.get_active_joints
def get_active_joints(self)
Definition: move_group.py:78
moveit_commander.move_group.MoveGroupCommander.get_planning_frame
def get_planning_frame(self)
Definition: move_group.py:117
moveit_commander.move_group.MoveGroupCommander.set_random_target
def set_random_target(self)
Definition: move_group.py:389
moveit_commander.move_group.MoveGroupCommander.set_start_state
def set_start_state(self, msg)
Definition: move_group.py:159
moveit_commander.move_group.MoveGroupCommander.enforce_bounds
def enforce_bounds(self, robot_state_msg)
Definition: move_group.py:788
moveit_commander.move_group.MoveGroupCommander.set_pose_target
def set_pose_target(self, pose, end_effector_link="")
Definition: move_group.py:328
moveit_commander.move_group.MoveGroupCommander._g
_g
Definition: move_group.py:64
moveit_commander.move_group.MoveGroupCommander.get_named_target_values
def get_named_target_values(self, target)
Definition: move_group.py:404
moveit_commander.move_group.MoveGroupCommander.set_planning_time
def set_planning_time(self, seconds)
Definition: move_group.py:522
moveit_commander.move_group.MoveGroupCommander.set_goal_tolerance
def set_goal_tolerance(self, value)
Definition: move_group.py:442
moveit_commander.move_group.MoveGroupCommander.set_support_surface_name
def set_support_surface_name(self, value)
Definition: move_group.py:756
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:450
moveit_commander.move_group.MoveGroupCommander.set_path_constraints
def set_path_constraints(self, value)
Definition: move_group.py:477
moveit_commander.move_group.MoveGroupCommander.get_known_constraints
def get_known_constraints(self)
Definition: move_group.py:466
moveit_commander.move_group.MoveGroupCommander.attach_object
def attach_object(self, object_name, link_name="", touch_links=[])
Definition: move_group.py:693
moveit_commander.move_group.MoveGroupCommander.get_planning_time
def get_planning_time(self)
Definition: move_group.py:526
moveit_commander.move_group.MoveGroupCommander.execute
def execute(self, trajectory, wait=True)
Definition: move_group.py:683
moveit_commander.move_group.MoveGroupCommander.get_current_joint_values
def get_current_joint_values(self)
Definition: move_group.py:121
moveit_commander.move_group.MoveGroupCommander.get_interface_description
def get_interface_description(self)
Definition: move_group.py:103
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:760
moveit_commander.move_group.MoveGroupCommander.get_joint_value_target
def get_joint_value_target(self)
Definition: move_group.py:196
moveit_commander.move_group.MoveGroupCommander.set_position_target
def set_position_target(self, xyz, end_effector_link="")
Definition: move_group.py:316
moveit_commander.move_group.MoveGroupCommander.remember_joint_values
def remember_joint_values(self, name, values=None)
Definition: move_group.py:408
pick
Definition: pick.py:1
moveit_commander.move_group.MoveGroupCommander.allow_replanning
def allow_replanning(self, value)
Definition: move_group.py:462
moveit_commander.move_group.MoveGroupCommander.get_current_state_bounded
def get_current_state_bounded(self)
Definition: move_group.py:182
moveit_commander.move_group.MoveGroupCommander.has_end_effector_link
def has_end_effector_link(self)
Definition: move_group.py:90
moveit_commander.move_group.MoveGroupCommander.set_named_target
def set_named_target(self, name)
Definition: move_group.py:397
moveit_commander.move_group.MoveGroupCommander.get_current_rpy
def get_current_rpy(self, end_effector_link="")
Definition: move_group.py:136
moveit_commander.move_group.MoveGroupCommander.limit_max_cartesian_link_speed
def limit_max_cartesian_link_speed(self, speed, link_name="")
Definition: move_group.py:585
moveit_commander.move_group.MoveGroupCommander.forget_joint_values
def forget_joint_values(self, name)
Definition: move_group.py:418
moveit_commander.move_group.MoveGroupCommander.get_end_effector_link
def get_end_effector_link(self)
Definition: move_group.py:94
moveit_commander.move_group.MoveGroupCommander.shift_pose_target
def shift_pose_target(self, axis, value, end_effector_link="")
Definition: move_group.py:364
moveit_commander.move_group.MoveGroupCommander.clear_pose_targets
def clear_pose_targets(self)
Definition: move_group.py:385
moveit_commander.move_group.MoveGroupCommander.set_orientation_target
def set_orientation_target(self, q, end_effector_link="")
Definition: move_group.py:301
moveit_commander.move_group.MoveGroupCommander.__init__
def __init__(self, name, robot_description="robot_description", ns="", wait_for_servers=5.0)
Definition: move_group.py:62
moveit_commander.move_group.MoveGroupCommander.get_goal_tolerance
def get_goal_tolerance(self)
Definition: move_group.py:422
moveit_commander.move_group.MoveGroupCommander.get_variable_count
def get_variable_count(self)
Definition: move_group.py:86
moveit_commander.move_group.MoveGroupCommander.get_random_joint_values
def get_random_joint_values(self)
Definition: move_group.py:143
moveit_commander.move_group.MoveGroupCommander.get_pose_reference_frame
def get_pose_reference_frame(self)
Definition: move_group.py:109


moveit_commander
Author(s): Ioan Sucan
autogenerated on Tue Feb 27 2024 03:29:21