Home > . > ikine.m

ikine

PURPOSE ^

IKINE Inverse manipulator kinematics

SYNOPSIS ^

function qt = ikine(robot, tr, q, m)

DESCRIPTION ^

IKINE Inverse manipulator kinematics

    Q = IKINE(ROBOT, T)
    Q = IKINE(ROBOT, T, Q)
    Q = IKINE(ROBOT, T, Q, M)

 Returns the joint coordinates corresponding to the end-effector transform T.
 Note that the inverse kinematic solution is generally not unique, and 
 depends on the initial guess Q (which defaults to 0).

    QT = IKINE(ROBOT, TG)
    QT = IKINE(ROBOT, TG, Q)
    QT = IKINE(ROBOT, TG, Q, M)

 Returns the joint coordinates corresponding to each of the transforms in 
 the 4x4xN trajectory TG.
 Returns one row of QT for each input transform.  The initial estimate 
 of QT for each time step is taken as the solution from the previous 
 time step.

 If the manipulator has fewer than 6 DOF then this method of solution
 will fail, since the solution space has more dimensions than can
 be spanned by the manipulator joint coordinates.  In such a case
 it is necessary to provide a mask matrix, M, which specifies the 
 Cartesian DOF (in the wrist coordinate frame) that will be ignored
 in reaching a solution.  The mask matrix has six elements that
 correspond to translation in X, Y and Z, and rotation about X, Y and
 Z respectively.  The value should be 0 (for ignore) or 1.  The number
 of non-zero elements should equal the number of manipulator DOF.

 Solution is computed iteratively using the pseudo-inverse of the
 manipulator Jacobian.

 Such a solution is completely general, though much less efficient 
 than specific inverse kinematic solutions derived symbolically.
 
 This approach allows a solution to obtained at a singularity, but 
 the joint angles within the null space are arbitrarily assigned.

 For instance with a typical 5 DOF manipulator one would ignore
 rotation about the wrist axis, that is, M = [1 1 1 1 1 0].


 See also: FKINE, TR2DIFF, JACOB0, IKINE560.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %IKINE Inverse manipulator kinematics
0002 %
0003 %    Q = IKINE(ROBOT, T)
0004 %    Q = IKINE(ROBOT, T, Q)
0005 %    Q = IKINE(ROBOT, T, Q, M)
0006 %
0007 % Returns the joint coordinates corresponding to the end-effector transform T.
0008 % Note that the inverse kinematic solution is generally not unique, and
0009 % depends on the initial guess Q (which defaults to 0).
0010 %
0011 %    QT = IKINE(ROBOT, TG)
0012 %    QT = IKINE(ROBOT, TG, Q)
0013 %    QT = IKINE(ROBOT, TG, Q, M)
0014 %
0015 % Returns the joint coordinates corresponding to each of the transforms in
0016 % the 4x4xN trajectory TG.
0017 % Returns one row of QT for each input transform.  The initial estimate
0018 % of QT for each time step is taken as the solution from the previous
0019 % time step.
0020 %
0021 % If the manipulator has fewer than 6 DOF then this method of solution
0022 % will fail, since the solution space has more dimensions than can
0023 % be spanned by the manipulator joint coordinates.  In such a case
0024 % it is necessary to provide a mask matrix, M, which specifies the
0025 % Cartesian DOF (in the wrist coordinate frame) that will be ignored
0026 % in reaching a solution.  The mask matrix has six elements that
0027 % correspond to translation in X, Y and Z, and rotation about X, Y and
0028 % Z respectively.  The value should be 0 (for ignore) or 1.  The number
0029 % of non-zero elements should equal the number of manipulator DOF.
0030 %
0031 % Solution is computed iteratively using the pseudo-inverse of the
0032 % manipulator Jacobian.
0033 %
0034 % Such a solution is completely general, though much less efficient
0035 % than specific inverse kinematic solutions derived symbolically.
0036 %
0037 % This approach allows a solution to obtained at a singularity, but
0038 % the joint angles within the null space are arbitrarily assigned.
0039 %
0040 % For instance with a typical 5 DOF manipulator one would ignore
0041 % rotation about the wrist axis, that is, M = [1 1 1 1 1 0].
0042 %
0043 %
0044 % See also: FKINE, TR2DIFF, JACOB0, IKINE560.
0045  
0046 % Copyright (C) 1993-2008, by Peter I. Corke
0047 %
0048 % This file is part of The Robotics Toolbox for Matlab (RTB).
0049 %
0050 % RTB is free software: you can redistribute it and/or modify
0051 % it under the terms of the GNU Lesser General Public License as published by
0052 % the Free Software Foundation, either version 3 of the License, or
0053 % (at your option) any later version.
0054 %
0055 % RTB is distributed in the hope that it will be useful,
0056 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0057 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0058 % GNU Lesser General Public License for more details.
0059 %
0060 % You should have received a copy of the GNU Leser General Public License
0061 % along with RTB.  If not, see <http://www.gnu.org/licenses/>.
0062 
0063 function qt = ikine(robot, tr, q, m)
0064     %
0065     %  solution control parameters
0066     %
0067     ilimit = 1000;
0068     stol = 1e-12;
0069 
0070     n = robot.n;
0071 
0072     if nargin == 2,
0073         q = zeros(n, 1);
0074     else
0075         q = q(:);
0076     end
0077     if nargin == 4,
0078         m = m(:);
0079         if length(m) ~= 6,
0080             error('Mask matrix should have 6 elements');
0081         end
0082         if length(find(m)) ~= robot.n 
0083             error('Mask matrix must have same number of 1s as robot DOF')
0084         end
0085     else
0086         if n < 6,
0087             disp('For a manipulator with fewer than 6DOF a mask matrix argument should be specified');
0088         end
0089         m = ones(6, 1);
0090     end
0091         
0092 
0093     tcount = 0;
0094     if ishomog(tr),        % single xform case
0095         nm = 1;
0096         count = 0;
0097         while nm > stol,
0098             e = tr2diff(fkine(robot, q'), tr) .* m;
0099             dq = pinv( jacob0(robot, q) ) * e;
0100             q = q + dq;
0101             nm = norm(dq);
0102             count = count+1;
0103             if count > ilimit,
0104                 error('Solution wouldn''t converge')
0105             end
0106         end
0107         qt = q';
0108     else            % trajectory case
0109         np = size(tr,3);
0110         qt = [];
0111         for i=1:np
0112             nm = 1;
0113             T = tr(:,:,i);
0114             count = 0;
0115             while nm > stol,
0116                 e = tr2diff(fkine(robot, q'), T) .* m;
0117                 dq = pinv( jacob0(robot, q) ) * e;
0118                 q = q + dq;
0119                 nm = norm(dq);
0120                 count = count+1;
0121                 if count > ilimit,
0122                     fprintf('i=%d, nm=%f\n', i, nm);
0123                     error('Solution wouldn''t converge')
0124                 end
0125             end
0126             qt = [qt; q'];
0127             tcount = tcount + count;
0128         end
0129     end

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