%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PARAMETERS Returns a data structure containing the parameters of the Stanford robot. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 10/04/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % PARAMETERS Returns a data structure containing the parameters of the 0003 % Stanford robot. 0004 % 0005 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0006 % email: arturo.gil@umh.es date: 10/04/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= 'stanford'; 0028 0029 robot.DH.theta = '[q(1) q(2) -pi/2 q(4) q(5) q(6)]'; 0030 robot.DH.d= '[0.412 0.154 q(3) 0 0 0.263]'; 0031 robot.DH.a= '[ 0 0 0 0 0 0]'; 0032 robot.DH.alpha= '[-pi/2 pi/2 0 -pi/2 pi/2 0]'; 0033 robot.J=[]; 0034 robot.name='Stanford robotic arm'; 0035 0036 0037 robot.inversekinematic_fn = 'inversekinematic_stanford(robot, T)'; 0038 0039 %R: rotational, T: translational 0040 robot.kind=['R' 'R' 'T' 'R' 'R' 'R']; 0041 0042 %number of degrees of freedom 0043 robot.DOF = 6; 0044 0045 %minimum and maximum rotation angle in rad 0046 %Please note that these values are approximate 0047 robot.maxangle =[deg2rad(-160) deg2rad(160); %Axis 1, minimum, maximum 0048 deg2rad(-110) deg2rad(110); %Axis 2, minimum, maximum 0049 -0.5 0.5; %Axis 3, translational, m 0050 deg2rad(-266) deg2rad(266); %Axis 4: Unlimited (400º default) 0051 deg2rad(-100) deg2rad(100); %Axis 5 0052 deg2rad(-266) deg2rad(266)]; %Axis 6: Unlimited (800º default) 0053 0054 %maximum absolute speed of each joint rad/s or m/s 0055 robot.velmax = [1 0056 1 0057 1 0058 1 0059 1 0060 1];%not available 0061 0062 robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time 0063 0064 % end effectors maximum velocity 0065 robot.linear_velmax = 0.5; %m/s, from datasheet 0066 0067 0068 %base reference system 0069 robot.T0 = eye(4); 0070 0071 %INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION 0072 %position, velocity and acceleration 0073 robot=init_sim_variables(robot); 0074 robot.path = pwd; 0075 0076 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0077 % GRAPHICS 0078 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0079 %read graphics files 0080 robot.graphical.has_graphics=0; 0081 robot.graphical.color = [25 20 40]; 0082 %for transparency 0083 robot.graphical.draw_transparent=0; 0084 %draw DH systems 0085 robot.graphical.draw_axes=1; 0086 %DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for 0087 %bigger robots 0088 robot.graphical.axes_scale=1; 0089 %adjust for a default view of the robot 0090 robot.axis=[-1 1 -1 1 0 1]; 0091 robot = read_graphics(robot); 0092 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0093 0094 0095 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0096 % DYNAMIC PARAMETERS 0097 % As extracted from: 0098 %- Kinematic data from "Modelling, Trajectory calculation and Servoing of 0099 % a computer controlled arm". Stanford AIM-177. Figure 2.3 0100 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0101 robot.has_dynamics=1; 0102 0103 %consider friction in the computations 0104 robot.dynamics.friction=0; 0105 0106 %link masses (kg) 0107 robot.dynamics.masses=[9.29 5.01 4.25 1.08 0.630 0.51]; 0108 0109 0110 %COM of each link with respect to own reference system 0111 robot.dynamics.r_com=[0 0.0175 -0.1105; %(rx, ry, rz) link 1 0112 0 -1.054 0; %(rx, ry, rz) link 2 0113 0 0 -6.447; %(rx, ry, rz) link 3 0114 0 0.092 -0.054;%(rx, ry, rz) link 4 0115 0 0.566 0.003;%(rx, ry, rz) link 5 0116 0 0 1.554];%(rx, ry, rz) link 6 0117 0118 %Inertia matrices of each link with respect to its D-H reference system. 0119 % Ixx Iyy Izz Ixy Iyz Ixz, for each row 0120 robot.dynamics.Inertia=[0.276 0.255 0.071 0 0 0; 0121 0.108 0.018 0.100 0 0 0; 0122 2.51 2.51 0.006 0 0 0 ; 0123 0.002 0.001 0.001 0 0 0 ; 0124 0.003 0.0004 0 0 0 0; 0125 0.013 0.013 0.0003 0 0 0]; 0126 0127 robot.motors=load_motors([5 5 5 4 4 4]); 0128 %Speed reductor at each joint 0129 robot.motors.G=[300 300 300 300 300 300];