move_group_interface.py
Go to the documentation of this file.
1 # Copyright 2011-2014, Michael Ferguson
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions
6 # are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above
11 # copyright notice, this list of conditions and the following
12 # disclaimer in the documentation and/or other materials provided
13 # with the distribution.
14 #
15 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
18 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
21 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 # POSSIBILITY OF SUCH DAMAGE.
27 
28 import rospy
29 import actionlib
30 from tf.listener import TransformListener
31 from geometry_msgs.msg import *
32 from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal
33 from moveit_msgs.msg import Constraints, JointConstraint, PositionConstraint, OrientationConstraint, BoundingVolume
34 from shape_msgs.msg import SolidPrimitive
35 
36 
37 
38 class MoveGroupInterface(object):
39 
40 
46  def __init__(self, group, frame, listener=None, plan_only=False, move_group="move_group"):
47  self._group = group
48  self._fixed_frame = frame
50  MoveGroupAction)
51  self._action.wait_for_server()
52  if listener == None:
53  self._listener = TransformListener()
54  else:
55  self._listener = listener
56  self.plan_only = plan_only
57  self.planner_id = None
58  self.planning_time = 15.0
59 
60  def get_move_action(self):
61  return self._action
62 
63 
64  def moveToJointPosition(self,
65  joints,
66  positions,
67  tolerance=0.01,
68  wait=True,
69  **kwargs):
70  # Check arguments
71  supported_args = ("max_velocity_scaling_factor",
72  "planner_id",
73  "planning_scene_diff",
74  "planning_time",
75  "plan_only",
76  "start_state")
77  for arg in kwargs.keys():
78  if not arg in supported_args:
79  rospy.loginfo("moveToJointPosition: unsupported argument: %s",
80  arg)
81 
82  # Create goal
83  g = MoveGroupGoal()
84 
85  # 1. fill in workspace_parameters
86 
87  # 2. fill in start_state
88  try:
89  g.request.start_state = kwargs["start_state"]
90  except KeyError:
91  g.request.start_state.is_diff = True
92 
93  # 3. fill in goal_constraints
94  c1 = Constraints()
95  for i in range(len(joints)):
96  c1.joint_constraints.append(JointConstraint())
97  c1.joint_constraints[i].joint_name = joints[i]
98  c1.joint_constraints[i].position = positions[i]
99  c1.joint_constraints[i].tolerance_above = tolerance
100  c1.joint_constraints[i].tolerance_below = tolerance
101  c1.joint_constraints[i].weight = 1.0
102  g.request.goal_constraints.append(c1)
103 
104  # 4. fill in path constraints
105 
106  # 5. fill in trajectory constraints
107 
108  # 6. fill in planner id
109  try:
110  g.request.planner_id = kwargs["planner_id"]
111  except KeyError:
112  if self.planner_id:
113  g.request.planner_id = self.planner_id
114 
115  # 7. fill in group name
116  g.request.group_name = self._group
117 
118  # 8. fill in number of planning attempts
119  try:
120  g.request.num_planning_attempts = kwargs["num_attempts"]
121  except KeyError:
122  g.request.num_planning_attempts = 1
123 
124  # 9. fill in allowed planning time
125  try:
126  g.request.allowed_planning_time = kwargs["planning_time"]
127  except KeyError:
128  g.request.allowed_planning_time = self.planning_time
129 
130  # Fill in velocity scaling factor
131  try:
132  g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
133  except KeyError:
134  pass # do not fill in at all
135 
136  # 10. fill in planning options diff
137  try:
138  g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
139  except KeyError:
140  g.planning_options.planning_scene_diff.is_diff = True
141  g.planning_options.planning_scene_diff.robot_state.is_diff = True
142 
143  # 11. fill in planning options plan only
144  try:
145  g.planning_options.plan_only = kwargs["plan_only"]
146  except KeyError:
147  g.planning_options.plan_only = self.plan_only
148 
149  # 12. fill in other planning options
150  g.planning_options.look_around = False
151  g.planning_options.replan = False
152 
153  # 13. send goal
154  self._action.send_goal(g)
155  if wait:
156  self._action.wait_for_result()
157  return self._action.get_result()
158  else:
159  return None
160 
161 
162  def moveToPose(self,
163  pose_stamped,
164  gripper_frame,
165  tolerance=0.01,
166  wait=True,
167  **kwargs):
168  # Check arguments
169  supported_args = ("max_velocity_scaling_factor",
170  "planner_id",
171  "planning_time",
172  "plan_only",
173  "start_state")
174  for arg in kwargs.keys():
175  if not arg in supported_args:
176  rospy.loginfo("moveToPose: unsupported argument: %s",
177  arg)
178 
179  # Create goal
180  g = MoveGroupGoal()
181  pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
182 
183  # 1. fill in request workspace_parameters
184 
185  # 2. fill in request start_state
186  try:
187  g.request.start_state = kwargs["start_state"]
188  except KeyError:
189  g.request.start_state.is_diff = True
190 
191  # 3. fill in request goal_constraints
192  c1 = Constraints()
193 
194  c1.position_constraints.append(PositionConstraint())
195  c1.position_constraints[0].header.frame_id = self._fixed_frame
196  c1.position_constraints[0].link_name = gripper_frame
197  b = BoundingVolume()
198  s = SolidPrimitive()
199  s.dimensions = [tolerance * tolerance]
200  s.type = s.SPHERE
201  b.primitives.append(s)
202  b.primitive_poses.append(pose_transformed.pose)
203  c1.position_constraints[0].constraint_region = b
204  c1.position_constraints[0].weight = 1.0
205 
206  c1.orientation_constraints.append(OrientationConstraint())
207  c1.orientation_constraints[0].header.frame_id = self._fixed_frame
208  c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
209  c1.orientation_constraints[0].link_name = gripper_frame
210  c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
211  c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
212  c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
213  c1.orientation_constraints[0].weight = 1.0
214 
215  g.request.goal_constraints.append(c1)
216 
217  # 4. fill in request path constraints
218 
219  # 5. fill in request trajectory constraints
220 
221  # 6. fill in request planner id
222  try:
223  g.request.planner_id = kwargs["planner_id"]
224  except KeyError:
225  if self.planner_id:
226  g.request.planner_id = self.planner_id
227 
228  # 7. fill in request group name
229  g.request.group_name = self._group
230 
231  # 8. fill in request number of planning attempts
232  try:
233  g.request.num_planning_attempts = kwargs["num_attempts"]
234  except KeyError:
235  g.request.num_planning_attempts = 1
236 
237  # 9. fill in request allowed planning time
238  try:
239  g.request.allowed_planning_time = kwargs["planning_time"]
240  except KeyError:
241  g.request.allowed_planning_time = self.planning_time
242 
243  # Fill in velocity scaling factor
244  try:
245  g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
246  except KeyError:
247  pass # do not fill in at all
248 
249  # 10. fill in planning options diff
250  g.planning_options.planning_scene_diff.is_diff = True
251  g.planning_options.planning_scene_diff.robot_state.is_diff = True
252 
253  # 11. fill in planning options plan only
254  try:
255  g.planning_options.plan_only = kwargs["plan_only"]
256  except KeyError:
257  g.planning_options.plan_only = self.plan_only
258 
259  # 12. fill in other planning options
260  g.planning_options.look_around = False
261  g.planning_options.replan = False
262 
263  # 13. send goal
264  self._action.send_goal(g)
265  if wait:
266  self._action.wait_for_result()
267  return self._action.get_result()
268  else:
269  return None
270 
271 
273  def setPlannerId(self, planner_id):
274  self.planner_id = str(planner_id)
275 
276 
277  def setPlanningTime(self, time):
278  self.planning_time = time
def setPlanningTime(self, time)
Set default planning time to be used for future planning request.
def moveToPose(self, pose_stamped, gripper_frame, tolerance=0.01, wait=True, kwargs)
Move the arm, based on a goal pose_stamped for the end effector.
def setPlannerId(self, planner_id)
Sets the planner_id used for all future planning requests.
def __init__(self, group, frame, listener=None, plan_only=False, move_group="move_group")
Constructor for this utility.
def moveToJointPosition(self, joints, positions, tolerance=0.01, wait=True, kwargs)
Move the arm to set of joint position goals.
Pure python interface to move_group action.


moveit_python
Author(s): Michael Ferguson
autogenerated on Sat Jan 7 2023 03:09:45