test_all_robots_kinematics_demo

 SCRIPT TO TEST THE KINEMATICS OF ALL SERIAL ROBOTS IN THE LIBRARY

0001 % SCRIPT TO TEST THE KINEMATICS OF ALL SERIAL ROBOTS IN THE LIBRARY
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 function test_all_robots_kinematics_demo
0021 %close all
0022 fprintf('\nTHE DEMO LOADS EVERY ROBOT IN THE LIBRARY AND PERFORMS A KINEMATICS TEST')
0023 
0024 %global variable that stores the number of solutions of the inverse
0025 %kinematic for the currently loaded robot
0026 global n_solutions robot fp debug configuration n_q_points
0027 
0028 %set debug=1 to draw the robot
0029 debug=1;
0030 
0031 %total number of robots in the library
0032 n_robots=44;
0033 
0034 %test every robot with this number of different joint values q
0035 n_q_points=500;
0036 
0037 %store last tested robot in i
0038 i=44
0039 for i=i:n_robots,
0040     i
0041     fp=fopen([configuration.libpath '/demos/test_all_robots_results'],'a');
0042     
0043     %local function to load one of the robots
0044     load_a_robot(i);
0045     
0046     fprintf(fp, '\n********TESTING ROBOT********: %s', robot.name);
0047     
0048     test_robot_kinematics();
0049     fclose(fp);
0050 end
0051 
0052 
0053 
0054 function test_robot_kinematics
0055 global robot n_solutions fp debug n_q_points
0056 
0057 for i=1:n_q_points,
0058     q=generate_random_q();
0059     
0060     q_inv=[];
0061     
0062     %Now compute direct kinematics for this position q
0063     T = directkinematic(robot, q)
0064     
0065     %Set to zero if you want to see the robot transparent
0066     robot.graphical.draw_transparent=0;
0067     
0068     %Call the inversekinematic for this robot
0069     % all the possible solutions are stored at qinv
0070     % at least, one of the possible solutions should be coincident with q
0071     qinv = inversekinematic(robot, T);
0072     
0073     
0074     fprintf('\nNOW WE CAN REPRESENT THE DIFFERENT SOLUTIONS TO ACHIEVE THE SAME POSITION AND ORIENTATION\n')
0075     fprintf('\nNot that some solutions may not be feasible. Some joints may be out of range\n')
0076     correct=zeros(1,n_solutions);
0077     %check that all of them are possible solutions!
0078     for i=1:size(qinv,2),
0079         
0080         Ti = directkinematic(robot, qinv(:,i)) %Ti is constant for the different solutions
0081         
0082         if debug
0083         %now draw the robot to see the solution
0084             drawrobot3d(robot, qinv(:,i))
0085             pause(1);
0086         end
0087         
0088         k=sum(sum((T-Ti).^2));
0089         if k < 0.01 % a simple threshold to find differences in the solution
0090             correct(1,i)= 1;
0091         else
0092             correct(1,i)= 0; %uncorrect solution
0093             fprintf('\nERROR: One of the solutions seems to be uncorrect. Sum of errors: %f', i, k);
0094             fprintf(fp, '\nERROR: One of the solutions seems to be uncorrect. Sum of errors: %f', i, k);
0095        end
0096     end
0097     %Display a message if any of the solutions is not correct
0098     if sum(correct)==n_solutions
0099         fprintf('\nOK: Every solution in qinv yields the same position/orientation T');
0100     else
0101         fprintf('\nERROR: One of the solutions seems to be uncorrect.');
0102         fprintf(fp, '\nERROR: One of the solutions seems to be uncorrect.');
0103     end
0104     
0105     %Now, test if any of the solutions in qinv matches q
0106     delta=(repmat(q',[1 n_solutions])-qinv).^2;
0107     %find the solution that matches the initial q
0108     %and corresponds
0109     i=find(sum(delta,1)<0.01);
0110     if ~isempty(i)
0111         fprintf('\nOK!: Found a matching solution:\n');
0112         qinv(:,i)
0113     else
0114         fprintf('\nERROR: Did not find a matching solution for the initial q');
0115         fprintf(fp, '\nERROR: Did not find a matching solution for the initial q');
0116    end
0117     
0118 end
0119 
0120 
0121 %LOAD A ROBOT IN THE LIBRARY FROM A INTEGER INDEX
0122 function load_a_robot(index)
0123 
0124 global robot n_solutions
0125 %load robot parameters
0126 % you can try different robots
0127 switch index
0128     %ABB
0129     case 1
0130         robot=load_robot('ABB', 'IRB140'); n_solutions = 8;    
0131     case 2
0132         robot=load_robot('ABB', 'IRB120'); n_solutions = 8;
0133     case 3
0134         robot=load_robot('ABB', 'IRB1600_6_120'); n_solutions = 8;
0135     case 4
0136         robot=load_robot('ABB', 'IRB1600_X145_M2004'); n_solutions = 8;
0137     case 5    
0138         robot=load_robot('ABB', 'IRB1600ID'); n_solutions = 8;
0139     
0140     case 6   
0141         robot=load_robot('ABB', 'IRB2400'); n_solutions = 8;
0142     case 7    
0143         robot=load_robot('ABB', 'IRB4400'); n_solutions = 2;
0144     case 8   
0145         robot=load_robot('ABB', 'IRB4600'); n_solutions = 8;
0146     case 9    
0147         robot=load_robot('ABB', 'IRB52'); n_solutions = 8;
0148     case 10    
0149         robot=load_robot('ABB', 'IRB6620'); n_solutions = 8;
0150     case 11    
0151         robot=load_robot('ABB', 'IRB6620LX'); n_solutions = 4;
0152     case 12    
0153         robot=load_robot('ABB', 'IRB6650S_125_350'); n_solutions = 8;
0154     case 13    
0155         robot=load_robot('ABB', 'IRB6650S_200_300'); n_solutions = 8;
0156     case 14    
0157         robot=load_robot('ABB', 'IRB6650S_90_390'); n_solutions = 8;
0158     case 15    
0159         robot=load_robot('ABB', 'IRB760'); n_solutions = 1;     
0160     case 16    
0161         robot=load_robot('ABB', 'IRB7600_150'); n_solutions = 8;
0162     case 17    
0163         robot=load_robot('ABB', 'IRB7600_400_255_m2000'); n_solutions = 8;
0164     case 18    
0165         robot=load_robot('ABB', 'IRB7600_500_230'); n_solutions = 8;
0166     case 19
0167          robot=load_robot('ABB', 'IRB1600X_140'); n_solutions = 8;
0168 
0169          %ADEPT
0170     case 20
0171         robot=load_robot('ADEPT', 'Viper_s1700D'); n_solutions = 8;
0172         
0173         %EPSON
0174     case 21    
0175         robot=load_robot('EPSON', 'Prosix_C3_A601C'); n_solutions = 8;
0176         
0177         %FANUC
0178     case 22
0179         robot=load_robot('FANUC', 'LR_MATE_200iC'); n_solutions = 8;
0180 
0181        %KUKA
0182     case 23
0183         robot=load_robot('KUKA', 'KR30_jet'); n_solutions = 4;
0184     case 24       
0185         robot=load_robot('KUKA', 'KR5_2ARC_HW'); n_solutions = 8;
0186     case 25
0187         robot=load_robot('KUKA', 'KR5_arc'); n_solutions = 8;
0188     case 26
0189         robot=load_robot('KUKA', 'KR5_sixx_R650'); n_solutions = 8;
0190     case 27
0191         robot=load_robot('KUKA', 'KR5_sixx_R850'); n_solutions = 8;
0192     case 28
0193         robot=load_robot('KUKA', 'KR6_2'); n_solutions = 8;
0194     case 29
0195         robot=load_robot('KUKA', 'KR90_R2700_pro'); n_solutions = 8;
0196     case 30
0197         robot=load_robot('KUKA', 'KR90_R3100_EXTRA'); n_solutions = 8;
0198     case 31
0199         robot=load_robot('KUKA', 'KR_1000_1300_TITAN'); n_solutions = 8;
0200     case 32
0201         robot=load_robot('KUKA', 'KR_16_arc_HW'); n_solutions = 8;
0202     case 33
0203         robot=load_robot('KUKA', 'KR_30_L16_2'); n_solutions = 8;
0204      case 34
0205         robot=load_robot('KUKA', 'KR5_scara_R350_Z200'); n_solutions = 2;   
0206 
0207   %MITSUBISHI
0208     case 35
0209         robot=load_robot('MITSUBISHI', 'PA-10'); n_solutions = 8;%q=[0 0 0 0 0 0];
0210     case 36
0211         robot=load_robot('MITSUBISHI', 'rv-6s'); n_solutions = 8;%q=[0 0 0 0 0 0];
0212   
0213         %STAUBLI
0214     case 37
0215         robot=load_robot('STAUBLI', 'RX160L'); n_solutions = 8; q=[0.1 -0.6 1 0.4 0.5 0.6]
0216     case 38
0217         robot=load_robot('STAUBLI', 'RX170BL'); n_solutions = 8; q=[0.1 -0.6 1 0.4 0.5 0.6]
0218 
0219         %UNIMATE
0220     case 39
0221         robot=load_robot('UNIMATE', 'puma560'); n_solutions = 8;
0222 
0223         %STANFORD ARM
0224     case 40
0225         robot=load_robot('example', 'stanford'); n_solutions = 4;%q=[0 0 0 0 0 0];
0226 
0227        %EXAMPLE ROBOTS
0228     case 41
0229         robot=load_robot('example', 'scara'); n_solutions = 2;
0230     case 42
0231         disp('ONLY THE POSOTION AND NOT THE ORIENTATION SHOULD BE COMPARED IN THIS ROBOT')
0232         robot=load_robot('example', '2dofplanar'); n_solutions = 2;
0233 
0234     case 43
0235         robot=load_robot('example', '3dofplanar'); n_solutions = 2;
0236     case 44 
0237         disp('ONLY THE POSOTION AND NOT THE ORIENTATION SHOULD BE COMPARED IN THIS ROBOT')
0238        
0239         robot=load_robot('example', '3dofspherical');n_solutions = 2; %just one possible solutions for this case
0240 
0241       %draw the robot
0242 drawrobot3d(robot)
0243 
0244 end
0245 
0246 %generate a random q considering the angle range
0247 function q=generate_random_q()
0248 
0249 global robot
0250 
0251 q=ones(1,robot.DOF);
0252 
0253 delta = 0.25;
0254 for i=1:robot.DOF,
0255     %this line poses a problem for angles with a total range greater than
0256     %2pi
0257     %delta = robot.maxangle(i,2)-robot.maxangle(i,1);
0258     %q(i)=delta*rand + robot.maxangle(i,1);
0259     q(i)=2*delta*rand-delta;
0260 end
0261 
0262 q(2)=-0.5;
0263 
0264 
0265 q=normalize(q);