%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Inverse kinematics for the 2dof 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 are returned, generally called elbow up and elbow down. Author: Arturo Gil Aparicio arturo.gil@umh.es Date: 08/03/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Inverse kinematics for the 2dof 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 are returned, 0007 % generally called elbow up and elbow down. 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_2dofplanar(robot, T) 0029 0030 fprintf('\nComputing inverse kinematics for the %s robot', robot.name); 0031 0032 0033 %Initialize q 0034 % q = [q1 q2], atends for two possible solutions 0035 q=zeros(2,2); 0036 0037 % theta = eval(robot.DH.theta); 0038 % d = eval(robot.DH.d); 0039 a = eval(robot.DH.a); 0040 % alpha = eval(robot.DH.alpha); 0041 0042 0043 %T= [ nx ox ax Px; 0044 % ny oy ay Py; 0045 % nz oz az Pz]; 0046 Px=T(1,4); 0047 Py=T(2,4); 0048 Pz=T(3,4); 0049 0050 %Distance of the point to the origin. 0051 R= sqrt(Px^2+Py^2); 0052 0053 %Link lengths 0054 L1=abs(a(1)); 0055 L2=abs(a(2)); 0056 0057 if R > (L1+L2) 0058 fprintf('\nERROR: inversekinematic_2dofplanar: unfeasible solution'); 0059 end 0060 0061 %compute geometric solution 0062 beta = atan2(Py,Px); 0063 gamma = real(acos((L1^2+R^2-L2^2)/(2*R*L1))); 0064 delta = real(acos((L1^2+L2^2-R^2)/(2*L1*L2))); 0065 0066 %arrange possible combinations 0067 %elbow up elbow down solutions 0068 q =[beta+gamma beta-gamma; 0069 delta-pi pi-delta]; 0070