import numpy as np
import params
from dynamics import Dynamics
from quadrotor_viewer import QuadRotor_Viewer
from scipy.spatial.transform import Rotation
from mpc import MPC, NMPC, LNMPC
from data_viewer import data_viewer
import time
if __name__ == "__main__":
dynamics = Dynamics(params.dt)
# viewer = QuadRotor_Viewer()
data_view = data_viewer()
A, B = dynamics.get_SS(dynamics.state)
#LNMPC doesn't seem to work any better
# controller = MPC(A, B, params.u_max, params.u_min, T=params.T) #Typicall MPC
controller = LNMPC(
A, B, params.u_max, params.u_min, T=params.T
) #Non-linear model predictive control relinearizing about the current state
# controller = NMPC(params.nu_max, params.nu_min, T = params.T) #Way to slow
t0 = params.t0
F_eq = params.mass * 9.81
T_eq = 0.0
u_eq = np.array([F_eq, T_eq, T_eq, T_eq])
xr = np.array([
5.0, -5.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0,
np.deg2rad(0), 0.0, 0.0, 0.0
])
