%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_IRB1600X_140(robot, T)
Solves the inverse kinematic problem for the ABB IRB 1600X_140 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_IRB1600X_140 returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
>>abb=load_robot('ABB', 'IRB1600X_140');
>>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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_IRB1600X_140(robot, T) 0003 % Solves the inverse kinematic problem for the ABB IRB 1600X_140 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_IRB1600X_140 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', 'IRB1600X_140'); 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 % 0021 % check that all of them are feasible solutions! 0022 % and every Ti equals T 0023 % for i=1:8, 0024 % Ti = directkinematic(abb, qinv(:,i)) 0025 % end 0026 % See also DIRECTKINEMATIC. 0027 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0028 0029 % Copyright (C) 2012, by Arturo Gil Aparicio 0030 % 0031 % This file is part of ARTE (A Robotics Toolbox for Education). 0032 % 0033 % ARTE is free software: you can redistribute it and/or modify 0034 % it under the terms of the GNU Lesser General Public License as published by 0035 % the Free Software Foundation, either version 3 of the License, or 0036 % (at your option) any later version. 0037 % 0038 % ARTE is distributed in the hope that it will be useful, 0039 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0040 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0041 % GNU Lesser General Public License for more details. 0042 % 0043 % You should have received a copy of the GNU Leser General Public License 0044 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0045 0046 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0047 % 0048 % 0049 % A partir del inverskinematic.m proporcionado en el robot IRB140 hemos 0050 % construido el nuestro. 0051 % 0052 % 0053 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0054 0055 function [q] = inversekinematic_irb1600X_140(robot, T) 0056 0057 %initialize q, 0058 %eight possible solutions are generally feasible 0059 q=zeros(6,8); 0060 0061 %Carga el DH del robot 0062 theta = eval(robot.DH.theta); 0063 d = eval(robot.DH.d); 0064 a = eval(robot.DH.a); 0065 alpha = eval(robot.DH.alpha); 0066 0067 0068 %Toma los datos de la geometr�a del robot 0069 L1=d(1); 0070 L2=a(2); 0071 L3=d(4); 0072 L6=d(6); 0073 0074 A1 = a(1); 0075 0076 0077 %T= [ nx ox ax Px; 0078 % ny oy ay Py; 0079 % nz oz az Pz]; 0080 Px=T(1,4); 0081 Py=T(2,4); 0082 Pz=T(3,4); 0083 0084 %Computa la posici�n del extremo del robot, siendo W la componente Z del sistema 0085 %efector 0086 W = T(1:3,3); 0087 0088 % Pm: Posici�n del extremo del robot 0089 Pm = [Px Py Pz]' - L6*W; 0090 0091 %para q(1) hay dos posibles soluciones 0092 % si q(1) es una soluci�n, entonces q(1) + pi es tambi�n una soluci�n 0093 q1=atan2(Pm(2), Pm(1)); 0094 0095 0096 %soluci�n para q2 a partir de q1 y q1 + pi 0097 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0098 0099 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0100 0101 %soluci�n para q3 a partir de q1 y q1 + pi 0102 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0103 0104 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0105 0106 0107 % Si q1 es una soluci�n, q1* = q1 + pi es tambi�n una soluci�n 0108 % para cada (q1, q1*) hay dos posibles soluciones para q2 y q3. 0109 % Hasta ahora tenemos 4 posibles soluciones. 0110 % Existen dos posibles soluciones m�s para las tres �ltimas uniones, 0111 % llamadas mu�eca arriba y mu�eca abajo. Por eso, 0112 % la siguiente matriz dobla cada columna. Por cada dos columnas, dos 0113 % configuraciones para theta4, theta 5 y theta 6 ser�n calculadas. 0114 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0115 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0116 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0117 0 0 0 0 0 0 0 0; 0118 0 0 0 0 0 0 0 0; 0119 0 0 0 0 0 0 0 0]; 0120 0121 %leave only the real part of the solutions 0122 q=real(q); 0123 0124 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0125 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0126 %step is avoided in this angle (the next line is commented). When solving 0127 %for the orientation, the solutions are normalized to the [-pi, pi] range 0128 %only for the theta4, theta5 and theta6 joints. 0129 0130 %normalize q to [-pi, pi] 0131 q(1,:) = normalize(q(1,:)); 0132 q(2,:) = normalize(q(2,:)); 0133 0134 % solve for the last three joints 0135 % for any of the possible combinations (theta1, theta2, theta3) 0136 for i=1:2:size(q,2), 0137 % use solve_spherical_wrist2 for the particular orientation 0138 % of the systems in this ABB robot 0139 % use either the geometric or algebraic method. 0140 % the function solve_spherical_wrist2 is used due to the relative 0141 % orientation of the last three DH reference systems. 0142 0143 %use either one algebraic method or the geometric 0144 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up 0145 qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up 0146 qtemp(4:6)=normalize(qtemp(4:6)); 0147 q(:,i)=qtemp; 0148 0149 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down 0150 qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down 0151 qtemp(4:6)=normalize(qtemp(4:6)); 0152 q(:,i+1)=qtemp; 0153 end 0154 0155 0156 0157 0158 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0159 % resuelve para la segunda articulaci�n theta2, dos soluciones diferentes 0160 % son devueltas, codo arriba y codo abajo 0161 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0162 function q2 = solve_for_theta2(robot, q, Pm) 0163 0164 %Evaluaci�n de los parametros a partir del DH 0165 d = eval(robot.DH.d); 0166 a = eval(robot.DH.a); 0167 0168 %Toma la geometr�a correspondiente a la articulaci�n. 0169 L2=a(2); 0170 L3=d(4); 0171 0172 0173 %given q1 is known, compute first DH transformation 0174 T01=dh(robot, q, 1); 0175 0176 %Expresa Pm en el sistema de referencia 1 0177 p1 = inv(T01)*[Pm; 1]; 0178 0179 r = sqrt(p1(1)^2 + p1(2)^2); 0180 0181 beta = atan2(-p1(2), p1(1)); 0182 gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2))); 0183 0184 if ~isreal(gamma) 0185 disp('WARNING:inversekinematic_irb1600X_140: the point is not reachable for this configuration, imaginary solutions'); 0186 %gamma = real(gamma); 0187 end 0188 0189 %Devuelve dos posibles soluciones 0190 q2(1) = pi/2 - beta - gamma; %codo arriba 0191 q2(2) = pi/2 - beta + gamma; %codo abajo 0192 0193 0194 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0195 % resuelve para la tercera articulaci�n theta3, se devuelven dos posibles 0196 % soluciones, codo arriba y codo abajo 0197 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0198 function q3 = solve_for_theta3(robot, q, Pm) 0199 0200 %Evaluaci�n de los par�metros a partir del DH 0201 d = eval(robot.DH.d); 0202 a = eval(robot.DH.a); 0203 0204 %Toma la geometr�a 0205 L2=a(2); 0206 L3=d(4); 0207 0208 %Conociendo q1, calcula la primera transformaci�n DH 0209 %given q1 is known, compute first DH transformation 0210 T01=dh(robot, q, 1); 0211 0212 %Expresa Pm en el sistema de referencia 1 0213 p1 = inv(T01)*[Pm; 1]; 0214 0215 r = sqrt(p1(1)^2 + p1(2)^2); 0216 0217 eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3))); 0218 0219 if ~isreal(eta) 0220 disp('WARNING:inversekinematic_irb1600X_140: the point is not reachable for this configuration, imaginary solutions'); 0221 %eta = real(eta); 0222 end 0223 0224 %Devuelve dos posibles soluciones, codo arriba y codo abajo, el orden es 0225 %importante 0226 q3(1) = pi/2 - eta; 0227 q3(2) = eta - 3*pi/2; 0228 0229