Home > demos > rttgdemo.m

rttgdemo

PURPOSE ^

RTTRDEMO Trajectory demo

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

RTTRDEMO Trajectory demo

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003