RTIDDEMO Inverse dynamics demo
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