%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_IRB6620LX(robot, T)
Solves the inverse kinematic problem for the ABB IRB 6620LX 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_IRB6620LX returns 4 possible solutions, thus,
Q is a 6x4 matrix where each column stores 6 feasible joint values.
Example code:
robot=load_robot('ABB', 'IRB6620LX');
q = [0 0 0 0 0 0];
T = directkinematic(abb, q);
%Call the inversekinematic for this robot
qinv = inversekinematic(robot, T);
check that all of them are feasible solutions!
and every Ti equals T
for i=1:4,
Ti = directkinematic(robot, qinv(:,i))
end
See also DIRECTKINEMATIC.
Author: Arturo Gil Aparicio
Universitas Miguel Hernandez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_IRB6620LX(robot, T) 0003 % Solves the inverse kinematic problem for the ABB IRB 6620LX 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_IRB6620LX returns 4 possible solutions, thus, 0010 % Q is a 6x4 matrix where each column stores 6 feasible joint values. 0011 % 0012 % 0013 % Example code: 0014 % 0015 % robot=load_robot('ABB', 'IRB6620LX'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(abb, q); 0018 % %Call the inversekinematic for this robot 0019 % qinv = inversekinematic(robot, T); 0020 % check that all of them are feasible solutions! 0021 % and every Ti equals T 0022 % for i=1:4, 0023 % Ti = directkinematic(robot, qinv(:,i)) 0024 % end 0025 % 0026 % See also DIRECTKINEMATIC. 0027 % 0028 % Author: Arturo Gil Aparicio 0029 % Universitas Miguel Hernandez, SPAIN. 0030 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0031 0032 % Copyright (C) 2012, by Arturo Gil Aparicio 0033 % 0034 % This file was written by Miguel Catalan Ba�uls and Jorge Diez Pomares. 0035 % 0036 % This file is part of ARTE (A Robotics Toolbox for Education). 0037 % 0038 % ARTE is free software: you can redistribute it and/or modify 0039 % it under the terms of the GNU Lesser General Public License as published by 0040 % the Free Software Foundation, either version 3 of the License, or 0041 % (at your option) any later version. 0042 % 0043 % ARTE is distributed in the hope that it will be useful, 0044 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0045 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0046 % GNU Lesser General Public License for more details. 0047 % 0048 % You should have received a copy of the GNU Leser General Public License 0049 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0050 function q = inversekinematic_irb6620lx(robot, T) 0051 0052 %initialize q, 0053 %eight possible solutions are generally feasible 0054 q=zeros(6,8); 0055 0056 %Evaluate the parameters 0057 theta = eval(robot.DH.theta); 0058 d = eval(robot.DH.d); 0059 a = eval(robot.DH.a); 0060 alpha = eval(robot.DH.alpha); 0061 0062 0063 %See geometry at the reference for this robot 0064 %L1=d(1); 0065 L3=sqrt((a(3)*a(3))+(d(4)*d(4))); 0066 L2=a(2); 0067 L6=d(6); 0068 0069 A1 = a(1); 0070 0071 0072 %T= [ nx ox ax Px; 0073 % ny oy ay Py; 0074 % nz oz az Pz]; 0075 Px=T(1,4); 0076 Py=T(2,4); 0077 Pz=T(3,4); 0078 0079 %Compute the position of the wrist, being W the Z component of the end effector's system 0080 W = T(1:3,3); 0081 0082 % Pm: wrist position 0083 Pm = [Px Py Pz]' - L6*W; 0084 0085 % first joint: only one solution is feasible. q(1) corresponds 0086 % to a translation 0087 q1=Pm(3); 0088 0089 0090 %solve for q2 0091 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0092 0093 %q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0094 0095 %solve for q3 0096 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0097 0098 %q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0099 0100 0101 %Arrange solutions, there are 4 possible solutions so far. 0102 % So far, we have 4 possible solutions. However, for each triplet (theta1, theta2, theta3), 0103 % there exist two more possible solutions for the last three joints, generally 0104 % called wrist up and wrist down solutions. For this reason, 0105 %the next matrix doubles each column. For each two columns, two different 0106 %configurations for theta4, theta5 and theta6 will be computed. These 0107 %configurations are generally referred as wrist up and wrist down solution 0108 q = [q1 q1 q1 q1 ; 0109 q2_1(1) q2_1(1) q2_1(2) q2_1(2) ; 0110 q3_1(1) q3_1(1) q3_1(2) q3_1(2); 0111 0 0 0 0 ; 0112 0 0 0 0 ; 0113 0 0 0 0 ]; 0114 0115 0116 q=real(q); 0117 0118 %normalize q to [-pi, pi] 0119 %do not normalize q1, it is a translation 0120 q(2,:) = normalize(q(2,:)); 0121 0122 0123 % solve for the last three joints 0124 % for any of the possible combinations (theta1, theta2, theta3) 0125 for i=1:2:size(q,2), 0126 % use solve_spherical_wrist2 for the particular orientation 0127 % of the systems in this ABB robot 0128 % use either the geometric or algebraic method. 0129 % the function solve_spherical_wrist2 is used due to the relative 0130 % orientation of the last three DH reference systems. 0131 0132 %use either one algebraic method or the geometric 0133 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up 0134 qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up 0135 qtemp(4:6)=normalize(qtemp(4:6)); 0136 q(:,i)=qtemp; 0137 0138 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down 0139 qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down 0140 qtemp(4:6)=normalize(qtemp(4:6)); 0141 q(:,i+1)=qtemp; 0142 end 0143 0144 0145 0146 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0147 % solve for second joint theta2, two different 0148 % solutions are returned, corresponding 0149 % to elbow up and down solution 0150 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0151 function q2 = solve_for_theta2(robot, q, Pm) 0152 0153 %Evaluate the parameters 0154 d = eval(robot.DH.d); 0155 a = eval(robot.DH.a); 0156 0157 %See geometry 0158 L3=sqrt((a(3)*a(3))+(d(4)*d(4))); 0159 L2=a(2); 0160 0161 %given q1 is known, compute first DH transformation 0162 T01=dh(robot, q, 1); 0163 0164 %Express Pm in the reference system 1, for convenience 0165 p1 = inv(T01)*[Pm; 1]; 0166 0167 r = sqrt(p1(1)^2 + p1(2)^2); 0168 0169 alpha = atan2(p1(2), p1(1)); 0170 beta = (acos((L2^2+r^2-L3^2)/(2*r*L2))); 0171 0172 if ~isreal(beta) 0173 disp('WARNING:inversekinematic_irb6620lx: the point is not reachable for this configuration, imaginary solutions'); 0174 %gamma = real(gamma); 0175 end 0176 0177 %return two possible solutions 0178 %elbow up and elbow down 0179 %the order here is important and is coordinated with the function 0180 %solve_for_theta3 0181 q2(1) = alpha + beta; %elbow up 0182 q2(2) = alpha-beta; %elbow down 0183 0184 0185 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0186 % solve for third joint theta3, two different 0187 % solutions are returned, corresponding 0188 % to elbow up and down solution 0189 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0190 function q3 = solve_for_theta3(robot, q, Pm) 0191 0192 %Evaluate the parameters 0193 d = eval(robot.DH.d); 0194 a = eval(robot.DH.a); 0195 0196 %See geometry 0197 0198 L3=sqrt((a(3)*a(3))+(d(4)*d(4))); 0199 L2=a(2); 0200 0201 %given q1 is known, compute first DH transformation 0202 T01=dh(robot, q, 1); 0203 0204 %Express Pm in the reference system 1, for convenience 0205 p1 = inv(T01)*[Pm; 1]; 0206 0207 r = sqrt(p1(2)^2 + p1(1)^2); 0208 0209 gamma = acos((L2^2 + L3^2 - r^2)/(2*L2*L3)); 0210 delta= atan(d(4)/a(3)); 0211 if ~isreal(gamma) 0212 disp('WARNING:inversekinematic_irb6620lx: the point is not reachable for this configuration, imaginary solutions'); 0213 %eta = real(eta); 0214 end 0215 0216 %return two possible solutions 0217 %elbow up and elbow down solutions 0218 %the order here is important 0219 q3(1) = delta-(pi-gamma); 0220 q3(2) = (pi-gamma)+delta ; 0221 0222 0223 0224 0225 0226 % %remove complex solutions for q for the q1+pi solutions 0227 % function qreal = arrange_solutions(q) 0228 % qreal=q; 0229 % 0230 % %sum along rows if any angle is complex, for any possible solutions, then v(i) is complex 0231 % v = sum(q, 1); 0232 % 0233 % for i=5:8, 0234 % if isreal(v(i)) 0235 % qreal=[qreal q(:,i)]; %store the real solutions 0236 % end 0237 % end 0238 % 0239 % qreal = real(qreal);