In [18]:

    

import sympy
import sympy.physics.mechanics as mech
import copy
import os
import scipy.integrate
import collections
import matplotlib.pyplot as plt
import sympy_utils
import numpy as np
%matplotlib inline
sympy.init_printing()
sympy.physics.vector.init_vprinting()


    

In [19]:

    

x, y, z, phi, theta, psi, t, J_x, J_y, J_z, Jxz, L, D, Y, M, \
    alpha, beta, V, h, g, P, Q, R, l, m, n = \
    sympy.symbols('x, y, z, phi, theta, psi, t, J_x, J_y, J_z, Jxz, L, D, Y, M, alpha, beta, V, h, g, P, Q, R, l, m, n')
frame_i = mech.ReferenceFrame('i')
frame_b = frame_i.orientnew('b', 'Body', (phi(t), theta(t), psi(t)), '321')
frame_b2 = copy.copy(frame_b)
frame_b.set_ang_vel(frame_i, P(t)*frame_b.x + Q(t)*frame_b.y + R(t)*frame_b.z)
frame_w = frame_b.orientnew('w', 'Body', (beta(t), -alpha(t), 0), '321')
point_cm = mech.Point('cm')
point_cm.set_vel(frame_b, 0)
point_cm.set_vel(frame_i, V(t)*frame_w.x)


    

In [20]:

    

w_ib = frame_i.orientnew('b', 'Body', (phi(t), theta(t), psi(t)), '321').ang_vel_in(frame_i)
w_ib


    

    
Out[20]:





$$(- \operatorname{sin}\left(\theta\right) \dot{\phi} + \dot{\psi})\mathbf{\hat{b}_x} + (\operatorname{sin}\left(\psi\right) \operatorname{cos}\left(\theta\right) \dot{\phi} + \operatorname{cos}\left(\psi\right) \dot{\theta})\mathbf{\hat{b}_y} + (- \operatorname{sin}\left(\psi\right) \dot{\theta} + \operatorname{cos}\left(\psi\right) \operatorname{cos}\left(\theta\right) \dot{\phi})\mathbf{\hat{b}_z}$$

In [21]:

    

P_eq = w_ib.dot(frame_b.x).simplify()
Q_eq = w_ib.dot(frame_b.y).simplify()
R_eq = w_ib.dot(frame_b.z).simplify()


    

In [22]:

    

euler_sol = sympy.solve([P(t) - P_eq, Q(t) - Q_eq, R(t) - R_eq],
                        [phi(t).diff(t), theta(t).diff(t), psi(t).diff(t)])
euler_sol


    

    
Out[22]:





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

In [23]:

    

aircraft = mech.RigidBody('aircraft', point_cm, frame_b, M, (mech.inertia(frame_b, J_x, J_y, J_z, 0, 0, Jxz), point_cm))
aircraft.set_potential_energy(m*g*h(t))


    

In [24]:

    

F_A = -D(t)*frame_b.x -Y(t)*frame_b.y -L(t)*frame_b.z
M_A = l(t)*frame_b.x + m(t)*frame_b.y + n(t)*frame_b.z


    

In [25]:

    

F_T = F_A + M*g*frame_i.z
M_T = M_A


    

In [26]:

    

H_cm_i = aircraft.angular_momentum(point_cm, frame_i)
di_H_cm_i = H_cm_i.diff(t, frame_b) + frame_b.ang_vel_in(frame_i).cross(H_cm_i)
eom_rot = di_H_cm_i - M_T
eom_rot


    

    
Out[26]:





$$(J_{x} \dot{P} - J_{y} Q R + Jxz \dot{R} + \left(J_{z} R + Jxz P\right) Q - l)\mathbf{\hat{b}_x} + (J_{y} \dot{Q} + \left(J_{x} P + Jxz R\right) R - \left(J_{z} R + Jxz P\right) P - m)\mathbf{\hat{b}_y} + (J_{y} P Q + J_{z} \dot{R} + Jxz \dot{P} - \left(J_{x} P + Jxz R\right) Q - n)\mathbf{\hat{b}_z}$$

In [27]:

    

rot_sol = sympy.solve([eom_rot.dot(frame_b.x) - P(t).diff(t),
             eom_rot.dot(frame_b.y) - Q(t).diff(t),
             eom_rot.dot(frame_b.z) - R(t).diff(t)], [P(t).diff(t), Q(t).diff(t), R(t).diff(t)])
