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
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091 import roslib; roslib.load_manifest('katana_interpolated_ik_motion_planner')
00092 import rospy
00093 import katana_interpolated_ik_motion_planner.ik_utilities as ik_utilities
00094 import math
00095 import sys
00096 import pdb
00097 from motion_planning_msgs.srv import GetMotionPlan, GetMotionPlanResponse
00098 from motion_planning_msgs.msg import ArmNavigationErrorCodes, RobotState
00099 from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Pose, Point, Quaternion
00100 from trajectory_msgs.msg import JointTrajectoryPoint
00101 from interpolated_ik_motion_planner.srv import SetInterpolatedIKMotionPlanParams, SetInterpolatedIKMotionPlanParamsResponse
00102 from sensor_msgs.msg import JointState
00103
00104
00105 class InterpolatedIKService:
00106
00107 def __init__(self):
00108
00109 rospy.loginfo("start iik node")
00110 self.node_name = rospy.get_param('~node_name', 'katana_interpolated_ik_motion_planner')
00111
00112 rospy.init_node(self.node_name)
00113
00114
00115
00116
00117 self.num_steps = rospy.get_param(self.node_name+'/num_steps', 6)
00118
00119
00120 self.consistent_angle = rospy.get_param(self.node_name+'/consistent_angle', math.pi/9)
00121
00122
00123 self.collision_check_resolution = rospy.get_param(self.node_name+'/collision_check_resolution', 1)
00124
00125
00126 self.steps_before_abort = rospy.get_param(self.node_name+'/steps_before_abort', 0)
00127
00128
00129 self.pos_spacing = rospy.get_param(self.node_name+'/pos_spacing', 0.01)
00130
00131
00132 self.rot_spacing = rospy.get_param(self.node_name+'/rot_spacing', .1)
00133
00134
00135 self.collision_aware = rospy.get_param(self.node_name+'/collision_aware', 1)
00136
00137
00138 self.start_from_end = rospy.get_param(self.node_name+'/start_from_end', 0)
00139
00140
00141 self.max_joint_vels = rospy.get_param(self.node_name+'/max_joint_vels', [.1]*5)
00142
00143
00144 self.max_joint_accs = rospy.get_param(self.node_name+'/max_joint_accs', [.25]*5)
00145
00146 self.ik_utils = ik_utilities.IKUtilities()
00147
00148
00149 s1 = rospy.Service('katana_interpolated_ik_motion_plan', \
00150 GetMotionPlan, self.interpolated_ik_motion_planner_callback)
00151
00152
00153 s2 = rospy.Service('katana_interpolated_ik_motion_plan_set_params', \
00154 SetInterpolatedIKMotionPlanParams, self.set_params_callback)
00155
00156
00157
00158 def add_header(self, msg, frame):
00159 msg.header.frame_id = frame
00160 msg.header.stamp = rospy.Time()
00161 return msg
00162
00163
00164
00165 def pplist(self, list):
00166 return ' '.join(['%2.3f'%x for x in list])
00167
00168
00169
00170 def set_params_callback(self, req):
00171 self.num_steps = req.num_steps
00172 self.consistent_angle = req.consistent_angle
00173 self.collision_check_resolution= req.collision_check_resolution
00174 self.steps_before_abort = req.steps_before_abort
00175 self.pos_spacing = req.pos_spacing
00176 self.rot_spacing = req.rot_spacing
00177 self.collision_aware = req.collision_aware
00178 self.start_from_end = req.start_from_end
00179 self.max_joint_vels = req.max_joint_vels
00180 self.max_joint_accs = req.max_joint_accs
00181 return SetInterpolatedIKMotionPlanParamsResponse()
00182
00183
00184
00185 def interpolated_ik_motion_planner_callback(self, req):
00186
00187 print 'calling IIK:'
00188
00189 joint_names = req.motion_plan_request.start_state.joint_state.name
00190 start_angles = req.motion_plan_request.start_state.joint_state.position
00191
00192
00193 if start_angles and len(joint_names) != len(start_angles):
00194 rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting")
00195 return 0
00196
00197
00198 reordered_start_angles = []
00199
00200
00201 joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0)
00202 if not joint_states_msg:
00203 rospy.logerr("unable to get joint_states message")
00204 return 0
00205
00206
00207
00208 for joint_name in self.ik_utils.joint_names:
00209
00210
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
00215
00216 elif joint_name in joint_states_msg.name:
00217 index = joint_states_msg.name.index(joint_name)
00218 current_position = joint_states_msg.position[index]
00219 reordered_start_angles.append(current_position)
00220
00221
00222 else:
00223 rospy.logerr("an expected arm joint,"+joint_name+"was not found!")
00224 return 0
00225
00226
00227 additional_joint_angles = []
00228 additional_joint_names = []
00229 for (ind, joint_name) in enumerate(joint_names):
00230 if joint_name not in self.ik_utils.joint_names:
00231 rospy.loginfo("found %s"%joint_name)
00232 additional_joint_angles.append(start_angles[ind])
00233 additional_joint_names.append(joint_name)
00234 IK_robot_state = None
00235 if additional_joint_angles:
00236 rospy.loginfo("adding additional start angles for:"+str(additional_joint_names))
00237 rospy.loginfo("additional joint angles:"+str(additional_joint_angles))
00238 IK_robot_state = RobotState()
00239 IK_robot_state.joint_state.name = additional_joint_names
00240 IK_robot_state.joint_state.position = additional_joint_angles
00241
00242
00243 link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0]
00244 if link_name != self.ik_utils.link_name:
00245 rospy.logerr("link_name not allowed: %s"%link_name)
00246 return 0
00247
00248
00249 start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0]
00250
00251 frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0]
00252
00253
00254 start_pose_stamped = self.add_header(PoseStamped(), frame_id)
00255 start_pose_stamped.pose = start_pose
00256
00257
00258 goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position
00259
00260
00261 goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id
00262
00263
00264 goal_ps = self.add_header(PointStamped(), goal_pos_frame)
00265 goal_ps.point = goal_pos
00266 goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, self.ik_utils.base_link)
00267
00268
00269 goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation
00270
00271
00272 goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id
00273
00274
00275 goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
00276 goal_qs.quaternion = goal_quat
00277 goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, self.ik_utils.base_link)
00278
00279
00280 goal_pose_stamped = self.add_header(PoseStamped(), self.ik_utils.base_link)
00281 goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list))
00282
00283
00284 ordered_collision_operations = req.motion_plan_request.ordered_collision_operations
00285 if ordered_collision_operations.collision_operations == []:
00286 ordered_collision_operations = None
00287
00288
00289 link_padding = req.motion_plan_request.link_padding
00290 if link_padding == []:
00291 link_padding = None
00292
00293 print 'start pose'
00294 print start_pose_stamped
00295 print 'goal pose'
00296 print goal_pose_stamped
00297
00298
00299 (trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
00300 goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
00301 self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
00302 self.steps_before_abort, self.num_steps, ordered_collision_operations, \
00303 self.start_from_end, IK_robot_state, link_padding)
00304
00305
00306
00307 start_ind = 0
00308 stop_ind = len(error_codes)
00309 if self.start_from_end:
00310 for ind in range(len(error_codes)-1, 0, -1):
00311 if error_codes[ind]:
00312 start_ind = ind+1
00313 break
00314 else:
00315 for ind in range(len(error_codes)):
00316 if error_codes[ind]:
00317 stop_ind = ind
00318 break
00319 (times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs)
00320 times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind)
00321 vels = [[0]*5]*start_ind + vels + [[0]*5]*(len(error_codes)-stop_ind)
00322
00323 rospy.logdebug("trajectory:")
00324 for ind in range(len(trajectory)):
00325 rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
00326 rospy.logdebug("")
00327 for ind in range(len(trajectory)):
00328 rospy.logdebug("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
00329
00330
00331 res = GetMotionPlanResponse()
00332
00333
00334 res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]
00335
00336
00337
00338 res.trajectory.joint_trajectory.points = []
00339 for i in range(len(trajectory)):
00340 joint_trajectory_point = JointTrajectoryPoint()
00341 joint_trajectory_point.positions = trajectory[i]
00342 joint_trajectory_point.velocities = vels[i]
00343 joint_trajectory_point.time_from_start = rospy.Duration(times[i])
00344 res.trajectory.joint_trajectory.points.append(joint_trajectory_point)
00345
00346
00347
00348
00349
00350
00351
00352 error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
00353 2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
00354 4:ArmNavigationErrorCodes.PLANNING_FAILED}
00355
00356 trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes]
00357 res.trajectory_error_codes = trajectory_error_codes
00358
00359
00360
00361
00362
00363
00364
00365 print res
00366 return res
00367
00368
00369 if __name__ == "__main__":
00370
00371 interpolated_ik_service = InterpolatedIKService()
00372 rospy.loginfo("Ready to serve interpolated IK motion plan requests.")
00373
00374 rospy.spin()