In [1]:
# -*- coding: utf-8 -*-

Hyppijä

Oheinen väkkärä hyppelehtii outomerellä ponttoniensa varassa. Sitä ohjataan jalkoja ja niiden välistä vartta lyhentämään ja pidentämään pyrkivillä voimilla.

väkkärä

Alkuperäinen tavoite oli laatia algoritmi, joka saa väkkärän hyppelehtimään iloisesti ja heittämään vaikka volttia, mutta onnistuin saamaan aikaiseksi vain yhden vaatimattoman hypyn.

Liian monimutkainen tehtävä käsityöksi

  • Jotta voisin edes simuloida väkkärää, tarvitsen sen liikeyhtälöt.
  • Hypyn vaatimat ohjausvoimat lasken Pontryaginin minimiperiaatteella.

Tein kaiken tietokoneella, koska tehtävä vaati lausekkeiden pyörittelyä enemmän kuin mihin käsivoimani riittivät. Simulointiin ja optimiohjauksen numeeriseen iterointiin tarvittavat lausekkeet olivat nekin liian monimutkaisia käsin koodattavaksi.

Ratkaisun alussa valitaan koordinaatisto, jossa järjestelmä tila esitetään. Vasta kolmannella osuin toimivaan. Joka yrityksellä piti kaikki laskea alusta loppuun uudestaan. Käsityönä siinä olisi mennyt ikä ja terveys. Tietokone laski kaiken uudestaan muutamassa sekunnissa.

Virheitäkin tuli. Onneksi tietokoneen siististi auki kirjoittamat välivaiheet auttoivat virheen paikallistamisessa. Tietokoneen ansiosta riitti korjata virhe, ei laskea kaikkea käsin uudestaan.

Kuvaajiakin annoin tietokoneen piirrellä, että minulle hahmoittui, millaisia muutamista keksimistäni luonnonlaeista seuraa. Kuvaajien piirtämisessä tietokone on paljon nopeampi ja tarkempi kuin minä. Se osaa piirtää sellaistakin, mihin käden taitoni ei riitä.

Tietokoneen tarjoamat apuvälineet

Maxima on sopiva tällaiseen, mutta minä otin käyttöön pythonin, koska sillä muutenkin ohjelmoin ja koska sillä on tarvittaessa helppo lisätä ohjelmanpätkiä ratkaisuun. Tämä "työkirja" on laadittu jupyterlab-nimisellä sovelluksella.

Lausekkeiden pyörittelyyn käytän sympy-kirjaston metodeja. Niitä ei ollut ihan helppo oppia käyttämään eikä niiden lukeminen liene helppoa. Ohjelmointiin tottumaton voi onneksi keskittyä matemaattisessa muodossa tulostettuihin lausekkeisiin. Jutun olen tosin kirjoittanut sillä mielellä, että siitä olisi apua tällaisesta ohjelmoinnista kiinnostuneelle.

Juttu on pikkuisen sekava helposti seurattavaksi, mutta antanee yleisvaikutelman tietokoneen tarjoamista matematiikan apuvälineistä.

Ensimmäisellä lukemisella voit hypätä suoraan pikku esimerkkiin

Simulaattori ja liikeradan optimointi Pythonilla

Alustuksia

Python-ohjelmien alussa luetellaan python-moduleja, joiden funktiota käytetään hyväksi.

Pyörittelen tässä ohjelmassa symbolisia lausekkeita, joista generoidaan python-koodia numeerista laskentaa varten. Koodista tulee niin monimutkaista, etteivät sen lausekkeet mahdu yhdelle riville. textwrap-paketista löytyvä metodi fill osaa katkoa rivit automaattisesti pythonin hyväksymällä tavalla.

... import fill tarkoittaa, että voin kutsua funktiota fill suoraan.

In [2]:
from textwrap import fill, wrap
from IPython.display import display

Numpy-moduli tarjoaa numeerisen laskennan apufunktioita. import numpy muotoinen import komento edellyttää, että esimerkiksi numpy-modulin funktioita array kutsuessani, minun pitää numpy.array

Tyylikkäintä olisi ollut käyttää vain yhtä import-muotoa, mutta tulin kokeilleeksi kaikkia vaihtoehtoja.

In [3]:
import pylab
import numpy
import math # matemaattisia funktioita kuten sin(), cos()

Re-modulissa on regular expressions funktioita merkkijonojen käsittelyyn. Regular expressions on käytettävissä kaikissa kunnollisissa editoreissa ja kaikki ohjelmointikielet tukevat niitä. Regular expressions-funktioita on vaikea oppia käyttämään, mutta ne nopeuttavat editointia ja ohjelmoinnissa ei aina edes selviä ilman niitä.

In [4]:
import re

Symboliseen laskentaan käytetään sympy-modulia. Se ei kuulu pythonin perusasennukseen, joten installoin sen erikseen komennolla pip3 install sympy.

In [5]:
import sympy as sp

... as sp tarkoittaa, että voin tästä ohjelmasta kutsua sympy:n funktioita lisäämällä funktion nimen eteen sp. Tämä tekee koodista selkeämpää.

In [6]:
sp.init_printing()

Ylläoleva varmistaa, että matemaattiset lausekkeet tulostetaan niin hyvin, kuin tietokoneesi sallii. Olen installoinut LaTeX ohjelmiston yms. joten minulle yhtälöt tulostuvat siististi.

(init_printing()-funktiolle voi antaa monenlaisia parametreja, mutta niihin perehtyminen ei tuntunut hyödylliseltä.)

Symbolisten muuttujien esittely

Käskyllä y = sp.Function('y')(t) muuttujalle y annetaan arvoksi määrittelemätön funktio y(t).

Käskyllä sp.dsolve(sp.diff(y, t) - y) ratkaistaan differentiaaliyhtälö $\frac{d}{dt}y(t) = y(t)$

Muotoa sp.dsolve(sp.diff(y, t) = y) ei voi käyttää vaan on käytettävä joko muotoa sp.dsolve(sp.diff(y, t) - y) tai sp.dsolve(sp.Eq(sp.diff(y, t), y))

Yhtälöä a = b ei voi tulostaa sellaiseen display-funktiolla, vaan a = b pitää korvata lausekkeella sp.Eq(a,b).

Yksinkertainen esimerkki tietokoneavusteisesta matematiikasta

Ennen kuin siirrytään tarkastelemaan väkkärän ohjaamisen matematiikka, tarkastellaan tietokoneavusteista matematiikkaa pienen esimerkin avulla.

Funktio, sen derivaatta ja derivaatan 0-kohdat

In [7]:
# Määritellään muutama symbolinen muuttuja ja funktio

a, b, c , d, x = sp.symbols("a, b, c , d , x")
f = sp.Function('f')(x)
print('f:')
display(f)
df = sp.Function('df')(x)
df = sp.diff(f,x)
print('df:')
display(df)
f:
$$f{\left (x \right )}$$
df:
$$\frac{d}{d x} f{\left (x \right )}$$
In [8]:
# funktion lauseke
f = a*x**3 + b*x**2 + c*x + d
display(f)
# funktion derivaatan lauseke
df = sp.diff(f,x)
display(df)
# derivaatan 0-kohtien lausekkeet
x0 = sp.solve(df,x)
display(x0)
$$a x^{3} + b x^{2} + c x + d$$
$$3 a x^{2} + 2 b x + c$$
$$\left [ \frac{- b + \sqrt{- 3 a c + b^{2}}}{3 a}, \quad - \frac{b + \sqrt{- 3 a c + b^{2}}}{3 a}\right ]$$

koodin kirjoittaminen

Yleensä matemaattisia lausekkeita tarvitaan jossain tietokone-ohjelmassa. Kirjoitetaan python-moduliin p.fy funktiot f(x) ja df(x)

In [9]:
with open('f.py', 'w') as fil:
    fil.write('def f(x,a,b,c,d):\n')
    fil.write('    return ')
    fil.write(str(f))
    fil.write('\n\n')
    
    fil.write('def df(x,a,b,c):\n')
    fil.write('    return ')
    fil.write(str(df))
    fil.write('\n\n')

Tulostetaan tiedoston sisältö.

In [10]:
with open('f.py', 'r') as fin:
    print(fin.read())
def f(x,a,b,c,d):
    return a*x**3 + b*x**2 + c*x + d

def df(x,a,b,c):
    return 3*a*x**2 + 2*b*x + c


Funktioiden piirtäminen

f ja df ovat funktion ja sen derivaatan lausekkeita. Tehdään niitä vastaavat numeeriset funktiot kuvoiden piirtämisen helpottamiseksi. (Myöhemmin tässä jutussa käytän hölmömpää tapaa, jonka joskus korjaan.)

In [11]:
g = sp.lambdify(x,f)
dg = sp.lambdify(x,df)
display(g(0.5), dg(0.5))
$$0.125 a + 0.25 b + 0.5 c + d$$
$$0.75 a + 1.0 b + c$$
In [12]:
pylab.rcParams['xtick.color'] = 'green'
pylab.rcParams['ytick.color'] = 'green'

Sijoitetaan kertoimille a, b, c ja d muutamia arvoja, tulostetaan parametrit, derivaatan 0.kohtien arvon ja piirretään funktion g ja sen derivaatan dg kuvaajat.

In [13]:
# x-akselin pisteet, jossa funktion ja sen derivaatan arvot piirretään.
xskaala = 0.1 
xx = [xskaala*i for i in range(-11,12)]
print("x-akselin pisteet: ")
print(xx,"\n")

print("funktio, sen derivaatta ja derivaatan 0-kohdat muutamilla parametrien arvoilla:\n")
for ar in range(1,2):  #range (1,2) = kokonaisluvut puoliavoimelta väliltä [1,2)
    for br in range(-1,2): # range(-1,2) = {-1, -0, 1}
        for cr in range(-1,2):
            for dr in range(-1,2):
                parsubs = {a: ar, b: br, c: cr, d: dr}
                yy1 = [g(x).evalf(subs=parsubs) for x in xx]  # funktion arvo x-akselin pisteissä xx
                yy2 = [dg(x).evalf(subs=parsubs) for x in xx]  # funktion derivaatan arvo
                pylab.title(str(parsubs))
                pylab.plot(xx, yy1, label="g")
                pylab.plot(xx, yy2, label="dg")
                pylab.plot([xx[0], xx[-1]],[0.0, 0.0])
                pylab.legend()
                pylab.show()

                xr0 = [x0[0].evalf(subs=parsubs), x0[1].evalf(subs=parsubs)] # 0-kohdan numeerinen arvo
                try:
                    print("x0: {1:5.2f}, {0:5.2f}\n\n".format(xr0[0], xr0[1])) # jos reealiluku niin onnistuu näin
                except:
                        print("x0: ", {xr0[0], xr0[1]}, "\n\n") # imaginaarilukuja en jaksa siistiä
x-akselin pisteet: 
[-1.1, -1.0, -0.9, -0.8, -0.7000000000000001, -0.6000000000000001, -0.5, -0.4, -0.30000000000000004, -0.2, -0.1, 0.0, 0.1, 0.2, 0.30000000000000004, 0.4, 0.5, 0.6000000000000001, 0.7000000000000001, 0.8, 0.9, 1.0, 1.1] 

