image thumbnail

Robot Simulator

by

 

02 Aug 2007 (Updated )

Simulation Model of Industrial Robot Manipulator

Desenha_robo.m
function Desenha_robo(hObject, eventdata, handles) 

global face1base face2base face3base face4base face5base face6base
global face1elo1 face2elo1 face3elo1 face4elo1 face5elo1 face6elo1
global face1topelo1 face2topelo1 face3topelo1 face4topelo1 face5topelo1 face6topelo1
global face1direlo2 face2direlo2 face3direlo2 face4direlo2 face5direlo2 face6direlo2
global face1bracoelo2 face2bracoelo2 face3bracoelo2 face4bracoelo2 face5bracoelo2
global face1esqelo2 face2esqelo2 face3esqelo2 face4esqelo2 face5esqelo2 face6esqelo2
global face1bracelo3 face2bracelo3 face3bracelo3 face4bracelo3 face5bracelo3 face6bracelo3
global face1bracp face2bracp face3bracp face4bracp face5bracp face6bracp
global face1bracpd face2bracpd face3bracpd face4bracpd face5bracpd face6bracpd
global face1bracpe face2bracpe face3bracpe face4bracpe face5bracpe face6bracpe

global teta1 teta4 teta5 teta6 d2 d3 d4 l1 T06

axes(handles.axes1);
cla; grid on; hold on;
axis([-300 300 -400 400 -30 350]);
cor_base='g';
cor_elo1='b';
cor_elo2='y';
cor_elo3='r';
cor_bp='g';
cor_bpd='b';
cor_bpe='c';
%%%%%%%%%%%%%%%%%%%%%%%%%%teste%%%%%%%%%%
%teta1=pi/2;
%teta4=-pi/2;   %punho
%teta5=pi/2;   %punho direito
%teta6=pi/2;  %punho esquerdo
%d2=185;  %0->  185 valor maximo
%d3=180;   %0-> 250 valor maximo
%d4=25;    %valor considerado no desenho  constante comprimento peca=2*25=50
%l1=110;       %-->valor considerado no desenho   constante 110

%Matrizes de Transformao

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];


%%%%%%%%%%%%%%%%%%%%%%%%%%%Desenha a Base%%%%%%%%%%%%%%%%%%%%%%%%
fill3(face1base(:,1),face1base(:,2),face1base(:,3),cor_base); 
fill3(face2base(:,1),face2base(:,2),face2base(:,3),cor_base);
fill3(face3base(:,1),face3base(:,2),face3base(:,3),cor_base);
fill3(face4base(:,1),face4base(:,2),face4base(:,3),cor_base);
fill3(face5base(:,1),face5base(:,2),face5base(:,3),cor_base);
fill3(face6base(:,1),face6base(:,2),face6base(:,3),cor_base);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%Desenha 1erElo %%%%%%%%%%%%%%%%%%%%%%%%
   obj1elo1=T01*transp(face1elo1);
   obj2elo1=T01*transp(face2elo1);
   obj3elo1=T01*transp(face3elo1);
   obj4elo1=T01*transp(face4elo1);
   obj5elo1=T01*transp(face5elo1);
   obj6elo1=T01*transp(face6elo1);
   obj1topelo1=T01*transp(face1topelo1);
   obj2topelo1=T01*transp(face2topelo1);
   obj3topelo1=T01*transp(face3topelo1);
   obj4topelo1=T01*transp(face4topelo1);
   obj5topelo1=T01*transp(face5topelo1);
   obj6topelo1=T01*transp(face6topelo1);
       
