%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_IRB6650S_125_350(robot, T)
Solves the inverse kinematic problem for the ABB IRB 6650S_125_350 robot
where:
robot stores the robot parameters.
T is an homogeneous transform that specifies the position/orientation
of the end effector.
A call to Q=INVERSEKINEMATIC_IRB6650S_125_350 returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
abb=load_robot('ABB', 'IRB6650S_125_350');
q = [0 0 0 0 0 0];
T = directkinematic(abb, q);
%Call the inversekinematic for this robot
qinv = inversekinematic(abb, T);
check that all of them are feasible solutions!
and every Ti equals T
for i=1:8,
Ti = directkinematic(abb, qinv(:,i))
end
See also DIRECTKINEMATIC.
Author: Arturo Gil Aparicio
Universitas Miguel Hern�ndez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_IRB6650S_125_350(robot, T) 0003 % Solves the inverse kinematic problem for the ABB IRB 6650S_125_350 robot 0004 % where: 0005 % robot stores the robot parameters. 0006 % T is an homogeneous transform that specifies the position/orientation 0007 % of the end effector. 0008 % 0009 % A call to Q=INVERSEKINEMATIC_IRB6650S_125_350 returns 8 possible solutions, thus, 0010 % Q is a 6x8 matrix where each column stores 6 feasible joint values. 0011 % 0012 % 0013 % Example code: 0014 % 0015 % abb=load_robot('ABB', 'IRB6650S_125_350'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(abb, q); 0018 % %Call the inversekinematic for this robot 0019 % qinv = inversekinematic(abb, T); 0020 % check that all of them are feasible solutions! 0021 % and every Ti equals T 0022 % for i=1:8, 0023 % Ti = directkinematic(abb, qinv(:,i)) 0024 % end 0025 % See also DIRECTKINEMATIC. 0026 % 0027 % Author: Arturo Gil Aparicio 0028 % Universitas Miguel Hern�ndez, SPAIN. 0029 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0030 0031 % Copyright (C) 2012, by Arturo Gil Aparicio 0032 % 0033 % This file is part of ARTE (A Robotics Toolbox for Education). 0034 % 0035 % ARTE is free software: you can redistribute it and/or modify 0036 % it under the terms of the GNU Lesser General Public License as published by 0037 % the Free Software Foundation, either version 3 of the License, or 0038 % (at your option) any later version. 0039 % 0040 % ARTE is distributed in the hope that it will be useful, 0041 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0042 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0043 % GNU Lesser General Public License for more details. 0044 % 0045 % You should have received a copy of the GNU Leser General Public License 0046 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0047 function q = inversekinematic_irb6650S_125_350(robot, T) 0048 0049 %initialize q, 0050 %eight possible solutions are generally feasible 0051 q=zeros(6,8); 0052 0053 % %Evaluate the parameters 0054 % theta = eval(robot.DH.theta); 0055 d = eval(robot.DH.d); 0056 L6=d(6); 0057 0058 0059 %T= [ nx ox ax Px; 0060 % ny oy ay Py; 0061 % nz oz az Pz]; 0062 Px=T(1,4); 0063 Py=T(2,4); 0064 Pz=T(3,4); 0065 0066 %Compute the position of the wrist, being W the Z component of the end effector's system 0067 W = T(1:3,3); 0068 0069 % Pm: wrist position 0070 Pm = [Px Py Pz]' - L6*W; 0071 0072 %first joint, two possible solutions admited: 0073 % if q(1) is a solution, then q(1) + pi is also a solution 0074 q1=atan2(Pm(2), Pm(1)); 0075 0076 0077 %solve for q2 0078 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0079 %the other possible solution is q1 + pi 0080 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0081 0082 %solve for q3 0083 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0084 %solver for q3 for both cases 0085 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0086 0087 0088 %the next matrix doubles each column. For each two columns, two different 0089 %configurations for theta4, theta5 and theta6 will be computed. These 0090 %configurations are generally referred as wrist up and wrist down solution 0091 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0092 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0093 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0094 0 0 0 0 0 0 0 0; 0095 0 0 0 0 0 0 0 0; 0096 0 0 0 0 0 0 0 0]; 0097 0098 %leave only the real part of the solutions 0099 q=real(q); 0100 0101 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0102 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0103 %step is avoided in this angle (the next line is commented). When solving 0104 %for the orientation, the solutions are normalized to the [-pi, pi] range 0105 %only for the theta4, theta5 and theta6 joints. 0106 0107 %normalize q to [-pi, pi] 0108 q(1,:) = normalize(q(1,:)); 0109 q(2,:) = normalize(q(2,:)); 0110 0111 0112 % solve for the last three joints 0113 % for any of the possible combinations (theta1, theta2, theta3) 0114 for i=1:2:size(q,2), 0115 % use solve_spherical_wrist2 for the particular orientation 0116 % of the systems in this ABB robot 0117 % use either the geometric or algebraic method. 0118 % the function solve_spherical_wrist2 is used due to the relative 0119 % orientation of the last three DH reference systems. 0120 0121 %use either one algebraic method or the geometric 0122 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up 0123 qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up 0124 qtemp(4:6)=normalize(qtemp(4:6)); 0125 q(:,i)=qtemp; 0126 0127 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down 0128 qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down 0129 qtemp(4:6)=normalize(qtemp(4:6)); 0130 q(:,i+1)=qtemp; 0131 end 0132 0133 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0134 % solve for second joint theta2, two different 0135 % solutions are returned, corresponding 0136 % to elbow up and down solution 0137 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0138 function q2 = solve_for_theta2(robot, q, Pm) 0139 0140 %Evaluate the parameters 0141 d = eval(robot.DH.d); 0142 a = eval(robot.DH.a); 0143 0144 %See geometry 0145 L2=a(2); 0146 L3=sqrt(d(4)^2+a(3)^2); 0147 0148 %given q1 is known, compute first DH transformation 0149 T01=dh(robot, q, 1); 0150 0151 %Express Pm in the reference system 1, for convenience 0152 p1 = inv(T01)*[Pm; 1]; 0153 0154 r = sqrt(p1(1)^2 + p1(2)^2); 0155 0156 beta = atan2(-p1(2), p1(1)); 0157 gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2))); 0158 0159 if ~isreal(gamma) 0160 disp('WARNING:inversekinematic_irb2400: the point is not reachable for this configuration, imaginary solutions'); 0161 %gamma = real(gamma); 0162 end 0163 0164 %return two possible solutions 0165 %elbow up and elbow down 0166 %the order here is important and is coordinated with the function 0167 %solve_for_theta3 0168 q2(1) = pi/2 - beta - gamma; %elbow up 0169 q2(2) = pi/2 - beta + gamma; %elbow down 0170 0171 0172 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0173 % solve for third joint theta3, two different 0174 % solutions are returned, corresponding 0175 % to elbow up and down solution 0176 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0177 function q3 = solve_for_theta3(robot, q, Pm) 0178 0179 %Evaluate the parameters 0180 d = eval(robot.DH.d); 0181 a = eval(robot.DH.a); 0182 0183 %See geometry 0184 L2=a(2); 0185 L3=sqrt(d(4)^2+a(3)^2); 0186 0187 %given q1 is known, compute first DH transformation 0188 T01=dh(robot, q, 1); 0189 0190 %Express Pm in the reference system 1, for convenience 0191 p1 = inv(T01)*[Pm; 1]; 0192 0193 r = sqrt(p1(1)^2 + p1(2)^2); 0194 0195 eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3))); 0196 0197 if ~isreal(eta) 0198 disp('WARNING:inversekinematic_irb2400: the point is not reachable for this configuration, imaginary solutions'); 0199 %eta = real(eta); 0200 end 0201 0202 %return two possible solutions 0203 %elbow up and elbow down solutions 0204 %the order here is important 0205 q3(1) = -(atan(d(4)/a(3))+eta - pi); 0206 q3(2) = -(pi+atan(d(4)/a(3))-eta); 0207 0208 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0209 % % solve for second joint theta2, two different 0210 % % solutions are returned, corresponding 0211 % % to elbow up and down solution 0212 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0213 % function q2 = solve_for_theta2(robot, q, Pm) 0214 % 0215 % %Evaluate the parameters 0216 % theta = eval(robot.DH.theta); 0217 % d = eval(robot.DH.d); 0218 % a = eval(robot.DH.a); 0219 % alpha = eval(robot.DH.alpha); 0220 % 0221 % %See geometry 0222 % L2=a(2); 0223 % L3=d(4); 0224 % A2=a(3); 0225 % 0226 % %See geometry of the robot 0227 % %compute L4 0228 % L4 = sqrt(A2^2 + L3^2); 0229 % 0230 % %The inverse kinematic problem can be solved as in the IRB 140 (for example) 0231 % 0232 % %given q1 is known, compute first DH transformation 0233 % T01=dh(robot, q, 1); 0234 % 0235 % %Express Pm in the reference system 1, for convenience 0236 % p1 = inv(T01)*[Pm; 1]; 0237 % 0238 % r = sqrt(p1(1)^2 + p1(2)^2); 0239 % 0240 % beta = atan2(-p1(2), p1(1)); 0241 % gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2))); 0242 % 0243 % %return two possible solutions 0244 % %elbow up and elbow down 0245 % %the order here is important and is coordinated with the function 0246 % %solve_for_theta3 0247 % q2(1) = pi/2 - beta - gamma; %elbow up 0248 % q2(2) = pi/2 - beta + gamma; %elbow down 0249 % 0250 % 0251 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0252 % % solve for third joint theta3, two different 0253 % % solutions are returned, corresponding 0254 % % to elbow up and down solution 0255 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0256 % function q3 = solve_for_theta3(robot, q, Pm) 0257 % 0258 % %Evaluate the parameters 0259 % theta = eval(robot.DH.theta); 0260 % d = eval(robot.DH.d); 0261 % a = eval(robot.DH.a); 0262 % alpha = eval(robot.DH.alpha); 0263 % 0264 % %See geometry 0265 % L2=a(2); 0266 % L3=d(4); 0267 % A2=a(3); 0268 % 0269 % %See geometry of the robot 0270 % %compute L4 0271 % L4 = sqrt(A2^2 + L3^2); 0272 % 0273 % %the angle phi is fixed 0274 % phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4)); 0275 % 0276 % %given q1 is known, compute first DH transformation 0277 % T01=dh(robot, q, 1); 0278 % 0279 % %Express Pm in the reference system 1, for convenience 0280 % p1 = inv(T01)*[Pm; 1]; 0281 % 0282 % r = sqrt(p1(1)^2 + p1(2)^2); 0283 % 0284 % eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); 0285 % 0286 % %return two possible solutions 0287 % %elbow up and elbow down solutions 0288 % %the order here is important 0289 % q3(1) = pi - phi- eta; 0290 % q3(2) = pi - phi + eta; 0291 %