rackup

matlab机械⼿ikine函数,关于MATLAB机器⼈⼯具箱中的ikine

实现-信息科学。。。

蒋凡truth

引⽤回帖:

weixiaominat2016-04-0508:20:15

发⼀个链接看看

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

%tdefaultparametersforsolution(设置默认参数)

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

=1000;%最⼤迭代计算次数

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

=1;%tstepsizegain(增加步长)

opt.=fal;%plotiterationstateversustime(绘制与时间对应的迭代状态)

=true;%upudo-inverinsteadofJacobiantranspo(使⽤伪逆矩阵⽽不是雅克⽐矩阵的转置)

p=fal;%enablevariablestepsizeifpinvisfal(如果pinv为fal则使步长为变步长)

%optout=tb_optpar(opt,arglist)isageneralizedoptionparrforToolboxfunctions.

%optisastructurethatcontainsthenamesanddefaultvaluesfortheoptions,

%andarglistisacellarraycontainingoptionparameters,typicallyitcomesfromVARARGIN.

%Itsupportsoptionsthathaveanassignedvalue,booleanorenumerationtypes(stringorint).

[opt,]=tb_optpar(opt,varargin);

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

%(tr,q)

if~impty(args)

q=args{1};

ifimpty(q)

q=zeros(1,n);

el

q=q(';%q('的意思是先将矩阵q按⼀列排放，然后取它的转置，最后为⼀⾏向量

end

el

q=zeros(1,n);

end

%(tr,q,m)

iflength(args)>1

m=args{2};

m=m(;

ifnumel(m)~=6

error('RTB:ikine:badarg','Maskmatrixshouldhave6elements');

end

ifnumel(find(m))>robot.n

error('RTB:ikine:badarg','NumberofrobotDOFmustbe>=thesamenumberof1sinthemaskmatrix')

end

el

ifn<6

error('RTB:ikine:badarg','Foramanipulatorwithfewerthan6DOFamaskmatrixargumentmustbespecified');

end

m=ones(6,1);

end

%makethisalogicalarraysowecanindexwithit

m=logical(m);

npoints=size(tr,3);%numberofpoints(什么点的数量？)

qt=zeros(npoints,n);%preallocatespaceforresults(给结果预分配空间)

tcount=0;%totaliterationcount(迭代总次数)

if~ishomog(tr)

error('RTB:ikine:badarg','Tisnotahomogxform');

end

J0=jacob0(robot,q);

J0=J0(m,;

ifcond(J0)>100

warning('RTB:ikine:singular','Initialjointanglesresultsinnear-singularconfiguration,thismayslowconvergence');

end

history=[];

failed=fal;

e=zeros(6,1);

revolutes=[]==0;

fori=1:npoints

T=tr(:,:,i);

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

%initializestatefortheikineloop

eprev=Inf;

save.e=[InfInfInfInfInfInf];

save.q=[];

count=0;

%迭代计算

whiletrue

%updatethecountandtestagainstiterationlimit

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

count=count+1;

ifcount>

warning('ikine:iterationlimit%dexceeded(row%d),finalerr%f',...

,i,nm);

failed=true;

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

break

end

%computetheerror(计算误差)

Tq=(q');

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

Rq=t2r(Tq);

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

e(4:6)=th*n;

%optionallyadjustthestepsize(随机调整步长)

p

%testagainstlastbesterror,onlyconsidertheDOFof

%interest

ifnorm(e(m))

%errorreduced,

%let'ssavecurrentstateofsolutionandrackupthestepsize

save.q=q;

save.e=e;

=*(2.0^(1.0/8));

e>1

fprintf('step%d:raialphato%fn',count,);

end

el

%rats!errorgotwor,

%restoretolastgoodsolutionandreducestepsize

q=save.q;

e=save.e;

=*0.5;

e>1

fprintf('step%d:dropalphato%fn',count,);

end

end

end

%computetheJacobian(计算雅克⽐)

J=jacob0(robot,q);

%computechangeinjointanglestoreducetheerror(计算关节⾓变化量来减少错误),

%badonthesquaresub-Jacobian

%pinv(J(m,)

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

el

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

dq=*dq;

end

%diagnosticstuff(诊断信息)

e>1

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

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

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

end

h.q=q';

=dq;

h.e=e;

=nm;

=;

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

end

%updatetheestimatedsolution

q=q+dq';

%wrapanglesforrevolutejoints

k=(q>pi)&revolutes;

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

k=(q<-pi)&revolutes;

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

nm=norm(e(m));

ifnorm(e)>1.5*norm(eprev)

warning('RTB:ikine:diverged','solutiondivergingatstep%d,tryreducingalpha',count);

end

eprev=e;

ifnm<=

break

end

end%endikinesolutionfortr(:,:,i)

qt(i,=q';

tcount=tcount+count;

e

fprintf('%diterationsn',count);

end

end

e&&npoints>1

fprintf('TOTAL%diterationsn',tcount);

end

%plotevolutionofvariables(绘制变量演化图)

figure(1);

plot([history.q]');

xlabel('iteration');

ylabel('q');

grid

figure(2);

plot([]');

xlabel('iteration');

ylabel('dq');

grid

figure(3);

plot([history.e]');

xlabel('iteration');

ylabel('e');

grid

figure(4);

milogy([]);

xlabel('iteration');

ylabel('|e|');

grid

figure(5);

plot([]);

xlabel('iteration');

ylabel('alpha');

grid

ifnargout>1

histout=history;

end

end

end

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

这些是ikine的源码，其中的中⽂注释是⾃⼰理解的

，