%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_KR_1000_1300_TITAN(robot, T)
Solves the inverse kinematic problem for the KUKA KR 1000 1300 TITAN 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_KR_1000_1300_TITAN returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
robot=load_robot('kuka', 'KR_1000_1300_TITAN');
q = [0 0 0 0 0 0];
T = directkinematic(robot, 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:8,
Ti = directkinematic(robot, qinv(:,i))
end
See also DIRECTKINEMATIC.
Authors: Javier Martinez Gonzalez
Jose Francisco Munoz Sempere
Silvia Carretero Monasor
Marcos Gomez Parres
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Copyright (C) 2012, by Arturo Gil Aparicio
This file is part of ARTE (A Robotics Toolbox for Education).
ARTE is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
ARTE is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Leser General Public License
along with ARTE. If not, see <http://www.gnu.org/licenses/>.0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_KR_1000_1300_TITAN(robot, T) 0003 % Solves the inverse kinematic problem for the KUKA KR 1000 1300 TITAN 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_KR_1000_1300_TITAN 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 % robot=load_robot('kuka', 'KR_1000_1300_TITAN'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(robot, 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:8, 0023 % Ti = directkinematic(robot, qinv(:,i)) 0024 % end 0025 % See also DIRECTKINEMATIC. 0026 % Authors: Javier Martinez Gonzalez 0027 % Jose Francisco Munoz Sempere 0028 % Silvia Carretero Monasor 0029 % Marcos Gomez Parres 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 0048 function q = inversekinematic_KR_1000_1300_TITAN(robot, T) 0049 0050 %initialize q, 0051 %eight possible solutions are generally feasible 0052 q=zeros(6,8); 0053 0054 %Evaluate the parameters 0055 theta = eval(robot.DH.theta); 0056 d = eval(robot.DH.d); 0057 a = eval(robot.DH.a); 0058 alpha = eval(robot.DH.alpha); 0059 0060 L6=d(6); %Distancia de la mu�eca al efector final. 0061 0062 A1 = a(1); 0063 0064 %T= [ nx ox ax Px; 0065 % ny oy ay Py; 0066 % nz oz az Pz]; 0067 Px=T(1,4); 0068 Py=T(2,4); 0069 Pz=T(3,4); 0070 0071 %Compute the position of the wrist, being W the Z component of the end effector's system 0072 %Tomamos el vector "a" 0073 W = T(1:3,3); %ax, ay, az 0074 0075 % Pm: wrist position (Posici�n de la mu�eca) 0076 Pm = [Px Py Pz]' - L6*W; 0077 0078 %first joint, two possible solutions admited: 0079 % if q(1) is a solution, then q(1) + pi is also a solution 0080 %Obtenemos "theta 1" mediante m�todos geom�tricos. 0081 q1=atan2(-Pm(2), Pm(1)); 0082 0083 0084 %solve for q2 0085 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0086 0087 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0088 0089 %solve for q3 0090 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0091 0092 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0093 0094 0095 %Arrange solutions, there are 8 possible solutions so far. 0096 % if q1 is a solution, q1* = q1 + pi is also a solution. 0097 % For each (q1, q1*) there are two possible solutions 0098 % for q2 and q3 (namely, elbow up and elbow up solutions) 0099 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3), 0100 % there exist two more possible solutions for the last three joints, generally 0101 % called wrist up and wrist down solutions. For this reason, 0102 %the next matrix doubles each column. For each two columns, two different 0103 %configurations for theta4, theta5 and theta6 will be computed. These 0104 %configurations are generally referred as wrist up and wrist down solution 0105 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0106 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0107 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0108 0 0 0 0 0 0 0 0; 0109 0 0 0 0 0 0 0 0; 0110 0 0 0 0 0 0 0 0]; 0111 0112 %At this point, we want to asure, that, at least, the function returns 0113 %4 real solutions q(1:4). If any of the solutions q(1:4) is complex, only 0114 %the real part will be returned. If any of the solutions q(5:8) is complex, 0115 %it will not be considered and removed 0116 q = arrange_solutions(q); 0117 0118 0119 %leave only the real part of the solutions 0120 q=real(q); 0121 0122 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0123 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0124 %step is avoided in this angle (the next line is commented). When solving 0125 %for the orientation, the solutions are normalized to the [-pi, pi] range 0126 %only for the theta4, theta5 and theta6 joints. 0127 0128 %normalize q to [-pi, pi] 0129 q(1,:) = normalize(q(1,:)); 0130 q(2,:) = normalize(q(2,:)); 0131 0132 % % solve for the last three joints 0133 % % for any of the possible combinations (theta1, theta2, theta3) 0134 % %Please note the special orientation of the wrist of this robot. In this 0135 % %case, we employ an 'ad hoc' function to solve for the final orientaion 0136 for i=1:2:size(q,2), 0137 qtemp = solve_for_last_three_joints(robot, q(:,i), T, 1); %wrist up 0138 qtemp(4:6)=normalize(qtemp(4:6)); 0139 q(:,i)=qtemp; 0140 0141 qtemp = solve_for_last_three_joints(robot, q(:,i), T, -1); %wrist down 0142 qtemp(4:6)=normalize(qtemp(4:6)); 0143 q(:,i+1)=qtemp; 0144 end 0145 0146 0147 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0148 % solve for second joint theta2, two different 0149 % solutions are returned, corresponding 0150 % to elbow up and down solution 0151 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0152 function q2 = solve_for_theta2(robot, q, Pm) 0153 0154 %Evaluate the parameters 0155 d = eval(robot.DH.d); 0156 a = eval(robot.DH.a); 0157 0158 %See geometry 0159 %Par�metros con los que calculamos "theta 2" y "theta 3", tambi�n mediante 0160 %m�todos geom�tricos. 0161 %Tenemos en cuenta el desfase de 65mm entre los centros de los sistemas 0162 %de referencia de los eslabones 2 y 3. 0163 L2=abs(a(2)); 0164 L3=abs(d(4)); 0165 A2 = abs(a(3)); %ESTE ELEMENTO ES EL DESFASE CITADO 0166 %given q1 is known, compute first DH transformation 0167 T01=dh(robot, q, 1); 0168 0169 L4 = sqrt(A2^2 + L3^2); %LONGITUD DEL ESLAB�N TENIENDO EN CUENTA EL DESFASE 0170 %(DISTANCIA REAL ENTRE MU�ECA Y ESLAB�N 2) 0171 %Express Pm in the reference system 1, for convenience 0172 p1 = inv(T01)*[Pm; 1]; 0173 0174 r = sqrt(p1(1)^2 + p1(2)^2); %r ES LA DISTANCIA DEL SIST. 1 A LA MU�ECA 0175 0176 beta = atan2(-p1(2), p1(1)); %BETA=ARCTG(-Y/X) 0177 gamma = (acos((L2^2+r^2-L4^2)/(2*r*L2))); %TEOREMA DEL COSENO 0178 0179 if ~isreal(gamma) 0180 disp('WARNING:inversekinematic_KUKA_1000_1300_TITAN: the point is not reachable for this configuration, imaginary solutions'); 0181 %gamma = real(gamma); 0182 end 0183 0184 %return two possible solutions 0185 %elbow up and elbow down 0186 %the order here is important and is coordinated with the function 0187 %solve_for_theta 0188 q2(1) = pi/2-gamma-beta; %elbow up (CODO ARRIBA) 0189 q2(2) = pi/2+gamma-beta; %elbow down (CODO ABAJO) 0190 %DEBEMOS A�ADIR UN �NGULO DE PI/2 DEBIDO A DESFASES EN NUESTROS SISTEMAS DE 0191 %REFERENCIA 0192 0193 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0194 % solve for third joint theta3, two different 0195 % solutions are returned, corresponding 0196 % to elbow up and down solution 0197 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0198 function q3 = solve_for_theta3(robot, q, Pm) 0199 0200 %Evaluate the parameters 0201 d = eval(robot.DH.d); 0202 a = eval(robot.DH.a); 0203 0204 %See geometry 0205 L2=abs(a(2)); 0206 L3=abs(d(4)); 0207 A2 = abs(a(3)); 0208 0209 L4 = sqrt(A2^2 + L3^2); %VOLVEMOS A TENER EN CUENTA EL DESFASE 0210 0211 phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4)); %TEOMERA DEL COSENO 0212 %PHI, ES EL �NGULO QUE FORMAN LAS L�NEA QUE DEFINEN EL DESFASE ENTRE 2 Y 3, 0213 %Y EL PROPIO ESLAB�N 4 0214 %given q1 is known, compute first DH transformation 0215 T01=dh(robot, q, 1); 0216 0217 %Express Pm in the reference system 1, for convenience 0218 p1 = inv(T01)*[Pm; 1]; 0219 0220 r = sqrt(p1(1)^2 + p1(2)^2); %DE NUEVO, DISTANCIA ENTRE SIST.1 Y MU�ECA 0221 0222 eta = (acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); %TEOREMA DEL COSENO 0223 %ETA, ES EL �NGULO REAL ENTRE LOS ESLABONES 3 Y 4 0224 0225 if ~isreal(eta) 0226 disp('WARNING:inversekinematic_KUKA_1000_1300_TITAN: the point is not reachable for this configuration, imaginary solutions'); 0227 %eta = real(eta); 0228 end 0229 0230 %return two possible solutions 0231 %elbow up and elbow down solutions 0232 %the order here is important 0233 q3(1) = pi - phi - eta; %CODO ARRIBA 0234 q3(2) = pi - phi + eta; %CODO ABAJO 0235 %SE A�ADEN LOS �NGULOS "PI" EN AMBAS SOLUCIONES DEBIDO A DESFASES ENTRE 0236 %NUESTROS SISTEMAS DE REFERENCIA. 0237 0238 % Solve for the last three joints asuming an spherical wrist 0239 function q = solve_for_last_three_joints(robot, q, T, wrist) 0240 0241 % T is the noa matrix defining the position/orientation of the end 0242 % effector's reference system 0243 vx6=T(1:3,1); 0244 vz6=T(1:3,3); 0245 vz5=T(1:3,3); % The vector a T(1:3,3) is coincident with z5 0246 0247 % Obtain the position and orientation of the system 3 0248 % using the already computed joints q1, q2 and q3 0249 T01=dh(robot, q, 1); 0250 T12=dh(robot, q, 2); 0251 T23=dh(robot, q, 3); 0252 T03=T01*T12*T23; 0253 0254 vx3=T03(1:3,1); 0255 vy3=T03(1:3,2); 0256 vz3=T03(1:3,3); 0257 0258 % find z4 normal to the plane formed by z3 and a 0259 vz4=cross(vz3, vz6); % end effector's vector a: T(1:3,3) 0260 0261 % in case of degenerate solution, 0262 % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution 0263 if norm(vz4) <= 0.00001 0264 if wrist == 1 %wrist up 0265 q(4)=0; 0266 else 0267 q(4)=-pi; %wrist down 0268 end 0269 else 0270 %this is the normal and most frequent solution 0271 cosq4=wrist*dot(-vy3,vz4); 0272 sinq4=wrist*dot(vx3,vz4); 0273 q(4)=atan2(sinq4, cosq4); 0274 end 0275 %propagate the value of q(4) to compute the system 4 0276 T34=dh(robot, q, 4); 0277 T04=T03*T34; 0278 vx4=T04(1:3,1); 0279 vy4=T04(1:3,2); 0280 0281 % solve for q5 0282 cosq5=dot(vx4,vz5); 0283 sinq5=dot(vy4,vz5); 0284 q(5)=atan2(sinq5, cosq5); 0285 0286 %propagate now q(5) to compute T05 0287 T45=dh(robot, q, 5); 0288 T05=T04*T45; 0289 vx5=T05(1:3,1); 0290 vy5=T05(1:3,2); 0291 0292 % solve for q6 0293 cosq6=dot(vx6,vx5); 0294 sinq6=dot(vx6,vy5); 0295 q(6)=atan2(sinq6, cosq6); 0296 0297 0298 %remove complex solutions for q for the q1+pi solutions 0299 function qreal = arrange_solutions(q) 0300 qreal=q(:,1:4); 0301 0302 %sum along rows if any angle is complex, for any possible solutions, then v(i) is complex 0303 v = sum(q, 1); 0304 0305 for i=5:8, 0306 if isreal(v(i)) 0307 qreal=[qreal q(:,i)]; %store the real solutions 0308 end 0309 end 0310 0311 qreal = real(qreal);