rot_sol


    

    
Out[27]:





$$\begin{Bmatrix}\dot{P} : \frac{1}{Jxz^{2} - \left(J_{x} - 1\right) \left(J_{z} - 1\right)} \left(Jxz \left(J_{x} P Q - J_{y} P Q + Jxz Q R + n\right) - \left(J_{z} - 1\right) \left(J_{y} Q R - J_{z} Q R - Jxz P Q + l\right)\right), & \dot{Q} : \frac{1}{J_{y} - 1} \left(- J_{x} P R + J_{z} P R + Jxz P^{2} - Jxz R^{2} + m\right), & \dot{R} : \frac{1}{Jxz^{2} - \left(J_{x} - 1\right) \left(J_{z} - 1\right)} \left(Jxz \left(J_{y} Q R - J_{z} Q R - Jxz P Q + l\right) - \left(J_{x} - 1\right) \left(J_{x} P Q - J_{y} P Q + Jxz Q R + n\right)\right)\end{Bmatrix}$$

In [28]:

    

L_i = aircraft.linear_momentum(frame_i)
di_L_i = L_i.diff(t, frame_w) + frame_w.ang_vel_in(frame_i).cross(L_i)
eom_trans = di_L_i - F_T
eom_trans


    

    
Out[28]:





$$M \dot{V}\mathbf{\hat{w}_x} + M \left(- P \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right) - Q \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) + R \operatorname{cos}\left(\alpha\right) + \operatorname{cos}\left(\alpha\right) \dot{\beta}\right) V\mathbf{\hat{w}_y} -  M \left(- P \operatorname{sin}\left(\beta\right) + Q \operatorname{cos}\left(\beta\right) - \dot{\alpha}\right) V\mathbf{\hat{w}_z} + D\mathbf{\hat{b}_x} + Y\mathbf{\hat{b}_y} + L\mathbf{\hat{b}_z} -  M g\mathbf{\hat{i}_z}$$

In [29]:

    

Lagrangian = mech.Lagrangian(frame_i, aircraft)
Lagrangian


    

    
Out[29]:





$$\frac{J_{y} Q^{2}}{2} + \frac{M V^{2}}{2} - g m h + \left(\frac{J_{x} P}{2} + \frac{Jxz R}{2}\right) P + \left(\frac{J_{z} R}{2} + \frac{Jxz P}{2}\right) R$$

In [30]:

    

v_cm_i = point_cm.vel(frame_i).express(frame_i)
v_cm_i.dot(frame_i.x)


    

    
Out[30]:





$$\left(\left(\operatorname{sin}\left(\phi\right) \operatorname{sin}\left(\psi\right) + \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\psi\right)\right) \operatorname{sin}\left(\alpha\right) + \left(- \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\psi\right) + \operatorname{sin}\left(\psi\right) \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\phi\right)\right) \operatorname{sin}\left(\beta\right) \operatorname{cos}\left(\alpha\right) + \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right)\right) V$$

In [31]:

    

x_vect = sympy.Matrix([P, Q, R, phi, theta, psi, V, alpha, beta, x, y, z])
u_vect = sympy.Matrix([L, D, Y, l, m, n])

State = collections.namedtuple('State', ['phi', 'theta', 'psi', 'P', 'Q', 'R', 'V', 'alpha', 'beta', 'x', 'y', 'z'])
Input = collections.namedtuple('Input', ['L', 'D', 'Y', 'l', 'm', 'n'])

remove_t_sub = {var[0](t):var[0] for var in x_vect.tolist() + u_vect.tolist()}

rhs = sympy.Matrix([
    rot_sol[P(t).diff(t)],
    rot_sol[Q(t).diff(t)],
    rot_sol[R(t).diff(t)],
    euler_sol[phi(t).diff(t)],
    euler_sol[theta(t).diff(t)],
    euler_sol[psi(t).diff(t)],
    sympy.solve(eom_trans.dot(frame_w.x), V(t).diff(t))[0],
    sympy.solve(eom_trans.dot(frame_w.z), alpha(t).diff(t))[0],
    sympy.solve(eom_trans.dot(frame_w.y), beta(t).diff(t))[0],
    v_cm_i.dot(frame_i.x),
    v_cm_i.dot(frame_i.y),
    v_cm_i.dot(frame_i.z),
    ]).subs(remove_t_sub)


    

In [45]:

    

