Double Integrator

Environment Definition

import logging

import numpy as np

from do_dpc.control_utils.lti_systems import create_1D_double_integrator

# Removes the INFO logging
logging.getLogger().setLevel(logging.WARNING)

noise_config = {
    "meas_noise_std": np.array([0.01]),
    "process_noise_std": np.array([0.01, 0.005]),
    "meas_noise_seed": 6423,
    "process_noise_seed": 6211,
}

env = create_1D_double_integrator(**noise_config)

Training Data Collection

from do_dpc.control_utils.trajectory_collector import collect_trajectory_data
from do_dpc.control_utils.pid_profiles import ONE_D_DOUBLE_INT_PID_COMBO
from do_dpc.utils.plots import plot_training_data

n_samples = 1000

pid_combo = ONE_D_DOUBLE_INT_PID_COMBO

_, m, p = env.get_dims()
training_data = collect_trajectory_data(env, m, p, pid_combo)

plot_training_data(training_data)

Collecting Training Data: 100%|██████████| 1000/1000 [00:00<00:00, 10039.24it/s]

Online DPC Application

from tqdm import tqdm
from do_dpc.control_utils.trajectory_collector import TrajectoryCollector
from do_dpc.control_utils.control_structs import Bounds
from do_dpc.dpc.tpc import TPC
from do_dpc.dpc.dpc_structs import DPCParameters

n_sim = 10
traj_col = TrajectoryCollector(m, p, n_sim)
dpc_params = DPCParameters(Q=np.diag([100, 1]), R=0.001 * np.eye(1), tau_p=3, tau_f=3)

ctrl_inst = TPC(dpc_params, training_data)

input_bounds = Bounds(max_values=np.array([10]), min_values=np.array([-20]))
ctrl_inst.add_input_constraints(input_bounds)
ctrl_inst.build_optimization_problem()

y_r = np.array([7, 1])
u_r = np.zeros((1,))
ctrl_inst.update_tracking_reference(y_r, u_r)

u_next, y_current, y_prev = np.zeros((m,)), env.step(np.zeros((m,))), env.step(np.zeros((m,)))

for _ in tqdm(range(n_sim), desc="Running Simulation", ncols=80):
    traj_col.store_measurements(y_prev, u_next)
    z_p_current = np.concatenate((y_prev, u_next))
    ctrl_inst.update_past_measurements(z_p_current)
    ctrl_inst.solve()
    u_next = ctrl_inst.get_next_control_action()
    y_prev, y_current = y_current, env.step(u_next)

sim_data = traj_col.get_trajectory_data()
print("Running of the DPC algorithm complete")

Running Simulation: 100%|██████████████████████| 10/10 [00:00<00:00, 145.18it/s]

Running of the DDPC algorithm complete

Analyse and Data Visualization

u_sim = sim_data.u
within_bounds = np.all((u_sim >= input_bounds.min_values) & (u_sim <= input_bounds.max_values))

if within_bounds:
    print("All values in u_sim are within the specified bounds.")
else:
    print("Some values in u_sim are out of the specified bounds.")

plot_training_data(sim_data)

All values in u_sim are within the specified bounds.