%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_Prosix_C3_A601C(robot, T)
Solves the inverse kinematic problem for the EPSON Prosix_C3_A601C 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__Prosix_C3_A601C returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
epson=load_robot('EPSON', 'Prosix_C3_A601C');
q = [0 0 0 0 0 0];
T = directkinematic(epson, q);
%Call the inversekinematic for this robot
qinv = inversekinematic(epson, T);
check that all of them are feasible solutions!
and every Ti equals T
for i=1:8,
Ti = directkinematic(epson, qinv(:,i))
end
See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_Prosix_C3_A601C(robot, T) 0003 % Solves the inverse kinematic problem for the EPSON Prosix_C3_A601C 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__Prosix_C3_A601C 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 % epson=load_robot('EPSON', 'Prosix_C3_A601C'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(epson, q); 0018 % %Call the inversekinematic for this robot 0019 % qinv = inversekinematic(epson, T); 0020 % check that all of them are feasible solutions! 0021 % and every Ti equals T 0022 % for i=1:8, 0023 % Ti = directkinematic(epson, 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_Prosix_C3_A601C(robot, T) 0045 0046 0047 %Evaluate the parameters 0048 d = eval(robot.DH.d); 0049 a = eval(robot.DH.a); 0050 0051 %See geometry at the reference for this robot 0052 L6=d(6); 0053 0054 %T= [ nx ox ax Px; 0055 % ny oy ay Py; 0056 % nz oz az Pz]; 0057 Px=T(1,4); 0058 Py=T(2,4); 0059 Pz=T(3,4); 0060 0061 %Compute the position of the wrist, being W the Z component of the end effector's system 0062 W = T(1:3,3); 0063 0064 % Pm: wrist position 0065 Pm = [Px Py Pz]' - L6*W; 0066 0067 %first joint, two possible solutions admited: 0068 % if q(1) is a solution, then q(1) + pi is also a solution 0069 q1=atan2(Pm(2), Pm(1)); 0070 0071 0072 %solve for q2 0073 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0074 0075 q2_2=solve_for_theta2(robot, [q1+pi 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 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0081 0082 0083 %Arrange solutions, there are 8 possible solutions so far. 0084 % if q1 is a solution, q1* = q1 + pi is also a solution. 0085 % For each (q1, q1*) there are two possible solutions 0086 % for q2 and q3 (namely, elbow up and elbow up solutions) 0087 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3), 0088 % there exist two more possible solutions for the last three joints, generally 0089 % called wrist up and wrist down solutions. For this reason, 0090 %the next matrix doubles each column. For each two columns, two different 0091 %configurations for theta4, theta5 and theta6 will be computed. These 0092 %configurations are generally referred as wrist up and wrist down solution 0093 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0094 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0095 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0096 0 0 0 0 0 0 0 0; 0097 0 0 0 0 0 0 0 0; 0098 0 0 0 0 0 0 0 0]; 0099 0100 %leave only the real part of the solutions 0101 q=real(q); 0102 0103 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0104 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0105 %step is avoided in this angle (the next line is commented). When solving 0106 %for the orientation, the solutions are normalized to the [-pi, pi] range 0107 %only for the theta4, theta5 and theta6 joints. 0108 0109 %normalize q to [-pi, pi] 0110 q(1,:) = normalize(q(1,:)); 0111 q(2,:) = normalize(q(2,:)); 0112 0113 % solve for the last three joints 0114 % for any of the possible combinations (theta1, theta2, theta3) 0115 for i=1:2:size(q,2), 0116 % use solve_spherical_wrist2 for the particular orientation 0117 % of the systems in this ABB robot 0118 % use either the geometric or algebraic method. 0119 % the function solve_spherical_wrist2 is used due to the relative 0120 % orientation of the last three DH reference systems. 0121 0122 %This robot uses a different function to compute the last three angles, 0123 %since the relative orientation of the systems S4, S5 and S6 differs 0124 %from that of the rest of the robots 0125 qtemp = solve_spherical_wrist_Prosix(robot, q(:,i), T, 1); %wrist up 0126 qtemp(4:6)=normalize(qtemp(4:6)); 0127 q(:,i)=qtemp; 0128 0129 qtemp = solve_spherical_wrist_Prosix(robot, q(:,i), T, -1); %wrist down 0130 0131 qtemp(4:6)=normalize(qtemp(4:6)); 0132 q(:,i+1)=qtemp; 0133 end 0134 0135 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0136 % solve for second joint theta2, two different 0137 % solutions are returned, corresponding 0138 % to elbow up and down solution 0139 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0140 function q2 = solve_for_theta2(robot, q, Pm) 0141 0142 %Evaluate the parameters 0143 d = eval(robot.DH.d); 0144 a = eval(robot.DH.a); 0145 0146 %See geometry 0147 L2=a(2); 0148 L3=d(4); 0149 0150 %given q1 is known, compute first DH transformation 0151 T01=dh(robot, q, 1); 0152 0153 %Express Pm in the reference system 1, for convenience 0154 p1 = inv(T01)*[Pm; 1]; 0155 0156 r = sqrt(p1(1)^2 + p1(2)^2); 0157 0158 beta = atan2(-p1(2), p1(1)); 0159 gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2))); 0160 0161 if ~isreal(gamma) 0162 disp('WARNING:inversekinematic_Prosix_C3_A601C: the point is not reachable for this configuration, imaginary solutions'); 0163 gamma = real(gamma); 0164 end 0165 0166 %return two possible solutions 0167 %elbow up and elbow down 0168 %the order here is important and is coordinated with the function 0169 %solve_for_theta3 0170 q2(1) = pi/2 - beta - gamma; %elbow up 0171 q2(2) = pi/2 - beta + gamma; %elbow down 0172 0173 0174 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0175 % solve for third joint theta3, two different 0176 % solutions are returned, corresponding 0177 % to elbow up and down solution 0178 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0179 function q3 = solve_for_theta3(robot, q, Pm) 0180 0181 %Evaluate the parameters 0182 d = eval(robot.DH.d); 0183 a = eval(robot.DH.a); 0184 0185 %See geometry 0186 L2=a(2); 0187 L3=d(4); 0188 0189 %given q1 is known, compute first DH transformation 0190 T01=dh(robot, q, 1); 0191 0192 %Express Pm in the reference system 1, for convenience 0193 p1 = inv(T01)*[Pm; 1]; 0194 0195 r = sqrt(p1(1)^2 + p1(2)^2); 0196 0197 eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3))); 0198 0199 if ~isreal(eta) 0200 disp('WARNING:inversekinematic_Prosix_C3_A601C: the point is not reachable for this configuration, imaginary solutions'); 0201 eta = real(eta); 0202 end 0203 0204 %return two possible solutions 0205 %elbow up and elbow down solutions 0206 %the order here is important 0207 q3(1) = -(pi/2 - eta); 0208 q3(2) = -(eta - 3*pi/2); 0209 0210 0211 % Solve the special case of this spherical wrist 0212 % For wrists that whose reference systems have been placed as in the 0213 % ABB IRB 140--> use solve_spherical_wrist2 0214 % For wrists with the same orientation as in the KUKA KR30_jet 0215 %--> use solve_spherical_wrist 0216 function q = solve_spherical_wrist_Prosix(robot, q, T, wrist) 0217 0218 0219 % T is the noa matrix defining the position/orientation of the end 0220 % effector's reference system 0221 vx6=T(1:3,1); 0222 vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5 0223 0224 % Obtain the position and orientation of the system 3 0225 % using the already computed joints q1, q2 and q3 0226 T01=dh(robot, q, 1); 0227 T12=dh(robot, q, 2); 0228 T23=dh(robot, q, 3); 0229 T03=T01*T12*T23; 0230 0231 vx3=T03(1:3,1); 0232 vy3=T03(1:3,2); 0233 vz3=T03(1:3,3); 0234 0235 % find z4 normal to the plane formed by z3 and a 0236 vz4=cross(vz3, vz5); % end effector's vector a: T(1:3,3) 0237 0238 % in case of degenerate solution, 0239 % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution 0240 if norm(vz4) <= 0.00000001 0241 if wrist == 1 %wrist up 0242 q(4)=0; 0243 else 0244 q(4)=-pi; %wrist down 0245 end 0246 else 0247 %this is the normal and most frequent solution 0248 cosq4=wrist*dot(vy3,vz4); 0249 sinq4=wrist*dot(-vx3,vz4); 0250 q(4)=atan2(sinq4, cosq4); 0251 end 0252 %propagate the value of q(4) to compute the system 4 0253 T34=dh(robot, q, 4); 0254 T04=T03*T34; 0255 vx4=T04(1:3,1); 0256 vy4=T04(1:3,2); 0257 0258 % solve for q5 0259 cosq5=dot(-vy4,vz5); 0260 sinq5=dot(vx4,vz5); 0261 q(5)=atan2(sinq5, cosq5); 0262 0263 %propagate now q(5) to compute T05 0264 T45=dh(robot, q, 5); 0265 T05=T04*T45; 0266 vx5=T05(1:3,1); 0267 vy5=T05(1:3,2); 0268 0269 % solve for q6 0270 cosq6=dot(vx6,vx5); 0271 sinq6=dot(vx6,vy5); 0272 q(6)=atan2(sinq6, cosq6); 0273 0274 0275 0276 0277