matlab ikine 源码,关于MATLAB机器人工具箱中的ikine实现 - 信息科学 - 小木虫 - 学术 科研 互动社区...

蒋凡truth

引用回帖:

weixiaomin at 2016-04-05 08:20:15

发一个链接看看

function [qt,histout] = ikine(robot, tr, varargin)                %tr是齐次转换矩阵

%  set default parameters for solution(设置默认参数)

%下面的opt是一个包含了ikine选项参数的名称和默认值的结构体

opt.ilimit = 1000;        %最大迭代计算次数

opt.tol = 1e-6;                %tolerance(误差)

opt.alpha = 1;                %set step size gain(增加步长)

opt.plot = false;        %plot iteration state versus time(绘制与时间对应的迭代状态)

opt.pinv = true;        %use pseudo-inverse instead of Jacobian transpose(使用伪逆矩阵而不是雅克比矩阵的转置)

opt.varstep = false;%enable variable step size if pinv is false(如果pinv为false则使步长为变步长)

%optout = tb_optparse(opt, arglist) is a generalized option parser for Toolbox functions.

%opt is a structure that contains the names and default values for the options,

%and arglist is a cell array containing option parameters, typically it comes from VARARGIN.

%It supports options that have an assigned value, boolean or enumeration types (string or int).

[opt,args] = tb_optparse(opt, varargin);

n = robot.n;                %串联机器人的关节数

% robot.ikine(tr, q)

if ~isempty(args)

q = args{1};

if isempty(q)

q = zeros(1, n);

else

q = q(';                %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('RTB:ikine:badarg', 'Mask matrix should have 6 elements');

end

if numel(find(m)) > robot.n

error('RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix')

end

else

if n < 6

error('RTB:ikine:badarg', '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(迭代总次数)

if ~ishomog(tr)

error('RTB:ikine:badarg', 'T is not a homog xform');

end

J0 = jacob0(robot, q);

J0 = J0(m, ;

if cond(J0) > 100

warning('RTB:ikine:singular', 'Initial joint angles results in near-singular configuration, this may slow convergence');

end

history = [];

failed = false;

e = zeros(6,1);

revolutes = [robot.links.sigma] == 0;

for i=1:npoints

T = tr(:,:,i);

nm = Inf;                %Inf<=>Infinity 正无穷

% initialize state for the ikine loop

eprev = Inf;

save.e = [Inf Inf Inf Inf Inf Inf];

save.q = [];

count = 0;

%迭代计算

while true

% update the count and test against iteration limit

%更新count值，并判断是否超过最大迭代计算次数

count = count + 1;

if count > opt.ilimit

warning('ikine: iteration limit %d exceeded (row %d), final err %f', ...

opt.ilimit, i, nm);

failed = true;

%q = NaN*ones(1,n);

break

end

% compute the error(计算误差)

Tq = robot.fkine(q');

e(1:3) = transl(T - Tq);

Rq = t2r(Tq);

[th,n] = tr2angvec(Rq'*t2r(T));

e(4:6) = th*n;

% optionally adjust the step size(随机调整步长)

if opt.varstep

% test against last best error, only consider the DOF of

% interest

if norm(e(m)) < norm(save.e(m))

% error reduced,

% let's save current state of solution and rack up the step size

save.q = q;

save.e = e;

opt.alpha = opt.alpha * (2.0^(1.0/8));

if opt.verbose > 1

fprintf('step %d: raise alpha to %f\n', count, opt.alpha);

end

else

% rats!  error got worse,

% restore to last good solution and reduce step size

q = save.q;

e = save.e;

opt.alpha = opt.alpha * 0.5;

if opt.verbose > 1

fprintf('step %d: drop alpha to %f\n', count, opt.alpha);

end

end

end

% compute the Jacobian(计算雅克比)

J = jacob0(robot, q);

% compute change in joint angles to reduce the error(计算关节角变化量来减少错误),

% based on the square sub-Jacobian

if opt.pinv

%pinv( J(m, )

dq = opt.alpha * pinv( J(m, ) * e(m);

else

dq = J(m,' * e(m);

dq = opt.alpha * dq;

end

% diagnostic stuff(诊断信息)

if opt.verbose > 1

fprintf('%d/%d: |e| = %f\n', i, count, nm);

fprintf('       e  = '); disp(e');

fprintf('       dq = '); disp(dq');

end

if opt.plot

h.q = q';

h.dq = dq;

h.e = e;

h.ne = nm;

h.alpha = opt.alpha;

history = [history; h]; %#ok<*AGROW>

end

% update the estimated solution

q = q + dq';

% wrap angles for revolute joints

k = (q > pi) & revolutes;

q(k) = q(k) - 2*pi;

k = (q < -pi) & revolutes;

q(k) = q(k) + 2*pi;

nm = norm(e(m));

if norm(e) > 1.5*norm(eprev)

warning('RTB:ikine:diverged', 'solution diverging at step %d, try reducing alpha', count);

end

eprev = e;

if nm <= opt.tol

break

end

end  % end ikine solution for tr(:,:,i)

qt(i, = q';

tcount = tcount + count;

if opt.verbose

fprintf('%d iterations\n', count);

end

end

if opt.verbose && npoints > 1

fprintf('TOTAL %d iterations\n', tcount);

end

% plot evolution of variables(绘制变量演化图)

if opt.plot

figure(1);

plot([history.q]');

xlabel('iteration');

ylabel('q');

grid

figure(2);

plot([history.dq]');

xlabel('iteration');

ylabel('dq');

grid

figure(3);

plot([history.e]');

xlabel('iteration');

ylabel('e');

grid

figure(4);

semilogy([history.ne]);

xlabel('iteration');

ylabel('|e|');

grid

figure(5);

plot([history.alpha]);

xlabel('iteration');

ylabel('\alpha');

grid

if nargout > 1

histout = history;

end

end

end

------------------------------------------------------------------------------

这些是ikine的源码，其中的中文注释是自己理解的

，