move_group_interface.py
Go to the documentation of this file.
00001 # Copyright 2011-2014, Michael Ferguson
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # are met:
00007 #
00008 #  * Redistributions of source code must retain the above copyright
00009 #    notice, this list of conditions and the following disclaimer.
00010 #  * Redistributions in binary form must reproduce the above
00011 #    copyright notice, this list of conditions and the following
00012 #    disclaimer in the documentation and/or other materials provided
00013 #    with the distribution.
00014 #
00015 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00016 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00017 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00018 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00019 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00021 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00023 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00024 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00025 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 # POSSIBILITY OF SUCH DAMAGE.
00027 
00028 import rospy
00029 import actionlib
00030 from tf.listener import TransformListener
00031 from geometry_msgs.msg import *
00032 from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal
00033 from moveit_msgs.msg import Constraints, JointConstraint, PositionConstraint, OrientationConstraint, BoundingVolume
00034 from shape_msgs.msg import SolidPrimitive
00035 
00036 
00037 ## @brief Pure python interface to move_group action
00038 class MoveGroupInterface(object):
00039 
00040     ## @brief Constructor for this utility
00041     ## @param group Name of the MoveIt! group to command
00042     ## @param frame Name of the fixed frame in which planning happens
00043     ## @param listener A TF listener instance (optional, will create a new one if None)
00044     ## @param plan_only Should we only plan, but not execute?
00045     def __init__(self, group, frame, listener=None, plan_only=False):
00046         self._group = group
00047         self._fixed_frame = frame
00048         self._action = actionlib.SimpleActionClient('move_group',
00049                                                     MoveGroupAction)
00050         self._action.wait_for_server()
00051         if listener == None:
00052             self._listener = TransformListener()
00053         else:
00054             self._listener = listener
00055         self.plan_only = plan_only
00056         self.planner_id = None
00057         self.planning_time = 15.0
00058 
00059     def get_move_action(self):
00060         return self._action
00061 
00062     ## @brief Move the arm to set of joint position goals
00063     def moveToJointPosition(self,
00064                             joints,
00065                             positions,
00066                             tolerance=0.01,
00067                             wait=True,
00068                             **kwargs):
00069         # Check arguments
00070         supported_args = ("max_velocity_scaling_factor",
00071                           "planner_id",
00072                           "planning_scene_diff",
00073                           "planning_time",
00074                           "plan_only",
00075                           "start_state")
00076         for arg in kwargs.keys():
00077             if not arg in supported_args:
00078                 rospy.loginfo("moveToJointPosition: unsupported argument: %s",
00079                               arg)
00080 
00081         # Create goal
00082         g = MoveGroupGoal()
00083 
00084         # 1. fill in workspace_parameters
00085 
00086         # 2. fill in start_state
00087         try:
00088             g.request.start_state = kwargs["start_state"]
00089         except KeyError:
00090             g.request.start_state.is_diff = True
00091 
00092         # 3. fill in goal_constraints
00093         c1 = Constraints()
00094         for i in range(len(joints)):
00095             c1.joint_constraints.append(JointConstraint())
00096             c1.joint_constraints[i].joint_name = joints[i]
00097             c1.joint_constraints[i].position = positions[i]
00098             c1.joint_constraints[i].tolerance_above = tolerance
00099             c1.joint_constraints[i].tolerance_below = tolerance
00100             c1.joint_constraints[i].weight = 1.0
00101         g.request.goal_constraints.append(c1)
00102 
00103         # 4. fill in path constraints
00104 
00105         # 5. fill in trajectory constraints
00106 
00107         # 6. fill in planner id
00108         try:
00109             g.request.planner_id = kwargs["planner_id"]
00110         except KeyError:
00111             if self.planner_id:
00112                 g.request.planner_id = self.planner_id
00113 
00114         # 7. fill in group name
00115         g.request.group_name = self._group
00116 
00117         # 8. fill in number of planning attempts
00118         try:
00119             g.request.num_planning_attempts = kwargs["num_attempts"]
00120         except KeyError:
00121             g.request.num_planning_attempts = 1
00122 
00123         # 9. fill in allowed planning time
00124         try:
00125             g.request.allowed_planning_time = kwargs["planning_time"]
00126         except KeyError:
00127             g.request.allowed_planning_time = self.planning_time
00128 
00129         # Fill in velocity scaling factor
00130         try:
00131             g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
00132         except KeyError:
00133             pass  # do not fill in at all
00134 
00135         # 10. fill in planning options diff
00136         try:
00137             g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
00138         except KeyError:
00139             g.planning_options.planning_scene_diff.is_diff = True
00140             g.planning_options.planning_scene_diff.robot_state.is_diff = True
00141 
00142         # 11. fill in planning options plan only
00143         try:
00144             g.planning_options.plan_only = kwargs["plan_only"]
00145         except KeyError:
00146             g.planning_options.plan_only = self.plan_only
00147 
00148         # 12. fill in other planning options
00149         g.planning_options.look_around = False
00150         g.planning_options.replan = False
00151 
00152         # 13. send goal
00153         self._action.send_goal(g)
00154         if wait:
00155             self._action.wait_for_result()
00156             return self._action.get_result()
00157         else:
00158             return None
00159 
00160     ## @brief Move the arm, based on a goal pose_stamped for the end effector.
00161     def moveToPose(self,
00162                    pose_stamped,
00163                    gripper_frame,
00164                    tolerance=0.01,
00165                    wait=True,
00166                    **kwargs):
00167         # Check arguments
00168         supported_args = ("max_velocity_scaling_factor",
00169                           "planner_id",
00170                           "planning_time",
00171                           "plan_only",
00172                           "start_state")
00173         for arg in kwargs.keys():
00174             if not arg in supported_args:
00175                 rospy.loginfo("moveToJointPosition: unsupported argument: %s",
00176                               arg)
00177 
00178         # Create goal
00179         g = MoveGroupGoal()
00180         pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
00181 
00182         # 1. fill in request workspace_parameters
00183 
00184         # 2. fill in request start_state
00185         try:
00186             g.request.start_state = kwargs["start_state"]
00187         except KeyError:
00188             g.request.start_state.is_diff = True
00189 
00190         # 3. fill in request goal_constraints
00191         c1 = Constraints()
00192 
00193         c1.position_constraints.append(PositionConstraint())
00194         c1.position_constraints[0].header.frame_id = self._fixed_frame
00195         c1.position_constraints[0].link_name = gripper_frame
00196         b = BoundingVolume()
00197         s = SolidPrimitive()
00198         s.dimensions = [tolerance * tolerance]
00199         s.type = s.SPHERE
00200         b.primitives.append(s)
00201         b.primitive_poses.append(pose_transformed.pose)
00202         c1.position_constraints[0].constraint_region = b
00203         c1.position_constraints[0].weight = 1.0
00204 
00205         c1.orientation_constraints.append(OrientationConstraint())
00206         c1.orientation_constraints[0].header.frame_id = self._fixed_frame
00207         c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
00208         c1.orientation_constraints[0].link_name = gripper_frame
00209         c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
00210         c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
00211         c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
00212         c1.orientation_constraints[0].weight = 1.0
00213 
00214         g.request.goal_constraints.append(c1)
00215 
00216         # 4. fill in request path constraints
00217 
00218         # 5. fill in request trajectory constraints
00219 
00220         # 6. fill in request planner id
00221         try:
00222             g.request.planner_id = kwargs["planner_id"]
00223         except KeyError:
00224             if self.planner_id:
00225                 g.request.planner_id = self.planner_id
00226 
00227         # 7. fill in request group name
00228         g.request.group_name = self._group
00229 
00230         # 8. fill in request number of planning attempts
00231         try:
00232             g.request.num_planning_attempts = kwargs["num_attempts"]
00233         except KeyError:
00234             g.request.num_planning_attempts = 1
00235 
00236         # 9. fill in request allowed planning time
00237         try:
00238             g.request.allowed_planning_time = kwargs["planning_time"]
00239         except KeyError:
00240             g.request.allowed_planning_time = self.planning_time
00241 
00242         # Fill in velocity scaling factor
00243         try:
00244             g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
00245         except KeyError:
00246             pass  # do not fill in at all
00247 
00248         # 10. fill in planning options diff
00249         g.planning_options.planning_scene_diff.is_diff = True
00250         g.planning_options.planning_scene_diff.robot_state.is_diff = True
00251 
00252         # 11. fill in planning options plan only
00253         try:
00254             g.planning_options.plan_only = kwargs["plan_only"]
00255         except KeyError:
00256             g.planning_options.plan_only = self.plan_only
00257 
00258         # 12. fill in other planning options
00259         g.planning_options.look_around = False
00260         g.planning_options.replan = False
00261 
00262         # 13. send goal
00263         self._action.send_goal(g)
00264         if wait:
00265             self._action.wait_for_result()
00266             return self._action.get_result()
00267         else:
00268             return None
00269 
00270     ## @brief Sets the planner_id used for all future planning requests.
00271     ## @param planner_id The string for the planner id, set to None to clear
00272     def setPlannerId(self, planner_id):
00273         self.planner_id = str(planner_id)
00274 
00275     ## @brief Set default planning time to be used for future planning request.
00276     def setPlanningTime(self, time):
00277         self.planning_time = time


moveit_python
Author(s): Michael Ferguson
autogenerated on Fri Aug 28 2015 11:32:16