SCRIPT TEST FOR THE KINEMATIC PROBLEM FOR SERIAL ROBOTS
0001 % SCRIPT TEST FOR THE KINEMATIC PROBLEM FOR SERIAL ROBOTS 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 0023 fprintf('\nTHE DEMO PRESENTS THE DIRECT AND INVERSE KINEMATIC PROBLEM') 0024 0025 %there are eight possible solutions for the 0026 %inverse kinematic problem for most of these robots 0027 n_solutions = 8; 0028 0029 %Try different configurations 0030 %q=[0 0 0 0 0 0] 0031 q = [0 -pi/4 -0.3 1.4 1.5 1.6] 0032 %q = [-0.1 -0.8 0.8 0.5 1.5 pi] 0033 0034 %q = [pi/4 pi/4 pi/8 pi/8 pi/8 pi/8] 0035 %q = [-0.9*pi 0 0 0 pi/3 0]; 0036 %q = [pi/4 pi/4 pi/4 0 0 0]; 0037 0038 %load robot parameters 0039 % you can try different robots 0040 %robot=load_robot('ABB', 'IRB140'); n_solutions = 8; 0041 %robot=load_robot('ABB', 'IRB120'); n_solutions = 8; 0042 %robot=load_robot('ABB', 'IRB1600_6_120'); n_solutions = 8; 0043 %* 0044 %robot=load_robot('ABB', 'IRB1600_X145_M2004'); n_solutions = 8; 0045 %robot=load_robot('ABB', 'IRB1600ID'); n_solutions = 8; 0046 %robot=load_robot('ABB', 'IRB2400'); n_solutions = 8; 0047 %robot=load_robot('ABB', 'IRB4400'); n_solutions = 8; 0048 %robot=load_robot('ABB', 'IRB4600'); n_solutions = 8; 0049 %robot=load_robot('ABB', 'IRB52'); n_solutions = 8; 0050 %robot=load_robot('ABB', 'IRB6620'); n_solutions = 8; 0051 %robot=load_robot('ABB', 'IRB6620LX'); n_solutions = 4; 0052 %robot=load_robot('ABB', 'IRB6650S_125_350'); n_solutions = 8; 0053 %robot=load_robot('ABB', 'IRB7600_150'); n_solutions = 8; 0054 %robot=load_robot('ABB', 'IRB7600_400_255_m2000'); n_solutions = 8; 0055 %robot=load_robot('ABB', 'IRB7600_500_230'); n_solutions = 8; 0056 %ADEPT 0057 %robot=load_robot('adept', 'Viper_s1700D'); n_solutions = 8; 0058 %EPSON 0059 %robot=load_robot('epson', 'Prosix_C3_A601C'); n_solutions = 8; 0060 %FANUC 0061 %robot=load_robot('fanuc', 'LR_MATE_200iC'); n_solutions = 8; 0062 %KUKA 0063 %robot=load_robot('kuka', 'KR30_jet'); n_solutions = 8; 0064 %robot=load_robot('kuka', 'KR5_2ARC_HW'); n_solutions = 8; 0065 %robot=load_robot('kuka', 'KR5_arc'); n_solutions = 8; 0066 %robot=load_robot('kuka', 'KR5_sixx_R650'); n_solutions = 8; 0067 %robot=load_robot('kuka', 'KR5_sixx_R850'); n_solutions = 8; 0068 %robot=load_robot('kuka', 'KR6_2'); n_solutions = 8; 0069 %robot=load_robot('kuka', 'KR90_R2700_pro'); n_solutions = 8; 0070 %robot=load_robot('kuka', 'KR90_R3100_EXTRA'); n_solutions = 8; 0071 %robot=load_robot('kuka', 'KR_1000_1300_TITAN'); n_solutions = 8; 0072 %robot=load_robot('kuka', 'KR_16_arc_HW'); n_solutions = 8; 0073 %robot=load_robot('kuka', 'KR_30_L16_2'); n_solutions = 8; 0074 %MITSUBISHI 0075 %robot=load_robot('mitsubishi', 'pa-10'); n_solutions = 8;%q=[0 0 0 0 0 0]; 0076 %robot=load_robot('mitsubishi', 'rv-6s'); n_solutions = 8;%q=[0 0 0 0 0 0]; 0077 %STANFORD ARM 0078 %robot=load_robot('stanford', ''); n_solutions = 4;%q=[0 0 0 0 0 0]; 0079 %STAUBLI 0080 %robot=load_robot('staubli', 'RX160L'); n_solutions = 8; q=[0.1 -0.6 1 0.4 0.5 0.6] 0081 %UNIMATE 0082 %robot=load_robot('unimate', 'puma560'); n_solutions = 8; 0083 0084 0085 0086 %adjust 3D view as desired 0087 adjust_view(robot) 0088 0089 %there are just 2 solutions for these robots 0090 %q = [pi/2 0.2 0.8 pi/4] 0091 %q = [-pi/4 pi/2 0.5 pi] 0092 %robot=load_robot('kuka', 'KR5_scara_R350_Z200'); n_solutions = 2; 0093 %robot=load_robot('example', 'scara'); n_solutions = 2; 0094 %robot=load_robot('example', '2dofplanar'); n_solutions = 2; 0095 %robot=load_robot('example', '3dofplanar'); n_solutions = 2; 0096 %robot=load_robot('example', 'prismatic');n_solutions = 1; %just one possible solutions for this case 0097 0098 0099 %draw the robot 0100 drawrobot3d(robot, q) 0101 0102 %Now compute direct kinematics for this position q 0103 T = directkinematic(robot, q) 0104 0105 %Set to zero if you want to see the robot transparent 0106 robot.graphical.draw_transparent=0; 0107 0108 %Set to one if you want to see the DH axes 0109 %abb.graphical.draw_axes=1; 0110 0111 %Call the inversekinematic for this robot 0112 % all the possible solutions are stored at qinv 0113 % at least, one of the possible solutions should be coincident with q 0114 qinv = inversekinematic(robot, T); 0115 0116 0117 fprintf('\nNOW WE CAN REPRESENT THE DIFFERENT SOLUTIONS TO ACHIEVE THE SAME POSITION AND ORIENTATION\n') 0118 fprintf('\nNot that some solutions may not be feasible. Some joints may be out of range\n') 0119 correct=zeros(1,n_solutions); 0120 %check that all of them are possible solutions! 0121 for i=1:size(qinv,2), 0122 0123 Ti = directkinematic(robot, qinv(:,i)) %Ti is constant for the different solutions 0124 0125 0126 % Note that all the solutions may not be feasible. Some of the joints may 0127 % be out of range. You can test this situation with test_joints 0128 test_joints(robot, qinv(:,i)); 0129 0130 0131 %now draw the robot to see the solution 0132 drawrobot3d(robot, qinv(:,i)) 0133 0134 pause(1); 0135 0136 k=sum(sum((T-Ti).^2)); 0137 if k < 0.01 % a simple threshold to find differences in the solution 0138 correct(1,i)= 1; 0139 else 0140 correct(1,i)= 0; %uncorrect solution 0141 fprintf('\nERROR: One of the solutions seems to be uncorrect. Sum of errors: %f', i, k); 0142 end 0143 end 0144 %Display a message if any of the solutions is not correct 0145 if sum(correct)==n_solutions 0146 fprintf('\nOK: Every solution in qinv yields the same position/orientation T'); 0147 else 0148 fprintf('\nERROR: One of the solutions seems to be uncorrect.'); 0149 end 0150 0151 %Now, test if any of the solutions in qinv matches q 0152 delta=(repmat(q',[1 n_solutions])-qinv).^2; 0153 %find the solution that matches the initial q 0154 %and corresponds 0155 i=find(sum(delta,1)<0.01); 0156 if ~isempty(i) 0157 fprintf('\nOK!: Found a matching solution:\n'); 0158 qinv(:,i) 0159 else 0160 fprintf('\nERROR: Did not find a matching solution for the initial q'); 0161 end