Go to the documentation of this file.00001
00002
00003 import roslib
00004 roslib.load_manifest( 'rospy' )
00005 roslib.load_manifest( 'rospy' )
00006 roslib.load_manifest( 'kinematics_msgs' )
00007 roslib.load_manifest( 'geometry_msgs' )
00008
00009 import rospy
00010 from kinematics_msgs.srv import \
00011 GetPositionIK, GetPositionIKRequest
00012 from geometry_msgs.msg import \
00013 Pose, Point, Quaternion
00014
00015 import numpy as np
00016 import dynamics_utils as dynutils
00017
00018 if __name__== '__main__':
00019 node_name = "circle_node"
00020 rospy.loginfo( 'register node %s ...'%node_name )
00021 rospy.init_node( node_name )
00022 rospy.loginfo( 'node %s up and running!'%node_name )
00023
00024 joint_names = dynutils.get_joint_names( 'l_arm_controller' )
00025 ik_service_name = 'pr2_left_arm_kinematics/get_ik'
00026 rospy.loginfo( 'wait for service %s ...'%ik_service_name )
00027 rospy.wait_for_service( ik_service_name )
00028 ik_service = rospy.ServiceProxy( ik_service_name, GetPositionIK )
00029 rospy.loginfo( 'ik service %s is up and running!'%ik_service_name )
00030
00031
00032 time = 5.0;
00033 n = int(time * 200)
00034 dt = time / n;
00035 alphas = np.linspace( 0, 2 * np.pi, n + 1 )
00036 alphas = alphas[0:-1]
00037 ys = np.cos( alphas ) * 0.2
00038 zs = -np.sin( alphas ) * 0.2
00039
00040
00041 rospy.loginfo( 'creating trajectory ...' )
00042 trajectory = []
00043 req = GetPositionIKRequest()
00044 req.timeout = rospy.Duration(5.0)
00045 req.ik_request.ik_link_name = "l_wrist_roll_link"
00046 req.ik_request.pose_stamped.header.frame_id = "torso_lift_link"
00047 req.ik_request.ik_seed_state.joint_state.name = joint_names
00048 req.ik_request.ik_seed_state.joint_state.position = [0.61309537, 0.45494851, 0.03, -1.03480809, 2.23232079, -0.79696399, -2.44271129]
00049 joint_positions = []
00050
00051 for (y, z) in zip( ys, zs ):
00052 pose = Pose( position = Point( 0.6 + z/2.0, 0.20 + y, 0.0 + z ),
00053 orientation = Quaternion( 0.0, 0.0, 0.0, 1.0 ) )
00054 req.ik_request.pose_stamped.pose = pose
00055
00056
00057 try:
00058
00059 res = ik_service( req )
00060
00061 if res.error_code.val == res.error_code.SUCCESS:
00062 joint_positions = np.asarray( res.solution.joint_state.position )
00063 else:
00064 print "IK failed, error code : ", res.error_code.val
00065 break
00066 except rospy.ServiceException, e:
00067 print "service call failed: %s"%e
00068 break
00069
00070 trajectory.append( joint_positions )
00071 req.ik_request.ik_seed_state.joint_state.position = joint_positions
00072
00073 rospy.loginfo( 'done!' )
00074
00075 t = np.cumsum( [dt] * n ) - dt
00076 pos = np.asarray( trajectory )
00077 dynutils.wrap_trajectory( pos )
00078
00079 vel = dynutils.compute_derivative( pos, dt )
00080 acc = dynutils.compute_derivative( vel, dt )
00081
00082 dynutils.save_motion( 'circle.pkl', 'circle', t, pos, vel, acc )