📄 robotarm.m.shtml
字号:
<html><head><title>The MathWorks - Gallery</title><link rel=stylesheet href="/site.css" type="text/css"></head><body bgcolor="#FFFFFF" link="0033CC" vlink="#666666"><table width="600" border="0" cellspacing="0" cellpadding="0"> <tr> <td> <!-- Site Navigation --> <!--#include virtual="/mason/sitenav.html" --> <!-- End Site Navigation --> <!-- Right Navigation --> <!--#include virtual="/products/gallery/gallery_top.shtml" --> <!-- End Right Navigation --> <p> <!-- Insert Body Here --><table border="0" cellpadding="0" cellspacing="0"> <tr> <td width=20><img src="/images/pixelwhite.gif" width="20" height="1"></td> <td> <p><font size="+1" face="Arial, Helvetica, sans-serif" color="000066"><b>Kinematic Simulation of a Robot Arm</b></font></p><p class=margin>The data file, qini.mat, is available for <a href="qini.exe">download</a>.<p class=margin><b class=subhead>Main.m</b><br><pre>% *This file demonstrates how simulate and draw% easily a moving mechanism using Matlab and the % automatically generated code from the 'Kinematic% Assistant'.%% This code shows an robot arm of 6 degrees of % freedom moving his hand along an spiral.%% -'qini.mat' contains the initial values of the % coordinates (q) of each point and vector% of the mechanism.% -'initpos' solves de assembly or initial-position% problem for the mechanism, given the values% of the degrees of freedom.% -'findesp' solves the problem of finite displacement% given small variation on the degrees of freedom.%% Author: Juan L. J. Bascones, 1999% e-mail: jljbascones@sportstt.com</pre></td></tr></table></td></tr></table><span class=margin><pre>global q;load qini.mat;initpos([7 0 8 1 0 0]);drawmech;% Loopfor i=[0:10:3*360]; j=pi/180*i; findesp([7+3*cos(j) 3*sin(j) 8+j/2 1 0 0]); drawmech;end;</pre></span><p class=margin><b class=subhead>Initpos.m</b><span class=margin><pre>function answer=initpos(dof,p)% Solve the initial position problem for mechanism: %% - * Robot arm * -%% N DOF: 6% N point and vectors: 11% N paramaters: 0%% Iteration level:% in initpos: FULL% in findesp: FULL% in veloc: FULL% in acel: FULL% Iteration structure: While(...);% Veloc. approx.: No% Symbolic solution: Identity equations% Convergence criterion: Norm of phi% Use of tmp's: Yes%% Matlab code automatically generated by the Kinematic Wizard.% Date: Thursday, September 26 of 1996 at 10:20:40%% Comments:% Author: Juan L. J. Bascones% e-mail: jljbascones@sportstt.com%global q;global tmp1 tmp2 tmp3 tmp4 tmp5;global phi1 phi2 phi3 phi4 phi5;global J1 J2 J3 J4 J5;J1=zeros(3,3);J2=zeros(3,3);J3=zeros(3,3);J4=zeros(3,3);J5=zeros(3,3);TOL=1e-24;NUMITMAX=100;q([1:6 19:24])=[0 0 0 0 0 7.5 dof(1) dof(2) dof(3) dof(4) dof(5) dof(6)];q(1,6)=+q(1,7)-q(1,8);q(2,6)=+q(2,7)-q(2,8);q(3,6)=+q(3,7)-q(3,8);numit=0;phi1=[0.5*(q(:,11)'*q(:,11)-1) q(:,11)'*(q(:,1)-q(:,2))-0 q(:,11)'*(q(:,1)-q(:,6))-0]';while (phi1'*phi1)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J1([1:9])=[q(1,11) q(1,1)-q(1,2) q(1,1)-q(1,6) q(2,11) q(2,1)-q(2,2) q(2,1)-q(2,6) q(3,11) q(3,1)-q(3,2) q(3,1)-q(3,6)]; q([31:33])=q([31:33])'-J1\phi1; phi1=[0.5*(q(:,11)'*q(:,11)-1) q(:,11)'*(q(:,1)-q(:,2))-0 q(:,11)'*(q(:,1)-q(:,6))-0]';end;numit=0;phi2=[0.5*(q(:,9)'*q(:,9)-1) q(:,9)'*q(:,11)-0 q(:,8)'*q(:,9)-0]';while (phi2'*phi2)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J2([1:9])=[q(1,9) q(1,11) q(1,8) q(2,9) q(2,11) q(2,8) q(3,9) q(3,11) q(3,8)]; q([25:27])=q([25:27])'-J2\phi2; phi2=[0.5*(q(:,9)'*q(:,9)-1) q(:,9)'*q(:,11)-0 q(:,8)'*q(:,9)-0]';end;numit=0;phi3=[0.5*(q(:,10)'*q(:,10)-1) q(:,8)'*q(:,10)-0 q(:,9)'*q(:,10)-0]';while (phi3'*phi3)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J3([1:9])=[q(1,10) q(1,8) q(1,9) q(2,10) q(2,8) q(2,9) q(3,10) q(3,8) q(3,9)]; q([28:30])=q([28:30])'-J3\phi3; phi3=[0.5*(q(:,10)'*q(:,10)-1) q(:,8)'*q(:,10)-0 q(:,9)'*q(:,10)-0]';end;numit=0;tmp4=q(:,[2 3])-q(:,[3 6]);phi4=[q(:,11)'*(tmp4(:,1))-0 0.5*((tmp4(:,1))'*(tmp4(:,1))-50.41) 0.5*((tmp4(:,2))'*(tmp4(:,2))-72.25)]';while (phi4'*phi4)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J4([1:9])=[-q(1,11) -tmp4(1,1) tmp4(1,2) -q(2,11) -tmp4(2,1) tmp4(2,2) -q(3,11) -tmp4(3,1) tmp4(3,2)]; q([7:9])=q([7:9])'-J4\phi4; tmp4=q(:,[2 3])-q(:,[3 6]); phi4=[q(:,11)'*(tmp4(:,1))-0 0.5*((tmp4(:,1))'*(tmp4(:,1))-50.41) 0.5*((tmp4(:,2))'*(tmp4(:,2))-72.25)]';end;q(1,4)=+(1+0.196470588)*q(1,3)-0.196470588*q(1,6);q(2,4)=+(1+0.196470588)*q(2,3)-0.196470588*q(2,6);q(3,4)=+(1+0.196470588)*q(3,3)-0.196470588*q(3,6);numit=0;tmp5=q(:,[2 5])-q(:,[5 4]);phi5=[q(:,11)'*(tmp5(:,1))-0 0.5*((tmp5(:,1))'*(tmp5(:,1))-2.7889) 0.5*((tmp5(:,2))'*(tmp5(:,2))-50.41)]';while (phi5'*phi5)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J5([1:9])=[-q(1,11) -tmp5(1,1) tmp5(1,2) -q(2,11) -tmp5(2,1) tmp5(2,2) -q(3,11) -tmp5(3,1) tmp5(3,2)]; q([13:15])=q([13:15])'-J5\phi5; tmp5=q(:,[2 5])-q(:,[5 4]); phi5=[q(:,11)'*(tmp5(:,1))-0 0.5*((tmp5(:,1))'*(tmp5(:,1))-2.7889) 0.5*((tmp5(:,2))'*(tmp5(:,2))-50.41)]';end;answer=1;</pre></span><p class=margin><b class=subhead>Findesp.m</b><span class=margin><pre>function answer=fd1(dof,p)% Solve the finite displacement problem for mechanism: %% - * Robot arm * -%% N DOF: 6% N point and vectors: 11% N paramaters: 0%% Iteration level:% in initpos: FULL% in findesp: FULL% in veloc: FULL% in acel: FULL% Iteration structure: While(...);% Veloc. approx.: No% Symbolic solution: Identity equations% Convergence criterion: Norm of phi% Use of tmp's: Yes%% Matlab code automatically generated by the Kinematic Wizard.% Date: Thursday, September 26 of 1996 at 10:20:39%% Comments:% Author: Juan L. J. Bascones% e-mail: jljbascones@sportstt.com%global q;global tmp1 tmp2 tmp3 tmp4 tmp5;global phi1 phi2 phi3 phi4 phi5;global J1 J2 J3 J4 J5;TOL=1e-24;NUMITMAX=100;q([19:24])=[dof(1) dof(2) dof(3) dof(4) dof(5) dof(6)];q(1,6)=+q(1,7)-q(1,8);q(2,6)=+q(2,7)-q(2,8);q(3,6)=+q(3,7)-q(3,8);numit=0;phi1=[0.5*(q(:,11)'*q(:,11)-1) q(:,11)'*(q(:,1)-q(:,2))-0 q(:,11)'*(q(:,1)-q(:,6))-0]';while (phi1'*phi1)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J1([1:9])=[q(1,11) q(1,1)-q(1,2) q(1,1)-q(1,6) q(2,11) q(2,1)-q(2,2) q(2,1)-q(2,6) q(3,11) q(3,1)-q(3,2) q(3,1)-q(3,6)]; q([31:33])=q([31:33])'-J1\phi1; phi1=[0.5*(q(:,11)'*q(:,11)-1) q(:,11)'*(q(:,1)-q(:,2))-0 q(:,11)'*(q(:,1)-q(:,6))-0]';end;numit=0;phi2=[0.5*(q(:,9)'*q(:,9)-1) q(:,9)'*q(:,11)-0 q(:,8)'*q(:,9)-0]';while (phi2'*phi2)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J2([1:9])=[q(1,9) q(1,11) q(1,8) q(2,9) q(2,11) q(2,8) q(3,9) q(3,11) q(3,8)]; q([25:27])=q([25:27])'-J2\phi2; phi2=[0.5*(q(:,9)'*q(:,9)-1) q(:,9)'*q(:,11)-0 q(:,8)'*q(:,9)-0]';end;numit=0;phi3=[0.5*(q(:,10)'*q(:,10)-1) q(:,8)'*q(:,10)-0 q(:,9)'*q(:,10)-0]';while (phi3'*phi3)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J3([1:9])=[q(1,10) q(1,8) q(1,9) q(2,10) q(2,8) q(2,9) q(3,10) q(3,8) q(3,9)]; q([28:30])=q([28:30])'-J3\phi3; phi3=[0.5*(q(:,10)'*q(:,10)-1) q(:,8)'*q(:,10)-0 q(:,9)'*q(:,10)-0]';end;numit=0;tmp4=q(:,[2 3])-q(:,[3 6]);phi4=[q(:,11)'*(tmp4(:,1))-0 0.5*((tmp4(:,1))'*(tmp4(:,1))-50.41) 0.5*((tmp4(:,2))'*(tmp4(:,2))-72.25)]';while (phi4'*phi4)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J4([1:9])=[-q(1,11) -tmp4(1,1) tmp4(1,2) -q(2,11) -tmp4(2,1) tmp4(2,2) -q(3,11) -tmp4(3,1) tmp4(3,2)]; q([7:9])=q([7:9])'-J4\phi4; tmp4=q(:,[2 3])-q(:,[3 6]); phi4=[q(:,11)'*(tmp4(:,1))-0 0.5*((tmp4(:,1))'*(tmp4(:,1))-50.41) 0.5*((tmp4(:,2))'*(tmp4(:,2))-72.25)]';end;q(1,4)=+(1+0.196470588)*q(1,3)-0.196470588*q(1,6);q(2,4)=+(1+0.196470588)*q(2,3)-0.196470588*q(2,6);q(3,4)=+(1+0.196470588)*q(3,3)-0.196470588*q(3,6);numit=0;tmp5=q(:,[2 5])-q(:,[5 4]);phi5=[q(:,11)'*(tmp5(:,1))-0 0.5*((tmp5(:,1))'*(tmp5(:,1))-2.7889) 0.5*((tmp5(:,2))'*(tmp5(:,2))-50.41)]';while (phi5'*phi5)>TOL; numit=numit+1;if numit>NUMITMAX;answer=0;return;end; J5([1:9])=[-q(1,11) -tmp5(1,1) tmp5(1,2) -q(2,11) -tmp5(2,1) tmp5(2,2) -q(3,11) -tmp5(3,1) tmp5(3,2)]; q([13:15])=q([13:15])'-J5\phi5; tmp5=q(:,[2 5])-q(:,[5 4]); phi5=[q(:,11)'*(tmp5(:,1))-0 0.5*((tmp5(:,1))'*(tmp5(:,1))-2.7889) 0.5*((tmp5(:,2))'*(tmp5(:,2))-50.41)]';end;answer=1;</pre></span><p class=margin><b class=subhead>Drawmech.m</b><span class=margin><pre>function drawmech% DRAWMECH - Draw the mechanism:%% - * Robot arm * -%% Matlab code automatically generated by the Kinematic Wizard.% Date: Sunday, August 31 of 1997 at 15:24:49global q drawdata;vS=3;handle=findobj('Type','figure','Name','DrawMech');if isempty(handle); x0=-5; y0=-10; z0= 0; range=20; linewidth=0.1; pointwidth=1; figure('Name','DrawMech','NumberTitle','Off','BackingStore','Off','Color','k'); set(gca,'DrawMode','Fast'); set(gca,'position',[0.01 0.1 0.99 1.1],'units','normalized') set(gca,'color','k','xcolor','w','ycolor','w','zcolor','w'); axis('square'); axis(1*[x0 x0+range y0 y0+range z0 z0+range]); view(30,30); hold on; drawdata(1)=line(q(1,[1 2]),q(2,[1 2]),q(3,[1 2]),'Color','r','LineStyle','-','LineWidth',linewidth,'EraseMode','xor'); drawdata(2)=line(q(1,[2 3]),q(2,[2 3]),q(3,[2 3]),'Color','g','LineStyle','-','LineWidth',linewidth,'EraseMode','xor'); drawdata(3)=line(q(1,[2 5]),q(2,[2 5]),q(3,[2 5]),'Color','b','LineStyle','-','LineWidth',linewidth,'EraseMode','xor'); drawdata(4)=line(q(1,[5 4]),q(2,[5 4]),q(3,[5 4]),'Color','m','LineStyle','-','LineWidth',linewidth,'EraseMode','xor'); drawdata(5)=line(q(1,[4 6]),q(2,[4 6]),q(3,[4 6]),'Color','y','LineStyle','-','LineWidth',linewidth,'EraseMode','xor'); drawdata(6)=line(q(1,[7 6]),q(2,[7 6]),q(3,[7 6]),'Color','c','LineStyle','-','LineWidth',linewidth,'EraseMode','xor'); drawdata(7)=plot3(q(1,7),q(2,7),q(3,7),'Color','r','Marker','o','LineWidth',pointwidth,'EraseMode','none'); drawdata(8)=line([q(1,7) vS*q(1,8)+q(1,7)],[q(2,7) vS*q(2,8)+q(2,7)],[q(3,7) vS*q(3,8)+q(3,7)],'Color','w','LineStyle','-','Marker','>','LineWidth',linewidth,'EraseMode','xor'); drawdata(9)=line([q(1,7) vS*q(1,9)+q(1,7)],[q(2,7) vS*q(2,9)+q(2,7)],[q(3,7) vS*q(3,9)+q(3,7)],'Color','w','LineStyle','-','Marker','>','LineWidth',linewidth,'EraseMode','xor'); drawdata(10)=line([q(1,7) vS*q(1,10)+q(1,7)],[q(2,7) vS*q(2,10)+q(2,7)],[q(3,7) vS*q(3,10)+q(3,7)],'Color','w','LineStyle','-','Marker','>','LineWidth',linewidth,'EraseMode','xor'); drawdata(11)=line([q(1,2) vS*q(1,11)+q(1,2)],[q(2,2) vS*q(2,11)+q(2,2)],[q(3,2) vS*q(3,11)+q(3,2)],'Color','w','LineStyle','-','Marker','>','LineWidth',linewidth,'EraseMode','xor'); drawnow; return;end;set(drawdata(1),'Xdata',q(1,[1 2]),'Ydata',q(2,[1 2]),'Zdata',q(3,[1 2]));set(drawdata(2),'Xdata',q(1,[2 3]),'Ydata',q(2,[2 3]),'Zdata',q(3,[2 3]));set(drawdata(3),'Xdata',q(1,[2 5]),'Ydata',q(2,[2 5]),'Zdata',q(3,[2 5]));set(drawdata(4),'Xdata',q(1,[5 4]),'Ydata',q(2,[5 4]),'Zdata',q(3,[5 4]));set(drawdata(5),'Xdata',q(1,[4 6]),'Ydata',q(2,[4 6]),'Zdata',q(3,[4 6]));set(drawdata(6),'Xdata',q(1,[7 6]),'Ydata',q(2,[7 6]),'Zdata',q(3,[7 6]));set(drawdata(7),'Xdata',q(1,7),'Ydata',q(2,7),'Zdata',q(3,7));set(drawdata(8),'Xdata',[q(1,7) vS*q(1,8)+q(1,7)],'Ydata',[q(2,7) vS*q(2,8)+q(2,7)],'Zdata',[q(3,7) vS*q(3,8)+q(3,7)]);set(drawdata(9),'Xdata',[q(1,7) vS*q(1,9)+q(1,7)],'Ydata',[q(2,7) vS*q(2,9)+q(2,7)],'Zdata',[q(3,7) vS*q(3,9)+q(3,7)]);set(drawdata(10),'Xdata',[q(1,7) vS*q(1,10)+q(1,7)],'Ydata',[q(2,7) vS*q(2,10)+q(2,7)],'Zdata',[q(3,7) vS*q(3,10)+q(3,7)]);set(drawdata(11),'Xdata',[q(1,2) vS*q(1,11)+q(1,2)],'Ydata',[q(2,2) vS*q(2,11)+q(2,2)],'Zdata',[q(3,2) vS*q(3,11)+q(3,2)]);figure(handle);drawnow;</pre></span> <!-- End Body --> <!-- Footer --> <!--#include virtual="/products/gallery/gallery_footer.shtml" --> <!-- End Footer --> </body></html>⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -