In [1]:

    

%matplotlib inline

import sys
import pprint
import numpy as np
import inspect
import os
import matplotlib.pyplot as plt

root_path =  os.path.abspath(os.path.join(os.path.pardir))
sys.path.insert(0, root_path)

import pymoca.parser
import pymoca.backends.sympy.generator as generator
import pylab as pl

import sympy
import sympy.physics.mechanics as mech

sympy.init_printing()
mech.mechanics_printing()

%load_ext autoreload
%autoreload 2


    

In [2]:

    

frame_i = mech.ReferenceFrame('i')
t = sympy.symbols('t')
phi, theta, psi, P, Q, R, x, y, z, U, V, W = mech.dynamicsymbols(
    'phi, theta, psi, P, Q, R, x, y, z, U, V, W')
frame_b = frame_i.orientnew('b', 'Body', (psi, theta, phi), '321')
omega_ib = P*frame_b.x + Q*frame_b.y + R*frame_b.z
euler_sol = sympy.solve((frame_b.ang_vel_in(frame_i) - omega_ib).to_matrix(frame_b),
            [v.diff(t) for v in [phi, theta, psi]])
frame_b.set_ang_vel(frame_i, omega_ib)
euler_sol


    

    
Out[2]:





$$\left \{ \dot{\phi} : P + Q \operatorname{sin}\left(\phi\right) \operatorname{tan}\left(\theta\right) + R \operatorname{cos}\left(\phi\right) \operatorname{tan}\left(\theta\right), \quad \dot{\psi} : \frac{1}{\operatorname{cos}\left(\theta\right)} \left(Q \operatorname{sin}\left(\phi\right) + R \operatorname{cos}\left(\phi\right)\right), \quad \dot{\theta} : Q \operatorname{cos}\left(\phi\right) - R \operatorname{sin}\left(\phi\right)\right \}$$

In [3]:

    

point_o = mech.Point('o')
point_cm = point_o.locatenew('cm', x*frame_i.x + y*frame_i.y + z*frame_i.z)
point_cm.set_vel(frame_b, 0)
M_x, M_y, M_z, F_x, F_y, F_z = mech.dynamicsymbols('M_x, M_y, M_z, F_x, F_y, F_z')
M_b = M_x*frame_b.x + M_y*frame_b.y + M_z*frame_b.z
F_b = F_x*frame_b.x + F_y*frame_b.y + F_z*frame_b.z
V_i = U*frame_b.x + V*frame_b.y + W*frame_b.z
point_cm.set_vel(frame_i, V_i)
J_x, J_y, J_z, m = sympy.symbols('J_x, J_y, J_z, m')
aircraft = mech.RigidBody('aircraft', point_cm,
                          frame_b, m,
                          (mech.inertia(frame_b, J_x, J_y, J_z), point_cm))


    

In [4]:

    

H_i = aircraft.angular_momentum(point_cm, frame_i)
i_H_i = H_i.diff(t, frame_b) + frame_b.ang_acc_in(frame_i).cross(H_i)
(i_H_i - M_b).to_matrix(frame_b)


    

    
Out[4]:





$$\left[\begin{matrix}J_{x} \dot{P} - J_{y} Q \dot{R} + J_{z} R \dot{Q} - M_{x}\\J_{x} P \dot{R} + J_{y} \dot{Q} - J_{z} R \dot{P} - M_{y}\\- J_{x} P \dot{Q} + J_{y} Q \dot{P} + J_{z} \dot{R} - M_{z}\end{matrix}\right]$$

In [5]:

    

L_i = aircraft.linear_momentum(frame_i)
i_L_i = L_i.diff(t, frame_b) + frame_b.ang_acc_in(frame_i).cross(L_i)
(i_L_i - F_b).to_matrix(frame_b)


    

    
Out[5]:





$$\left[\begin{matrix}- m V \dot{R} + m W \dot{Q} + m \dot{U} - F_{x}\\m U \dot{R} - m W \dot{P} + m \dot{V} - F_{y}\\- m U \dot{Q} + m V \dot{P} + m \dot{W} - F_{z}\end{matrix}\right]$$

In [6]:

    

ast = pymoca.parser.parse('''
model Quad
    Real F_x, F_y, F_z;
    Real M_x, M_y, M_z;
    Real phi, theta, psi;
    Real P, Q, R;
    Real x, y, z;
    Real U, V, W;
    parameter Real J_x=1, J_y=1, J_z=1, m=1;
equation
    M_x = -P - phi;
    M_y = -Q - theta;
    M_z = -R - psi;
    F_x = -U -x;
    F_y = -V -y;
    F_z = -W -z;
    der(x) = U;
    der(y) = V;
    der(z) = W;
    -m*V*der(R) + m*W*der(Q) + m*der(U) = F_x;
    m*U*der(R) - m*W*der(P) + m*der(V) = F_y;
    -m*U*der(Q) + m*V*der(P) + m*der(W) = F_z;
    der(phi) = P + Q*sin(phi)*tan(theta) + R*cos(phi)*tan(theta);
    der(theta) = Q*cos(phi) - R*sin(phi);
    cos(theta)*der(psi) = Q*sin(phi) + R*cos(phi);
    J_x*der(P) = M_x;
    J_y*der(Q) = M_y;
    J_z*der(R) = M_z;
end Quad;
''')
#print(ast)


    

