%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_PA_10_6DOF(robot, T)
Solves the inverse kinematic problem for the MITSUBISHI PA-10 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_PA_10_6DOF returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
robot=load_robot('MISUBISHI', 'pa-10');
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_PA_10_6DOF(robot, T) 0003 % Solves the inverse kinematic problem for the MITSUBISHI PA-10 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_PA_10_6DOF 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('MISUBISHI', 'pa-10'); 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 % Copyright (C) 2012, by Arturo Gil Aparicio 0029 % 0030 % This file is part of ARTE (A Robotics Toolbox for Education). 0031 % 0032 % ARTE is free software: you can redistribute it and/or modify 0033 % it under the terms of the GNU Lesser General Public License as published by 0034 % the Free Software Foundation, either version 3 of the License, or 0035 % (at your option) any later version. 0036 % 0037 % ARTE is distributed in the hope that it will be useful, 0038 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0039 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0040 % GNU Lesser General Public License for more details. 0041 % 0042 % You should have received a copy of the GNU Leser General Public License 0043 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0044 function q = inversekinematic_pa_10_6DOF(robot, T) 0045 0046 %initialize, eight possible solutions 0047 q=zeros(6,8); 0048 0049 %theta = eval(robot.DH.theta); 0050 d = eval(robot.DH.d); 0051 a = eval(robot.DH.a); 0052 %alpha = eval(robot.DH.alpha); 0053 0054 %L1=d(1); 0055 %L2=a(2); 0056 %L3=d(4); 0057 L6=d(6); 0058 0059 %T= [ nx ox ax Px; 0060 % ny oy ay Py; 0061 % nz oz az Pz]; 0062 Px=T(1,4); 0063 Py=T(2,4); 0064 Pz=T(3,4); 0065 0066 %Calculamos la posici�n de la mu�eca. W es el tercer vector en el extremo 0067 %del brazo 0068 W = T(1:3,3); 0069 %Pm: posici�n de la mu�eca 0070 Pm = [Px Py Pz]' - L6*W; 0071 0072 %first joint, two possible solutions admited: 0073 % if q(1) is a solution, then q(1) + pi is also a solution 0074 q1=atan2(Pm(2), Pm(1)); 0075 0076 0077 %solve for q2 0078 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0079 0080 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0081 0082 %solve for q3 0083 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0084 0085 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0086 0087 0088 %the next matrix doubles each column. For each two columns, two different 0089 %configurations for theta4, theta5 and theta6 will be computed. These 0090 %configurations are generally referred as wrist up and wrist down solution 0091 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0092 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0093 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0094 0 0 0 0 0 0 0 0; 0095 0 0 0 0 0 0 0 0; 0096 0 0 0 0 0 0 0 0]; 0097 0098 %leave only the real part of the solutions 0099 q=real(q); 0100 0101 0102 %normalize q to [-pi, pi] 0103 q(1,:) = normalize(q(1,:)); 0104 q(2,:) = normalize(q(2,:)); 0105 q(3,:) = normalize(q(3,:)); 0106 0107 % solve for the last three joints 0108 % for any of the possible combinations (theta1, theta2, theta3) 0109 for i=1:2:size(q,2), 0110 qtemp = solve_spherical_wrist_pa_10(robot, q(:,i), T, 1); %wrist up 0111 qtemp(4:6)=normalize(qtemp(4:6)); 0112 q(:,i)=qtemp; 0113 0114 qtemp = solve_spherical_wrist_pa_10(robot, q(:,i), T, -1); %wrist down 0115 qtemp(4:6)=normalize(qtemp(4:6)); 0116 q(:,i+1)=qtemp; 0117 end 0118 0119 0120 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0121 % solve for second joint theta2, two different 0122 % solutions are returned, corresponding 0123 % to elbow up and down solution 0124 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0125 function q2 = solve_for_theta2(robot, q, Pm) 0126 0127 %Evaluate the parameters 0128 theta = eval(robot.DH.theta); 0129 d = eval(robot.DH.d); 0130 a = eval(robot.DH.a); 0131 alpha = eval(robot.DH.alpha); 0132 0133 %See geometry 0134 L2=a(2); 0135 L3=d(4); 0136 0137 %given q1 is known, compute first DH transformation 0138 T01=dh(robot, q, 1); 0139 0140 %Express Pm in the reference system 1, for convenience 0141 p1 = inv(T01)*[Pm; 1]; 0142 0143 r = sqrt(p1(1)^2 + p1(2)^2); 0144 0145 beta = atan2(p1(2), p1(1)); 0146 gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2))); 0147 if ~isreal(gamma) 0148 disp('WARNING:inversekinematic_pa_10: the point is not reachable for this configuration, imaginary solutions'); 0149 %gamma = real(gamma); 0150 end 0151 %return two possible solutions 0152 %elbow up and elbow down 0153 %the order here is important and is coordinated with the function 0154 %solve_for_theta3 0155 q2(1) = beta + gamma-pi/2; %elbow up 0156 q2(2) = beta - gamma-pi/2; %elbow down 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 %theta = eval(robot.DH.theta); 0167 d = eval(robot.DH.d); 0168 a = eval(robot.DH.a); 0169 %alpha = eval(robot.DH.alpha); 0170 0171 %See geometry 0172 L2=a(2); 0173 L3=d(4); 0174 0175 %given q1 is known, compute first DH transformation 0176 T01=dh(robot, q, 1); 0177 0178 %Express Pm in the reference system 1, for convenience 0179 p1 = inv(T01)*[Pm; 1]; 0180 0181 r = sqrt(p1(1)^2 + p1(2)^2); 0182 0183 eta = acos((L2^2 + L3^2 - r^2)/(2*L2*L3)); 0184 0185 if ~isreal(eta) 0186 disp('WARNING:inversekinematic_pa_10: the point is not reachable for this configuration, imaginary solutions'); 0187 %eta = real(eta); 0188 end 0189 %return two possible solutions 0190 %elbow up and elbow down solutions 0191 %the order here is important 0192 q3(1) = -pi + eta; 0193 q3(2) = -eta + pi; 0194 0195 0196 % Solve the special case of this spherical wrist 0197 % For wrists that whose reference systems have been placed as in the 0198 % ABB IRB 140--> use solve_spherical_wrist2 0199 % For wrists with the same orientation as in the KUKA KR30_jet 0200 %--> use solve_spherical_wrist 0201 function q = solve_spherical_wrist_pa_10(robot, q, T, wrist) 0202 0203 0204 % T is the noa matrix defining the position/orientation of the end 0205 % effector's reference system 0206 vx6=T(1:3,1); 0207 vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5 0208 0209 % Obtain the position and orientation of the system 3 0210 % using the already computed joints q1, q2 and q3 0211 T01=dh(robot, q, 1); 0212 T12=dh(robot, q, 2); 0213 T23=dh(robot, q, 3); 0214 T03=T01*T12*T23; 0215 0216 vx3=T03(1:3,1); 0217 vy3=T03(1:3,2); 0218 vz3=T03(1:3,3); 0219 0220 % find z4 normal to the plane formed by z3 and a 0221 vz4=cross(vz3, vz5); % end effector's vector a: T(1:3,3) 0222 0223 % in case of degenerate solution, 0224 % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution 0225 if norm(vz4) <= 0.00000001 0226 if wrist == 1 %wrist up 0227 q(4)=0; 0228 else 0229 q(4)=-pi; %wrist down 0230 end 0231 else 0232 %this is the normal and most frequent solution 0233 cosq4=wrist*dot(vy3,vz4); 0234 sinq4=wrist*dot(-vx3,vz4); 0235 q(4)=atan2(sinq4, cosq4); 0236 end 0237 %propagate the value of q(4) to compute the system 4 0238 T34=dh(robot, q, 4); 0239 T04=T03*T34; 0240 vx4=T04(1:3,1); 0241 vy4=T04(1:3,2); 0242 0243 % solve for q5 0244 cosq5=dot(-vy4,vz5); 0245 sinq5=dot(vx4,vz5); 0246 q(5)=atan2(sinq5, cosq5); 0247 0248 %propagate now q(5) to compute T05 0249 T45=dh(robot, q, 5); 0250 T05=T04*T45; 0251 vx5=T05(1:3,1); 0252 vy5=T05(1:3,2); 0253 0254 % solve for q6 0255 cosq6=dot(vx6,vx5); 0256 sinq6=dot(vx6,vy5); 0257 q(6)=atan2(sinq6, cosq6); 0258 0259 0260 0261 0262 0263