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.
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