You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
131 lines
4.4 KiB
131 lines
4.4 KiB
%SerialLink.IKINE Inverse manipulator kinematics
|
|
%
|
|
% Q = R.ikine(T) is the joint coordinates corresponding to the robot
|
|
% end-effector pose T which is a homogenenous transform.
|
|
%
|
|
% Q = R.ikine(T, Q0, OPTIONS) specifies the initial estimate of the joint
|
|
% coordinates.
|
|
%
|
|
% Q = R.ikine(T, Q0, M, OPTIONS) specifies the initial estimate of the joint
|
|
% coordinates and a mask matrix. For the case where the manipulator
|
|
% has fewer than 6 DOF the solution space has more dimensions than can
|
|
% be spanned by the manipulator joint coordinates. In this case
|
|
% the mask matrix M 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.
|
|
%
|
|
% For example when using a 5 DOF manipulator rotation about the wrist z-axis
|
|
% might be unimportant in which case M = [1 1 1 1 1 0].
|
|
%
|
|
% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence
|
|
% and R.ikine() returns the joint coordinates corresponding to each of the
|
|
% transforms in the sequence. Q is MxN where N is the number of robot joints.
|
|
% The initial estimate of Q for each time step is taken as the solution
|
|
% from the previous time step.
|
|
%
|
|
% Options::
|
|
%
|
|
% Notes::
|
|
% - Solution is computed iteratively using the pseudo-inverse of the
|
|
% manipulator Jacobian.
|
|
% - The inverse kinematic solution is generally not unique, and
|
|
% depends on the initial guess Q0 (defaults to 0).
|
|
% - 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.
|
|
%
|
|
% See also SerialLink.fkine, tr2delta, SerialLink.jacob0, SerialLink.ikine6s.
|
|
|
|
% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
|
|
%
|
|
% Copyright (C) 1993-2011, by Peter I. Corke
|
|
%
|
|
% This file is part of The Robotics Toolbox for MATLAB (RTB).
|
|
%
|
|
% RTB is free software: you can redistribute it and/or modify
|
|
% it under the terms of the GNU Lesser General Public License as published by
|
|
% the Free Software Foundation, either version 3 of the License, or
|
|
% (at your option) any later version.
|
|
%
|
|
% RTB is distributed in the hope that it will be useful,
|
|
% but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
% GNU Lesser General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Leser General Public License
|
|
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
|
|
%
|
|
% http://www.petercorke.com
|
|
|
|
function qt = ikine(robot, tr, varargin)
|
|
% set default parameters for solution
|
|
opt.ilimit = 100;
|
|
opt.tol = 1e-6;
|
|
opt.alpha = 1;
|
|
opt.plot = false;
|
|
opt.pinv = false;
|
|
|
|
[opt,args] = tb_optparse(opt, varargin);
|
|
|
|
n = robot.n;
|
|
|
|
% robot.ikine(tr, q)
|
|
if length(args) > 0
|
|
q = args{1};
|
|
if isempty(q)
|
|
q = zeros(1, n);
|
|
else
|
|
q = q(:)';
|
|
end
|
|
else
|
|
q = zeros(1, n);
|
|
end
|
|
|
|
% robot.ikine(tr, q, m)
|
|
if length(args) > 1
|
|
m = args{2};
|
|
m = m(:);
|
|
if numel(m) ~= 6
|
|
error('Mask matrix should have 6 elements');
|
|
end
|
|
if numel(find(m)) ~= robot.n
|
|
error('Mask matrix must have same number of 1s as robot DOF')
|
|
end
|
|
else
|
|
if n < 6
|
|
error('For a manipulator with fewer than 6DOF a mask matrix argument must be specified');
|
|
end
|
|
m = ones(6, 1);
|
|
end
|
|
% make this a logical array so we can index with it
|
|
m = logical(m);
|
|
|
|
npoints = size(tr,3); % number of points
|
|
qt = zeros(npoints, n); % preallocate space for results
|
|
tcount = 0; % total iteration count
|
|
eprev = Inf;
|
|
|
|
save.e = Inf;
|
|
save.q = [];
|
|
|
|
history = [];
|
|
for i=1:npoints
|
|
T = tr(:,:,i);
|
|
nm = Inf;
|
|
count = 0;
|
|
|
|
optim = optimset('Display', 'iter', 'TolX', 0, 'TolFun', opt.tol, 'MaxIter', opt.ilimit);
|
|
optim
|
|
q = fminsearch(@(x) ikinefunc(x, T, robot, m), q, optim);
|
|
|
|
end
|
|
qt = q;
|
|
end
|
|
|
|
function E = ikinefunc(q, T, robot, m)
|
|
e = tr2delta(fkine(robot, q'), T);
|
|
E = norm(e(m));
|
|
end
|