L1（1）=link（[000pi/2]）;
L1（2）=link（[000.43180]）;
L1（3）=link（[0?0.150050.0203-pi/2]）;
L1（4）=link（[00.43180pi/2]）;
L1（5）=link（[000-pi/2]）;
L1（6）=link（[0000]）;
robot=Seriallink（L1‘name‘‘six?link‘）;

%?T1T2直線起點(diǎn)和終點(diǎn)
T1=transl（0.40.4-0.3）;
T2=transl（0.40-0.3）;
T3=transl（0.4-0.6-0.3）;
T4=transl（0.4-0.40.4）;
T5=transl（0.40.60.4）;
T6=transl（0.40.4-0.3）;
t=[0:0.4:5]‘;
Ts1=ctraj（T1T2length（t））;
Ts2=ctraj（T2T3length（t））;
Ts3=ctraj（T3T4length（t））;
Ts4=ctraj（T4T5length（t））;
Ts5=ctraj（T5T6length（t））;
q_s1=robot.ikine6s（Ts1）;
q_s2=robot.ikine6s（Ts2）;
q_s3=robot.ikine6s（Ts3）;
q_s4=robot.ikine6s（Ts4）;
q_s5=robot.ikine6s（Ts5）;
Ts1=ctraj（T1T3length（t））;
Ts2=ctraj（T3T4length（t））;
Ts3=ctraj（T4T5length（t））;
Ts4=ctraj（T5T6length（t