%%?PUMA機器人軌跡規劃
%%?PUMA560機器人的桿件坐標參數?該參數對應于付京遜版本中的臂型
%L?=link（[alpha?A?theta?D?sigma]?CONVENTION）
L1=link（[-pi/2?0?pi/2?0?0]‘standard‘）;
L2=link（[0?431.8?0?149.09?0]‘standard‘）;
L3=link（[pi/2?-20.32?pi/2?0?0]‘standard‘）;
L4=link（[-pi/2?0?0?433.07?0]‘standard‘）;
L5=link（[pi/2?0?0?0?0]‘standard‘）;
L6=link（[0?0?0?56.25?0]‘standard‘）;
r=robot（{L1?L2?L3?L4?L5?L6}‘PUMA560‘）;
plot（r?[pi/2?0?pi/2?0?0?0]）;
%L=r.links?將定義L{n}
%%?求解運動參數
q1=[0?0?0?0?0?0];
q2=[pi/2?pi/3?pi/2?pi/4?pi/3?pi/8];
t=?[0:0.056:2]‘;
[qqdqdd]=jtraj（q1q2t）;
%%?繪制關節軌跡
plot（?r?q）;
figure
subplot（321）;plot（tq（:1）‘c‘）;title（‘q1‘）;
xlabel（‘T/s‘）