RTTRDEMO Trajectory demo
0001 %RTTRDEMO Trajectory demo 0002 0003 % Copyright (C) 1993-2002, by Peter I. Corke 0004 % $Log: not supported by cvs2svn $ 0005 % Revision 1.3 2002-04-02 12:26:49 pic 0006 % Handle figures better, control echo at end of each script. 0007 % Fix bug in calling ctraj. 0008 % 0009 % Revision 1.2 2002/04/01 11:47:18 pic 0010 % General cleanup of code: help comments, see also, copyright, remnant dh/dyn 0011 % references, clarification of functions. 0012 % 0013 % $Revision: 1.1 $ 0014 %*****************************trajectory**************************************** 0015 figure(2) 0016 echo on 0017 % The path will move the robot from its zero angle pose to the upright (or 0018 % READY) pose. 0019 % 0020 % First create a time vector, completing the motion in 2 seconds with a 0021 % sample interval of 56ms. 0022 t = [0:.056:2]; 0023 pause % hit any key to continue 0024 % 0025 % A polynomial trajectory between the 2 poses is computed using jtraj() 0026 % 0027 q = jtraj(qz, qr, t); 0028 pause % hit any key to continue 0029 0030 % 0031 % For this particular trajectory most of the motion is done by joints 2 and 3, 0032 % and this can be conveniently plotted using standard MATLAB operations 0033 subplot(2,1,1) 0034 plot(t,q(:,2)) 0035 title('Theta') 0036 xlabel('Time (s)'); 0037 ylabel('Joint 2 (rad)') 0038 subplot(2,1,2) 0039 plot(t,q(:,3)) 0040 xlabel('Time (s)'); 0041 ylabel('Joint 3 (rad)') 0042 0043 0044 pause % hit any key to continue 0045 % 0046 % We can also look at the velocity and acceleration profiles. We could 0047 % differentiate the angle trajectory using diff(), but more accurate results 0048 % can be obtained by requesting that jtraj() return angular velocity and 0049 % acceleration as follows 0050 [q,qd,qdd] = jtraj(qz, qr, t); 0051 % 0052 % which can then be plotted as before 0053 0054 subplot(2,1,1) 0055 plot(t,qd(:,2)) 0056 title('Velocity') 0057 xlabel('Time (s)'); 0058 ylabel('Joint 2 vel (rad/s)') 0059 subplot(2,1,2) 0060 plot(t,qd(:,3)) 0061 xlabel('Time (s)'); 0062 ylabel('Joint 3 vel (rad/s)') 0063 pause(2) 0064 % and the joint acceleration profiles 0065 subplot(2,1,1) 0066 plot(t,qdd(:,2)) 0067 title('Acceleration') 0068 xlabel('Time (s)'); 0069 ylabel('Joint 2 accel (rad/s2)') 0070 subplot(2,1,2) 0071 plot(t,qdd(:,3)) 0072 xlabel('Time (s)'); 0073 ylabel('Joint 3 accel (rad/s2)') 0074 pause % any key to continue 0075 echo off