simulation.m

这是一个能解决一系列机器人手运动学和动力学 方程的matlab符号工具箱。

function simulation

options = odeset('MaxStep',.01);
[T,Y] = ode23(@tet,[0 1.000000],[0.523599 0 1.047198 0 -0.125000 0 ],options);

mw=vrworld('link.wrl');
open(mw);
view(mw);

l1=3.000000;
l2=2.000000;
l3=1.500000;

mw.LS1.size = [l1 .2 .1];
mw.LS2.size = [l2 .2 .1];
mw.LS3.size = [l3 .2 .1];

x1=l1/2*cos(Y(:,1));
y1=l1/2*sin(Y(:,1));
xc1=l1*cos(Y(:,1));
yc1=l1*sin(Y(:,1));

x2=xc1 + l2/2*cos(Y(:,3));
y2=yc1 + l2/2*sin(Y(:,3));
xc2=xc1 + l2*cos(Y(:,3));
yc2=yc1 + l2*sin(Y(:,3));

x3=xc2 + l3/2*cos(Y(:,5));
y3=yc2 + l3/2*sin(Y(:,5));

mw.view.position = [1 0 20];

mw.BG.translation = [0 -10 -40];
mw.BC.translation = [2000 1000 0];
mw.CC1.radius=.2;
mw.CC1.height=.2;
mw.CC2.radius=.2;
mw.CC2.height=.2;
mw.CC3.radius=.3;
mw.CC3.height=.3;
mw.C1.rotation = [1 0 0 pi/2];
mw.C2.rotation = [1 0 0 pi/2];
mw.C3.rotation = [1 0 0 pi/2];
mw.BB.size = [.5 2 .5];
mw.Base.translation = [0 -.5 -.2];
mw.C3.translation = [0 0 0.2];

mw.view.position = [1 0 10];
for i=1:length(Y(:,1))
mw.L1.translation = [x1(i) y1(i) 0];vrdrawnow;
mw.L2.translation = [x2(i) y2(i) 0];vrdrawnow;
mw.L3.translation = [x3(i) y3(i) 0];vrdrawnow;

mw.C1.translation = [xc1(i) yc1(i) 0];vrdrawnow;
mw.C2.translation = [xc2(i) yc2(i) 0];vrdrawnow;

mw.L1.rotation = [0 0 1 Y(i,1)];vrdrawnow;
mw.L2.rotation = [0 0 1 Y(i,3)];vrdrawnow;
mw.L3.rotation = [0 0 1 Y(i,5)];vrdrawnow;
pause(0.1);
end
subplot(3,2,1);
plot(T,Y(:,1))
grid;
title('Teta');
ylabel('Teta(1)');
subplot(3,2,2);
plot(T,Y(:,2))
grid;
title('Teta-dot');
ylabel('Teta-dot-1');

subplot(3,2,3);
plot(T,Y(:,3))
grid;
ylabel('Teta(2)');
subplot(3,2,4);
plot(T,Y(:,4))
grid;
ylabel('Teta-dot-2');

subplot(3,2,5);
plot(T,Y(:,5));
grid;
xlabel('time t');
ylabel('Teta(3)');
subplot(3,2,6);
plot(T,Y(:,6));
grid;
xlabel('time t');
ylabel('Teta-dot-3');

