1
Matematický popis laboratorního modelu rotačního kyvadla Vojtěch Myslivec, ČVUT FEL,
[email protected] Abstrakt — Tento článek pojednává o principu získání matematického popisu laboratorního modelu rotačního kyvadla, který je umístěn v laboratoři KN-K26 Českého vysokého učení technického, Fakulty elektrotechnické, na Karlově náměstí. Ukazuje princip získání matematického popisu modelu, který co možná nejvíce odpovídá skutečnosti. Klíčová slova — rotační kyvadlo, vazební grafy, matematický model, simulace, Langrangeovy rovnice.
kyvadlo, na jehož konci je volně zavěšeno další. Poloha obou kyvadel je snímána senzorem S.
III. MATEMATICKÝ POPIS SYSTÉMU Systém dvojitého rotačního kyvadla popíšeme pomocí Lagrangeových rovnic druhého druhu. Analýzou systému, jeho popisem pomocí souřadnicových systémů a určením podmínek omezujících pohyb jednotlivých částí dostáváme následující schéma (obrázek 2).
I. ÚVOD DO PROBLÉMU RO účely řízení různorodých systémů, jako jsou například jeřáby, robotická ramena a mnohé další, potřebujeme často znát matematický popis jejich chování. Tento popis následně poslouží při návrhu regulátorů, pro simulace chování a podobně. Čím blíže odpovídá matematický model skutečnosti, tím přesnější jsou následné simulace a další výpočty na něm prováděné.
P
II. POPIS MODELOVANÉHO SYSTÉMU Naším úkolem je nalézt a odsimulovat matematický popis laboratorního modelu rotačního kyvadla. Principielní schéma tohoto laboratorního modelu je na následujícím obrázku 1.
Obrázek 2. Popis systému pomocí souřadnicových systémů.
V bodech 0, A a B jsou jednotlivé části spojeny ložisky. Díky tomu se mohou otáčet pouze v rovině a navíc také pouze po kružnici. Například pro bod A dostáváme omezující podmínky ve tvaru (1) (2) Obrázek 1. Schéma laboratorního modelu rotačního kyvadla.
Systém tvoří pevný podstavec, na kterém je umístěn motor otáčející ramenem. Na tomto rameni je volně zavěšeno
a podobně pro ostatní body. Celý systém tak můžeme popsat pouze třemi zobecněnými souřadnicemi (vycházíme ze vztahu 3 x počet bodů – počet vazeb, tedy 9 – 6 = 3). Souřadnice, kterými stav systému popíšeme, jsou souřadnice ψ, φ a ϴ tak, jak je vidět na obrázku 2.
Publikováno dne 21. 12. 2012.
Dále systému přiřadíme celkem 4 souřadnicové systémy. Nultý umístíme do středu otáčení vodorovného nosníku. První
2 umístíme do bodu uchycení prvního kyvadla tak, že osa z 1 je rovnoběžná s osou z0 a osa x1 směřuje od počátku nultého souřadnicového systému. Druhý souřadnicový systém bude reprezentovat rotaci prvního kyvadla, osa z2 bude totožná s podélnou osou tohoto kyvadla a osa x2 bude totožná s osou x1. Poslední, třetí souřadnicový systém bude reprezentovat rotaci druhého kyvadla, tedy osa z3 bude totožná s podélnou osou druhého kyvadla a osa x3 bude rovnoběžná s osou x2. Zobecněná souřadnice ψ v této konfiguraci vyjadřuje úhel natočení vodorovného nosníku, φ vyjadřuje úhel výchylky prvního kyvadla od vztyčené polohy a ϴ úhel výchylky druhého kyvadla od osy prvního kyvadla.
IV. ZÍSKÁNÍ KINETICKÉ A POTENCIÁLNÍ ENERGIE Pro vyjádření Lagrangeových rovnic druhého druhu potřebujeme znát kinetickou a potenciální energii systému. Během výpočtů budeme často potřebovat vyjádřit polohu bodu v jednom souřadnicovém systému v jiném souřadnicovém systému. To zajištují takzvané rotační matice, které mají tvar
a jejich součtem dostáváme celkovou potenciální energii . (8) Kynetickou energii opět zíkáme sečtením kynetických energií všech částí systému. Kynetická energii každé části se ještě bude skládat z rotační kynetické energie (index R) a translační kynetické energie (index T). Pro celkovou kynetickou energii tedy můžeme psát .
Budeme se snažit všechny rovnice psát z pohledu nultého souřadnicového systému za použití rotačních matic. Tím získáme náhled na celý sstém z jediného bodu, což je nutné pro správné sestavení Lagrangeových rovnic. Translační kynetická energie vodorovného nosníku je nulová, neboť se nosník otáčí pouze kolem svého středu, tedy TDT = 0. Rotační kynetickou energii vyjádříme pomocí druhého newtonova zákona pro rotující těleso ,
,
(3)
,
(4)
.
(5)
(10)
kde ω010 je úhlová rychlost otáčení soustavy 1 vůči soustavě 0 (tedy nosníku) vyjádřená v souřadnicích soustavy 0 (tato notace bude také dále standardně používána v textu), a vyjádříme jí jako
.
Pokud souřadnice v jednom souřadnicovém systému vynásobíme příslušnou rotační maticí, dostáváme jejich vyjádření v jiném souřadnicovém systému. Notace u R říká, že rotační matice rotuje ze systému v dolním indexu matice do systému v horním indexu matice. Kombinací těchto matic (násobením) lze vyjádřit souřadnice v libovolné souřadnicové soustavě. Navíc transpozicí rotační matice dostáváme matici rotující v opačném směru (tedy například z namísto původní rotace 1 2 po transpozici matice zajišťuje rotaci 2 1). Potenciální energii získáme jednoduše. Budeme jí vztahovat k bodu nula na ose z0. Bude součtem pouze potenciální energie obou kyvadel, protože potenciální energie vodorovného nosníku se nemění. Pro jednotlivé potenciální energie dostáváme ,
(6) ,
(9)
(11)
Tento tvar vyplývá z obrázku 2, soustava 1 rotuje kolem soustavy 0 okolo osy z, proto jsou první dva řádky nulové. A úhlová rychlost se rovná derivaci úhlu. JD0 je tenzor setrvačnosti nosníku vyjádřený v souřadnicové soustavě 0. Ten odvodíme později (stejně jako další tenzory níže). Translační kynetickou energii prvního kyvadla K1 vyjádříme jako ,
(12)
kde vk10 je translační rychlost prvního kyvadla vyjádřená v souřadnicích nultého systému a r je vektor rozkádající vzdálenost bodu A od středu 0 do souřadnic nultého systému. Vypočteme jí jako
,
(13)
(7) kde l1 je délka prvního kyvadla a ω121 snadno vyjádříme jako
3
.
(14) V.
Rotační kynetickou energii prvního kyvadla napíšeme jako ,
(15)
kde ω020 je úhlová rychlost druhého souřadnicového systému vůči nultému. Opět tuto rychlost vyjádříme pomocí snadno určitelné rychlosti systému 2 vůči prvnímu a aplikace příslušné rotační matice. K ní pak přičteme rychlost otáčení prvního systému vůči nultému. Tedy .
(16)
VÝPOČET TENZORŮ SETRVAČNOSTI
Tenzor setrvačnosti pro trojrozměrný prostor je matice o třech sloupcích a třech řádcích. Každý prvek matice vyjadřuje setrvačnost tělesa vůči příslušné ose rotace. Vzhledem k tomu, že v našem případě rotujeme pouze kolem tří hlavních os x, y, z, bude mít tenzor setrvačnosti obecně tvar
.
(22)
Prvky na diagonále vyjadřují setrvačnost tělesa vůči danné ose rotace. Obecně lze tuto setrvačnost vypočítat podle vztahů ,
(23)
,
(24)
,
(25)
Zbývá určit kynetické energie druhého kyvadla. Začneme rotační energií, tu napíšeme jako ,
(17)
kde ω030 je úhlová rychlost třetího souřadnicového systému vůči nultému, tedy rychlost druhého kyvadla z pohledu nultého souřadnicového systému. Analogicky s předchozím případem píšeme ,
kde je hustota materiálu příslušného tělesa. Pro všechna tělesa, tedy dvě kyvadla a vodorovný rotující nosník, vzhledem ke stejným tvarům, a pokud pracujeme s obecnými rozměry, dostáváme
(18)
pouze v tomto případě obsahuje rovnice o jeden člen navíc, protože jsme „přidali“ další těleso (druhé kyvadlo). Také vyjádření v nultém souřadnicovém systému vyžaduje použití dvou rotačních matic. Zbývá vyjádřit translační kynetickou energii druhého kyvadla. Tu dostaneme jako ,
,
(26)
,
(27)
.
(28)
Konkrétně pak pro obě kyvadla dostáváme
(19)
opět vycházíme z druhého newtonova zákona. vk20 je translační rychlost druhého kyvadla vyjádřená v souřadnicích nultého systému, vypočteme jí jako
,
(29)
,
(30)
.
(31)
, (20)
kde l2 je délka druhého kyvadla a ω232 snadno vyjádříme jako .
(21)
Obrázek 3. Rozměry těles vzhledem k souřadnicovým soustavám, vlevo kyvadlo, vpravo nosník.
4 . Pro nosník dostáváme obdobně ,
(32)
,
(33)
.
(34)
Vzálenost l je vždy vzdálenost od počátku jednoho souřadnicového systému k počátku druhého, tedy vzdálenost otočných uchycení. Výš uvedené vztahy předpokládají rovnoměrné rozložení hmoty, což pro fyzické nosníky neplatí, protože jsou duté, nicméně můžeme je takto aproximovat vzhledem k tomu, že vždy rotujeme kolem os. Výslednou hustotu pro výpočet pak dostaneme vydělením hmotnosti objemem. Na rozdíl od kyvadel, která mají tvar uvedený na obrázku 3 vlevo, tvar nosníku uvedený na tomto obrázku vpravo je pouze proximací. Ve skutečnosti se jedná o rameno, které je na druhém konci vyvýženo závažím. Tato konstrukce se ale snaží docílit vyváženého stavu a tedy námi použitá aproximace bude dostatečně přesná.
(39)
Pokud známe tvar funkce L, pak již můžeme přistoupit k vyjádření rovnic popisující systém. Vzhledem ke třem zobecněným souřadnicím bude systém popsán právě třemi diferenciálními rovnicemi. Obecně můžeme psát ,
(40)
,
(41)
.
(42)
Vzhledem ke složitosti odvozených rovnic provedeme výpočet jednotlivých parciálních derivací, dosazení konkrétních hodnot parametrů i výpočet celých rovnic v MatLabu. Kód pro zpracování je uveden v příloze 1. Výsledné rovnice jsou, vzhledem k jejich rozsáhlosti, uvedeny v příloze 2.
VII.
SIMULACE VÝSLEDKŮ
Simulací těchto rovnic v programu Open Modelica (viz příloha 3), dostáváme následující průběhy
Obecně tedy
,
(35)
,
(36)
Obrázek 4. Simulace pro úhel
tenzor (36) platí obecně pro obě kyvadla, bude se lišit až dosazenými konstantami. Výsledné tenzory setrvačnosti je ještě třeba transformovat do nultého souřadnicového systému pomocí rotačních matic, tedy
Obrázek 5. Simulace pro úhel
(37) (38) Tenzor setrvačnosti pro vodorovný nosník není třeba rotovat. VI.
VÝSLEDNÉ ROVNICE SYSTÉMU
Výše jsme tedy odovodili vztahy pro celkovou kynetickou energii T a celkovou potenciální energii U. Pro odvození Lagrangeových rovnic potřebujeme vyjádřit takzvanou Lagrangeovu funkci. Tu vyjádříme jako
Obrázek 6. Simulace pro úhel
5 VIII.
ZHODNOCENÍ VÝSLEDKŮ
Z dosud neznámých důvodů nebylo možné simulovat v programu Open Modelica delší čas než zhruba 5 sekund při zachování rozumné tolerance. Pravděpodobně je ještě někde v kódu chyba, kterou se mi nepodařilo nalézt. Vzhledem k tomu, že tento nástroj je freeware, má stále řadu nedokonalostí. I díky tomu jsem nad simulací strávil přes 20 hodin, přesto jsem chybu nenalezl. Nicméně z grafu je vidět, že se simulace zhruba chová dle očekávání. Provedl jsem další experimenty, jako například kývání okolo svislé polohy a podobně. Postup se mi jeví jako správný, nicméně někde došlo k chybě. Vzhledem k rozsáhlosti rovnic (viz příloha) je téměř nemožné dopracovat se k výsledným rovnicím ručně, natož pak provést například klasickou simulaci v Simulinku. Zde se tedy ukazuje síla některých výpočetních nástrojů, jako je MatLab, Modelica a podobně. Nicméně v mém případě mne spíše zklamaly, především Open Modelica, která má potíže s kopírováním a vkládáním textu nebo s výpisem chybových hlášek, který není zdaleka dokonalý. Během odvozování modelu jsem provedl minimum zjednodušení a aproximací, tedy v případě odstranění výše zmíněné chyby (chyb) lze model považovat za velmi přesný.
Nicméně jako možnosti zlepšení bych viděl přesnější určení momentu setrvačnosti rotujícího vodorovného nosníku, u kterého jsem provedl nahrazení obdélníkovým profilem. Jeho přesnější odvození za použití skutečných fyzikálních parametrů by mohlo vést ke zpřesnění modelu. V případě nutnosti simulace řízení modelu motorem by bylo třeba modelovat také tento motor, pravděpodobně za použití vazebních grafů. Nicméně to vyžaduje velmi podrobnou studii jednotlivých součástí pohonu, tedy PWM regulace, samotného motoru, ozubených kol převodu atd. Jistým zjednodušením by mohlo být nahrazení tohoto modelu převodní charakteristikou mezi vstupním napětím a momentem na výstupu. Tato úloha je teoreticky velmi zajímavá, při praktickém řešení časově náročná.
IX. REFERENCE [1] Hurak, Zdenek: Mathematical modeling using Lagrange approach, 2012
6 Příloha 1 – Zpracování pomocí MatLabu %Double-inverse-rotation pendulum %Vojtech Myslivec, CTU FEE, 2012 clear; clc; %deklarace promennych syms t psi(t) phi(t) theta(t); dpsidt(t) = diff(psi(t), t); dphidt(t) = diff(phi(t), t); dthetadt(t) = diff(theta(t), t); syms l1 l2 r m1 m2 g; syms rod rok1 rok2; syms xd yd ld xk1 yk1 lk1 xk2 yk2 lk2; %vypocet rotacnich matic R01 = [cos(psi(t)) -sin(psi(t)) 0; sin(psi(t)) cos(psi(t)) 0; 0 0 1]; R12 = [1 0 0; 0 cos(phi(t)) -sin(phi(t)); 0 sin(phi(t)) cos(phi(t))]; R23 = [1 0 0; 0 cos(theta(t)) -sin(theta(t)); 0 sin(theta(t)) cos(theta(t))]; %vypocet omega010 omega121 omega232 omega020 omega030
uhlovych rychlosti = [0; 0; dpsidt(t)]; = [dphidt(t); 0; 0]; = [dthetadt(t); 0; 0]; = R01*omega121 + omega010; = omega010 + R01*omega121 + R01*R12*omega232;
%vypocet translacnich rychlosti vk10 = R01*cross(omega121.',[0; 0; l1/2]).'; vk20 = R01*cross(omega121.',[0; 0; l1] + R12*cross(omega232.',[0; 0; l2/2]).').'; %vypocet tenzoru setrvacnosti jxxk1 = rok1*(4/3*xk1*yk1^3*lk1 + 4/3*xk1*yk1*lk1^3); jyyk1 = rok1*(4/3*xk1^3*yk1*lk1 + 4/3*xk1*yk1*lk1^3); jzzk1 = rok1*(4/3*xk1*yk1^3*lk1 + 4/3*xk1^3*yk1*lk1); jxxk2 = rok2*(4/3*xk2*yk2^3*lk2 + 4/3*xk2*yk2*lk2^3); jyyk2 = rok2*(4/3*xk2^3*yk2*lk2 + 4/3*xk2*yk2*lk2^3); jzzk2 = rok2*(4/3*xk2*yk2^3*lk2 + 4/3*xk2^3*yk2*lk2); jxxd = rod*(4/3*xd*yd^3*ld+ 4/3*xd*yd*ld^3); jyyd = rod*(4/3*xd^3*yk1*ld + 4/3*xd*yd*ld^3); jzzd = rod*(4/3*xd*yd^3*ld + 4/3*xd^3*yd*ld); Jd = [jxxd 0 0; 0 jyyd 0; 0 0 jzzd]; Jk1 = R01*[jxxk1 0 0; 0 jyyk1 0; 0 0 jzzk1]*R01.'; Jk2 = R01*R12*[jxxk2 0 0; 0 jyyk2 0; 0 0 jzzk2]*R12.'*R01.'; %vypocet jednotlivych slozek kineticke energie Tdr = 1/2*omega010.'*Jd*omega010; Tdt = 0; Tk1r = 1/2*omega020.'*Jk1*omega020; Tk1t = 1/2*(vk10.' + cross(omega010, [r*cos(psi(t)) r*sin(psi(t)) 0]))*(vk10.' + cross(omega010, [r*cos(psi(t)) r*sin(psi(t)) 0])).'*m1; Tk2r = 1/2*omega030.'*Jk2*omega030; Tk2t = 1/2*(vk20.' + cross(omega010, [r*cos(psi(t)) r*sin(psi(t)) 0]))*(vk20.' + cross(omega010, [r*cos(psi(t)) r*sin(psi(t)) 0])).'*m2; T = Tdr + Tdt + Tk1r + Tk1t + Tk2r + Tk2t; %vypocet potencialni energie U = l1*cos(phi)*g*(1/2*m1 + m2) + 1/2*l2*cos(phi(t) + theta(t))*m2*g; %vypocet L L = T - U; %derivovani a vysledne rovnice L = subs(L, {dthetadt, dphidt, dpsidt, theta, psi, phi}, {'dthetadt', 'dphidt', 'dpsidt', 'theta', 'psi', 'phi'}); dLdpsi = diff(L, 'dpsidt'); dLdphi = diff(L, 'dphidt'); dLdtheta = diff(L, 'dthetadt'); dLpsi = diff(L, 'psi'); dLphi = diff(L, 'phi'); dLtheta = diff(L, 'theta'); dLdpsidt = diff(subs(dLdpsi, {'dthetadt', 'dphidt', 'dpsidt', 'theta', 'psi', 'phi'}, {dthetadt, dphidt, dpsidt, theta, psi, phi}), t); dLdphidt = diff(subs(dLdphi, {'dthetadt', 'dphidt', 'dpsidt', 'theta', 'psi', 'phi'}, {dthetadt, dphidt, dpsidt, theta, psi, phi}), t); dLdthetadt = diff(subs(dLdtheta, {'dthetadt', 'dphidt', 'dpsidt', 'theta', 'psi', 'phi'}, {dthetadt, dphidt, dpsidt, theta, psi, phi}), t); r1 = dLdpsidt - dLpsi; r2 = dLdphidt - dLphi; r3 = dLdthetadt - dLtheta; sr1 = simplify(r1); sr2 = simplify(r2); sr3 = simplify(r3);
7 Příloha 2 – Výsledné rovnice (ve tvaru z MatLabu)
m1*r^2*D(D(psi))(t) + m2*r^2*D(D(psi))(t) - (l1*m1*r*D(D(phi))(t))/2 l1*m2*r*D(D(phi))(t) + (4*ld*rod*xd*yd^3*D(D(psi))(t))/3 + (4*ld*rod*xd^3*yd*D(D(psi))(t))/3 + (4*lk1*rok1*xk1*yk1^3*D(D(psi))(t))/3 + (4*lk1*rok1*xk1^3*yk1*D(D(psi))(t))/3 + (4*lk2*rok2*xk2^3*yk2*D(D(psi))(t))/3 + (4*lk2^3*rok2*xk2*yk2*D(D(psi))(t))/3 + (l2*m2*r*D(phi)(t)*sin(phi(t))*D(D(theta))(t))/2 + (l2*m2*r*D(theta)(t)*sin(phi(t))*D(D(phi))(t))/2 + (4*lk2*rok2*xk2*yk2^3*cos(phi(t))^2*D(D(psi))(t))/3 (4*lk2^3*rok2*xk2*yk2*cos(phi(t))^2*D(D(psi))(t))/3 + (l2*m2*r*D(phi)(t)^2*D(theta)(t)*cos(phi(t)))/2 (8*lk2*rok2*xk2*yk2^3*D(phi)(t)*D(psi)(t)*cos(phi(t))*sin(phi(t)))/3 + (8*lk2^3*rok2*xk2*yk2*D(phi)(t)*D(psi)(t)*cos(phi(t))*sin(phi(t)))/3 (l1^2*m1*D(D(phi))(t))/4 + l1^2*m2*D(D(phi))(t) - (l1*m1*r*D(D(psi))(t))/2 l1*m2*r*D(D(psi))(t) - (g*l2*m2*sin(phi + theta))/2 - (g*l1*m1*sin(phi))/2 g*l1*m2*sin(phi) + (l2^2*m2*D(theta)(t)^2*D(D(phi))(t))/4 + (l2^2*m2*D(phi)(t)*D(theta)(t)*D(D(theta))(t))/2 + (4*lk1*rok1*xk1*yk1^3*D(D(phi))(t))/3 + (4*lk1^3*rok1*xk1*yk1*D(D(phi))(t))/3 + (4*lk2*rok2*xk2*yk2^3*D(D(phi))(t))/3 + (4*lk2^3*rok2*xk2*yk2*D(D(phi))(t))/3 + (4*lk2*rok2*xk2*yk2^3*D(D(theta))(t))/3 + (4*lk2^3*rok2*xk2*yk2*D(D(theta))(t))/3 l1*l2*m2*D(phi)(t)*sin(phi(t))*D(D(theta))(t) l1*l2*m2*D(theta)(t)*sin(phi(t))*D(D(phi))(t) + (2*dpsidt^2*lk2*rok2*xk2*yk2^3*sin(2*phi))/3 (2*dpsidt^2*lk2^3*rok2*xk2*yk2*sin(2*phi))/3 + (l2*m2*r*D(psi)(t)*sin(phi(t))*D(D(theta))(t))/2 + (l2*m2*r*D(theta)(t)*sin(phi(t))*D(D(psi))(t))/2 l1*l2*m2*D(phi)(t)^2*D(theta)(t)*cos(phi(t)) + (dphidt^2*dthetadt*l1*l2*m2*cos(phi))/2 + (l2*m2*r*D(phi)(t)*D(psi)(t)*D(theta)(t)*cos(phi(t)))/2 (dphidt*dpsidt*dthetadt*l2*m2*r*cos(phi))/2 (l2^2*m2*D(phi)(t)^2*D(D(theta))(t))/4 - (g*l2*m2*sin(phi + theta))/2 + (l2^2*m2*D(phi)(t)*D(theta)(t)*D(D(phi))(t))/2 - (l1*l2*m2*D(phi)(t)^3*cos(phi(t)))/2 + (4*lk2*rok2*xk2*yk2^3*D(D(phi))(t))/3 + (4*lk2^3*rok2*xk2*yk2*D(D(phi))(t))/3 + (4*lk2*rok2*xk2*yk2^3*D(D(theta))(t))/3 + (4*lk2^3*rok2*xk2*yk2*D(D(theta))(t))/3 l1*l2*m2*D(phi)(t)*sin(phi(t))*D(D(phi))(t) + (l2*m2*r*D(phi)(t)*sin(phi(t))*D(D(psi))(t))/2 + (l2*m2*r*D(psi)(t)*sin(phi(t))*D(D(phi))(t))/2 + (l2*m2*r*D(phi)(t)^2*D(psi)(t)*cos(phi(t)))/2
8 Příloha 3 – Kód simulace v programu Open Modelica model DoublePendulumFromRotations parameter Real l1 = 0.163; parameter Real l2 = 0.155; parameter Real r = 0.201; parameter Real m1 = 0.0386; parameter Real m2 = 0.0456; parameter Real rod = 1.34; parameter Real rok1 = 0.88; parameter Real rok2 = 1.17; parameter Real xd = 0.0075; parameter Real yd = 0.402; parameter Real ld = 0.0075; parameter Real xk1 = 0.0075; parameter Real yk1 = 0.0075; parameter Real lk1 = l1; parameter Real xk2 = 0.00775; parameter Real yk2 = 0.00775; parameter Real lk2 = l2; parameter Real g = 9.81; Real theta(start = -90, fixed = true); Real phi(start = 180, fixed = true); Real psi(start = 0, fixed = true); Real Dtheta(start = 0, fixed = true); Real Dphi(start = 0, fixed = true); Real Dpsi(start = 0, fixed = true); annotation(experiment (StartTime = 0.0, StopTime = 5.0, Tolerance = 1)) ; equation m1*r^2*der(Dpsi) + m2*r^2*der(Dpsi) - (l1*m1*r*der(Dphi))/2 - l1*m2*r*der(Dphi) + (4*ld*rod*xd*yd^3*der(Dpsi))/3 + (4*ld*rod*xd^3*yd*der(Dpsi))/3 + (4*lk1*rok1*xk1*yk1^3*der(Dpsi))/3 + (4*lk1*rok1*xk1^3*yk1*der(Dpsi))/3 + (4*lk2*rok2*xk2^3*yk2*der(Dpsi))/3 + (4*lk2^3*rok2*xk2*yk2*der(Dpsi))/3 + (l2*m2*r*Dphi*sin(phi)*der(Dtheta))/2 + (l2*m2*r*Dtheta*sin(phi)*der(Dphi))/2 + (4*lk2*rok2*xk2*yk2^3*cos(phi)^2*der(Dpsi))/3 (4*lk2^3*rok2*xk2*yk2*cos(phi)^2*der(Dpsi))/3 + (l2*m2*r*Dphi^2*Dtheta*cos(phi))/2 (8*lk2*rok2*xk2*yk2^3*Dphi*Dpsi*cos(phi)*sin(phi))/3 + (8*lk2^3*rok2*xk2*yk2*Dphi*Dpsi*cos(phi)*sin(phi))/3 = 0; (l1^2*m1*der(Dphi))/4 + l1^2*m2*der(Dphi) - (l1*m1*r*der(Dpsi))/2 - l1*m2*r*der(Dpsi) - (g*l2*m2*sin(phi + theta))/2 - (g*l1*m1*sin(phi))/2 - g*l1*m2*sin(phi) + (l2^2*m2*Dtheta^2*der(Dphi))/4 + (l2^2*m2*Dphi*Dtheta*der(Dtheta))/2 + (4*lk1*rok1*xk1*yk1^3*der(Dphi))/3 + (4*lk1^3*rok1*xk1*yk1*der(Dphi))/3 + (4*lk2*rok2*xk2*yk2^3*der(Dphi))/3 + (4*lk2^3*rok2*xk2*yk2*der(Dphi))/3 + (4*lk2*rok2*xk2*yk2^3*der(Dtheta))/3 + (4*lk2^3*rok2*xk2*yk2*der(Dtheta))/3 - l1*l2*m2*Dphi*sin(phi)*der(Dtheta) l1*l2*m2*Dtheta*sin(phi)*der(Dphi) + (2*Dpsi^2*lk2*rok2*xk2*yk2^3*sin(2*phi))/3 (2*Dpsi^2*lk2^3*rok2*xk2*yk2*sin(2*phi))/3 + (l2*m2*r*Dpsi*sin(phi)*der(Dtheta))/2 + (l2*m2*r*Dtheta*sin(phi)*der(Dpsi))/2 - l1*l2*m2*Dphi^2*Dtheta*cos(phi) + (Dphi^2*Dtheta*l1*l2*m2*cos(phi))/2 + (l2*m2*r*Dphi*Dpsi*Dtheta*cos(phi))/2 (Dphi*Dpsi*Dtheta*l2*m2*r*cos(phi))/2 = 0; (l2^2*m2*Dphi^2*der(Dtheta))/4 - (g*l2*m2*sin(phi + theta))/2 + (l2^2*m2*Dphi*Dtheta*der(Dphi))/2 (l1*l2*m2*Dphi^3*cos(phi))/2 + (4*lk2*rok2*xk2*yk2^3*der(Dphi))/3 + (4*lk2^3*rok2*xk2*yk2*der(Dphi))/3 + (4*lk2*rok2*xk2*yk2^3*der(Dtheta))/3 + (4*lk2^3*rok2*xk2*yk2*der(Dtheta))/3 l1*l2*m2*Dphi*sin(phi)*der(Dphi) + (l2*m2*r*Dphi*sin(phi)*der(Dpsi))/2 + (l2*m2*r*Dpsi*sin(phi)*der(Dphi))/2 + (l2*m2*r*Dphi^2*Dpsi*cos(phi))/2 = 0; Dtheta = der(theta); Dphi = der(phi); Dpsi = der(psi); end DoublePendulumFromRotations;
simulate(DoublePendulumFromRotations, stopTime = 5.0); plot({psi, phi, theta})
