%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_IRB760(robot, T)
Solves the inverse kinematic problem for the ABB IRB 760 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_IRB760 returns only 1 possible solution, thus,
Q is a 5x1 matrix where each column stores 5 feasible joint values.
Beware that the 4th axis is a combination of q(2) and q(3).
Example code:
>>abb=load_robot('ABB', 'IRB760');
>>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: Arturo Gil Aparicio
Universitas Miguel Hernandez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_IRB760(robot, T) 0003 % Solves the inverse kinematic problem for the ABB IRB 760 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_IRB760 returns only 1 possible solution, thus, 0010 % Q is a 5x1 matrix where each column stores 5 feasible joint values. 0011 % Beware that the 4th axis is a combination of q(2) and q(3). 0012 % 0013 % 0014 % Example code: 0015 % 0016 % >>abb=load_robot('ABB', 'IRB760'); 0017 % >>q = [0 0 0 0 0 0]; 0018 % >>T = directkinematic(abb, q); 0019 % %Call the inversekinematic for this robot 0020 % >>qinv = inversekinematic(abb, T); 0021 % check that all of them are feasible solutions! 0022 % and every Ti equals T 0023 % 0024 % for i=1:8, 0025 % Ti = directkinematic(abb, qinv(:,i)) 0026 % end 0027 % 0028 % See also DIRECTKINEMATIC. 0029 % 0030 % Author: Arturo Gil Aparicio 0031 % Universitas Miguel Hernandez, SPAIN. 0032 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0033 0034 % Copyright (C) 2012, by Arturo Gil Aparicio 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_irb760(robot, T) 0051 0052 %initialize q, 0053 %eight possible solutions are generally feasible 0054 q=zeros(6,8); 0055 0056 % %Evaluate the parameters 0057 a = eval(robot.DH.a); 0058 d = eval(robot.DH.d); 0059 L6=d(5); 0060 L4=a(4); 0061 0062 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 W = T(1:3,3); 0073 0074 % Pm: wrist position 0075 Pm = [Px Py Pz]';%' - L6*W; 0076 0077 %first joint, two possible solutions admited: 0078 % if q(1) is a solution, then q(1) + pi is also a solution 0079 q1=atan2(Pm(2), Pm(1)); 0080 0081 %compute the wrist, that, in this case is the origin of the 0082 % system X3Y3Z3 0083 Pm = [Px Py Pz]' - L6*W -L4*[cos(q1) sin(q1) 0]'; 0084 0085 %solve for q2 0086 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0087 %the other possible solution is q1 + pi 0088 %q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0089 0090 %solve for q3 0091 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0092 %solver for q3 for both cases 0093 %q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0094 0095 0096 %the next matrix doubles each column. For each two columns, two different 0097 %configurations for theta4, theta5 and theta6 will be computed. These 0098 %configurations are generally referred as wrist up and wrist down solution 0099 q = [q1 q1 ; 0100 q2_1(1) q2_1(2); 0101 q3_1(1) q3_1(2); 0102 -q2_1(1)-q3_1(1) -q2_1(2)-q3_1(2); 0103 0 0]; 0104 0105 %leave only the real part of the solutions 0106 q=real(q); 0107 0108 %normalize q to [-pi, pi] 0109 q(1,:) = normalize(q(1,:)); 0110 q(2,:) = normalize(q(2,:)); 0111 0112 %leave only the elbow up solution, which is the only physically feasible 0113 q=q(:,1); 0114 0115 T01=dh(robot, q, 1); 0116 T12=dh(robot, q, 2); 0117 T23=dh(robot, q, 3); 0118 T34=dh(robot, q, 4); 0119 0120 T04=T01*T12*T23*T34; 0121 0122 y4 = T04(1:3,2); 0123 x4 = T04(1:3,1); 0124 0125 sq5 = dot(T(1:3,1), y4); % Vector orientación n: T(1:3,1) 0126 cq5 = dot(T(1:3,1), x4); % Vector orientación o: T(1:3,2) 0127 q5 = atan2(sq5, cq5); 0128 0129 % Joint positions vector 0130 %q(4)=-q(2)-q(3); 0131 q(5)=q5; 0132 0133 0134 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0135 % solve for second joint theta2, two different 0136 % solutions are returned, corresponding 0137 % to elbow up and down solution 0138 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0139 function q2 = solve_for_theta2(robot, q, Pm) 0140 0141 %Evaluate the parameters 0142 theta = eval(robot.DH.theta); 0143 d = eval(robot.DH.d); 0144 a = eval(robot.DH.a); 0145 alpha = eval(robot.DH.alpha); 0146 0147 %See geometry 0148 L2=a(2); 0149 L3=a(3); 0150 0151 0152 %The inverse kinematic problem can be solved as in the IRB 140 (for example) 0153 0154 %given q1 is known, compute first DH transformation 0155 T01=dh(robot, q, 1); 0156 0157 %Express Pm in the reference system 1, for convenience 0158 p1 = inv(T01)*[Pm; 1]; 0159 0160 r = sqrt(p1(1)^2 + p1(2)^2); 0161 0162 beta = atan2(-p1(2), p1(1)); 0163 gamma = real(acos((L2^2+r^2-L3^2)/(2*r*L2))); 0164 0165 %return two possible solutions 0166 %elbow up and elbow down 0167 %the order here is important and is coordinated with the function 0168 %solve_for_theta3 0169 q2(1) = pi/2 - beta - gamma; %elbow up 0170 q2(2) = pi/2 - beta + gamma; %elbow down 0171 0172 0173 % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0174 % % % solve for third joint theta3, two different 0175 % % % solutions are returned, corresponding 0176 % % % to elbow up and down solution 0177 % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0178 % % function q3 = solve_for_theta3(robot, q, Pm) 0179 % % 0180 % % %Evaluate the parameters 0181 % % theta = eval(robot.DH.theta); 0182 % % d = eval(robot.DH.d); 0183 % % a = eval(robot.DH.a); 0184 % % alpha = eval(robot.DH.alpha); 0185 % % 0186 % % %See geometry 0187 % % L2=a(2); 0188 % % L3=d(4); 0189 % % A2=a(3); 0190 % % 0191 % % %See geometry of the robot 0192 % % %compute L4 0193 % % L4 = sqrt(A2^2 + L3^2); 0194 % % 0195 % % %the angle phi is fixed 0196 % % phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4)); 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 = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); 0207 % % 0208 % % %return two possible solutions 0209 % % %elbow up and elbow down solutions 0210 % % %the order here is important 0211 % % q3(1) = pi - phi- eta; 0212 % % q3(2) = pi - phi + eta; 0213 % 0214 % 0215 % %return two possible solutions 0216 % %elbow up and elbow down solutions 0217 % %the order here is important 0218 % q3(1) = pi/2 - eta; 0219 % q3(2) = eta - 3*pi/2; 0220 0221 0222 0223 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0224 % solve for third joint theta3, two different 0225 % solutions are returned, corresponding 0226 % to elbow up and down solution 0227 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0228 function q3 = solve_for_theta3(robot, q, Pm) 0229 0230 %Evaluate the parameters 0231 d = eval(robot.DH.d); 0232 a = eval(robot.DH.a); 0233 0234 %See geometry 0235 %The parameters are obtained, in this particular robot 0236 % from the a vector 0237 L2=a(2); 0238 L3=a(3); 0239 0240 %given q1 is known, compute first DH transformation 0241 T01=dh(robot, q, 1); 0242 0243 %Express Pm in the reference system 1, for convenience 0244 p1 = inv(T01)*[Pm; 1]; 0245 0246 r = sqrt(p1(1)^2 + p1(2)^2); 0247 0248 eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3))); 0249 0250 if ~isreal(eta) 0251 disp('WARNING:inversekinematic_irb140: the point is not reachable for this configuration, imaginary solutions'); 0252 %eta = real(eta); 0253 end 0254 0255 %return two possible solutions 0256 %elbow up and elbow down solutions 0257 %the order here is important 0258 q3(1) = pi/2 - eta; 0259 q3(2) = eta - 3*pi/2; 0260 0261 0262