sinoid.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 from scipy import pi, sin, cos, array, asarray, linspace, zeros, ones, sqrt
00004 import matplotlib.pyplot as plt  
00005 
00006 import roslib
00007 roslib.load_manifest( 'pr2_controllers_msgs' )
00008 roslib.load_manifest( 'trajectory_msgs' )
00009 roslib.load_manifest( 'actionlib' )
00010 import rospy
00011 from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
00012 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00013 import actionlib
00014 
00015 
00016 def main():
00017     joint_names = ['l_shoulder_pan_joint',
00018                    'l_shoulder_lift_joint',
00019                    'l_upper_arm_roll_joint',
00020                    'l_elbow_flex_joint',
00021                    'l_forearm_roll_joint',
00022                    'l_wrist_flex_joint',
00023                    'l_wrist_roll_joint']
00024 
00025     N = len( joint_names )
00026 
00027     # lower_limit = asarray( [-2.13, -0.35, -3.75, -2.32, -2.0 * pi, -2.09, -2.0 * pi ] )
00028     # upper_limit = asarray( [ 0.56, 
00029     #                          0.8, #1.29, 
00030     #                          0.65,  0.0,   2.0 * pi,   0.0,  2.0 * pi ] )
00031 
00032     lower_limit = asarray( [2.13,
00033                             -0.35,
00034                             -0.5, #3.75,
00035                             -2.0, #-2.32,
00036                             -2.0 * pi,
00037                             -2.09,
00038                             -2.0 * pi ] )
00039     upper_limit = asarray( [-0.56, 
00040                              0.6, #1.29, 
00041                              2, #0.65,
00042                              0.0,
00043                              2.0 * pi,
00044                              0.0,
00045                              2.0 * pi ] )
00046 
00047     A =  (lower_limit - upper_limit) * 1/4 
00048 
00049     f = 0.1
00050 
00051     f1 = asarray([f * sqrt(2), 
00052                   f * sqrt(2), 
00053                   f * sqrt(2), 
00054                   f * sqrt(9),  
00055                   f * sqrt(7),
00056                   f * sqrt(1),
00057                   f * sqrt(7)])
00058     
00059     f2 = asarray([0.4, 
00060                   0.4, 
00061                   0.6, 
00062                   0.9, 
00063                   1.1, 
00064                   0.3, 
00065                   0.9])
00066 
00067     start_time = 0.0
00068     dt = 0.001
00069     max_time = 60.0
00070     t = linspace( 0.0, max_time - dt, int( max_time / dt ) )
00071     
00072     q = zeros( (N, len( t )))
00073     dq = zeros( (N, len( t )))
00074     ddq = zeros( (N, len( t )))
00075 
00076     # start_pos = [ 0, 0, 0, 0, 0, 0, 0 ]
00077     # joint = 2;
00078 
00079     for i in range(N):
00080         #if i == joint:
00081             q[i,:] = A[i] * sin( 2.0 * pi * f1[i] * t ) \
00082                 + A[i]/3.0 * sin( 2.0 * pi * f2[i] * t ) + ( upper_limit[i] + lower_limit[i]) / 2.0
00083             dq[i,:] = 2.0 * pi * f1[i] * A[i] * cos( 2.0 * pi * f1[i] * t ) \
00084                 + 2.0/3.0 * pi * f2[i] * A[i] * cos( 2.0 * pi * f2[i] * t )
00085             ddq[i,:] = - 4.0 * pi ** 2 * f1[i] ** 2 * A[i] * sin( 2.0 * pi * f1[i] * t ) \
00086                 - 4.0/3.0 * pi ** 2 * f2[i] ** 2 * A[i] * sin( 2.0 * pi * f2[i] * t )
00087         #else:
00088         #     q[i,:] = ones( (1, len( t )) ) * start_pos[i]
00089 
00090 
00091     # plt.subplot( 3, 1, 1);
00092     # plt.plot( q[joint,:] )
00093     # plt.subplot( 3, 1, 2);
00094     # plt.plot( dq[joint,:] )
00095     # plt.subplot( 3, 1, 3);
00096     # plt.plot( ddq[joint,:] )
00097     # plt.show()                  
00098     
00099     rospy.init_node( 'gpr_controller_trajectory_generator' )
00100     
00101 
00102     client = actionlib.SimpleActionClient( 'l_arm_controller/joint_trajectory_action', 
00103                                            JointTrajectoryAction )
00104     client.wait_for_server()
00105 
00106     jt = JointTrajectory()
00107     jt.joint_names = joint_names
00108     jt.points = []
00109 
00110     # start at neutral position
00111     jp = JointTrajectoryPoint()
00112     jp.time_from_start = rospy.Time.from_seconds( start_time )
00113     jp.positions = q[:,0]
00114     jp.velocities = [0] * len( joint_names )
00115     jp.accelerations = [0] * len( joint_names )
00116     jt.points.append( jp )
00117 
00118     # add precomputed trajectory
00119     for i in range( len( t ) ):
00120         jp = JointTrajectoryPoint()
00121         jp.time_from_start = rospy.Time.from_seconds( t[i] + start_time )
00122         jp.positions = q[:,i]
00123         jp.velocities = dq[:,i]
00124         jp.accelerations = ddq[:,i]
00125         jt.points.append( jp )
00126 
00127     # push trajectory goal to ActionServer
00128     goal = JointTrajectoryGoal()
00129     goal.trajectory = jt
00130     goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(4.0)
00131     client.send_goal(goal)
00132     client.wait_for_result()
00133     
00134 
00135 if __name__ == "__main__":
00136     main()


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