Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- % this script determines a Homogeneous Transformation HT matrix
- clear all
- % declaration of symbols
- syms d1 a1 a2 th2 d3 q1 q2 q3 th4 a4 a5 al1 al4 q4 q5 th5 a3
- % determination of a symbolic form of HT matrices –
- % application of mA function
- A1=mA(0,d1,a1,al1);
- A2=mA(th2,0,a2,0);
- A3=mA(0,d3,0,0);
- A4=mA(th4,0,a4,al4);
- A5=mA(th5,0,a5,0);
- % multiplication of matrices
- T01=A1;
- T02=A1*A2;;
- T03=A1*A2*A3;
- T04=A1*A2*A3*A4;
- T35=A4*A5;
- T05=A1*A2*A3*A4*A5;
- % substitution of rotational joint variables
- % for the simplification purpose
- T01v=subs(T02,{al1},{q1});
- T02v=subs(T02,{th2},{q2});
- T03v=subs(T03,{al1,th2},{q1,q2});
- T04v=subs(T04,{al1,th2,th4,al4},{q1,q2,q3,q4})
- T35v=subs(T35,{th4,th5},{q4,q5});
- T05v=subs(T05,{al1,th2,th4,al4,th5},{q1,q2,q3,q4,q5});
- % indication of joint coordinates
- % variables: d1,th2,a3,th4 and al5 indicated by ‘1’s
- zmie1=[[0,1,0,0]];
- zmie2=[[0,1,0,0];[1,0,0,0]];
- zmie3=[[0,1,0,0];[1,0,0,0];[0,1,0,0]];
- zmie4=[[0,1,0,0];[1,0,0,0];[0,1,0,0];[1,0,0,0]];
- zmie5=[[0,1,0,0];[1,0,0,0];[0,1,0,0];[1,0,0,0];[1,0,0,0]];
- zmie6=[[1,0,0,0];[1,0,0,0]];
- % a simplified form of the evaluated HT matrices
- % for interpretation purpose for a user
- T01u=zam(zmie1,T01v,'q');
- T02u=zam(zmie2,T02v,'q');
- T03u=zam(zmie3,T03v,'q');
- T04u=zam(zmie4,T04v,'q');
- T05u=zam(zmie5,T05v,'q');
- T35u=zam(zmie6,T35v,'q');
- % numerical matrices
- syms d1_V th2_V d3_V th4_V th5_V
- %variables
- d1_V = 900;
- th2_V = -pi/3;
- d3_V = 500;
- th4_V = pi/4;
- th5_V = pi/3;
- T01n=double(subs(T01,{d1,a1,al1},{d1_V,500,pi/2}));
- T02n=double(subs(T02,{d1,a1,al1,th2,a2},{d1_V,500,pi/2,th2_V,500}));
- T03n=double(subs(T03,{d1,a1,al1,th2,a2,d3},{d1_V,500,pi/2,th2_V,500,d3_V}));
- T04n=double(subs(T04,{d1,a1,al1,th2,a2,d3,th4,a4,al4},{d1_V,500,pi/2,th2_V,500,d3_V,th4_V,500,-pi/2}));
- T05n=double(subs(T05,{d1,a1,al1,th2,a2,d3,th4,a4,al4,th5,a5},{d1_V,500,pi/2,th2_V,500,d3_V,th4_V,500,-pi/2,th5_V,500}));
- K=[0 0 0 1];
- T01c=T01n.*K;
- T02c=T02n.*K;
- T03c=T03n.*K;
- T04c=T04n.*K;
- T05c=T05n.*K;
- figure(1)
- grid on
- title('Manipulator')
- xlabel('X-axis')
- ylabel('Y-axis')
- zlabel('Z-axis')
- text(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'Origin')
- text(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'')
- plot3([double(T01c(1,4)) double(T02c(1,4))],[double(T01c(2,4)) double(T02c(2,4))],[double(T01c(3,4)) double(T02c(3,4))])
- hold on
- plot3([double(T02c(1,4)) double(T03c(1,4))],[double(T02c(2,4)) double(T03c(2,4))],[double(T02c(3,4)) double(T03c(3,4))])
- hold on
- plot3([double(T03c(1,4)) double(T04c(1,4))],[double(T03c(2,4)) double(T04c(2,4))],[double(T03c(3,4)) double(T04c(3,4))])
- hold on
- plot3([double(T04c(1,4)) double(T05c(1,4))],[double(T04c(2,4)) double(T05c(2,4))],[double(T04c(3,4)) double(T05c(3,4))])
- plot3(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'-o')
- plot3(double(T02c(1,4)),double(T02c(2,4)),double(T02c(3,4)),'-o')
- plot3(double(T03c(1,4)),double(T03c(2,4)),double(T03c(3,4)),'-o')
- plot3(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'-o')
- plot3(double(T05c(1,4)),double(T05c(2,4)),double(T05c(3,4)),'-d')
- axis([0 2000 -1000 1000 -1000 1000])
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement