%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Inverse kinematics for the 3dof planar robot T: homogeneous matrix robot: structure with arm parameters returns: all possible solutions or q = [q1 q2] that place the end effectors at the position specified by T. Two possible solutions q1 and q2 q3 and q4 are returned, generally called elbow up and elbow down, combined with Author: Arturo Gil Aparicio arturo.gil@umh.es Date: 08/03/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Inverse kinematics for the 3dof planar robot 0003 % T: homogeneous matrix 0004 % robot: structure with arm parameters 0005 % returns: all possible solutions or q = [q1 q2] that place the end effectors at the 0006 % position specified by T. Two possible solutions q1 and q2 q3 and q4 are returned, 0007 % generally called elbow up and elbow down, combined with 0008 % Author: Arturo Gil Aparicio arturo.gil@umh.es 0009 % Date: 08/03/2012 0010 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0011 0012 % Copyright (C) 2012, by Arturo Gil Aparicio 0013 % 0014 % This file is part of ARTE (A Robotics Toolbox for Education). 0015 % 0016 % ARTE is free software: you can redistribute it and/or modify 0017 % it under the terms of the GNU Lesser General Public License as published by 0018 % the Free Software Foundation, either version 3 of the License, or 0019 % (at your option) any later version. 0020 % 0021 % ARTE is distributed in the hope that it will be useful, 0022 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0023 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0024 % GNU Lesser General Public License for more details. 0025 % 0026 % You should have received a copy of the GNU Leser General Public License 0027 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0028 function q = inversekinematic_3dofplanar(robot, T) 0029 0030 fprintf('\nComputing inverse kinematics for the %s robot', robot.name); 0031 0032 0033 %Initialize q 0034 % q = [q1 q2 q3 q4], atends for two possible solutions 0035 q=zeros(3,4); 0036 0037 a = eval(robot.DH.a); 0038 0039 %Link lengths 0040 L1=abs(a(1)); 0041 L2=abs(a(2)); 0042 L3=abs(a(2)); 0043 0044 0045 %T= [ nx ox ax Px; 0046 % ny oy ay Py; 0047 % nz oz az Pz]; 0048 Q=T(1:3,4); 0049 0050 %find angle Phi 0051 x3 = T(1:3,1); 0052 0053 cphi = x3'*[1 0 0]'; 0054 sphi = x3'*[0 1 0]'; 0055 phi = atan2(sphi, cphi); 0056 0057 0058 %Find point P from P and vector (nx, ny, nz) 0059 P = Q - a(3)*T(1:3,1); 0060 0061 %Distance of the point to the origin. 0062 R= sqrt(P(1)^2+P(2)^2); 0063 0064 if R > (L1+L2) 0065 disp('\ninversekinematic_3dofplanar: unfeasible solution. The point cannot be reached'); 0066 end 0067 0068 %compute geometric solution 0069 beta = atan2(P(2),P(1)); 0070 gamma = real(acos((L1^2+R^2-L2^2)/(2*R*L1))); 0071 delta = real(acos((L1^2+L2^2-R^2)/(2*L1*L2))); 0072 0073 %arrange possible combinations for q(1) and q(2) 0074 %elbow down elbow up solutions 0075 q =[beta+gamma beta-gamma; 0076 delta-pi pi-delta]; 0077 0078 %in this case, phi = q(1) + q(2) + q(3) and 0079 %q(3) can be computed as q(3) = phi - q(1) - q(2) 0080 %corresponding to each of the previous solutions, a unique q(3) can be 0081 %computed for each case 0082 for i=1:2, %iterate through columns 0083 q(3,i) = phi - q(1,i) - q(2,i); 0084 end 0085 0086