inversekinematic_KUKA_KR5_scara_R350_Z200

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   Q = INVERSEKINEMATIC_KUKA_KR5_scara_R350_Z200(robot, T)    
   Solves the inverse kinematic problem for the KUKA KR5 scara R350 Z200 scara 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_KUKA_KR5_scara_R350_Z200 returns 4 possible solutions, thus,
   Q is a 4x4 matrix where each column stores 6 feasible joint values.

   
   Example code:

   robot=load_robot('kuka', 'KR5_scara_R350_Z200');
   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_KUKA_KR5_scara_R350_Z200(robot, T)
0003 %   Solves the inverse kinematic problem for the KUKA KR5 scara R350 Z200 scara 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_KUKA_KR5_scara_R350_Z200 returns 4 possible solutions, thus,
0010 %   Q is a 4x4 matrix where each column stores 6 feasible joint values.
0011 %
0012 %
0013 %   Example code:
0014 %
0015 %   robot=load_robot('kuka', 'KR5_scara_R350_Z200');
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 %    See also DIRECTKINEMATIC.
0026 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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_KUKA_KR5_scara_R350_Z200(robot, T)
0046 
0047 
0048 fprintf('\nComputing inverse kinematics for the %s robot', robot.name);
0049 
0050 
0051 %initialize q
0052 q=zeros(4,2);
0053 
0054 %Evaluate the DH table to obtain geometric parameters
0055 theta = eval(robot.DH.theta);
0056 d = eval(robot.DH.d);
0057 a = eval(robot.DH.a);
0058 alpha = eval(robot.DH.alpha);
0059 
0060 %Store geometric parameters
0061 L1=abs(d(1));
0062 L2=abs(a(1));
0063 L3=abs(a(2));
0064 
0065 shift = d(3);
0066 
0067 %T= [ nx ox ax Px;
0068 %     ny oy ay Py;
0069 %     nz oz az Pz];
0070 Px=T(1,4);
0071 Py=T(2,4);
0072 Pz=T(3,4);
0073 
0074 
0075 %Distance of the point to the origin of S0
0076 R = sqrt(Px^2+Py^2);
0077 
0078 %Compute angles
0079 gamma = real(acos((L2^2+R^2-L3^2)/(2*R*L2)));
0080 beta = atan2(Py,Px); 
0081 delta = real(acos((L2^2+L3^2-R^2)/(2*L2*L3)));
0082 
0083 %find the last rotation for the two possible configurations
0084 q4_1= find_last_rotation(robot,[beta+gamma delta-pi L1-Pz 0], T);
0085 q4_2= find_last_rotation(robot,[beta-gamma pi-delta L1-Pz 0], T);
0086 
0087 %Arrange all possible solutions
0088 q=[beta+gamma       beta-gamma;
0089     delta-pi         pi-delta;
0090     -L1+Pz+shift    -L1+Pz+shift ;
0091     q4_1 q4_2];
0092 
0093 
0094 % Compute the last rotation
0095 function q4 = find_last_rotation(robot, q, T)
0096 
0097 U = T(1:3,1);
0098 
0099 %Recompute the DH table according to q1, q2 and q3
0100 theta = eval(robot.DH.theta);
0101 d = eval(robot.DH.d);
0102 a = eval(robot.DH.a);
0103 alpha = eval(robot.DH.alpha);
0104 
0105 %now compute the position/orientation of the system S3
0106 H=eye(4);
0107 for i=1:3,
0108     H=H*dh(theta(i), d(i), a(i), alpha(i));
0109 end
0110 
0111 X3=H(1:3,1);
0112 Y3=H(1:3,2);
0113 
0114 coseno=X3'*U;
0115 seno=U'*Y3;
0116 %compute the last rotation
0117 q4=atan2(seno,coseno);
0118 
0119 
0120