%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Inverse kinematics for the 3dof spherical robot T: homogeneous matrix robot: structure with arm parameters returns: all possible solutions for q = [theta beta delta] that place the end effectors at the position specified by T. Two possible solutions are returned, generally called elbow in and elbow out: FIRST column contain elbow OUT solution SECOND column contains elbow IN solution Author: Ángel Rodríguez email: arodgre@gmail.com date: 18/12/2013 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Inverse kinematics for the 3dof spherical robot 0003 % T: homogeneous matrix 0004 % robot: structure with arm parameters 0005 % returns: all possible solutions for q = [theta beta delta] that place the end effectors at the 0006 % position specified by T. Two possible solutions are returned, 0007 % generally called elbow in and elbow out: 0008 % 0009 % FIRST column contain elbow OUT solution 0010 % SECOND column contains elbow IN solution 0011 % 0012 % Author: Ángel Rodríguez 0013 % email: arodgre@gmail.com date: 18/12/2013 0014 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0015 0016 % Copyright (C) 2012, by Arturo Gil Aparicio 0017 % 0018 % This file is part of ARTE (A Robotics Toolbox for Education). 0019 % 0020 % ARTE is free software: you can redistribute it and/or modify 0021 % it under the terms of the GNU Lesser General Public License as published by 0022 % the Free Software Foundation, either version 3 of the License, or 0023 % (at your option) any later version. 0024 % 0025 % ARTE is distributed in the hope that it will be useful, 0026 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0027 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0028 % GNU Lesser General Public License for more details. 0029 % 0030 % You should have received a copy of the GNU Leser General Public License 0031 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0032 function q = inversekinematic_3dofspherical(robot, T) 0033 0034 fprintf('\nComputing inverse kinematics for the %s robot.\n', robot.name); 0035 0036 L1=robot.L1; 0037 L2=robot.L2; 0038 0039 %Eprima is the robot's arm end 0040 Eprima=T(1:3,4); 0041 if Eprima(3)==0 0042 L2prima=L2; 0043 delta=0; 0044 else 0045 delta=asin(Eprima(3)/L2);%Angle between arm 2 and the projection 0046 L2prima= Eprima(3)/tan(delta); 0047 end 0048 0049 %Distance of the projection on the XYplane to the origin. 0050 Ro=sqrt(Eprima(1)^2+Eprima(2)^2); 0051 %Distance of the point to the origin. 0052 Xi=sqrt(Eprima(1)^2+Eprima(2)^2+Eprima(3)^2); 0053 %Passive joint's angle. Between arm1 & arm2 0054 betaprima= real(acos((L2^2+L1^2-Xi^2)/(2*L2*L1))); 0055 beta= pi - betaprima; 0056 0057 0058 phi= atan2(Eprima(2),Eprima(1)); 0059 gamma= real(acos((L1^2+Ro^2-L2prima^2)/(2*Ro*L1))); 0060 theta=[phi-gamma, phi+gamma];%phi-gamma is the out elbow solution 0061 %phi+gamma is the in elbow solution 0062 0063 0064 %q contains elbow in and elbow out solution 0065 %FIRST column contain elbow OUT solution 0066 %SECOND column contains elbow IN solution 0067 q=[theta(1), theta(2); 0068 beta, -beta; 0069 delta, delta]; 0070 0071 %Puesto en cuarentena porque hay muchas opciones de brazos. 0072 % if q(2)<pi & q(2)>-0.35 0073 % fprintf('\nWARNING: inversekinematic_3dofplanar: unfeasible solution. The arms impact aganinst each other.\n'); 0074 % %set(gca,'color','r'); 0075 % end 0076 0077 if Xi > (L1+L2) 0078 fprintf('WARNING: inversnekinematic_3dofplanar: unfeasible solution. The point cannot be reached.\n'); 0079 %set(gca,'color','r'); 0080 end 0081 0082