jacobiano.asv

是一个在matlab下的机器人运动仿真的软件

function Jacobiano(hObject, eventdata, handles)

global teta1 d2 d3 d4 l1 teta4 teta5 teta6 Vx Vy Vz

% teta1 = sym('teta1');
% teta4 = sym('teta4');
% teta5 = sym('teta5');
% teta6 = sym('teta6');
% l1= sym('l1');
% d2= sym('d2');
% d3= sym('d3');
% d4= sym('d4');
% 
% 
% T01=[cos(teta1),-sin(teta1),0,0;
%     sin(teta1),cos(teta1),0,0;
%     0,0,1,0;
%     0,0,0,1];
% 
% T12=[1,0,0,0;
%     0,1,0,0;
%     0,0,1,d2;
%     0,0,0,1];
% 
% T23=[1,0,0,l1;
%     0,0,-1,-d3;
%     0,1,0,0;
%     0,0,0,1];
% 
% T34=[cos(teta4),-sin(teta4),0,0;
%     sin(teta4),cos(teta4),0,0;
%     0,0,1,d4;
%     0,0,0,1];
% 
% T45=[cos(teta5),-sin(teta5),0,0;
%     0,0,1,0;
%     -sin(teta5),-cos(teta5),0,0;
%     0,0,0,1];
% 
% T56=[cos(teta6),-sin(teta6),0,0;
%     0,0,-1,0;
%     sin(teta6),cos(teta6),0,0;
%     0,0,0,1];
% 
% T06=T01*T12*T23*T34*T45*T56;


% Px=sin(teta1)*d4+cos(teta1)*l1+sin(teta1)*d3;
% Py=-cos(teta1)*d4+sin(teta1)*l1-cos(teta1)*d3;
% Pz=d2;

%_______Teste
% teta1=0;
% d2=100;
% d3=100;
% d4=20;
% l1=80;

J11=d4*cos(teta1)-l1*sin(teta1)+d3*cos(teta1);
J12=0;
J13=sin(teta1);


J21=4*sin(teta1)+l1*cos(teta1)+d3*sin(teta1);
J22=0;
J23=-cos(teta1);

J31=0;
J32=1;
J33=0;


J=[J11,J12,J13;
   J21,J22,J23;
   J31,J32,J33];

JInverso=inv(J);%JACOBIANO INVERSO

%JACOBIANO DIRECTO  V=J.TETA


%JACOBIANO INVERSO  TETA=J^-1.TETA


Vx=1;
Vy=1;
Vz=1;
% [teta1;d2;d3]
VelJuntas=JInverso*[Vx;Vy;Vz]