inversekinematic_scara

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   Q = INVERSEKINEMATIC_SCARA(robot, T)    
   Solves the inverse kinematic problem for the SCARA example 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_SCARA returns 2 possible solutions, thus,
   Q is a 4x4 matrix where each column stores 4 feasible joint values.

   
   Example code:

   robot=load_robot('example', 'scara');
   q = [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:2,
        Ti = directkinematic(robot, qinv(:,i))
   end

    See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %   Q = INVERSEKINEMATIC_SCARA(robot, T)
0003 %   Solves the inverse kinematic problem for the SCARA example 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_SCARA returns 2 possible solutions, thus,
0010 %   Q is a 4x4 matrix where each column stores 4 feasible joint values.
0011 %
0012 %
0013 %   Example code:
0014 %
0015 %   robot=load_robot('example', 'scara');
0016 %   q = [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:2,
0023 %        Ti = directkinematic(robot, qinv(:,i))
0024 %   end
0025 %
0026 %    See also DIRECTKINEMATIC.
0027 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0028 
0029 % Copyright (C) 2012, by Arturo Gil Aparicio
0030 %
0031 % This file is part of ARTE (A Robotics Toolbox for Education).
0032 %
0033 % ARTE is free software: you can redistribute it and/or modify
0034 % it under the terms of the GNU Lesser General Public License as published by
0035 % the Free Software Foundation, either version 3 of the License, or
0036 % (at your option) any later version.
0037 %
0038 % ARTE is distributed in the hope that it will be useful,
0039 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0040 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0041 % GNU Lesser General Public License for more details.
0042 %
0043 % You should have received a copy of the GNU Leser General Public License
0044 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0045 function q = inversekinematic_scara(robot, T)
0046 
0047 fprintf('\nComputing inverse kinematics for the %s robot', robot.name);
0048 
0049 
0050 %initialize q
0051 q=zeros(4,2);
0052 
0053 %Evaluate the DH table to obtain geometric parameters
0054 d = eval(robot.DH.d);
0055 a = eval(robot.DH.a);
0056 
0057 %Store geometric parameters
0058 L1=abs(d(1));
0059 L2=abs(a(1));
0060 L3=abs(a(2));
0061 
0062 %T= [ nx ox ax Px;
0063 %     ny oy ay Py;
0064 %     nz oz az Pz];
0065 Px=T(1,4);
0066 Py=T(2,4);
0067 Pz=T(3,4);
0068 
0069 
0070 %Distance of the point to the origin of S0
0071 R = sqrt(Px^2+Py^2);
0072 
0073 %Compute angles
0074 gamma = real(acos((L2^2+R^2-L3^2)/(2*R*L2)));
0075 beta = atan2(Py,Px); 
0076 delta = real(acos((L2^2+L3^2-R^2)/(2*L2*L3)));
0077 
0078 %find the last rotation for the two possible configurations
0079 q4_1= find_last_rotation(robot,[beta+gamma delta-pi L1-Pz 0], T);
0080 q4_2= find_last_rotation(robot,[beta-gamma pi-delta L1-Pz 0], T);
0081 
0082 %Arrange all possible solutions
0083 q=[beta+gamma beta-gamma;
0084     delta-pi pi-delta;
0085     L1-Pz    L1-Pz;
0086     q4_1 q4_2];
0087 
0088 
0089 % Compute the last rotation
0090 function q4 = find_last_rotation(robot, q, T)
0091 
0092 U = T(1:3,1);
0093 
0094 %Recompute the DH table according to q1, q2 and q3
0095 theta = eval(robot.DH.theta);
0096 d = eval(robot.DH.d);
0097 a = eval(robot.DH.a);
0098 alpha = eval(robot.DH.alpha);
0099 
0100 %now compute the position/orientation of the system S3
0101 H=eye(4);
0102 for i=1:3,
0103     H=H*dh(theta(i), d(i), a(i), alpha(i));
0104 end
0105 
0106 X3=H(1:3,1);
0107 Y3=H(1:3,2);
0108 
0109 coseno=X3'*U;
0110 seno=U'*Y3;
0111 %compute the last rotation
0112 q4=atan2(seno,coseno);
0113 
0114 
0115