From 5aac754e49bae4f6dd98c141f3772797b9aeb5b7 Mon Sep 17 00:00:00 2001 From: Dominic Hofer <6570912+dominichofer@users.noreply.github.com> Date: Thu, 13 Feb 2025 01:00:08 +0100 Subject: [PATCH 1/5] cpp: Add offset to controller --- src/deepcpp/controller.cpp | 15 +++++++++++---- src/deepcpp/controller.h | 4 ++-- tests/deepcpp/test_controller.cpp | 25 +++++++++++++++++++++++-- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/deepcpp/controller.cpp b/src/deepcpp/controller.cpp index 3d2e768..f22e7f5 100644 --- a/src/deepcpp/controller.cpp +++ b/src/deepcpp/controller.cpp @@ -137,18 +137,22 @@ void Controller::clear() y_ini.clear(); } -std::vector Controller::apply(const std::vector &target) +std::vector Controller::apply(const std::vector &target, const std::vector &offset) { if (!is_initialized()) return {}; check_dimensions(target, "target", target_size, output_dims); + if (!offset.empty()) + check_dimensions(offset, "offset", target_size, output_dims); + // Flatten VectorXd target_ = concat(target); + VectorXd offset_ = offset.empty() ? VectorXd::Zero(target_size * output_dims) : concat(offset); auto x = concat(u_ini.get(), y_ini.get()); - auto w = M_u.transpose() * Q * (target_ - M_x * x); + auto w = M_u.transpose() * Q * (target_ - M_x * x) + R * offset_; auto u_star = G.ldlt().solve(w).eval(); if (input_constrain_fkt != nullptr) @@ -163,7 +167,10 @@ std::vector Controller::apply(const std::vector &target) return split(u_star, target_size); } -std::vector Controller::apply(const VectorXd &target) +std::vector Controller::apply(const VectorXd &target, const VectorXd &offset) { - return apply(std::vector{target}); + return apply( + std::vector{target}, + offset.size() == 0 ? std::vector{} : std::vector{offset} + ); } diff --git a/src/deepcpp/controller.h b/src/deepcpp/controller.h index 623890b..1e26bbc 100644 --- a/src/deepcpp/controller.h +++ b/src/deepcpp/controller.h @@ -74,6 +74,6 @@ class Controller bool is_initialized() const; void update(VectorXd u, VectorXd y); void clear(); - std::vector apply(const std::vector &); - std::vector apply(const VectorXd &); + std::vector apply(const std::vector &target, const std::vector &offset = {}); + std::vector apply(const VectorXd &target, const VectorXd &offset = {}); }; diff --git a/tests/deepcpp/test_controller.cpp b/tests/deepcpp/test_controller.cpp index ca8e3c6..9d3cda2 100644 --- a/tests/deepcpp/test_controller.cpp +++ b/tests/deepcpp/test_controller.cpp @@ -76,12 +76,17 @@ void warm_up_controller(Controller& controller, DiscreteLTI& system, const Vecto // Control the system for a given number of time steps. // Returns the output of the system after the last time step. -VectorXd control_system(Controller& controller, DiscreteLTI& system, const std::vector& target, int time_steps) +VectorXd control_system( + Controller& controller, + DiscreteLTI& system, + const std::vector& target, + int time_steps, + const std::vector& offset = {}) { VectorXd u, y; for (int i = 0; i < time_steps; ++i) { - u = controller.apply(target).front(); + u = controller.apply(target, offset).front(); y = system.apply(u); controller.update(u, y); } @@ -119,6 +124,14 @@ TEST_F(Test_1D_in_1D_out_LTI, Constrained) expect_near(y, target[0], 1e-5); } +TEST_F(Test_1D_in_1D_out_LTI, Offset) +{ + Controller controller{u_d, y_d, T_ini, target.size(), /*R*/0.001}; + warm_up_controller(controller, system, Vector(1)); + VectorXd y = control_system(controller, system, target, T_ini, {Vector(10), Vector(10)}); + expect_near(y, target[0], 1e-5); +} + class Test_2D_in_3D_out_LTI : public ::testing::Test { @@ -150,3 +163,11 @@ TEST_F(Test_2D_in_3D_out_LTI, Constrained) VectorXd y = control_system(controller, system, target, 2 * T_ini); expect_near(y, target[0], 0.05); } + +TEST_F(Test_2D_in_3D_out_LTI, Offset) +{ + Controller controller{u_d, y_d, T_ini, target.size(), /*R*/0.001}; + warm_up_controller(controller, system, Vector(1, 1)); + VectorXd y = control_system(controller, system, target, T_ini, {Vector(10, 10), Vector(10, 10), Vector(10, 10)}); + expect_near(y, target[0], 0.05); +} From 760b1a6aa74fb5640c663d6f883318521703a3a0 Mon Sep 17 00:00:00 2001 From: Dominic Hofer <6570912+dominichofer@users.noreply.github.com> Date: Thu, 13 Feb 2025 01:17:09 +0100 Subject: [PATCH 2/5] Add missing Q --- tests/deepcpp/test_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/deepcpp/test_controller.cpp b/tests/deepcpp/test_controller.cpp index 9d3cda2..9dc5ba8 100644 --- a/tests/deepcpp/test_controller.cpp +++ b/tests/deepcpp/test_controller.cpp @@ -126,7 +126,7 @@ TEST_F(Test_1D_in_1D_out_LTI, Constrained) TEST_F(Test_1D_in_1D_out_LTI, Offset) { - Controller controller{u_d, y_d, T_ini, target.size(), /*R*/0.001}; + Controller controller{u_d, y_d, T_ini, target.size(), /*Q*/1.0, /*R*/0.001}; warm_up_controller(controller, system, Vector(1)); VectorXd y = control_system(controller, system, target, T_ini, {Vector(10), Vector(10)}); expect_near(y, target[0], 1e-5); @@ -166,7 +166,7 @@ TEST_F(Test_2D_in_3D_out_LTI, Constrained) TEST_F(Test_2D_in_3D_out_LTI, Offset) { - Controller controller{u_d, y_d, T_ini, target.size(), /*R*/0.001}; + Controller controller{u_d, y_d, T_ini, target.size(), /*Q*/1.0, /*R*/0.001}; warm_up_controller(controller, system, Vector(1, 1)); VectorXd y = control_system(controller, system, target, T_ini, {Vector(10, 10), Vector(10, 10), Vector(10, 10)}); expect_near(y, target[0], 0.05); From 33c766f35c201baa7bd7bdbb93a38a5c1833dc5b Mon Sep 17 00:00:00 2001 From: Dominic Hofer <6570912+dominichofer@users.noreply.github.com> Date: Thu, 13 Feb 2025 01:21:31 +0100 Subject: [PATCH 3/5] Fix tests --- tests/deepcpp/test_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/deepcpp/test_controller.cpp b/tests/deepcpp/test_controller.cpp index 9dc5ba8..4bcc6d9 100644 --- a/tests/deepcpp/test_controller.cpp +++ b/tests/deepcpp/test_controller.cpp @@ -129,7 +129,7 @@ TEST_F(Test_1D_in_1D_out_LTI, Offset) Controller controller{u_d, y_d, T_ini, target.size(), /*Q*/1.0, /*R*/0.001}; warm_up_controller(controller, system, Vector(1)); VectorXd y = control_system(controller, system, target, T_ini, {Vector(10), Vector(10)}); - expect_near(y, target[0], 1e-5); + expect_near(y, target[0], 0.02); } @@ -168,6 +168,6 @@ TEST_F(Test_2D_in_3D_out_LTI, Offset) { Controller controller{u_d, y_d, T_ini, target.size(), /*Q*/1.0, /*R*/0.001}; warm_up_controller(controller, system, Vector(1, 1)); - VectorXd y = control_system(controller, system, target, T_ini, {Vector(10, 10), Vector(10, 10), Vector(10, 10)}); + VectorXd y = control_system(controller, system, target, T_ini, {Vector(1, 1)); expect_near(y, target[0], 0.05); } From 0dddd867524fb12e53e8115a19eb433e86a5f7a9 Mon Sep 17 00:00:00 2001 From: Dominic Hofer <6570912+dominichofer@users.noreply.github.com> Date: Thu, 13 Feb 2025 01:23:03 +0100 Subject: [PATCH 4/5] Typo fix --- tests/deepcpp/test_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/deepcpp/test_controller.cpp b/tests/deepcpp/test_controller.cpp index 4bcc6d9..92bc4cb 100644 --- a/tests/deepcpp/test_controller.cpp +++ b/tests/deepcpp/test_controller.cpp @@ -168,6 +168,6 @@ TEST_F(Test_2D_in_3D_out_LTI, Offset) { Controller controller{u_d, y_d, T_ini, target.size(), /*Q*/1.0, /*R*/0.001}; warm_up_controller(controller, system, Vector(1, 1)); - VectorXd y = control_system(controller, system, target, T_ini, {Vector(1, 1)); + VectorXd y = control_system(controller, system, target, T_ini, {Vector(1, 1)}); expect_near(y, target[0], 0.05); } From 2b772067c785629358a2614ad7ba119424330ef1 Mon Sep 17 00:00:00 2001 From: Dominic Hofer <6570912+dominichofer@users.noreply.github.com> Date: Thu, 13 Feb 2025 01:30:43 +0100 Subject: [PATCH 5/5] Fix bug --- src/deepcpp/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/deepcpp/controller.cpp b/src/deepcpp/controller.cpp index f22e7f5..6056ef2 100644 --- a/src/deepcpp/controller.cpp +++ b/src/deepcpp/controller.cpp @@ -145,7 +145,7 @@ std::vector Controller::apply(const std::vector &target, con check_dimensions(target, "target", target_size, output_dims); if (!offset.empty()) - check_dimensions(offset, "offset", target_size, output_dims); + check_dimensions(offset, "offset", target_size, input_dims); // Flatten VectorXd target_ = concat(target);