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