P = np.eye(4) * 1
# Instantiate controller
ctl = MPC(model=pendulum,
dynamics=pendulum.discrete_time_dynamics,
horizon=7,
Q=Q,
R=R,
P=P,
ulb=-5,
uub=5,
xlb=[-2, -10, -np.pi / 2, -np.pi / 2],
xub=[12, 10, np.pi / 2, np.pi / 2])
# Solve without disturbance
ctl.set_reference(x_sp=np.array([10, 0, 0, 0]))
sim_env_full = EmbeddedSimEnvironment(
model=pendulum,
dynamics=pendulum.pendulum_linear_dynamics_with_disturbance,
controller=ctl.mpc_controller,
time=6)
sim_env_full.run([0, 0, 0, 0])
# Solve witho disturbance
pendulum.enable_disturbance(w=0.05)
ctl.set_reference(x_sp=np.array([10, 0, 0, 0]))
sim_env_full_dist = EmbeddedSimEnvironment(
model=pendulum,
dynamics=pendulum.discrete_time_dynamics,
controller=ctl.mpc_controller,
time=10)
R=R,
P=P,
horizon=0.5,
ulb=[-max_fz, -max_tau, -max_tau, -tau_z],
uub=[max_fz, max_tau, max_tau, tau_z],
xlb=[
-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.pi / 2,
-np.pi / 2, -np.pi / 2, -np.inf, -np.inf, -np.inf
],
xub=[
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.pi / 2, np.pi / 2,
np.pi / 2, np.inf, np.inf, np.inf
],
integrator=False)
ctl.set_reference(x_sp=np.array([0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Q2-1
ctl.set_constant_control([0, 0, 0, 0])
sim_env = EmbeddedSimEnvironment(
model=Quadcopter_orginal,
dynamics=Quadcopter_orginal.discrete_time_dynamics,
controller=ctl.constant_control,
time=15)
#t, y, u = sim_env.run(x0 = [0,0,0,0,0,0,0,0,0,0,0,0])
# Q2-2
ctl.set_constant_control([0.1, 0, 0, 0])
sim_env = EmbeddedSimEnvironment(
model=Quadcopter_orginal,
dynamics=Quadcopter_orginal.discrete_time_dynamics,