funktio, sen derivaatta ja derivaatan 0-kohdat muutamilla parametrien arvoilla:

x0: -0.33,  1.00


x0: -0.33,  1.00


x0: -0.33,  1.00


x0: -0.00,  0.67


x0: -0.00,  0.67


x0: -0.00,  0.67


x0:  {0.333333333333333 - 0.471404520791032*I, 0.333333333333333 + 0.471404520791032*I} 


x0:  {0.333333333333333 - 0.471404520791032*I, 0.333333333333333 + 0.471404520791032*I} 


x0:  {0.333333333333333 - 0.471404520791032*I, 0.333333333333333 + 0.471404520791032*I} 


x0: -0.58,  0.58


x0: -0.58,  0.58


x0: -0.58,  0.58


x0:  {0} 


x0:  {0} 


x0:  {0} 


x0:  {-0.577350269189626*I, 0.577350269189626*I} 


x0:  {-0.577350269189626*I, 0.577350269189626*I} 


x0:  {-0.577350269189626*I, 0.577350269189626*I} 


x0: -1.00,  0.33


x0: -1.00,  0.33


x0: -1.00,  0.33


x0: -0.67,  0.00


x0: -0.67,  0.00


x0: -0.67,  0.00


x0:  {-0.333333333333333 + 0.471404520791032*I, -0.333333333333333 - 0.471404520791032*I} 


x0:  {-0.333333333333333 + 0.471404520791032*I, -0.333333333333333 - 0.471404520791032*I} 


x0:  {-0.333333333333333 + 0.471404520791032*I, -0.333333333333333 - 0.471404520791032*I} 


Operaatiota matriiseilla:

Transpoosi, kertolasku ja summa

In [14]:
B = sp.Matrix([["a"+str(i)+str(j) for j in range(0,3)] for i in range(0,3)])
u = sp.Matrix([sp.symbols("u_" + str(i)) for i in range(1, 4)])
v = sp.Matrix([sp.symbols("v_" + str(i)) for i in range(1, 4)])
print("u ja v")
display(u, v)
u ja v
$$\left[\begin{matrix}u_{1}\\u_{2}\\u_{3}\end{matrix}\right]$$
$$\left[\begin{matrix}v_{1}\\v_{2}\\v_{3}\end{matrix}\right]$$

Muodostin yllä matriisit B, u ja v kimurantilla, mutta pythonmaisella tavalla. sp.symbols palauttaa palauttaa symbolisen muuttujan, jonka nimeksi tulee sen argumenttina oleva merkkijono. Pythonissa listoja käsitellään usein noin, että for-silmukka on "listan sisällä". Saman voisi tehdä tavallisessa for-silmukassa, mutta ylläoleva lienee laskennallisesti tehokkaampi ja kätevä, kun siihen tottuu.

Matriisin tai vektorin transpoosissa u.T rivit on käännetty sarakkeiksi tai toisinpäin.

Matriiseja voi kertoa tai laskea yhteen, kunhan dimensiot täsmäävät.

In [15]:
ut = u.T
print("u:n transpoosi")
display(ut)
u:n transpoosi
$$\left[\begin{matrix}u_{1} & u_{2} & u_{3}\end{matrix}\right]$$
In [16]:
uvt = u * v.T
print("uvt: vektorien tulo")
display(uvt)
uvt: vektorien tulo
$$\left[\begin{matrix}u_{1} v_{1} & u_{1} v_{2} & u_{1} v_{3}\\u_{2} v_{1} & u_{2} v_{2} & u_{2} v_{3}\\u_{3} v_{1} & u_{3} v_{2} & u_{3} v_{3}\end{matrix}\right]$$
In [17]:
utv = u.T * v
print("utv: vektorien tulo")
display(utv)
utv: vektorien tulo
$$\left[\begin{matrix}u_{1} v_{1} + u_{2} v_{2} + u_{3} v_{3}\end{matrix}\right]$$
In [18]:
udotv = u.dot(v)
print("udottv: vektorien pistetulo")
display(udotv)
udottv: vektorien pistetulo
$$u_{1} v_{1} + u_{2} v_{2} + u_{3} v_{3}$$
In [19]:
btb = B * B
print("btb: matriisi kerrottuna itsellään")
display(btb)
btb: matriisi kerrottuna itsellään
$$\left[\begin{matrix}a_{00}^{2} + a_{01} a_{10} + a_{02} a_{20} & a_{00} a_{01} + a_{01} a_{11} + a_{02} a_{21} & a_{00} a_{02} + a_{01} a_{12} + a_{02} a_{22}\\a_{00} a_{10} + a_{10} a_{11} + a_{12} a_{20} & a_{01} a_{10} + a_{11}^{2} + a_{12} a_{21} & a_{02} a_{10} + a_{11} a_{12} + a_{12} a_{22}\\a_{00} a_{20} + a_{10} a_{21} + a_{20} a_{22} & a_{01} a_{20} + a_{11} a_{21} + a_{21} a_{22} & a_{02} a_{20} + a_{12} a_{21} + a_{22}^{2}\end{matrix}\right]$$
In [20]:
bpb = B + B
print("bpb: matriisi lisättynä itseensä")
display(bpb)
bpb: matriisi lisättynä itseensä
$$\left[\begin{matrix}2 a_{00} & 2 a_{01} & 2 a_{02}\\2 a_{10} & 2 a_{11} & 2 a_{12}\\2 a_{20} & 2 a_{21} & 2 a_{22}\end{matrix}\right]$$

Matriisien derivaattoja

Jatkossa tarvitsemme monen muuttujan funktion gradienttia eli matriisia, jossa kukin alkio on funktion osittaisderivaatta yhden muuttujan suhteen. Lisäksi saatetaan tarvita jacobian matriisia ja hessian matriisia, joista näytän myöhemmin esimerkin.

On pitkä juttu selittää, mihin näitä tarvitaan. Vielä pitempi jutusta tulee, jos selittää kunnolla. Tyydyn pariin yleiseen toteamukseen, joiden jälkeen näytän niistä esimerkin

Numeeristen menetelmien "käyttöohjeissa" pyydetään kirjoittamaan funktiot, jotka palauttavat gradientin, jacobin matriisin tai hessian matriisin. Helpottaa, jos tietää, mitä ne tarkoittavat.

Funktion $y = f(x_i), i = 0..n$ gradientti tietyssä pisteessä kertoo, kuinka jyrkästi $y$ muuttuu kutakin muuttujaa $x_i$ muutettaessa. Kun gradientti on jossain pisteessä nolla, piste on yksi funktion ääripisteistä.

Numeerisin menetelmin haetaan usein funktion minimiä tai maksimia. Gradienttia käytetään apuna päätettäessä, mistä suunnasta ja kuinka kaukaa ääriarvoa haetaan.

Jacobian matriisi on vektoriarvoiselle funktiolle $\underline y = \underline f(x_i), i = 0..n$ sama kuin gradientti yksiarvoiselle funktiolle.

Hessian matriisi on gradientin jacobin matriisi. Selviää parhaiten allaolevasta esimerkistä. Numeeriset optimointimenetelmät tarvitsevat tätä, mutta laskevat sen itse, ellei käyttäjä ohjelmoi sitä valmiiksi. $ y = H(\underline x_0)\underline x$ on funtkion lineaariapproksimaatio pisteessä $\underline x_0$, mihin perustuu sen hyödyllisyys numeerisissa menetelmissä.

Funktion $f(\mathbf x)$ gradienttia voi kutsua $f$:n derivaataksi vektorin $\mathbf x$ suhteen ja merkitä $grad\,f = \frac{d}{d\mathbf x}f(\mathbf x) = f_{\mathbf x}(\mathbf x)$

In [21]:
f = sp.Function("f")
xx = [sp.symbols("x_" + str(i)) for i in range(0, 3)]
gradfx = [0 for i in range(0, 3)]

for i in range(0, 3):
    gradfx[i] = sp.diff(f(xx[0], xx[1], xx[2]), xx[i])

display(gradfx)
$$\left [ \frac{\partial}{\partial x_{0}} f{\left (x_{0},x_{1},x_{2} \right )}, \quad \frac{\partial}{\partial x_{1}} f{\left (x_{0},x_{1},x_{2} \right )}, \quad \frac{\partial}{\partial x_{2}} f{\left (x_{0},x_{1},x_{2} \right )}\right ]$$
In [22]:
xx = sp.Matrix([sp.symbols("x_" + str(i)) for i in range(0, 3)])
print('xx:')
display(xx)

ff = sp.Matrix([sp.Function("f_" + str(i)) for i in range(0, 3)])
print('ff:')
display(ff)

jacob_ff = sp.eye(3)
for i in range(0, 3):
    for j in range(0, 3):
        jacob_ff[i, j] = sp.diff(ff[i](xx[0], xx[1], xx[2]), xx[j])
        
print('jacob_ff:')
display(jacob_ff)
xx:
$$\left[\begin{matrix}x_{0}\\x_{1}\\x_{2}\end{matrix}\right]$$
ff:
$$\left[\begin{matrix}\operatorname{f_{0}}\\\operatorname{f_{1}}\\\operatorname{f_{2}}\end{matrix}\right]$$
jacob_ff:
$$\left[\begin{matrix}\frac{\partial}{\partial x_{0}} \operatorname{f_{0}}{\left (x_{0},x_{1},x_{2} \right )} & \frac{\partial}{\partial x_{1}} \operatorname{f_{0}}{\left (x_{0},x_{1},x_{2} \right )} & \frac{\partial}{\partial x_{2}} \operatorname{f_{0}}{\left (x_{0},x_{1},x_{2} \right )}\\\frac{\partial}{\partial x_{0}} \operatorname{f_{1}}{\left (x_{0},x_{1},x_{2} \right )} & \frac{\partial}{\partial x_{1}} \operatorname{f_{1}}{\left (x_{0},x_{1},x_{2} \right )} & \frac{\partial}{\partial x_{2}} \operatorname{f_{1}}{\left (x_{0},x_{1},x_{2} \right )}\\\frac{\partial}{\partial x_{0}} \operatorname{f_{2}}{\left (x_{0},x_{1},x_{2} \right )} & \frac{\partial}{\partial x_{1}} \operatorname{f_{2}}{\left (x_{0},x_{1},x_{2} \right )} & \frac{\partial}{\partial x_{2}} \operatorname{f_{2}}{\left (x_{0},x_{1},x_{2} \right )}\end{matrix}\right]$$

Esimerkki gradientista ja jacobin/hessian matriisista

In [23]:
xx = [sp.symbols("x_"+str(i)) for i in range(0, 3)]
print('xx:')
display(xx)

f = xx[0]**2 + xx[0] * xx[1] + sp.sin(xx[0]) * sp.cos(xx[2]) + 3 * xx[2]
print('f:')
display(f)

dfdx = [0 for i in range(0, 3)]
for i in range(0, 3):
    dfdx[i] = sp.diff(f, xx[i])
