%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_IRB4400(robot, T)
Solves the inverse kinematic problem for the ABB IRB 4400 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_IRB4400 returns 4 possible solutions, thus,
Q is a 6x4 matrix where each column stores 6 feasible joint values.
Example code:
abb=load_robot('abb', 'IRB4400');
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:4,
Ti = directkinematic(abb, qinv(:,i))
end
See also DIRECTKINEMATIC.
Author: Arturo Gil Aparicio
Universitas Miguel Hernandez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_IRB4400(robot, T) 0003 % Solves the inverse kinematic problem for the ABB IRB 4400 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_IRB4400 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 % abb=load_robot('abb', 'IRB4400'); 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:4, 0023 % Ti = directkinematic(abb, 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 is part of ARTE (A Robotics Toolbox for Education). 0035 % 0036 % ARTE is free software: you can redistribute it and/or modify 0037 % it under the terms of the GNU Lesser General Public License as published by 0038 % the Free Software Foundation, either version 3 of the License, or 0039 % (at your option) any later version. 0040 % 0041 % ARTE is distributed in the hope that it will be useful, 0042 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0043 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0044 % GNU Lesser General Public License for more details. 0045 % 0046 % You should have received a copy of the GNU Leser General Public License 0047 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0048 function q = inversekinematic_irb4400(robot, T) 0049 0050 %initialize q, 0051 %eight possible solutions are generally feasible 0052 %this robot, due to physical restrictions does allow only 2 0053 %possible solutions (called wrist up and wrist down) 0054 q=zeros(6,1); 0055 0056 % %Evaluate the parameters 0057 % theta = eval(robot.DH.theta); 0058 d = eval(robot.DH.d); 0059 L6=d(6); 0060 0061 0062 %T= [ nx ox ax Px; 0063 % ny oy ay Py; 0064 % nz oz az Pz]; 0065 Px=T(1,4); 0066 Py=T(2,4); 0067 Pz=T(3,4); 0068 0069 %Compute the position of the wrist, being W the Z component of the end effector's system 0070 W = T(1:3,3); 0071 0072 % Pm: wrist position 0073 Pm = [Px Py Pz]' - L6*W; 0074 0075 %first joint, two possible solutions admited: 0076 % if q(1) is a solution, then q(1) + pi is also a solution 0077 q1=atan2(Pm(2), Pm(1)); 0078 0079 0080 %solve for q2 0081 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0082 %the other possible solution is q1 + pi 0083 %q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0084 0085 %solve for q3 0086 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0087 %solver for q3 for both cases 0088 %q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0089 0090 0091 %Arrange solutions, there are 4 possible solutions so far, being 0092 % each column repeated twice. For each triplet (theta1, theta2, theta3), 0093 % there exist two possible solutions for the last three joints, generally 0094 % called wrist up and wrist down solutions 0095 % NOTE: so far there exist 4 possible solutions 0096 % q = [q1 q1 q1+pi q1+pi; 0097 % q2_1(1) q2_1(2) q2_2(1) q2_2(2); 0098 % q3_1(1) q3_1(2) q3_2(1) q3_2(2); 0099 % 0 0 0 0; 0100 % 0 0 0 0; 0101 % 0 0 0 0]; 0102 q = [q1 q1; 0103 q2_1(1) q2_1(1); 0104 q3_1(1) q3_1(1); 0105 0 0 ; 0106 0 0 ; 0107 0 0]; 0108 0109 0110 0111 %leave only the real part of the solutions 0112 q=real(q); 0113 0114 %normalize q to [-pi, pi] 0115 q(1,:) = normalize(q(1,:)); 0116 q(2,:) = normalize(q(2,:)); 0117 0118 % solve for the last three joints 0119 % for any of the possible combinations (theta1, theta2, theta3) 0120 for i=1:2:size(q,2), 0121 % use solve_spherical_wrist2 for the particular orientation 0122 % of the systems in this ABB robot 0123 % use either the geometric or algebraic method. 0124 % the function solve_spherical_wrist2 is used due to the relative 0125 % orientation of the last three DH reference systems. 0126 0127 %use either one algebraic method or the geometric 0128 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up 0129 qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up 0130 qtemp(4:6)=normalize(qtemp(4:6)); 0131 q(:,i)=qtemp; 0132 0133 %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down 0134 qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down 0135 qtemp(4:6)=normalize(qtemp(4:6)); 0136 q(:,i+1)=qtemp; 0137 end 0138 0139 0140 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0141 % solve for second joint theta2, two different 0142 % solutions are returned, corresponding 0143 % to elbow up and down solution 0144 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0145 function q2 = solve_for_theta2(robot, q, Pm) 0146 0147 %Evaluate the parameters 0148 %theta = eval(robot.DH.theta); 0149 d = eval(robot.DH.d); 0150 a = eval(robot.DH.a); 0151 %alpha = eval(robot.DH.alpha); 0152 0153 %See geometry 0154 L2=a(2); 0155 L3=d(4); 0156 A2=a(3); 0157 0158 %See geometry of the robot 0159 %compute L4 0160 L4 = sqrt(A2^2 + L3^2); 0161 0162 %The inverse kinematic problem can be solved as in the IRB 140 (for example) 0163 0164 %given q1 is known, compute first DH transformation 0165 T01=dh(robot, q, 1); 0166 0167 %Express Pm in the reference system 1, for convenience 0168 p1 = inv(T01)*[Pm; 1]; 0169 0170 r = sqrt(p1(1)^2 + p1(2)^2); 0171 0172 beta = atan2(-p1(2), p1(1)); 0173 gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2))); 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; %imposible este caso 0181 0182 q2(1) = pi/2 - beta - gamma; %elbow up, only this solution 0183 %is feasible in this robot 0184 %q2(2) = pi/2 - beta + gamma; %elbow down 0185 0186 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0187 % solve for third joint theta3, two different 0188 % solutions are returned, corresponding 0189 % to elbow up and down solution 0190 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0191 function q3 = solve_for_theta3(robot, q, Pm) 0192 0193 %Evaluate the parameters 0194 %theta = eval(robot.DH.theta); 0195 d = eval(robot.DH.d); 0196 a = eval(robot.DH.a); 0197 %alpha = eval(robot.DH.alpha); 0198 0199 %See geometry 0200 L2=a(2); 0201 L3=d(4); 0202 A2=a(3); 0203 0204 %See geometry of the robot 0205 %compute L4 0206 L4 = sqrt(A2^2 + L3^2); 0207 0208 %the angle phi is fixed 0209 phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4)); 0210 0211 %given q1 is known, compute first DH transformation 0212 T01=dh(robot, q, 1); 0213 0214 %Express Pm in the reference system 1, for convenience 0215 p1 = inv(T01)*[Pm; 1]; 0216 0217 r = sqrt(p1(1)^2 + p1(2)^2); 0218 0219 eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); 0220 0221 %return two possible solutions 0222 %elbow up and elbow down solutions 0223 %the order here is important 0224 q3(1) = pi - phi- eta; 0225 %q3(2) = pi - phi + eta; 0226