00001
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
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
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
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
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
00111
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
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141 approachpos = [0.310897399268, -0.0657663117783, 0.00387362057751]
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
00150 start_angles = [0.]*5
00151 approachpos = [.72, -.05, .11]
00152 approachquat = [-0.5, 0.5, 0.5, 0.5]
00153 grasppos = [.67, -.05, .01]
00154 graspquat = [0.00000, 0.00000, 0.70711, 0.70711]
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168 start_angles = [0.]*5
00169 approachpos = [.67, -.05, .01]
00170 approachquat = [-0.5, 0.5, 0.5, 0.5]
00171 grasppos = [.67, -.05, -.5]
00172 graspquat = approachquat[:]
00173
00174
00175
00176
00177
00178
00179
00180 collision_oper = CollisionOperation(object1 = "points", \
00181 object2 = CollisionOperation.COLLISION_SET_ALL, \
00182 operation = CollisionOperation.DISABLE)
00183 ordered_collision_operations = OrderedCollisionOperations([collision_oper,])
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193