$search
00001 #!/usr/bin/python 00002 '''test client for interpolated_ik_motion_planner''' 00003 00004 import roslib 00005 roslib.load_manifest('interpolated_ik_motion_planner') 00006 import rospy 00007 from arm_navigation_msgs.srv import GetMotionPlanRequest, GetMotionPlanResponse, GetMotionPlan 00008 from arm_navigation_msgs.msg import PositionConstraint, OrientationConstraint, ArmNavigationErrorCodes, OrderedCollisionOperations, CollisionOperation 00009 from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Pose, Point, Quaternion 00010 from interpolated_ik_motion_planner.srv import SetInterpolatedIKMotionPlanParams 00011 import math 00012 import tf 00013 import numpy 00014 import pdb 00015 00016 00017 #pretty-print list to string 00018 def pplist(list): 00019 return ' '.join(['%5.3f'%x for x in list]) 00020 00021 00022 def keypause(): 00023 print "press enter to continue" 00024 raw_input() 00025 00026 00027 #call the service 00028 def call_get_interpolated_ik_motion_plan(start_pose, goal_pose, start_angles, joint_names, pos_spacing = 0.01, \ 00029 rot_spacing = 0.1, consistent_angle = math.pi/9, collision_aware = 1, \ 00030 collision_check_resolution = 1, steps_before_abort = -1, num_steps = 0, \ 00031 ordered_collision_operations = None, frame = 'torso_lift_link', start_from_end = 0, \ 00032 max_joint_vels = [.2]*7, max_joint_accs = [.5]*7): 00033 print "waiting for service" 00034 rospy.wait_for_service("r_interpolated_ik_motion_plan") 00035 print "service found" 00036 00037 try: 00038 serv = rospy.ServiceProxy("r_interpolated_ik_motion_plan_set_params", SetInterpolatedIKMotionPlanParams) 00039 res = serv(num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs) 00040 except rospy.ServiceException, e: 00041 print "error when calling r_interpolated_ik_motion_plan_set_params: %s"%e 00042 return 0 00043 00044 req = GetMotionPlanRequest() 00045 req.motion_plan_request.start_state.joint_state.name = joint_names 00046 req.motion_plan_request.start_state.joint_state.position = start_angles 00047 req.motion_plan_request.start_state.multi_dof_joint_state.pose = start_pose.pose 00048 req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_id = 'r_wrist_roll_link' 00049 req.motion_plan_request.start_state.multi_dof_joint_state.frame_id = start_pose.header.frame_id 00050 pos_constraint = PositionConstraint() 00051 pos_constraint.position = goal_pose.pose.position 00052 pos_constraint.header.frame_id = goal_pose.header.frame_id 00053 req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint,] 00054 00055 orient_constraint = OrientationConstraint() 00056 orient_constraint.orientation = goal_pose.pose.orientation 00057 orient_constraint.header.frame_id = goal_pose.header.frame_id 00058 req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint,] 00059 00060 if ordered_collision_operations != None: 00061 req.motion_plan_request.ordered_collision_operations = ordered_collision_operations 00062 00063 try: 00064 serv = rospy.ServiceProxy("r_interpolated_ik_motion_plan", GetMotionPlan) 00065 res = serv(req) 00066 except rospy.ServiceException, e: 00067 print "error when calling r_interpolated_ik_motion_plan: %s"%e 00068 return 0 00069 00070 error_code_dict = {ArmNavigationErrorCodes.SUCCESS:0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED:1, \ 00071 ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED:2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED:3, \ 00072 ArmNavigationErrorCodes.PLANNING_FAILED:4} 00073 00074 00075 trajectory_len = len(res.trajectory.joint_trajectory.points) 00076 trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)] 00077 vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)] 00078 times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)] 00079 error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes] 00080 00081 rospy.loginfo("trajectory:") 00082 for ind in range(len(trajectory)): 00083 rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + pplist(trajectory[ind])) 00084 rospy.loginfo("") 00085 for ind in range(len(trajectory)): 00086 rospy.loginfo("time: " + "%5.3f "%times[ind].to_sec() + "vels: " + pplist(vels[ind])) 00087 00088 00089 #create a PoseStamped message 00090 def create_pose_stamped(pose, frame_id = 'torso_lift_link'): 00091 m = PoseStamped() 00092 m.header.frame_id = frame_id 00093 m.header.stamp = rospy.Time() 00094 m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7])) 00095 return m 00096 00097 00098 #convert to PoseStamped then call the service 00099 def check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.01, \ 00100 rot_spacing = 0.1, consistent_angle = math.pi/9., collision_aware = 1, \ 00101 collision_check_resolution = 1, steps_before_abort = -1, num_steps = 0, \ 00102 ordered_collision_operations = None, frame = 'torso_lift_link'): 00103 joint_names = ["r_shoulder_pan_joint", 00104 "r_shoulder_lift_joint", 00105 "r_upper_arm_roll_joint", 00106 "r_elbow_flex_joint", 00107 "r_forearm_roll_joint", 00108 "r_wrist_flex_joint", 00109 "r_wrist_roll_joint"] 00110 start_pose = create_pose_stamped(approachpos+approachquat) 00111 goal_pose = create_pose_stamped(grasppos+graspquat) 00112 call_get_interpolated_ik_motion_plan(start_pose, goal_pose, start_angles, joint_names, pos_spacing, rot_spacing, \ 00113 consistent_angle, collision_aware, collision_check_resolution, \ 00114 steps_before_abort, num_steps, ordered_collision_operations) 00115 00116 00117 if __name__ == "__main__": 00118 00119 #side grasp 00120 #start_angles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, 1.355] 00121 start_angles = [0]*7 00122 sideapproachmat = numpy.array([[0., -1., 0., 0.], 00123 [1., 0., 0., 0.], 00124 [0., 0., 1., 0.], 00125 [0., 0., 0., 1.]]) 00126 sideapproachpos = [.67, -.3, .01] 00127 approachmat = sideapproachmat 00128 approachpos = sideapproachpos 00129 approachquat = list(tf.transformations.quaternion_from_matrix(approachmat)) 00130 sidegrasppos = sideapproachpos[:] 00131 sidegrasppos[1] += .05 00132 grasppos = sidegrasppos 00133 graspquat = approachquat[:] 00134 print "side grasp" 00135 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles) 00136 00137 00138 #top to side grasp 00139 start_angles = [0.]*7 00140 approachpos = [.72, -.05, .11] 00141 approachquat = [-0.5, 0.5, 0.5, 0.5] #from the top 00142 grasppos = [.67, -.05, .01] 00143 graspquat = [0.00000, 0.00000, 0.70711, 0.70711] #from the side 00144 print "top to side grasp, collision-aware" 00145 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = .01, rot_spacing = .2) 00146 00147 print "more finely spaced rotations, not collision-aware" 00148 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = .02, rot_spacing = .05, collision_aware = 0) 00149 00150 print "3 steps, too far apart for default consistent_angle" 00151 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, num_steps = 3) 00152 00153 print "3 steps, consistent_angle of pi/4" 00154 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, num_steps = 3, consistent_angle = math.pi/4) 00155 00156 00157 #top grasp through the table (if it's there) 00158 start_angles = [0.]*7 00159 approachpos = [.67, -.05, .01] 00160 approachquat = [-0.5, 0.5, 0.5, 0.5] #from the top 00161 grasppos = [.67, -.05, -.5] 00162 graspquat = approachquat[:] 00163 print "top grasp through the table" 00164 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1) 00165 00166 print "using current start angles" 00167 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles = [], pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1) 00168 00169 print "ignoring collisions with all collision points" 00170 collision_oper = CollisionOperation(object1 = "points", \ 00171 object2 = CollisionOperation.COLLISION_SET_ALL, \ 00172 operation = CollisionOperation.DISABLE) 00173 ordered_collision_operations = OrderedCollisionOperations([collision_oper,]) 00174 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1, ordered_collision_operations = ordered_collision_operations) 00175 00176 print "abort early" 00177 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = 1) 00178 00179 print "not collision-aware" 00180 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing = 0.02, collision_aware = 0, steps_before_abort = -1) 00181 00182 print "12 steps, collision-aware, collision_check_resolution of 3" 00183 check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles, collision_aware = 1, collision_check_resolution = 3, num_steps = 12) 00184 00185 00186