$search
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 # modified to suit the Katana's needs: Henning Deeken 00036 00037 ## @package interpolated_ik_motion_planner 00038 # Server to provide interpolated IK motion plans 00039 # relevant params: 00040 # num_steps: the number of steps to use when interpolating including start and finish (0 means use pos_spacing and 00041 # rot_spacing to calculate the number of steps, anything ==1 or <0 will become 2, start and finish) 00042 # consistent_angle: the max angle distance (in any joint) before we declare that the path is inconsistent 00043 # collision_check_resolution: how many steps between collision checks (0 or 1 is check every step; 2 is every other, etc.) 00044 # steps_before_abort: the number of steps in the plan (starting from the end and going backwards) that can be invalid 00045 # due to collisions or inconsistency before aborting (0 is abort as soon as you find one, 1 is allow one and still 00046 # continue, -1 or >=num_steps to never abort early) 00047 # pos_spacing: the max translation (m) to move the wrist between waypoints (only used if num_steps is 0) 00048 # rot_spacing: the max rotation (rad) to move the wrist between waypoints (only used if num_steps is 0) 00049 # collision_aware: if this is 0, collisions won't be checked for (returns non-collision aware IK solutions) 00050 # start_from_end: if this is 1, the planner searches for an IK solution for the end first, then works backwards from there 00051 # max_joint_vels: a list of maximum joint velocities to use when computing times and velocities for the joint trajectory (defaults to [.2]*7 if left empty) 00052 # max_joint_accs: a list of maximum accelerations to use when computing times and velocities for the joint trajectory (defaults to [.5]*7 if left empty) 00053 00054 # Parameters can be changed by calling the r_interpolated_ik_motion_plan_set_params service (or l_inter...) 00055 00056 00057 # The main service message is GetMotionPlan.srv, in motion_planning_msgs. Parts that were hijacked 00058 # for the relevant inputs and outputs: 00059 00060 # inputs: 00061 # 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) 00062 # 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) 00063 00064 # req.motion_plan_request.start_state.multi_dof_joint_state.pose: the start pose for the r_wrist_roll_link 00065 # 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') 00066 # req.motion_plan_request.start_state.multi_dof_joint_state.frame_id: the frame for the start pose 00067 00068 # req.motion_plan_request.goal_constraints.position_constraints[0].position: the (x,y,z) goal position of the same link as the start pose 00069 # req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id: the frame that goal position is in 00070 # 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 00071 # req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id: the frame that goal orientation is in 00072 00073 # req.motion_plan_request.workspace_parameters.ordered_collision_operations: the list of collision operations to modify the collision space 00074 # req.motion_plan_request.link_padding: links and collision-space padding to change from the default dynamically 00075 00076 # outputs: 00077 # res.trajectory.joint_trajectory.joint_names: the arm joint names in the normal order, as spit out by IKQuery 00078 # 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), 00079 # 'velocities' fields are reasonable joint velocities, and 'time_from_start' fields are reasonable times to get to each waypoint. ('accelerations' will be empty.) 00080 # res.trajectory_error_codes: a list of ArmNavigationErrorCodes messages the same length as the trajectory, 00081 # with values for each trajectory point as follows: 00082 # ArmNavigationErrorCodes.SUCCESS (1): no problem 00083 # ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided) 00084 # ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point 00085 # ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution) 00086 # ArmNavigationErrorCodes.PLANNING_FAILED (-1): aborted before getting to this point 00087 00088 # Everything else is completely ignored. 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 # class to provide the interpolated ik motion planner service 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 #get parameters from the parameter server 00115 00116 #the number of steps to use when interpolating (including start and finish) 00117 self.num_steps = rospy.get_param(self.node_name+'/num_steps', 6) 00118 00119 #the max angle distance (in any joint) before we declare that the path is inconsistent 00120 self.consistent_angle = rospy.get_param(self.node_name+'/consistent_angle', math.pi/9) 00121 00122 #how many steps between collision checks (0 or 1 is check every step; 2 is every other, etc.) 00123 self.collision_check_resolution = rospy.get_param(self.node_name+'/collision_check_resolution', 1) 00124 00125 #how many steps in the plan can be invalid before aborting 00126 self.steps_before_abort = rospy.get_param(self.node_name+'/steps_before_abort', 0) 00127 00128 #the max translation (m) to move the wrist between waypoints (only used if num_steps is 0) 00129 self.pos_spacing = rospy.get_param(self.node_name+'/pos_spacing', 0.01) 00130 00131 #the max rotation (rad) to move the wrist between waypoints (only used if num_steps is 0) 00132 self.rot_spacing = rospy.get_param(self.node_name+'/rot_spacing', .1) 00133 00134 #whether to check for collisions or not 00135 self.collision_aware = rospy.get_param(self.node_name+'/collision_aware', 1) 00136 00137 #if 1, starts the IK calculation from the end and works backwards (if 0, starts from the start) 00138 self.start_from_end = rospy.get_param(self.node_name+'/start_from_end', 0) 00139 00140 #max joint velocities to use when calculating times and vels for the trajectory 00141 self.max_joint_vels = rospy.get_param(self.node_name+'/max_joint_vels', [.1]*5) 00142 00143 #max joint accelerations to use when calculating times and vels for the trajectory 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 #advertise interpolated IK service 00149 s1 = rospy.Service('katana_interpolated_ik_motion_plan', \ 00150 GetMotionPlan, self.interpolated_ik_motion_planner_callback) 00151 00152 #advertise param changing service 00153 s2 = rospy.Service('katana_interpolated_ik_motion_plan_set_params', \ 00154 SetInterpolatedIKMotionPlanParams, self.set_params_callback) 00155 00156 00157 ##add a header to a message with a 0 timestamp (good for getting the latest TF transform) 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 ##pretty-print list to string 00165 def pplist(self, list): 00166 return ' '.join(['%2.3f'%x for x in list]) 00167 00168 00169 ##callback for the set_params service 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 ##callback for get_interpolated_ik_motion_plan service 00185 def interpolated_ik_motion_planner_callback(self, req): 00186 00187 print 'calling IIK:' 00188 #names and angles for the joints in their desired order 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 #sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified 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 #reorder the start angles to the order needed by IK 00198 reordered_start_angles = [] 00199 00200 #get the current joint states for the robot 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 #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 00215 #desired start angle not specified, use the current angle 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 #malformed joint_states message? 00222 else: 00223 rospy.logerr("an expected arm joint,"+joint_name+"was not found!") 00224 return 0 00225 00226 #get additional desired joint angles (such as for the gripper) to pass through to IK 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 #check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now) 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 #the start pose for that link 00249 start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0] 00250 #the frame that start pose is in 00251 frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0] 00252 00253 #turn it into a PoseStamped 00254 start_pose_stamped = self.add_header(PoseStamped(), frame_id) 00255 start_pose_stamped.pose = start_pose 00256 00257 #the desired goal position 00258 goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position 00259 00260 #the frame that goal position is in 00261 goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id 00262 00263 #convert the position to base_link frame 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 #the desired goal orientation 00269 goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation 00270 00271 #the frame that goal orientation is in 00272 goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id 00273 00274 #convert the quaternion to base_link frame 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 #assemble the goal pose into a PoseStamped 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 #get the ordered collision operations, if there are any 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 #get the link paddings, if there are any 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 #RUN! Check the Cartesian path for consistent, non-colliding IK solutions 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 #find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0) 00306 #if we're searching from the end, keep the end; if we're searching from the start, keep the start 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 #the response 00331 res = GetMotionPlanResponse() 00332 00333 #the arm joint names in the normal order, as spit out by IKQuery 00334 res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:] 00335 00336 #a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal 00337 #(all 0s if there was no IK solution for a point on the path) 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 #a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows: 00347 #ArmNavigationErrorCodes.SUCCESS (1): no problem 00348 #ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided) 00349 #ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point 00350 #ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution) 00351 #ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point 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 # rospy.loginfo("trajectory:") 00360 # for ind in range(len(trajectory)): 00361 # rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) 00362 # rospy.loginfo("") 00363 # for ind in range(len(trajectory)): 00364 # rospy.loginfo("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind])) 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()