katana_interpolated_ik_motion_planner: test_interpolated_ik_motion

Go to the documentation of this file.
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)