%连杆变换矩阵+正向运动学
function T04=Zkine(q)

pi=3.14156;

%标准DH参数
a=[0.15,0.58,0.725,0];   %连杆长度，单位m
alpha=[pi/2,0,pi/2,0];   %连杆转角，单位rad
d=[0.423,0,0,0];     %连杆偏距，单位m

n=numel(a);
T=cell(n,1);
for i=1:n
    T{i}=[cos(q(i)) -sin(q(i))*cos(alpha(i)) sin(q(i))*sin(alpha(i)) a(i)*cos(q(i));
          sin(q(i)) cos(q(i))*cos(alpha(i)) -cos(q(i))*sin(alpha(i)) a(i)*sin(q(i));
          0 sin(alpha(i)) cos(alpha(i)) d(i);
            0 0 0 1];      
end
T04=T{1}*T{2}*T{3}*T{4};
T04=vpa(T04,4);
end