interpolated_ik_motion_planner.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # author: Kaijen Hsiao
00035 
00036 ## @package interpolated_ik_motion_planner
00037 # Server to provide interpolated IK motion plans
00038 # relevant params:
00039 # num_steps: the number of steps to use when interpolating including start and finish (0 means use pos_spacing and 
00040 #     rot_spacing to calculate the number of steps, anything ==1 or <0 will become 2, start and finish)
00041 # consistent_angle: the max angle distance (in any joint) before we declare that the path is inconsistent
00042 # collision_check_resolution: how many steps between collision checks (0 or 1 is check every step; 2 is every other, etc.)
00043 # steps_before_abort: the number of steps in the plan (starting from the end and going backwards) that can be invalid 
00044 #     due to collisions or inconsistency before aborting (0 is abort as soon as you find one, 1 is allow one and still 
00045 #     continue, -1 or >=num_steps to never abort early)
00046 # pos_spacing: the max translation (m) to move the wrist between waypoints (only used if num_steps is 0)
00047 # rot_spacing: the max rotation (rad) to move the wrist between waypoints (only used if num_steps is 0)
00048 # collision_aware: if this is 0, collisions won't be checked for (returns non-collision aware IK solutions)
00049 # start_from_end: if this is 1, the planner searches for an IK solution for the end first, then works backwards from there
00050 # max_joint_vels: a list of maximum joint velocities to use when computing times and velocities for the joint trajectory (defaults to [.1]*7 if left empty)
00051 # max_joint_accs: a list of maximum accelerations to use when computing times and velocities for the joint trajectory (defaults to [.25]*7 if left empty)
00052 
00053 # Parameters can be changed by calling the r_interpolated_ik_motion_plan_set_params service (or l_inter...)
00054 
00055 
00056 # The main service message is GetMotionPlan.srv, in arm_navigation_msgs.  Parts that were hijacked 
00057 # for the relevant inputs and outputs:
00058 
00059 # inputs:
00060 # req.motion_plan_request.start_state.joint_state.name: the names of the arm joints in order (defaults to those provided by IKQuery, if not specified)
00061 # req.motion_plan_request.start_state.joint_state.position: a set of joint angles to stay sort of close to if possible (defaults to current joint angles if not specified)
00062 
00063 # req.motion_plan_request.start_state.multi_dof_joint_state.pose: the start pose for the r_wrist_roll_link
00064 # req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_id: the link to put at that pose (better be 'r_wrist_roll_link' or 'l_wrist_roll_link')
00065 # req.motion_plan_request.start_state.multi_dof_joint_state.frame_id: the frame for the start pose
00066 
00067 # req.motion_plan_request.goal_constraints.position_constraints[0].position: the (x,y,z) goal position of the same link as the start pose
00068 # req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id: the frame that goal position is in
00069 # req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation: the (x,y,z,w) goal orientation of the same link as the start pose
00070 # req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id: the frame that goal orientation is in
00071 
00072 # req.motion_plan_request.workspace_parameters.ordered_collision_operations: the list of collision operations to modify the collision space
00073 # req.motion_plan_request.link_padding: links and collision-space padding to change from the default dynamically
00074 
00075 # outputs:
00076 # res.trajectory.joint_trajectory.joint_names: the arm joint names in the normal order, as spit out by IKQuery
00077 # res.trajectory.joint_trajectory.points: a list of JointTrajectoryPoints whose 'positions' fields are lists of joint angles in a trajectory that gets you from start to goal (positions will be all 0s if a point on the path is out of reach),
00078 # 'velocities' fields are reasonable joint velocities, and 'time_from_start' fields are reasonable times to get to each waypoint. ('accelerations' will be empty.)
00079 # res.trajectory_error_codes: a list of ArmNavigationErrorCodes messages the same length as the trajectory, 
00080 # with values for each trajectory point as follows:
00081 # ArmNavigationErrorCodes.SUCCESS (1): no problem
00082 # ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
00083 # ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-22): inconsistency in path between this point and the next point
00084 # ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
00085 # ArmNavigationErrorCodes.PLANNING_FAILED (-1): aborted before getting to this point
00086 
00087 # Everything else is completely ignored.
00088 
00089 
00090 import roslib; roslib.load_manifest('interpolated_ik_motion_planner')
00091 import rospy
00092 import interpolated_ik_motion_planner.ik_utilities as ik_utilities
00093 import math
00094 import sys
00095 import pdb
00096 from arm_navigation_msgs.srv import GetMotionPlan, GetMotionPlanResponse
00097 from arm_navigation_msgs.msg import ArmNavigationErrorCodes, RobotState
00098 from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Pose, Point, Quaternion
00099 from trajectory_msgs.msg import JointTrajectoryPoint
00100 from interpolated_ik_motion_planner.srv import SetInterpolatedIKMotionPlanParams, SetInterpolatedIKMotionPlanParamsResponse
00101 from sensor_msgs.msg import JointState
00102 
00103 # class to provide the interpolated ik motion planner service
00104 class InterpolatedIKService:
00105 
00106     def __init__(self, which_arm): #which_arm is 'r', 'l', or None
00107         self.which_arm = which_arm 
00108         self.node_name = 'interpolated_ik_motion_planner_server'
00109         if self.which_arm != None:
00110             self.node_name = which_arm+'_'+self.node_name
00111         rospy.init_node(self.node_name)
00112 
00113         #get parameters from the parameter server
00114 
00115         #the number of steps to use when interpolating (including start and finish)
00116         self.num_steps = rospy.get_param(self.node_name+'/num_steps', 6)
00117 
00118         #the max angle distance (in any joint) before we declare that the path is inconsistent
00119         self.consistent_angle = rospy.get_param(self.node_name+'/consistent_angle', math.pi/9) 
00120 
00121         #how many steps between collision checks (0 or 1 is check every step; 2 is every other, etc.)
00122         self.collision_check_resolution = rospy.get_param(self.node_name+'/collision_check_resolution', 1)
00123 
00124         #how many steps in the plan can be invalid before aborting
00125         self.steps_before_abort = rospy.get_param(self.node_name+'/steps_before_abort', 0)
00126 
00127         #the max translation (m) to move the wrist between waypoints (only used if num_steps is 0)
00128         self.pos_spacing = rospy.get_param(self.node_name+'/pos_spacing', 0.01)
00129 
00130         #the max rotation (rad) to move the wrist between waypoints (only used if num_steps is 0)
00131         self.rot_spacing = rospy.get_param(self.node_name+'/rot_spacing', .1)
00132 
00133         #whether to check for collisions or not
00134         self.collision_aware = rospy.get_param(self.node_name+'/collision_aware', 1)
00135 
00136         #if 1, starts the IK calculation from the end and works backwards (if 0, starts from the start)
00137         self.start_from_end = rospy.get_param(self.node_name+'/start_from_end', 0)
00138 
00139         #max joint velocities to use when calculating times and vels for the trajectory
00140         self.max_joint_vels = rospy.get_param(self.node_name+'/max_joint_vels', [.1]*7)
00141 
00142         #max joint accelerations to use when calculating times and vels for the trajectory
00143         self.max_joint_accs = rospy.get_param(self.node_name+'/max_joint_accs', [.25]*7)
00144 
00145         #initialize an IKUtilities class object
00146         if which_arm == 'r':
00147             self.ik_utils = ik_utilities.IKUtilities('right')
00148         elif which_arm == 'l':
00149             self.ik_utils = ik_utilities.IKUtilities('left')            
00150         else:
00151             self.ik_utils = ik_utilities.IKUtilities()
00152 
00153         #advertise interpolated IK service
00154         service_name = 'interpolated_ik_motion_plan'
00155         if which_arm != None:
00156             service_name = which_arm+'_'+service_name
00157         s1 = rospy.Service(service_name, GetMotionPlan, self.interpolated_ik_motion_planner_callback)
00158 
00159         #advertise param changing service
00160         s2 = rospy.Service(service_name+'_set_params', \
00161                 SetInterpolatedIKMotionPlanParams, self.set_params_callback)
00162 
00163 
00164     ##add a header to a message with a 0 timestamp (good for getting the latest TF transform)
00165     def add_header(self, msg, frame):
00166         msg.header.frame_id = frame
00167         msg.header.stamp = rospy.Time()
00168         return msg
00169 
00170 
00171     ##pretty-print list to string
00172     def pplist(self, list):
00173         return ' '.join(['%2.3f'%x for x in list])
00174 
00175 
00176     ##callback for the set_params service
00177     def set_params_callback(self, req):
00178         self.num_steps = req.num_steps
00179         self.consistent_angle = req.consistent_angle
00180         self.collision_check_resolution= req.collision_check_resolution
00181         self.steps_before_abort = req.steps_before_abort
00182         self.pos_spacing = req.pos_spacing
00183         self.rot_spacing = req.rot_spacing
00184         self.collision_aware = req.collision_aware
00185         self.start_from_end = req.start_from_end
00186         self.max_joint_vels = req.max_joint_vels
00187         self.max_joint_accs = req.max_joint_accs
00188         return SetInterpolatedIKMotionPlanParamsResponse()
00189 
00190 
00191     ##callback for get_interpolated_ik_motion_plan service
00192     def interpolated_ik_motion_planner_callback(self, req):
00193 
00194         #names and angles for the joints in their desired order
00195         joint_names = req.motion_plan_request.start_state.joint_state.name
00196         start_angles = req.motion_plan_request.start_state.joint_state.position
00197 
00198         #sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified
00199         if start_angles and len(joint_names) != len(start_angles):
00200             rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position!  Quitting")
00201             return 0
00202     
00203         #reorder the start angles to the order needed by IK
00204         reordered_start_angles = []
00205 
00206         #get the desired start angles for each IK arm joint in turn from start_state.joint_state.position
00207         #(use the current angle if not specified)
00208         for joint_name in self.ik_utils.joint_names:
00209 
00210             #desired start angle specified
00211             if joint_name in joint_names and start_angles:
00212                 index = joint_names.index(joint_name)
00213                 reordered_start_angles.append(start_angles[index])
00214             else:
00215                 rospy.logerr("missing joint angle, can't deal")
00216                 return 0
00217 
00218         #get additional desired joint angles (such as for the gripper) to pass through to IK
00219         additional_joint_angles = []
00220         additional_joint_names = []
00221         for (ind, joint_name) in enumerate(joint_names):
00222             if joint_name not in self.ik_utils.joint_names:
00223                 #rospy.loginfo("found %s"%joint_name)
00224                 additional_joint_angles.append(start_angles[ind])
00225                 additional_joint_names.append(joint_name)
00226         IK_robot_state = None
00227         if additional_joint_angles:
00228             IK_robot_state = RobotState()
00229             IK_robot_state.joint_state.name = additional_joint_names
00230             IK_robot_state.joint_state.position = additional_joint_angles
00231 
00232         #check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now)
00233         link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0]
00234         if link_name != self.ik_utils.link_name:
00235             rospy.logerr("link_name not allowed: %s"%link_name)
00236             return 0
00237 
00238         #the start pose for that link
00239         start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0]
00240 
00241         #the frame that start pose is in
00242         frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0]
00243 
00244         #turn it into a PoseStamped
00245         start_pose_stamped = self.add_header(PoseStamped(), frame_id)
00246         start_pose_stamped.pose = start_pose
00247 
00248         #the desired goal position
00249         goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position         
00250         
00251         #the frame that goal position is in
00252         goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id
00253 
00254         #convert the position to base_link frame
00255         goal_ps = self.add_header(PointStamped(), goal_pos_frame)
00256         goal_ps.point = goal_pos
00257         goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, 'base_link')
00258 
00259         #the desired goal orientation
00260         goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation
00261 
00262         #the frame that goal orientation is in
00263         goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id 
00264 
00265         #convert the quaternion to base_link frame
00266         goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
00267         goal_qs.quaternion = goal_quat
00268         goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, 'base_link')
00269 
00270         #assemble the goal pose into a PoseStamped
00271         goal_pose_stamped = self.add_header(PoseStamped(), 'base_link')
00272         goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list))
00273 
00274         #get the ordered collision operations, if there are any
00275         ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations
00276 
00277         #get the link paddings, if there are any
00278         link_padding = None #req.motion_plan_request.link_padding
00279 
00280         #RUN!  Check the Cartesian path for consistent, non-colliding IK solutions
00281         (trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
00282                  goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
00283                  self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
00284                  self.steps_before_abort, self.num_steps, ordered_collision_operations, \
00285                  self.start_from_end, IK_robot_state, link_padding)
00286 
00287         #find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0)
00288         #if we're searching from the end, keep the end; if we're searching from the start, keep the start
00289         start_ind = 0
00290         stop_ind = len(error_codes)
00291         if self.start_from_end:
00292             for ind in range(len(error_codes)-1, 0, -1):
00293                 if error_codes[ind]:
00294                     start_ind = ind+1
00295                     break
00296         else:
00297             for ind in range(len(error_codes)):
00298                 if error_codes[ind]:
00299                     stop_ind = ind
00300                     break
00301         (times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs)
00302         times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind)
00303         vels = [[0]*len(self.max_joint_vels)]*start_ind + vels + [[0]*len(self.max_joint_vels)]*(len(error_codes)-stop_ind)
00304 
00305         rospy.logdebug("trajectory:")
00306         for ind in range(len(trajectory)):
00307             rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
00308         rospy.logdebug("")
00309         for ind in range(len(trajectory)):
00310             rospy.logdebug("time: " + "%5.3f  "%times[ind] + "vels: " + self.pplist(vels[ind]))
00311 
00312         #the response
00313         res = GetMotionPlanResponse()
00314 
00315         #the arm joint names in the normal order, as spit out by IKQuery
00316         res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]
00317 
00318         #a list of lists of joint angles, velocities, and times for a trajectory that gets you from start to goal 
00319         #(all 0s if there was no IK solution for a point on the path)
00320         res.trajectory.joint_trajectory.points = []
00321         for i in range(len(trajectory)):
00322             joint_trajectory_point = JointTrajectoryPoint()
00323             joint_trajectory_point.positions = trajectory[i]
00324             joint_trajectory_point.velocities = vels[i]
00325             joint_trajectory_point.time_from_start = rospy.Duration(times[i])
00326             res.trajectory.joint_trajectory.points.append(joint_trajectory_point)
00327 
00328         #a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows:
00329         #ArmNavigationErrorCodes.SUCCESS (1): no problem
00330         #ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
00331         #ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-22): inconsistency in path between this point and the next point
00332         #ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
00333         #ArmNavigationErrorCodes.PLANNING_FAILED (-1): aborted before getting to this point
00334         error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
00335                            2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
00336                            4:ArmNavigationErrorCodes.PLANNING_FAILED}
00337 
00338         trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes]
00339         res.trajectory_error_codes = trajectory_error_codes 
00340         res.error_code.val = ArmNavigationErrorCodes.SUCCESS
00341 #         rospy.loginfo("trajectory:")
00342 #         for ind in range(len(trajectory)):
00343 #             rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
00344 #         rospy.loginfo("")
00345 #         for ind in range(len(trajectory)):
00346 #             rospy.loginfo("time: " + "%5.3f  "%times[ind] + "vels: " + self.pplist(vels[ind]))
00347 
00348         return res
00349 
00350 
00351 if __name__ == "__main__":
00352 
00353     which_arm = None
00354     if len(rospy.myargv()) >= 2:
00355         which_arm = rospy.myargv()[1]
00356     interpolated_ik_service = InterpolatedIKService(which_arm)
00357     rospy.loginfo("Ready to serve interpolated IK motion plan requests.")
00358 
00359     rospy.spin()    
00360 


interpolated_ik_motion_planner
Author(s): Kaijen Hsiao
autogenerated on Thu Dec 12 2013 11:08:26