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直线起点和终点
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