From 22be42d9366b14a981749969b19dd44f5be85749 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Sun, 12 May 2024 13:12:28 +0800 Subject: [PATCH 01/26] Update vcpkg version The commit that the vcpkg submodule was linked to depended on a version of zlib that was downloaded from the zlib website that is outdated. This commit updates the vcpkg to the newest version, which gets zlib directly from the github repository. --- vcpkg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vcpkg b/vcpkg index b98afc9f..a1212c93 160000 --- a/vcpkg +++ b/vcpkg @@ -1 +1 @@ -Subproject commit b98afc9f1192becb2f447cee485ce36ba111f9f6 +Subproject commit a1212c93cabaa9c5c36c1ffdb4bddd59fdf31e43 From f6d1f148dbdc01617272274c4fd0891d8249f4e8 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Sun, 12 May 2024 20:41:08 +0800 Subject: [PATCH 02/26] Implemented foundations for MPC controller example Roughly implemented the example code for MPC controller, based on the example from cloctools/lqmpc. --- examples/eg_lqmpc_ctrl.cpp | 155 +++++++++++++++++++++++++++++++++++++ 1 file changed, 155 insertions(+) create mode 100644 examples/eg_lqmpc_ctrl.cpp diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp new file mode 100644 index 00000000..ed59d3b4 --- /dev/null +++ b/examples/eg_lqmpc_ctrl.cpp @@ -0,0 +1,155 @@ +//===-- 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::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); + } + + // 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); + + cout << ".....................................\n"; + cout << "controlled_system:\n"; + cout << ".....................................\n"; + controlled_system.Print(); + cout << ".....................................\n"; + + // Initialize the controller + lds::gaussian::MpcController controller; // TODO: Implement MpcController + { + 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}; + } + + controller = std::move(lds::gaussian::MpcController(std::move(controlled_system))); + controller.set_control(Q, R, S, 25, 20); + controller.set_constraints(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); + 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; + + // 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 = int(t_sim / dt); + + 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 + 1) * n_sim - 1; + + if (t == 0) { + controller.step(t_sim, x0, u0, xr.cols(start_idx, end_idx), false); + } else { + // TODO: Implement step function and check controller.xi and controller.ui + controller.step(t_sim, controller.xi, controller.ui, xr.cols(start_idx, end_idx), false); + } + } + + 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"; + + // TODO: Implement saving simulation data to disk + + cout << "fin.\n"; + return 0; +} \ No newline at end of file From 29b6a9cb856ff94354ed6ca8448d81d50d231200 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Sat, 18 May 2024 11:31:45 +0800 Subject: [PATCH 03/26] Updated vcpkg version --- vcpkg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vcpkg b/vcpkg index a1212c93..7eb700c9 160000 --- a/vcpkg +++ b/vcpkg @@ -1 +1 @@ -Subproject commit a1212c93cabaa9c5c36c1ffdb4bddd59fdf31e43 +Subproject commit 7eb700c9688daed6d8bdcdc571ebe3eedea6a774 From 281fafbdeb180e4c39eec4be29e982da16593398 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 22 May 2024 13:14:55 +0800 Subject: [PATCH 04/26] Added osqp as a submodule and updated CMakeLists to include osqp as a dependency --- .gitmodules | 3 +++ CMakeLists.txt | 1 + cmake/Modules/OSQP.cmake | 2 ++ osqp | 1 + 4 files changed, 7 insertions(+) create mode 100644 cmake/Modules/OSQP.cmake create mode 160000 osqp diff --git a/.gitmodules b/.gitmodules index 42fe1bfb..611eb5b6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -11,3 +11,6 @@ [submodule "python/carma"] path = python/carma url = https://github.com/RUrlus/carma.git +[submodule "osqp"] + path = osqp + url = https://github.com/osqp/osqp 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..f271271a --- /dev/null +++ b/cmake/Modules/OSQP.cmake @@ -0,0 +1,2 @@ +find_package(osqp REQUIRED) +list(APPEND PROJECT_REQUIRED_LIBRARIES_ABSOLUTE_NAME osqp::osqp) \ No newline at end of file diff --git a/osqp b/osqp new file mode 160000 index 00000000..4e81a9d0 --- /dev/null +++ b/osqp @@ -0,0 +1 @@ +Subproject commit 4e81a9d0d72c7523e776fdd8f32a79614ebd1a8b From 0e5048ed988668ee62577d0640379061d84cb8d2 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Thu, 23 May 2024 10:35:44 +0800 Subject: [PATCH 05/26] Corrected matrix multiplication syntax --- examples/eg_lqmpc_ctrl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index ed59d3b4..2587e08a 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -80,7 +80,7 @@ auto main() -> int { // Initialize the controller lds::gaussian::MpcController controller; // TODO: Implement MpcController { - Matrix Q = C.t() % C * 1e5; + 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 @@ -120,8 +120,8 @@ auto main() -> int { } 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; + Matrix ur = arma::pinv(C * arma::inv(I - A) * B) * yr; + Matrix xr = arma::inv(I - A) * B * ur; // Simulate the system cout << "Starting " << n_t * t_sim << " sec simulation ... \n"; From 2f386be5414b141f40d5ddba01f150ff96c48cc9 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Mon, 10 Jun 2024 22:10:57 -0700 Subject: [PATCH 06/26] Implemented part of the code for MpcController - The code for the MpcController is partially implemented (only necessary methods have been added, getters and setters need to be added) - Code still needs to be debugged (there are multiple memory leaks that need to be fixed) --- examples/eg_lqmpc_ctrl.cpp | 34 +- include/ldsCtrlEst.in | 4 + include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h | 58 +++ include/ldsCtrlEst_h/lds_mpcctrl.h | 533 ++++++++++++++++++++ 4 files changed, 617 insertions(+), 12 deletions(-) create mode 100644 include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h create mode 100644 include/ldsCtrlEst_h/lds_mpcctrl.h diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index 2587e08a..f59103ac 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -65,11 +65,13 @@ auto main() -> int { C = arma::join_cols(C, 2 * C, 3 * C); } + // 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.Reset(); cout << ".....................................\n"; cout << "controlled_system:\n"; @@ -78,7 +80,8 @@ auto main() -> int { cout << ".....................................\n"; // Initialize the controller - lds::gaussian::MpcController controller; // TODO: Implement MpcController + lds::gaussian::MpcController controller; + const size_t N = 25; // Prediction 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 @@ -94,9 +97,17 @@ auto main() -> int { umax = {5, 5, 5}; } - controller = std::move(lds::gaussian::MpcController(std::move(controlled_system))); - controller.set_control(Q, R, S, 25, 20); - controller.set_constraints(umin, umax); + 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), umin, umax)); + controller.set_control(Q, R, S, N, 20); + controller.set_constraint(xmin, xmax, umin, umax); } cout << ".....................................\n"; @@ -109,10 +120,11 @@ auto main() -> int { 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); + Matrix yr = z_to_y(zr.t()); if (n_y == 2) { yr = arma::join_cols(yr, 2 * yr); } else if (n_y == 3) { @@ -131,14 +143,12 @@ auto main() -> int { 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 + 1) * n_sim - 1; + size_t end_idx = (t + N) * n_sim - 1; - if (t == 0) { - controller.step(t_sim, x0, u0, xr.cols(start_idx, end_idx), false); - } else { - // TODO: Implement step function and check controller.xi and controller.ui - controller.step(t_sim, controller.xi, controller.ui, xr.cols(start_idx, end_idx), false); - } + u0 = controller.Control(t_sim, x0, u0, xr.cols(start_idx, end_idx)); + + controlled_system.Simulate(u0); + x0 = controlled_system.x(); } auto t2 = std::chrono::high_resolution_clock::now(); diff --git a/include/ldsCtrlEst.in b/include/ldsCtrlEst.in index b1c918d0..9b6da3b4 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" diff --git a/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h new file mode 100644 index 00000000..54822784 --- /dev/null +++ b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h @@ -0,0 +1,58 @@ +//===-- 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; + + // getters + + // setters + using lds::MpcController::set_control; + using lds::MpcController::set_constraint; +}; +} // 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..bcbc6d50 --- /dev/null +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -0,0 +1,533 @@ +//===-- 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.h" +#include "osqp_api_constants.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 + * @param u_lb The lower bound of the control input + * @param u_ub The upper bound of the control input + * + * @tparam System The system type + */ + MpcController(const System& sys, Vector u_lb, Vector u_ub); + + /** + * @brief Constructs a new MpcController. + * + * @param sys The system being controlled + * @param u_lb The lower bound of the control input + * @param u_ub The upper bound of the control input + * + * @tparam System The system type + */ + MpcController(System&& sys, Vector u_lb, Vector u_ub); + + /** + * + */ + Vector Control(data_t t_sim, const Vector& x0, const Vector& u0, + const Matrix& xr); + + // getters + + // setters + void set_control(Matrix Q, Matrix R, Matrix S, size_t N, size_t M) { + 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_ = arma::trimatu(2 * block_diag(Px, Pu)); // Taking only the upper triangular part + } + + void set_constraint(Vector xmin, Vector xmax, Vector umin, Vector umax) { + 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()); + size_t Aineq_dim = N_ * n_ + M_ * m_; + Aineq_ = arma::eye>(Aineq_dim, Aineq_dim); + } + + 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 P_; ///< penalty matrix + Matrix Q_; + Matrix S_; + + Matrix lineq_; ///< lower inequality bound + Matrix uineq_; ///< upper inequality bound + arma::SpMat Aineq_; ///< inequality condition matrix + + Matrix Acon_; ///< update condition matrix + Vector lb_; ///< lower bound + Vector ub_; ///< upper bound + + Vector xi_; ///< previous step end state + size_t t_sim_; ///< previous step simulation time step + + private: + /** + * @brief Set the matrix for the OSQP solver from an Armadillo matrix + * + * @param A The matrix to convert + */ + OSQPCscMatrix* from_matrix(const Matrix& A); + + /** + * @brief Set the matrix for the OSQP solver from an Armadillo sparse + * matrix + * + * @param A The sparse matrix to convert + */ + OSQPCscMatrix* from_sparse(const arma::SpMat& A); + + /** + * @brief Set the vector for the OSQP solver from an Armadillo vector + * + * @param v The vector to convert + */ + OSQPFloat* from_vec(const Vector& v); + + /** + * @brief Calculate the trajectory for the simulation step, + * used when the simulation time step is the same as + * the prior 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 + */ + OSQPSolution* fast_update(const Vector& x0, const Vector& u0, + const Matrix& xr, size_t n_sim); + + /** + * @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 + */ + OSQPSolution* slow_update(const Vector& x0, const Vector& u0, + const Matrix& xr, 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 + */ + arma::SpMat eye_offset(size_t n, int k = -1) { + arma::SpMat mat(n, n); + int start = (k < 0) ? -k : 0; + for (int i = start; i < n && i + k < n; i++) { + mat(i, i + k) = 1; + } + 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 print_osqp_csc_matrix(const OSQPCscMatrix* matrix) { + // std::cout << "[" << matrix->m << ", " << matrix->n << "]:\n"; + + // for (int j = 0, k = 0; j < (matrix->p[matrix->n]); j++) { + // while (k <= matrix->n && matrix->p[k + 1] <= j) k++; + // std::cout << "(" << matrix->i[j] << ", " << k << ") " << matrix->x[j] << "\n"; + // } + // std::cout << std::endl; + // } + + void Init() { + A_ = sys_.A(); + B_ = sys_.B(); + n_ = B_.n_rows; + m_ = B_.n_cols; + xi_ = arma::zeros(n_); + t_sim_ = 0; + } +}; + +// Implement methods + +template +inline MpcController::MpcController(const System& sys, Vector u_lb, + Vector u_ub) + : sys_(sys), lb_(u_lb), ub_(u_ub) { + Init(); +} + +template +inline MpcController::MpcController(System&& sys, Vector u_lb, + Vector u_ub) + : sys_(std::move(sys)), lb_(u_lb), ub_(u_ub) { + Init(); +} + +template +Vector MpcController::Control(data_t t_sim, const Vector& x0, + const Vector& u0, const Matrix& xr) { + size_t n_sim = t_sim / sys_.dt(); // Number of points per simulation step + + OSQPSolution* sol; + if (arma::all(xi_ == x0) && (t_sim_ == t_sim)) { + sol = fast_update(x0, u0, xr, n_sim); + } else { + sol = slow_update(x0, u0, xr, n_sim); + } + t_sim_ = t_sim; + + Vector ui(m_); + for (int i = 0; i < m_; i++) { + ui(i) = sol->x[N_ * n_ + i]; + } + + if (sol) free(sol); + + return ui; +} + +template +OSQPSolution* MpcController::fast_update(const Vector& x0, + const Vector& u0, + const Matrix& xr, + size_t n_sim) { + lb_.rows(0, n_ - 1) = -x0.t(); + ub_.rows(0, n_ - 1) = -x0.t(); + + // Convert state penalty from reference to OSQP format + arma::Row q_arma; + { + 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; + arma::Row Qxr = + Qxr_full.as_col().t(); // Qxr for every simulation time step + + arma::Row qu = + join_horiz(-2 * S_ * u0, arma::zeros((M_ - 1) * m_)); + arma::Row qx = Qxr.cols(0, N_ * n_ - 1); + q_arma = join_horiz(qx, qu); + } + + // Solver, settings, matrices + OSQPSolver* solver; + OSQPSettings* settings; + OSQPCscMatrix* A = from_matrix(Acon_); + OSQPCscMatrix* P = from_matrix(P_); + OSQPFloat* q = from_vec(q_arma.t()); + OSQPFloat* lb = from_vec(lb_); + OSQPFloat* ub = from_vec(ub_); + + // Set settings + settings = (OSQPSettings*)malloc(sizeof(OSQPSettings)); + if (settings) { + osqp_set_default_settings(settings); + settings->verbose = false; + } + + // Set up solver + OSQPInt exit_flag = osqp_setup(&solver, P, q, A, lb, ub, A->m, A->n, settings); // n and m are set by A cols and rows + + if (solver->info->status_val != OSQP_SOLVED) { + std::string error_message = "OSQP solver failed to solve."; + throw std::runtime_error(error_message); + } + + // Solve problem + if (!exit_flag) exit_flag = osqp_solve(solver); + + OSQPSolution* sol = std::move(solver->solution); + + // Clean up + osqp_cleanup(solver); + if (q) free(q); + if (lb) free(lb); + if (ub) free(ub); + if (A) free(A); + if (P) free(P); + if (settings) free(settings); + + return sol; +} + +template +OSQPSolution* MpcController::slow_update(const Vector& x0, + const Vector& u0, + const Matrix& xr, + size_t n_sim) { + Matrix leq = join_horiz( + -x0.t(), arma::zeros((N_ - 1) * n_).t()); // Lower equality bound + Matrix ueq = leq; // Upper equality bound + + // 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_), arma::SpMat(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 + lb_ = join_horiz(leq, lineq_).t(); // Lower bound + ub_ = join_horiz(ueq, uineq_).t(); // Upper bound + + // Convert state penalty from reference to OSQP format + Vector q_arma; + { + 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_arma = join_vert(qx, qu); + + } + + // Problem values for OSQP + OSQPFloat* q = from_vec(q_arma); + OSQPFloat* lb = from_vec(lb_); + OSQPFloat* ub = from_vec(ub_); + + // Solver, settings, matrices + OSQPSolver* solver; + OSQPSettings* settings; + OSQPCscMatrix* A = from_matrix(Acon_); + OSQPCscMatrix* P = from_matrix(P_); + + // Set settings + settings = (OSQPSettings*)malloc(sizeof(OSQPSettings)); + if (settings) { + osqp_set_default_settings(settings); + settings->verbose = false; + settings->warm_starting = false; + } + + // Set up solver + OSQPInt exit_flag = osqp_setup(&solver, P, q, A, lb, ub, A->m, A->n, settings); // n and m are set by A cols and rows + + // Solve problem + if (!exit_flag) exit_flag = osqp_solve(solver); + + if (solver->info->status_val != OSQP_SOLVED) { + std::string error_message = "OSQP solver failed to solve."; + throw std::runtime_error(error_message); + } + + OSQPSolution* sol = std::move(solver->solution); + + // Clean up + osqp_cleanup(solver); + if (q) free(q); + if (lb) free(lb); + if (ub) free(ub); + if (A->i) free(A->i); + if (A->p) free(A->p); + if (A->x) free(A->x); + if (A) free(A); + if (P->i) free(P->i); + if (P->p) free(P->p); + if (P->x) free(P->x); + if (P) free(P); + if (settings) free(settings); + + return sol; +} + +template +OSQPCscMatrix* MpcController::from_matrix(const Matrix& A) { + OSQPCscMatrix* mat = (OSQPCscMatrix*)malloc(sizeof(OSQPCscMatrix)); + + mat->m = A.n_rows; + mat->n = A.n_cols; + mat->nzmax = A.n_rows * A.n_cols; // Can be made smaller if needed + mat->nz = -1; // -1 means the matrix is in CSC format (required for API) + + mat->p = (OSQPInt*)malloc((mat->n + 1) * sizeof(OSQPInt)); + mat->i = (OSQPInt*)malloc(mat->nzmax * sizeof(OSQPInt)); + mat->x = (OSQPFloat*)malloc(mat->nzmax * sizeof(OSQPFloat)); + + if (!mat->p || !mat->i || !mat->x) { + std::string error_message = "Failed to allocate memory for OSQP matrix."; + throw std::runtime_error(error_message); + } + + OSQPInt n = 0; + mat->p[0] = 0; + for (OSQPInt j = 0; j < mat->n; j++) { + for (OSQPInt i = 0; i < mat->m; i++) { + if (A(i, j) != 0) { + mat->i[n] = i; + mat->x[n] = A(i, j); + n++; + } + } + mat->p[j + 1] = n; + } + + return mat; +} + +template +OSQPCscMatrix* MpcController::from_sparse( + const arma::SpMat& A) { + OSQPCscMatrix* mat = (OSQPCscMatrix*)malloc(sizeof(OSQPCscMatrix)); + + mat->m = A.n_rows; + mat->n = A.n_cols; + mat->nzmax = A.n_nonzero; + mat->nz = -1; // -1 means the matrix is in CSC format (required for API) + + mat->p = (OSQPInt*)malloc((mat->n + 1) * sizeof(OSQPInt)); + mat->i = (OSQPInt*)malloc(mat->nzmax * sizeof(OSQPInt)); + mat->x = (OSQPFloat*)malloc(mat->nzmax * sizeof(OSQPFloat)); + + if (!mat->p || !mat->i || !mat->x) { + std::string error_message = "Failed to allocate memory for OSQP matrix."; + throw std::runtime_error(error_message); + } + + arma::sp_mat::iterator it = A.begin(); + for (OSQPInt j = 0; it != A.end(); ++it, ++j) { + mat->i[j] = it.row(); + mat->x[j] = *it; + mat->p[it.col() + 1] = j; + } + + mat->p[0] = 0; + for (OSQPInt j = 0; j < mat->n; j++) { + // Set values for all empty columns + if (mat->p[j + 1] == 0) { + mat->p[j + 1] = mat->p[j]; + } + } + mat->p[mat->n] = mat->nzmax; + + return mat; +} + +template +OSQPFloat* MpcController::from_vec(const Vector& v) { + OSQPFloat* arr = (OSQPFloat*)malloc(v.n_elem * sizeof(OSQPFloat)); + + for (OSQPInt i = 0; i < v.n_elem; i++) { + arr[i] = v(i); + } + + return arr; +} + +} // namespace lds + +#endif \ No newline at end of file From 7f4976fd1f55308c29e7332a131ed984d3f63163 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Fri, 14 Jun 2024 16:30:29 -0700 Subject: [PATCH 07/26] Renamed sparse matrices Defined and renamed arma::SpMat to the Sparse shorthand --- include/ldsCtrlEst_h/lds.h | 1 + include/ldsCtrlEst_h/lds_mpcctrl.h | 18 +++++++++--------- 2 files changed, 10 insertions(+), 9 deletions(-) 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_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index bcbc6d50..b033f434 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -105,7 +105,7 @@ class MpcController { uineq_ = join_horiz(arma::kron(Vector(N_, arma::fill::ones), xmax).t(), arma::kron(Vector(M_, arma::fill::ones), umax).t()); size_t Aineq_dim = N_ * n_ + M_ * m_; - Aineq_ = arma::eye>(Aineq_dim, Aineq_dim); + Aineq_ = arma::eye Aineq_; ///< inequality condition matrix + Sparse Aineq_; ///< inequality condition matrix Matrix Acon_; ///< update condition matrix Vector lb_; ///< lower bound @@ -150,7 +150,7 @@ class MpcController { * * @param A The sparse matrix to convert */ - OSQPCscMatrix* from_sparse(const arma::SpMat& A); + OSQPCscMatrix* from_sparse(const Sparse& A); /** * @brief Set the vector for the OSQP solver from an Armadillo vector @@ -195,8 +195,8 @@ class MpcController { * * @return The identity matrix with an offset axis */ - arma::SpMat eye_offset(size_t n, int k = -1) { - arma::SpMat mat(n, n); + Sparse eye_offset(size_t n, int k = -1) { + Sparse mat(n, n); int start = (k < 0) ? -k : 0; for (int i = start; i < n && i + k < n; i++) { mat(i, i + k) = 1; @@ -366,9 +366,9 @@ OSQPSolution* MpcController::slow_update(const Vector& x0, } // Ax + Bu = 0 - Matrix Ax(arma::kron(arma::speye>(N_, N_), - -arma::speye>(n_, n_)) + - arma::kron(eye_offset(N_), arma::SpMat(Axs))); + 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), @@ -481,7 +481,7 @@ OSQPCscMatrix* MpcController::from_matrix(const Matrix& A) { template OSQPCscMatrix* MpcController::from_sparse( - const arma::SpMat& A) { + const Sparse& A) { OSQPCscMatrix* mat = (OSQPCscMatrix*)malloc(sizeof(OSQPCscMatrix)); mat->m = A.n_rows; From 0d7fe2eebd173f2fdaeede882d00418efd36b977 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Mon, 17 Jun 2024 22:16:41 -0700 Subject: [PATCH 08/26] Refactored OSQP to a new osqp_arma.h file, which acts as a partial wrapper for OSQP that interfaces with Armadillo matrices --- examples/eg_lqmpc_ctrl.cpp | 20 +- include/ldsCtrlEst_h/lds_mpcctrl.h | 297 ++++------------- include/ldsCtrlEst_h/osqp_arma.h | 512 +++++++++++++++++++++++++++++ 3 files changed, 592 insertions(+), 237 deletions(-) create mode 100644 include/ldsCtrlEst_h/osqp_arma.h diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index f59103ac..f7652578 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -1,4 +1,5 @@ -//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control ---------------------------===// +//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control +//---------------------------===// // // Copyright 2024 Chia-Chien Hung and Kyle Johnsen // Copyright 2024 Georgia Institute of Technology @@ -65,7 +66,6 @@ auto main() -> int { C = arma::join_cols(C, 2 * C, 3 * C); } - // Initialize the system that is being controlled lds::gaussian::System controlled_system(n_u, n_x, n_y, dt); controlled_system.set_A(A); @@ -84,8 +84,10 @@ auto main() -> int { const size_t N = 25; // Prediction 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 + 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}; @@ -102,10 +104,10 @@ auto main() -> int { 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), umin, umax)); + controller = std::move( + lds::gaussian::MpcController(std::move(controller_system), umin, umax)); controller.set_control(Q, R, S, N, 20); controller.set_constraint(xmin, xmax, umin, umax); } @@ -120,7 +122,6 @@ auto main() -> int { 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; @@ -138,7 +139,7 @@ auto main() -> int { // 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 = int(t_sim / dt); + const size_t n_sim = static_cast(t_sim / dt); for (size_t t = 0; t < n_t; ++t) { // Calculate the slice indices @@ -149,6 +150,9 @@ auto main() -> int { controlled_system.Simulate(u0); x0 = controlled_system.x(); + + u0.brief_print("u0"); + x0.brief_print("x0"); } auto t2 = std::chrono::high_resolution_clock::now(); diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index b033f434..408f8b88 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -1,5 +1,4 @@ -//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - MPC Controller ------*- C++ -//-*-===// +//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - MPC Controller ----*- C++ -*-===// // // Copyright 2024 Chia-Chien Hung and Kyle Johnsen // Copyright 2024 Georgia Institute of Technology @@ -36,8 +35,7 @@ #include "lds_sys.h" // osqp -#include "osqp.h" -#include "osqp_api_constants.h" +#include "osqp_arma.h" namespace lds { @@ -91,12 +89,16 @@ class MpcController { 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 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_ = arma::trimatu(2 * block_diag(Px, Pu)); // Taking only the upper triangular part + P_ = Sparse(arma::trimatu( + 2 * block_diag(Px, Pu))); // Taking only the upper triangular part + P_.brief_print("P_:"); } void set_constraint(Vector xmin, Vector xmax, Vector umin, Vector umax) { @@ -105,7 +107,7 @@ class MpcController { uineq_ = join_horiz(arma::kron(Vector(N_, arma::fill::ones), xmax).t(), arma::kron(Vector(M_, arma::fill::ones), umax).t()); size_t Aineq_dim = N_ * n_ + M_ * m_; - Aineq_ = arma::eye(Aineq_dim, Aineq_dim); } void Print() { @@ -121,12 +123,12 @@ class MpcController { size_t M_; ///< number of inputs Matrix A_; ///< state transition matrix Matrix B_; ///< input matrix - Matrix P_; ///< penalty matrix + Sparse P_; ///< penalty matrix Matrix Q_; Matrix S_; - Matrix lineq_; ///< lower inequality bound - Matrix uineq_; ///< upper inequality bound + Matrix lineq_; ///< lower inequality bound + Matrix uineq_; ///< upper inequality bound Sparse Aineq_; ///< inequality condition matrix Matrix Acon_; ///< update condition matrix @@ -137,28 +139,6 @@ class MpcController { size_t t_sim_; ///< previous step simulation time step private: - /** - * @brief Set the matrix for the OSQP solver from an Armadillo matrix - * - * @param A The matrix to convert - */ - OSQPCscMatrix* from_matrix(const Matrix& A); - - /** - * @brief Set the matrix for the OSQP solver from an Armadillo sparse - * matrix - * - * @param A The sparse matrix to convert - */ - OSQPCscMatrix* from_sparse(const Sparse& A); - - /** - * @brief Set the vector for the OSQP solver from an Armadillo vector - * - * @param v The vector to convert - */ - OSQPFloat* from_vec(const Vector& v); - /** * @brief Calculate the trajectory for the simulation step, * used when the simulation time step is the same as @@ -171,8 +151,8 @@ class MpcController { * * @return The trajectory for the simulation step */ - OSQPSolution* fast_update(const Vector& x0, const Vector& u0, - const Matrix& xr, size_t n_sim); + osqp_arma::Solution* fast_update(const Vector& x0, const Vector& u0, + const Matrix& xr, size_t n_sim); /** * @brief Calculate the trajectory for the simulation step @@ -184,32 +164,29 @@ class MpcController { * * @return The trajectory for the simulation step */ - OSQPSolution* slow_update(const Vector& x0, const Vector& u0, - const Matrix& xr, size_t n_sim); + osqp_arma::Solution* slow_update(const Vector& x0, const Vector& u0, + const Matrix& xr, 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); - int start = (k < 0) ? -k : 0; - for (int i = start; i < n && i + k < n; i++) { - mat(i, i + k) = 1; - } + 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) { @@ -230,7 +207,8 @@ class MpcController { // for (int j = 0, k = 0; j < (matrix->p[matrix->n]); j++) { // while (k <= matrix->n && matrix->p[k + 1] <= j) k++; - // std::cout << "(" << matrix->i[j] << ", " << k << ") " << matrix->x[j] << "\n"; + // std::cout << "(" << matrix->i[j] << ", " << k << ") " << matrix->x[j] + // << "\n"; // } // std::cout << std::endl; // } @@ -248,15 +226,14 @@ class MpcController { // Implement methods template -inline MpcController::MpcController(const System& sys, Vector u_lb, - Vector u_ub) +MpcController::MpcController(const System& sys, Vector u_lb, + Vector u_ub) : sys_(sys), lb_(u_lb), ub_(u_ub) { Init(); } template -inline MpcController::MpcController(System&& sys, Vector u_lb, - Vector u_ub) +MpcController::MpcController(System&& sys, Vector u_lb, Vector u_ub) : sys_(std::move(sys)), lb_(u_lb), ub_(u_ub) { Init(); } @@ -266,7 +243,7 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, const Vector& u0, const Matrix& xr) { size_t n_sim = t_sim / sys_.dt(); // Number of points per simulation step - OSQPSolution* sol; + osqp_arma::Solution* sol; if (arma::all(xi_ == x0) && (t_sim_ == t_sim)) { sol = fast_update(x0, u0, xr, n_sim); } else { @@ -276,7 +253,7 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, Vector ui(m_); for (int i = 0; i < m_; i++) { - ui(i) = sol->x[N_ * n_ + i]; + ui(i) = sol->x(N_ * n_ + i); } if (sol) free(sol); @@ -285,15 +262,15 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, } template -OSQPSolution* MpcController::fast_update(const Vector& x0, - const Vector& u0, - const Matrix& xr, - size_t n_sim) { +osqp_arma::Solution* MpcController::fast_update(const Vector& x0, + const Vector& u0, + const Matrix& xr, + size_t n_sim) { lb_.rows(0, n_ - 1) = -x0.t(); ub_.rows(0, n_ - 1) = -x0.t(); // Convert state penalty from reference to OSQP format - arma::Row q_arma; + arma::Row q; { arma::uvec indices = arma::regspace(0, n_sim, N_ * n_sim - 1); Matrix sliced_xr = xr.cols(indices); @@ -304,55 +281,28 @@ OSQPSolution* MpcController::fast_update(const Vector& x0, arma::Row qu = join_horiz(-2 * S_ * u0, arma::zeros((M_ - 1) * m_)); arma::Row qx = Qxr.cols(0, N_ * n_ - 1); - q_arma = join_horiz(qx, qu); - } - - // Solver, settings, matrices - OSQPSolver* solver; - OSQPSettings* settings; - OSQPCscMatrix* A = from_matrix(Acon_); - OSQPCscMatrix* P = from_matrix(P_); - OSQPFloat* q = from_vec(q_arma.t()); - OSQPFloat* lb = from_vec(lb_); - OSQPFloat* ub = from_vec(ub_); - - // Set settings - settings = (OSQPSettings*)malloc(sizeof(OSQPSettings)); - if (settings) { - osqp_set_default_settings(settings); - settings->verbose = false; + q = join_horiz(qx, qu); } - // Set up solver - OSQPInt exit_flag = osqp_setup(&solver, P, q, A, lb, ub, A->m, A->n, settings); // n and m are set by A cols and rows + osqp_arma::OSQP* OSQP = new osqp_arma::OSQP(); - if (solver->info->status_val != OSQP_SOLVED) { - std::string error_message = "OSQP solver failed to solve."; - throw std::runtime_error(error_message); - } - - // Solve problem - if (!exit_flag) exit_flag = osqp_solve(solver); + // set settings + OSQP->set_default_settings(); + OSQP->set_verbose(false); - OSQPSolution* sol = std::move(solver->solution); + // set problem + OSQP->setup(P_, q, Acon_, lb_, ub_); - // Clean up - osqp_cleanup(solver); - if (q) free(q); - if (lb) free(lb); - if (ub) free(ub); - if (A) free(A); - if (P) free(P); - if (settings) free(settings); + osqp_arma::Solution* sol = OSQP->solve(); return sol; } template -OSQPSolution* MpcController::slow_update(const Vector& x0, - const Vector& u0, - const Matrix& xr, - size_t n_sim) { +osqp_arma::Solution* MpcController::slow_update(const Vector& x0, + const Vector& u0, + const Matrix& xr, + size_t n_sim) { Matrix leq = join_horiz( -x0.t(), arma::zeros((N_ - 1) * n_).t()); // Lower equality bound Matrix ueq = leq; // Upper equality bound @@ -365,10 +315,13 @@ OSQPSolution* MpcController::slow_update(const Vector& x0, Aus += arma::powmat(A_, i); } + Axs.brief_print("Axs:"); + Aus.brief_print("Aus:"); + // Ax + Bu = 0 - Matrix Ax(arma::kron(arma::speye(N_, N_), - -arma::speye(n_, n_)) + - arma::kron(eye_offset(N_), Sparse(Axs))); + 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), @@ -380,8 +333,12 @@ OSQPSolution* MpcController::slow_update(const Vector& x0, lb_ = join_horiz(leq, lineq_).t(); // Lower bound ub_ = join_horiz(ueq, uineq_).t(); // Upper bound + Acon_.print("Acon:"); + lb_.brief_print("lb:"); + ub_.brief_print("ub:"); + // Convert state penalty from reference to OSQP format - Vector q_arma; + Vector q; { arma::uvec indices = arma::regspace(0, n_sim, N_ * n_sim - 1); Matrix sliced_xr = xr.cols(indices); @@ -391,143 +348,25 @@ OSQPSolution* MpcController::slow_update(const Vector& x0, Vector qu = join_vert((-2 * S_ * u0), Vector((M_ - 1) * m_, arma::fill::zeros)); Vector qx = Qxr.rows(0, N_ * n_ - 1); - q_arma = join_vert(qx, qu); - + q = join_vert(qx, qu); } - // Problem values for OSQP - OSQPFloat* q = from_vec(q_arma); - OSQPFloat* lb = from_vec(lb_); - OSQPFloat* ub = from_vec(ub_); - - // Solver, settings, matrices - OSQPSolver* solver; - OSQPSettings* settings; - OSQPCscMatrix* A = from_matrix(Acon_); - OSQPCscMatrix* P = from_matrix(P_); - - // Set settings - settings = (OSQPSettings*)malloc(sizeof(OSQPSettings)); - if (settings) { - osqp_set_default_settings(settings); - settings->verbose = false; - settings->warm_starting = false; - } + q.brief_print("q:"); - // Set up solver - OSQPInt exit_flag = osqp_setup(&solver, P, q, A, lb, ub, A->m, A->n, settings); // n and m are set by A cols and rows + osqp_arma::OSQP* OSQP = new osqp_arma::OSQP(); - // Solve problem - if (!exit_flag) exit_flag = osqp_solve(solver); + // set settings + OSQP->set_default_settings(); + OSQP->set_verbose(false); - if (solver->info->status_val != OSQP_SOLVED) { - std::string error_message = "OSQP solver failed to solve."; - throw std::runtime_error(error_message); - } + // set problem + OSQP->setup(P_, q, Acon_, lb_, ub_); - OSQPSolution* sol = std::move(solver->solution); - - // Clean up - osqp_cleanup(solver); - if (q) free(q); - if (lb) free(lb); - if (ub) free(ub); - if (A->i) free(A->i); - if (A->p) free(A->p); - if (A->x) free(A->x); - if (A) free(A); - if (P->i) free(P->i); - if (P->p) free(P->p); - if (P->x) free(P->x); - if (P) free(P); - if (settings) free(settings); + osqp_arma::Solution* sol = OSQP->solve(); return sol; } -template -OSQPCscMatrix* MpcController::from_matrix(const Matrix& A) { - OSQPCscMatrix* mat = (OSQPCscMatrix*)malloc(sizeof(OSQPCscMatrix)); - - mat->m = A.n_rows; - mat->n = A.n_cols; - mat->nzmax = A.n_rows * A.n_cols; // Can be made smaller if needed - mat->nz = -1; // -1 means the matrix is in CSC format (required for API) - - mat->p = (OSQPInt*)malloc((mat->n + 1) * sizeof(OSQPInt)); - mat->i = (OSQPInt*)malloc(mat->nzmax * sizeof(OSQPInt)); - mat->x = (OSQPFloat*)malloc(mat->nzmax * sizeof(OSQPFloat)); - - if (!mat->p || !mat->i || !mat->x) { - std::string error_message = "Failed to allocate memory for OSQP matrix."; - throw std::runtime_error(error_message); - } - - OSQPInt n = 0; - mat->p[0] = 0; - for (OSQPInt j = 0; j < mat->n; j++) { - for (OSQPInt i = 0; i < mat->m; i++) { - if (A(i, j) != 0) { - mat->i[n] = i; - mat->x[n] = A(i, j); - n++; - } - } - mat->p[j + 1] = n; - } - - return mat; -} - -template -OSQPCscMatrix* MpcController::from_sparse( - const Sparse& A) { - OSQPCscMatrix* mat = (OSQPCscMatrix*)malloc(sizeof(OSQPCscMatrix)); - - mat->m = A.n_rows; - mat->n = A.n_cols; - mat->nzmax = A.n_nonzero; - mat->nz = -1; // -1 means the matrix is in CSC format (required for API) - - mat->p = (OSQPInt*)malloc((mat->n + 1) * sizeof(OSQPInt)); - mat->i = (OSQPInt*)malloc(mat->nzmax * sizeof(OSQPInt)); - mat->x = (OSQPFloat*)malloc(mat->nzmax * sizeof(OSQPFloat)); - - if (!mat->p || !mat->i || !mat->x) { - std::string error_message = "Failed to allocate memory for OSQP matrix."; - throw std::runtime_error(error_message); - } - - arma::sp_mat::iterator it = A.begin(); - for (OSQPInt j = 0; it != A.end(); ++it, ++j) { - mat->i[j] = it.row(); - mat->x[j] = *it; - mat->p[it.col() + 1] = j; - } - - mat->p[0] = 0; - for (OSQPInt j = 0; j < mat->n; j++) { - // Set values for all empty columns - if (mat->p[j + 1] == 0) { - mat->p[j + 1] = mat->p[j]; - } - } - mat->p[mat->n] = mat->nzmax; - - return mat; -} - -template -OSQPFloat* MpcController::from_vec(const Vector& v) { - OSQPFloat* arr = (OSQPFloat*)malloc(v.n_elem * sizeof(OSQPFloat)); - - for (OSQPInt i = 0; i < v.n_elem; i++) { - arr[i] = v(i); - } - - return arr; -} - } // namespace lds #endif \ No newline at end of file diff --git a/include/ldsCtrlEst_h/osqp_arma.h b/include/ldsCtrlEst_h/osqp_arma.h new file mode 100644 index 00000000..407a320d --- /dev/null +++ b/include/ldsCtrlEst_h/osqp_arma.h @@ -0,0 +1,512 @@ +//===-- 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, 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); + } + + /** + * @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_; } + + protected: + arma::vec x_; + arma::vec y_; + arma::vec prim_inf_cert_; + arma::vec dual_inf_cert_; +}; // 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; + } + + 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, 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_problem_ = true; + } + + void set_A(arma::mat A) { + A_ = from_matrix(A); + 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 + + OSQPSettings* settings; + bool updated_settings_ = true; // whether any settings have been updated +}; // class OSQP +} // namespace osqp_arma + +#endif // LDSCTRLEST_OSQP_ARMA_H From 86c9baf81ae90c7f4f4b32ad160c61c410f90636 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Mon, 17 Jun 2024 22:20:30 -0700 Subject: [PATCH 09/26] Added .idea to .gitignore for Jetbrains IDEs --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 2f2f72a1..5d184b60 100755 --- a/.gitignore +++ b/.gitignore @@ -102,6 +102,7 @@ cpp.sublime-workspace .vs/ .vscode/ .cache/ +.idea/ # whitelist documentation !docs/** From 1a73ea54952188d56a22c594883d66436f0a9b4d Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Tue, 18 Jun 2024 13:54:28 -0700 Subject: [PATCH 10/26] Stored data and began adding visualization to example --- examples/eg_lqmpc_ctrl.cpp | 44 ++++++++++++++++++++++++++++-- include/ldsCtrlEst_h/lds_mpcctrl.h | 22 +-------------- 2 files changed, 42 insertions(+), 24 deletions(-) diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index f7652578..fd9e2186 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -136,6 +136,23 @@ auto main() -> int { 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 = Matrix(n_y, n_t, arma::fill::zeros); + + // simulated control signal + 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_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); + Matrix m_true(n_x, 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(); @@ -151,8 +168,17 @@ auto main() -> int { controlled_system.Simulate(u0); x0 = controlled_system.x(); - u0.brief_print("u0"); - x0.brief_print("x0"); + // save the signals + u.col(t) = u0; + y_ref.col(t) = xr.col(start_idx); // FIXME: only using first state as reference output + + y_hat.col(t) = controller.sys().y(); + x_hat.col(t) = controller.sys().x(); + m_hat.col(t) = controller.sys().m(); + + y_true.col(t) = controlled_system.y(); + x_true.col(t) = controlled_system.x(); + m_true.col(t) = controlled_system.m(); } auto t2 = std::chrono::high_resolution_clock::now(); @@ -162,7 +188,19 @@ auto main() -> int { cout << "Saving simulation data to disk.\n"; - // TODO: Implement saving simulation data to disk + // 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_ctrl.h5", "dt")); + y_ref.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_ref", replace)); + u.save(arma::hdf5_name("eg_glds_ctrl.h5", "u", replace)); + x_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_true", replace)); + m_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_true", replace)); + y_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_true", replace)); + x_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_hat", replace)); + m_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_hat", replace)); + y_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_hat", replace)); cout << "fin.\n"; return 0; diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 408f8b88..ae8c3dd6 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -79,6 +79,7 @@ class MpcController { const Matrix& xr); // getters + const System& sys() const { return sys_; } // setters void set_control(Matrix Q, Matrix R, Matrix S, size_t N, size_t M) { @@ -98,7 +99,6 @@ class MpcController { Matrix Pu = Pu1 + Pu2 + Pu3; P_ = Sparse(arma::trimatu( 2 * block_diag(Px, Pu))); // Taking only the upper triangular part - P_.brief_print("P_:"); } void set_constraint(Vector xmin, Vector xmax, Vector umin, Vector umax) { @@ -202,17 +202,6 @@ class MpcController { return bd; } - // void print_osqp_csc_matrix(const OSQPCscMatrix* matrix) { - // std::cout << "[" << matrix->m << ", " << matrix->n << "]:\n"; - - // for (int j = 0, k = 0; j < (matrix->p[matrix->n]); j++) { - // while (k <= matrix->n && matrix->p[k + 1] <= j) k++; - // std::cout << "(" << matrix->i[j] << ", " << k << ") " << matrix->x[j] - // << "\n"; - // } - // std::cout << std::endl; - // } - void Init() { A_ = sys_.A(); B_ = sys_.B(); @@ -315,9 +304,6 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, Aus += arma::powmat(A_, i); } - Axs.brief_print("Axs:"); - Aus.brief_print("Aus:"); - // Ax + Bu = 0 Matrix Ax( arma::kron(arma::speye(N_, N_), -arma::speye(n_, n_)) + @@ -333,10 +319,6 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, lb_ = join_horiz(leq, lineq_).t(); // Lower bound ub_ = join_horiz(ueq, uineq_).t(); // Upper bound - Acon_.print("Acon:"); - lb_.brief_print("lb:"); - ub_.brief_print("ub:"); - // Convert state penalty from reference to OSQP format Vector q; { @@ -351,8 +333,6 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, q = join_vert(qx, qu); } - q.brief_print("q:"); - osqp_arma::OSQP* OSQP = new osqp_arma::OSQP(); // set settings From 6f4a300763a464a4ea8c55c4e56a3a04e47938cb Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 26 Jun 2024 13:31:43 -0700 Subject: [PATCH 11/26] Fixed problems with example script and added lqmpc plotting Matlab script --- examples/eg_lqmpc_ctrl.cpp | 49 ++++++++++--------------------- examples/plot_lqmpc_ctrl_output.m | 37 +++++++++++++++++++++++ 2 files changed, 53 insertions(+), 33 deletions(-) create mode 100644 examples/plot_lqmpc_ctrl_output.m diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index fd9e2186..dd7f4f46 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -28,6 +28,7 @@ using lds::data_t; using lds::Matrix; using lds::Vector; +using lds::Sparse; using std::cout; auto main() -> int { @@ -124,7 +125,10 @@ auto main() -> int { 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 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); @@ -138,20 +142,8 @@ auto main() -> int { // create Matrix to save outputs in... Matrix y_ref = Matrix(n_y, n_t, arma::fill::zeros); - - // simulated control signal - 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_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); - Matrix m_true(n_x, n_t, arma::fill::zeros); + Matrix u(n_u, n_t, arma::fill::zeros); // Simulate the system cout << "Starting " << n_t * t_sim << " sec simulation ... \n"; @@ -165,20 +157,16 @@ auto main() -> int { u0 = controller.Control(t_sim, x0, u0, xr.cols(start_idx, end_idx)); - controlled_system.Simulate(u0); + for (size_t i = 0; i < n_sim; i++) { + controlled_system.Simulate(u0); + } + x0 = controlled_system.x(); // save the signals - u.col(t) = u0; - y_ref.col(t) = xr.col(start_idx); // FIXME: only using first state as reference output - - y_hat.col(t) = controller.sys().y(); - x_hat.col(t) = controller.sys().x(); - m_hat.col(t) = controller.sys().m(); - + y_ref.col(t) = yr.col(end_idx); y_true.col(t) = controlled_system.y(); - x_true.col(t) = controlled_system.x(); - m_true.col(t) = controlled_system.m(); + u.col(t) = u0; } auto t2 = std::chrono::high_resolution_clock::now(); @@ -192,15 +180,10 @@ auto main() -> int { arma::hdf5_opts::opts replace = arma::hdf5_opts::replace; auto dt_vec = Vector(1).fill(dt); - dt_vec.save(arma::hdf5_name("eg_glds_ctrl.h5", "dt")); - y_ref.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_ref", replace)); - u.save(arma::hdf5_name("eg_glds_ctrl.h5", "u", replace)); - x_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_true", replace)); - m_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_true", replace)); - y_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_true", replace)); - x_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_hat", replace)); - m_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_hat", replace)); - y_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_hat", replace)); + 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)); + u.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "u", replace)); cout << "fin.\n"; return 0; diff --git a/examples/plot_lqmpc_ctrl_output.m b/examples/plot_lqmpc_ctrl_output.m new file mode 100644 index 00000000..cb30222d --- /dev/null +++ b/examples/plot_lqmpc_ctrl_output.m @@ -0,0 +1,37 @@ +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'); + +% 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; + +% Plot the data + figure; + +% First graph + subplot(211); 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(212); hold on; +for i = 1:2 + stairs(t, u(i, :)', 'LineWidth', 2, 'color', c_input(i, :)); +end +ylabel({'Input'}) + +% Save the figure +printfig('eg_glds_ctrl_output', 'png', gcf, [8 8]); From a6ad1521ab432468b697f61c9c92d6496b28ce6e Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 26 Jun 2024 13:53:42 -0700 Subject: [PATCH 12/26] Added option to get cost from control, and plotted cost in example --- examples/eg_lqmpc_ctrl.cpp | 7 ++++++- examples/plot_lqmpc_ctrl_output.m | 11 +++++++++-- include/ldsCtrlEst_h/lds_mpcctrl.h | 6 ++++-- include/ldsCtrlEst_h/osqp_arma.h | 8 ++++++-- 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index dd7f4f46..3951ebfa 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -144,6 +144,7 @@ auto main() -> int { Matrix y_ref = Matrix(n_y, n_t, arma::fill::zeros); Matrix y_true(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"; @@ -155,7 +156,9 @@ auto main() -> int { size_t start_idx = t * n_sim; size_t end_idx = (t + N) * n_sim - 1; - u0 = controller.Control(t_sim, x0, u0, xr.cols(start_idx, end_idx)); + auto* j = new data_t; // cost + + u0 = controller.Control(t_sim, x0, u0, xr.cols(start_idx, end_idx), j); for (size_t i = 0; i < n_sim; i++) { controlled_system.Simulate(u0); @@ -167,6 +170,7 @@ auto main() -> int { y_ref.col(t) = yr.col(end_idx); y_true.col(t) = controlled_system.y(); u.col(t) = u0; + J.col(t).fill(*j); } auto t2 = std::chrono::high_resolution_clock::now(); @@ -184,6 +188,7 @@ auto main() -> int { 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)); 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/plot_lqmpc_ctrl_output.m b/examples/plot_lqmpc_ctrl_output.m index cb30222d..0fe73d4f 100644 --- a/examples/plot_lqmpc_ctrl_output.m +++ b/examples/plot_lqmpc_ctrl_output.m @@ -5,6 +5,7 @@ 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); @@ -14,12 +15,13 @@ 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(211); hold on; + subplot(311); hold on; for i = 1:3 plot(t, y_true(i, :)', 'LineWidth', 2, 'color', c_true(i, :)); end @@ -27,11 +29,16 @@ ylabel({'Output'}) % Second graph -subplot(212); hold on; +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_glds_ctrl_output', 'png', gcf, [8 8]); diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index ae8c3dd6..1f8ed18e 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -76,7 +76,7 @@ class MpcController { * */ Vector Control(data_t t_sim, const Vector& x0, const Vector& u0, - const Matrix& xr); + const Matrix& xr, data_t* J = NULL); // getters const System& sys() const { return sys_; } @@ -229,7 +229,8 @@ MpcController::MpcController(System&& sys, Vector u_lb, Vector u_ub) template Vector MpcController::Control(data_t t_sim, const Vector& x0, - const Vector& u0, const Matrix& xr) { + const Vector& u0, const Matrix& xr, + data_t* J) { size_t n_sim = t_sim / sys_.dt(); // Number of points per simulation step osqp_arma::Solution* sol; @@ -244,6 +245,7 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, for (int i = 0; i < m_; i++) { ui(i) = sol->x(N_ * n_ + i); } + if (J != NULL) *J = sol->obj_val(); if (sol) free(sol); diff --git a/include/ldsCtrlEst_h/osqp_arma.h b/include/ldsCtrlEst_h/osqp_arma.h index 407a320d..0375c067 100644 --- a/include/ldsCtrlEst_h/osqp_arma.h +++ b/include/ldsCtrlEst_h/osqp_arma.h @@ -186,11 +186,12 @@ OSQPFloat* from_vector(arma::vec armaVector) { class Solution { public: - Solution(OSQPSolution* sol, int n, int m) { + 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; } /** @@ -225,11 +226,14 @@ class Solution { */ 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 { @@ -286,7 +290,7 @@ class OSQP { std::to_string(exitflag)); } - return new Solution(solver->solution, n_, m_); + return new Solution(solver->solution, solver->info, n_, m_); } /** From d050d9f08541da0c53d8443264747cd0b5971376 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 26 Jun 2024 14:32:05 -0700 Subject: [PATCH 13/26] Optimized calculation of problem for slow update and fixed conditions for fast update --- include/ldsCtrlEst_h/lds_mpcctrl.h | 72 ++++++++++++++++++------------ 1 file changed, 43 insertions(+), 29 deletions(-) diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 1f8ed18e..11180302 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -35,6 +35,8 @@ #include "lds_sys.h" // osqp +#include + #include "osqp_arma.h" namespace lds { @@ -99,6 +101,8 @@ class MpcController { Matrix Pu = Pu1 + Pu2 + Pu3; P_ = Sparse(arma::trimatu( 2 * block_diag(Px, Pu))); // Taking only the upper triangular part + + upd_ctrl_ = true; } void set_constraint(Vector xmin, Vector xmax, Vector umin, Vector umax) { @@ -108,6 +112,8 @@ class MpcController { arma::kron(Vector(M_, arma::fill::ones), umax).t()); size_t Aineq_dim = N_ * n_ + M_ * m_; Aineq_ = arma::eye(Aineq_dim, Aineq_dim); + + upd_cons_ = true; } void Print() { @@ -138,6 +144,9 @@ class MpcController { Vector xi_; ///< previous step end state size_t t_sim_; ///< previous step simulation time step + bool upd_ctrl_; ///< control was updated since last step + bool upd_cons_; ///< constraint was updated since last step + private: /** * @brief Calculate the trajectory for the simulation step, @@ -245,6 +254,7 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, for (int i = 0; i < m_; i++) { ui(i) = sol->x(N_ * n_ + i); } + xi_ = A_ * x0 + B_ * ui; if (J != NULL) *J = sol->obj_val(); if (sol) free(sol); @@ -261,18 +271,17 @@ osqp_arma::Solution* MpcController::fast_update(const Vector& x0, ub_.rows(0, n_ - 1) = -x0.t(); // Convert state penalty from reference to OSQP format - arma::Row q; + 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; - arma::Row Qxr = - Qxr_full.as_col().t(); // Qxr for every simulation time step + Vector Qxr = Qxr_full.as_col(); // Qxr for every simulation time step - arma::Row qu = - join_horiz(-2 * S_ * u0, arma::zeros((M_ - 1) * m_)); - arma::Row qx = Qxr.cols(0, N_ * n_ - 1); - q = join_horiz(qx, qu); + 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); } osqp_arma::OSQP* OSQP = new osqp_arma::OSQP(); @@ -297,30 +306,35 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, Matrix leq = join_horiz( -x0.t(), arma::zeros((N_ - 1) * n_).t()); // Lower equality bound Matrix ueq = leq; // Upper equality bound - - // 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); + lb_ = join_horiz(leq, lineq_).t(); // Lower bound + ub_ = join_horiz(ueq, uineq_).t(); // Upper bound + + if (upd_ctrl_ || upd_cons_) { + // 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_cons_ = false; } - // 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 - lb_ = join_horiz(leq, lineq_).t(); // Lower bound - ub_ = join_horiz(ueq, uineq_).t(); // Upper bound - // Convert state penalty from reference to OSQP format Vector q; { From ddbe2b6a2fdfa78dd3ce86e47b77c71145e80d14 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 26 Jun 2024 14:43:52 -0700 Subject: [PATCH 14/26] Added documentation for the Control method --- include/ldsCtrlEst_h/lds_mpcctrl.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 11180302..24fedef6 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -75,7 +75,15 @@ class MpcController { MpcController(System&& sys, Vector u_lb, Vector u_ub); /** + * @brief Perform one control step. * + * @param t_sim Simulation time step + * @param x0 Initial state (n) + * @param u0 Initial input (m) + * @param xr Reference/Target state (n x N*n_sim) + * @param J Pointer to variable storing cost + * + * @return A vector of the optimal control */ Vector Control(data_t t_sim, const Vector& x0, const Vector& u0, const Matrix& xr, data_t* J = NULL); From 48bdd71f79dae37dfec89a5c78e7ac58aec0e01e Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Thu, 27 Jun 2024 00:18:50 -0700 Subject: [PATCH 15/26] Optimized controller so it saves the OSQP object and only sets matrices if problem is updated --- include/ldsCtrlEst_h/lds_mpcctrl.h | 30 ++++++++++++++---------------- include/ldsCtrlEst_h/osqp_arma.h | 13 ++++++++++++- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 24fedef6..746a96a0 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -35,8 +35,6 @@ #include "lds_sys.h" // osqp -#include - #include "osqp_arma.h" namespace lds { @@ -110,6 +108,8 @@ class MpcController { P_ = Sparse(arma::trimatu( 2 * block_diag(Px, Pu))); // Taking only the upper triangular part + OSQP->set_P(P_); + upd_ctrl_ = true; } @@ -130,6 +130,8 @@ class MpcController { } protected: + osqp_arma::OSQP* OSQP; + System sys_; ///< system being controlled size_t n_; ///< number of states size_t m_; ///< number of output states @@ -226,6 +228,10 @@ class MpcController { m_ = B_.n_cols; xi_ = arma::zeros(n_); t_sim_ = 0; + + OSQP = new osqp_arma::OSQP(); + OSQP->set_default_settings(); + OSQP->set_verbose(false); } }; @@ -292,14 +298,8 @@ osqp_arma::Solution* MpcController::fast_update(const Vector& x0, q = join_vert(qx, qu); } - osqp_arma::OSQP* OSQP = new osqp_arma::OSQP(); - - // set settings - OSQP->set_default_settings(); - OSQP->set_verbose(false); - // set problem - OSQP->setup(P_, q, Acon_, lb_, ub_); + OSQP->set_q(q); osqp_arma::Solution* sol = OSQP->solve(); @@ -316,6 +316,8 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, Matrix ueq = leq; // Upper equality bound lb_ = join_horiz(leq, lineq_).t(); // Lower bound ub_ = join_horiz(ueq, uineq_).t(); // Upper bound + OSQP->set_l(lb_); + OSQP->set_u(ub_); if (upd_ctrl_ || upd_cons_) { // Update x over n_sim many steps @@ -339,6 +341,8 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, Acon_ = join_vert(Aeq, Matrix(Aineq_)); // Update condition + OSQP->set_A(Acon_); + upd_ctrl_ = false; upd_cons_ = false; } @@ -357,14 +361,8 @@ osqp_arma::Solution* MpcController::slow_update(const Vector& x0, q = join_vert(qx, qu); } - osqp_arma::OSQP* OSQP = new osqp_arma::OSQP(); - - // set settings - OSQP->set_default_settings(); - OSQP->set_verbose(false); - // set problem - OSQP->setup(P_, q, Acon_, lb_, ub_); + OSQP->set_q(q); osqp_arma::Solution* sol = OSQP->solve(); diff --git a/include/ldsCtrlEst_h/osqp_arma.h b/include/ldsCtrlEst_h/osqp_arma.h index 0375c067..f94de208 100644 --- a/include/ldsCtrlEst_h/osqp_arma.h +++ b/include/ldsCtrlEst_h/osqp_arma.h @@ -282,6 +282,14 @@ class OSQP { } 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); @@ -318,11 +326,13 @@ class OSQP { void set_q(arma::vec q) { q_ = from_vector(q); - updated_problem_ = true; + updated_data_ = true; } void set_A(arma::mat A) { A_ = from_matrix(A); + n_ = A_->n; + m_ = A_->m; updated_problem_ = true; } @@ -507,6 +517,7 @@ class OSQP { 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 From 8db0a88d17c74db7cf9f6e139755e66ab938db15 Mon Sep 17 00:00:00 2001 From: Kyle Johnsen Date: Wed, 3 Jul 2024 11:55:07 -0400 Subject: [PATCH 16/26] automate OSQP build --- .gitignore | 2 ++ cmake/Modules/OSQP.cmake | 11 +++++++++++ examples/plot_lqmpc_ctrl_output.m | 2 +- 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 5d184b60..38738813 100755 --- a/.gitignore +++ b/.gitignore @@ -103,6 +103,8 @@ cpp.sublime-workspace .vscode/ .cache/ .idea/ +**/CMakeCache.txt +CMakeFiles/** # whitelist documentation !docs/** diff --git a/cmake/Modules/OSQP.cmake b/cmake/Modules/OSQP.cmake index f271271a..912e08fa 100644 --- a/cmake/Modules/OSQP.cmake +++ b/cmake/Modules/OSQP.cmake @@ -1,2 +1,13 @@ +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}) find_package(osqp REQUIRED) list(APPEND PROJECT_REQUIRED_LIBRARIES_ABSOLUTE_NAME osqp::osqp) \ No newline at end of file diff --git a/examples/plot_lqmpc_ctrl_output.m b/examples/plot_lqmpc_ctrl_output.m index 0fe73d4f..14ffb437 100644 --- a/examples/plot_lqmpc_ctrl_output.m +++ b/examples/plot_lqmpc_ctrl_output.m @@ -41,4 +41,4 @@ ylabel({'Cost'}) % Save the figure -printfig('eg_glds_ctrl_output', 'png', gcf, [8 8]); +printfig('eg_lqmpc_ctrl_output', 'png', gcf, [8 8]); From 9598e985d3e1984037795d25b6de4ebbd05e0fa5 Mon Sep 17 00:00:00 2001 From: Kyle Johnsen Date: Wed, 3 Jul 2024 11:56:44 -0400 Subject: [PATCH 17/26] removed osqp submodule --- .gitmodules | 5 +---- osqp | 1 - 2 files changed, 1 insertion(+), 5 deletions(-) delete mode 160000 osqp diff --git a/.gitmodules b/.gitmodules index 611eb5b6..15e4cb3c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,7 +10,4 @@ url = https://github.com/pybind/pybind11.git [submodule "python/carma"] path = python/carma - url = https://github.com/RUrlus/carma.git -[submodule "osqp"] - path = osqp - url = https://github.com/osqp/osqp + url = https://github.com/RUrlus/carma.git \ No newline at end of file diff --git a/osqp b/osqp deleted file mode 160000 index 4e81a9d0..00000000 --- a/osqp +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4e81a9d0d72c7523e776fdd8f32a79614ebd1a8b From accd3a96c56d50f0eacf64293ffd9f2e7bee93fe Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Thu, 4 Jul 2024 21:01:40 -0700 Subject: [PATCH 18/26] Refactored slow_update to calc_trajectory and removed less used fast_update method --- include/ldsCtrlEst_h/lds_mpcctrl.h | 64 ++++-------------------------- 1 file changed, 7 insertions(+), 57 deletions(-) diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 746a96a0..b58aabbd 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -158,21 +158,6 @@ class MpcController { bool upd_cons_; ///< constraint was updated since last step private: - /** - * @brief Calculate the trajectory for the simulation step, - * used when the simulation time step is the same as - * the prior 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* fast_update(const Vector& x0, const Vector& u0, - const Matrix& xr, size_t n_sim); - /** * @brief Calculate the trajectory for the simulation step * @@ -183,8 +168,8 @@ class MpcController { * * @return The trajectory for the simulation step */ - osqp_arma::Solution* slow_update(const Vector& x0, const Vector& u0, - const Matrix& xr, size_t n_sim); + osqp_arma::Solution* calc_trajectory(const Vector& x0, const Vector& u0, + const Matrix& xr, size_t n_sim); /** * @brief Create an identity matrix with an offset axis @@ -256,12 +241,7 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, data_t* J) { size_t n_sim = t_sim / sys_.dt(); // Number of points per simulation step - osqp_arma::Solution* sol; - if (arma::all(xi_ == x0) && (t_sim_ == t_sim)) { - sol = fast_update(x0, u0, xr, n_sim); - } else { - sol = slow_update(x0, u0, xr, n_sim); - } + osqp_arma::Solution* sol = calc_trajectory(x0, u0, xr, n_sim); t_sim_ = t_sim; Vector ui(m_); @@ -277,40 +257,10 @@ Vector MpcController::Control(data_t t_sim, const Vector& x0, } template -osqp_arma::Solution* MpcController::fast_update(const Vector& x0, - const Vector& u0, - const Matrix& xr, - size_t n_sim) { - lb_.rows(0, n_ - 1) = -x0.t(); - ub_.rows(0, n_ - 1) = -x0.t(); - - // 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::slow_update(const Vector& x0, - const Vector& u0, - const Matrix& xr, - size_t n_sim) { +osqp_arma::Solution* MpcController::calc_trajectory(const Vector& x0, + const Vector& u0, + const Matrix& xr, + size_t n_sim) { Matrix leq = join_horiz( -x0.t(), arma::zeros((N_ - 1) * n_).t()); // Lower equality bound Matrix ueq = leq; // Upper equality bound From c451f9364d76770b41d2a56563f0fdb223dbe44b Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Fri, 5 Jul 2024 18:00:44 -0700 Subject: [PATCH 19/26] Modified control to use measurements and estimate state instead of full-state MPC --- examples/eg_lqmpc_ctrl.cpp | 27 +++++++++--- include/ldsCtrlEst_h/lds_mpcctrl.h | 70 ++++++++++++++++++++---------- 2 files changed, 67 insertions(+), 30 deletions(-) diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index 3951ebfa..6b6030a0 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -27,8 +27,8 @@ using lds::data_t; using lds::Matrix; -using lds::Vector; using lds::Sparse; +using lds::Vector; using std::cout; auto main() -> int { @@ -67,11 +67,14 @@ auto main() -> int { 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"; @@ -141,8 +144,11 @@ auto main() -> int { Matrix xr = arma::inv(I - A) * B * ur; // create Matrix to save outputs in... - Matrix y_ref = Matrix(n_y, n_t, arma::fill::zeros); + 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 x_true(n_x, n_t, arma::fill::zeros); + Matrix x_pred(n_x, n_t, arma::fill::zeros); Matrix u(n_u, n_t, arma::fill::zeros); Matrix J(1, n_t, arma::fill::zeros); @@ -151,24 +157,30 @@ auto main() -> int { 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 + auto* j = new data_t; // cost + + controlled_system.x().brief_print("True State"); - u0 = controller.Control(t_sim, x0, u0, xr.cols(start_idx, end_idx), j); + u0 = controller.Control(t_sim, z, xr.cols(start_idx, end_idx), true, j); + x_true.col(t) = controlled_system.x(); for (size_t i = 0; i < n_sim; i++) { - controlled_system.Simulate(u0); + z = controlled_system.Simulate(u0); } - x0 = controlled_system.x(); + 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(); + x_pred.col(t) = controller.x_pred(); u.col(t) = u0; J.col(t).fill(*j); } @@ -187,6 +199,9 @@ auto main() -> int { 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)); + x_true.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "x_true", replace)); + x_pred.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "x_pred", replace)); u.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "u", replace)); J.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "j", replace)); diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index b58aabbd..3cdd693b 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -75,16 +75,17 @@ class MpcController { /** * @brief Perform one control step. * - * @param t_sim Simulation time step - * @param x0 Initial state (n) - * @param u0 Initial input (m) - * @param xr Reference/Target state (n x N*n_sim) - * @param J Pointer to variable storing cost + * @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& x0, const Vector& u0, - const Matrix& xr, data_t* J = NULL); + Vector Control(data_t t_sim, const Vector& z, const Matrix& xr, + bool do_control = true, data_t* J = NULL); // getters const System& sys() const { return sys_; } @@ -129,9 +130,9 @@ class MpcController { // TODO: Implement print } - protected: - osqp_arma::OSQP* OSQP; + Vector x_pred() { return x_pred_; } + protected: System sys_; ///< system being controlled size_t n_; ///< number of states size_t m_; ///< number of output states @@ -142,6 +143,13 @@ class MpcController { Sparse P_; ///< penalty matrix Matrix Q_; Matrix S_; + Matrix C_; ///< output matrix + + osqp_arma::OSQP* OSQP; + Sparse P_; ///< penalty matrix + Matrix Q_; ///< cost matrix + + Matrix S_; ///< input cost matrix Matrix lineq_; ///< lower inequality bound Matrix uineq_; ///< upper inequality bound @@ -151,11 +159,14 @@ class MpcController { Vector lb_; ///< lower bound Vector ub_; ///< upper bound - Vector xi_; ///< previous step end state + 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_cons_; ///< constraint was updated since last 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 + + Vector x_pred_; private: /** @@ -209,9 +220,10 @@ class MpcController { void Init() { A_ = sys_.A(); B_ = sys_.B(); + C_ = sys_.C(); n_ = B_.n_rows; m_ = B_.n_cols; - xi_ = arma::zeros(n_); + u_ = Vector(m_, arma::fill::zeros); t_sim_ = 0; OSQP = new osqp_arma::OSQP(); @@ -236,24 +248,34 @@ MpcController::MpcController(System&& sys, Vector u_lb, Vector u_ub) } template -Vector MpcController::Control(data_t t_sim, const Vector& x0, - const Vector& u0, const Matrix& xr, +Vector MpcController::Control(data_t t_sim, const Vector& z, + const Matrix& xr, bool do_control, data_t* J) { - size_t n_sim = t_sim / sys_.dt(); // Number of points per simulation step + // TODO: Variable checks + + sys_.Filter(u_, z); + x_pred_ = sys_.x(); + x_pred_.brief_print("Predicted State"); - osqp_arma::Solution* sol = calc_trajectory(x0, u0, xr, n_sim); + 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); - Vector ui(m_); - for (int i = 0; i < m_; i++) { - ui(i) = sol->x(N_ * n_ + i); + 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); } - xi_ = A_ * x0 + B_ * ui; - 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 ui; + return u_; } template From c719b75fad92631f84a86247e5bb3227e0a49b5a Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Sat, 6 Jul 2024 13:35:20 -0700 Subject: [PATCH 20/26] Updated and removed some unnecessary variables, and added ControlOutputReference method --- examples/eg_lqmpc_ctrl.cpp | 14 +- include/ldsCtrlEst.in | 2 + include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h | 5 + include/ldsCtrlEst_h/lds_mpcctrl.h | 226 ++++++++++++++++---- 4 files changed, 196 insertions(+), 51 deletions(-) diff --git a/examples/eg_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index 6b6030a0..8016bbeb 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -1,5 +1,4 @@ -//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control -//---------------------------===// +//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control ---------------------------===// // // Copyright 2024 Chia-Chien Hung and Kyle Johnsen // Copyright 2024 Georgia Institute of Technology @@ -86,6 +85,7 @@ auto main() -> int { // 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) * @@ -112,7 +112,7 @@ auto main() -> int { controller = std::move( lds::gaussian::MpcController(std::move(controller_system), umin, umax)); - controller.set_control(Q, R, S, N, 20); + controller.set_control(Q, R, S, N, M); controller.set_constraint(xmin, xmax, umin, umax); } @@ -147,8 +147,6 @@ auto main() -> int { 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 x_true(n_x, n_t, arma::fill::zeros); - Matrix x_pred(n_x, n_t, arma::fill::zeros); Matrix u(n_u, n_t, arma::fill::zeros); Matrix J(1, n_t, arma::fill::zeros); @@ -165,10 +163,7 @@ auto main() -> int { auto* j = new data_t; // cost - controlled_system.x().brief_print("True State"); - u0 = controller.Control(t_sim, z, xr.cols(start_idx, end_idx), true, j); - x_true.col(t) = controlled_system.x(); for (size_t i = 0; i < n_sim; i++) { z = controlled_system.Simulate(u0); @@ -180,7 +175,6 @@ auto main() -> int { y_ref.col(t) = yr.col(end_idx); y_true.col(t) = controlled_system.y(); y_hat.col(t) = controller.sys().y(); - x_pred.col(t) = controller.x_pred(); u.col(t) = u0; J.col(t).fill(*j); } @@ -200,8 +194,6 @@ auto main() -> int { 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)); - x_true.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "x_true", replace)); - x_pred.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "x_pred", replace)); u.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "u", replace)); J.save(arma::hdf5_name("eg_lqmpc_ctrl.h5", "j", replace)); diff --git a/include/ldsCtrlEst.in b/include/ldsCtrlEst.in index 9b6da3b4..77c17a01 100755 --- a/include/ldsCtrlEst.in +++ b/include/ldsCtrlEst.in @@ -74,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_gaussian_mpcctrl.h b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h index 54822784..cd7da321 100644 --- a/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h @@ -45,12 +45,17 @@ class MpcController : public lds::MpcController { // make sure base class template methods available using lds::MpcController::MpcController; using lds::MpcController::Control; + using lds::MpcController::ControlOutputReference; // getters + using lds::MpcController::sys; // setters using lds::MpcController::set_control; + using lds::MpcController::set_control_output; using lds::MpcController::set_constraint; + + using lds::MpcController::Print; }; } // namespace gaussian } // namespace lds diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 3cdd693b..b118a5b7 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -87,11 +87,28 @@ class MpcController { 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 + */ + 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_; } // setters void set_control(Matrix Q, Matrix R, Matrix S, size_t N, size_t M) { + // TODO: Variable checks + Q_ = Q; // R_ = R; // Isn't used S_ = S; @@ -114,7 +131,36 @@ class MpcController { upd_ctrl_ = true; } + void set_control_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_ = Sparse(arma::trimatu( + 2 * block_diag(Px, Pu))); // Taking only the upper triangular part + + OSQP_y->set_P(P_y_); + + upd_ctrl_out_ = true; + } + void 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(), @@ -130,8 +176,6 @@ class MpcController { // TODO: Implement print } - Vector x_pred() { return x_pred_; } - protected: System sys_; ///< system being controlled size_t n_; ///< number of states @@ -140,15 +184,16 @@ class MpcController { size_t M_; ///< number of inputs Matrix A_; ///< state transition matrix Matrix B_; ///< input matrix - Sparse P_; ///< penalty matrix - Matrix Q_; - Matrix S_; 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 @@ -166,8 +211,6 @@ class MpcController { bool upd_ctrl_out_; ///< output control was updated since last step bool upd_cons_; ///< constraint was updated since last step - Vector x_pred_; - private: /** * @brief Calculate the trajectory for the simulation step @@ -182,6 +225,32 @@ class MpcController { 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); + + /** + * + * @param x0 + */ + void update_bounds(const Vector& x0); + + /** + * + */ + void update_constraints(size_t n_sim); + /** * @brief Create an identity matrix with an offset axis * @@ -229,6 +298,10 @@ class MpcController { 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); } }; @@ -254,8 +327,6 @@ Vector MpcController::Control(data_t t_sim, const Vector& z, // TODO: Variable checks sys_.Filter(u_, z); - x_pred_ = sys_.x(); - x_pred_.brief_print("Predicted State"); size_t n_sim = t_sim / sys_.dt(); // Number of points to simulate t_sim_ = t_sim; @@ -278,46 +349,50 @@ Vector MpcController::Control(data_t t_sim, const Vector& z, 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 != 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 osqp_arma::Solution* MpcController::calc_trajectory(const Vector& x0, const Vector& u0, const Matrix& xr, size_t n_sim) { - 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 + update_bounds(x0); OSQP->set_l(lb_); OSQP->set_u(ub_); - if (upd_ctrl_ || upd_cons_) { - // 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 - - OSQP->set_A(Acon_); - - upd_ctrl_ = false; - upd_cons_ = false; + 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; @@ -341,6 +416,77 @@ osqp_arma::Solution* MpcController::calc_trajectory(const Vector& x0, 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 \ No newline at end of file From 6e01606cce06a1d733d49a5b40afbb08a137df09 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Sat, 6 Jul 2024 13:36:07 -0700 Subject: [PATCH 21/26] Began implementing eg_plds_mpc --- examples/eg_plds_mpc.cpp | 224 +++++++++++++++++++++ include/ldsCtrlEst_h/lds_poisson_mpcctrl.h | 63 ++++++ 2 files changed, 287 insertions(+) create mode 100644 examples/eg_plds_mpc.cpp create mode 100644 include/ldsCtrlEst_h/lds_poisson_mpcctrl.h diff --git a/examples/eg_plds_mpc.cpp b/examples/eg_plds_mpc.cpp new file mode 100644 index 00000000..470cbe72 --- /dev/null +++ b/examples/eg_plds_mpc.cpp @@ -0,0 +1,224 @@ +//===-- 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::Matrix; +using lds::Vector; +using lds::data_t; +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(10.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.0 * 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); + Matrix c_true = Matrix(n_y, n_x, arma::fill::eye); + + /// 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_C(c_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(y_ref0); + 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::eye) * 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 u_lb = Vector(n_u) * 0.0; + Vector u_ub = Vector(n_u) * 5.0; + controller = std::move(lds::poisson::MpcController(std::move(controller_system), u_lb, u_ub)); + controller.set_control_output(Q_y, R, S, N, M); + } + + 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); + + // 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 + 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); + } + + 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++) { + // 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) { // swithces high -> low disturbance + m0_true = std::vector(n_x, m_low); + which_m = 0; + } + } + controlled_system.set_m(m0_true); + + // 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 + 1; + + u.col(t) = controller.ControlOutputReference(dt, z.col(t), y_ref.cols(start_idx, end_idx), true); + + 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(); + m_hat.col(t) = controller.sys().m(); + } + + 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)); + + return 0; +} \ No newline at end of file diff --git a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h new file mode 100644 index 00000000..fdcbb78d --- /dev/null +++ b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h @@ -0,0 +1,63 @@ +//===-- 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; + using lds::MpcController::ControlOutputReference; + + // getters + using lds::MpcController::sys; + + // setters + using lds::MpcController::set_control; + using lds::MpcController::set_control_output; + using lds::MpcController::set_constraint; + + using lds::MpcController::Print; +}; +} // namespace poisson +} // namespace lds + +#endif // LDSCTRLEST_LDS_POISSON_MPCCTRL_H From 03444734294a86c28989dee0d499a0b31997aa0a Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 17 Jul 2024 23:02:36 -0700 Subject: [PATCH 22/26] Added MPC implementation for Poisson system Added MPC control for Poisson system, and added example to show it in action --- examples/CMakeLists.txt | 2 +- examples/eg_lqmpc_ctrl.cpp | 11 +- examples/eg_plds_ctrl.cpp | 16 +- examples/eg_plds_mpc.cpp | 59 ++++-- include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h | 13 +- include/ldsCtrlEst_h/lds_mpcctrl.h | 215 ++++++++++++-------- include/ldsCtrlEst_h/lds_poisson_mpcctrl.h | 39 +++- osqp | 1 + 8 files changed, 235 insertions(+), 121 deletions(-) create mode 160000 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_lqmpc_ctrl.cpp b/examples/eg_lqmpc_ctrl.cpp index 8016bbeb..a717d15a 100644 --- a/examples/eg_lqmpc_ctrl.cpp +++ b/examples/eg_lqmpc_ctrl.cpp @@ -1,4 +1,5 @@ -//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control ---------------------------===// +//===-- eg_lqmpc_ctrl.cpp - Example LQMPC Control +//---------------------------===// // // Copyright 2024 Chia-Chien Hung and Kyle Johnsen // Copyright 2024 Georgia Institute of Technology @@ -110,9 +111,9 @@ auto main() -> int { lds::gaussian::System controller_system(controlled_system); - controller = std::move( - lds::gaussian::MpcController(std::move(controller_system), umin, umax)); - controller.set_control(Q, R, S, N, M); + 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); } @@ -199,4 +200,4 @@ auto main() -> int { cout << "fin.\n"; return 0; -} \ No newline at end of file +} 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 index 470cbe72..ac1e7e53 100644 --- a/examples/eg_plds_mpc.cpp +++ b/examples/eg_plds_mpc.cpp @@ -23,9 +23,9 @@ #include +using lds::data_t; using lds::Matrix; using lds::Vector; -using lds::data_t; using std::cout; auto main() -> int { @@ -52,7 +52,6 @@ auto main() -> int { 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); - Matrix c_true = Matrix(n_y, n_x, arma::fill::eye); /// Going to simulate a switching disturbance (m) acting on system size_t which_m = 0; @@ -68,7 +67,6 @@ auto main() -> int { // Assign params. controlled_system.set_A(a_true); controlled_system.set_B(b_true); - controlled_system.set_C(c_true); controlled_system.set_m(m0_true); controlled_system.set_x0(x0_true); // reset to initial conditions @@ -93,7 +91,7 @@ auto main() -> int { Vector x0_controller = arma::log(y_ref0); controller_system.set_m(m0_controller); controller_system.set_x0(x0_controller); - controller_system.Reset(); //reset to new init condition + controller_system.Reset(); // reset to new init condition // adaptively re-estimate process disturbance (m) controller_system.do_adapt_m = true; @@ -104,14 +102,21 @@ auto main() -> int { controller_system.set_Q_m(q_m); // set control penalties - Matrix Q_y = Matrix(n_y, n_y, arma::fill::eye) * 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 u_lb = Vector(n_u) * 0.0; - Vector u_ub = Vector(n_u) * 5.0; - controller = std::move(lds::poisson::MpcController(std::move(controller_system), u_lb, u_ub)); - controller.set_control_output(Q_y, R, S, N, M); + Matrix Q_y = Matrix(n_y, n_y, arma::fill::ones) * 1e5; + Matrix R = Matrix(n_u, n_u, arma::fill::zeros) * 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"; @@ -141,6 +146,8 @@ auto main() -> int { 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(); @@ -160,8 +167,9 @@ auto main() -> int { y_ref0 % arma::sin(f * 2 * lds::kPi * dt * t_vec - lds::kPi / 4); } - 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); @@ -172,29 +180,39 @@ 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)); // Simulate the true system. - z.col(t) = controlled_system.Simulate(u.col(t-1)); + 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 + 1; - u.col(t) = controller.ControlOutputReference(dt, z.col(t), y_ref.cols(start_idx, end_idx), true); + 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(); - m_true.col(t) = controlled_system.m(); 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(); @@ -219,6 +237,7 @@ auto main() -> int { 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; -} \ No newline at end of file +} diff --git a/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h index cd7da321..65f0b8b9 100644 --- a/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_gaussian_mpcctrl.h @@ -49,10 +49,19 @@ class MpcController : public lds::MpcController { // 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_control; - using lds::MpcController::set_control_output; + using lds::MpcController::set_cost; + using lds::MpcController::set_cost_output; using lds::MpcController::set_constraint; using lds::MpcController::Print; diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index b118a5b7..e965a69d 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -54,23 +54,19 @@ class MpcController { * @brief Constructs a new MpcController. * * @param sys The system being controlled - * @param u_lb The lower bound of the control input - * @param u_ub The upper bound of the control input * * @tparam System The system type */ - MpcController(const System& sys, Vector u_lb, Vector u_ub); + MpcController(const System& sys); /** * @brief Constructs a new MpcController. * * @param sys The system being controlled - * @param u_lb The lower bound of the control input - * @param u_ub The upper bound of the control input * * @tparam System The system type */ - MpcController(System&& sys, Vector u_lb, Vector u_ub); + MpcController(System&& sys); /** * @brief Perform one control step. @@ -93,83 +89,61 @@ class MpcController { * @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 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 ControlOutputReference(data_t t_sim, const Vector& z, const Matrix& yr, - bool do_control = true, data_t* J = NULL); + 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 - void set_control(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; - } - - void set_control_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_ = Sparse(arma::trimatu( - 2 * block_diag(Px, Pu))); // Taking only the upper triangular part - - OSQP_y->set_P(P_y_); - - upd_ctrl_out_ = true; - } - - void set_constraint(Vector xmin, Vector xmax, Vector umin, Vector umax) { - // TODO: Check constraints + /** + * @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); - 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()); - size_t Aineq_dim = N_ * n_ + M_ * m_; - Aineq_ = arma::eye(Aineq_dim, Aineq_dim); + /** + * @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); - upd_cons_ = true; - } + /** + * @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(); @@ -241,13 +215,16 @@ class MpcController { const Matrix& yr, size_t n_sim); /** + * @brief Update the OSQP constraint bounds * - * @param x0 + * @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); @@ -308,15 +285,12 @@ class MpcController { // Implement methods template -MpcController::MpcController(const System& sys, Vector u_lb, - Vector u_ub) - : sys_(sys), lb_(u_lb), ub_(u_ub) { +MpcController::MpcController(const System& sys) : sys_(sys) { Init(); } template -MpcController::MpcController(System&& sys, Vector u_lb, Vector u_ub) - : sys_(std::move(sys)), lb_(u_lb), ub_(u_ub) { +MpcController::MpcController(System&& sys) : sys_(std::move(sys)) { Init(); } @@ -367,9 +341,13 @@ Vector MpcController::ControlOutputReference(data_t t_sim, for (int i = 0; i < m_; i++) { u_(i) = sol->x(N_ * n_ + i); } - if (J != NULL) *J = sol->obj_val(); + if (J != nullptr) { + *J = sol->obj_val(); + } - if (sol) free(sol); + if (sol) { + free(sol); + } } for (int i = 0; i < n_sim; i++) { @@ -380,6 +358,81 @@ Vector MpcController::ControlOutputReference(data_t t_sim, 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_ = Sparse(arma::trimatu( + block_diag(Px, Pu))); // Taking only the upper triangular part + + 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, @@ -489,4 +542,4 @@ void MpcController::update_constraints(size_t n_sim) { } // namespace lds -#endif \ No newline at end of file +#endif diff --git a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h index fdcbb78d..a92039e4 100644 --- a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h @@ -1,4 +1,5 @@ -//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - PLDS MPC Controller ------*- C++ -*-===// +//===-- ldsCtrlEst_h/lds_gaussian_mpcctrl.h - PLDS MPC Controller ------*- C++ +//-*-===// // // Copyright 2024 Chia-Chien Hung and Kyle Johnsen // Copyright 2024 Georgia Institute of Technology @@ -41,23 +42,47 @@ 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; - using lds::MpcController::ControlOutputReference; + + 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 = log10(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_control; - using lds::MpcController::set_control_output; + 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-4; ///< lower bound on yRef (to avoid numerical log(0) issue) }; -} // namespace poisson -} // namespace lds +} // namespace poisson +} // namespace lds #endif // LDSCTRLEST_LDS_POISSON_MPCCTRL_H diff --git a/osqp b/osqp new file mode 160000 index 00000000..c2ab44ed --- /dev/null +++ b/osqp @@ -0,0 +1 @@ +Subproject commit c2ab44edf7417212500065683a3f22cc06a23fd5 From 0232f60c2797078eceb478543212e29279149099 Mon Sep 17 00:00:00 2001 From: aaronhung Date: Thu, 6 Feb 2025 14:24:14 -0500 Subject: [PATCH 23/26] Fixed OSQP installation --- .gitmodules | 5 ++++- cmake/Modules/OSQP.cmake | 10 ++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index 15e4cb3c..69533532 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,4 +10,7 @@ url = https://github.com/pybind/pybind11.git [submodule "python/carma"] path = python/carma - url = https://github.com/RUrlus/carma.git \ No newline at end of file + url = https://github.com/RUrlus/carma.git +[submodule "osqp"] + path = osqp + url = https://github.com/osqp/osqp.git diff --git a/cmake/Modules/OSQP.cmake b/cmake/Modules/OSQP.cmake index 912e08fa..c04e0cf8 100644 --- a/cmake/Modules/OSQP.cmake +++ b/cmake/Modules/OSQP.cmake @@ -9,5 +9,11 @@ FetchContent_Declare( FetchContent_MakeAvailable(osqp) message(STATUS "Installed osqp to ${osqp_BINARY_DIR}") list(APPEND CMAKE_PREFIX_PATH ${osqp_BINARY_DIR}) -find_package(osqp REQUIRED) -list(APPEND PROJECT_REQUIRED_LIBRARIES_ABSOLUTE_NAME osqp::osqp) \ No newline at end of file + + +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) From 14498e96f7f55d6fc7f25b000aadbe38777f2118 Mon Sep 17 00:00:00 2001 From: aaronhung Date: Thu, 6 Feb 2025 14:29:30 -0500 Subject: [PATCH 24/26] Removed unnecessary osqp submodule --- .gitmodules | 3 --- osqp | 1 - 2 files changed, 4 deletions(-) delete mode 160000 osqp diff --git a/.gitmodules b/.gitmodules index 69533532..42fe1bfb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -11,6 +11,3 @@ [submodule "python/carma"] path = python/carma url = https://github.com/RUrlus/carma.git -[submodule "osqp"] - path = osqp - url = https://github.com/osqp/osqp.git diff --git a/osqp b/osqp deleted file mode 160000 index c2ab44ed..00000000 --- a/osqp +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c2ab44edf7417212500065683a3f22cc06a23fd5 From f361b92320f97c50fba08ad9e05cd3acbe3ffc46 Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Wed, 9 Jul 2025 16:09:20 -0400 Subject: [PATCH 25/26] Added eg_glds_mpc and fixed incorrect parameters for ControlOutputReference, eg_plds_mpc WIP --- examples/eg_glds_mpc.cpp | 279 +++++++++++++++++++++ examples/eg_plds_mpc.cpp | 6 +- include/ldsCtrlEst_h/lds_mpcctrl.h | 9 +- include/ldsCtrlEst_h/lds_poisson_mpcctrl.h | 2 +- 4 files changed, 289 insertions(+), 7 deletions(-) create mode 100644 examples/eg_glds_mpc.cpp diff --git a/examples/eg_glds_mpc.cpp b/examples/eg_glds_mpc.cpp new file mode 100644 index 00000000..5e72f272 --- /dev/null +++ b/examples/eg_glds_mpc.cpp @@ -0,0 +1,279 @@ +//===-- 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; + + /// Going to simulate a switching disturbance (m) acting on system + size_t which_m = 0; // whether low or high disturbance (0, 1) + data_t m_low = 5 * dt * (1 - a_true[0]) * 0; + data_t pr_lo2hi = 1e-3; // probability of going from low to high disturb. + data_t m_high = 20 * dt * (1 - a_true[0]) * 0; + data_t pr_hi2lo = pr_lo2hi; + + // initially let m be low + Vector m0_true = Vector(n_x).fill(m_low); + + // Assign params. + controlled_system.set_A(a_true); + controlled_system.set_B(b_true); + controlled_system.set_m(m0_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 + { + // Create **incorrect** model used for control. + // (e.g., imperfect model fitting) + Matrix b_controller = b_true; + + // let's assume zero process disturbance initially + // (will be re-estimating) + Vector m_controller = Vector(n_x, arma::fill::zeros); + + // 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_B(b_controller); + controller_system.set_m(m_controller); + 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; + + // 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-6; + controller_system.set_Q_m(q_m); + + 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_controller.n_rows); + xmin.fill(-arma::datum::inf); + Vector xmax(b_controller.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); + Matrix m_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); + Matrix m_true(n_x, 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(); + + m_hat.submat(0, 0, n_x - 1, 0) = controller.sys().m(); + m_true.submat(0, 0, n_x - 1, 0) = controlled_system.m(); + + 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(); + m_true.col(i) = controlled_system.m(); + + y_hat.col(i) = controller.sys().y(); + x_hat.col(i) = controller.sys().x(); + m_hat.col(i) = controller.sys().m(); + } + + for (size_t s = 1; s < n_c; s++) { + // 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; + } + } + controlled_system.set_m(m0_true); + + // 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 u_next = controller.ControlOutputReference(t_sim, z.col(start_idx - 1), y_ref.cols(start_idx, end_idx)); + for (int t = 0; t < n_sim; t++) { + u.col(start_idx + t) = u_next; + } + + // 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(); + m_true.col(t) = controlled_system.m(); + + y_hat.col(t) = controller.sys().y(); + x_hat.col(t) = controller.sys().x(); + m_hat.col(t) = controller.sys().m(); + } + } + + 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_ctrl.h5", "dt")); + Matrix y_ref_clip = y_ref.cols(0, n_t - 1); + y_ref_clip.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_ref", replace)); + u.save(arma::hdf5_name("eg_glds_ctrl.h5", "u", replace)); + z.save(arma::hdf5_name("eg_glds_ctrl.h5", "z", replace)); + x_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_true", replace)); + m_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_true", replace)); + y_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_true", replace)); + x_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_hat", replace)); + m_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_hat", replace)); + y_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_hat", replace)); + + cout << "fin.\n"; + return 0; +} diff --git a/examples/eg_plds_mpc.cpp b/examples/eg_plds_mpc.cpp index ac1e7e53..54666a96 100644 --- a/examples/eg_plds_mpc.cpp +++ b/examples/eg_plds_mpc.cpp @@ -55,9 +55,9 @@ auto main() -> int { /// 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 m_low = log(1 * dt) * (1 - a_true[0]) * 0; data_t pr_lo2hi = 1e-3; - data_t m_high = log(20 * dt) * (1 - a_true[0]); + data_t m_high = log(20 * dt) * (1 - a_true[0]) * 0; data_t pr_hi2lo = pr_lo2hi; Vector m0_true = Vector(n_x, arma::fill::ones) * m_low; @@ -103,7 +103,7 @@ auto main() -> int { // set control penalties Matrix Q_y = Matrix(n_y, n_y, arma::fill::ones) * 1e5; - Matrix R = Matrix(n_u, n_u, arma::fill::zeros) * 1e-1; + 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); diff --git a/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index e965a69d..89d5bd4c 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -265,7 +265,7 @@ class MpcController { void Init() { A_ = sys_.A(); - B_ = sys_.B(); + B_ = sys_.B() * arma::diagmat(sys_.g()); C_ = sys_.C(); n_ = B_.n_rows; m_ = B_.n_cols; @@ -406,8 +406,7 @@ void MpcController::set_cost_output(Matrix Q_y, Matrix R, Matrix S, Matrix Pu3 = block_diag(Matrix((M_ - 1) * m_, (M_ - 1) * m_, arma::fill::zeros), -S_); Matrix Pu = Pu1 + Pu2 + Pu3; - P_y_ = Sparse(arma::trimatu( - block_diag(Px, Pu))); // Taking only the upper triangular part + P_y_ = 2 * Sparse(block_diag(Px, Pu)); OSQP_y->set_P(P_y_); @@ -489,6 +488,8 @@ osqp_arma::Solution* MpcController::calc_output_trajectory( Matrix Qxr_full = -2 * sliced_yr.t() * Q_y_ * C_; Vector Qxr = Qxr_full.as_row().t(); // Qxr for every simulation time step + sliced_yr.print("y_ref = "); + Vector qu = join_vert((-2 * S_ * u0), Vector((M_ - 1) * m_, arma::fill::zeros)); Vector qx = Qxr.rows(0, N_ * n_ - 1); @@ -500,6 +501,8 @@ osqp_arma::Solution* MpcController::calc_output_trajectory( osqp_arma::Solution* sol = OSQP_y->solve(); + sol->x()->t().print("sol = "); + return sol; } diff --git a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h index a92039e4..69bb49ab 100644 --- a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h @@ -80,7 +80,7 @@ class MpcController : public lds::MpcController { private: constexpr static const data_t kYRefLb = - 1e-4; ///< lower bound on yRef (to avoid numerical log(0) issue) + 1e-12; ///< lower bound on yRef (to avoid numerical log(0) issue) }; } // namespace poisson } // namespace lds From d98959304184b600010613a734497dd381cc070d Mon Sep 17 00:00:00 2001 From: Aaron Hung Date: Thu, 10 Jul 2025 10:09:44 -0400 Subject: [PATCH 26/26] Fixed example and slight bug fixes Fixed the eg_glds_mpc example to remove unused code sections (imperfect model, disturbance) and modified output to include cost over time. Added output plotting matlab file. --- examples/eg_glds_mpc.cpp | 81 +++++----------------- examples/eg_plds_mpc.cpp | 13 ++-- examples/plot_glds_mpc_output.m | 47 +++++++++++++ include/ldsCtrlEst_h/lds_mpcctrl.h | 4 -- include/ldsCtrlEst_h/lds_poisson_mpcctrl.h | 2 +- 5 files changed, 72 insertions(+), 75 deletions(-) create mode 100644 examples/plot_glds_mpc_output.m diff --git a/examples/eg_glds_mpc.cpp b/examples/eg_glds_mpc.cpp index 5e72f272..53ebf22e 100644 --- a/examples/eg_glds_mpc.cpp +++ b/examples/eg_glds_mpc.cpp @@ -57,20 +57,9 @@ auto main() -> int { // output noise covariance Matrix r_true = Matrix(n_y, n_y, arma::fill::eye) * 1e-4; - /// Going to simulate a switching disturbance (m) acting on system - size_t which_m = 0; // whether low or high disturbance (0, 1) - data_t m_low = 5 * dt * (1 - a_true[0]) * 0; - data_t pr_lo2hi = 1e-3; // probability of going from low to high disturb. - data_t m_high = 20 * dt * (1 - a_true[0]) * 0; - data_t pr_hi2lo = pr_lo2hi; - - // initially let m be low - Vector m0_true = Vector(n_x).fill(m_low); - // Assign params. controlled_system.set_A(a_true); controlled_system.set_B(b_true); - controlled_system.set_m(m0_true); controlled_system.set_g(g_true); controlled_system.set_R(r_true); @@ -85,31 +74,16 @@ auto main() -> int { const size_t N = 25; // Prediction horizon const size_t M = 20; // Control horizon { - // Create **incorrect** model used for control. - // (e.g., imperfect model fitting) - Matrix b_controller = b_true; - - // let's assume zero process disturbance initially - // (will be re-estimating) - Vector m_controller = Vector(n_x, arma::fill::zeros); - // 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_B(b_controller); - controller_system.set_m(m_controller); 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; - // 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-6; - controller_system.set_Q_m(q_m); - 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 @@ -125,9 +99,9 @@ auto main() -> int { umax = {5, 5, 5}; } - Vector xmin(b_controller.n_rows); + Vector xmin(b_true.n_rows); xmin.fill(-arma::datum::inf); - Vector xmax(b_controller.n_rows); + Vector xmax(b_true.n_rows); xmax.fill(arma::datum::inf); controller = @@ -163,12 +137,13 @@ auto main() -> int { // *_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_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); - Matrix m_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(); @@ -177,9 +152,6 @@ auto main() -> int { x_hat.submat(0, 0, n_x - 1, 0) = controller.sys().x(); x_true.submat(0, 0, n_x - 1, 0) = controlled_system.x(); - m_hat.submat(0, 0, n_x - 1, 0) = controller.sys().m(); - m_true.submat(0, 0, n_x - 1, 0) = controlled_system.m(); - cout << "Starting " << n_t * dt << " sec simulation ... \n"; auto start = std::chrono::high_resolution_clock::now(); @@ -193,30 +165,12 @@ auto main() -> int { // save the signals y_true.col(i) = controlled_system.y(); x_true.col(i) = controlled_system.x(); - m_true.col(i) = controlled_system.m(); y_hat.col(i) = controller.sys().y(); x_hat.col(i) = controller.sys().x(); - m_hat.col(i) = controller.sys().m(); } for (size_t s = 1; s < n_c; s++) { - // 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; - } - } - controlled_system.set_m(m0_true); - // Calculate the slice indices size_t start_idx = s * n_sim; size_t end_idx = ((s + N) * n_sim) - 1; @@ -225,9 +179,11 @@ auto main() -> int { // 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 u_next = controller.ControlOutputReference(t_sim, z.col(start_idx - 1), y_ref.cols(start_idx, end_idx)); + 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. @@ -242,11 +198,9 @@ auto main() -> int { // save the signals 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(); - m_hat.col(t) = controller.sys().m(); } } @@ -262,17 +216,16 @@ auto main() -> int { arma::hdf5_opts::opts replace = arma::hdf5_opts::replace; auto dt_vec = Vector(1).fill(dt); - dt_vec.save(arma::hdf5_name("eg_glds_ctrl.h5", "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_ctrl.h5", "y_ref", replace)); - u.save(arma::hdf5_name("eg_glds_ctrl.h5", "u", replace)); - z.save(arma::hdf5_name("eg_glds_ctrl.h5", "z", replace)); - x_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_true", replace)); - m_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_true", replace)); - y_true.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_true", replace)); - x_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "x_hat", replace)); - m_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "m_hat", replace)); - y_hat.save(arma::hdf5_name("eg_glds_ctrl.h5", "y_hat", replace)); + 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_plds_mpc.cpp b/examples/eg_plds_mpc.cpp index 54666a96..81763438 100644 --- a/examples/eg_plds_mpc.cpp +++ b/examples/eg_plds_mpc.cpp @@ -39,11 +39,11 @@ auto main() -> int { size_t n_y = 1; // no time steps for simulation. - auto n_t = static_cast(10.0 / dt); + 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.0 * dt; + 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) @@ -55,9 +55,9 @@ auto main() -> int { /// 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]) * 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]) * 0; + 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; @@ -88,7 +88,7 @@ auto main() -> int { // for this example, assume model correct, except disturbance Vector m0_controller = Vector(n_x, arma::fill::ones) * m_low; - Vector x0_controller = arma::log(y_ref0); + 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 @@ -159,6 +159,7 @@ auto main() -> int { 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 @@ -198,7 +199,7 @@ auto main() -> int { // Calculate the slice indices size_t start_idx = t; - size_t end_idx = t + N + 1; + size_t end_idx = t + N; auto* j = new data_t; 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/include/ldsCtrlEst_h/lds_mpcctrl.h b/include/ldsCtrlEst_h/lds_mpcctrl.h index 89d5bd4c..3d707c46 100644 --- a/include/ldsCtrlEst_h/lds_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_mpcctrl.h @@ -488,8 +488,6 @@ osqp_arma::Solution* MpcController::calc_output_trajectory( Matrix Qxr_full = -2 * sliced_yr.t() * Q_y_ * C_; Vector Qxr = Qxr_full.as_row().t(); // Qxr for every simulation time step - sliced_yr.print("y_ref = "); - Vector qu = join_vert((-2 * S_ * u0), Vector((M_ - 1) * m_, arma::fill::zeros)); Vector qx = Qxr.rows(0, N_ * n_ - 1); @@ -501,8 +499,6 @@ osqp_arma::Solution* MpcController::calc_output_trajectory( osqp_arma::Solution* sol = OSQP_y->solve(); - sol->x()->t().print("sol = "); - return sol; } diff --git a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h index 69bb49ab..cbd79dc9 100644 --- a/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h +++ b/include/ldsCtrlEst_h/lds_poisson_mpcctrl.h @@ -53,7 +53,7 @@ class MpcController : public lds::MpcController { // 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 = log10(yr_transformed); + yr_transformed = log(yr_transformed); return lds::MpcController::ControlOutputReference( t_sim, z, yr_transformed, do_control, J);