看不懂matlab工具箱,要命的机器人工具箱函数，完全看不懂。。。请高人解释

本帖最后由 wangming24 于 2012-12-3 08:38 编辑

第一个函数：雅克比矩阵function J = jacobn(robot, q)

n = robot.n;

L = robot.link;  % get the links

J = [];

U = robot.tool;

for j=n:-1:1,

if robot.mdh == 0,

% standard DH convention

U = L{j}( q(j) ) * U;

end

if L{j}.RP == 'R',

% revolute axis

d = [ -U(1,1)*U(2,4)+U(2,1)*U(1,4)

-U(1,2)*U(2,4)+U(2,2)*U(1,4)

-U(1,3)*U(2,4)+U(2,3)*U(1,4)];   delta = U(3,1:3)'; % nz oz az

else

% prismatic axis

d = U(3,1:3)';  % nz oz az

delta = zeros(3,1); %  0  0  0

end

J = [[d; delta] J];

if robot.mdh ~= 0,

% modified DH convention

U = L{j}( q(j) ) * U;

end

end

红色的代码看得我云里雾里的。。。

第二个函数还是：function J0 = jacob0(robot, q)

%

%   dX_tn = Jn dq

%

Jn = jacobn(robot, q); % Jacobian from joint to wrist space

%

%  convert to Jacobian in base coordinates

%

Tn = fkine(robot, q); % end-effector transformationR = tr2rot(Tn);

J0 = [R zeros(3,3); zeros(3,3) R] * Jn;

第三个函数：

function d = tr2diff(t1, t2)

if nargin == 1,

d = [ t1(1:3,4);

0.5*[t1(3,2)-t1(2,3); t1(1,3)-t1(3,1); t1(2,1)-t1(1,2)]];

else

d = [ t2(1:3,4)-t1(1:3,4);

0.5*( cross(t1(1:3,1), t2(1:3,1)) + ...    %  这一串要命，不知道来龙去脉。。。

cross(t1(1:3,2), t2(1:3,2)) + ...

cross(t1(1:3,3), t2(1:3,3)) ...   )];

end

接下来就是我们可爱的，反解逆运动学方程。。。同样云里雾里。。。function qt = ikine(robot, tr, q, m)

%

%  solution control parameters

%

ilimit = 1000;

stol = 1e-12;

n = robot.n;

if nargin == 2,

q = zeros(n, 1);

else

q = q(:);

end

if nargin == 4,

m = m(:);

if length(m) ~= 6,

error('Mask matrix should have 6 elements');

end

if length(find(m)) ~= robot.n

error('Mask matrix must have same number of 1s as robot DOF')

end

else

if n < 6,

disp('For a manipulator with fewer than 6DOF a mask matrix argument should be specified');

end

m = ones(6, 1);

end

tcount = 0;

if ishomog(tr),  % single xform case

nm = 1;

count = 0;

while nm > stol,

e = tr2diff(fkine(robot, q'), tr) .* m;% fkine(robot,q') 得到末端初始位姿

dq = pinv( jacob0(robot, q) ) * e;

q = q + dq;

nm = norm(dq);%求矩阵2范数 |A’A-λI|=0 ，2范数=(max(λ))^0.5

count = count+1;

if count > ilimit,

error('Solution wouldn''t converge')

end

end

qt = q';

else   % trajectory case

np = size(tr,3);

qt = [];

for i=1:np

nm = 1;

T = tr(:,:,i);

count = 0;

while nm > stol,

e = tr2diff(fkine(robot, q'), T) .* m;  %要命的几行代码。。。。太要命了。。。不知道怎么回事

dq = pinv( jacob0(robot, q) ) * e;

q = q + dq;

nm = norm(dq);    count = count+1;

if count > ilimit,

fprintf('i=%d, nm=%f\n', i, nm);

error('Solution wouldn''t converge')

end

end

qt = [qt; q'];

tcount = tcount + count;

end

end