Home > . > ikine560.m

ikine560

PURPOSE ^

IKINE560 Inverse kinematics for Puma 560

SYNOPSIS ^

function theta = ikine560(robot, T,configuration)

DESCRIPTION ^

IKINE560 Inverse kinematics for Puma 560 

    Q = IKINE560(ROBOT, T, CONFIG)

 Solve the inverse kinematics of the Puma-like (spherical wristed) robot 
 ROBOT whose end-effector pose is given by T.

 The optional third argument specifies the configuration of the arm in
 the form of a string containing one or more of the configuration codes:
    'l' or 'r'    lefty/righty
    'u' or 'd'    elbow
    'n' or 'f'    wrist flip or noflip.

 The default configuration is 'lun'.

 REFERENCE:

 Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang
 From The International Journal of Robotics Research
 Vol. 5, No. 2, Summer 1986, p. 32-44


 AUTHOR:
 Robert Biro        gt2231a@prism.gatech.edu
 with Gary Von McMurray

 GTRI/ATRP/IIMB
 Georgia Institute of Technology
 2/13/95

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %IKINE560 Inverse kinematics for Puma 560
0002 %
0003 %    Q = IKINE560(ROBOT, T, CONFIG)
0004 %
0005 % Solve the inverse kinematics of the Puma-like (spherical wristed) robot
0006 % ROBOT whose end-effector pose is given by T.
0007 %
0008 % The optional third argument specifies the configuration of the arm in
0009 % the form of a string containing one or more of the configuration codes:
0010 %    'l' or 'r'    lefty/righty
0011 %    'u' or 'd'    elbow
0012 %    'n' or 'f'    wrist flip or noflip.
0013 %
0014 % The default configuration is 'lun'.
0015 %
0016 % REFERENCE:
0017 %
0018 % Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang
0019 % From The International Journal of Robotics Research
0020 % Vol. 5, No. 2, Summer 1986, p. 32-44
0021 %
0022 %
0023 % AUTHOR:
0024 % Robert Biro        gt2231a@prism.gatech.edu
0025 % with Gary Von McMurray
0026 %
0027 % GTRI/ATRP/IIMB
0028 % Georgia Institute of Technology
0029 % 2/13/95
0030 
0031 % MOD HISTORY
0032 %  4/99 use new robot object
0033 %  4/02 tidyup, remove multiple solutions
0034 
0035 function theta = ikine560(robot, T,configuration)
0036 
0037     if robot.n ~= 6,
0038         error('Solution only applicable for 6DOF manipulator');
0039     end
0040 
0041     if robot.mdh ~= 0,
0042         error('Solution only applicable for standard DH conventions');
0043     end
0044 
0045     if ndims(T) == 3,
0046         theta = [];
0047         for k=1:size(T,3),
0048             if nargin < 3,
0049                 theta = [theta; ikine560(robot, T(:,:,k))];
0050             else
0051                 theta = [theta; ikine560(robot, T(:,:,k), configuration)];
0052             end
0053         end
0054 
0055         return;
0056     end
0057     L = robot.links;
0058     a1 = L{1}.A;
0059     a2 = L{2}.A;
0060     a3 = L{3}.A;
0061 
0062     if ~isempty( find( [L{4}.A L{5}.A L{6}.A] ~= 0 ))
0063         error('wrist is not spherical')
0064     end
0065 
0066     d1 = L{1}.D;
0067     d2 = L{2}.D;
0068     d3 = L{3}.D;
0069     d4 = L{4}.D;
0070 
0071     if ~ishomog(T),
0072         error('T is not a homog xform');
0073     end
0074 
0075     % undo base transformation
0076     T = inv(robot.base) * T;
0077 
0078     % The following parameters are extracted from the Homogeneous
0079     % Transformation as defined in equation 1, p. 34
0080 
0081     Ox = T(1,2);
0082     Oy = T(2,2);
0083     Oz = T(3,2);
0084 
0085     Ax = T(1,3);
0086     Ay = T(2,3);
0087     Az = T(3,3);
0088 
0089     Px = T(1,4);
0090     Py = T(2,4);
0091     Pz = T(3,4);
0092 
0093     % The configuration parameter determines what n1,n2,n4 values are used
0094     % and how many solutions are determined which have values of -1 or +1.
0095 
0096     if nargin < 3,
0097         configuration = '';
0098     else
0099         configuration = lower(configuration);
0100     end
0101 
0102     % default configuration
0103 
0104     n1 = -1;    % L
0105     n2 = -1;    % U
0106     n4 = -1;    % N
0107     if ~isempty(findstr(configuration, 'l')),
0108         n1 = -1;
0109     end
0110     if ~isempty(findstr(configuration, 'r')),
0111         n1 = 1;
0112     end
0113     if ~isempty(findstr(configuration, 'u')),
0114         if n1 == 1,
0115             n2 = 1;
0116         else
0117             n2 = -1;
0118         end
0119     end
0120     if ~isempty(findstr(configuration, 'd')),
0121         if n1 == 1,
0122             n2 = -1;
0123         else
0124             n2 = 1;
0125         end
0126     end
0127     if ~isempty(findstr(configuration, 'n')),
0128         n4 = 1;
0129     end
0130     if ~isempty(findstr(configuration, 'f')),
0131         n4 = -1;
0132     end
0133 
0134 
0135     %
0136     % Solve for theta(1)
0137     %
0138     % r is defined in equation 38, p. 39.
0139     % theta(1) uses equations 40 and 41, p.39,
0140     % based on the configuration parameter n1
0141     %
0142 
0143     r=sqrt(Px^2 + Py^2);
0144     if (n1 == 1),
0145         theta(1)= atan2(Py,Px) + asin(d3/r);
0146     else
0147         theta(1)= atan2(Py,Px) + pi - asin(d3/r);
0148     end
0149 
0150     %
0151     % Solve for theta(2)
0152     %
0153     % V114 is defined in equation 43, p.39.
0154     % r is defined in equation 47, p.39.
0155     % Psi is defined in equation 49, p.40.
0156     % theta(2) uses equations 50 and 51, p.40, based on the configuration
0157     % parameter n2
0158     %
0159 
0160     V114= Px*cos(theta(1)) + Py*sin(theta(1));
0161     r=sqrt(V114^2 + Pz^2);
0162     Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
0163     if ~isreal(Psi),
0164         error('point not reachable');
0165     end
0166     theta(2) = atan2(Pz,V114) + n2*Psi;
0167 
0168     %
0169     % Solve for theta(3)
0170     %
0171     % theta(3) uses equation 57, p. 40.
0172     %
0173 
0174     num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
0175     den = cos(theta(2))*Pz - sin(theta(2))*V114;
0176     theta(3) = atan2(a3,d4) - atan2(num, den);
0177 
0178     %
0179     % Solve for theta(4)
0180     %
0181     % V113 is defined in equation 62, p. 41.
0182     % V323 is defined in equation 62, p. 41.
0183     % V313 is defined in equation 62, p. 41.
0184     % theta(4) uses equation 61, p.40, based on the configuration
0185     % parameter n4
0186     %
0187 
0188     V113 = cos(theta(1))*Ax + sin(theta(1))*Ay;
0189     V323 = cos(theta(1))*Ay - sin(theta(1))*Ax;
0190     V313 = cos(theta(2)+theta(3))*V113 + sin(theta(2)+theta(3))*Az;
0191     theta(4) = atan2((n4*V323),(n4*V313));
0192     %[(n4*V323),(n4*V313)]
0193 
0194     %
0195     % Solve for theta(5)
0196     %
0197     % num is defined in equation 65, p. 41.
0198     % den is defined in equation 65, p. 41.
0199     % theta(5) uses equation 66, p. 41.
0200     %
0201      
0202     num = -cos(theta(4))*V313 - V323*sin(theta(4));
0203     den = -V113*sin(theta(2)+theta(3)) + Az*cos(theta(2)+theta(3));
0204     theta(5) = atan2(num,den);
0205     %[num den]
0206 
0207     %
0208     % Solve for theta(6)
0209     %
0210     % V112 is defined in equation 69, p. 41.
0211     % V122 is defined in equation 69, p. 41.
0212     % V312 is defined in equation 69, p. 41.
0213     % V332 is defined in equation 69, p. 41.
0214     % V412 is defined in equation 69, p. 41.
0215     % V432 is defined in equation 69, p. 41.
0216     % num is defined in equation 68, p. 41.
0217     % den is defined in equation 68, p. 41.
0218     % theta(6) uses equation 70, p. 41.
0219     %
0220 
0221     V112 = cos(theta(1))*Ox + sin(theta(1))*Oy;
0222     V132 = sin(theta(1))*Ox - cos(theta(1))*Oy;
0223     V312 = V112*cos(theta(2)+theta(3)) + Oz*sin(theta(2)+theta(3));
0224     V332 = -V112*sin(theta(2)+theta(3)) + Oz*cos(theta(2)+theta(3));
0225     V412 = V312*cos(theta(4)) - V132*sin(theta(4));
0226     V432 = V312*sin(theta(4)) + V132*cos(theta(4));
0227     num = -V412*cos(theta(5)) - V332*sin(theta(5));
0228     den = - V432;
0229     theta(6) = atan2(num,den);
0230     %[num den]

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003