MPC Drone

This is a custom implementation of MPC control. This might get implemented into jdrones in the future.

[1] M. Islam, M. Okasha, and M. M. Idres, ‘Dynamics and control of quadcopter using linear model predictive control approach’, IOP Conf. Ser.: Mater. Sci. Eng., vol. 270, p. 012007, Dec. 2017, doi: 10.1088/1757-899X/270/1/012007.

[2] M. Ireland and D. Anderson, ‘Development of Navigation Algorithms for Nap-of-the-Earth UAV Flight in a Constrained Urban Environment’, presented at the ICAS 2012 - 28th Congress of the International Council of the Aeronautical Sciences, Sep. 2012. Accessed: Feb. 07, 2023. [Online]. Available: http://www.icas.org/ICAS_ARCHIVE/ICAS2012/PAPERS/184.PDF

[3] M. L. Ireland and D. Anderson, ‘Optimisation of Trajectories for Wireless Power Transmission to a Quadrotor Aerial Robot’, J Intell Robot Syst, vol. 95, no. 2, pp. 567–584, Aug. 2019, doi: 10.1007/s10846-018-0824-6.

[4] Wang, P. et al. (2016) ‘Dynamics modelling and linear control of quadcopter’, in 2016 International Conference on Advanced Mechatronic Systems (ICAMechS). 2016 International Conference on Advanced Mechatronic Systems (ICAMechS), pp. 498–503. Available at: https://doi.org/10.1109/ICAMechS.2016.7813499.

%matplotlib notebook
from notebook_quick_setup import *

Beginning notebook setup...
        Added /home/jhewers/Documents/projects/jdrones/src to path
        Imported gymnasium version 0.27.1
        Imported jdrones version unknown

pybullet build time: Feb  2 2023 13:13:41

        Imported scipy==1.7.3, numpy==1.22.4, pandas==1.3.5
        Imported functools, collections and itertools
        Imported tqdm (standard and trange)
        Imported seaborn==0.11.2, matplotlib==3.5.1
End of notebook setup

T = 3
dt = 1/50
mpc_dt = 1/5
M = 1  # Control horizon
P = 5  # Prediction horizon

initial_state = State()
initial_state.pos = (0,0,-0.1)

def reshape(u):
    return np.reshape(u,(-1,4))

def prediction(u, x, dt=mpc_dt):
    env = gymnasium.make("LinearDynamicModelDroneEnv-v0", dt=dt, initial_state=x)
    y = collections.deque()
    obs, _ = env.reset()
    assert np.allclose(obs.pos, x.pos)
    assert np.allclose(obs.vel, x.vel)
    assert np.allclose(obs.rpy, x.rpy)
    assert np.allclose(obs.ang_vel, x.ang_vel)
    for ui in u:
        obs, *_ = env.step(ui)
        y.append(np.copy(obs))
    return np.array(y)

%timeit prediction(reshape(np.ones((4,M+P))),State())

4.1 ms ± 117 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)

Q = State(np.ones(20))
Q.quat = Q.prop_omega = (0, 0, 0, 0)
Q.pos = (10, 10, 10)
Q.rpy = (1000, 1000, 1000)
print(Q)


def cost_fn(y):
    return np.sum((Q * y) ** 2)


def objective(u, x, _P=P, _mpc_dt=mpc_dt):
    """Calculate the sum of the square error for the control problem"""
    y = prediction(reshape(u), x, dt=_mpc_dt)
    return cost_fn(y)

[  10.   10.   10.    0.    0.    0.    0. 1000. 1000. 1000.    1.    1.
    1.    1.    1.    1.    0.    0.    0.    0.]

env = gymnasium.wrappers.TimeLimit(
    gymnasium.make(
        "NonLinearDynamicModelDroneEnv-v0", dt=dt, initial_state=initial_state
    ),
    max_episode_steps=int(T / dt),
)
obs, _ = env.reset()

observations = collections.deque()
cost = collections.deque()
controls = collections.deque()

for i in trange(int(T / dt) - 1):
    # Control
    res = scipy.optimize.minimize(
        functools.partial(objective, x=obs), np.zeros((P + M, 4))
    )
    uopt = reshape(res.x)[0]
    control_action = uopt.squeeze()
    # Store
    cost.append((cost_fn(obs), res.fun))
    observations.append(np.copy(obs))
    controls.append(control_action)
    # Step
    u = np.sqrt(np.clip(control_action, 0, np.inf))
    obs, _, term, trunc, info = env.step(u)
    # Termination
    if trunc or term:
        break

/home/jhewers/.local/lib/python3.10/site-packages/gymnasium/utils/passive_env_checker.py:181: UserWarning: WARN: The obs returned by the `reset()` method is not within the observation space.
  logger.warn(f"{pre} is not within the observation space.")

/home/jhewers/.local/lib/python3.10/site-packages/gymnasium/utils/passive_env_checker.py:181: UserWarning: WARN: The obs returned by the `step()` method is not within the observation space.
  logger.warn(f"{pre} is not within the observation space.")

N = 1000
df = States(observations).to_df(tag="States", N=N, dt=dt)
df_meta = Conversions.iter_to_df(
    np.array(cost), tag="Cost", dt=dt, N=N, cols=["state", "optim"]
)
df_ctrl = Conversions.iter_to_df(
    np.array(controls), tag="Control", dt=dt, N=N, cols=["roll", "pitch", "yaw", "T"]
)

fig, ax = plt.subplots(1, 1)
sns.lineplot(data=df_ctrl, x="t", y="value", hue="variable", ax=ax)
plt.show()

fig, ax = plt.subplots(2, 2, figsize=(10, 8))
ax = ax.flatten()

sns.lineplot(
    data=df.query("variable in ('x','y','z')"),
    x="t",
    y="value",
    hue="variable",
    ax=ax[0],
)
ax[0].legend()

sns.lineplot(
    data=df.query("variable in ('phi','theta','psi')"),
    x="t",
    y="value",
    hue="variable",
    ax=ax[1],
)
ax[1].legend()

sns.lineplot(
    data=df.query("variable in ('vx','vy','vz')"),
    x="t",
    y="value",
    hue="variable",
    ax=ax[2],
)
ax[2].legend()

sns.lineplot(
    data=df.query("variable in ('P0','P1','P2','P3')"),
    x="t",
    y="value",
    hue="variable",
    ax=ax[3],
)
ax[3].legend()

fig.tight_layout()

fig, ax = plt.subplots(1, 1)
sns.lineplot(data=df_meta, x="t", y="value", hue="variable", ax=ax)
plt.show()