%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_FANUC_MATE(robot, T)
Solves the inverse kinematic problem for the FANUC LR MATE 200iC 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_FANUC_MATE returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
fanuc=load_robot('fanuc', 'LR_MATE_200iC');
q = [0 0 0 0 0 0];
T = directkinematic(fanuc, q);
%Call the inversekinematic for this robot
qinv = inversekinematic(fanuc, T);
check that all of them are feasible solutions!
and every Ti equals T
for i=1:8,
Ti = directkinematic(fanuc, qinv(:,i))
end
See also DIRECTKINEMATIC.
Author:Juan Alejandro And�jar Cuenca & Israel Mira Ant�n
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_FANUC_MATE(robot, T) 0003 % Solves the inverse kinematic problem for the FANUC LR MATE 200iC 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_FANUC_MATE 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 % fanuc=load_robot('fanuc', 'LR_MATE_200iC'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(fanuc, q); 0018 % %Call the inversekinematic for this robot 0019 % qinv = inversekinematic(fanuc, T); 0020 % check that all of them are feasible solutions! 0021 % and every Ti equals T 0022 % for i=1:8, 0023 % Ti = directkinematic(fanuc, qinv(:,i)) 0024 % end 0025 % See also DIRECTKINEMATIC. 0026 % 0027 % Author:Juan Alejandro And�jar Cuenca & Israel Mira Ant�n 0028 % 0029 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0030 0031 % Copyright (C) 2012, by Juan Alejandro And�jar Cuenca & Israel Mira Ant�n 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 function q = inversekinematic_fanuc_mate(robot, T) 0048 0049 %initialize q, 0050 %eight possible solutions are generally feasible 0051 q=zeros(6,8); 0052 0053 %Evaluate the parameters 0054 d = eval(robot.DH.d); 0055 0056 %See geometry at the reference for this robot 0057 L6=d(6); 0058 0059 0060 %T= [ nx ox ax Px; 0061 % ny oy ay Py; 0062 % nz oz az Pz]; 0063 Px=T(1,4); 0064 Py=T(2,4); 0065 Pz=T(3,4); 0066 0067 %Compute the position of the wrist, being W the Z component of the end effector's system 0068 W = T(1:3,3); 0069 0070 % Pm: wrist position 0071 Pm = [Px Py Pz]' - L6*W; 0072 0073 %first joint, two possible solutions admited: 0074 % if q(1) is a solution, then q(1) + pi is also a solution 0075 q1=atan2(Pm(2), Pm(1)); 0076 0077 0078 %solve for q2 0079 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0080 0081 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0082 0083 %solve for q3 0084 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0085 0086 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0087 0088 0089 %Arrange solutions, there are 8 possible solutions so far. 0090 % if q1 is a solution, q1* = q1 + pi is also a solution. 0091 % For each (q1, q1*) there are two possible solutions 0092 % for q2 and q3 (namely, elbow up and elbow up solutions) 0093 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3), 0094 % there exist two more possible solutions for the last three joints, generally 0095 % called wrist up and wrist down solutions. For this reason, 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 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0100 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0101 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0102 0 0 0 0 0 0 0 0; 0103 0 0 0 0 0 0 0 0; 0104 0 0 0 0 0 0 0 0]; 0105 0106 %leave only the real part of the solutions 0107 q=real(q); 0108 0109 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0110 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0111 %step is avoided in this angle (the next line is commented). When solving 0112 %for the orientation, the solutions are normalized to the [-pi, pi] range 0113 %only for the theta4, theta5 and theta6 joints. 0114 0115 %normalize q to [-pi, pi] 0116 q(1,:) = normalize(q(1,:)); 0117 q(2,:) = normalize(q(2,:)); 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 qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up 0122 qtemp(4:6)=normalize(qtemp(4:6)); 0123 q(:,i)=qtemp; 0124 0125 qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up 0126 qtemp(4:6)=normalize(qtemp(4:6)); 0127 q(:,i+1)=qtemp; 0128 end 0129 0130 0131 0132 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0133 % solve for second joint theta2, two different 0134 % solutions are returned, corresponding 0135 % to elbow up and down solution 0136 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0137 function q2 = solve_for_theta2(robot, q, Pm) 0138 0139 %Evaluate the parameters 0140 d = eval(robot.DH.d); 0141 a = eval(robot.DH.a); 0142 0143 %See geometry 0144 L2=a(2); 0145 L3=d(4); 0146 0147 A3=a(3); % desfase 0148 0149 %given q1 is known, compute first DH transformation 0150 T01=dh(robot, q, 1); 0151 0152 %Eslab�n equivalente para simplificar el desfase 0153 l3 = sqrt (A3^2 + L3^2); 0154 0155 %Express Pm in the reference system 1, for convenience 0156 p1 = inv(T01)*[Pm; 1]; 0157 0158 r = sqrt(p1(1)^2 + p1(2)^2); 0159 0160 beta = atan2(-p1(2), p1(1)); 0161 gamma = (acos((L2^2+r^2-l3^2)/(2*r*L2))); 0162 0163 if ~isreal(gamma) 0164 disp('WARNING:inversekinematic_fanuc_mate: the point is not reachable for this configuration, imaginary solutions'); 0165 %gamma = real(gamma); 0166 end 0167 0168 %return two possible solutions 0169 %elbow up and elbow down 0170 %the order here is important and is coordinated with the function 0171 %solve_for_theta3 0172 q2(1) = pi/2 - beta - gamma; %elbow up 0173 q2(2) = pi/2 - beta + gamma; %elbow down 0174 0175 0176 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0177 % solve for third joint theta3, two different 0178 % solutions are returned, corresponding 0179 % to elbow up and down solution 0180 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0181 function q3 = solve_for_theta3(robot, q, Pm) 0182 0183 %Evaluate the parameters 0184 d = eval(robot.DH.d); 0185 a = eval(robot.DH.a); 0186 0187 %See geometry 0188 L2=a(2); 0189 L3=d(4); 0190 0191 A3= a(3); %desfase 0192 0193 %See geometry of the robot 0194 %compute L4 0195 l3 = sqrt(A3^2 + L3^2); 0196 0197 %the angle phi is fixed 0198 phi=acos((A3^2+l3^2-L3^2)/(2*A3*l3)); 0199 0200 %given q1 is known, compute first DH transformation 0201 T01=dh(robot, q, 1); 0202 0203 %Express Pm in the reference system 1, for convenience 0204 p1 = inv(T01)*[Pm; 1]; 0205 0206 r = sqrt(p1(1)^2 + p1(2)^2); 0207 0208 eta = (acos((L2^2 + l3^2 - r^2)/(2*L2*l3))); 0209 0210 if ~isreal(eta) 0211 disp('WARNING:inversekinematic_fanuc_mate: the point is not reachable for this configuration, imaginary solutions'); 0212 %eta = real(eta); 0213 end 0214 0215 %return two possible solutions 0216 %elbow up and elbow down solutions 0217 %the order here is important 0218 q3(1) = pi - phi - eta; 0219 q3(2) = pi - phi + eta; 0220 0221 0222 %remove complex solutions for q for the q1+pi solutions 0223 function qreal = arrange_solutions(q) 0224 qreal=q(:,1:4); 0225 0226 %sum along rows if any angle is complex, for any possible solutions, then v(i) is complex 0227 v = sum(q, 1); 0228 0229 for i=5:8, 0230 if isreal(v(i)) 0231 qreal=[qreal q(:,i)]; %store the real solutions 0232 end 0233 end 0234 0235 qreal = real(qreal);