function dy = tet(t,y)
torque2 = 40.000000;
torque3 = 20.000000;
torque4 = -5.000000;
dy = zeros(6,1);
dy(1) = y(2);
dy(2) = (-6122496686959988497059406852585702889936513398576713695003896971264*3^(1/2)*y(6)^2-39028904691258019144395402327078894882148947179687781687592217822276*3^(1/2)*y(4)^2+35811842795882238921819534384156898584463249910463939384491114496*y(2)^2+992117082415648426700281048703689557166541513524970333799289102245045540573848968453/1759218604441600+196716231702639130269449212442174879888957121734798842521553207296*3^(1/2)*torque2-12244993373919976994118813705171405779873026797153427390007793942528*3^(1/2)*y(4)*y(6)-12244993373919976994118813705171405779873026797153427390007793942528*3^(1/2)*y(2)*y(6)-78057809382516038288790804654157789764297894359375563375184435644552*3^(1/2)*y(2)*y(4)-62264486345673600151325431201034622746696921915750142360720546543479*3^(1/2)*y(2)^2+100935198248353653532488140455526296364531498662132522131772145664*y(6)*y(2)+7573132992187034811615268570030482475922285270653730772396260192658981/200*3^(1/2)+159558223561532004910186283829951294755741496344541808857351979008*y(2)*y(4)-29311512656589175688849071687212499195604998841204643362789916672*3^(1/2)*torque3+79779111780766002455093141914975647377870748172270904428675989504*y(4)^2+50467599124176826766244070227763148182265749331066261065886072832*y(6)^2-8242255355110379467543991128754363656894873896039495908847207317504/3*torque4+100935198248353653532488140455526296364531498662132522131772145664*y(4)*y(6)+14019152750342147381831862056411028175340276295123257194098854985728/3*torque3-1847264507484168115661585600148282432108282302111834106737490657280*torque2)/(-178511834297358254279377846658953393823275250307764542165226725300633+43967268984883763533273607530818748793407498261806965044184875008*3^(1/2));
dy(3) = y(4);
dy(4) = (-358851537661205214357746964042469587639120309825669808973949173760*3^(1/2)*torque2+132765884489787201864413326463657533616196571975692343115687919251182*3^(1/2)*y(2)*y(4)+8180674726323882566955500260812486171240133716763138608140415664128*y(6)*y(2)-2684343874047187035914232738739370880728491566341148945933648324036513/10*3^(1/2)+231019916965948346449606540923895007876230537165384232490483673007171*3^(1/2)*y(2)^2+11053202966669621784462709286159311092409423603201193657693827497984*y(2)*y(4)+66382942244893600932206663231828766808098285987846171557843959625591*3^(1/2)*y(4)^2+102590294298062114910971750905243747184617495944216251769764708352*3^(1/2)*torque3+5526601483334810892231354643079655546204711801600596828846913748992*y(4)^2+4090337363161941283477750130406243085620066858381569304070207832064*y(6)^2-29311512656589175688849071687212499195604998841204643362789916672*3^(1/2)*torque4+14019152750342147381831862056411028175340276295123257194098854985728/3*torque4+8180674726323882566955500260812486171240133716763138608140415664128*y(4)*y(6)+10413142365181548424246847016534839241546936722425542167300375838720*3^(1/2)*y(6)^2-58916156397334085674245339910888028072291070304526528130850928721920/3*torque3+21626944277743113705655265062577258628332051150229921091942644449280*torque2+6669750476941788744096468438880943014833306756407577919995720499200*y(2)^2-1067229024400179730112684588966972661204551781899097690080214870243627287193704635653/1759218604441600+20826284730363096848493694033069678483093873444851084334600751677440*3^(1/2)*y(2)*y(6)+20826284730363096848493694033069678483093873444851084334600751677440*3^(1/2)*y(4)*y(6))/(-178511834297358254279377846658953393823275250307764542165226725300633+43967268984883763533273607530818748793407498261806965044184875008*3^(1/2));
dy(5) = y(6);
dy(6) = (280303342932248404643321895669770666706918877919934967860607057920*3^(1/2)*torque2-52482864567033936832632388854328485470560972380228558289678185260646*3^(1/2)*y(2)*y(4)-11124826652261386262306348354927624889578350103022121536462809726976*y(6)*y(2)+43519098905192947384234011338544041800641565894510766172056439768794991/200*3^(1/2)-161882780467064175659930541621835129960978438873362807990642030922956*3^(1/2)*y(2)^2+1296883335166734957337792469302866242744992768172382720882389068808192/25-31032785896472663930361317887284064796387573656160133791434552639488*y(2)*y(4)-26241432283516968416316194427164242735280486190114279144839092630323*3^(1/2)*y(4)^2-358851537661205214357746964042469587639120309825669808973949173760*3^(1/2)*torque3-15516392948236331965180658943642032398193786828080066895717276319744*y(4)^2-5562413326130693131153174177463812444789175051511060768231404863488*y(6)^2+196716231702639130269449212442174879888957121734798842521553207296*3^(1/2)*torque4-1847264507484168115661585600148282432108282302111834106737490657280*torque4-11124826652261386262306348354927624889578350103022121536462809726976*y(4)*y(6)-4118455899220000780881232030794144061401364072096029197123413082112*3^(1/2)*y(6)^2+21626944277743113705655265062577258628332051150229921091942644449280*torque3-261307614412511287548560688213814688654613334718429937852346309007769/4*torque2-23247755748006346311950076842432018270899945962622215494500154867712*y(2)^2-8236911798440001561762464061588288122802728144192058394246826164224*3^(1/2)*y(2)*y(6)-8236911798440001561762464061588288122802728144192058394246826164224*3^(1/2)*y(4)*y(6))/(-178511834297358254279377846658953393823275250307764542165226725300633+43967268984883763533273607530818748793407498261806965044184875008*3^(1/2));