clear;
clc;
close
%建立机器人模型
%       theta    d           a        alpha     offset
SL1=Link('a', 0.1, 'd',0, 'alpha',pi/2 ,'qlim',[0,pi],'standard'); 
SL2=Link('a', 1.8, 'd',0, 'alpha',0 ,'qlim',[0,pi],'standard');
SL3=Link('a', 2.3, 'd',0, 'alpha',pi/2 ,'qlim',[0,pi],'standard');
SL4=Link('a', 0, 'd',0, 'alpha',0 ,'qlim',[0,0],'standard'); %正逆解
SL5=Link('a', 0, 'd',0, 'alpha',0 ,'qlim',[0,0],'standard');
SL6=Link('a', 0, 'd',0, 'alpha',0 ,'qlim',[0,0],'standard');

robot=SerialLink([SL1,SL2,SL3,SL4,SL5,SL6],'name','r-sta');
 figure(1),teach(robot);
 robot.display();
 q=[pi/2,pi/2,pi/4,0,0,0];
 Vf= robot.fkine(q);
 robot.teach(q);

 robot.ikine(Vf)