RTFDDEMO Forward dynamics demo
0001 %RTFDDEMO Forward dynamics demo 0002 0003 % Copyright (C) 1993-2002, by Peter I. Corke 0004 0005 % $Log: not supported by cvs2svn $ 0006 % Revision 1.4 2002-04-02 12:26:48 pic 0007 % Handle figures better, control echo at end of each script. 0008 % Fix bug in calling ctraj. 0009 % 0010 % Revision 1.3 2002/04/01 11:47:17 pic 0011 % General cleanup of code: help comments, see also, copyright, remnant dh/dyn 0012 % references, clarification of functions. 0013 % 0014 % $Revision: 1.1 $ 0015 figure(2) 0016 echo on 0017 % 0018 % Forward dynamics is the computation of joint accelerations given position and 0019 % velocity state, and actuator torques. It is useful in simulation of a robot 0020 % control system. 0021 % 0022 % Consider a Puma 560 at rest in the zero angle pose, with zero applied joint 0023 % torques. The joint acceleration would be given by 0024 accel(p560, qz, zeros(1,6), zeros(1,6)) 0025 pause % any key to continue 0026 % 0027 % To be useful for simulation this function must be integrated. fdyn() uses the 0028 % MATLAB function ode45() to integrate the joint acceleration. It also allows 0029 % for a user written function to compute the joint torque as a function of 0030 % manipulator state. 0031 % 0032 % To simulate the motion of the Puma 560 from rest in the zero angle pose 0033 % with zero applied joint torques 0034 tic 0035 [t q qd] = fdyn(nofriction(p560), 0, 10); 0036 toc 0037 % 0038 % and the resulting motion can be plotted versus time 0039 subplot(3,1,1) 0040 plot(t,q(:,1)) 0041 xlabel('Time (s)'); 0042 ylabel('Joint 1 (rad)') 0043 subplot(3,1,2) 0044 plot(t,q(:,2)) 0045 xlabel('Time (s)'); 0046 ylabel('Joint 2 (rad)') 0047 subplot(3,1,3) 0048 plot(t,q(:,3)) 0049 xlabel('Time (s)'); 0050 ylabel('Joint 3 (rad)') 0051 % 0052 % Clearly the robot is collapsing under gravity, but it is interesting to 0053 % note that rotational velocity of the upper and lower arm are exerting 0054 % centripetal and Coriolis torques on the waist joint, causing it to rotate. 0055 0056 pause % hit any key to continue 0057 % 0058 % This can be shown in animation also 0059 clf 0060 plot(p560, q) 0061 pause % hit any key to continue 0062 echo off