fill3(obj1elo1(1,:),obj1elo1(2,:),obj1elo1(3,:),cor_elo1);
fill3(obj2elo1(1,:),obj2elo1(2,:),obj2elo1(3,:),cor_elo1);
fill3(obj3elo1(1,:),obj3elo1(2,:),obj3elo1(3,:),cor_elo1);
fill3(obj4elo1(1,:),obj4elo1(2,:),obj4elo1(3,:),cor_elo1);
fill3(obj5elo1(1,:),obj5elo1(2,:),obj5elo1(3,:),cor_elo1);
fill3(obj6elo1(1,:),obj6elo1(2,:),obj6elo1(3,:),cor_elo1);
fill3(obj1topelo1(1,:),obj1topelo1(2,:),obj1topelo1(3,:),cor_elo1);
fill3(obj2topelo1(1,:),obj2topelo1(2,:),obj2topelo1(3,:),cor_elo1);
fill3(obj3topelo1(1,:),obj3topelo1(2,:),obj3topelo1(3,:),cor_elo1);
fill3(obj4topelo1(1,:),obj4topelo1(2,:),obj4topelo1(3,:),cor_elo1);
fill3(obj5topelo1(1,:),obj5topelo1(2,:),obj5topelo1(3,:),cor_elo1);
fill3(obj6topelo1(1,:),obj6topelo1(2,:),obj6topelo1(3,:),cor_elo1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%Desenha 2do Elo%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
obj1direlo2=T01*T12*transp(face1direlo2);
obj2direlo2=T01*T12*transp(face2direlo2);
obj3direlo2=T01*T12*transp(face3direlo2);
obj4direlo2=T01*T12*transp(face4direlo2);
obj5direlo2=T01*T12*transp(face5direlo2);
obj6direlo2=T01*T12*transp(face6direlo2);
obj1bracoelo2=T01*T12*transp(face1bracoelo2);
obj2bracoelo2=T01*T12*transp(face2bracoelo2);
obj3bracoelo2=T01*T12*transp(face3bracoelo2);
obj4bracoelo2=T01*T12*transp(face4bracoelo2);
obj5bracoelo2=T01*T12*transp(face5bracoelo2);
obj1esqelo2=T01*T12*transp(face1esqelo2);
obj2esqelo2=T01*T12*transp(face2esqelo2);
obj3esqelo2=T01*T12*transp(face3esqelo2);
obj4esqelo2=T01*T12*transp(face4esqelo2);
obj5esqelo2=T01*T12*transp(face5esqelo2);
obj6esqelo2=T01*T12*transp(face6esqelo2);
     
fill3(obj1direlo2(1,:),obj1direlo2(2,:),obj1direlo2(3,:),cor_elo2);
fill3(obj2direlo2(1,:),obj2direlo2(2,:),obj2direlo2(3,:),cor_elo2);
fill3(obj3direlo2(1,:),obj3direlo2(2,:),obj3direlo2(3,:),cor_elo2);
fill3(obj4direlo2(1,:),obj4direlo2(2,:),obj4direlo2(3,:),cor_elo2);
fill3(obj5direlo2(1,:),obj5direlo2(2,:),obj5direlo2(3,:),cor_elo2);
fill3(obj6direlo2(1,:),obj6direlo2(2,:),obj6direlo2(3,:),cor_elo2);

fill3(obj1bracoelo2(1,:),obj1bracoelo2(2,:),obj1bracoelo2(3,:),cor_elo2);
fill3(obj2bracoelo2(1,:),obj2bracoelo2(2,:),obj2bracoelo2(3,:),cor_elo2);
fill3(obj3bracoelo2(1,:),obj3bracoelo2(2,:),obj3bracoelo2(3,:),cor_elo2);
fill3(obj4bracoelo2(1,:),obj4bracoelo2(2,:),obj4bracoelo2(3,:),cor_elo2);
fill3(obj5bracoelo2(1,:),obj5bracoelo2(2,:),obj5bracoelo2(3,:),cor_elo2);

fill3(obj1esqelo2(1,:),obj1esqelo2(2,:),obj1esqelo2(3,:),cor_elo2);
fill3(obj2esqelo2(1,:),obj2esqelo2(2,:),obj2esqelo2(3,:),cor_elo2);
fill3(obj3esqelo2(1,:),obj3esqelo2(2,:),obj3esqelo2(3,:),cor_elo2);
fill3(obj4esqelo2(1,:),obj4esqelo2(2,:),obj4esqelo2(3,:),cor_elo2);
fill3(obj5esqelo2(1,:),obj5esqelo2(2,:),obj5esqelo2(3,:),cor_elo2);
fill3(obj6esqelo2(1,:),obj6esqelo2(2,:),obj6esqelo2(3,:),cor_elo2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%Desenha 3er Elo%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
obj1bracelo3=T01*T12*T23*transp(face1bracelo3);
obj2bracelo3=T01*T12*T23*transp(face2bracelo3);
obj3bracelo3=T01*T12*T23*transp(face3bracelo3);
obj4bracelo3=T01*T12*T23*transp(face4bracelo3);
obj5bracelo3=T01*T12*T23*transp(face5bracelo3);
obj6bracelo3=T01*T12*T23*transp(face6bracelo3);

fill3(obj1bracelo3(1,:),obj1bracelo3(2,:),obj1bracelo3(3,:),cor_elo3);
fill3(obj2bracelo3(1,:),obj2bracelo3(2,:),obj2bracelo3(3,:),cor_elo3);
fill3(obj3bracelo3(1,:),obj3bracelo3(2,:),obj3bracelo3(3,:),cor_elo3);
fill3(obj4bracelo3(1,:),obj4bracelo3(2,:),obj4bracelo3(3,:),cor_elo3);
fill3(obj5bracelo3(1,:),obj5bracelo3(2,:),obj5bracelo3(3,:),cor_elo3);
fill3(obj6bracelo3(1,:),obj6bracelo3(2,:),obj6bracelo3(3,:),cor_elo3);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%Desenha Punho%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 obj1bracp=T01*T12*T23*T34*transp(face1bracp);
 obj2bracp=T01*T12*T23*T34*transp(face2bracp);
 obj3bracp=T01*T12*T23*T34*transp(face3bracp);
 obj4bracp=T01*T12*T23*T34*transp(face4bracp);
 obj5bracp=T01*T12*T23*T34*transp(face5bracp);
 obj6bracp=T01*T12*T23*T34*transp(face6bracp);
     
fill3(obj1bracp(1,:),obj1bracp(2,:),obj1bracp(3,:),cor_bp);
fill3(obj2bracp(1,:),obj2bracp(2,:),obj2bracp(3,:),cor_bp);
fill3(obj3bracp(1,:),obj3bracp(2,:),obj3bracp(3,:),cor_bp);
fill3(obj4bracp(1,:),obj4bracp(2,:),obj4bracp(3,:),cor_bp);
fill3(obj5bracp(1,:),obj5bracp(2,:),obj5bracp(3,:),cor_bp);
fill3(obj6bracp(1,:),obj6bracp(2,:),obj6bracp(3,:),cor_bp);

 obj1bracpd=T01*T12*T23*T34*T45*transp(face1bracpd);
 obj2bracpd=T01*T12*T23*T34*T45*transp(face2bracpd);
 obj3bracpd=T01*T12*T23*T34*T45*transp(face3bracpd);
 obj4bracpd=T01*T12*T23*T34*T45*transp(face4bracpd);
 obj5bracpd=T01*T12*T23*T34*T45*transp(face5bracpd);
 obj6bracpd=T01*T12*T23*T34*T45*transp(face6bracpd);  
     
fill3(obj1bracpd(1,:),obj1bracpd(2,:),obj1bracpd(3,:),cor_bpd);
fill3(obj2bracpd(1,:),obj2bracpd(2,:),obj2bracpd(3,:),cor_bpd);
fill3(obj3bracpd(1,:),obj3bracpd(2,:),obj3bracpd(3,:),cor_bpd);
fill3(obj4bracpd(1,:),obj4bracpd(2,:),obj4bracpd(3,:),cor_bpd);
fill3(obj5bracpd(1,:),obj5bracpd(2,:),obj5bracpd(3,:),cor_bpd);
fill3(obj6bracpd(1,:),obj6bracpd(2,:),obj6bracpd(3,:),cor_bpd);

 obj1bracpe=T01*T12*T23*T34*T45*T56*transp(face1bracpe);
 obj2bracpe=T01*T12*T23*T34*T45*T56*transp(face2bracpe);
 obj3bracpe=T01*T12*T23*T34*T45*T56*transp(face3bracpe);
 obj4bracpe=T01*T12*T23*T34*T45*T56*transp(face4bracpe);
 obj5bracpe=T01*T12*T23*T34*T45*T56*transp(face5bracpe);
 obj6bracpe=T01*T12*T23*T34*T45*T56*transp(face6bracpe);  
     

fill3(obj1bracpe(1,:),obj1bracpe(2,:),obj1bracpe(3,:),cor_bpe);
fill3(obj2bracpe(1,:),obj2bracpe(2,:),obj2bracpe(3,:),cor_bpe);
fill3(obj3bracpe(1,:),obj3bracpe(2,:),obj3bracpe(3,:),cor_bpe);
fill3(obj4bracpe(1,:),obj4bracpe(2,:),obj4bracpe(3,:),cor_bpe);
fill3(obj5bracpe(1,:),obj5bracpe(2,:),obj5bracpe(3,:),cor_bpe);
fill3(obj6bracpe(1,:),obj6bracpe(2,:),obj6bracpe(3,:),cor_bpe);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T06=T01*T12*T23*T34*T45*T56;

Contact us