00001
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
00028
00029
00030
00031
00032 lower_limit = asarray( [2.13,
00033 -0.35,
00034 -0.5,
00035 -2.0,
00036 -2.0 * pi,
00037 -2.09,
00038 -2.0 * pi ] )
00039 upper_limit = asarray( [-0.56,
00040 0.6,
00041 2,
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
00077
00078
00079 for i in range(N):
00080
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
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
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
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
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
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()