SCRIPT TEST THE DIRECT DYNAMICS OF THE PUMA 560 ROBOT
0001 % SCRIPT TEST THE DIRECT DYNAMICS OF THE PUMA 560 ROBOT 0002 0003 % Copyright (C) 2012, by Arturo Gil Aparicio 0004 % 0005 % This file is part of ARTE (A Robotics Toolbox for Education). 0006 % 0007 % ARTE is free software: you can redistribute it and/or modify 0008 % it under the terms of the GNU Lesser General Public License as published by 0009 % the Free Software Foundation, either version 3 of the License, or 0010 % (at your option) any later version. 0011 % 0012 % ARTE is distributed in the hope that it will be useful, 0013 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0014 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0015 % GNU Lesser General Public License for more details. 0016 % 0017 % You should have received a copy of the GNU Lesser General Public License 0018 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0019 0020 fprintf('\nTHE SIMULATION PRESENTS THE ROBOT AT AN INITIAL POSITION WHEN NO TORQUES ARE APPLIED\n') 0021 0022 %load robot parameters 0023 robot=load_robot('unimate', 'puma560'); 0024 0025 total_simulation_time = 2; %simulate for 1 second 0026 0027 %initial position and joint speed 0028 q0 = [0 0 0 0 0 0]'; 0029 qd0 = [0 0 0 0 0 0]'; 0030 0031 drawrobot3d(robot, q0); 0032 adjust_view(robot); 0033 0034 %try both 0035 tau = [0 0 0 0 0 0]';%no torques applied 0036 %tau = [0 200 1 1 1 1]'; 0037 0038 %no friction 0039 robot.friction = 0; 0040 0041 fprintf('\nCOMPUTING FORWARD DYNAMICS (this may take a while)') 0042 0043 %this may take a while, since it requires integration 0044 %of the acceleration at each time step 0045 [t q qd] = forwarddynamic(robot, total_simulation_time, q0, qd0, tau, []); 0046 0047 %animate it!! 0048 animate(robot, q) 0049 0050 figure, plot(t, q), grid, title('Position vs. time') 0051 xlabel('time (s)'), ylabel('Position (rad)') 0052 legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6'); 0053 0054 figure, plot(t, qd), grid, title('Speed vs. time') 0055 xlabel('time (s)'), ylabel('Speed (rad/s)') 0056 legend('qd_1', 'qd_2', 'qd_3', 'qd_4', 'qd_5', 'qd_6');