ik_utilities.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 ik_utilities
00037 #Utility functions for doing inverse kinematics, forward kinematics, checking a Cartesian path
00038 
00039 import roslib; roslib.load_manifest('interpolated_ik_motion_planner')
00040 import rospy
00041 from kinematics_msgs.srv import GetKinematicSolverInfo, GetPositionIK, GetPositionFK, GetConstraintAwarePositionIK, GetConstraintAwarePositionIKRequest
00042 from kinematics_msgs.msg import PositionIKRequest
00043 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, PointStamped, Vector3Stamped
00044 from visualization_msgs.msg import Marker
00045 from arm_navigation_msgs.msg import RobotState, MultiDOFJointState, ArmNavigationErrorCodes
00046 from arm_navigation_msgs.srv import GetStateValidity, GetStateValidityRequest
00047 from sensor_msgs.msg import JointState
00048 import math
00049 import random
00050 import time
00051 import tf
00052 import numpy
00053 import pdb
00054 
00055 #pretty-print list to string
00056 def pplist(list):
00057     return ' '.join(['%8.5f'%x for x in list])
00058 
00059 #utility functions for doing inverse kinematics, forward kinematics, checking a Cartesian path
00060 class IKUtilities:
00061 
00062     #initialize all service functions
00063     #if wait_for_services = 0, you must call check_services_and_get_ik_info externally before running any of the IK/FK functions
00064     def __init__(self, whicharm = None, tf_listener = None, wait_for_services = 1): #whicharm is 'right', 'left', or None
00065         
00066         #gets the robot_prefix from the parameter server. Default is pr2 
00067         robot_prefix = rospy.get_param('~robot_prefix', 'pr2') 
00068         if whicharm != None:
00069             self.srvroot = '/'+robot_prefix+'_'+whicharm+'_arm_kinematics/' 
00070         else:
00071             self.srvroot = '/'+robot_prefix+'_arm_kinematics/'
00072 
00073         #If collision_aware_ik is set to 0, then collision-aware IK is disabled 
00074         self.perception_running = rospy.get_param('~collision_aware_ik', 1) 
00075 
00076         self._ik_service = rospy.ServiceProxy(self.srvroot+'get_ik', GetPositionIK, True)
00077         if self.perception_running:
00078             self._ik_service_with_collision = rospy.ServiceProxy(self.srvroot+'get_constraint_aware_ik', GetConstraintAwarePositionIK, True)
00079 
00080         self._fk_service = rospy.ServiceProxy(self.srvroot+'get_fk', GetPositionFK, True)
00081         self._query_service = rospy.ServiceProxy(self.srvroot+'get_ik_solver_info', GetKinematicSolverInfo, True)
00082         self._check_state_validity_service = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity, True)
00083 
00084         #wait for IK/FK/query services and get the joint names and limits 
00085         if wait_for_services:
00086             self.check_services_and_get_ik_info()
00087         
00088         if tf_listener == None:
00089             rospy.loginfo("ik_utilities: starting up tf_listener")
00090             self.tf_listener = tf.TransformListener()
00091         else:
00092             self.tf_listener = tf_listener
00093 
00094         self.marker_pub = rospy.Publisher('interpolation_markers', Marker)
00095 
00096         #dictionary for the possible kinematics error codes
00097         self.error_code_dict = {}  #codes are things like SUCCESS, NO_IK_SOLUTION
00098         for element in dir(ArmNavigationErrorCodes):
00099             if element[0].isupper():
00100                 self.error_code_dict[eval('ArmNavigationErrorCodes.'+element)] = element
00101 
00102         #reads the start angles from the parameter server 
00103         start_angles_list = rospy.get_param('~ik_start_angles', []) 
00104                          
00105         #good additional start angles to try for IK for the PR2, used  
00106         #if no start angles were provided 
00107         if start_angles_list == []: 
00108             self.start_angles_list = [[-0.697, 1.002, 0.021, -0.574, 0.286, -0.095, 1.699], 
00109                                       [-1.027, 0.996, 0.034, -0.333, -3.541, -0.892, 1.694], 
00110                                       [0.031, -0.124, -2.105, -1.145, -1.227, -1.191, 2.690], 
00111                                       [0.410, 0.319, -2.231, -0.839, -2.751, -1.763, 5.494], 
00112                                       [0.045, 0.859, 0.059, -0.781, -1.579, -0.891, 7.707], 
00113                                       [0.420, 0.759, 0.014, -1.099, -3.204, -1.907, 8.753], 
00114                                       [-0.504, 1.297, -1.857, -1.553, -4.453, -1.308, 9.572]] 
00115         else: 
00116             self.start_angles_list = start_angles_list 
00117 
00118         if whicharm == 'left':
00119             for i in range(len(self.start_angles_list)):
00120                 for joint_ind in [0, 2, 4]:
00121                     self.start_angles_list[i][joint_ind] *= -1.
00122 
00123         #changes the set of ids used to show the arrows every other call
00124         self.pose_id_set = 0
00125 
00126         rospy.loginfo("ik_utilities: done init")
00127 
00128 
00129     #wait for the various services and run the IK query to get the joint names and limits
00130     def check_services_and_get_ik_info(self):
00131 
00132         rospy.loginfo("ik_utilities: waiting for IK services to be there")
00133         rospy.wait_for_service(self.srvroot+'get_ik')
00134         if self.perception_running:
00135             rospy.wait_for_service(self.srvroot+'get_constraint_aware_ik')
00136         rospy.wait_for_service(self.srvroot+'get_fk')
00137         rospy.wait_for_service(self.srvroot+'get_ik_solver_info')
00138         rospy.loginfo("ik_utilities: services found")
00139 
00140         #get and store the joint names and limits
00141         #only one IK link available so far ('r_wrist_roll_link' or 'l_wrist_roll_link')
00142         rospy.loginfo("getting the IK solver info")
00143         (self.joint_names, self.min_limits, self.max_limits, self.link_names) = \
00144             self.run_query()
00145         self.link_name = self.link_names[-1]
00146         rospy.loginfo("done getting the IK solver info")
00147 
00148 
00149     ##draw a PoseStamped in rviz as a set of arrows (x=red, y=green, z=blue)
00150     #id is the id number for the x-arrow (y is id+1, z is id+2)
00151     def draw_pose(self, pose_stamped, id):
00152         marker = Marker()
00153         marker.header = pose_stamped.header
00154         marker.ns = "basic_shapes"
00155         marker.type = 0 #arrow
00156         marker.action = 0 #add
00157         marker.scale.x = 0.01
00158         marker.scale.y = 0.02
00159         marker.color.a = 1.0
00160         marker.lifetime = rospy.Duration(30.0)
00161 
00162         orientation = pose_stamped.pose.orientation
00163         quat = [orientation.x, orientation.y, orientation.z, orientation.w]
00164         mat = tf.transformations.quaternion_matrix(quat)
00165         start = [pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z]
00166         x_end = list(mat[:,0][0:3]*.05 + numpy.array(start))
00167         y_end = list(mat[:,1][0:3]*.05 + numpy.array(start))
00168         z_end = list(mat[:,2][0:3]*.05 + numpy.array(start))
00169         #print "start: %5.3f, %5.3f, %5.3f"%(pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z)
00170         #print "x_end:", pplist(x_end)
00171         #print "y_end:", pplist(y_end)
00172         #print "z_end:", pplist(z_end)
00173         marker.id = id
00174         marker.points = [pose_stamped.pose.position, Point(*x_end)]
00175         marker.color.r = 1.0
00176         marker.color.g = 0.0
00177         marker.color.b = 0.0
00178         self.marker_pub.publish(marker)
00179         marker.id = id+1
00180         marker.points = [pose_stamped.pose.position, Point(*y_end)]
00181         marker.color.r = 0.0
00182         marker.color.g = 1.0
00183         marker.color.b = 0.0
00184         self.marker_pub.publish(marker)
00185         marker.id = id+2
00186         marker.points = [pose_stamped.pose.position, Point(*z_end)]
00187         marker.color.r = 0.0
00188         marker.color.g = 0.0
00189         marker.color.b = 1.0
00190         self.marker_pub.publish(marker)
00191 
00192 
00193     ##get the joint names and limits, and the possible link names for running IK
00194     def run_query(self):
00195         try:
00196             resp = self._query_service()
00197         except rospy.ServiceException, e:
00198             rospy.logerr("GetKinematicSolverInfo service call failed! error msg: %s"%e)
00199             return (None, None, None)
00200 
00201         min_limits = [limit.min_position for limit in resp.kinematic_solver_info.limits]
00202         max_limits = [limit.max_position for limit in resp.kinematic_solver_info.limits]
00203 #         rospy.loginfo("joint_names:" + str(resp.kinematic_solver_info.joint_names))
00204 #         rospy.loginfo("min_limits:" + pplist(min_limits))
00205 #         rospy.loginfo("max_limits:" + pplist(max_limits))
00206 #         rospy.loginfo("link names:" + str(resp.kinematic_solver_info.link_names))
00207         return (resp.kinematic_solver_info.joint_names, min_limits, max_limits, resp.kinematic_solver_info.link_names)
00208 
00209 
00210     ##run forward kinematics on a set of 7 joint angles 
00211     #link_name specifies the desired output frame
00212     #returns a PoseStamped
00213     def run_fk(self, angles, link_name):
00214         if link_name not in self.link_names:
00215             rospy.logerr("link name %s not possible!"%link_name)
00216             return None
00217         self.link_name = link_name
00218 
00219         header = rospy.Header()
00220         header.stamp = rospy.get_rostime()
00221         header.frame_id = 'base_link'
00222         joint_state = JointState(header, self.joint_names, angles, [], [])
00223         try:
00224             resp = self._fk_service(header, [link_name], RobotState(joint_state, MultiDOFJointState())) 
00225         except rospy.ServiceException, e:
00226             rospy.logerr("FK service call failed! error msg: %s"%e)
00227             return None
00228 
00229         #ik failed, print the error code
00230         if resp.error_code.val != 1:
00231             rospy.loginfo("FK error code: %s"%self.error_code_dict[resp.error_code.val])
00232 
00233         return resp.pose_stamped[0]
00234 
00235 
00236     ##run inverse kinematics on a PoseStamped (7-dof pose 
00237     #(position + quaternion orientation) + header specifying the 
00238     #frame of the pose)
00239     #tries to stay close to double start_angles[7]
00240     #returns the solution angles as double solution[7] 
00241     #link_name is the link frame to position at pose
00242     #if collision_aware is 1, runs ik_service_with_collision
00243     #ordered_collision_operations is a list of collision operations to feed to IK to modify the collision space
00244     def run_ik(self, pose_stamped, start_angles, link_name, collision_aware = 1, ordered_collision_operations = None, IK_robot_state = None, link_padding = None):
00245 
00246         if link_name not in self.link_names:
00247             rospy.logerr("link name %s not possible!"%link_name)
00248             return None
00249         self.link_name = link_name
00250 
00251         #print "request:\n", pose_stamped
00252 
00253         ik_request = PositionIKRequest()
00254         ik_request.ik_link_name = self.link_name
00255         ik_request.pose_stamped = pose_stamped
00256         ik_request.ik_seed_state.joint_state.header.stamp = rospy.get_rostime()
00257         ik_request.ik_seed_state.joint_state.position = start_angles
00258         ik_request.ik_seed_state.joint_state.name = self.joint_names
00259 
00260         if IK_robot_state:
00261             ik_request.robot_state = IK_robot_state
00262 
00263         try:
00264             if collision_aware and self.perception_running:
00265                 col_free_ik_request = GetConstraintAwarePositionIKRequest()
00266                 col_free_ik_request.ik_request = ik_request
00267                 col_free_ik_request.timeout = rospy.Duration(10.0) #timeout after 10 seconds
00268             
00269                 if ordered_collision_operations != None:
00270                     col_free_ik_request.ordered_collision_operations = ordered_collision_operations
00271                 if link_padding != None:
00272                     col_free_ik_request.link_padding = link_padding
00273                     
00274                 resp = self._ik_service_with_collision(col_free_ik_request)
00275             else:
00276                 resp = self._ik_service(ik_request, rospy.Duration(10.0))        
00277         except rospy.ServiceException, e:
00278             rospy.logwarn("IK service call failed! error msg: %s"%e)
00279             return (None, None)
00280         
00281         #ik failed, print the error code
00282         if resp.error_code.val != 1:
00283             rospy.loginfo("IK error code: %s"%self.error_code_dict[resp.error_code.val])
00284             #print "requested pose:\n", pose_stamped
00285         
00286         return (resp.solution.joint_state.position, self.error_code_dict[resp.error_code.val])
00287 
00288 
00289     ##check whether a set of joint angles is in collision with the environment
00290     #allows the same modifications as IK
00291     def check_state_validity(self, joint_angles, ordered_collision_operations = None, \
00292                                  robot_state = None, link_padding = None):
00293         
00294         req = GetStateValidityRequest()
00295         if robot_state != None:
00296             req.robot_state = robot_state
00297         req.robot_state.joint_state.name.extend(self.joint_names)
00298         req.robot_state.joint_state.position.extend(joint_angles)
00299         req.robot_state.joint_state.header.stamp = rospy.Time.now()
00300         req.check_collisions = True
00301 
00302         if ordered_collision_operations != None:
00303             req.ordered_collision_operations = ordered_collision_operations
00304         if link_padding != None:
00305             req.link_padding = link_padding
00306 
00307         try:
00308             res = self._check_state_validity_service(req)
00309         except rospy.ServiceException, e:
00310             rospy.logwarn("Check state validity call failed!  error msg: %s"%e)
00311             return 0
00312         if res.error_code.val == res.error_code.SUCCESS:
00313             return 1
00314         rospy.loginfo("Check state validity error code: %s"%self.error_code_dict[res.error_code.val])
00315         return 0
00316 
00317 
00318     ##convert a pointStamped to a pos list in a desired frame
00319     def point_stamped_to_list(self, point, frame):
00320 
00321         #convert the pointStamped to the desired frame, if necessary
00322         if point.header.frame_id != frame:
00323             try:
00324                 trans_point = self.tf_listener.transformPoint(frame, point)
00325             except rospy.ServiceException, e:
00326                 print "point:\n", point
00327                 print "frame:", frame
00328                 rospy.logerr("point_stamped_to_list: error in transforming point from " + point.header.frame_id + " to " + frame + "error msg: %s"%e)
00329                 return None
00330         else:
00331             trans_point = point
00332         
00333         #extract position as a list
00334         pos = [trans_point.point.x, trans_point.point.y, trans_point.point.z]
00335 
00336         return pos
00337 
00338 
00339     ##convert a Vector3Stamped to a rot list in a desired frame
00340     def vector3_stamped_to_list(self, vector3, frame):
00341 
00342         #convert the vector3Stamped to the desired frame, if necessary
00343         if vector3.header.frame_id != frame:
00344             try:
00345                 trans_vector3 = self.tf_listener.transformVector3(frame, vector3)
00346             except rospy.ServiceException, e:
00347                 print "vector3:\n", vector3
00348                 print "frame:", frame
00349                 rospy.logerr("vector3_stamped_to_list: error in transforming point from " + vector3.header.frame_id + " to " + frame + "error msg: %s"%e)
00350                 return None
00351         else:
00352             trans_vector3 = vector3
00353         
00354         #extract vect as a list
00355         vect = [trans_vector3.vector.x, trans_vector3.vector.y, trans_vector3.vector.z]
00356 
00357         return vect
00358 
00359 
00360     ##convert a QuaternionStamped to a quat list in a desired frame
00361     def quaternion_stamped_to_list(self, quaternion, frame):
00362 
00363         #convert the QuaternionStamped to the desired frame, if necessary
00364         if quaternion.header.frame_id != frame:
00365             try:
00366                 trans_quat = self.tf_listener.transformQuaternion(frame, quaternion)
00367             except rospy.ServiceException, e:
00368                 print "quaternion:\n", quaternion
00369                 print "frame:", frame
00370                 rospy.logerr("quaternion_stamped_to_list: error in transforming point from " + quaternion.header.frame_id + " to " + frame + "error msg: %s"%e)
00371                 return None
00372         else:
00373             trans_quat = quaternion
00374         
00375         #extract quat as a list
00376         quat = [trans_quat.quaternion.x, trans_quat.quaternion.y, trans_quat.quaternion.z, trans_quat.quaternion.w]
00377 
00378         return quat
00379 
00380 
00381     ##convert a poseStamped to pos and rot (quaternion) lists in a desired frame
00382     def pose_stamped_to_lists(self, pose, frame):
00383 
00384         #convert the poseStamped to the desired frame, if necessary
00385         if pose.header.frame_id != frame:
00386             pose.header.stamp = rospy.Time(0)
00387             self.tf_listener.waitForTransform(frame, pose.header.frame_id, pose.header.stamp, rospy.Duration(5))
00388             try:
00389                 trans_pose = self.tf_listener.transformPose(frame, pose)
00390             except rospy.ServiceException, e:
00391                 print "pose:\n", pose
00392                 print "frame:", frame
00393                 rospy.logerr("pose_stamped_to_lists: error in transforming pose from " + pose.header.frame_id + " to " + frame + "error msg: %s"%e)
00394                 return (None, None)
00395         else:
00396             trans_pose = pose
00397         
00398         #extract position and orientation as quaternion
00399         pos = [trans_pose.pose.position.x, trans_pose.pose.position.y, trans_pose.pose.position.z]
00400         rot = [trans_pose.pose.orientation.x, trans_pose.pose.orientation.y, \
00401                    trans_pose.pose.orientation.z, trans_pose.pose.orientation.w]
00402 
00403         return (pos, rot)
00404 
00405 
00406     ##convert pos and rot lists (relative to in_frame) to a poseStamped (relative to to_frame)
00407     def lists_to_pose_stamped(self, pos, rot, in_frame, to_frame):
00408         
00409         #stick lists in a poseStamped
00410         m = PoseStamped()
00411         m.header.frame_id = in_frame
00412         m.header.stamp = rospy.get_rostime()
00413         m.pose = Pose(Point(*pos), Quaternion(*rot))
00414         
00415         try:
00416             pose_stamped = self.tf_listener.transformPose(to_frame, m)
00417         except rospy.ServiceException, e:            
00418             rospy.logerr("error in transforming pose from " + in_frame + " to " + to_frame + "err msg: %s"%e)
00419             return None
00420         return pose_stamped
00421 
00422 
00423     ##vector norm of a list
00424     def vect_norm(self, vect):
00425         return sum([x**2 for x in vect])**.5
00426 
00427 
00428     ##normalize a vector
00429     def normalize_vect(self, vect):
00430         return list(numpy.array(vect)/self.vect_norm(vect))
00431 
00432 
00433     ##angle between two quaternions (as lists)
00434     def quat_angle(self, quat1, quat2):
00435         dot = sum([x*y for (x,y) in zip(quat1, quat2)])
00436         if dot > 1.:
00437             dot = 1.
00438         if dot < -1.:
00439             dot = -1.
00440         angle = 2*math.acos(math.fabs(dot))
00441         return angle
00442 
00443 
00444     ##interpolate a Cartesian path (expressed as pos and rot lists)
00445     #pos_spacing is max wrist translation in meters between trajectory points
00446     #rot_spacing is max wrist rotation in radians between trajectory points
00447     #num_steps overrides the number of steps (if != 0, ignore pos_spacing and rot_spacing)
00448     def interpolate_cartesian(self, start_pos, start_rot, end_pos, end_rot, pos_spacing, rot_spacing, num_steps = 0):
00449 
00450         #normalize quaternion rotations just in case
00451         norm_start_rot = self.normalize_vect(start_rot)
00452         norm_end_rot = self.normalize_vect(end_rot)
00453 
00454         #the desired wrist translation
00455         diff = [x-y for (x,y) in zip(end_pos, start_pos)]
00456 
00457         if num_steps == 0:
00458             #compute how far the wrist translates
00459             pos_move = self.vect_norm(diff)
00460 
00461             #compute how far the wrist rotates
00462             rot_move = self.quat_angle(norm_start_rot, norm_end_rot)
00463 
00464             #compute the number of steps to move no more than pos_spacing and rot_spacing in each step
00465             #(min 2, start and end)
00466             num_steps_pos = math.floor(pos_move/pos_spacing)+1
00467             num_steps_rot = math.floor(rot_move/rot_spacing)+1
00468             num_steps = int(max([num_steps_pos, num_steps_rot])+1)  
00469         
00470         #interpolate
00471         steps = []
00472         for stepind in range(num_steps):
00473             fraction = float(stepind)/(num_steps-1)  #add both start (0) and end (1)
00474             rot = list(tf.transformations.quaternion_slerp(norm_start_rot, norm_end_rot, fraction))
00475             pos = list(numpy.array(diff)*fraction + numpy.array(start_pos))
00476             steps.append((pos, rot))
00477             #print "fraction: %5.3f"%fraction, "pos:", pplist(pos), "rot:", pplist(rot)
00478 
00479         return steps
00480             
00481     
00482     ##check that all differences between angles1 and angles2 (lists of joint angles) are within consistent_range
00483     def check_consistent(self, angles1, angles2, consistent_range):
00484         diff = [math.fabs(x-y) for (x,y) in zip(angles1, angles2)]
00485         inconsistent = any([x>consistent_range for x in diff])
00486         return not inconsistent
00487 
00488 
00489     ##generate appropriate times and joint velocities for a joint path (such as that output by check_cartesian_path)
00490     #max_joint_vels is a list of maximum velocities to move the arm joints
00491     #max_joint_accs is a list of maximum accelerations to move the arm joints (can be ignored)
00492     #starts and ends in stop
00493     def trajectory_times_and_vels(self, joint_path, max_joint_vels = [.2]*7, max_joint_accs = [.5]*7):
00494 
00495         #min time for each segment
00496         min_segment_time = .01
00497         
00498         if not joint_path:
00499             rospy.logdebug("joint path was empty!")
00500             return([], [])
00501         traj_length = len(joint_path)
00502         num_joints = len(joint_path[0])
00503 
00504         #sanity-check max vels and accelerations
00505         if not max_joint_vels:
00506             max_joint_vels = [.2]*7
00507         elif len(max_joint_vels) != num_joints:
00508             rospy.logerr("invalid max_joint_vels!")
00509             return ([], [])
00510         if not max_joint_accs:
00511             max_joint_accs = [.5]*7
00512         elif len(max_joint_accs) != num_joints:
00513             rospy.logerr("invalid max_joint_accs!")
00514             return ([], [])
00515         for ind in range(num_joints):
00516             if max_joint_vels[ind] <= 0.:
00517                 max_joint_vels[ind] = .2
00518             if max_joint_accs[ind] <= 0.:
00519                 max_joint_accs[ind] = .5
00520             
00521         vels = [[None]*num_joints for i in range(traj_length)]
00522         
00523         #give the trajectory a bit of time to start
00524         segment_times = [None]*traj_length
00525         segment_times[0] = 0.05 
00526 
00527         #find vaguely appropriate segment times, assuming that we're traveling at max_joint_vels at the fastest joint
00528         for ind in range(traj_length-1):
00529             joint_diffs = [math.fabs(joint_path[ind+1][x]-joint_path[ind][x]) for x in range(num_joints)]
00530             joint_times = [diff/vel for (diff, vel) in zip(joint_diffs, max_joint_vels)]
00531             segment_times[ind+1] = max(joint_times+[min_segment_time])
00532             
00533         #set the initial and final velocities to 0 for all joints
00534         vels[0] = [0.]*num_joints
00535         vels[traj_length-1] = [0.]*num_joints
00536 
00537         #also set the velocity where any joint changes direction to be 0 for that joint
00538         #and otherwise use the average velocity (assuming piecewise-linear velocities for the segments before and after)
00539         for ind in range(1, traj_length-1):
00540             for joint in range(num_joints):
00541                 diff0 = joint_path[ind][joint]-joint_path[ind-1][joint]
00542                 diff1 = joint_path[ind+1][joint]-joint_path[ind][joint]
00543                 if (diff0>0 and diff1<0) or (diff0<0 and diff1>0):
00544                     vels[ind][joint] = 0.
00545                 else:
00546                     vel0 = diff0/segment_times[ind]
00547                     vel1 = diff1/segment_times[ind+1]
00548                     vels[ind][joint] = (vel0+vel1)/2.
00549 
00550         #increase the times if the desired velocities would require overly large accelerations
00551         for ind in range(1, traj_length):
00552             for joint in range(num_joints):
00553                 veldiff = math.fabs(vels[ind][joint]-vels[ind-1][joint])
00554                 acc = veldiff/segment_times[ind]
00555                 try:
00556                     if acc > max_joint_accs[joint]:
00557                         segment_times[ind] = veldiff/max_joint_accs[joint]
00558                 except:
00559                     pdb.set_trace()
00560 
00561         #turn the segment_times into waypoint times (cumulative)
00562         times = [None]*traj_length
00563         times[0] = segment_times[0]
00564         for ind in range(1, traj_length):
00565             try:
00566                 times[ind] = times[ind-1]+segment_times[ind]
00567             except:
00568                 pdb.set_trace()
00569 
00570         #return the times and velocities
00571         return (times, vels)
00572 
00573             
00574     ##check a Cartesian path for consistent, non-colliding IK solutions
00575     #start_pose and end_pose are PoseStamped messages with the wrist poses
00576     #start_angles are angles to try to stay close to
00577     #num_steps is the number of interpolation steps to use (if 0, use 
00578     #  pos_spacing and rot_spacing instead to calculate the number of steps)
00579     #pos_spacing is max wrist translation in meters between trajectory points 
00580     #rot_spacing is max wrist rotation in radians between trajectory points
00581     #consistent_angle is the max joint angle change before 2 steps are declared inconsistent
00582     #collision_check_resolution is the resolution at which to check collisions (0 or 1 for every)
00583     #steps_before_abort is the number of invalid steps found before aborting (-1 to ignore)
00584     #if collision_aware is 0, ignore collisions
00585     #ordered_collision_operations is an optional list of collision operations to feed to IK to modify the collision space
00586     #link_padding is an optional list of link paddings to feed to IK to modify the robot collision padding
00587     #IK_robot_state is an optional RobotState message to pass to IK
00588     #if start_from_end is 1, find an IK solution for the end first and work backwards
00589     #returns the joint angle trajectory and the error codes (0=good, 
00590     #  1=collisions, 2=inconsistent, 3=out of reach, 4=aborted before checking)
00591     def check_cartesian_path(self, start_pose, end_pose, start_angles, pos_spacing = 0.01, rot_spacing = 0.1, consistent_angle = math.pi/9., collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1, num_steps = 0, ordered_collision_operations = None, start_from_end = 0, IK_robot_state = None, link_padding = None, use_additional_start_angles = 0):
00592 
00593         #sanity-checking
00594         if num_steps != 0 and num_steps < 2:
00595             num_steps = 2
00596         if collision_check_resolution < 1:
00597             collision_check_resolution = 1
00598         if num_steps == 0 and (pos_spacing <= 0 or rot_spacing <= 0):
00599             rospy.logerr("invalid pos_spacing or rot_spacing")
00600             return ([], [])
00601 
00602         #convert to lists
00603         (start_pos, start_rot) = self.pose_stamped_to_lists(start_pose, 'base_link')
00604         (end_pos, end_rot) = self.pose_stamped_to_lists(end_pose, 'base_link')
00605         if start_pos == None or end_pos == None:
00606             return (None, None)
00607         
00608         #interpolate path
00609         steps = self.interpolate_cartesian(start_pos, start_rot, end_pos, end_rot, pos_spacing, rot_spacing, num_steps)
00610 
00611         #run collision-aware ik on each step, starting from the end and going backwards if start_from_end is true
00612         if start_from_end:
00613             steps.reverse()
00614 
00615         #use additional start angles from a pre-chosen set
00616         if use_additional_start_angles:
00617             num_to_use = max(use_additional_start_angles, len(self.start_angles_list))
00618             start_angles_list = [start_angles,] + self.start_angles_list[0:num_to_use]
00619         else:
00620             start_angles_list = [start_angles,]
00621 
00622         #go through each set of start angles, see if we can find a consistent trajectory
00623         for (start_angles_ind, start_angles) in enumerate(start_angles_list):
00624             trajectory = []
00625             error_codes = [] 
00626             
00627             if use_additional_start_angles:
00628                 rospy.loginfo("start_angles_ind: %d"%start_angles_ind)
00629 
00630             for stepind in range(len(steps)):
00631                 (pos,rot) = steps[stepind]
00632                 pose_stamped = self.lists_to_pose_stamped(pos, rot, 'base_link', 'base_link')
00633 
00634                 #draw the pose in rviz that we're checking in IK
00635                 self.draw_pose(pose_stamped, stepind*3+self.pose_id_set*50)
00636                 self.pose_id_set = (self.pose_id_set+1)%2
00637 
00638                 #check for a non-collision_aware IK solution first
00639                 (colliding_solution, error_code) = self.run_ik(pose_stamped, start_angles, self.link_name, collision_aware = 0, IK_robot_state = IK_robot_state)
00640                 if not colliding_solution:
00641                     rospy.loginfo("non-collision-aware IK solution not found for step %d!"%stepind)
00642                     trajectory.append([0.]*7)
00643                     error_codes.append(3)          #3=out of reach
00644 
00645                 else:
00646                     #if we're checking for collisions, then look for a collision-aware solution
00647                     collision_aware_this_step = collision_aware and (stepind % collision_check_resolution == 0 or stepind == len(steps)-1)
00648                     if collision_aware_this_step:
00649                         (solution, error_code) = self.run_ik(pose_stamped, start_angles, self.link_name, collision_aware, ordered_collision_operations, IK_robot_state = IK_robot_state, link_padding = link_padding)
00650                         if not solution:
00651                             rospy.loginfo("non-colliding IK solution not found for step %d!"%stepind)
00652                             collision_aware_solution_found = 0
00653                             solution = colliding_solution
00654                         else:
00655                             collision_aware_solution_found = 1
00656                     else:                
00657                         solution = colliding_solution
00658 
00659                     trajectory.append(list(solution))
00660 
00661                     #first trajectory point, or last point was all 0s, or consistent with previous point
00662                     if stepind == 0 or error_codes[-1] == 3 or self.check_consistent(trajectory[-2], solution, consistent_angle):
00663                         if not collision_aware_this_step or collision_aware_solution_found:
00664                             error_codes.append(0)  #0=good
00665                         else:
00666                             error_codes.append(1)  #1=collisions
00667                     else:
00668                         rospy.loginfo("IK solution not consistent for step %d!"%stepind)
00669                         error_codes.append(2)      #2=inconsistent
00670 
00671                     start_angles = solution
00672 
00673                 #check if we should abort due to finding too many invalid points
00674                 if error_codes[-1] > 0 and steps_before_abort >= 0 and stepind >= steps_before_abort:
00675                     rospy.loginfo("aborting due to too many invalid steps")
00676                     trajectory.extend([[0.]*7 for i in range(len(steps)-stepind-1)])
00677                     error_codes.extend([4]*(len(steps)-stepind-1)) #4=aborted before checking
00678                     break
00679 
00680             #if we didn't abort, stop and return the trajectory
00681             #if not any(error_codes):
00682             else:
00683                 break
00684 
00685         if start_from_end:
00686             trajectory.reverse()        
00687             error_codes.reverse()
00688         return (trajectory, error_codes)
00689 
00690 
00691 #test functions
00692 if __name__ == '__main__':
00693 
00694     def keypause():
00695         print "press enter to continue"
00696         raw_input()
00697 
00698     #run check_cartesian_path after converting lists to poseStamped
00699     def check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.01, \
00700                                        rot_spacing = 0.1, consistent_angle = math.pi/9., collision_aware = 1, \
00701                                        collision_check_resolution = 1, steps_before_abort = -1, num_steps = 0, frame = 'base_link'):
00702 
00703         print "approachpos:", pplist(approachpos), "  approachquat:", pplist(approachquat)
00704         print "grasppos:   ", pplist(grasppos), "  graspquat:   ", pplist(graspquat)
00705 
00706         start_pose = ik_utilities.lists_to_pose_stamped(approachpos, approachquat, frame, frame)
00707         end_pose = ik_utilities.lists_to_pose_stamped(grasppos, graspquat, frame, frame)
00708 
00709         (trajectory, error_codes) = ik_utilities.check_cartesian_path(start_pose, \
00710                       end_pose, start_angles, pos_spacing, rot_spacing, consistent_angle, collision_aware, collision_check_resolution, \
00711                       steps_before_abort, num_steps, use_additional_start_angles = 2)
00712         (times, vels) = ik_utilities.trajectory_times_and_vels(trajectory, [.2]*7, [.5]*7) 
00713 
00714         rospy.loginfo("trajectory:")
00715         for ind in range(len(trajectory)):
00716             rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + pplist(trajectory[ind]))
00717         rospy.loginfo("")
00718         for ind in range(len(trajectory)):
00719             rospy.loginfo("time: " + "%5.3f  "%times[ind] + "vels: " + pplist(vels[ind]))
00720 
00721         return (trajectory, error_codes)
00722 
00723 
00724     #test check_cartesian_path
00725     def run_cartesian_path_test(ik_utilities):
00726 
00727         #side grasp
00728         #start_angles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, 1.355]
00729         start_angles = [0]*7
00730         sideapproachmat = numpy.array([[0., -1., 0., 0.],  
00731                                        [1., 0., 0., 0.],
00732                                        [0., 0., 1., 0.],
00733                                        [0., 0., 0., 1.]])
00734         sideapproachpos = [.62, -.3, .75]
00735         approachmat = sideapproachmat
00736         approachpos = sideapproachpos
00737         approachquat = list(tf.transformations.quaternion_from_matrix(approachmat))
00738         sidegrasppos = sideapproachpos[:]
00739         sidegrasppos[1] += .05
00740         grasppos = sidegrasppos
00741         graspquat = approachquat[:]
00742         print "side grasp"
00743         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles)        
00744 
00745 
00746         #top to side grasp
00747         start_angles = [0.]*7
00748         approachpos = [.62, -.05, .85]
00749         approachquat = [-0.5, 0.5, 0.5, 0.5]  #from the top
00750         grasppos = [.62, -.05, .75]
00751         graspquat = [0.00000, 0.00000, 0.70711, 0.70711]  #from the side
00752         print "top to side grasp, collision-aware"
00753         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = .01, rot_spacing = .2)
00754 
00755         print "more finely spaced rotations, not collision-aware"
00756         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = .02, rot_spacing = .05, collision_aware = 0)
00757 
00758         print "5 steps, too far apart for default consistent_angle"
00759         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, num_steps = 5)
00760 
00761         print "5 steps, consistent_angle of pi/4"
00762         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, num_steps = 5, consistent_angle = math.pi/4)
00763 
00764         #top grasp through the table
00765         start_angles = [0.]*7
00766         approachpos = [.62, -.05, .65]
00767         approachquat = [-0.5, 0.5, 0.5, 0.5]  #from the top
00768         grasppos = [.62, -.05, .25]
00769         graspquat = approachquat[:]
00770         print "top grasp through the table"
00771         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 2, steps_before_abort = -1)
00772 
00773         print "abort early"
00774         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 2, steps_before_abort = 1)
00775 
00776         print "not collision-aware"
00777         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 0, steps_before_abort = -1)
00778 
00779         print "12 steps, collision-aware, collision_check_resolution of 3"
00780         check_cartesian_path_lists(ik_utilities, approachpos, approachquat, grasppos, graspquat, start_angles, collision_aware = 1, collision_check_resolution = 3, num_steps = 12)
00781 
00782 
00783     #test ik and fk using a randomly-generated joint pose
00784     def run_ik_and_fk_test(ik_utilities, collision_aware = 1):
00785 
00786         #check if two lists of numbers are near each other
00787         trans_near = 1e-4
00788         rot_near = 1e-4
00789         def not_near(list1, list2, near):
00790             return any([math.fabs(v1-v2) > near for (v1, v2) in zip(list1, list2)])        
00791 
00792         #random seed based on the current time
00793         random.seed()
00794 
00795         #generate a random joint-angle configuration
00796         random_angles = [random.uniform(min, max) for (min, max) in \
00797                              zip(ik_utilities.min_limits, ik_utilities.max_limits)]
00798 
00799         #run forward kinematics to get the corresponding Cartesian pose
00800         fk_pose = ik_utilities.run_fk(random_angles, ik_utilities.link_name)
00801         pos1 = fk_pose.pose.position
00802         rot1 = fk_pose.pose.orientation
00803         pos1list = [pos1.x, pos1.y, pos1.z]
00804         rot1list = [rot1.x, rot1.y, rot1.z, rot1.w]
00805 
00806         #run inverse kinematics on that pose to get a set of angles
00807         #that also make that Cartesian pose (starting from all 0 angles)
00808         rospy.loginfo("IK request: pos "+pplist(pos1list)+" rot "+pplist(rot1list))
00809         (ik_angles, error_code) = ik_utilities.run_ik(fk_pose, [0]*7, ik_utilities.link_name, collision_aware)
00810         if not ik_angles:
00811             
00812             #test check_state_validity to see if it is in agreement about random_angles being in collision
00813             if collision_aware:
00814                 valid = ik_utilities.check_state_validity(random_angles)
00815                 if valid:
00816                     rospy.logerr("check state validity thinks random_angles is not in collision!")
00817 
00818             return 0
00819 
00820         #run forward kinematics on the ik solution to see if the
00821         #resulting pose matches the first
00822         fk_pose2 = ik_utilities.run_fk(ik_angles, ik_utilities.link_name)
00823         pos2 = fk_pose2.pose.position
00824         rot2 = fk_pose2.pose.orientation
00825         pos2list = [pos2.x, pos2.y, pos2.z]
00826         rot2list = [rot2.x, rot2.y, rot2.z, rot2.w]
00827 
00828         #test check_state_validity to see if it is in agreement about ik_angles not being in collision
00829         if collision_aware:
00830             valid = ik_utilities.check_state_validity(ik_angles)
00831             if not valid:
00832                 rospy.logerr("check state validity thinks ik_angles is in collision!")
00833 
00834         #check the resulting solution 
00835         if not_near(pos1list, pos2list, trans_near) or \
00836            not_near(rot1list, rot2list, rot_near):
00837             rospy.loginfo("IK solution incorrect!")
00838             correct = 0
00839         else:
00840             rospy.loginfo("IK solution good")
00841             correct = 1
00842         rospy.loginfo("FK response:pos "+pplist(pos2list)+" rot "+pplist(rot2list))
00843         if not correct:
00844             return 0
00845 
00846         return 1
00847 
00848 
00849     def test_ik_with_specific_pose(ik_utilities):
00850         pose_stamped = ik_utilities.lists_to_pose_stamped([.62, -.05, .5], [.5, -.5, -.5, -.5], 'base_link', 'base_link')
00851         print "link name:", ik_utilities.link_name
00852         print "desired pose:\n", pose_stamped
00853         (solution, error_code) = ik_utilities.run_ik(pose_stamped, [0]*7, ik_utilities.link_name, 0)
00854         print "solution:", solution
00855 
00856 
00857     rospy.init_node('test_ik_and_fk', anonymous=True)
00858     
00859     ik_utilities = IKUtilities('right', wait_for_services = 0)
00860     ik_utilities.check_services_and_get_ik_info()
00861 
00862     print "testing ik and fk with random joint angles"
00863     for i in range(20):
00864        run_ik_and_fk_test(ik_utilities)
00865 
00866     print "testing the cartesian path interpolator"
00867     run_cartesian_path_test(ik_utilities)
00868 
00869     print "running a specific pose"
00870     test_ik_with_specific_pose(ik_utilities)
00871 
00872 
00873 
00874 
00875 


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