SCRIPT TEST THE DIRECT DYNAMICS OF A 3 DOF PLANAR ROBOT ROBOT
0001 % SCRIPT TEST THE DIRECT DYNAMICS OF A 3 DOF PLANAR ROBOT 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 Leser General Public License 0018 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0019 0020 close all 0021 0022 fprintf('\nTHE SIMULATION PRESENTS THE ROBOT AT AN INITIAL POSITION WHEN NO TORQUES ARE APPLIED\n') 0023 0024 %load robot parameters 0025 robot=load_robot('example', '3dofplanar'); 0026 0027 %simulate for 50 seconds, change this depending on your computer speed and 0028 %the total time that you want to simulate 0029 total_simulation_time = 50; 0030 0031 %initial position and joint speed 0032 q0 = [0 0 0]'; 0033 qd0 = [0 0 0]'; 0034 0035 drawrobot3d(robot, q0); 0036 adjust_view(robot); 0037 0038 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0039 % The student should try different combinations of the following parameters: 0040 % g: the direction of the gravity vector. In this case, if we select g=[0 0 9.81]'; 0041 % the movement of the arm is not affected by the gravity, since it 0042 % moves in a plane perpendicular to the gravity vector. 0043 % tau: the torques applied to each joint. The student should observe the effects of selecting 0044 % tau = [0 0 0]' or different values in combination with the 0045 % direction of the vector g. 0046 % robot.dynamics.friction = 0 selects no friction at the joints, whereas 0047 % robot.dynamics.friction = 1 considers that there exists friction. This 0048 % friction is modelled by robot.motors.Viscous (viscous friction) and 0049 % robot.motors.Coulomb (Coulomb friction). The student should observe 0050 % that selecting g=[0 9.81 0]' and tau = [0 0 0]' and 0051 % robot.dynamics.friction = 0 turns into an infinite triple pendulum 0052 % movement. In addition, selecting selecting g=[0 9.81 0]' and tau = [0 0 0]' and 0053 % robot.dynamics.friction = 1 simulates the case in which the triple 0054 % pendulum converges to a steady solution with the three links 0055 % hanging along the Y direction. 0056 % 0057 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0058 %you may redefine the gravity vector 0059 %in this case you may one of the next two lines, that define 0060 %the gravity acting along the Y axis or the Z axis, respectively. 0061 g=[0 9.81 0]'; %y axis 0062 %g=[0 0 9.81]'; % Z axis 0063 0064 %tau = [3 2 1]'; 0065 tau = [0 0 0]';%no torques applied 0066 0067 %select friction or not 0068 robot.dynamics.friction = 1; 0069 0070 fprintf('\nCOMPUTING FORWARD DYNAMICS (this may take a while)') 0071 0072 %this may take a while, since it requires integration 0073 %of the acceleration at each time step 0074 [t q qd] = forwarddynamic(robot, total_simulation_time, q0, qd0, tau, g, []); 0075 0076 %animate it!! 0077 animate(robot, q) 0078 0079 figure, plot(t, q), grid, title('Position vs. time') 0080 xlabel('time (s)'), ylabel('Position (rad)') 0081 legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6'); 0082 0083 figure, plot(t, qd), grid, title('Speed vs. time') 0084 xlabel('time (s)'), ylabel('Speed (rad/s)') 0085 legend('qd_1', 'qd_2', 'qd_3', 'qd_4', 'qd_5', 'qd_6');