print('dfdx:')
display(dfdx)
xx:
$$\left [ x_{0}, \quad x_{1}, \quad x_{2}\right ]$$
f:
$$x_{0}^{2} + x_{0} x_{1} + 3 x_{2} + \sin{\left (x_{0} \right )} \cos{\left (x_{2} \right )}$$
dfdx:
$$\left [ 2 x_{0} + x_{1} + \cos{\left (x_{0} \right )} \cos{\left (x_{2} \right )}, \quad x_{0}, \quad - \sin{\left (x_{0} \right )} \sin{\left (x_{2} \right )} + 3\right ]$$
In [24]:
Hessian = sp.eye(3)  # Matriisi pitää jotenkin esitellä/alustaa ennen käyttöä
for i in range(0, 3):
    for j in range(0, 3):
        Hessian[i, j] = sp.diff(dfdx[i], xx[j])
display(xx, dfdx, Hessian)
$$\left [ x_{0}, \quad x_{1}, \quad x_{2}\right ]$$
$$\left [ 2 x_{0} + x_{1} + \cos{\left (x_{0} \right )} \cos{\left (x_{2} \right )}, \quad x_{0}, \quad - \sin{\left (x_{0} \right )} \sin{\left (x_{2} \right )} + 3\right ]$$
$$\left[\begin{matrix}- \sin{\left (x_{0} \right )} \cos{\left (x_{2} \right )} + 2 & 1 & - \sin{\left (x_{2} \right )} \cos{\left (x_{0} \right )}\\1 & 0 & 0\\- \sin{\left (x_{2} \right )} \cos{\left (x_{0} \right )} & 0 & - \sin{\left (x_{0} \right )} \cos{\left (x_{2} \right )}\end{matrix}\right]$$

Hyppijän tilayhtälön johtaminen

Miten saada selkoa siitä, miten kuvan 1 hyppijä reagoi sitä ohjaaviin voimiin?

Tilayhtälöllä tarkoitetaan differentiaaliyhtälöä, joka kertoo, miten hyppijän tila — tilamuuttujien arvo — muuttuu ajan myötä ja miten ulkoiset ohjaukset vaikuttavat siihen.

Tilamuuttujat pitää valita niin, että niiden tietynhetkisen arvon ja ja tuosta hetkestä eteenpäin vaikuttavien ulkoisten voimien perusteella voidaan päätellä, mitä järjestelmälle tapahtuu tuosta hetkestä eteenpäin.

Tykinkuulan lentorata hetkestä t eteenpäin voidaan laskea, jos tiedetään kuulan paikka ja nopeus hetkellä t. Hyppijässä on kolme "kuulaa". Koska ne on kiinnitetty toisiinsa, meidän ei tarvitse pitää kirjaa niiden jokaisen paikasta ja nopeudesta joten tilamuuttujia ei tarvita aivan kahtatoista. Jos hyppijän jalat kiinnittää paikalleen, "vartalo" pääsee liikkumaan vain sidottua rataa pitkin. Jalkojen ja vartalon paikan ilmoittamiseen tarvittaneen siis viisi tilamuuttujaa. Nopeuksia varten vastaavasti toiset viisi. Mikäli vartalon ja jalkojen massa olisi jakautunut — ne olisivat sauvoja, joilla on inertiamomentti — tarvittaisiin lisäksi kunkin osan pyörimisnopeus tilamuuttujaksi. Se vähintäänkin hidastaa, ellei vaikeuta ratkaisun etsimistä.

Koordinaatisto

Lagrangen mekaniikan mukaisissa esimerkkiratkaisuissa neuvottiin valitsemaan koordinaatisto niin, että järjestelmän liike- ja potentiaalienergian lausekkeiden kirjoittaminen on mahdollisimman helppoa. Eli luovutaan xy-koordinaatistosta

Tämä wikipedia artikkeli auttanee ymmärtämään koordinaatiston valintaa. (Ei se kyllä helppoa ollut.)

Vektoriesitystä käytetään eräänlaisena lyhennysmerkintänä niin, että voimme käyttää samoja yhtälöitä riippumatta siitä, tarkastelemmeko 1,2 vai 3 -ulotteista avaruutta.

Vektorien avulla voimme merkitä $\underline{F}(t)=m \underline{a}(t) = m\underline{\dot{v}}(t) = m\underline{\ddot{x}}(t)$ niin tasossa kuin 2 tai 3 -ulotteisessakin avaruudessa.

Tästä saamme differentiaaliyhtälön $\underline{\ddot{x}}(t) = \underline{F}(t)/m$

Juonipaljastus: Homman edetessä saamme aikaiseksi differentiaaliyhtälöitä, joita ei voi ratkaista kuin numeerisilla menetelmillä. Differentiaaliyhtälöiden numeeriset ratkaisumenetelmät taas soveltuvat usein ainoastaan sellaisille differentiaaliyhtälöryhmille, joissa on vain ensimmäisiä derivaattoja. Niin minunkin valitsemani (= löytämäni).

Sijoituksilla $\underline{a}(t) \mapsto \underline{\dot{v}}(t) \text{ ja } \underline{v}(t) \mapsto \underline{\dot{x}}(t)$ saamme matriisimuotoisen yhtälöryhmän:

Yhtälö kuvaa, miten hyppijä reagoi sitä ohjaaviin voimiin.

Lagrangen mekaniikka

Ylläolevassa yhtälössä esiintyvien voiman $F_{load}$ komponenttien $F^{x}_{load}(t) $ ja $F^{y}_{load}(t) $ ratkaiseminen näytti helpolta, mutta osoittautui minulle liian vaikeaksi. Merkit ehkä sain lopulta oikein, mutta kuinka suuri on vaijeria kuormittava voima missäkin tilanteessa? Sen sijaan, että olisin saman tien päässyt kirjoittamaan koodia jouduinkin opiskelemaan aivan uusia asioita mekaniikasta.

Googlaimella minulle selvisi, että Lagrangen mekaniikka saattaisi auttaa. Lagrangen mekaniikkaa sovellettaessa voimien lausekkeiden sijasta kirjoitetaan liike- ja potentiaalienergian lausekkeet, mikä sopivasti valitussa koordinaatistossa on paljon helpompaa kuin yrittää ratkaista voimien lausekkeita.

Myöhemmin opin, että järjestelmän sisäisiä voimia — vaijeria jännittävää voimaa — ei tarvitse miettiä lainkaan, mutta ulkoiset voimat — ohjausvoimat ja kitkavoimat — aiheuttivat kuitenkin pikkuisen päänvaivaa.

Lagrangen mekaniikan mukaan järjestelmän liike- ja potentiaalienergian erotukselle $L$ ja q-koordinaatistossa esitetyille ulkoisille voimille $Q$ pätee:

$$ \frac{d}{dt}(\frac{\partial L}{\partial \dot q_{i}}) - \frac{\partial L}{\partial q_{i}} = Q_i, \text{ } i = 1 \dots n $$

Sitä, miksi liike- ja potentiaalienergian erotuksen dervioinnilla monella eri tavoin, on jotain järkeä, ei voi ymmärtää ylläolevaa yhtälöä tuijottamalla eikä "maalaisjärjellä." Yritin ymmärtää yhtälön johtamista ja mielenkiintoiselta vaikutti, mutta syvimpiä perusteita en osaa selittää. Variaatiolaskenta oli keskeistä ja sitä en lähde tässä esittelemään.

Ulkoisten voimien laskemista selitetään mm wikipedia artikkelissa generalized forces.

virtual work and generalized forces

Löysin samalle asialle mielestäni yksinkertaisemman muotoilun. Sen mukaan

$$ Q_i = \frac{\partial \dot W}{\partial \dot q_i} \text{ ja } \dot W = \mathbf{F}\cdot \mathbf{v} + \mathbf{\tau}\cdot \mathbf{\omega} $$

Voimien $Q_i$ yhtälö on johdettu virtuaalityön käsitteen avulla. Virtuaalityön idea on laskea järjestelmän siirtämiseen tilasta A tilaan B tarvittava energia kahdella toisistaan äärettömän vähän poikkeavaa polkua pitkin. Jonkun teoreeman mukaan virtuaalityö minimoituu fysiikan lakeja vastaavalla polulla. Tai niin ymmärsin teorioita äkkiseltään silmäilemällä. Toisaalta $\mathbf F\cdot \mathbf v$ on tehon kaava. Lagrangen mekaniikka on tunnettu siitä, että yhtälöiden fysikaalinen tulkinta "maalaisjärjellä" ei ola aina helppoa. Jätetään siis enempi hämmästely.

Yhtälö näyttää jokatapauksessa pahalta. Onneksi voimme jättää sen ratkaisemisen tietokoneelle, kunhan ensin onnistumme kirjoittamaan nosturin liike- ja potentiaalienergian lausekkeet. Siinäkin suurimman osan työstä jätämme tietokoneelle.

Tilayhtälöiden johtaminen

Järjestemän paikan q-koordinaatistossa ilmoittavat vaunun paikka, nostovaijerin pituus ja kulma, jonka se muodostaan pystysuoran kanssa.

Järjestelmän tilamuuttujiksi tarvitaan myös nopeudet, eli paikkavektorin derivaatta ajan suhteen.

Kun tilayhtälöitä johdetaan Lagrangen mekaniikan mukaisesti, tarvitaan vielä vektorin q toinen derivaatta ajan suhteen.

In [25]:
l0, m1, m2, t = sp.symbols('l_0, m_1, m_2, t', real=True)
axf = sp.Function('a_x', real=True)
ayf = sp.Function('a_y', real=True)
bxf = sp.Function('b_x', real=True)
byf = sp.Function('b_y', real=True)
cxf = sp.Function('c_x', real=True)
cyf = sp.Function('c_y', real=True)
In [26]:
q = sp.Matrix([axf(t), ayf(t), bxf(t), byf(t), cxf(t), cyf(t)])
sysdim = len(q)
display("paikka ", q.T)
'paikka '
$$\left[\begin{matrix}\operatorname{a_{x}}{\left (t \right )} & \operatorname{a_{y}}{\left (t \right )} & \operatorname{b_{x}}{\left (t \right )} & \operatorname{b_{y}}{\left (t \right )} & \operatorname{c_{x}}{\left (t \right )} & \operatorname{c_{y}}{\left (t \right )}\end{matrix}\right]$$
In [27]:
dqdt = sp.Matrix(sp.diff(q,t))
dqdt = sp.diff(q,t)

display("nopeus ",dqdt.T)

