diff --git a/.gitignore b/.gitignore index 2f2f72a1..38738813 100755 --- a/.gitignore +++ b/.gitignore @@ -102,6 +102,9 @@ cpp.sublime-workspace .vs/ .vscode/ .cache/ +.idea/ +**/CMakeCache.txt +CMakeFiles/** # whitelist documentation !docs/** diff --git a/CMakeLists.txt b/CMakeLists.txt index 999c81dc..bf806fd8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,7 @@ find_package(Doxygen COMPONENTS dot message(STATUS "CMAKE_PREFIX_PATH = ${CMAKE_PREFIX_PATH}") include(HDF5) include(Armadillo) +include(OSQP) if (LDSCTRLEST_BUILD_FIT AND LDSCTRLEST_BUILD_STATIC) # for mex compat. diff --git a/cmake/Modules/OSQP.cmake b/cmake/Modules/OSQP.cmake new file mode 100644 index 00000000..c04e0cf8 --- /dev/null +++ b/cmake/Modules/OSQP.cmake @@ -0,0 +1,19 @@ +include(FetchContent) +message(STATUS "Fetching & installing osqp") +FetchContent_Declare( + osqp + PREFIX ${CMAKE_BINARY_DIR}/osqp + GIT_REPOSITORY https://github.com/osqp/osqp.git + GIT_TAG 4e81a9d0 +) +FetchContent_MakeAvailable(osqp) +message(STATUS "Installed osqp to ${osqp_BINARY_DIR}") +list(APPEND CMAKE_PREFIX_PATH ${osqp_BINARY_DIR}) + + +if(TARGET osqp AND NOT TARGET osqp::osqp) + add_library(osqp::osqp ALIAS osqp) +endif() + +# find_package(osqp REQUIRED) +list(APPEND PROJECT_REQUIRED_LIBRARIES_ABSOLUTE_NAME osqp::osqp) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 6a2da8e0..bdc54f13 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -18,5 +18,5 @@ foreach(file ${files}) # FAIL_REGULAR_EXPRESSION "(Exception|Test failed)") set_tests_properties(${file_without_ext} PROPERTIES - TIMEOUT 20) + TIMEOUT 40) endforeach() diff --git a/examples/eg_glds_mpc.cpp b/examples/eg_glds_mpc.cpp new file mode 100644 index 00000000..53ebf22e --- /dev/null +++ b/examples/eg_glds_mpc.cpp @@ -0,0 +1,232 @@ +//===-- eg_glds_mpc.cpp - Example GMPC Control ---------------------------===// +// +// Copyright 2025 Chia-Chien Hung, Kyle Johnsen +// Copyright 2025 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// \brief Example GMPC Control +/// +/// \example eg_glds_mpc.cpp +//===----------------------------------------------------------------------===// + +#include + +using lds::Matrix; +using lds::Vector; +using lds::data_t; +using std::cout; + +auto main() -> int { + cout << " ********** Example Gaussian LDS Control ********** \n\n"; + + // Make 1st-order SISO system, sampled at 1kHz + data_t dt = 1e-3; + size_t n_u = 1; + size_t n_x = 1; + size_t n_y = 1; + + data_t t_sim = 0.05; // time for each control step + size_t n_sim = static_cast(t_sim / dt); // number of time steps per control step + auto n_t = static_cast(5.0 / dt); // number of time steps for simulation + auto n_c = static_cast(5.0 / t_sim); // number of control steps for simulation + + // construct ground truth system to be controlled... + // initializes to random walk model with top-most n_y state observed + lds::gaussian::System controlled_system(n_u, n_x, n_y, dt); + + // Ground-truth parameters for the controlled system + // (stand-in for physical system to be controlled) + Matrix a_true(n_x, n_x, arma::fill::eye); + a_true[0] = exp(-dt / 0.01); + Matrix b_true = Matrix(n_x, n_u).fill(2e-4); + // control signal to model input unit conversion e.g., V -> mW/mm2: + Vector g_true = Vector(n_y).fill(10.0); + + // output noise covariance + Matrix r_true = Matrix(n_y, n_y, arma::fill::eye) * 1e-4; + + // Assign params. + controlled_system.set_A(a_true); + controlled_system.set_B(b_true); + controlled_system.set_g(g_true); + controlled_system.set_R(r_true); + + cout << ".....................................\n"; + cout << "controlled_system:\n"; + cout << ".....................................\n"; + controlled_system.Print(); + cout << ".....................................\n"; + + // make a controller + lds::gaussian::MpcController controller; + const size_t N = 25; // Prediction horizon + const size_t M = 20; // Control horizon + { + // for this demo, just use arbitrary default R + Matrix r_controller = Matrix(n_y, n_y, arma::fill::eye) * lds::kDefaultR0; + + lds::gaussian::System controller_system(controlled_system); + controller_system.set_R(r_controller); + controller_system.Reset(); // reset to new m + + // going to adaptively re-estimate the disturbance + controller_system.do_adapt_m = true; + + Matrix C = Matrix(n_y, n_x, arma::fill::eye); + Matrix Q_y = C.t() * C * 1e5; + Matrix R = Matrix(n_u, n_u, arma::fill::eye) * 1e-1; // using dense instead of sparse matrix + Matrix S = Matrix(n_u, n_u, arma::fill::zeros); // using dense instead of sparse matrix + + Vector umin = {0}; + Vector umax = {5}; + if (n_u == 2) { + umin = {0, 0}; + umax = {5, 5}; + } else if (n_u == 3) { + umin = {0, 0, 0}; + umax = {5, 5, 5}; + } + + Vector xmin(b_true.n_rows); + xmin.fill(-arma::datum::inf); + Vector xmax(b_true.n_rows); + xmax.fill(arma::datum::inf); + + controller = + std::move(lds::gaussian::MpcController(std::move(controller_system))); + controller.set_cost_output(Q_y, R, S, N, M); + controller.set_constraint(xmin, xmax, umin, umax); + } + + // Control variables: + // Reference/target output, controller gains + Vector y_ref0 = Vector(n_y).fill(20.0 * dt); + + // setting initial state to target to avoid error at onset: + Vector x0 = Vector(n_x).fill(y_ref0[0]); + + cout << ".....................................\n"; + cout << "control system:\n"; + cout << ".....................................\n"; + controller.Print(); + cout << ".....................................\n"; + + // set up variables for simulation + // create Matrix to save outputs in... + Matrix y_ref = Matrix(n_y, n_t + (N * n_sim), arma::fill::ones) * y_ref0[0]; + + // Simulated measurements + Matrix z(n_y, n_t, arma::fill::zeros); + + // simulated control signal ([=] V) + Matrix u(n_u, n_t, arma::fill::zeros); + + // outputs, states and gain/disturbance params + // *_hat indicates online estimates + Matrix y_hat(n_y, n_t, arma::fill::zeros); + Matrix x_hat(n_x, n_t, arma::fill::zeros); + + // *_true indicates ground truth (system being controlled) + Matrix y_true(n_y, n_t, arma::fill::zeros); + Matrix x_true(n_x, n_t, arma::fill::zeros); + + // MPC cost + Matrix J(1, n_t, arma::fill::zeros); + + // set initial val + y_hat.submat(0, 0, n_y - 1, 0) = controller.sys().y(); + y_true.submat(0, 0, n_y - 1, 0) = controlled_system.y(); + + x_hat.submat(0, 0, n_x - 1, 0) = controller.sys().x(); + x_true.submat(0, 0, n_x - 1, 0) = controlled_system.x(); + + cout << "Starting " << n_t * dt << " sec simulation ... \n"; + auto start = std::chrono::high_resolution_clock::now(); + + // Simulate the true system. + for (int i = 0; i < n_sim; i++) { + // input + Vector u_tm1(u.colptr(i), u.n_rows, false, true); + + z.col(i) = controlled_system.Simulate(u_tm1); + + // save the signals + y_true.col(i) = controlled_system.y(); + x_true.col(i) = controlled_system.x(); + + y_hat.col(i) = controller.sys().y(); + x_hat.col(i) = controller.sys().x(); + } + + for (size_t s = 1; s < n_c; s++) { + // Calculate the slice indices + size_t start_idx = s * n_sim; + size_t end_idx = ((s + N) * n_sim) - 1; + + // This method uses a steady-state solution to control problem to calculate + // x_ref, u_ref from reference output y_ref. Therefore, it is only + // applicable to regulation problems or cases where reference trajectory + // changes slowly compared to system dynamics. + auto* j = new data_t; + auto u_next = controller.ControlOutputReference(t_sim, z.col(start_idx - 1), y_ref.cols(start_idx, end_idx), true, j); + for (int t = 0; t < n_sim; t++) { + u.col(start_idx + t) = u_next; + J.col(start_idx + t) = *j; + } + + // Simulate the true system. + for (int i = 0; i < n_sim; i++) { + int t = start_idx + i; + + // input + Vector u_tm1(u.colptr(t), u.n_rows, false, true); + + z.col(t) = controlled_system.Simulate(u_tm1); + + // save the signals + y_true.col(t) = controlled_system.y(); + x_true.col(t) = controlled_system.x(); + + y_hat.col(t) = controller.sys().y(); + x_hat.col(t) = controller.sys().x(); + } + } + + auto finish = std::chrono::high_resolution_clock::now(); + std::chrono::duration sim_time_ms = finish - start; + cout << "Finished simulation in " << sim_time_ms.count() << " ms.\n"; + cout << "(app. " << (sim_time_ms.count() / n_t) * 1e3 << " us/time-step)\n"; + + cout << "Saving simulation data to disk.\n"; + + // saved variables: dt, lambdaHat, xHat, mHat, z, u, lambdaRef, lambdaTrue, + // xTrue, mTrue saving with hdf5 via armadillo + arma::hdf5_opts::opts replace = arma::hdf5_opts::replace; + + auto dt_vec = Vector(1).fill(dt); + dt_vec.save(arma::hdf5_name("eg_glds_mpc.h5", "dt")); + Matrix y_ref_clip = y_ref.cols(0, n_t - 1); + y_ref_clip.save(arma::hdf5_name("eg_glds_mpc.h5", "y_ref", replace)); + u.save(arma::hdf5_name("eg_glds_mpc.h5", "u", replace)); + z.save(arma::hdf5_name("eg_glds_mpc.h5", "z", replace)); + x_true.save(arma::hdf5_name("eg_glds_mpc.h5", "x_true", replace)); + y_true.save(arma::hdf5_name("eg_glds_mpc.h5", "y_true", replace)); + x_hat.save(arma::hdf5_name("eg_glds_mpc.h5", "x_hat", replace)); + y_hat.save(arma::hdf5_name("eg_glds_mpc.h5", "y_hat", replace)); + J.save(arma::hdf5_name("eg_glds_mpc.h5", "j", replace)); + + cout << "fin.\n"; + return 0; +} diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp new file mode 100644 index 00000000..a717d15a --- /dev/null +++ b/examples/eg_lqmpc_ctrl.cpp @@ -0,0 +1,203 @@ +//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control +//---------------------------===// +// +// Copyright 2024 Chia-Chien Hung and Kyle Johnsen +// Copyright 2024 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// \brief Example LQMPC Control +/// +/// \example eg_lqmpc_ctrl.cpp +//===----------------------------------------------------------------------===// + +#include +#include + +using lds::data_t; +using lds::Matrix; +using lds::Sparse; +using lds::Vector; +using std::cout; + +auto main() -> int { + cout << " ********** Example Gaussian MPC Control ********** \n\n"; + + // Example as provided by CLOCTools/lqmpc + + auto z_to_y = [](const Matrix& z) -> Matrix { + return (arma::log(z) + 5.468) / 61.4; + }; + // Didn't implement y_to_z because it's unused + + const data_t dt = 1e-3; // Sample period + const size_t n_u = 2; // Input dimensions + const size_t n_x = 4; // State dimensions + const size_t n_y = 3; // Output dimensions + + // Define the system matrices + Matrix A = Matrix({{1, -6.66e-13, -2.03e-9, -4.14e-6}, + {9.83e-4, 1, -4.09e-8, -8.32e-5}, + {4.83e-7, 9.83e-4, 1, -5.34e-4}, + {1.58e-10, 4.83e-7, 9.83e-4, .9994}}); + + Matrix B = Matrix({{9.83e-4, 4.83e-7, 1.58e-10, 3.89e-14}}).t(); + Matrix B_ = Matrix({{1e-5, 1e-5, 1e-10, 1e-14}}).t(); + if (n_u == 2) { + B = arma::join_rows(B, -B); + } else if (n_u == 3) { + B = arma::join_rows(B, -B, B_); + } + + Matrix C = Matrix({{-.0096, .0135, .005, -.0095}}); + if (n_y == 2) { + C = arma::join_cols(C, 2 * C); + } else if (n_y == 3) { + C = arma::join_cols(C, 2 * C, 3 * C); + } + + Vector g_true = Vector(n_u).fill(1.0); + + // Initialize the system that is being controlled + lds::gaussian::System controlled_system(n_u, n_x, n_y, dt); + controlled_system.set_A(A); + controlled_system.set_B(B); + controlled_system.set_C(C); + controlled_system.set_g(g_true); + controlled_system.Reset(); + + cout << ".....................................\n"; + cout << "controlled_system:\n"; + cout << ".....................................\n"; + controlled_system.Print(); + cout << ".....................................\n"; + + // Initialize the controller + lds::gaussian::MpcController controller; + const size_t N = 25; // Prediction horizon + const size_t M = 20; // Control horizon + { + Matrix Q = C.t() * C * 1e5; + Matrix R = Matrix(n_u, n_u, arma::fill::eye) * + 1e-1; // using dense instead of sparse matrix + Matrix S = Matrix( + n_u, n_u, arma::fill::zeros); // using dense instead of sparse matrix + + Vector umin = {0}; + Vector umax = {5}; + if (n_u == 2) { + umin = {0, 0}; + umax = {5, 5}; + } else if (n_u == 3) { + umin = {0, 0, 0}; + umax = {5, 5, 5}; + } + + Vector xmin(B.n_rows); + xmin.fill(-arma::datum::inf); + Vector xmax(B.n_rows); + xmax.fill(arma::datum::inf); + + lds::gaussian::System controller_system(controlled_system); + + controller = + std::move(lds::gaussian::MpcController(std::move(controller_system))); + controller.set_cost(Q, R, S, N, M); + controller.set_constraint(xmin, xmax, umin, umax); + } + + cout << ".....................................\n"; + cout << "control system:\n"; + cout << ".....................................\n"; + controller.Print(); + cout << ".....................................\n"; + + // Set up variables for simulation + Vector u0 = Vector(n_u, arma::fill::zeros); + Vector x0 = Vector(n_x, arma::fill::zeros); + + const size_t n_t = 120; // Number of time steps + const data_t t_sim = 0.25; // Simulation time step + Matrix zr = 0.05 * arma::sin(arma::linspace(0, 2 * arma::datum::pi, + (n_t + 25) * 250) * + 12) + + 0.1; + Matrix yr = z_to_y(zr.t()); + if (n_y == 2) { + yr = arma::join_cols(yr, 2 * yr); + } else if (n_y == 3) { + yr = arma::join_cols(yr, 2 * yr, 3 * yr); + } + + Matrix I = Matrix(n_x, n_x, arma::fill::eye); + Matrix ur = arma::pinv(C * arma::inv(I - A) * B) * yr; + Matrix xr = arma::inv(I - A) * B * ur; + + // create Matrix to save outputs in... + Matrix y_ref(n_y, n_t, arma::fill::zeros); + Matrix y_true(n_y, n_t, arma::fill::zeros); + Matrix y_hat(n_y, n_t, arma::fill::zeros); + Matrix u(n_u, n_t, arma::fill::zeros); + Matrix J(1, n_t, arma::fill::zeros); + + // Simulate the system + cout << "Starting " << n_t * t_sim << " sec simulation ... \n"; + auto t1 = std::chrono::high_resolution_clock::now(); + const size_t n_sim = static_cast(t_sim / dt); + + Vector z(n_y); // to get initial measurement + for (size_t t = 0; t < n_t; ++t) { + // Calculate the slice indices + size_t start_idx = t * n_sim; + size_t end_idx = (t + N) * n_sim - 1; + + auto* j = new data_t; // cost + + u0 = controller.Control(t_sim, z, xr.cols(start_idx, end_idx), true, j); + + for (size_t i = 0; i < n_sim; i++) { + z = controlled_system.Simulate(u0); + } + + lds::gaussian::System controller_system_test(controlled_system); + + // save the signals + y_ref.col(t) = yr.col(end_idx); + y_true.col(t) = controlled_system.y(); + y_hat.col(t) = controller.sys().y(); + u.col(t) = u0; + J.col(t).fill(*j); + } + + auto t2 = std::chrono::high_resolution_clock::now(); + std::chrono::duration sim_time_ms = t2 - t1; + cout << "Finished simulation in " << sim_time_ms.count() << " ms.\n"; + cout << "(app. " << (sim_time_ms.count() / n_t) * 1e3 << " µs/time-step)\n"; + + cout << "Saving simulation data to disk.\n"; + + // saving with hdf5 via armadillo + arma::hdf5_opts::opts replace = arma::hdf5_opts::replace; + + auto dt_vec = Vector(1).fill(dt); + dt_vec.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "dt")); + y_ref.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "y_ref", replace)); + y_true.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "y_true", replace)); + y_hat.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "y_hat", replace)); + u.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "u", replace)); + J.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "j", replace)); + + cout << "fin.\n"; + return 0; +} diff --git a/examples/eg_plds_ctrl.cpp b/examples/eg_plds_ctrl.cpp index 8334f439..f9fe0c5b 100755 --- a/examples/eg_plds_ctrl.cpp +++ b/examples/eg_plds_ctrl.cpp @@ -159,8 +159,9 @@ auto main() -> int { m_hat.col(0) = controller.sys().m(); m_true.col(0) = controlled_system.m(); - cout << "Starting " << n_t * dt << " sec simulation ... \n"; - auto start = std::chrono::high_resolution_clock::now(); + // get the disturbance at each time step ahead of time + // to maintain consistent between examples + arma::arma_rng::set_seed(100); for (size_t t = 1; t < n_t; t++) { // simulate a stochastically switched disturbance Vector chance = arma::randu(1); @@ -171,12 +172,18 @@ auto main() -> int { which_m = 1; } } else { // high disturbance - if (chance[0] < pr_hi2lo) { // swithces high -> low disturbance + if (chance[0] < pr_hi2lo) { // switches high -> low disturbance m0_true = std::vector(n_x, m_low); which_m = 0; } } - controlled_system.set_m(m0_true); + m_true.col(t) = m0_true; + } + + cout << "Starting " << n_t * dt << " sec simulation ... \n"; + auto start = std::chrono::high_resolution_clock::now(); + for (size_t t = 1; t < n_t; t++) { + controlled_system.set_m(m_true.col(t)); // e.g., use sinusoidal reference data_t f = 0.5; // freq [=] Hz @@ -198,7 +205,6 @@ auto main() -> int { y_true.col(t) = controlled_system.y(); x_true.col(t) = controlled_system.x(); - m_true.col(t) = controlled_system.m(); y_hat.col(t) = controller.sys().y(); x_hat.col(t) = controller.sys().x(); diff --git a/examples/eg_plds_mpc.cpp b/examples/eg_plds_mpc.cpp new file mode 100644 index 00000000..81763438 --- /dev/null +++ b/examples/eg_plds_mpc.cpp @@ -0,0 +1,244 @@ +//===-- eg_plds_mpc.cpp - Example PLDS MPC Control ---------------------===// +// +// Copyright 2024 Chia-Chien Hung and Kyle Johnsen +// Copyright 2024 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// \brief Example MPC control +/// +/// \example eg_plds_mpc.cpp +//===----------------------------------------------------------------------===// + +#include + +using lds::data_t; +using lds::Matrix; +using lds::Vector; +using std::cout; + +auto main() -> int { + cout << " ********** Example Poisson MPC Control ********** \n\n"; + // Same example system as eg_plds_mpc.cpp + + // Make SISO system sampled at 1kHz + data_t dt = 1e-3; + size_t n_u = 1; + size_t n_x = 1; + size_t n_y = 1; + + // no time steps for simulation. + auto n_t = static_cast(2.0 / dt); + + // Control variables: _reference/target output, controller gains + // n.b., Can either use Vector (arma::Col) or std::vector + Vector y_ref0 = Vector(n_y, arma::fill::ones) * 30 * dt; + + // Ground-truth parameters for the controlled system + // (stand-in for physical system to be controlled) + Matrix a_true(n_x, n_x, arma::fill::eye); + a_true[0] = 0.986; + Matrix b_true(n_x, n_u, arma::fill::zeros); + b_true[0] = 0.054; + Vector x0_true = Vector(n_x, arma::fill::ones) * log(1 * dt); + + /// Going to simulate a switching disturbance (m) acting on system + size_t which_m = 0; + data_t m_low = log(1 * dt) * (1 - a_true[0]); + data_t pr_lo2hi = 1e-3; + data_t m_high = log(20 * dt) * (1 - a_true[0]); + data_t pr_hi2lo = pr_lo2hi; + + Vector m0_true = Vector(n_x, arma::fill::ones) * m_low; + // construct ground truth system to be controlled... + lds::poisson::System controlled_system(n_u, n_x, n_y, dt); + + // Assign params. + controlled_system.set_A(a_true); + controlled_system.set_B(b_true); + controlled_system.set_m(m0_true); + controlled_system.set_x0(x0_true); + // reset to initial conditions + controlled_system.Reset(); + + cout << ".....................................\n"; + cout << "controlled_system:\n"; + cout << ".....................................\n"; + controlled_system.Print(); + cout << ".....................................\n"; + + // Create the controller + lds::poisson::MpcController controller; + const size_t N = 25; // Prediction horizon + const size_t M = 20; // Control horizon + { + // Create model used for control. + lds::poisson::System controller_system(controlled_system); + + // for this example, assume model correct, except disturbance + Vector m0_controller = Vector(n_x, arma::fill::ones) * m_low; + Vector x0_controller = arma::log(Vector(n_y, arma::fill::ones) * 1 * dt); + controller_system.set_m(m0_controller); + controller_system.set_x0(x0_controller); + controller_system.Reset(); // reset to new init condition + + // adaptively re-estimate process disturbance (m) + controller_system.do_adapt_m = true; + + // set adaptation rate by changing covariance of assumed process noise + // acting on random-walk evolution of m + Matrix q_m = Matrix(n_x, n_x, arma::fill::eye) * 1e-5; + controller_system.set_Q_m(q_m); + + // set control penalties + Matrix Q_y = Matrix(n_y, n_y, arma::fill::ones) * 1e5; + Matrix R = Matrix(n_u, n_u, arma::fill::eye) * 1e-1; + Matrix S = Matrix(n_u, n_u, arma::fill::zeros); + + Vector xmin = Vector(n_u); + xmin.fill(-arma::datum::inf); + Vector xmax = Vector(n_u); + xmax.fill(arma::datum::inf); + Vector umin = Vector(n_u) * 0.0; + Vector umax = Vector(n_u, arma::fill::ones) * 5.0; + + controller = + std::move(lds::poisson::MpcController(std::move(controller_system))); + controller.set_cost_output(Q_y, R, S, N, M); + controller.set_constraint(xmin, xmax, umin, umax); + } + + cout << ".....................................\n"; + cout << "controller:\n"; + cout << ".....................................\n"; + controller.Print(); + cout << ".....................................\n"; + + // create Matrix to save outputs in... + Matrix y_ref = Matrix(n_y, n_t + N + 1, arma::fill::zeros); + y_ref.each_col() += y_ref0; + + // Simulated measurements + Matrix z(n_y, n_t, arma::fill::zeros); + + // simulated control signal ([=] V) + Matrix u(n_u, n_t, arma::fill::zeros); + + // outputs, states and gain/disturbance params + // *_hat indicates online estimates + Matrix y_hat(n_y, n_t, arma::fill::zeros); + Matrix x_hat(n_x, n_t, arma::fill::zeros); + Matrix m_hat(n_y, n_t, arma::fill::zeros); + + // *_true indicates ground truth (system being controlled) + Matrix y_true(n_y, n_t, arma::fill::zeros); + Matrix x_true(n_x, n_t, arma::fill::zeros); + Matrix m_true(n_y, n_t, arma::fill::zeros); + + Matrix J(1, n_t, arma::fill::zeros); + + // set initial val + y_hat.col(0) = controller.sys().y(); + y_true.col(0) = controlled_system.y(); + + x_hat.col(0) = controller.sys().x(); + x_true.col(0) = controlled_system.x(); + + m_hat.col(0) = controller.sys().m(); + m_true.col(0) = controlled_system.m(); + + // calculate the target output + y_ref.col(0) = Vector(n_y, arma::fill::ones) * 1 * dt; + for (size_t t = 1; t < n_t + N + 1; t++) { + // e.g., use sinusoidal reference + data_t f = 0.5; // freq [=] Hz + Vector t_vec = Vector(n_y, arma::fill::ones) * t; + y_ref.col(t) += + y_ref0 % arma::sin(f * 2 * lds::kPi * dt * t_vec - lds::kPi / 4); + } + + // get the disturbance at each time step ahead of time + // to maintain consistent between examples + arma::arma_rng::set_seed(100); + for (size_t t = 1; t < n_t; t++) { + // simulate a stochastically switched disturbance + Vector chance = arma::randu(1); + if (which_m == 0) // low disturbance + { + if (chance[0] < pr_lo2hi) { // switches low -> high disturbance + m0_true = std::vector(n_x, m_high); + which_m = 1; + } + } else { // high disturbance + if (chance[0] < pr_hi2lo) { // switches high -> low disturbance + m0_true = std::vector(n_x, m_low); + which_m = 0; + } + } + m_true.col(t) = m0_true; + } + + cout << "Starting " << n_t * dt << " sec simulation ... \n"; + auto start = std::chrono::high_resolution_clock::now(); + for (size_t t = 1; t < n_t; t++) { + controlled_system.set_m(m_true.col(t)); + + // Simulate the true system. + z.col(t) = controlled_system.Simulate(u.col(t - 1)); + + // Calculate the slice indices + size_t start_idx = t; + size_t end_idx = t + N; + + auto* j = new data_t; + + u.col(t) = controller.ControlOutputReference( + dt, z.col(t), y_ref.cols(start_idx, end_idx), true, j); + + y_true.col(t) = controlled_system.y(); + x_true.col(t) = controlled_system.x(); + + y_hat.col(t) = controller.sys().y(); + x_hat.col(t) = controller.sys().x(); + m_hat.col(t) = controller.sys().m(); + + J.col(t) = *j; + } + + auto finish = std::chrono::high_resolution_clock::now(); + std::chrono::duration sim_time_ms = finish - start; + cout << "Finished simulation in " << sim_time_ms.count() << " ms.\n"; + cout << "(app. " << (sim_time_ms.count() / n_t) * 1e3 << " us/time-step)\n"; + + // saved variables: dt, y_hat, x_hat, m_hat, z, u, y_ref, y_true, + // x_true, m_true saving with hdf5 via armadillo + arma::hdf5_opts::opts replace = arma::hdf5_opts::replace; + + Matrix y_ref_vis = y_ref.cols(0, n_t - 1); + + auto dt_vec = Vector(1).fill(dt); + dt_vec.save(arma::hdf5_name("eg_plds_mpc.h5", "dt")); + y_ref_vis.save(arma::hdf5_name("eg_plds_mpc.h5", "y_ref", replace)); + u.save(arma::hdf5_name("eg_plds_mpc.h5", "u", replace)); + z.save(arma::hdf5_name("eg_plds_mpc.h5", "z", replace)); + x_true.save(arma::hdf5_name("eg_plds_mpc.h5", "x_true", replace)); + m_true.save(arma::hdf5_name("eg_plds_mpc.h5", "m_true", replace)); + y_true.save(arma::hdf5_name("eg_plds_mpc.h5", "y_true", replace)); + x_hat.save(arma::hdf5_name("eg_plds_mpc.h5", "x_hat", replace)); + m_hat.save(arma::hdf5_name("eg_plds_mpc.h5", "m_hat", replace)); + y_hat.save(arma::hdf5_name("eg_plds_mpc.h5", "y_hat", replace)); + J.save(arma::hdf5_name("eg_plds_mpc.h5", "J", replace)); + + return 0; +} diff --git a/examples/plot_glds_mpc_output.m b/examples/plot_glds_mpc_output.m new file mode 100644 index 00000000..3394e920 --- /dev/null +++ b/examples/plot_glds_mpc_output.m @@ -0,0 +1,47 @@ +clear; clc; + +dt = h5read('eg_glds_mpc.h5','/dt'); +u = h5read('eg_glds_mpc.h5','/u'); +z = h5read('eg_glds_mpc.h5','/z'); + +y_ref = h5read('eg_glds_mpc.h5','/y_ref'); +y_true = h5read('eg_glds_mpc.h5','/y_true'); +y_hat= h5read('eg_glds_mpc.h5','/y_hat'); + +x_true = h5read('eg_glds_mpc.h5','/x_true'); +x_hat= h5read('eg_glds_mpc.h5','/x_hat'); + +J = h5read('eg_glds_mpc.h5', '/j'); + +n_t = size(y_hat,2); +t = ((1:n_t)-1)*dt; + +c_data = 0.25 + zeros(1,3); +c_true = 0.5 + zeros(1,3); +c_est = [0.85, 0.5, 0.85]; +c_ref = [.25, .75, 0]; + +figure; +subplot(411); hold on; +plot(t, z', 'LineWidth', 0.5, 'color', c_data); +plot(t, y_hat', 'LineWidth', 2, 'color', c_est); +plot(t, y_ref', 'LineWidth', 2, 'color', c_ref); +legend({'measurements'; 'estimated output'; 'reference'}) +ylabel({'(a.u.)'}) + +subplot(412); hold on; +plot(t, x_hat', 'LineWidth', 2, 'color', c_est); +plot(t, x_true', 'LineWidth', 2, 'color', c_true); +legend({'estimated'; 'ground truth'}) +ylabel({'States'; '(a.u.)'}) + +subplot(413); hold on; +plot(t, u', 'LineWidth', 2, 'color', c_data) +ylabel({'Input';'(V)'}) +xlabel('Time (s)') + +subplot(414); hold on; +plot(t, J', 'LineWidth', 2, 'color', c_est); +ylabel({'Cost'}) + +printfig('eg_glds_mpc_output','png',gcf,[8 8]); diff --git a/examples/plot_lqmpc_ctrl_output.m b/examples/plot_lqmpc_ctrl_output.m new file mode 100644 index 00000000..14ffb437 --- /dev/null +++ b/examples/plot_lqmpc_ctrl_output.m @@ -0,0 +1,44 @@ +clear; clc; + +% Read data + dt = h5read('eg_lqmpc_ctrl.h5','/dt'); +u = h5read('eg_lqmpc_ctrl.h5','/u'); +y_true = h5read('eg_lqmpc_ctrl.h5','/y_true'); +y_ref = h5read('eg_lqmpc_ctrl.h5','/y_ref'); +j = h5read('eg_lqmpc_ctrl.h5','/j'); + +% Time vector + n_t = size(y_ref,2); +t = ((1:n_t)-1)*dt; + +% Define colors + c_true = [64, 121, 177; 241, 148, 74; 105, 172, 89] / 255; +c_ref = [235, 71, 58] / 255; +c_input = [64, 121, 177; 241, 148, 74] / 255; +c_cost = [64, 121, 177] / 255; + +% Plot the data + figure; + +% First graph + subplot(311); hold on; +for i = 1:3 + plot(t, y_true(i, :)', 'LineWidth', 2, 'color', c_true(i, :)); +end +plot(t, y_ref', 'LineWidth', 2, 'color', c_ref, 'LineStyle', '--'); +ylabel({'Output'}) + +% Second graph +subplot(312); hold on; +for i = 1:2 + stairs(t, u(i, :)', 'LineWidth', 2, 'color', c_input(i, :)); +end +ylabel({'Input'}) + +% Third graph +subplot(313); hold on; +stairs(t, j', 'LineWidth', 2, 'color', c_cost); +ylabel({'Cost'}) + +% Save the figure +printfig('eg_lqmpc_ctrl_output', 'png', gcf, [8 8]); diff --git a/include/ldsCtrlEst.in b/include/ldsCtrlEst.in index b1c918d0..77c17a01 100755 --- a/include/ldsCtrlEst.in +++ b/include/ldsCtrlEst.in @@ -52,6 +52,8 @@ #include "ldsCtrlEst_h/lds_ctrl.h" // SwitchedController type: #include "ldsCtrlEst_h/lds_sctrl.h" +// MpcController type: +#include "ldsCtrlEst_h/lds_mpcctrl.h" // lds::gaussian namespace: #include "ldsCtrlEst_h/lds_gaussian.h" @@ -61,6 +63,8 @@ #include "ldsCtrlEst_h/lds_gaussian_ctrl.h" // Gaussian SwitchedController type: #include "ldsCtrlEst_h/lds_gaussian_sctrl.h" +// Gaussian MpcController type: +#include "ldsCtrlEst_h/lds_gaussian_mpcctrl.h" // lds::poisson namespace: #include "ldsCtrlEst_h/lds_poisson.h" @@ -70,6 +74,8 @@ #include "ldsCtrlEst_h/lds_poisson_ctrl.h" // Gaussian SwitchedController type: #include "ldsCtrlEst_h/lds_poisson_sctrl.h" +// Poisson MpcController type: +#include "ldsCtrlEst_h/lds_poisson_mpcctrl.h" #ifdef LDSCTRLEST_BUILD_FIT // lds fit type: diff --git a/include/ldsCtrlEst_h/lds.h b/include/ldsCtrlEst_h/lds.h index 90a95c5c..fc268544 100755 --- a/include/ldsCtrlEst_h/lds.h +++ b/include/ldsCtrlEst_h/lds.h @@ -43,6 +43,7 @@ namespace lds { using data_t = double; // may change to float (but breaks mex functions) using Vector = arma::Col; using Matrix = arma::Mat; +using Sparse = arma::SpMat; using Cube = arma::Cube; using View = arma::subview; diff --git a/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h new file mode 100644 index 00000000..65f0b8b9 --- /dev/null +++ b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h @@ -0,0 +1,72 @@ +//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - MPC Controller ------*- C++ -*-===// +// +// Copyright 2024 Chia-Chien Hung and Kyle Johnsen +// Copyright 2024 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// +/// \file +/// This file declares the type for model predictive Model Predictive +/// Control (MPC) on linear system dynamics by converting the MPC optimization +/// problem to a quadratic cost problem. The new problem is optimized using the +/// Operator Splitting Quadratic Program (OSQP). +/// +/// \brief MPC Controller +//===----------------------------------------------------------------------===// + +#ifndef LDSCTRLEST_LDS_GAUSSIAN_MPCCTRL_H +#define LDSCTRLEST_LDS_GAUSSIAN_MPCCTRL_H + +// namespace +#include "lds_gaussian.h" +// system +#include "lds_gaussian_sys.h" +// controller +#include "lds_mpcctrl.h" + +namespace lds { +namespace gaussian { +/// Gaussian-observation MPC Controller Type +class MpcController : public lds::MpcController { + public: + + // make sure base class template methods available + using lds::MpcController::MpcController; + using lds::MpcController::Control; + using lds::MpcController::ControlOutputReference; + + // getters + using lds::MpcController::sys; + using lds::MpcController::n; + using lds::MpcController::m; + using lds::MpcController::N; + using lds::MpcController::M; + using lds::MpcController::A; + using lds::MpcController::B; + using lds::MpcController::C; + using lds::MpcController::S; + using lds::MpcController::u; + + // setters + using lds::MpcController::set_cost; + using lds::MpcController::set_cost_output; + using lds::MpcController::set_constraint; + + using lds::MpcController::Print; +}; +} // namespace gaussian +} // namespace lds + +#endif \ No newline at end of file diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h new file mode 100644 index 00000000..3d707c46 --- /dev/null +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -0,0 +1,544 @@ +//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - MPC Controller ----*- C++ -*-===// +// +// Copyright 2024 Chia-Chien Hung and Kyle Johnsen +// Copyright 2024 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// +/// \file +/// This file defines the type for model predictive Model Predictive +/// Control (MPC) on linear system dynamics by converting the MPC optimization +/// problem to a quadratic cost problem. The new problem is optimized using the +/// Operator Splitting Quadratic Program (OSQP). +/// +/// \brief MPC Controller +//===----------------------------------------------------------------------===// + +#ifndef LDSCTRLEST_LDS_MPCCTRL_H +#define LDSCTRLEST_LDS_MPCCTRL_H + +// namespace +#include "lds.h" +// system type +#include "lds_sys.h" + +// osqp +#include "osqp_arma.h" + +namespace lds { + +template +class MpcController { + static_assert(std::is_base_of::value, + "System must be derived from lds::System"); + + public: + /** + * @brief Constructs a new MpcController. + */ + MpcController() = default; + + /** + * @brief Constructs a new MpcController. + * + * @param sys The system being controlled + * + * @tparam System The system type + */ + MpcController(const System& sys); + + /** + * @brief Constructs a new MpcController. + * + * @param sys The system being controlled + * + * @tparam System The system type + */ + MpcController(System&& sys); + + /** + * @brief Perform one control step. + * + * @param t_sim Simulation time step + * @param z Measurement + * @param xr Reference/Target state (n x N*n_sim) + * @param do_control [optional] whether to update control (true) or + * simply feed through u_ref (false) + * @param J [optional] Pointer to variable storing cost + * + * @return A vector of the optimal control + */ + Vector Control(data_t t_sim, const Vector& z, const Matrix& xr, + bool do_control = true, data_t* J = NULL); + + /** + * @brief Perform one control step. + * + * @param t_sim Simulation time step + * @param z Measurement + * @param yr Reference/Target output (n x N*n_sim) + * @param do_control [optional] whether to update control (true) + * or simply feed through u_ref (false) + * @param J [optional] Pointer to variable storing cost + * + * @return A vector of the optimal control + */ + virtual Vector ControlOutputReference(data_t t_sim, const Vector& z, + const Matrix& yr, + bool do_control = true, + data_t* J = NULL); + + // getters + const System& sys() const { return sys_; } + const size_t n() const { return n_; } + const size_t m() const { return m_; } + const size_t N() const { return N_; } + const size_t M() const { return M_; } + const Matrix A() const { return A_; } + const Matrix B() const { return B_; } + const Matrix C() const { return C_; } + const Matrix S() const { return S_; } + const Vector u() const { return u_; } + + // setters + /** + * @brief Set the MPC cost matrices + * + * @param Q state cost matrix + * @param R input cost matrix + * @param S input change cost matrix + * @param N state prediction horizon + * @param M input horizon + */ + void set_cost(Matrix Q, Matrix R, Matrix S, size_t N, size_t M); + + /** + * @brief Set the MPC cost matrices for controlling output + * + * @param Q_y output cost matrix + * @param R input cost matrix + * @param S input change cost matrix + * @param N state prediction horizon + * @param M input horizon + */ + void set_cost_output(Matrix Q_y, Matrix R, Matrix S, size_t N, size_t M); + + /** + * @brief Set the MPC state and input bounds + * + * @param xmin state lower bound + * @param xmax state upper bound + * @param umin input lower bound + * @param umax input upper bound + */ + void set_constraint(Vector xmin, Vector xmax, Vector umin, Vector umax); + + void Print() { + sys_.Print(); + // TODO: Implement print + } + + protected: + System sys_; ///< system being controlled + size_t n_; ///< number of states + size_t m_; ///< number of output states + size_t N_; ///< number of steps + size_t M_; ///< number of inputs + Matrix A_; ///< state transition matrix + Matrix B_; ///< input matrix + Matrix C_; ///< output matrix + + osqp_arma::OSQP* OSQP; + Sparse P_; ///< penalty matrix + Matrix Q_; ///< cost matrix + + osqp_arma::OSQP* OSQP_y; + Sparse P_y_; ///< output penalty matrix + Matrix Q_y_; ///< output cost matrix + + Matrix S_; ///< input cost matrix + + Matrix lineq_; ///< lower inequality bound + Matrix uineq_; ///< upper inequality bound + Sparse Aineq_; ///< inequality condition matrix + + Matrix Acon_; ///< update condition matrix + Vector lb_; ///< lower bound + Vector ub_; ///< upper bound + + Vector u_; ///< previous step input + size_t t_sim_; ///< previous step simulation time step + + bool upd_ctrl_; ///< control was updated since last step + bool upd_ctrl_out_; ///< output control was updated since last step + bool upd_cons_; ///< constraint was updated since last step + + private: + /** + * @brief Calculate the trajectory for the simulation step + * + * @param x0 The initial state + * @param u0 The initial control input + * @param xr The reference trajectory + * @param out Print out step information + * + * @return The trajectory for the simulation step + */ + osqp_arma::Solution* calc_trajectory(const Vector& x0, const Vector& u0, + const Matrix& xr, size_t n_sim); + + /** + * @brief Calculate the trajectory based on output for the simulation + * step + * + * @param x0 The initial state + * @param u0 The initial control input + * @param yr The reference output trajectory + * @param out Print out step information + * + * @return The trajectory for the simulation step + */ + osqp_arma::Solution* calc_output_trajectory(const Vector& x0, + const Vector& u0, + const Matrix& yr, size_t n_sim); + + /** + * @brief Update the OSQP constraint bounds + * + * @param x0 Initial state + */ + void update_bounds(const Vector& x0); + + /** + * @brief Update the OSQP constraint matrices + * + * @param n_sim Number of time steps to simulate + */ + void update_constraints(size_t n_sim); + + /** + * @brief Create an identity matrix with an offset axis + * + * @param n The size of the matrix + * @param k The offset axis + * + * @return The identity matrix with an offset axis + */ + Sparse eye_offset(size_t n, int k = -1) { + Sparse mat(n, n); + mat.diag(k).ones(); + return mat; + } + + /** + * @brief Create a block diagonal matrix given two input matrices + * + * @param m1 The first matrix + * @param m2 The second matrix + * + * @return The block diagonal matrix + */ + Matrix block_diag(Matrix m1, Matrix m2) { + // Calculate the total rows and columns of the block diagonal matrix + size_t rows = m1.n_rows + m2.n_rows; + size_t cols = m1.n_cols + m2.n_cols; + + // Create the block diagonal matrix + Matrix bd = arma::zeros(rows, cols); + bd.submat(0, 0, m1.n_rows - 1, m1.n_cols - 1) = m1; + bd.submat(m1.n_rows, m1.n_cols, rows - 1, cols - 1) = m2; + + return bd; + } + + void Init() { + A_ = sys_.A(); + B_ = sys_.B() * arma::diagmat(sys_.g()); + C_ = sys_.C(); + n_ = B_.n_rows; + m_ = B_.n_cols; + u_ = Vector(m_, arma::fill::zeros); + t_sim_ = 0; + + OSQP = new osqp_arma::OSQP(); + OSQP->set_default_settings(); + OSQP->set_verbose(false); + + OSQP_y = new osqp_arma::OSQP(); + OSQP_y->set_default_settings(); + OSQP_y->set_verbose(false); + } +}; + +// Implement methods + +template +MpcController::MpcController(const System& sys) : sys_(sys) { + Init(); +} + +template +MpcController::MpcController(System&& sys) : sys_(std::move(sys)) { + Init(); +} + +template +Vector MpcController::Control(data_t t_sim, const Vector& z, + const Matrix& xr, bool do_control, + data_t* J) { + // TODO: Variable checks + + sys_.Filter(u_, z); + + size_t n_sim = t_sim / sys_.dt(); // Number of points to simulate + t_sim_ = t_sim; + if (do_control) { + osqp_arma::Solution* sol = calc_trajectory(sys_.x(), u_, xr, n_sim); + + for (int i = 0; i < m_; i++) { + u_(i) = sol->x(N_ * n_ + i); + } + if (J != NULL) *J = sol->obj_val(); + + if (sol) free(sol); + } + + for (int i = 0; i < n_sim; i++) { + // simulate for each time step + sys_.Simulate(u_); + } + + return u_; +} + +template +Vector MpcController::ControlOutputReference(data_t t_sim, + const Vector& z, + const Matrix& yr, + bool do_control, + data_t* J) { + // TODO: Variable checks + + sys_.Filter(u_, z); + + size_t n_sim = t_sim / sys_.dt(); // Number of points to simulate + t_sim_ = t_sim; + if (do_control) { + osqp_arma::Solution* sol = calc_output_trajectory(sys_.x(), u_, yr, n_sim); + + for (int i = 0; i < m_; i++) { + u_(i) = sol->x(N_ * n_ + i); + } + if (J != nullptr) { + *J = sol->obj_val(); + } + + if (sol) { + free(sol); + } + } + + for (int i = 0; i < n_sim; i++) { + // simulate for each time step + sys_.Simulate(u_); + } + + return u_; +} + +template +void MpcController::set_cost(Matrix Q, Matrix R, Matrix S, size_t N, + size_t M) { + // TODO: Variable checks + + Q_ = Q; + // R_ = R; // Isn't used + S_ = S; + N_ = N; + M_ = M; + + // Set up P matrix + Matrix Px = arma::kron(Matrix(N_, N_, arma::fill::eye), Q_); + Matrix Pu1 = arma::kron(Matrix(M_, M_, arma::fill::eye), 2 * S_ + R); + Matrix Pu2 = + arma::kron(Matrix(eye_offset(M)) + Matrix(eye_offset(M, 1)), -S_); + Matrix Pu3 = + block_diag(Matrix((M_ - 1) * m_, (M_ - 1) * m_, arma::fill::zeros), -S_); + Matrix Pu = Pu1 + Pu2 + Pu3; + P_ = Sparse(arma::trimatu( + 2 * block_diag(Px, Pu))); // Taking only the upper triangular part + + OSQP->set_P(P_); + + upd_ctrl_ = true; +} + +template +void MpcController::set_cost_output(Matrix Q_y, Matrix R, Matrix S, + size_t N, size_t M) { + // TODO: Variable checks + + Q_y_ = Q_y; + // R_ = R; // Isn't used + S_ = S; + N_ = N; + M_ = M; + + Matrix Q = C_.t() * Q_y_ * C_; + + // Set up P matrix + Matrix Px = arma::kron(Matrix(N_, N_, arma::fill::eye), Q); + Matrix Pu1 = arma::kron(Matrix(M_, M_, arma::fill::eye), 2 * S_ + R); + Matrix Pu2 = + arma::kron(Matrix(eye_offset(M)) + Matrix(eye_offset(M, 1)), -S_); + Matrix Pu3 = + block_diag(Matrix((M_ - 1) * m_, (M_ - 1) * m_, arma::fill::zeros), -S_); + Matrix Pu = Pu1 + Pu2 + Pu3; + P_y_ = 2 * Sparse(block_diag(Px, Pu)); + + OSQP_y->set_P(P_y_); + + upd_ctrl_out_ = true; +} + +template +void MpcController::set_constraint(Vector xmin, Vector xmax, + Vector umin, Vector umax) { + // TODO: Check constraints + + lineq_ = join_horiz(arma::kron(Vector(N_, arma::fill::ones), xmin).t(), + arma::kron(Vector(M_, arma::fill::ones), umin).t()); + uineq_ = join_horiz(arma::kron(Vector(N_, arma::fill::ones), xmax).t(), + arma::kron(Vector(M_, arma::fill::ones), umax).t()); + std::cout << "N_: " << N_ << "\n"; + std::cout << "n_: " << n_ << "\n"; + std::cout << "M_: " << M_ << "\n"; + std::cout << "m_: " << m_ << "\n"; + size_t Aineq_dim = N_ * n_ + M_ * m_; + Aineq_ = arma::eye(Aineq_dim, Aineq_dim); + + upd_cons_ = true; +} + +template +osqp_arma::Solution* MpcController::calc_trajectory(const Vector& x0, + const Vector& u0, + const Matrix& xr, + size_t n_sim) { + update_bounds(x0); + OSQP->set_l(lb_); + OSQP->set_u(ub_); + + if (upd_ctrl_ || upd_ctrl_out_ || upd_cons_) { + update_constraints(n_sim); + } + OSQP->set_A(Acon_); + + // Convert state penalty from reference to OSQP format + Vector q; + { + arma::uvec indices = arma::regspace(0, n_sim, N_ * n_sim - 1); + Matrix sliced_xr = xr.cols(indices); + Matrix Qxr_full = -2 * Q_ * sliced_xr; + Vector Qxr = Qxr_full.as_col(); // Qxr for every simulation time step + + Vector qu = + join_vert((-2 * S_ * u0), Vector((M_ - 1) * m_, arma::fill::zeros)); + Vector qx = Qxr.rows(0, N_ * n_ - 1); + q = join_vert(qx, qu); + } + + // set problem + OSQP->set_q(q); + + osqp_arma::Solution* sol = OSQP->solve(); + + return sol; +} + +template +osqp_arma::Solution* MpcController::calc_output_trajectory( + const Vector& x0, const Vector& u0, const Matrix& yr, size_t n_sim) { + update_bounds(x0); + OSQP_y->set_l(lb_); + OSQP_y->set_u(ub_); + + if (upd_ctrl_ || upd_ctrl_out_ || upd_cons_) { + update_constraints(n_sim); + } + OSQP_y->set_A(Acon_); + + // Convert state penalty from reference to OSQP format + Vector q; + { + arma::uvec indices = arma::regspace(0, n_sim, N_ * n_sim - 1); + Matrix sliced_yr = yr.cols(indices); + Matrix Qxr_full = -2 * sliced_yr.t() * Q_y_ * C_; + Vector Qxr = Qxr_full.as_row().t(); // Qxr for every simulation time step + + Vector qu = + join_vert((-2 * S_ * u0), Vector((M_ - 1) * m_, arma::fill::zeros)); + Vector qx = Qxr.rows(0, N_ * n_ - 1); + q = join_vert(qx, qu); + } + + // set problem + OSQP_y->set_q(q); + + osqp_arma::Solution* sol = OSQP_y->solve(); + + return sol; +} + +template +void MpcController::update_bounds(const Vector& x0) { + Matrix leq = join_horiz( + -x0.t(), arma::zeros((N_ - 1) * n_).t()); // Lower equality bound + Matrix ueq = leq; // Upper equality bound + lb_ = join_horiz(leq, lineq_).t(); // Lower bound + ub_ = join_horiz(ueq, uineq_).t(); // Upper bound +} + +template +void MpcController::update_constraints(size_t n_sim) { + // Update x over n_sim many steps + Matrix Axs = arma::real( + arma::powmat(A_, static_cast(n_sim))); // State multiplier + Matrix Aus = arma::zeros(n_, n_); // Input multiplier + for (int i = 0; i < n_sim; i++) { + Aus += arma::powmat(A_, i); + } + + // Ax + Bu = 0 + Matrix Ax( + arma::kron(arma::speye(N_, N_), -arma::speye(n_, n_)) + + arma::kron(eye_offset(N_), Sparse(Axs))); + Matrix B0(1, M_); + Matrix Bstep(M_, M_, arma::fill::eye); + Matrix Bend = arma::join_horiz(Matrix(N_ - M_ - 1, M_ - 1), + Matrix(N_ - M_ - 1, 1, arma::fill::ones)); + Matrix Bu = kron(join_vert(B0, Matrix(Bstep), Bend), Aus * B_); + Matrix Aeq = join_horiz(Ax, Bu); // Equality condition + + Acon_ = join_vert(Aeq, Matrix(Aineq_)); // Update condition + + upd_ctrl_ = false; + upd_ctrl_out_ = false; + upd_cons_ = false; +} + +} // namespace lds + +#endif diff --git a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h new file mode 100644 index 00000000..cbd79dc9 --- /dev/null +++ b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h @@ -0,0 +1,88 @@ +//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - PLDS MPC Controller ------*- C++ +//-*-===// +// +// Copyright 2024 Chia-Chien Hung and Kyle Johnsen +// Copyright 2024 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// +/// \file +/// This file declares the type for model predictive Model Predictive +/// Control (MPC) on linear system dynamics by converting the MPC optimization +/// problem to a quadratic cost problem. The new problem is optimized using the +/// Operator Splitting Quadratic Program (OSQP). +/// +/// \brief MPC Controller +//===----------------------------------------------------------------------===// + +#ifndef LDSCTRLEST_LDS_POISSON_MPCCTRL_H +#define LDSCTRLEST_LDS_POISSON_MPCCTRL_H + +// namespace +#include "lds_poisson.h" +// system +#include "lds_poisson_sys.h" +// controller +#include "lds_mpcctrl.h" + +namespace lds { +namespace poisson { +/// Poisson-observation MPC Controller Type +class MpcController : public lds::MpcController { + public: + // make sure base class template methods available + using lds::MpcController::MpcController; + using lds::MpcController::Control; + + virtual Vector ControlOutputReference(data_t t_sim, const Vector& z, + const Matrix& yr, bool do_control, + data_t* J) { + Matrix yr_transformed = yr; + // clamping the target output to address log10(0) issues + yr_transformed.clamp(kYRefLb, arma::datum::inf); + // log transforming the output to make it linear + yr_transformed = log(yr_transformed); + + return lds::MpcController::ControlOutputReference( + t_sim, z, yr_transformed, do_control, J); + } + + // getters + using lds::MpcController::sys; + using lds::MpcController::n; + using lds::MpcController::m; + using lds::MpcController::N; + using lds::MpcController::M; + using lds::MpcController::A; + using lds::MpcController::B; + using lds::MpcController::C; + using lds::MpcController::S; + using lds::MpcController::u; + + // setters + using lds::MpcController::set_cost; + using lds::MpcController::set_cost_output; + using lds::MpcController::set_constraint; + + using lds::MpcController::Print; + + private: + constexpr static const data_t kYRefLb = + 1e-12; ///< lower bound on yRef (to avoid numerical log(0) issue) +}; +} // namespace poisson +} // namespace lds + +#endif // LDSCTRLEST_LDS_POISSON_MPCCTRL_H diff --git a/include/ldsCtrlEst_h/osqp_arma.h b/include/ldsCtrlEst_h/osqp_arma.h new file mode 100644 index 00000000..f94de208 --- /dev/null +++ b/include/ldsCtrlEst_h/osqp_arma.h @@ -0,0 +1,527 @@ +//===-- ldsCtrlEst_h/osqp_arma.h - OSQP/armadillo wrapper ---------*- C++ +//-*-===// +// +// Copyright 2024 Aaron Hung +// Copyright 2024 Georgia Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +//===----------------------------------------------------------------------===// +/// +/// \file +/// This file defines a wrapper class for the osqp solver that uses +/// armadillo matrices and vectors for some methods that are used in +/// lds_mpcctrl.h +/// +/// \brief osqp/armadillo partial wrapper class +//===----------------------------------------------------------------------===// + +#ifndef LDSCTRLEST_OSQP_ARMA_H +#define LDSCTRLEST_OSQP_ARMA_H + +#include + +#include "osqp.h" +#include "osqp_api_constants.h" + +namespace osqp_arma { + +/** + * @brief Converts an OSQP CSC matrix to an Armadillo dense matrix. + * + * @param osqpMatrix Pointer to the OSQP CSC matrix. + * + * @return Armadillo dense matrix. + */ +arma::mat to_matrix(OSQPCscMatrix* osqpMatrix) { + arma::mat result(osqpMatrix->m, osqpMatrix->n, arma::fill::zeros); + for (OSQPInt col = 0; col < osqpMatrix->n; ++col) { + for (OSQPInt idx = osqpMatrix->p[col]; idx < osqpMatrix->p[col + 1]; + ++idx) { + result(osqpMatrix->i[idx], col) = osqpMatrix->x[idx]; + } + } + return result; +} + +/** + * @brief Converts an OSQP CSC matrix to an Armadillo sparse matrix. + * + * @param osqpMatrix Pointer to the OSQP CSC matrix. + * + * @return Armadillo sparse matrix. + */ +arma::sp_mat to_sparse(OSQPCscMatrix* osqpMatrix) { + arma::sp_mat result(osqpMatrix->m, osqpMatrix->n); + for (OSQPInt col = 0; col < osqpMatrix->n; ++col) { + for (OSQPInt idx = osqpMatrix->p[col]; idx < osqpMatrix->p[col + 1]; + ++idx) { + result(osqpMatrix->i[idx], col) = osqpMatrix->x[idx]; + } + } + return result; +} + +/** + * @brief Converts an OSQP float array to an Armadillo vector. + * + * @param osqpFloatArr Pointer to the OSQP float array. + * + * @return Armadillo vector. + */ +arma::vec to_vector(OSQPFloat* osqpFloatArr, OSQPInt size) { + arma::vec result(size); + for (OSQPInt i = 0; i < size; ++i) { + result[i] = osqpFloatArr[i]; + } + return result; +} + +/** + * @brief Converts an Armadillo dense matrix to an OSQP CSC matrix. + * + * @param armaMatrix Armadillo dense matrix. + * + * @return Pointer to the converted OSQP CSC matrix. + */ +OSQPCscMatrix* from_matrix(arma::mat armaMatrix) { + OSQPInt m = armaMatrix.n_rows; + OSQPInt n = armaMatrix.n_cols; + + std::vector i_vec; + std::vector x_vec; + std::vector p_vec(n + 1); + + OSQPInt nnz = 0; + for (OSQPInt col = 0; col < n; ++col) { + p_vec[col] = nnz; + for (OSQPInt row = 0; row < m; ++row) { + OSQPFloat value = armaMatrix(row, col); + if (value != 0.0) { + i_vec.push_back(row); + x_vec.push_back(value); + ++nnz; + } + } + } + p_vec[n] = nnz; + + OSQPCscMatrix* osqpMatrix = new OSQPCscMatrix; + osqpMatrix->m = m; + osqpMatrix->n = n; + osqpMatrix->nzmax = nnz; + osqpMatrix->nz = -1; + osqpMatrix->x = new OSQPFloat[nnz]; + osqpMatrix->i = new OSQPInt[nnz]; + osqpMatrix->p = new OSQPInt[n + 1]; + + std::copy(x_vec.begin(), x_vec.end(), osqpMatrix->x); + std::copy(i_vec.begin(), i_vec.end(), osqpMatrix->i); + std::copy(p_vec.begin(), p_vec.end(), osqpMatrix->p); + + return osqpMatrix; +} + +/** + * @brief Converts an Armadillo sparse matrix to an OSQP CSC matrix. + * + * @param armaSparse Armadillo sparse matrix. + * + * @return Pointer to the converted OSQP CSC matrix. + */ +OSQPCscMatrix* from_sparse(arma::sp_mat armaSparse) { + OSQPInt m = armaSparse.n_rows; + OSQPInt n = armaSparse.n_cols; + OSQPInt nnz = armaSparse.n_nonzero; + + OSQPCscMatrix* osqpMatrix = new OSQPCscMatrix; + osqpMatrix->m = m; + osqpMatrix->n = n; + osqpMatrix->nzmax = nnz; + osqpMatrix->nz = -1; + osqpMatrix->x = new OSQPFloat[nnz]; + osqpMatrix->i = new OSQPInt[nnz]; + osqpMatrix->p = new OSQPInt[n + 1]; + + OSQPInt idx = 0; + for (OSQPInt col = 0; col < n; ++col) { + osqpMatrix->p[col] = idx; + for (auto it = armaSparse.begin_col(col); it != armaSparse.end_col(col); + ++it) { + osqpMatrix->i[idx] = it.row(); + osqpMatrix->x[idx] = *it; + ++idx; + } + } + osqpMatrix->p[n] = idx; + + return osqpMatrix; +} + +/** + * @brief Converts an Armadillo vector to an OSQP float array. + * + * @param armaVector Armadillo vector. + * + * @return Pointer to the converted OSQP float array. + */ +OSQPFloat* from_vector(arma::vec armaVector) { + OSQPInt size = armaVector.n_elem; + OSQPFloat* osqpFloatArr = new OSQPFloat[size]; + for (OSQPInt i = 0; i < size; ++i) { + osqpFloatArr[i] = armaVector[i]; + } + return osqpFloatArr; +} + +class Solution { + public: + Solution(OSQPSolution* sol, OSQPInfo* info, int n, int m) { + x_ = to_vector(sol->x, n); + y_ = to_vector(sol->y, m); + prim_inf_cert_ = to_vector(sol->prim_inf_cert, n); + dual_inf_cert_ = to_vector(sol->dual_inf_cert, n); + obj_val_ = info->obj_val; + } + + /** + * @brief get the primal solution + * + * @return primal solution + */ + arma::vec* x() { return &x_; } + + double x(int i) { return x_(i); } + + /** + * @brief get the Lagrange multiplier + * + * @return Lagrange multiplier associated with Ax + */ + arma::vec* y() { return &y_; } + + double y(int i) { return y_(i); } + + /** + * @brief get the primal infeasibility certificate + * + * @return Primal infeasibility certificate + */ + arma::vec* prim_inf_cert() { return &prim_inf_cert_; } + + /** + * @brief get the dual infeasibility certificate + * + * @return Dual infeasibility certificate + */ + arma::vec* dual_inf_cert() { return &dual_inf_cert_; } + + double obj_val() { return obj_val_; } + + protected: + arma::vec x_; + arma::vec y_; + arma::vec prim_inf_cert_; + arma::vec dual_inf_cert_; + double obj_val_; +}; // class Solution + +class OSQP { + public: + OSQP() { + solver = new OSQPSolver(); + settings = new OSQPSettings(); + } + + /** + * @brief set up the problem + * + * @param P Problem data (upper triangular part of quadratic cost term, csc + * format) + * @param q Problem data (linear cost term) + * @param A Problem data (constraint matrix, csc format) + * @param l Problem data (constraint lower bound) + * @param u Problem data (constraint upper bound) + * @param m Problem data (number of constraints) + * @param n Problem data (number of variables) + */ + void setup(arma::sp_mat P, arma::vec q, arma::mat A, arma::vec l, + arma::vec u) { + P_ = from_sparse(P); + q_ = from_vector(q); + A_ = from_matrix(A); + l_ = from_vector(l); + u_ = from_vector(u); + m_ = A_->m; + n_ = A_->n; + updated_problem_ = true; + } + + /** + * @brief solve the problem + * + * @return Solution containing the optimal trajectory + */ + Solution* solve() { + if (updated_problem_ || updated_settings_) { + OSQPInt exitflag = + osqp_setup(&solver, P_, q_, A_, l_, u_, m_, n_, settings); + if (exitflag != 0) { + throw std::runtime_error("osqp_setup failed with exit flag: " + + std::to_string(exitflag)); + } + updated_problem_ = false; + updated_settings_ = false; + updated_data_ = false; + } else if (updated_data_) { + OSQPInt exitflag = osqp_update_data_vec(solver, q_, NULL, NULL); + if (exitflag != 0) { + throw std::runtime_error( + "osqp_update_data_vec failed with exit flag: " + + std::to_string(exitflag)); + } + } + + OSQPInt exitflag = osqp_solve(solver); + if (exitflag != 0) { + throw std::runtime_error("osqp_solve failed with exit flag: " + + std::to_string(exitflag)); + } + + return new Solution(solver->solution, solver->info, n_, m_); + } + + /** + * @brief set the settings of the solver to the default settings + */ + void set_default_settings() { + osqp_set_default_settings(settings); + updated_settings_ = true; + } + + /** + * @brief set the settings of the solver + * + * @param newSettings new settings to assign to solver settings + */ + void set_settings(OSQPSettings* newSettings) { + std::memcpy(settings, newSettings, sizeof(OSQPSettings)); + updated_settings_ = true; + } + + void set_P(arma::sp_mat P) { + P_ = from_sparse(P); + updated_problem_ = true; + } + + void set_q(arma::vec q) { + q_ = from_vector(q); + updated_data_ = true; + } + + void set_A(arma::mat A) { + A_ = from_matrix(A); + n_ = A_->n; + m_ = A_->m; + updated_problem_ = true; + } + + void set_l(arma::vec l) { + l_ = from_vector(l); + updated_problem_ = true; + } + + void set_u(arma::vec u) { + u_ = from_vector(u); + updated_problem_ = true; + } + + void set_m(OSQPInt m) { + m_ = m; + updated_problem_ = true; + } + + void set_n(OSQPInt n) { + n_ = n; + updated_problem_ = true; + } + + void set_device(OSQPInt device) { + settings->device = device; + updated_settings_ = true; + } + + void set_linsys_solver(osqp_linsys_solver_type linsys_solver) { + settings->linsys_solver = linsys_solver; + updated_settings_ = true; + } + + void set_allocate_solution(OSQPInt allocate_solution) { + settings->allocate_solution = allocate_solution; + updated_settings_ = true; + } + + void set_verbose(OSQPInt verbose) { + settings->verbose = verbose; + updated_settings_ = true; + } + + void set_profiler_level(OSQPInt profiler_level) { + settings->profiler_level = profiler_level; + updated_settings_ = true; + } + + void set_warm_starting(OSQPInt warm_starting) { + settings->warm_starting = warm_starting; + updated_settings_ = true; + } + + void set_scaling(OSQPInt scaling) { + settings->scaling = scaling; + updated_settings_ = true; + } + + void set_polishing(OSQPInt polishing) { + settings->polishing = polishing; + updated_settings_ = true; + } + + void set_rho(OSQPFloat rho) { + settings->rho = rho; + updated_settings_ = true; + } + + void set_rho_is_vec(OSQPInt rho_is_vec) { + settings->rho_is_vec = rho_is_vec; + updated_settings_ = true; + } + + void set_sigma(OSQPFloat sigma) { + settings->sigma = sigma; + updated_settings_ = true; + } + + void set_alpha(OSQPFloat alpha) { + settings->alpha = alpha; + updated_settings_ = true; + } + + void set_cg_max_iter(OSQPInt cg_max_iter) { + settings->cg_max_iter = cg_max_iter; + updated_settings_ = true; + } + + void set_cg_tol_reduction(OSQPInt cg_tol_reduction) { + settings->cg_tol_reduction = cg_tol_reduction; + updated_settings_ = true; + } + + void set_cg_tol_fraction(OSQPFloat cg_tol_fraction) { + settings->cg_tol_fraction = cg_tol_fraction; + updated_settings_ = true; + } + + void set_cg_precond(osqp_precond_type cg_precond) { + settings->cg_precond = cg_precond; + updated_settings_ = true; + } + + void set_adaptive_rho(OSQPInt adaptive_rho) { + settings->adaptive_rho = adaptive_rho; + updated_settings_ = true; + } + + void set_adaptive_rho_interval(OSQPInt adaptive_rho_interval) { + settings->adaptive_rho_interval = adaptive_rho_interval; + updated_settings_ = true; + } + + void set_adaptive_rho_fraction(OSQPFloat adaptive_rho_fraction) { + settings->adaptive_rho_fraction = adaptive_rho_fraction; + updated_settings_ = true; + } + + void set_adaptive_rho_tolerance(OSQPFloat adaptive_rho_tolerance) { + settings->adaptive_rho_tolerance = adaptive_rho_tolerance; + updated_settings_ = true; + } + + void set_max_iter(OSQPInt max_iter) { + settings->max_iter = max_iter; + updated_settings_ = true; + } + + void set_eps_abs(OSQPFloat eps_abs) { + settings->eps_abs = eps_abs; + updated_settings_ = true; + } + + void set_eps_rel(OSQPFloat eps_rel) { + settings->eps_rel = eps_rel; + updated_settings_ = true; + } + + void set_eps_prim_inf(OSQPFloat eps_prim_inf) { + settings->eps_prim_inf = eps_prim_inf; + updated_settings_ = true; + } + + void set_eps_dual_inf(OSQPFloat eps_dual_inf) { + settings->eps_dual_inf = eps_dual_inf; + updated_settings_ = true; + } + + void set_scaled_termination(OSQPInt scaled_termination) { + settings->scaled_termination = scaled_termination; + updated_settings_ = true; + } + + void set_check_termination(OSQPInt check_termination) { + settings->check_termination = check_termination; + updated_settings_ = true; + } + + void set_time_limit(OSQPFloat time_limit) { + settings->time_limit = time_limit; + updated_settings_ = true; + } + + void set_delta(OSQPFloat delta) { + settings->delta = delta; + updated_settings_ = true; + } + + void set_polish_refine_iter(OSQPInt polish_refine_iter) { + settings->polish_refine_iter = polish_refine_iter; + updated_settings_ = true; + } + + protected: + OSQPSolver* solver; + OSQPCscMatrix* P_; // Problem data (upper triangular part of quadratic cost + // term, csc format) + OSQPFloat* q_; // Problem data (linear cost term) + OSQPCscMatrix* A_; // Problem data (constraint matrix, csc format) + OSQPFloat* l_; // Problem data (constraint lower bound) + OSQPFloat* u_; // Problem data (constraint upper bound) + OSQPInt m_; // Problem data (number of constraints) + OSQPInt n_; // Problem data (number of variables) + bool updated_problem_ = true; // whether the problem has been updated + bool updated_data_ = true; // whether the data has been updated + + OSQPSettings* settings; + bool updated_settings_ = true; // whether any settings have been updated +}; // class OSQP +} // namespace osqp_arma + +#endif // LDSCTRLEST_OSQP_ARMA_H diff --git a/vcpkg b/vcpkg index b98afc9f..7eb700c9 160000 --- a/vcpkg +++ b/vcpkg @@ -1 +1 @@ -Subproject commit b98afc9f1192becb2f447cee485ce36ba111f9f6 +Subproject commit 7eb700c9688daed6d8bdcdc571ebe3eedea6a774