# x_history[i, :] = xref
# xref_history[i, :] = xref
# goal_history[i, :] = goal
# u_history[i, :] = uref
# Give back real control
planning = False
kp = np.diag([240, 240, 2000])
kd = np.diag([240, 240, 0])
thrust_max = np.array([300, 300, 300, 300])
# Track plan
for i, t in enumerate(t_arr):
# Planner's decision
xref = planner.get_state(t)
uref = planner.get_effort(t)
# Controller's decision
u = lqr(x, uref)[1].dot(erf(xref, np.copy(x)))
# Record this instant
x_history[i, :] = x
xref_history[i, :] = xref
goal_history[i, :] = goal
u_history[i, :] = u
# Step dynamics
x = dynamics(x, u, dt)
################################################# VISUALIZATION
dt = 0.03 # s
T = planner.T # s
t_arr = np.arange(0, T, dt)
framerate = 10
# Preallocate results memory
x = np.copy(x0)
x_history = np.zeros((len(t_arr), nstates))
goal_history = np.zeros((len(t_arr), nstates))
u_history = np.zeros((len(t_arr), ncontrols))
# Interpolate plan
for i, t in enumerate(t_arr):
# Planner's decision
x = planner.get_state(t)
u = planner.get_effort(t)
# Record this instant
x_history[i, :] = x
goal_history[i, :] = goal
u_history[i, :] = u
################################################# VISUALIZATION
print("\n...plotting...")
from matplotlib import pyplot as plt
import matplotlib.animation as ani
# Plot results
fig1 = plt.figure()