d2qdt2 = sp.Matrix(sp.diff(q,t,2))
display("kiihtyvyys ",d2qdt2.T)
'nopeus '
$$\left[\begin{matrix}\frac{d}{d t} \operatorname{a_{x}}{\left (t \right )} & \frac{d}{d t} \operatorname{a_{y}}{\left (t \right )} & \frac{d}{d t} \operatorname{b_{x}}{\left (t \right )} & \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )} & \frac{d}{d t} \operatorname{c_{x}}{\left (t \right )} & \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )}\end{matrix}\right]$$
'kiihtyvyys '
$$\left[\begin{matrix}\frac{d^{2}}{d t^{2}} \operatorname{a_{x}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{a_{y}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{b_{x}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{b_{y}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{c_{x}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{c_{y}}{\left (t \right )}\end{matrix}\right]$$
In [28]:
## rajoitusyhtälöt
In [29]:
lab2 = (axf(t)-bxf(t))**2 + (ayf(t)-byf(t))**2
lac2 = (axf(t)-cxf(t))**2 + (ayf(t)-cyf(t))**2
lbc2 = (bxf(t)-cxf(t))**2 + (byf(t)-cyf(t))**2

lab = sp.sqrt(lab2)
lac = sp.sqrt(lac2)
lbc = sp.sqrt(lbc2)

#lab = sp.sqrt(((ax(t)-bx(t))**2 + (ay(t)-by(t))**2))
#lac = sp.sqrt(((ax(t)-cx(t))**2 + (ay(t)-cy(t))**2))
#lbc = sp.sqrt(((bx(t)-cx(t))**2 + (by(t)-cy(t))**2))
display(lab, lac, lbc)
$$\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}$$
$$\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}$$
$$\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}$$

Järjestelmän potentiaali- ja liike-energia

Lagrangen mekaniikassa tilayhtälöiden johtamisessa käytetään apuna järjestelmän potentiaali- ja liike-energiaa. Ellei järjestelmään vaikuttaisi ulkoisia ohjaus- tai kitkavoimia, voimia ei tarvitsisi miettiä lainkaan.

järjestelmän potentiaali- ja liike-energia on helppo esittää xy-koordinaatistossa, joten laskemme vaunun ja kuorman paikat ja nopeudet xy-koordinaatistossa q-koordinaattien avulla. Vaunun osalta ne tosin ovat samat.

Potentiaalienergia.

In [30]:
hf1, hf2, hf3, mx, Ks, Cfrx, Cfry, g  = sp.symbols('hf1 hf2 hf3 mx K_s C_frx C_fry g', real=True)
h, vh, x, xv = sp.symbols('h vh x xv', real=True)

y = sp.Function('y')
vx = sp.Function('vx')
vy = sp.Function('vy')
In [31]:
#def Fpmp(h):
#    return sp.exp(-2*(h-2))
#    return Ks*(sp.atan(-10*h)/sp.pi + sp.Rational(1,2))*h**2

def potFn(h,mx):
    potF = mx*g*(h + sp.exp(-2*h))
    return potF
                 
display(potFn(y(t), m1))
$$g m_{1} \left(y{\left (t \right )} + e^{- 2 y{\left (t \right )}}\right)$$
In [32]:
def frictx(h, xv):
    Fx = sp.diff(sp.exp(-2*h),h)
    return Cfrx*Fx*xv

display(frictx(y(t), vx(t)))

#math.exp(1)-math.exp(-((xx[i]-2)/2))
$$- 2 C_{frx} \operatorname{vx}{\left (t \right )} e^{- 2 y{\left (t \right )}}$$
In [33]:
def fricty(h, vh):
    Fy = sp.diff(sp.exp(-2*h),h)
    return Cfry*Fy*vh # (sp.exp(1)-sp.exp(-(vy-2)/2))

display(fricty(y(t), vy(t)))
$$- 2 C_{fry} \operatorname{vy}{\left (t \right )} e^{- 2 y{\left (t \right )}}$$
In [34]:
h = sp.Symbol('h', real=True)
def ground(mx):
    h0 = sp.solve(sp.diff(potFn(h,mx), h), h)
    return h0[0]

gr0 = ground(m1/2+m2)

#y1 = h(t)

display(gr0)
$$\frac{\log{\left (2 \right )}}{2}$$
In [35]:
pylab.rcParams['xtick.color'] = 'green'
pylab.rcParams['ytick.color'] = 'green'
evsubs = {m1:1.0, g:9.81, Cfrx: 1.0, Cfry: 0.5, hf1: 1, hf2: 0, hf3: -1, m2: 2}

#print(pylab.rcParams.keys())
In [36]:
print("\n potentiaalienergia")
hh2 = [0.1*(i-2) for i in range(12)]
zz = [potFn(z,1).evalf(subs=evsubs) for z in hh2]
pylab.plot(hh2, zz)
zz = [potFn(z,2).evalf(subs=evsubs) for z in hh2]
pylab.plot(hh2, zz)
#zz = [potFn(z,3).evalf(subs=evsubs) for z in hh2]
#pylab.plot(hh2, zz)
pylab.show()
 potentiaalienergia
In [37]:
print("\n kitkavoima nopeuden funktiona eri korkeuksilla" )

hh = [0.2*(i-10) for i in range(20)]

zzm = [frictx(hf1, vx).evalf(subs=evsubs) for vx in hh]
zzmy = [fricty(hf1, vx).evalf(subs=evsubs) for vx in hh]
zz0 = [frictx(hf2, vx).evalf(subs=evsubs) for vx in hh]
zz0y = [fricty(hf2, vx).evalf(subs=evsubs) for vx in hh]
zzp = [frictx(hf3, vx).evalf(subs=evsubs) for vx in hh]
zzpy = [fricty(hf3, vx).evalf(subs=evsubs) for vx in hh]

#for i in range(5):
#    vx = 0.5*(i+1)
#    zz = [frict(z, vx).evalf(subs=evsubs) for z in hh]
pylab.plot(hh, zzm)
pylab.plot(hh, zzmy)
pylab.show()
 kitkavoima nopeuden funktiona eri korkeuksilla
In [38]:
pylab.plot(hh, zz0)
pylab.plot(hh, zz0y)
pylab.show()
In [39]:
pylab.plot(hh, zzp)
pylab.plot(hh, zzpy)
pylab.show()
In [40]:
#m1, m2 = sp.symbols('m1, m2', real=True)

#Vg = g * (m1*ay(t) + m2*by(t) + m2*cy(t))
Vg = potFn(q[1],m1) + potFn(q[3],m2) + potFn(q[5],m2) 
display(Vg)
$$g m_{1} \left(\operatorname{a_{y}}{\left (t \right )} + e^{- 2 \operatorname{a_{y}}{\left (t \right )}}\right) + g m_{2} \left(\operatorname{b_{y}}{\left (t \right )} + e^{- 2 \operatorname{b_{y}}{\left (t \right )}}\right) + g m_{2} \left(\operatorname{c_{y}}{\left (t \right )} + e^{- 2 \operatorname{c_{y}}{\left (t \right )}}\right)$$

massojen paikat ja nopeudet xy-koordinaatistossa q-koordinaattien avulla

In [41]:
va = dqdt[0:2,:]
vb = dqdt[2:4,:]
vc = dqdt[4:6,:]
display(va.T, vb.T, vc.T)
$$\left[\begin{matrix}\frac{d}{d t} \operatorname{a_{x}}{\left (t \right )} & \frac{d}{d t} \operatorname{a_{y}}{\left (t \right )}\end{matrix}\right]$$
$$\left[\begin{matrix}\frac{d}{d t} \operatorname{b_{x}}{\left (t \right )} & \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )}\end{matrix}\right]$$
$$\left[\begin{matrix}\frac{d}{d t} \operatorname{c_{x}}{\left (t \right )} & \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )}\end{matrix}\right]$$
In [42]:
E_kin = (m1*va.dot(va) + m2*vb.dot(vb) + m2*vc.dot(vc))/2
display(E_kin)
$$\frac{m_{1} \left(\left(\frac{d}{d t} \operatorname{a_{x}}{\left (t \right )}\right)^{2} + \left(\frac{d}{d t} \operatorname{a_{y}}{\left (t \right )}\right)^{2}\right)}{2} + \frac{m_{2} \left(\left(\frac{d}{d t} \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\frac{d}{d t} \operatorname{b_{y}}{\left (t \right )}\right)^{2}\right)}{2} + \frac{m_{2} \left(\left(\frac{d}{d t} \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\frac{d}{d t} \operatorname{c_{y}}{\left (t \right )}\right)^{2}\right)}{2}$$

Tilayhtälöiden johtaminen Lagrangen periaatteella

Lagrangen mekaniikkaa sovellettaessa muodostetaan langrangen funktio L, joka on järjestelmän kineettisen ja potentiaalienergian erotus ja ratkaistaan yhtälöistä

$$ \frac{d}{dt}(\frac{\partial L}{\partial \dot q_{i}}) - \frac{\partial L}{\partial q_{i}} - \mu_{i}f_{i} = Q_i, \text{ } i= 1 \dots n $$$$ Q_i = \frac{\partial \dot W}{\partial \dot q_i} $$$$f{i} \text{ on rajoitusyhtälö i ja } \dot W = \mathbf F\cdot \mathbf v$$

Kiihtyvyydet $\ddot q(t) = $ {{d2qdt2}} niin, että saadaan yhtälö

$$ \mathbf{\ddot{q}}(t) = \mathbf{f}(\mathbf{\dot{q}}(t), \mathbf{q}(t)) + \mathbf{u} $$

Meidän siis pitää kirjoittaa järjestelmän potentiaali- ja liike-energian lausekkeet sekä lauseke ohjausten $u_i$ ja ulkoisten voimien $F_i$ teholle.

Ohjaukset ja ulkoiset voimat

In [43]:
#print("\n q \n")
#display(q.T)
Fab, Fac, Fbc = sp.symbols('Fab, Fac, Fbc', real=True) # Vetävät "kolmiota" kasaan !!!
In [44]:
Fabx = Fab*(bxf(t)-axf(t))/lab
Faby = Fab*(byf(t)-ayf(t))/lab

Fbax = -Fabx
Fbay = -Faby

Facx = Fac*(cxf(t)-axf(t))/lac
Facy = Fac*(cyf(t)-ayf(t))/lac

Fcax = -Facx
Fcay = -Facy

Fbcx = Fbc*(cxf(t)-bxf(t))/lbc
Fbcy = Fbc*(cyf(t)-byf(t))/lbc

Fcbx = -Fbcx
Fcby = -Fbcy
In [45]:
Frbx = frictx(q[3],dqdt[2])
Frcx = frictx(q[5],dqdt[4])

