%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_rx170bl(robot, T)
Solves the inverse kinematic problem for the Staubli RX170BL 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_RX170BL returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
robot=load_robot('staubli', 'RX170BL');
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_rx170bl(robot, T) 0003 % Solves the inverse kinematic problem for the Staubli RX170BL 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_RX170BL 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('staubli', 'RX170BL'); 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_rx170bl(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 a = eval(robot.DH.a); 0055 alpha = eval(robot.DH.alpha); 0056 0057 0058 %See geometry at the reference for this robot 0059 L6=abs(d(6)); 0060 0061 A1 = a(1); 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 0082 %solve for q2 0083 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0084 0085 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0086 0087 %solve for q3 0088 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0089 0090 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0091 0092 0093 %Arrange solutions, there are 8 possible solutions so far. 0094 % if q1 is a solution, q1* = q1 + pi is also a solution. 0095 % For each (q1, q1*) there are two possible solutions 0096 % for q2 and q3 (namely, elbow up and elbow up solutions) 0097 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3), 0098 % there exist two more possible solutions for the last three joints, generally 0099 % called wrist up and wrist down solutions. For this reason, 0100 %the next matrix doubles each column. For each two columns, two different 0101 %configurations for theta4, theta5 and theta6 will be computed. These 0102 %configurations are generally referred as wrist up and wrist down solution 0103 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0104 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0105 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0106 0 0 0 0 0 0 0 0; 0107 0 0 0 0 0 0 0 0; 0108 0 0 0 0 0 0 0 0]; 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 0119 0120 % solve for the last three joints 0121 % for any of the possible combinations (theta1, theta2, theta3) 0122 for i=1:2:size(q,2), 0123 qtemp = solve_spherical_wrist_rx170bl(robot, q(:,i), T, 1); %wrist up 0124 qtemp(4:6)=normalize(qtemp(4:6)); 0125 q(:,i)=qtemp; 0126 0127 qtemp = solve_spherical_wrist_rx170bl(robot, q(:,i), T, -1); %wrist down 0128 qtemp(4:6)=normalize(qtemp(4:6)); 0129 q(:,i+1)=qtemp; 0130 end 0131 0132 0133 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0134 % solve for second joint theta2, two different 0135 % solutions are returned, corresponding 0136 % to elbow up and down solution 0137 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0138 function q2 = solve_for_theta2(robot, q, Pm) 0139 0140 %Evaluate the parameters 0141 d = eval(robot.DH.d); 0142 a = eval(robot.DH.a); 0143 0144 %See geometry 0145 L2=abs(a(2)); 0146 L3=abs(d(4)); 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 =(acos((L2^2+r^2-L3^2)/(2*r*L2))); 0158 0159 if ~isreal(gamma) 0160 disp('WARNING:inversekinematic_rx170bl: the point is not reachable for this configuration, imaginary solutions'); 0161 %gamma = real(gamma); 0162 end 0163 0164 %return two possible solutions 0165 %elbow up and elbow down 0166 %the order here is important and is coordinated with the function 0167 %solve_for_theta3 0168 q2(1) = pi/2 - beta - gamma; %elbow up 0169 q2(2) = pi/2 - beta + gamma; %elbow down 0170 0171 0172 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0173 % solve for third joint theta3, two different 0174 % solutions are returned, corresponding 0175 % to elbow up and down solution 0176 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0177 function q3 = solve_for_theta3(robot, q, Pm) 0178 0179 %Evaluate the parameters 0180 d = eval(robot.DH.d); 0181 a = eval(robot.DH.a); 0182 0183 %See geometry 0184 L2=abs(a(2)); 0185 L3=abs(d(4)); 0186 0187 %given q1 is known, compute first DH transformation 0188 T01=dh(robot, q, 1); 0189 0190 %Express Pm in the reference system 1, for convenience 0191 p1 = inv(T01)*[Pm; 1]; 0192 0193 r = sqrt(p1(1)^2 + p1(2)^2); 0194 0195 eta =(acos((L2^2 + L3^2 - r^2)/(2*L2*L3))); 0196 0197 if ~isreal(eta) 0198 disp('WARNING:inversekinematic_rx170bl: the point is not reachable for this configuration, imaginary solutions'); 0199 %eta = real(eta); 0200 end 0201 0202 %return two possible solutions 0203 %elbow up and elbow down solutions 0204 %the order here is important 0205 q3(1) = pi - eta; 0206 q3(2) = eta - pi; 0207 0208 % Solve the special case of this spherical wrist 0209 % For wrists that whose reference systems have been placed as in the 0210 % ABB IRB 140--> use solve_spherical_wrist2 0211 % For wrists with the same orientation as in the KUKA KR30_jet 0212 %--> use solve_spherical_wrist 0213 function q = solve_spherical_wrist_rx170bl(robot, q, T, wrist) 0214 0215 0216 % T is the noa matrix defining the position/orientation of the end 0217 % effector's reference system 0218 vx6=T(1:3,1); 0219 vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5 0220 0221 % Obtain the position and orientation of the system 3 0222 % using the already computed joints q1, q2 and q3 0223 T01=dh(robot, q, 1); 0224 T12=dh(robot, q, 2); 0225 T23=dh(robot, q, 3); 0226 T03=T01*T12*T23; 0227 0228 vx3=T03(1:3,1); 0229 vy3=T03(1:3,2); 0230 vz3=T03(1:3,3); 0231 0232 % find z4 normal to the plane formed by z3 and a 0233 vz4=cross(vz3, vz5); % end effector's vector a: T(1:3,3) 0234 0235 % in case of degenerate solution, 0236 % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution 0237 if norm(vz4) <= 0.00000001 0238 if wrist == 1 %wrist up 0239 q(4)=0; 0240 else 0241 q(4)=-pi; %wrist down 0242 end 0243 else 0244 %this is the normal and most frequent solution 0245 cosq4=wrist*dot(vy3,vz4); 0246 sinq4=wrist*dot(-vx3,vz4); 0247 q(4)=atan2(sinq4, cosq4); 0248 end 0249 %propagate the value of q(4) to compute the system 4 0250 T34=dh(robot, q, 4); 0251 T04=T03*T34; 0252 vx4=T04(1:3,1); 0253 vy4=T04(1:3,2); 0254 0255 % solve for q5 0256 cosq5=dot(-vy4,vz5); 0257 sinq5=dot(vx4,vz5); 0258 q(5)=atan2(sinq5, cosq5); 0259 0260 %propagate now q(5) to compute T05 0261 T45=dh(robot, q, 5); 0262 T05=T04*T45; 0263 vx5=T05(1:3,1); 0264 vy5=T05(1:3,2); 0265 0266 % solve for q6 0267 cosq6=dot(vx6,vx5); 0268 sinq6=dot(vx6,vy5); 0269 q(6)=atan2(sinq6, cosq6); 0270 0271 0272