clc;

clear;

close all;

%% KR 21O R2700 extra DH Table:

alpha=[-90,0,-90,90,-90,0];

a=[350,1150,-41,0,0,0];

d=[675,0,0,1200,0,215];

B=[0,-90,90,0,0,0];

A(1)=-B(1);

A(2)=B(2);

A(3)=B(3)-90;

A(4)=-B(4);

A(5)=B(5);

A(6)=-B(6)+180;

B1=[10,-100,100,20,30,40];

B2=[20,-110,110,30,40,50];

B3=[35,-125,115,45,55,65];

%% SDH

K01=SDH(alpha(1),a(1),A(1),d(1));

K12=SDH(alpha(2),a(2),A(2),d(2));

K23=SDH(alpha(3),a(3),A(3),d(3));

K34=SDH(alpha(4),a(4),A(4),d(4));

K45=SDH(alpha(5),a(5),A(5),d(5));

K56=SDH(alpha(6),a(6),A(6),d(6));

K=K01*K12*K23*K34*K45*K56;

%%正运动学

Pos1=KUKA_FK(B);

Pos2=KUKA_FK(B1);

Pos3=KUKA_FK(B2);

Pos4=KUKA_FK(B3);

%% Function for DH tranformation

function T=MDH(alpha,a,theta,d)

% alpha : rotate along X  in deg

% a: linear move in X

% theta: rotae along Z in deg

% d:linear move Z

T=trotx(alpha)*transl(a,0,0)*trotz(theta)*transl(0,0,d);

end

function T=SDH(alpha,a,theta,d)

% alpha : rotate along X  in deg

% a: linear move in X

% theta: rotae along Z in deg

% d:linear move Z

T=trotz(theta)*transl(0,0,d)*transl(a,0,0)*trotx(alpha);

end

function POS=KUKA_FK(Axis)

%% DH 参数

alpha=[-90,0,-90,90,-90,0];

a=[350,1150,-41,0,0,0];

d=[675,0,0,1200,0,215];

A(1)=-Axis(1);

A(2)=Axis(2);

A(3)=Axis(3)-90;

A(4)=-Axis(4);

A(5)=Axis(5);

A(6)=-Axis(6)+180;

%% 计算各frame

T01=SDH(alpha(1),a(1),A(1),d(1));

T12=SDH(alpha(2),a(2),A(2),d(2));

T23=SDH(alpha(3),a(3),A(3),d(3));

T34=SDH(alpha(4),a(4),A(4),d(4));

T45=SDH(alpha(5),a(5),A(5),d(5));

T56=SDH(alpha(6),a(6),A(6),d(6));

T=T01*T12*T23*T34*T45*T56;

%% 转换机器人格式数据

T15=T12*T23*T34*T45;

XYZ=transl(T);

ABC=rad2deg(rotm2eul(T(1:3,1:3)));

Bit0=(T15(1,4)<0);

Bit1=Axis(3)>rad2deg(atan2(T45(2,4),T45(1,4)));

Bit2=(Axis(5)>=-180 &Axis(5)<=0)|(Axis(5)>=180);

Status=Bit0*1+Bit1*2+Bit2*4;

for i=1:6

   turn(i)=Axis(i)<=0;

end

Turn=turn(1)*1+turn(2)*2+turn(3)*4+turn(4)*8+turn(5)*16+turn(6)*32

POS=transpose([transpose(XYZ)  ABC Status Turn]);

end

咨询详情：KUKA库卡机器人