%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PARAMETERS Returns a data structure containing the parameters of the UNIMATE PUMA 560 robot. Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. email: arturo.gil@umh.es date: 03/03/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % PARAMETERS Returns a data structure containing the parameters of the 0003 % UNIMATE PUMA 560 robot. 0004 % 0005 % Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. 0006 % email: arturo.gil@umh.es date: 03/03/2012 0007 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0008 0009 % Copyright (C) 2012, by Arturo Gil Aparicio 0010 % 0011 % This file is part of ARTE (A Robotics Toolbox for Education). 0012 % 0013 % ARTE is free software: you can redistribute it and/or modify 0014 % it under the terms of the GNU Lesser General Public License as published by 0015 % the Free Software Foundation, either version 3 of the License, or 0016 % (at your option) any later version. 0017 % 0018 % ARTE is distributed in the hope that it will be useful, 0019 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0020 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0021 % GNU Lesser General Public License for more details. 0022 % 0023 % You should have received a copy of the GNU Leser General Public License 0024 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0025 function robot = parameters() 0026 %KYNEMATICS 0027 robot.name= 'puma_560'; 0028 0029 robot.DH.theta = '[q(1) q(2) q(3) q(4) q(5) q(6)]'; 0030 robot.DH.d='[0 0 0.15005 0.4318 0 0.04]'; 0031 robot.DH.a='[0 0.4318 0.0203 0 0 0]'; 0032 robot.DH.alpha= '[pi/2 0 -pi/2 pi/2 -pi/2 0]'; 0033 robot.J=[]; 0034 robot.name='Puma 560 robotic arm'; 0035 0036 robot.inversekinematic_fn = 'inversekinematic_puma560(robot, T)'; 0037 0038 0039 %R: rotational, T: translational 0040 robot.kind=['R' 'R' 'R' 'R' 'R' 'R']; 0041 0042 %number of degrees of freedom 0043 robot.DOF = 6; 0044 0045 %minimum and maximum rotation angle in rad 0046 robot.maxangle =[deg2rad(-160) deg2rad(160); %Axis 1, minimum, maximum 0047 deg2rad(-110) deg2rad(110); %Axis 2, minimum, maximum 0048 deg2rad(-135) deg2rad(135); %Axis 3 0049 deg2rad(-266) deg2rad(266); %Axis 4: Unlimited (400� default) 0050 deg2rad(-100) deg2rad(100); %Axis 5 0051 deg2rad(-266) deg2rad(266)]; %Axis 6: Unlimited (800� default) 0052 0053 %maximum absolute speed of each joint rad/s or m/s 0054 robot.velmax = [1 0055 1 0056 1 0057 1 0058 1 0059 1];%not available 0060 % end effectors maximum velocity 0061 robot.linear_velmax = 0.5; %m/s, from datasheet 0062 robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time 0063 0064 %base reference system 0065 robot.T0 = eye(4); 0066 0067 %INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION 0068 %position, velocity and acceleration 0069 robot=init_sim_variables(robot); 0070 robot.path = pwd; 0071 0072 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0073 % GRAPHICS 0074 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0075 %read graphics files 0076 robot.graphical.has_graphics=1; 0077 robot.graphical.color = [150 180 130]./255; 0078 %for transparency 0079 robot.graphical.draw_transparent=0; 0080 %draw DH systems 0081 robot.graphical.draw_axes=1; 0082 %DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for 0083 %bigger robots 0084 robot.graphical.axes_scale=1; 0085 %adjust for a default view of the robot 0086 robot.axis=[-1 1 -1 1 -0.66 2]; 0087 robot = read_graphics(robot); 0088 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0089 0090 0091 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0092 %DYNAMIC PARAMETERS 0093 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0094 robot.has_dynamics=1; 0095 0096 %consider friction in the computations 0097 robot.dynamics.friction=0; 0098 0099 %link masses (kg) 0100 robot.dynamics.masses=[0 17.4 4.8 0.82 0.34 0.09]; 0101 0102 %COM of each link with respect to own reference system 0103 robot.dynamics.r_com=[0 0 0; %(rx, ry, rz) link 1 0104 -0.3638 0.006 0.2275; %(rx, ry, rz) link 2 0105 -0.0203 -0.0141 0.070; %(rx, ry, rz) link 3 0106 0 0.019 0;%(rx, ry, rz) link 4 0107 0 0 0;%(rx, ry, rz) link 5 0108 0 0 -0.008];%(rx, ry, rz) link 6 0109 0110 %Inertia matrices of each link with respect to its D-H reference system. 0111 % Ixx Iyy Izz Ixy Iyz Ixz, for each row 0112 robot.dynamics.Inertia=[0 0.35 0 0 0 0; 0113 0.13 0.524 0.539 0 0 0; 0114 0.066 0.086 0.0125 0 0 0; 0115 1.8e-3 1.3e-3 1.8e-3 0 0 0; 0116 0.3e-3 0.4e-3 0.3e-3 0 0 0; 0117 0.15e-3 0.15e-3 0.04e-3 0 0 0]; 0118 0119 0120 %Please note that we are simulating the motors as presented in MAXON 0121 %catalog 0122 robot.motors=load_motors([5 5 5 5 5 5]); 0123 0124 0125 %Actuator rotor inertia 0126 %robot.motors.Inertia=[200e-6 200e-6 200e-6 33e-6 33e-6 33e-6]; 0127 %Speed reductor at each joint 0128 %robot.motors.G=[-62.6111 107.815 -53.7063 76.0364 71.923 76.686]; 0129 %Please note that, for simplicity in control, we consider that the gear 0130 %ratios are all positive 0131 robot.motors.G=[62.6111 107.815 53.7063 76.0364 71.923 76.686];