1robkine.m

6轴机器人的matlab构造程序

function T06=robkine(x1,x2,x3,x4,x5,x6)
a1=100;
a2=290;
a3=121;
d4=127;
d5=310-127;
d6=112.5;
T01=[cos(x1) -sin(x1) 0 0;sin(x1) cos(x1) 0 0;0 0 1 0;0 0 0 1];
T12=[cos(x2) -sin(x2) 0 a1;0 0 1 0;-sin(x2) -cos(x2) 0 0;0 0 0 1];
T23=[cos(x3) -sin(x3) 0 a2;sin(x3) cos(x3) 0 0;0 0 1 0;0 0 0 1];
T34=[cos(x4) -sin(x4) 0 a3;0 0 1 d4+d5;-sin(x4) -cos(x4) 0 0;0 0 0 1];
T45=[cos(x5) -sin(x5) 0 0;0 0 -1 0;sin(x5) cos(x5) 0 0;0 0 0 1];
T56=[cos(x6) -sin(x6) 0 0;0 0 1 d6;-sin(x6) -cos(x6) 0 0;0 0 0 1];

%T34=[cos(x4) -sin(x4) 0 a3;0 0 1 d4;-sin(x4) -cos(x4) 0 0;0 0 0 1];
%T45=[cos(x5) -sin(x5) 0 0;0 0 -1 0;sin(x5) cos(x5) 0 d5;0 0 0 1];

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