%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PARAMETERS Returns a data structure containing the parameters of the Adept Viper_s1700D. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 09/01/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % PARAMETERS Returns a data structure containing the parameters of the 0003 % Adept Viper_s1700D. 0004 % 0005 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0006 % email: arturo.gil@umh.es date: 09/01/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 0027 robot.name= 'Adept_Viper_s1700D'; 0028 0029 %Path where everything is stored for this robot 0030 robot.path = 'robots/adept/Viper_s1700D'; 0031 0032 robot.DH.theta= '[q(1)-pi/2 q(2)-pi/2 q(3) q(4) q(5) q(6)]'; 0033 robot.DH.d='[0.505 0 0 0.795 0 0.105]'; 0034 robot.DH.a='[0.150 0.760 0.140 0 0 0]'; 0035 robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]'; 0036 0037 robot.J=[]; 0038 0039 0040 robot.inversekinematic_fn = 'inversekinematic_Viper_s1700D(robot, T)'; 0041 0042 %number of degrees of freedom 0043 robot.DOF = 6; 0044 0045 %rotational: 0, translational: 1 0046 robot.kind=['R' 'R' 'R' 'R' 'R' 'R']; 0047 0048 %minimum and maximum rotation angle in rad 0049 robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum 0050 deg2rad(-110) deg2rad(155); %Axis 2, minimum, maximum 0051 deg2rad(-235) deg2rad(80); %Axis 3, minimum, maximum 0052 deg2rad(-200) deg2rad(200); %Axis 4, minimum, maximum 0053 deg2rad(-140) deg2rad(140); %Axis 5, minimum, maximum 0054 deg2rad(-360) deg2rad(360)]; %Axis 6, minimum, maximum 0055 0056 %maximum absolute speed of each joint rad/s or m/s 0057 robot.velmax = [deg2rad(197); %Axis 1, rad/s 0058 deg2rad(170); %Axis 2, rad/s 0059 deg2rad(187); %Axis 3, rad/s 0060 deg2rad(400); %Axis 4, rad/s 0061 deg2rad(400); %Axis 5, rad/s 0062 deg2rad(600)];%Axis 6, rad/s 0063 % end effectors maximum velocity 0064 robot.linear_velmax = 1.0; %m/s, unavailable from datasheet 0065 0066 robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time 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 0075 0076 % GRAPHICS 0077 robot.graphical.has_graphics=1; 0078 robot.graphical.color = [204 51 0]./255; 0079 %for transparency 0080 robot.graphical.draw_transparent=0; 0081 %draw DH systems 0082 robot.graphical.draw_axes=1; 0083 %DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for 0084 %bigger robots 0085 robot.graphical.axes_scale=1; 0086 %adjust for a default view of the robot 0087 robot.axis=[-3 3 -3 3 0 3]; 0088 %read graphics files 0089 robot = read_graphics(robot); 0090 0091 0092 %DYNAMICS 0093 robot.has_dynamics=1; 0094 %consider friction in the computations 0095 robot.dynamics.friction=0; 0096 0097 %link masses (kg) 0098 robot.dynamics.masses=[121.853 42.013 36.973 11.477 2.579 0.273]; 0099 0100 %COM of each link with respect to own reference system (m) 0101 robot.dynamics.r_com=[0.018153 -0.078706 0.129726; %(rx, ry, rz) link 1 0102 0.173856 0.016125 0.415181; %(rx, ry, rz) link 2 0103 -0.000496 0.029239 0.091890; %(rx, ry, rz) link 3 0104 -0.000878 -0.291253 0.000043;%(rx, ry, rz) link 4 0105 -0.004461 0.024509 -0.000119;%(rx, ry, rz) link 5 0106 0.000349 -0.011501 0];%(rx, ry, rz) link 6 0107 0108 %Inertia matrix of each link with respect to its D-H reference system. 0109 % Ixx Iyy Izz Ixy Iyz Ixz, for each row (kg*m^2) 0110 robot.dynamics.Inertia=[355.368 321.310 242.895 0 0 0; 0111 323.236 315.283 24.310 0 0 0; 0112 50.930 39.258 36.902 0 0 0; 0113 42.088 4.435 42.516 0 0 0; 0114 0.607 0.361 0.526 0 0 0; 0115 0.009 0.016 0.009 0 0 0]; 0116 0117 robot.motors=load_motors([5 5 5 4 4 4]); 0118 %Speed reductor at each joint 0119 robot.motors.G=[300 300 300 300 300 300];