Frby = fricty(q[3],dqdt[3])
Frcy = fricty(q[5],dqdt[5])
In [46]:
ohj = sp.Matrix([Fabx+Facx, Faby+Facy, Fbax+Fbcx+Frbx, Fbay+Fbcy+Frby, Fcbx+Fcax+Frcx, Fcby+Fcay+Frcy]) # ylin pallo ilman kitkaa !!!
display(ohj)
$$\left[\begin{matrix}\frac{Fab \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{b_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fac \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\\\frac{Fab \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{b_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fac \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\\- 2 C_{frx} e^{- 2 \operatorname{b_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{b_{x}}{\left (t \right )} - \frac{Fab \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{b_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fbc \left(- \operatorname{b_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\\- 2 C_{fry} e^{- 2 \operatorname{b_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )} - \frac{Fab \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{b_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fbc \left(- \operatorname{b_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\\- 2 C_{frx} e^{- 2 \operatorname{c_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{c_{x}}{\left (t \right )} - \frac{Fac \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}} - \frac{Fbc \left(- \operatorname{b_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\\- 2 C_{fry} e^{- 2 \operatorname{c_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )} - \frac{Fac \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}} - \frac{Fbc \left(- \operatorname{b_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\end{matrix}\right]$$

Ulkoiset voimat

In [47]:
Wu = ohj.dot(dqdt)
#Qu = [sp.diff(Wu,dq) for dq in dqdt]
Qu = sp.simplify([sp.diff(Wu,dq) for dq in dqdt])
display(Wu, Qu)
$$\left(\frac{Fab \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{b_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fac \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right) \frac{d}{d t} \operatorname{a_{x}}{\left (t \right )} + \left(\frac{Fab \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{b_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fac \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right) \frac{d}{d t} \operatorname{a_{y}}{\left (t \right )} + \left(- 2 C_{frx} e^{- 2 \operatorname{b_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{b_{x}}{\left (t \right )} - \frac{Fab \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{b_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fbc \left(- \operatorname{b_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right) \frac{d}{d t} \operatorname{b_{x}}{\left (t \right )} + \left(- 2 C_{frx} e^{- 2 \operatorname{c_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{c_{x}}{\left (t \right )} - \frac{Fac \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}} - \frac{Fbc \left(- \operatorname{b_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right) \frac{d}{d t} \operatorname{c_{x}}{\left (t \right )} + \left(- 2 C_{fry} e^{- 2 \operatorname{b_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )} - \frac{Fab \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{b_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fbc \left(- \operatorname{b_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right) \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )} + \left(- 2 C_{fry} e^{- 2 \operatorname{c_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )} - \frac{Fac \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}} - \frac{Fbc \left(- \operatorname{b_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right) \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )}$$
$$\left [ \frac{Fab \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{b_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fac \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}, \quad \frac{Fab \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{b_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fac \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}, \quad - 4 C_{frx} e^{- 2 \operatorname{b_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{b_{x}}{\left (t \right )} - \frac{Fab \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{b_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fbc \left(- \operatorname{b_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}, \quad - 4 C_{fry} e^{- 2 \operatorname{b_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )} - \frac{Fab \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{b_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{b_{y}}{\left (t \right )}\right)^{2}}} + \frac{Fbc \left(- \operatorname{b_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}, \quad - 4 C_{frx} e^{- 2 \operatorname{c_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{c_{x}}{\left (t \right )} - \frac{Fac \left(- \operatorname{a_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}} - \frac{Fbc \left(- \operatorname{b_{x}}{\left (t \right )} + \operatorname{c_{x}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}, \quad - 4 C_{fry} e^{- 2 \operatorname{c_{y}}{\left (t \right )}} \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )} - \frac{Fac \left(- \operatorname{a_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{a_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{a_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}} - \frac{Fbc \left(- \operatorname{b_{y}}{\left (t \right )} + \operatorname{c_{y}}{\left (t \right )}\right)}{\sqrt{\left(\operatorname{b_{x}}{\left (t \right )} - \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\operatorname{b_{y}}{\left (t \right )} - \operatorname{c_{y}}{\left (t \right )}\right)^{2}}}\right ]$$
In [48]:
L = E_kin - Vg 
display(L)
$$- g m_{1} \left(\operatorname{a_{y}}{\left (t \right )} + e^{- 2 \operatorname{a_{y}}{\left (t \right )}}\right) - g m_{2} \left(\operatorname{b_{y}}{\left (t \right )} + e^{- 2 \operatorname{b_{y}}{\left (t \right )}}\right) - g m_{2} \left(\operatorname{c_{y}}{\left (t \right )} + e^{- 2 \operatorname{c_{y}}{\left (t \right )}}\right) + \frac{m_{1} \left(\left(\frac{d}{d t} \operatorname{a_{x}}{\left (t \right )}\right)^{2} + \left(\frac{d}{d t} \operatorname{a_{y}}{\left (t \right )}\right)^{2}\right)}{2} + \frac{m_{2} \left(\left(\frac{d}{d t} \operatorname{b_{x}}{\left (t \right )}\right)^{2} + \left(\frac{d}{d t} \operatorname{b_{y}}{\left (t \right )}\right)^{2}\right)}{2} + \frac{m_{2} \left(\left(\frac{d}{d t} \operatorname{c_{x}}{\left (t \right )}\right)^{2} + \left(\frac{d}{d t} \operatorname{c_{y}}{\left (t \right )}\right)^{2}\right)}{2}$$
In [49]:
dl_1 = [sp.diff(L, dq) for dq in dqdt]
dl_1
Out[49]:
$$\left [ m_{1} \frac{d}{d t} \operatorname{a_{x}}{\left (t \right )}, \quad m_{1} \frac{d}{d t} \operatorname{a_{y}}{\left (t \right )}, \quad m_{2} \frac{d}{d t} \operatorname{b_{x}}{\left (t \right )}, \quad m_{2} \frac{d}{d t} \operatorname{b_{y}}{\left (t \right )}, \quad m_{2} \frac{d}{d t} \operatorname{c_{x}}{\left (t \right )}, \quad m_{2} \frac{d}{d t} \operatorname{c_{y}}{\left (t \right )}\right ]$$
In [50]:
dl_2 = [sp.diff(L, qi) for qi in q]
dl_2
Out[50]:
$$\left [ 0, \quad - g m_{1} \left(1 - 2 e^{- 2 \operatorname{a_{y}}{\left (t \right )}}\right), \quad 0, \quad - g m_{2} \left(1 - 2 e^{- 2 \operatorname{b_{y}}{\left (t \right )}}\right), \quad 0, \quad - g m_{2} \left(1 - 2 e^{- 2 \operatorname{c_{y}}{\left (t \right )}}\right)\right ]$$
In [51]:
#sys = [sp.diff(dl_1[i], t) - dl_2[i] - (Qu[i] + ohj[i]) for i in range(0, sysdim)]
syseq = [sp.diff(dl_1[i], t) - dl_2[i] - Qu[i] for i in range(0, sysdim)]
# display(sys)
In [52]:
display(d2qdt2.T)
kiihtyvyydet = sp.solve(syseq, d2qdt2)
#display(kiihtyvyydet)
$$\left[\begin{matrix}\frac{d^{2}}{d t^{2}} \operatorname{a_{x}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{a_{y}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{b_{x}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{b_{y}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{c_{x}}{\left (t \right )} & \frac{d^{2}}{d t^{2}} \operatorname{c_{y}}{\left (t \right )}\end{matrix}\right]$$

Siinä meillä on tilayhtälö toisen kertaluvun differentiaaliyhtälöinä. Numeerinen ratkaisu vaatii tilayhtölän esittämistä ensimmäisen kertaluvun differentiaaliyhtlöinä, joten alamme vääntää ylläolevaa tilayhtälöä muotoon $\dot{\underline{z}}(t) = \underline{f}(\underline{z}(t))$

In [53]:
#f = sp.Matrix([0 for i in range(0, 2 * (sysdim))])
f = [0 for i in range(0, 2 * (sysdim))]

for i in range(0, sysdim):
    f[i] = sp.simplify(kiihtyvyydet[d2qdt2[i]])
    f[i + sysdim] = sp.simplify(dqdt[i])
#display(f)
In [54]:
vax, vay, vbx, vby, vcx, vcy = sp.symbols('va_x, va_y, vb_x, vb_y, vc_x, vc_y', real=True)
ax, ay, bx, by, cx, cy = sp.symbols('a_x, a_y, b_x, b_y, c_x, c_y', real=True)

dx_subs = [(sp.diff(axf(t), t), vax),(sp.diff(ayf(t), t), vay),
          (sp.diff(bxf(t), t), vbx), (sp.diff(byf(t), t), vby),
          (sp.diff(cxf(t), t), vcx), (sp.diff(cyf(t), t), vcy),
           (axf(t), ax),(ayf(t), ay),(bxf(t), bx),(byf(t), by), (cxf(t), cx),(cyf(t), cy)
          ]

Joitakuita saattoi yllä hämmentää lauseke f[i] = kiihtyvyydet[d2qdt2[i]].

sp.solve antaa yhtälön ratkaisun niin kutsuttuna dictionaryna, josta alla r on esimerkki. Dictionaryn alkiohin ei voi viitata numeroilla vaan pitää käyttää "avaimia", joita r:ssä ovat 'a' ja 'b'

In [55]:
r = {'a': 5, 'b': 7}
eka = r['a']
eka
Out[55]:
$$5$$

Yhtälöryhmän ratkaisussa avaimina ovat muuttujien toiset derivaatat, joten voin käyttää avaimina matriisin d2qdt2 alkioita {{d2qdt2}} samoin kuin käytän alla listan ab alkioita.

(Itse ainakin hämmästelen, miten keksinkään tällaisen ratkaisun ;-)

In [56]:
ab = ['a', 'b']
eka = r[ab[0]]
eka
Out[56]:
$$5$$

Koodin generointi simulointia varten

Alla ensiksi pari esimerkkiä selittämään, mitä kannattaa tehdä ennen koodin generoimista. Emme esimerkiksi halua python-koodiin turhaa Matrix-määrittelyä. (Tämä on amatöörin ajattelua, kokeneempi koodari tietänee paremman ratkaisun.)

In [57]:
esim1 = sp.Matrix([1, 2, 3])
print("esim1 tuottaisi tällaista koodia: ", esim1)
esim1 tuottaisi tällaista koodia:  Matrix([[1], [2], [3]])
In [58]:
esim2 = [x for x in esim1]
print("esim2 tuottaisi tällaista koodia: ", esim2)
esim2
esim2 tuottaisi tällaista koodia:  [1, 2, 3]
Out[58]:
$$\left [ 1, \quad 2, \quad 3\right ]$$
In [59]:
display(gr0)
#grini = sp.N(gr0)
grini = gr0
display(grini)
$$\frac{\log{\left (2 \right )}}{2}$$
$$\frac{\log{\left (2 \right )}}{2}$$
In [60]:
Tf, ds, v0, l0, lf, h0, hf = sp.symbols('Tf ds v_0 l_0 l_f h_0 h_f', real=True)
cfv, cfx, Cfb, Ctau, Cu, Cfr = sp.symbols('c_fv, c_fx, C_fb, C_tau, C_u, C_fr', real=True)
cfx0,cfx1,cfx2,cfx3,cfx4,cfx5 = sp.symbols('cfx_0,cfx_1,cfx_2,cfx_3,cfx_4,cfx_5', real=True) 
cfv0,cfv1,cfv2,cfv3,cfv4,cfv5 = sp.symbols('cfv_0,cfv_1,cfv_2,cfv_3,cfv_4,cfv_5', real=True) 
ax0, ay0, bx0, by0, cx0, cy0 = sp.symbols('ax_0, ay_0, bx_0, by_0, cx_0, cy_0', real=True)
avx0, avy0, bvx0, bvy0, cvx0, cvy0 = sp.symbols('avx_0, avy_0, bvx_0, bvy_0, cvx_0, cvy_0', real=True)
axf, ayf, bxf, byf, cxf, cyf = sp.symbols('ax_f, ay_f, bx_f, by_f, cx_f, cy_f', real=True)
avxf, avyf, bvxf, bvyf, cvxf, cvyf = sp.symbols('avx_f, avy_f, bvx_f, bvy_f, cvx_f, cvy_f', real=True)

cfxya, cfxybc, cfva, cfvbc = sp.symbols('cfxy_a, cfxy_bc, cfv_a, cfv_bc', real=True)
    
par_subs = []

#ds = 5
#v0 = 0.0
#L = 3.0
#s0 = 0
#h0 = gr0
#hf = gr0

params = [
    (Tf, 2.0), 
    ('sysdim', sysdim),
    (g, 9.81), 
    (m1, 3), (m2, 1),
    (l0, 2.0), 
    (lf, 2.0), 
    (ds, 5), 
    ('gr0', grini), 
    (h0, grini),     (hf, grini), 
    (ax0, l0/2), (axf, lf/2+ds), 
    (ay0, h0+l0), (ayf, hf+lf), 
    (bx0, 0), (bxf, ds), 
    (by0, h0), (byf, hf), 
    (cx0, l0), (cxf, ds+lf), 
    (cy0, h0), (cyf, hf), 

    (avx0, 0), (avxf, 0.0), 
    (avy0, 0.0), (avyf, 0.0), 
    (bvx0, 0), (bvxf, 0.0), 
    (bvy0, 0.0), (bvyf, 0.0), 
    (cvx0, 0), (cvxf, 0.0), 
    (cvy0, 0.0), (cvyf, 0.0), 

    (Ctau, 1.0), 
    (Cu, 1.0),
    (Cfrx, 1.0),
    (Cfry, 1.0),
    (Cfb, 10.0),    
    (cfxya, 1), 
    (cfxybc, 1), 
    (cfva, 1), 
    (cfvbc, 1), 
]

with open('parNew.py', 'w') as fil:
    fil.write("# -*- coding: utf-8 -*-\n")
    for (Nam, Val) in params:
        fil.write("{} = {}\n".format(Nam, Val))
        
par_subs = [(Name, "pp(" + str(Name) + ")")
            for i, (Name, Value) in enumerate(params)]
display(par_subs)
[(Tf, 'pp(Tf)'),
 ('sysdim', 'pp(sysdim)'),
 (g, 'pp(g)'),
 (m_1, 'pp(m_1)'),
 (m_2, 'pp(m_2)'),
 (l_0, 'pp(l_0)'),
 (l_f, 'pp(l_f)'),
 (ds, 'pp(ds)'),
 ('gr0', 'pp(gr0)'),
 (h_0, 'pp(h_0)'),
 (h_f, 'pp(h_f)'),
 (ax_0, 'pp(ax_0)'),
 (ax_f, 'pp(ax_f)'),
 (ay_0, 'pp(ay_0)'),
 (ay_f, 'pp(ay_f)'),
 (bx_0, 'pp(bx_0)'),
 (bx_f, 'pp(bx_f)'),
 (by_0, 'pp(by_0)'),
 (by_f, 'pp(by_f)'),
 (cx_0, 'pp(cx_0)'),
 (cx_f, 'pp(cx_f)'),
 (cy_0, 'pp(cy_0)'),
 (cy_f, 'pp(cy_f)'),
 (avx_0, 'pp(avx_0)'),
 (avx_f, 'pp(avx_f)'),
 (avy_0, 'pp(avy_0)'),
 (avy_f, 'pp(avy_f)'),
 (bvx_0, 'pp(bvx_0)'),
 (bvx_f, 'pp(bvx_f)'),
 (bvy_0, 'pp(bvy_0)'),
 (bvy_f, 'pp(bvy_f)'),
 (cvx_0, 'pp(cvx_0)'),
 (cvx_f, 'pp(cvx_f)'),
 (cvy_0, 'pp(cvy_0)'),
 (cvy_f, 'pp(cvy_f)'),
 (C_tau, 'pp(C_tau)'),
 (C_u, 'pp(C_u)'),
 (C_frx, 'pp(C_frx)'),
 (C_fry, 'pp(C_fry)'),
 (C_fb, 'pp(C_fb)'),
 (cfxy_a, 'pp(cfxy_a)'),
 (cfxy_bc, 'pp(cfxy_bc)'),
 (cfv_a, 'pp(cfv_a)'),
 (cfv_bc, 'pp(cfv_bc)')]

Systeemissä on vakioparametrejä, joita on pikkuisen hankala käsitellä koodin generoinnissa. Luon omaan moduulinsa olion par, jonka elementteinä ovat järjestelmän parametrit.

Syystä, joka selviää myöhemmin, upotan aluksi vakioiden nimet lausekkeen pp() sisään.

with open('par.py', 'w') as fil: fil.write("# -- coding: utf-8 --\n") for (Nam, Val) in params: fil.write("{} = {}\n".format(Nam, Val))

Valmistaudutaan generoimaan simuloinnissa tarvittava järjestelmän tilan derivaatan palauttava funktio. Muuttujien vaihdoilla saamme kuusi ensimmäisen kertaluvun differentiaalyhtälöä.

In [61]:
#display(f)
#display(dx_subs)
In [62]:
rs = sp.Symbol('r')
r = sp.Function('r')(t)
static = sp.exp(r).subs(r, rs)
display(static)
$$e^{r}$$
In [63]:
#dx = [fi.subs(dx_subs) for fi in f]
dx = [sp.simplify(fi.subs(dx_subs)) for fi in f]
In [64]:
dxsim = [dxi.subs(par_subs) for dxi in dx]
#dx = [sp.expand(sp.powsimp(dxi)) for dxi in dx0]
#display(dx)

Järjestelmän tilavektori

In [65]:
yyx = [yi for yi in q.subs(dx_subs)]
yyv = [yi for yi in dqdt.subs(dx_subs)]
yy = [*yyv, *yyx]
display(yy)
$$\left [ va_{x}, \quad va_{y}, \quad vb_{x}, \quad vb_{y}, \quad vc_{x}, \quad vc_{y}, \quad a_{x}, \quad a_{y}, \quad b_{x}, \quad b_{y}, \quad c_{x}, \quad c_{y}\right ]$$
In [66]:
uu = [Fab, Fac, Fbc]

Numeerinen integrointi

Kirjoitetaan simuloinnissa tarvittava funktio fdxdt(), joka palauttaa järjestelmämuuttujien derivaatat järjestelmän tilan ja ohjausten funktiona. Videopelimäisessä simulaattorissa tätä funktiota käytetään seuraavasti: Generoimamme funktion fdxdt() nimi annetaan argumentiksi integrointifuntiolle rk4, joka kutsuu sitä laskiessaan järjestelmän siirtymää $yy(t) \to yy(t+\Delta t)$

from dxdt_sympy import fdxdt
from rk4 import rk4

def liike(yy):
    uu = ohjaukset(yy)
    yy = rk4(fdxdt, yy, uu, Aika.Dt)
    return yy

rk4 taas on seuraavanlainen järjestelmää yhden aika-askeleen eteenpäin siirtävä funktio (runge-kutta integrointi)

def rk4(fdydt, yy, uu, dt):
    kk1 = fdydt(yy, uu)
    yk1 = [yy[i] + k1 * dt / 2.0 for i, k1 in enumerate(kk1)]
    kk2 = fdydt(yk1, uu)
    yk2 = [yy[i] + k2 * dt / 2.0 for i, k2 in enumerate(kk2)]
    kk3 = fdydt(yk2, uu)
    yk3 = [yy[i] + k3 * dt for i, k3 in enumerate(kk3)]
    kk4 = fdydt(yk3, uu)
    yy1 = [
        y + dt / 6.0 * (kk1[i] + 2.0 * kk2[i] + 2.0 * kk3[i] + kk4[i])
        for i, y in enumerate(yy)
    ]
    return yy1

Symbolisista lausekkeista merkkijonoja

Edelläjohdetut lausekkeet vaativat pientä manipulointia, ennenkuin ne voidaan kirjoittaa tiedostoon python-ohjelmaksi. Osa lausekkeista on listoja, jotka pitää käsitellä alkio kerrallaan niin, että tuloksenakin on lista.

Lausekkeesta expr syntyvä koodi kirjoitetaan tiedostoon fil, sitä sisennetään merkkijonon indent verran ja symbolisen lausekkeen osia korvataan listan par_subs mukaan.

In [67]:
def wr_expr(fil, expr, indent, subs):
#    print(expr)
    if (type(expr) == list):
        dim = len(expr) - 1
        fil.write('[')
        for i, ex in enumerate(expr):
            wr_expr(fil, ex, indent, subs)
            if (i < dim):
                fil.write(',')
        fil.write(']')

    else:
        subs_ex(fil, expr, indent, subs)

Alla subs_ex(), eli lausekkeen, joka ei ole lista, kirjoittaminen koodiksi.

Generoitavassa koodissa tilamuuttujat esitetään vektorina zz. Ennen tämän funktion kutsumista järjestelmän tilamuuttujat on sympy.subs-funktiolla koodattu muotoon zz(i), koska en sympy.subs-komennolla onnistunut saamaan niitä muotoon zz[i]. Esimerkiksi $v_{car} = zz(0)$, mutta haluamme sen muotoon $zz[0]$

Seuraavassa lauseke muutetaan ensin merkkijonoksi ja sen jälkeen käytetään regular expressions komentoja, jotka ovat muotoa re.sub(r'korvattava merkkijono', r'millä korvataan', merkkijono)

r'[0-9]' tarkoittaa mitä hyvänsä numeroa välitltä 0-9. r'[0-9]*' tarkoittaa edellinen yksi tai useampi kertaa, eli mikä hyvänsä numerojono.

Edellinen käärittynä kaarisulkeisiin — r'([0-9]*)' — tarkoittaa, että löytyvään merkkijonoon voidaan myöhemmin viitata merkinnällä '\1'.

Kenoviiva — esimerkiksi '\(' — tarkoittaa, että kenoviivaa seuraava merkki — edellä '(' — otetaan sellaisenaan.

In [68]:
tst1 = re.sub(r'zz\(([0-9]*)\)', r'zz[\1]', "pp(M_load)*zz(8)")
tst1
Out[68]:
'pp(M_load)*zz[8]'
In [69]:
def subs_ex(fil, ex, indent, subs):
#    print(ex)
    try:
        ex = ex.subs(subs)
    except:
        pass
#    s0 = re.sub(r'\^', r'',str(ex))
    s1 = re.sub(r'zz\(([0-9]*)\)', r'zz[\1]', str(ex))
    s2 = re.sub(r'pp\(([^)]*)\)', r'par.\1', s1)
    s = re.sub(r'cf\(([0-9]*)\)', r'par.cf_\1', s2)
#    s = s.replace('Min', 'min')
#    s = s.replace('sign', 'numpy.sign')
#    s = re.sub(r'Derivative(*)', '0', s)
#    s = re.sub(r'DiracDelta\([^)]*\)', r'0', s)
#    s = re.sub(r'Heaviside\(([^)]*)\)', r'numpy.heaviside(\1,0.5)', s)
#    s = s.replace('Heaviside', 'numpy.heaviside')
    fil.write(
        fill(s,initial_indent=indent, subsequent_indent="    ", break_long_words=False))

Kirjoitetaan simulointiin tarvittava systeemiyhtälö

In [70]:
with open('dxdt.py', 'w') as fil:
    fil.write("# -*- coding: utf-8 -*-\n")
    fil.write("from math import sqrt, exp \n")
    # fil.write("import numpy\n")
    fil.write("import par \n\n")
    fil.write("E = exp(1.0)\n")

    fil.write("Musta = (0, 0, 0)\n")
    fil.write("Valk = (255, 255, 255)\n")
    fil.write("Sin = (0, 0, 255)\n")
    fil.write("Pun = (255, 0, 0)\n")
    fil.write("Vihr = (0, 255, 0)\n")
    fil.write("Kelt = (255, 255, 0)\n")

    fil.write("def fdxdt(yy, uu):\n    ")
    wr_expr(fil, yy, " ", [])
    fil.write(" = yy\n    ")
    wr_expr(fil, uu, "", [])
    fil.write(" = uu\n")
    fil.write("    dxdt =")
    wr_expr(fil, dxsim, " ", [])
    fil.write("\n")
    fil.write("    return dxdt\n")
In [71]:
with open('par.py', 'r') as fin:
    print(fin.read())
    
with open('dxdt.py', 'r') as fin:
    print(fin.read())
# -*- coding: utf-8 -*-
Tf = 1.5
sysdim = 6
g = 9.81
m_1 = 3
m_2 = 1
l_0 = 2.0
l_f = 2.0
ds = 5
gr0 = 0.4
h_0 = 0.0
h_f = 0.2
ax_0 = l_0/2
ax_f = ds + l_f/2
ay_0 = h_0 + l_0
ay_f = h_f + l_f
bx_0 = 0
bx_f = ds
by_0 = h_0
by_f = h_f
cx_0 = l_0
cx_f = ds + l_f
cy_0 = h_0
cy_f = h_f
avx_0 = 0
avx_f = 0.5
avy_0 = 0.0
avy_f = 0.0
bvx_0 = 0
bvx_f = 0.5
bvy_0 = 0.0
bvy_f = 0.0
cvx_0 = 0
cvx_f = 0.5
cvy_0 = 0.0
cvy_f = 0.0
C_tau = 1.0
C_u = 1.0
C_frx = 1.0
C_fry = 1.0
C_fb = 10.0
cfxy_a = 1
cfxy_bc = 1
cfv_a = 1
cfv_bc = 1

# -*- coding: utf-8 -*-
from math import sqrt, exp 
import par 

E = exp(1.0)
Musta = (0, 0, 0)
Valk = (255, 255, 255)
Sin = (0, 0, 255)
Pun = (255, 0, 0)
Vihr = (0, 255, 0)
Kelt = (255, 255, 0)
def fdxdt(yy, uu):
    [ va_x, va_y, vb_x, vb_y, vc_x, vc_y, a_x, a_y, b_x, b_y, c_x, c_y] = yy
    [Fab,Fac,Fbc] = uu
    dxdt =[ -(Fab*(a_x - b_x)*sqrt(a_x**2 - 2*a_x*c_x + a_y**2 - 2*a_y*c_y +
    c_x**2 + c_y**2) + Fac*(a_x - c_x)*sqrt(a_x**2 - 2*a_x*b_x +
    a_y**2 - 2*a_y*b_y + b_x**2 + b_y**2))/(par.m_1*sqrt(a_x**2 -
    2*a_x*b_x + a_y**2 - 2*a_y*b_y + b_x**2 + b_y**2)*sqrt(a_x**2 -
    2*a_x*c_x + a_y**2 - 2*a_y*c_y + c_x**2 + c_y**2)), -Fab*a_y/(par.m_1*sqrt(a_x**2 - 2*a_x*b_x + a_y**2 - 2*a_y*b_y +
    b_x**2 + b_y**2)) + Fab*b_y/(par.m_1*sqrt(a_x**2 - 2*a_x*b_x +
    a_y**2 - 2*a_y*b_y + b_x**2 + b_y**2)) -
    Fac*a_y/(par.m_1*sqrt(a_x**2 - 2*a_x*c_x + a_y**2 - 2*a_y*c_y +
    c_x**2 + c_y**2)) + Fac*c_y/(par.m_1*sqrt(a_x**2 - 2*a_x*c_x +
    a_y**2 - 2*a_y*c_y + c_x**2 + c_y**2)) - par.g +
    2*par.g*exp(-2*a_y), Fab*a_x/(par.m_2*sqrt(a_x**2 - 2*a_x*b_x + a_y**2 - 2*a_y*b_y +
    b_x**2 + b_y**2)) - Fab*b_x/(par.m_2*sqrt(a_x**2 - 2*a_x*b_x +
    a_y**2 - 2*a_y*b_y + b_x**2 + b_y**2)) -
    Fbc*b_x/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x + b_y**2 - 2*b_y*c_y +
    c_x**2 + c_y**2)) + Fbc*c_x/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x +
    b_y**2 - 2*b_y*c_y + c_x**2 + c_y**2)) -
    4*par.C_frx*vb_x*exp(-2*b_y)/par.m_2, Fab*a_y/(par.m_2*sqrt(a_x**2 - 2*a_x*b_x + a_y**2 - 2*a_y*b_y +
    b_x**2 + b_y**2)) - Fab*b_y/(par.m_2*sqrt(a_x**2 - 2*a_x*b_x +
    a_y**2 - 2*a_y*b_y + b_x**2 + b_y**2)) -
    Fbc*b_y/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x + b_y**2 - 2*b_y*c_y +
    c_x**2 + c_y**2)) + Fbc*c_y/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x +
    b_y**2 - 2*b_y*c_y + c_x**2 + c_y**2)) -
    4*par.C_fry*vb_y*exp(-2*b_y)/par.m_2 - par.g + 2*par.g*exp(-2*b_y), Fac*a_x/(par.m_2*sqrt(a_x**2 - 2*a_x*c_x + a_y**2 - 2*a_y*c_y +
    c_x**2 + c_y**2)) - Fac*c_x/(par.m_2*sqrt(a_x**2 - 2*a_x*c_x +
    a_y**2 - 2*a_y*c_y + c_x**2 + c_y**2)) +
    Fbc*b_x/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x + b_y**2 - 2*b_y*c_y +
    c_x**2 + c_y**2)) - Fbc*c_x/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x +
    b_y**2 - 2*b_y*c_y + c_x**2 + c_y**2)) -
    4*par.C_frx*vc_x*exp(-2*c_y)/par.m_2, Fac*a_y/(par.m_2*sqrt(a_x**2 - 2*a_x*c_x + a_y**2 - 2*a_y*c_y +
    c_x**2 + c_y**2)) - Fac*c_y/(par.m_2*sqrt(a_x**2 - 2*a_x*c_x +
    a_y**2 - 2*a_y*c_y + c_x**2 + c_y**2)) +
    Fbc*b_y/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x + b_y**2 - 2*b_y*c_y +
    c_x**2 + c_y**2)) - Fbc*c_y/(par.m_2*sqrt(b_x**2 - 2*b_x*c_x +
    b_y**2 - 2*b_y*c_y + c_x**2 + c_y**2)) -
    4*par.C_fry*vc_y*exp(-2*c_y)/par.m_2 - par.g + 2*par.g*exp(-2*c_y), va_x, va_y, vb_x, vb_y, vc_x, vc_y]
    return dxdt

Optimiohjauksen laskeminen

Järjestelmän alku- ja lopputila

In [72]:
#ax0, ay0, avx0, avy0, axf, ayf, bx0, by0, bvx0, bvy0, bxf, byf, cx0, cy0, cvx0, cvy0, cxf, cyf = \
#sp.symbols('ax_0, ay_0, avx_0, avy_0, ax_f, ay_f, bx_0, by_0, bvx_0, bvy_0, bx_f, by_f, \
#           cx_0, cy_0, cvx_0, cvy_0, cx_f, cy_f', real=True)

yy0 = [avx0, avy0, bvx0, bvy0, cvx0, cvy0, ax0, ay0, bx0, by0, cx0, cy0]
#yyf = [sp.N(0), sp.N(0), sp.N(0), sp.N(0), sp.N(0), sp.N(0), axf, ayf, bxf, byf, cxf, cyf]
yyf = [0, 0, 0, 0, 0, 0, axf, ayf, bxf, byf, cxf, cyf]
display(yy, yy0, yyf)
$$\left [ va_{x}, \quad va_{y}, \quad vb_{x}, \quad vb_{y}, \quad vc_{x}, \quad vc_{y}, \quad a_{x}, \quad a_{y}, \quad b_{x}, \quad b_{y}, \quad c_{x}, \quad c_{y}\right ]$$
$$\left [ avx_{0}, \quad avy_{0}, \quad bvx_{0}, \quad bvy_{0}, \quad cvx_{0}, \quad cvy_{0}, \quad ax_{0}, \quad ay_{0}, \quad bx_{0}, \quad by_{0}, \quad cx_{0}, \quad cy_{0}\right ]$$
$$\left [ 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad ax_{f}, \quad ay_{f}, \quad bx_{f}, \quad by_{f}, \quad cx_{f}, \quad cy_{f}\right ]$$

Optimaalinen siirtymä alusta loppuun

Siirretään kuorma alkutilasta lopputilaan optimaalisesti laskemalla sellaiset vaunua ja kuormaa liikuttavat voimat, että kustannusfunktion $gl$ integraali siirtymän alkuhetkestä loppuhetkeen minimoituu ohjauksien $\underline u(t)$ suhteen.

Merkitään ensin aikaa $\tau$:lla niin, että alussa $\tau = 0$ ja lopussa $\tau = T_{f}$ Tehdään sitten muuttujan vaihdos $ t = \tau/T_{f}$ Tavoitteena on tehdä $T_f$:stä optimoitava muuttuja, eli ratkaistaan vapaan loppuajan tehtävää.

$$ J = G_f(\underline x(1)) + \int_0^{1}T_f gl(\underline x(t),\underline u(t),t) \,\mathrm{d}t $$
In [73]:
# dx = sp.Matrix([Tf*dxi for dxi in dx])  minimum time !!!
dx = sp.Matrix([dxi for dxi in dx])
#display(dx)
In [74]:
Cu, Ctau, Ch = sp.symbols('C_u, C_tau, C_h', real=True)

#gl0 = Cu*((Fab)**2 + (Fac)**2  + (Fbc)**2) + Cl*((bx-ax-l0)**2 + (ax-cx-l0)**2 + (ay-by-l0)**2 + (ay-cy-l0)**2)
#gl0 = Cu*((Fab)**2 + (Fac)**2  + (Fbc)**2) + Cl*(ay - l0*2/sp.sqrt(5))**2 + Cl*((bx-ax-l0/2)**2 + (ax-cx-l0/2)**2 + (ay-by-l0)**2 + (ay-cy-l0)**2)
#gl = (Cu*((Fab + m1/2*g*sp.sqrt(5)/2)**2 + (Fac + m1/2*g*sp.sqrt(5)/2)**2  + (Fbc - m1/2*g/2)**2)) # + Ch*(l0*2/sp.sqrt(5) - ay)**2 + Cl*((lab-l0)**2 + (lac-l0)**2 + (lbc-l0)**2)

Fab0 = -m1*g*sp.sqrt(5)*sp.Rational(1,4)
Fac0 = Fab0
Fbc0 = m1*g*sp.Rational(1,4)
gl = Cu*((Fab-Fab0)**2 + (Fac-Fac0)**2)  + Ctau*(Fbc-Fbc0)**2 # + Cxend*((ax - axf)**2 + (bx - bxf)**2 + (cx - cxf)**2)

#gl = 1 

#display(gl0)
#gl = gl.subs(par_subs) 
#gl = Tf*gl0.subs(par_subs) # Minimum time  
display(gl)
$$C_{\tau} \left(Fbc - \frac{g m_{1}}{4}\right)^{2} + C_{u} \left(\left(Fab + \frac{\sqrt{5} g m_{1}}{4}\right)^{2} + \left(Fac + \frac{\sqrt{5} g m_{1}}{4}\right)^{2}\right)$$

Kustannuksessa sakotetaan neliöllisesti ohjausvoimia ja ison pallon korkeutta. Millä perusteella tällainen kriteeri? Se on helppo laskea ja vie kuorman paikasta toiseen turhia tempoilematta.

Loppukustannus

Optimointialgoritmin voi vaatia viemään järjestelmä tarkalleen lopputilaan. Tehtävä ratkesi niin muotoiltuna hyvin.

Kokeilin kuitenkin tarkan lopputilan vaatimisen sijasta nelilöllistä sakkoa poikkeamalle halutusta lopputilasta, joten dokumentoin senkin ratkaisun tässä samalla.

Lopputulos oli jokseenkin sama, mutta joskus numeerinen algoritmi antoi ratkaisun silloinkin, kun tarkalleen lopputilaan ei voinut päästä esimerkiksi siksi, että olin asettanut ohjausvoimille liian tiukat rajat.

In [75]:
display(yy, yyf)
#cfx = [sp.symbols('cfx'+str(i)) for i in range(sysdim)]
#cfv = [sp.symbols('cfv'+str(i)) for i in range(sysdim)]
#display(cfx, cfv)
$$\left [ va_{x}, \quad va_{y}, \quad vb_{x}, \quad vb_{y}, \quad vc_{x}, \quad vc_{y}, \quad a_{x}, \quad a_{y}, \quad b_{x}, \quad b_{y}, \quad c_{x}, \quad c_{y}\right ]$$
$$\left [ 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad ax_{f}, \quad ay_{f}, \quad bx_{f}, \quad by_{f}, \quad cx_{f}, \quad cy_{f}\right ]$$
In [76]:
#cfxya, cfxybc, cfva, cfvbc 
G_f = 0
for i in range(2):
    G_f = G_f + cfva*(yy[i] - yyf[i])**2
for i in range(2, sysdim):
    G_f = G_f + cfvbc*(yy[i] - yyf[i])**2
for i in range(sysdim, sysdim+2):
    G_f = G_f + cfxya*(yy[i] - yyf[i])**2
for i in range(sysdim+2, 2*sysdim):
    G_f = G_f + cfxybc*(yy[i] - yyf[i])**2
display(G_f)
$$cfv_{a} va_{x}^{2} + cfv_{a} va_{y}^{2} + cfv_{bc} vb_{x}^{2} + cfv_{bc} vb_{y}^{2} + cfv_{bc} vc_{x}^{2} + cfv_{bc} vc_{y}^{2} + cfxy_{a} \left(a_{x} - ax_{f}\right)^{2} + cfxy_{a} \left(a_{y} - ay_{f}\right)^{2} + cfxy_{bc} \left(b_{x} - bx_{f}\right)^{2} + cfxy_{bc} \left(b_{y} - by_{f}\right)^{2} + cfxy_{bc} \left(c_{x} - cx_{f}\right)^{2} + cfxy_{bc} \left(c_{y} - cy_{f}\right)^{2}$$
In [77]:
lamda_f = [sp.diff(G_f, yy[i]) for i in range(0, 2 * sysdim)]
lamda_f
Out[77]:
$$\left [ 2 cfv_{a} va_{x}, \quad 2 cfv_{a} va_{y}, \quad 2 cfv_{bc} vb_{x}, \quad 2 cfv_{bc} vb_{y}, \quad 2 cfv_{bc} vc_{x}, \quad 2 cfv_{bc} vc_{y}, \quad cfxy_{a} \left(2 a_{x} - 2 ax_{f}\right), \quad cfxy_{a} \left(2 a_{y} - 2 ay_{f}\right), \quad cfxy_{bc} \left(2 b_{x} - 2 bx_{f}\right), \quad cfxy_{bc} \left(2 b_{y} - 2 by_{f}\right), \quad cfxy_{bc} \left(2 c_{x} - 2 cx_{f}\right), \quad cfxy_{bc} \left(2 c_{y} - 2 cy_{f}\right)\right ]$$

Sovelletaan Pontryaginin minimiperiaatetta, joka johtaa nk kahden pisteen reuna-arvotehtävään. Sellaisten ratkaisemiseen on kehitetty numeerisia algoritmeja, joita varten väännämme ongelman niiden vaatimaan standardimuotoon.

Tilamuuttujien lisäksi tarvitaan yhtä monta nk. liittotilamuuttujaa.

H on Pontryaginin periaatteen mukainen Hamiltonin funktio ja yy_lt järjestelmän liittotila

liittotilamuuttujat

In [78]:
yy_lamda = sp.Matrix([sp.symbols('lamda_{}'.format(i)) for i in range(0,2*sysdim)])
display(yy_lamda.T)
$$\left[\begin{array}{cccccccccccc}\lambda_{0} & \lambda_{1} & \lambda_{2} & \lambda_{3} & \lambda_{4} & \lambda_{5} & \lambda_{6} & \lambda_{7} & \lambda_{8} & \lambda_{9} & \lambda_{10} & \lambda_{11}\end{array}\right]$$

Hamiltonin funktio

In [79]:
H = (yy_lamda.dot(dx) + gl) #.subs(par_subs)
display(H)
$$C_{\tau} \left(Fbc - \frac{g m_{1}}{4}\right)^{2} + C_{u} \left(\left(Fab + \frac{\sqrt{5} g m_{1}}{4}\right)^{2} + \left(Fac + \frac{\sqrt{5} g m_{1}}{4}\right)^{2}\right) - \frac{\lambda_{0} \left(Fab \left(a_{x} - b_{x}\right) \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}} + Fac \left(a_{x} - c_{x}\right) \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}\right)}{m_{1} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} + \lambda_{1} \left(- \frac{Fab a_{y}}{m_{1} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}} + \frac{Fab b_{y}}{m_{1} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}} - \frac{Fac a_{y}}{m_{1} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} + \frac{Fac c_{y}}{m_{1} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - g + 2 g e^{- 2 a_{y}}\right) + \lambda_{10} vc_{x} + \lambda_{11} vc_{y} + \lambda_{2} \left(- \frac{4 C_{frx} vb_{x} e^{- 2 b_{y}}}{m_{2}} + \frac{Fab a_{x}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}} - \frac{Fab b_{x}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}} - \frac{Fbc b_{x}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} + \frac{Fbc c_{x}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}}\right) + \lambda_{3} \left(- \frac{4 C_{fry} vb_{y} e^{- 2 b_{y}}}{m_{2}} + \frac{Fab a_{y}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}} - \frac{Fab b_{y}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} b_{x} + a_{y}^{2} - 2 a_{y} b_{y} + b_{x}^{2} + b_{y}^{2}}} - \frac{Fbc b_{y}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} + \frac{Fbc c_{y}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - g + 2 g e^{- 2 b_{y}}\right) + \lambda_{4} \left(- \frac{4 C_{frx} vc_{x} e^{- 2 c_{y}}}{m_{2}} + \frac{Fac a_{x}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - \frac{Fac c_{x}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} + \frac{Fbc b_{x}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - \frac{Fbc c_{x}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}}\right) + \lambda_{5} \left(- \frac{4 C_{fry} vc_{y} e^{- 2 c_{y}}}{m_{2}} + \frac{Fac a_{y}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - \frac{Fac c_{y}}{m_{2} \sqrt{a_{x}^{2} - 2 a_{x} c_{x} + a_{y}^{2} - 2 a_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} + \frac{Fbc b_{y}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - \frac{Fbc c_{y}}{m_{2} \sqrt{b_{x}^{2} - 2 b_{x} c_{x} + b_{y}^{2} - 2 b_{y} c_{y} + c_{x}^{2} + c_{y}^{2}}} - g + 2 g e^{- 2 c_{y}}\right) + \lambda_{6} va_{x} + \lambda_{7} va_{y} + \lambda_{8} vb_{x} + \lambda_{9} vb_{y}$$

Ohjausvektori

In [80]:
u = sp.Matrix(uu)
u_lst = uu
display(u.T)
$$\left[\begin{matrix}Fab & Fac & Fbc\end{matrix}\right]$$

Optimaalinen ohjaus ajan funktiona löytyy minimoilla Hamiltonin funktio ohjauksen suhteen. Yksinkertaisen kustannusfunktiomme ansiosta minimi löytyy derivaatan 0-kohdasta

In [81]:
dHdu = [sp.diff(H, ui) for ui in u]
#dHdu
In [82]:
uopt = sp.solve(dHdu, u)
uopt = sp.simplify(uopt)

Hessian matriisia voidaan joissain tapauksissa käyttää apuna numeerisessa ratkaisussa.

HessH = [[0 for i in range(0,udim)]for i in range(0,udim)]
for i in range(0,udim):
    for j in range(0,udim):
        HessH[i][j] = sp.diff(dHdu[i],u[j])

Liittotilan tilayhtälöt (= liittotilamuuttujien aikaderivaatat) saadaan derivoimalla hamiltonin funktio varsinaisilla tilamuuttujilla.

In [83]:
dlt = sp.Matrix([-sp.diff(H, yi) for yi in yy])
#dlt = [sp.simplify(d) for d in dlt]
#dlt

Optimiohjauksen numeerinen ratkaiseminen vaatii alkuarvauksen ratkaisulle ajan funktiona. Alkuarvauksen ei helpoissa tapauksissa tarvitse olla kovin hyvä, mutta ratkaisu ei välttämättä lähde konvergoimaan, jos alkuarvaus on esimerkiksi pelkkiä nollia.

Laitan alkuarvaukseksi vakionopeuden vaunulle ja vaijerin pituudelle, minkä perusteella vaunun paikan ja vaijerin pituuden alkuarvaus on helppo laskea.

Liittotiloilla laitoin alkuarvauksen summamutikassa. Arvelin, että joku nollasta poikkeava olisi hyvä.

In [84]:
guess = [0 for i in range(4*sysdim)]
for i in range(2*sysdim):
    guess[i] = yy0[i]
for i in range(2*sysdim, 4*sysdim):
    guess[i] = 0 
In [85]:
for i in range(0, 2*sysdim):  # subs ei onnistu reaaliluvuille, siksi vajaa range
    guess[i] = guess[i].subs(par_subs)
In [86]:
display(sysdim)
display(yy0, yyf, guess)
$$6$$
$$\left [ avx_{0}, \quad avy_{0}, \quad bvx_{0}, \quad bvy_{0}, \quad cvx_{0}, \quad cvy_{0}, \quad ax_{0}, \quad ay_{0}, \quad bx_{0}, \quad by_{0}, \quad cx_{0}, \quad cy_{0}\right ]$$
$$\left [ 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad ax_{f}, \quad ay_{f}, \quad bx_{f}, \quad by_{f}, \quad cx_{f}, \quad cy_{f}\right ]$$
$$\left [ pp(avx_{0)}, \quad pp(avy_{0)}, \quad pp(bvx_{0)}, \quad pp(bvy_{0)}, \quad pp(cvx_{0)}, \quad pp(cvy_{0)}, \quad pp(ax_{0)}, \quad pp(ay_{0)}, \quad pp(bx_{0)}, \quad pp(by_{0)}, \quad pp(cx_{0)}, \quad pp(cy_{0)}, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0, \quad 0\right ]$$
</