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