circle.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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     # trajectory in cartesian space
00032     time = 5.0; # trajectory time in sec
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     # creating trajectory in joint space
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         # make the call
00057         try:
00058             # print "send request :" #, req
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 )


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:03