%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_KUKA_KR90_R3100_EXTRA(robot, T)
Solves the inverse kinematic problem for the KUKA KR90 R3100 EXTRA 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__KUKA_KR90_R3100_EXTRA returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
robot=load_robot('kuka', 'KR90_R3100_EXTRA');
q = [0 0 0 0 0 0];
T = directkinematic(robot, q);
%Call the inversekinematic for this robot
qinv = inversekinematic(robot, T);
check that all of them are feasible solutions!
and every Ti equals T
for i=1:8,
Ti = directkinematic(robot, qinv(:,i))
end
See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_KUKA_KR90_R3100_EXTRA(robot, T) 0003 % Solves the inverse kinematic problem for the KUKA KR90 R3100 EXTRA 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__KUKA_KR90_R3100_EXTRA 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 % robot=load_robot('kuka', 'KR90_R3100_EXTRA'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(robot, q); 0018 % %Call the inversekinematic for this robot 0019 % qinv = inversekinematic(robot, T); 0020 % check that all of them are feasible solutions! 0021 % and every Ti equals T 0022 % for i=1:8, 0023 % Ti = directkinematic(robot, qinv(:,i)) 0024 % end 0025 % See also DIRECTKINEMATIC. 0026 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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 = inversekinematic_kuka_KR90_R3100_EXTRA(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 L6=abs(d(6)); 0055 0056 0057 %T= [ nx ox ax Px; 0058 % ny oy ay Py; 0059 % nz oz az Pz]; 0060 Px=T(1,4); 0061 Py=T(2,4); 0062 Pz=T(3,4); 0063 0064 %Compute the position of the wrist, being W the Z component of the end effector's system 0065 W = T(1:3,3); 0066 0067 % Pm: wrist position 0068 Pm = [Px Py Pz]' - L6*W; 0069 0070 %first joint, two possible solutions admited: 0071 % if q(1) is a solution, then q(1) + pi is also a solution 0072 q1=atan2(Pm(2), Pm(1)); 0073 %q1=atan2(Py, Px); 0074 0075 0076 %solve for q2 0077 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0078 %the other possible solution is q1 + pi 0079 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0080 0081 %solve for q3 0082 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0083 %solver for q3 for both cases 0084 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0085 0086 0087 %the next matrix doubles each column. For each two columns, two different 0088 %configurations for theta4, theta5 and theta6 will be computed. These 0089 %configurations are generally referred as wrist up and wrist down solution 0090 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0091 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0092 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0093 0 0 0 0 0 0 0 0; 0094 0 0 0 0 0 0 0 0; 0095 0 0 0 0 0 0 0 0]; 0096 0097 0098 %leave only the real part of the solutions 0099 q=real(q); 0100 0101 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0102 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0103 %step is avoided in this angle (the next line is commented). When solving 0104 %for the orientation, the solutions are normalized to the [-pi, pi] range 0105 %only for the theta4, theta5 and theta6 joints. 0106 0107 %normalize q to [-pi, pi] 0108 q(1,:) = normalize(q(1,:)); 0109 q(2,:) = normalize(q(2,:)); 0110 % solve for the last three joints 0111 % for any of the possible combinations (theta1, theta2, theta3) 0112 for i=1:2:size(q,2), 0113 qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up 0114 qtemp(4:6)=normalize(qtemp(4:6)); 0115 q(:,i)=qtemp; 0116 0117 qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up 0118 qtemp(4:6)=normalize(qtemp(4:6)); 0119 q(:,i+1)=qtemp; 0120 end 0121 0122 0123 0124 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0125 % solve for second joint theta2, two different 0126 % solutions are returned, corresponding 0127 % to elbow up and down solution 0128 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0129 function q2 = solve_for_theta2(robot, q, Pm) 0130 0131 %Evaluate the parameters 0132 theta = eval(robot.DH.theta); 0133 d = eval(robot.DH.d); 0134 a = eval(robot.DH.a); 0135 alpha = eval(robot.DH.alpha); 0136 0137 %See geometry 0138 L2=abs(a(2)); 0139 L3=abs(d(4)); 0140 A2 = abs(a(3)); 0141 0142 %See geometry of the robot 0143 %compute L4 0144 Lx = sqrt(A2^2 + L3^2); 0145 0146 %The inverse kinematic problem can be solved as in the IRB 140 (for example) 0147 0148 %given q1 is known, compute first DH transformation 0149 T01=dh(robot, q, 1); 0150 0151 %Express Pm in the reference system 1, for convenience 0152 p1 = inv(T01)*[Pm; 1]; 0153 0154 r = sqrt(p1(1)^2 + p1(2)^2); 0155 0156 beta = atan2(-p1(2), p1(1)); 0157 gamma = real(acos((L2^2+r^2-Lx^2)/(2*r*L2))); 0158 0159 %return two possible solutions 0160 %elbow up and elbow down 0161 %the order here is important and is coordinated with the function 0162 %solve_for_theta3 0163 q2(1) = pi/2 - beta - gamma; %elbow up 0164 q2(2) = pi/2 - beta + gamma; %elbow down 0165 0166 0167 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0168 % solve for third joint theta3, two different 0169 % solutions are returned, corresponding 0170 % to elbow up and down solution 0171 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0172 function q3 = solve_for_theta3(robot, q, Pm) 0173 0174 %Evaluate the parameters 0175 d = eval(robot.DH.d); 0176 a = eval(robot.DH.a); 0177 0178 %See geometry 0179 L2=abs(a(2)); 0180 L3=abs(d(4)); 0181 0182 A2 = abs(a(3)); 0183 0184 %See geometry of the robot 0185 %compute L4 0186 Lx = sqrt(A2^2 + L3^2); 0187 0188 %the angle phi is fixed 0189 phi=acos((A2^2+Lx^2-L3^2)/(2*A2*Lx)); 0190 0191 %given q1 is known, compute first DH transformation 0192 T01=dh(robot, q, 1); 0193 0194 %Express Pm in the reference system 1, for convenience 0195 p1 = inv(T01)*[Pm; 1]; 0196 0197 r = sqrt(p1(1)^2 + p1(2)^2); 0198 0199 eta = real(acos((L2^2 + Lx^2 - r^2)/(2*L2*Lx))); 0200 0201 %return two possible solutions 0202 %elbow up and elbow down solutions 0203 %the order here is important 0204 q3(1) = atan(L3/ A2)-eta; 0205 q3(2) = atan(L3/ A2)+ eta;