正向运动学

不用twist也不用D-Htable，直接随便建立坐标系，根据相邻坐标系之间的坐标变换，来计算总的变换。经试验，的确可行。
%输入是各个关节的数值，输出(BASE)世界坐标系的6维向量
in=[7952, 2913, -815, 56554, -2050, -34617];

s1=in(1)*pi/2/120000;
s2=-in(2)*pi/2/160000; % -
s3=-s2+in(3)*pi/2/120000; % + 和2有耦合
s4=-in(4)*pi/2/80000; % -
s5=in(5)*pi/2/80000; % +
s6=-in(6)*pi/2/50000; % -

%从世界坐标系c0到坐标系c1，仅有绕z轴的转动
g01=rot(3,s1,[0 0 0]);

%从c1到c2，包括一个y轴平移和一个绕x轴的转动
g12=rot(1,s2,[0 150 0]);

%从c2到c3，包括一个z轴平移和一个绕x轴的转动
g23=rot(1,s3,[0 0 260]);

%从c3到c4，包括一个z轴平移和y轴转动
g34=rot(2,s4,[0 0 60 ]);

%从c4到c5，包括一个y轴平移和x轴转动
g45=rot(1,s5,[0 260 0]);

%从c5到c6，包括一个y轴转动
g56=rot(2,s6,[0 0 0]);
%c6到ctool
g6t=[0 1 0 0;0 0 1 90;1 0 0 0;0 0 0 1];

gw0=[0 1 0 0;-1 0 0 0;0 0 1 0;0 0 0 1];
g01
g02=g01*g12
g03=g02*g23
g04=g03*g34
g05=g04*g45
g06=g05*g56

gw6=gw0*g06
gwt=gw6*g6t

out=[gwt(1:3,4).' oula(gwt(1:3,1:3).')*180/pi]

function y=oula(x)
[h,w]=size(x);
if h==3 & w==3
    y=oula1(x);
else
    y=oula2(x);
end

function [sitaxyz]=oula1(R)
R=R.';
if abs(det(R)-1)>1e-2,error;end
sita=asin(-R(3));

if abs(1-R(3)^2)<1e-5
    sai=0;
    fai=0;
    sitaxyz=[fai sita sai];
    return;
end

sai=asin(R(2)/sqrt(1-R(3)^2));
if R(2)>=0 & R(1)>=0
elseif R(1)<0
    sai=pi-sai;
else
    sai=2*pi+sai;
end
   
fai=asin(R(6)/sqrt(1-R(3)^2));
if R(9)<0
    fai=pi-fai;
elseif R(6)<0 & R(9)>0
    fai=2*pi+fai;
end

if fai>pi
    fai=fai-2*pi;
end
if sai>pi
    sai=sai-2*pi;
end
sitaxyz=[fai sita sai];

function R=oula2(sitaxyz)
if length(sitaxyz)~=3,error;end
fai=sitaxyz(1);
sita=sitaxyz(2);
sai=sitaxyz(3);
R=[1 0 0;0 cos(fai) sin(fai);0 -sin(fai) cos(fai)]* [cos(sita) 0 -sin(sita);0 1 0;sin(sita) 0 cos(sita)]* [cos(sai) sin(sai) 0;-sin(sai) cos(sai) 0;0 0 1];

例子：
----PulseNO.S 0,L 0,U 0,R 0,B 0,T 0
----CartPos.S 500.001,L 0.001,U 319.999,R 180,B -90,T 0

----PulseNO.S 1568,L 75,U -846,R 1088,B -777,T 1816
----CartPos.S 500.699,L 10.249,U 314.796,R 108.51,B -85.29,T 72.71

----PulseNO. 7952, 2913, -815, 56554, -2050, -34617
----CartPos. 505.507, 49.537, 314.702, 109.49, -85.34, 74.46