%SerialLink.IKINEM Numerical inverse kinematics by minimization % % Q = R.ikinem(T) is the joint coordinates corresponding to the robot % end-effector pose T which is a homogenenous transform. % % Q = R.ikinem(T, Q0, OPTIONS) specifies the initial estimate of the joint % coordinates. % % In all cases if T is 4x4xM it is taken as a homogeneous transform sequence % and R.ikinem() 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:: % 'pweight',P weighting on position error norm compared to rotation % error (default 1) % 'stiffness',S Stiffness used to impose a smoothness contraint on joint % angles, useful when N is large (default 0) % 'qlimits' Enforce joint limits % 'ilimit',L Iteration limit (default 1000) % 'nolm' Disable Levenberg-Marquadt % % Notes:: % - PROTOTYPE CODE UNDER DEVELOPMENT, intended to do numerical inverse kinematics % with joint limits % - The inverse kinematic solution is generally not unique, and % depends on the initial guess Q0 (defaults to 0). % - The function to be minimized is highly nonlinear and the solution is % often trapped in a local minimum, adjust Q0 if this happens. % - The default value of Q0 is zero which is a poor choice for most % manipulators (eg. puma560, twolink) since it corresponds to a kinematic % singularity. % - Such a solution is completely general, though much less efficient % than specific inverse kinematic solutions derived symbolically, like % ikine6s or ikine3.% - Uses Levenberg-Marquadt minimizer LMFsolve if it can be found, % if 'nolm' is not given, and 'qlimits' false % - The error function to be minimized is computed on the norm of the error % between current and desired tool pose. This norm is computed from distances % and angles and 'pweight' can be used to scale the position error norm to % be congruent with rotation error norm. % - This approach allows a solution to obtained at a singularity, but % the joint angles within the null space are arbitrarily assigned. % - Joint offsets, if defined, are added to the inverse kinematics to % generate Q. % - Joint limits become explicit contraints if 'qlimits' is set. % % See also fminsearch, fmincon, SerialLink.fkine, SerialLink.ikine, tr2angvec. % Copyright (C) 1993-2015, 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.petercorke.com function qt = ikinem(robot, tr, varargin) opt.pweight = 1; opt.stiffness = 0; opt.qlimits = false; opt.ilimit = 1000; opt.lm = true; opt.col = 2; [opt,args] = tb_optparse(opt, varargin); % check if optional argument is a valid q q0 = args{1}; if numel(q0) ~= robot.n error('q0 length must match number of joints in robot'); end for i=1:size(tr,3) T = tr(:,:,i); if opt.qlimits % constrained optimization to handle joint limits options = optimset('MaxIter', opt.ilimit); qlim = robot.qlim; [q, ef, exflag, output] = fmincon( @(x) costfun(x, robot, T, opt), q0, ... [], [], [], [], ... qlim(:,1), qlim(:,2), ... [], options); if opt.verbose fprintf('final error %f, %d iterations, %d evalations\n', ... ef, output.iterations, output.funcCount); end else % no joint limits, unconstrained optimization if exist('LMFsolve') == 2 && opt.lm [q, ef, count] = LMFsolve( @(x) costfun(x, robot, T, opt), q0, 'MaxIter', opt.ilimit); q = q'; if opt.verbose fprintf('final error %f, %d iterations\n', ... ef, count); end else options = optimset('MaxIter', opt.ilimit); [q, ef, exflag, output] = fminsearch( @(x) costfun(x, robot, T, opt), q0, options); if opt.verbose fprintf('final error %f, %d iterations, %d evalations\n', ... ef, output.iterations, output.funcCount); end end end qt(i,:) = q; end if opt.verbose robot.fkine(qt) end end % The cost function, this is the value to be minimized function E = costfun(q, robot, T, opt) Tq = robot.fkine(q); % find the pose error in SE(3) dT = transl(T) - transl(Tq); % translation error E = norm(dT) * opt.pweight; % rotation error % find dot product of dd = dot(T(1:3,opt.col), Tq(1:3,opt.col)); %E = E + (1 - dd)^2*100000 ; E = E + acos(dd)^2*1000 ; if opt.stiffness > 0 % enforce a continuity constraint on joints, minimum bend E = E + sum( diff(q).^2 ) * opt.stiffness; end end