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