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()
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)
w_ib = frame_i.orientnew('b', 'Body', (phi(t), theta(t), psi(t)), '321').ang_vel_in(frame_i)
w_ib
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()
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
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))
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
F_T = F_A + M*g*frame_i.z
M_T = M_A
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
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
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
Lagrangian = mech.Lagrangian(frame_i, aircraft)
Lagrangian
v_cm_i = point_cm.vel(frame_i).express(frame_i)
v_cm_i.dot(frame_i.x)
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)
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))]])
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]
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'))
wind_kinematics = sympy_utils.load_sympy_expr(
os.path.join('save', 'wind_kinematics.sympy'))
f_sim, f_jac = sympy_utils.rhs_to_scipy_ode(**wind_kinematics)
%%timeit
f_sim(1, x0, u0)
10000 loops, best of 3: 21.4 µs per loop
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)
plt.plot(data.t, np.array([hist.x, hist.y, hist.z]).T);