In [7]:

    

quad_src = generator.generate(ast, 'Quad')
exec(quad_src)
print(quad_src)


    

    




# do not edit, generated by pymoca

from __future__ import print_function, division
import sympy
import sympy.physics.mechanics as mech
from pymoca.backends.sympy.runtime import OdeModel
from sympy import sin, cos, tan


class Quad(OdeModel):

    def __init__(self):

        super(Quad, self).__init__()

        # states
        phi, theta, psi_, P, Q, R, x, y, z, U, V, W = mech.dynamicsymbols('phi, theta, psi_, P, Q, R, x, y, z, U, V, W')
        self.x = sympy.Matrix([phi, theta, psi_, P, Q, R, x, y, z, U, V, W])
        self.x0 = {
            phi : 0,
            theta : 0,
            psi_ : 0,
            P : 0,
            Q : 0,
            R : 0,
            x : 0,
            y : 0,
            z : 0,
            U : 0,
            V : 0,
            W : 0,
            }

        # variables
        F_x, F_y, F_z, M_x, M_y, M_z = mech.dynamicsymbols('F_x, F_y, F_z, M_x, M_y, M_z')
        self.v = sympy.Matrix([F_x, F_y, F_z, M_x, M_y, M_z])

        # constants
        self.c = sympy.Matrix([])
        self.c0 = {
            }

        # parameters
        J_x, J_y, J_z, m = sympy.symbols('J_x, J_y, J_z, m')
        self.p = sympy.Matrix([J_x, J_y, J_z, m])
        self.p0 = {
            J_x : 1.0,
            J_y : 1.0,
            J_z : 1.0,
            m : 1.0,
            }

        # inputs
        self.u = sympy.Matrix([])
        self.u0 = {
            }

        # outputs
        self.y = sympy.Matrix([])

        # equations
        self.eqs = [
            M_x - (- P - phi),
            M_y - (- Q - theta),
            M_z - (- R - psi_),
            F_x - (- U - x),
            F_y - (- V - y),
            F_z - (- W - z),
            (x).diff(self.t) - (U),
            (y).diff(self.t) - (V),
            (z).diff(self.t) - (W),
            - m * V * (R).diff(self.t) + m * W * (Q).diff(self.t) + m * (U).diff(self.t) - (F_x),
            m * U * (R).diff(self.t) - m * W * (P).diff(self.t) + m * (V).diff(self.t) - (F_y),
            - m * U * (Q).diff(self.t) + m * V * (P).diff(self.t) + m * (W).diff(self.t) - (F_z),
            (phi).diff(self.t) - (P + Q * sin(phi) * tan(theta) + R * cos(phi) * tan(theta)),
            (theta).diff(self.t) - (Q * cos(phi) - R * sin(phi)),
            cos(theta) * (psi_).diff(self.t) - (Q * sin(phi) + R * cos(phi)),
            J_x * (P).diff(self.t) - (M_x),
            J_y * (Q).diff(self.t) - (M_y),
            J_z * (R).diff(self.t) - (M_z),
            ]

        self.compute_fg()

In [8]:

    

exec(quad_src)
quad = Quad()


    

In [9]:

    

res = quad.simulate(x0 = [0,0,0, 1,2,3, 0, 0, 0, 1, 0, 0], tf=20)


    

In [10]:

    

plt.plot(res['t'], res['x']);
plt.grid()


    

In [11]:

    

v_x, v_y, v_z, x, y, z = sympy.physics.mechanics.dynamicsymbols('v_x, v_y, v_z, x, y, z')
m, g = sympy.symbols('m, g')
Lyap = aircraft.kinetic_energy(frame_i) + phi**2/2 + theta**2/2 + psi**2/2 + \
    x**2/2 + y**2/2 + z**2/2
Lyap


    

    
Out[11]:





$$\frac{J_{x} P^{2}}{2} + \frac{J_{y} Q^{2}}{2} + \frac{J_{z} R^{2}}{2} + \frac{m}{2} \left(U^{2} + V^{2} + W^{2}\right) + \frac{\phi^{2}}{2} + \frac{\psi^{2}}{2} + \frac{\theta^{2}}{2} + \frac{x^{2}}{2} + \frac{y^{2}}{2} + \frac{z^{2}}{2}$$

In [12]:

    

LyapDot = sympy.Matrix([Lyap]).jacobian(quad.x).dot(quad.f).expand().simplify()
LyapDot = LyapDot.expand().collect([P, Q, R, U, V, W], sympy.factor)
LyapDot


    

    
Out[12]:





$$\left(\phi \operatorname{sin}\left(\phi\right) \operatorname{tan}\left(\theta\right) + \theta \operatorname{cos}\left(\phi\right) - \theta\right) Q + \left(\phi \operatorname{cos}\left(\phi\right) \operatorname{tan}\left(\theta\right) - \psi_{} - \theta \operatorname{sin}\left(\phi\right)\right) R - P^{2} - Q^{2} - R^{2} - U^{2} - V^{2} - W^{2}$$