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,