Home > demos > rtidemo.m

rtidemo

PURPOSE ^

RTIDDEMO Inverse dynamics demo

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

RTIDDEMO Inverse dynamics demo

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %RTIDDEMO Inverse dynamics demo
0002 
0003 % Copyright (C) 1993-2002, by Peter I. Corke
0004 echo off
0005 % 6/99    fix syntax errors
0006 % $Log: not supported by cvs2svn $
0007 % Revision 1.3  2002-04-02 12:26:49  pic
0008 % Handle figures better, control echo at end of each script.
0009 % Fix bug in calling ctraj.
0010 %
0011 % Revision 1.2  2002/04/01 11:47:17  pic
0012 % General cleanup of code: help comments, see also, copyright, remnant dh/dyn
0013 % references, clarification of functions.
0014 %
0015 % $Revision: 1.1 $
0016 figure(2)
0017 echo on
0018 %
0019 % Inverse dynamics computes the joint torques required to achieve the specified
0020 % state of joint position, velocity and acceleration.
0021 % The recursive Newton-Euler formulation is an efficient matrix oriented
0022 % algorithm for computing the inverse dynamics, and is implemented in the
0023 % function rne().
0024 %
0025 % Inverse dynamics requires inertial and mass parameters of each link, as well
0026 % as the kinematic parameters.  This is achieved by augmenting the kinematic
0027 % description matrix with additional columns for the inertial and mass
0028 % parameters for each link.
0029 %
0030 % For example, for a Puma 560 in the zero angle pose, with all joint velocities
0031 % of 5rad/s and accelerations of 1rad/s/s, the joint torques required are
0032 %
0033     tau = rne(p560, qz, 5*ones(1,6), ones(1,6))
0034 pause % any key to continue
0035 
0036 % As with other functions the inverse dynamics can be computed for each point
0037 % on a trajectory.  Create a joint coordinate trajectory and compute velocity
0038 % and acceleration as well
0039     t = [0:.056:2];         % create time vector
0040     [q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory
0041     tau = rne(p560, q, qd, qdd); % compute inverse dynamics
0042 %
0043 %  Now the joint torques can be plotted as a function of time
0044     plot(t, tau(:,1:3))
0045     xlabel('Time (s)');
0046     ylabel('Joint torque (Nm)')
0047 pause % any key to continue
0048 
0049 %
0050 % Much of the torque on joints 2 and 3 of a Puma 560 (mounted conventionally) is
0051 % due to gravity.  That component can be computed using gravload()
0052     taug = gravload(p560, q);
0053     plot(t, taug(:,1:3))
0054     xlabel('Time (s)');
0055     ylabel('Gravity torque (Nm)')
0056 pause % any key to continue
0057 
0058 % Now lets plot that as a fraction of the total torque required over the
0059 % trajectory
0060     subplot(2,1,1)
0061     plot(t,[tau(:,2) taug(:,2)])
0062     xlabel('Time (s)');
0063     ylabel('Torque on joint 2 (Nm)')
0064     subplot(2,1,2)
0065     plot(t,[tau(:,3) taug(:,3)])
0066     xlabel('Time (s)');
0067     ylabel('Torque on joint 3 (Nm)')
0068 pause % any key to continue
0069 %
0070 % The inertia seen by the waist (joint 1) motor changes markedly with robot
0071 % configuration.  The function inertia() computes the manipulator inertia matrix
0072 % for any given configuration.
0073 %
0074 %  Let's compute the variation in joint 1 inertia, that is M(1,1), as the
0075 % manipulator moves along the trajectory (this may take a few minutes)
0076     M = inertia(p560, q);
0077     M11 = squeeze(M(1,1,:));
0078     plot(t, M11);
0079     xlabel('Time (s)');
0080     ylabel('Inertia on joint 1 (kgms2)')
0081 % Clearly the inertia seen by joint 1 varies considerably over this path.
0082 % This is one of many challenges to control design in robotics, achieving
0083 % stability and high-performance in the face of plant variation.  In fact
0084 % for this example the inertia varies by a factor of
0085     max(M11)/min(M11)
0086 pause % any key to continue
0087 echo off

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