print rhs


    

    




Matrix([[(Jxz*(J_x*P*Q - J_y*P*Q + Jxz*Q*R + n) - (J_z - 1)*(J_y*Q*R - J_z*Q*R - Jxz*P*Q + l))/(Jxz**2 - (J_x - 1)*(J_z - 1))], [(-J_x*P*R + J_z*P*R + Jxz*P**2 - Jxz*R**2 + m)/(J_y - 1)], [(Jxz*(J_y*Q*R - J_z*Q*R - Jxz*P*Q + l) - (J_x - 1)*(J_x*P*Q - J_y*P*Q + Jxz*Q*R + n))/(Jxz**2 - (J_x - 1)*(J_z - 1))], [(Q*sin(psi) + R*cos(psi))/cos(theta)], [Q*cos(psi) - R*sin(psi)], [P + Q*sin(psi)*tan(theta) + R*cos(psi)*tan(theta)], [(-D*cos(alpha)*cos(beta) - L*sin(alpha) + M*g*(sin(alpha)*cos(psi)*cos(theta) + sin(beta)*sin(psi)*cos(alpha)*cos(theta) - sin(theta)*cos(alpha)*cos(beta)) - Y*sin(beta)*cos(alpha))/M], [(D*sin(alpha)*cos(beta) - L*cos(alpha) + M*V*(-P*sin(beta) + Q*cos(beta)) + M*g*(-sin(alpha)*sin(beta)*sin(psi)*cos(theta) + sin(alpha)*sin(theta)*cos(beta) + cos(alpha)*cos(psi)*cos(theta)) + Y*sin(alpha)*sin(beta))/(M*V)], [(D*sin(beta) - M*R*V*cos(alpha) + M*V*(P*cos(beta) + Q*sin(beta))*sin(alpha) + M*g*(sin(beta)*sin(theta) + sin(psi)*cos(beta)*cos(theta)) - Y*cos(beta))/(M*V*cos(alpha))], [V*((sin(phi)*sin(psi) + sin(theta)*cos(phi)*cos(psi))*sin(alpha) + (-sin(phi)*cos(psi) + sin(psi)*sin(theta)*cos(phi))*sin(beta)*cos(alpha) + cos(alpha)*cos(beta)*cos(phi)*cos(theta))], [V*((sin(phi)*sin(psi)*sin(theta) + cos(phi)*cos(psi))*sin(beta)*cos(alpha) + (sin(phi)*sin(theta)*cos(psi) - sin(psi)*cos(phi))*sin(alpha) + sin(phi)*cos(alpha)*cos(beta)*cos(theta))], [V*(sin(alpha)*cos(psi)*cos(theta) + sin(beta)*sin(psi)*cos(alpha)*cos(theta) - sin(theta)*cos(alpha)*cos(beta))]])

In [56]:

    

const = {J_x:0.1, J_y:0.1, J_z:0.1, Jxz:0.1, M:1, g:9.8}
x0 = [0,0,0,0,0,0,10,0,0,0,0,0]
u0 = [0,0,0,0,0,0]


    

In [92]:

    

sympy_utils.save_sympy_expr(
    {'rhs': rhs, 't': t, 'x_vect': x_vect, 'u_vect': u_vect,
     'constants': const},
    os.path.join('save', 'wind_kinematics.sympy'))


    

In [93]:

    

wind_kinematics = sympy_utils.load_sympy_expr(
    os.path.join('save', 'wind_kinematics.sympy'))


    

In [94]:

    

f_sim, f_jac = sympy_utils.rhs_to_scipy_ode(**wind_kinematics)


    

In [95]:

    

%%timeit
f_sim(1, x0, u0)


    

    




10000 loops, best of 3: 21.4 µs per loop

In [96]:

    

ode = scipy.integrate.ode(f_sim, f_jac)
ode.set_initial_value(x0)

dt = 0.01
Data = collections.namedtuple('Data', ['t', 'x'])

data = Data([], [])
u = u0

while ode.t < 10:
    data.x.append(ode.y)
    data.t.append(ode.t)
    state = State(*ode.y)
    state.P
    u = [9.7,1,1,1,1,10]
    ode.set_f_params(u)
    ode.set_jac_params(u)
    ode.integrate(ode.t + dt)

hist = State(*np.array(data.x).T)


    

In [97]:

    

plt.plot(data.t, np.array([hist.x, hist.y, hist.z]).T);