%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_IRB1600_(robot, T)
Solves the inverse kinematic problem for the ABB IRB 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_IRB1600 returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
abb=load_robot('abb', 'IRB140');
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:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_IRB1600_(robot, T) 0003 % Solves the inverse kinematic problem for the ABB IRB 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_IRB1600 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', 'IRB140'); 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 % Author: 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 function [q, valid] = inversekinematic_abb_irb1600_X145_m2004(robot, T) 0046 0047 %initialize q, 0048 %eight possible solutions are generally feasible 0049 q=zeros(6,8); 0050 0051 %Evaluate the parameters 0052 theta = eval(robot.DH.theta); 0053 d = eval(robot.DH.d); 0054 a = eval(robot.DH.a); 0055 alpha = eval(robot.DH.alpha); 0056 0057 0058 %See geometry at the reference for this robot 0059 L1=d(1); 0060 L2=a(2); 0061 L3=d(4); 0062 L6=d(6); 0063 0064 A1 = a(1); 0065 0066 %T= [ nx ox ax Px; 0067 % ny oy ay Py; 0068 % nz oz az Pz]; 0069 Px=T(1,4); 0070 Py=T(2,4); 0071 Pz=T(3,4); 0072 0073 %Compute the position of the wrist, being W the Z component of the end effector's system 0074 W = T(1:3,3); 0075 0076 % Pm: wrist position 0077 Pm = [Px Py Pz]' - L6*W; 0078 0079 %first joint, two possible solutions admited: 0080 % if q(1) is a solution, then q(1) + pi is also a solution 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 0113 %leave only the real part of the solutions 0114 q=real(q); 0115 0116 %normalize q to [-pi, pi] 0117 q(1,:) = normalize(q(1,:)); 0118 q(2,:) = normalize(q(2,:)); 0119 0120 % solve for the last three joints 0121 % for any of the possible combinations (theta1, theta2, theta3) 0122 for i=1:2:size(q,2), 0123 % use solve_spherical_wrist2 for the particular orientation 0124 % of the systems in this ABB robot 0125 % use either the geometric or algebraic method. 0126 % the function solve_spherical_wrist2 is used due to the relative 0127 % orientation of the last three DH reference systems. 0128 0129 %use either one algebraic method or the geometric 0130 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up 0131 qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up 0132 qtemp(4:6)=normalize(qtemp(4:6)); 0133 q(:,i)=qtemp; 0134 0135 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down 0136 qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down 0137 qtemp(4:6)=normalize(qtemp(4:6)); 0138 q(:,i+1)=qtemp; 0139 end 0140 0141 0142 0143 0144 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0145 % solve for second joint theta2, two different 0146 % solutions are returned, corresponding 0147 % to elbow up and down solution 0148 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0149 function q2 = solve_for_theta2(robot, q, Pm) 0150 0151 %Evaluate the parameters 0152 d = eval(robot.DH.d); 0153 a = eval(robot.DH.a); 0154 0155 %See geometry 0156 L2=a(2); 0157 L3=d(4); 0158 0159 %given q1 is known, compute first DH transformation 0160 T01=dh(robot, q, 1); 0161 0162 %Express Pm in the reference system 1, for convenience 0163 p1 = inv(T01)*[Pm; 1]; 0164 0165 r = sqrt(p1(1)^2 + p1(2)^2); 0166 0167 beta = atan2(-p1(2), p1(1)); 0168 gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2))); 0169 0170 if ~isreal(gamma) 0171 disp('WARNING:inversekinematic_irb1600_X145: the point is not reachable for this configuration, imaginary solutions'); 0172 %gamma = real(gamma); 0173 end 0174 0175 %return two possible solutions 0176 %elbow up and elbow down 0177 %the order here is important and is coordinated with the function 0178 %solve_for_theta3 0179 q2(1) = pi/2 - beta - gamma; %elbow up 0180 q2(2) = pi/2 - beta + gamma; %elbow down 0181 0182 0183 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0184 % solve for third joint theta3, two different 0185 % solutions are returned, corresponding 0186 % to elbow up and down solution 0187 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0188 function q3 = solve_for_theta3(robot, q, Pm) 0189 0190 %Evaluate the parameters 0191 d = eval(robot.DH.d); 0192 a = eval(robot.DH.a); 0193 0194 %See geometry 0195 L2=a(2); 0196 L3=d(4); 0197 0198 %given q1 is known, compute first DH transformation 0199 T01=dh(robot, q, 1); 0200 0201 %Express Pm in the reference system 1, for convenience 0202 p1 = inv(T01)*[Pm; 1]; 0203 0204 r = sqrt(p1(1)^2 + p1(2)^2); 0205 0206 eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3))); 0207 0208 if ~isreal(eta) 0209 disp('WARNING:inversekinematic_irb140: the point is not reachable for this configuration, imaginary solutions'); 0210 %eta = real(eta); 0211 end 0212 0213 %return two possible solutions 0214 %elbow up and elbow down solutions 0215 %the order here is important 0216 q3(1) = pi/2 - eta; 0217 q3(2) = eta - 3*pi/2; 0218 0219 0220