00001
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
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
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.poses.append(start_pose.pose)
00048 req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append('r_wrist_roll_link')
00049 req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(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
00061
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
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
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
00120
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
00139 start_angles = [0.]*7
00140 approachpos = [.72, -.05, .11]
00141 approachquat = [-0.5, 0.5, 0.5, 0.5]
00142 grasppos = [.67, -.05, .01]
00143 graspquat = [0.00000, 0.00000, 0.70711, 0.70711]
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
00158 start_angles = [0.]*7
00159 approachpos = [.67, -.05, .01]
00160 approachquat = [-0.5, 0.5, 0.5, 0.5]
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
00167
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