From daa79ad776bb87ed99e69cc9dc93f99bbc44bf6a Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Wed, 8 Jan 2025 09:28:18 +0100 Subject: [PATCH 01/61] [lfc] Add the first Ctor unittest including cmake target --- CMakeLists.txt | 5 +++++ tests/test_linear_feedback_controller.cpp | 9 +++++++++ 2 files changed, 14 insertions(+) create mode 100644 tests/test_linear_feedback_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7ddb5670..b2856567 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,8 +90,13 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_pd_controller tests/test_pd_controller.cpp) target_link_libraries(test_pd_controller ${PROJECT_NAME}) + ament_auto_add_gtest(test_lf_controller tests/test_lf_controller.cpp) target_link_libraries(test_lf_controller ${PROJECT_NAME}) + + ament_auto_add_gtest(test_linear_feedback_controller + tests/test_linear_feedback_controller.cpp) + target_link_libraries(test_linear_feedback_controller ${PROJECT_NAME}) endif() # diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp new file mode 100644 index 00000000..995cf485 --- /dev/null +++ b/tests/test_linear_feedback_controller.cpp @@ -0,0 +1,9 @@ +#include "linear_feedback_controller/linear_feedback_controller.hpp" +using linear_feedback_controller::LinearFeedbackController; + +#include "example-robot-data/path.hpp" +#include "gtest/gtest.h" + +TEST(LinearFeedbackControllerTest, Ctor) { + EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); +} From c29e35762bb569a2ae024e79c70ad66063c55c11 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Wed, 8 Jan 2025 10:35:00 +0100 Subject: [PATCH 02/61] [lfc] Add LoadEmptyParams unit-test --- tests/test_linear_feedback_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 995cf485..de255047 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -7,3 +7,8 @@ using linear_feedback_controller::LinearFeedbackController; TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } + +TEST(LinearFeedbackControllerTest, LoadEmptyParams) { + auto ctrl = LinearFeedbackController{}; + EXPECT_FALSE(ctrl.load({})); +} From 535e3e28b27d674742b36f1fb32ee1b03ad71d4b Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 10 Jan 2025 09:40:46 +0100 Subject: [PATCH 03/61] [lfc] Fix wrong merge conflics resolution (missing ')') --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b2856567..5a96446f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,7 +90,6 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_pd_controller tests/test_pd_controller.cpp) target_link_libraries(test_pd_controller ${PROJECT_NAME}) - ament_auto_add_gtest(test_lf_controller tests/test_lf_controller.cpp) target_link_libraries(test_lf_controller ${PROJECT_NAME}) From b7463be10cd219c8669130b0bf21f955ac4d4630 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 10 Jan 2025 09:41:45 +0100 Subject: [PATCH 04/61] [lfc] Fix merge conflict resolution (missing example- deps on lf) --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5a96446f..c3147e12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,7 +91,8 @@ if(BUILD_TESTING) target_link_libraries(test_pd_controller ${PROJECT_NAME}) ament_auto_add_gtest(test_lf_controller tests/test_lf_controller.cpp) - target_link_libraries(test_lf_controller ${PROJECT_NAME}) + target_link_libraries(test_lf_controller ${PROJECT_NAME} + example-robot-data::example-robot-data) ament_auto_add_gtest(test_linear_feedback_controller tests/test_linear_feedback_controller.cpp) From 2763133d02b7c9bce3b99e0a286108fad76c0526 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 20 Jan 2025 15:21:04 +0100 Subject: [PATCH 05/61] [lfc] Remove example-robot-data deps --- tests/test_linear_feedback_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index de255047..50cc09d4 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,7 +1,6 @@ #include "linear_feedback_controller/linear_feedback_controller.hpp" using linear_feedback_controller::LinearFeedbackController; -#include "example-robot-data/path.hpp" #include "gtest/gtest.h" TEST(LinearFeedbackControllerTest, Ctor) { From f3dfd867d76cf5e1ab1196177ea8ba24736ec2a2 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Wed, 22 Jan 2025 09:41:10 +0100 Subject: [PATCH 06/61] [lfc] Fix wrong rebase merge conflict resolution --- CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3147e12..5a96446f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,8 +91,7 @@ if(BUILD_TESTING) target_link_libraries(test_pd_controller ${PROJECT_NAME}) ament_auto_add_gtest(test_lf_controller tests/test_lf_controller.cpp) - target_link_libraries(test_lf_controller ${PROJECT_NAME} - example-robot-data::example-robot-data) + target_link_libraries(test_lf_controller ${PROJECT_NAME}) ament_auto_add_gtest(test_linear_feedback_controller tests/test_linear_feedback_controller.cpp) From 9ac5b1f58b718fcec775c3502bafd194f904c033 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Wed, 22 Jan 2025 09:41:34 +0100 Subject: [PATCH 07/61] [lfc] Add .Load* tests templates --- tests/test_linear_feedback_controller.cpp | 29 +++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 50cc09d4..7b5f9580 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,4 +1,5 @@ #include "linear_feedback_controller/linear_feedback_controller.hpp" +using linear_feedback_controller::ControllerParameters; using linear_feedback_controller::LinearFeedbackController; #include "gtest/gtest.h" @@ -11,3 +12,31 @@ TEST(LinearFeedbackControllerTest, LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; EXPECT_FALSE(ctrl.load({})); } + +TEST(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { + auto ctrl = LinearFeedbackController{}; + auto params = ControllerParameters{}; + // TODO + EXPECT_FALSE(ctrl.load(params)); +} + +TEST(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { + auto ctrl = LinearFeedbackController{}; + auto params = ControllerParameters{}; + // TODO + EXPECT_FALSE(ctrl.load(params)); +} + +TEST(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { + auto ctrl = LinearFeedbackController{}; + auto params = ControllerParameters{}; + // TODO + EXPECT_FALSE(ctrl.load(params)); +} + +TEST(LinearFeedbackControllerTest, Load) { + auto ctrl = LinearFeedbackController{}; + auto params = ControllerParameters{}; + // TODO + EXPECT_TRUE(ctrl.load(params)); +} From ed07ee40dc00d72eb03293bc36ff82ee6996efbd Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Wed, 22 Jan 2025 16:30:56 +0100 Subject: [PATCH 08/61] [utils] Add individual function to create the Joint names lists --- tests/utils/robot_model.hpp | 63 +++++++++++++++++++++++-------------- 1 file changed, 39 insertions(+), 24 deletions(-) diff --git a/tests/utils/robot_model.hpp b/tests/utils/robot_model.hpp index 2ecd45f7..8a15387e 100644 --- a/tests/utils/robot_model.hpp +++ b/tests/utils/robot_model.hpp @@ -1,7 +1,6 @@ #ifndef LINEAR_FEEDBACK_CONTROLLER_TESTS__ROBOT_MODEL_HPP_ #define LINEAR_FEEDBACK_CONTROLLER_TESTS__ROBOT_MODEL_HPP_ -#include #include #include // std::quoted #include @@ -101,6 +100,40 @@ constexpr auto PrintTo(JointDescription joint, std::ostream *os) noexcept *os << "}"; } +/// Return type of MakePairOfJointNamesFrom +struct JointNamesPair { + std::vector controlled; + std::vector moving; +}; + +/** + * @brief Create the controlled/moving list based on a list of JointDescription + * + * @param[in] joint_list List of JointDescription + * + * @return JointNamesPair Containing both controlled and moving joint names + */ +inline auto MakePairOfJointNamesFrom( + const std::vector &joint_list) -> JointNamesPair { + const auto number_of_joints = size(joint_list); + + JointNamesPair out; + out.controlled.reserve(number_of_joints); + out.moving.reserve(number_of_joints); + + for (const auto &joint : joint_list) { + if (IsControlled(joint.type)) { + out.controlled.emplace_back(joint.name); + } + + if (IsMoving(joint.type)) { + out.moving.emplace_back(joint.name); + } + } + + return out; +} + /// Global information about the model we wish to create struct ModelDescription { /// PrintFormat used by PrintTo to format a Model @@ -209,36 +242,18 @@ inline auto PrintTo(const ModelDescription &model, std::ostream *os, /** * @brief Helper function to create a RobotModelBuilder using a given \a model - * and a list of Joints to control * * @param[in] model The model description (URDF) we wish to build * - * @return std::unique_ptr A valid RobotModelBuilder (i.e. - * build_model() returned true), nullptr otherwise + * @return std::unique_ptr A valid RobotModelBuilder + * (i.e. build_model() returned true), nullptr otherwise */ inline auto MakeRobotModelBuilderFrom(const ModelDescription &model) -> std::unique_ptr { - const auto number_of_joints = size(model.joint_list); - - auto controlled_joints = std::vector{}; - controlled_joints.reserve(number_of_joints); - - auto moving_joints = std::vector{}; - moving_joints.reserve(number_of_joints); - - for (const auto &joint : model.joint_list) { - if (IsControlled(joint.type)) { - controlled_joints.emplace_back(joint.name); - } - - if (IsMoving(joint.type)) { - moving_joints.emplace_back(joint.name); - } - } - + const auto joint_lists = MakePairOfJointNamesFrom(model.joint_list); auto rmb = std::make_unique(); - if (rmb->build_model(std::string{model.urdf}, moving_joints, - controlled_joints, model.has_free_flyer)) { + if (rmb->build_model(std::string{model.urdf}, joint_lists.moving, + joint_lists.controlled, model.has_free_flyer)) { return rmb; } else { return nullptr; From c2ce62cd0572cc6ffd4f63abe79674c8fa0520c1 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Wed, 22 Jan 2025 16:32:04 +0100 Subject: [PATCH 09/61] [lfc] Add all Load tests with param setup --- tests/test_linear_feedback_controller.cpp | 142 +++++++++++++++++++++- 1 file changed, 137 insertions(+), 5 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 7b5f9580..9bbd21a1 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,14 +1,74 @@ +#include +using namespace std::literals::chrono_literals; +using std::chrono::duration_cast; +using std::chrono::milliseconds; + +#include +using namespace std::literals::string_view_literals; + +#include "utils/pd_controller.hpp" +using tests::utils::Gains; +// using tests::utils::References; + +#include "utils/robot_model.hpp" +using tests::utils::JointType; +using tests::utils::MakeAllModelDescriptionsFor; +using tests::utils::MakePairOfJointNamesFrom; +using tests::utils::ModelDescription; + #include "linear_feedback_controller/linear_feedback_controller.hpp" using linear_feedback_controller::ControllerParameters; +using linear_feedback_controller::Duration; using linear_feedback_controller::LinearFeedbackController; #include "gtest/gtest.h" +namespace { + +using LinearFeedbackControllerTestParams = + std::tuple; + +auto MakeValidParamsFrom(const LinearFeedbackControllerTestParams& test_params) + -> ControllerParameters { + const auto& [model, duration] = test_params; + + ControllerParameters out; + + out.urdf = std::string{model.urdf}; + out.robot_has_free_flyer = model.has_free_flyer; + + { + const auto joint_list = MakePairOfJointNamesFrom(model.joint_list); + out.controlled_joint_names = std::move(joint_list.controlled); + out.moving_joint_names = std::move(joint_list.moving); + } + + { + // FIXME: Size ? number of moving joints ? + const auto gains = Gains::Random(out.moving_joint_names.size()); + + out.d_gains.resize(gains.d.size()); + Eigen::Map(out.d_gains.data(), out.d_gains.size()) = + gains.d; + + out.p_gains.resize(gains.p.size()); + Eigen::Map(out.p_gains.data(), out.p_gains.size()) = + gains.p; + } + + out.pd_to_lf_transition_duration = duration; + + return out; +} + +struct LinearFeedbackControllerTest + : public ::testing::TestWithParam {}; + TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } -TEST(LinearFeedbackControllerTest, LoadEmptyParams) { +TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; EXPECT_FALSE(ctrl.load({})); } @@ -34,9 +94,81 @@ TEST(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { EXPECT_FALSE(ctrl.load(params)); } -TEST(LinearFeedbackControllerTest, Load) { +TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - auto params = ControllerParameters{}; - // TODO - EXPECT_TRUE(ctrl.load(params)); + EXPECT_TRUE(ctrl.load(MakeValidParamsFrom(GetParam()))); } + +constexpr std::string_view dummy_urdf = + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + +INSTANTIATE_TEST_SUITE_P( + DummyUrdf, LinearFeedbackControllerTest, + ::testing::Combine( + ::testing::ValuesIn(MakeAllModelDescriptionsFor( + dummy_urdf, + { + { + {.name = "l01"}, + }, + { + {.name = "l02", .type = JointType::Controlled}, + }, + { + {.name = "l01"}, + {.name = "l12"}, + }, + })), + ::testing::ValuesIn(std::vector{500ms, 1s})), + [](const auto& info) { + // NOTE: Can't use structured binding inside GTest macros + const auto& model = std::get(info.param); + const auto& duration = std::get(info.param); + + std::string str; + if (model.has_free_flyer) { + str.append("FreeFlyer_"); + } + + str.append(std::to_string(size(model.joint_list))); + str.append("_Joints"); + + for (const auto& [name, type] : model.joint_list) { + str.append("_"); + str.append(name); + str.append("_"); + str.append(ToString(type)); + } + + str.append("_Duration_"); + str.append(std::to_string(duration_cast(duration).count())); + str.append("_ms"); + + return str; + }); + +} // namespace From 3bdf08b631a0c2a08b266f8f3257fc06750f17e9 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 23 Jan 2025 10:28:39 +0100 Subject: [PATCH 10/61] [lf_controller] Refactor ModelDesc param name into PrintTo --- tests/test_lf_controller.cpp | 22 +++---------- tests/utils/robot_model.hpp | 62 +++++++++++++++++++++++------------- 2 files changed, 44 insertions(+), 40 deletions(-) diff --git a/tests/test_lf_controller.cpp b/tests/test_lf_controller.cpp index 0e6c54d7..6f3e26ec 100644 --- a/tests/test_lf_controller.cpp +++ b/tests/test_lf_controller.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -234,23 +235,10 @@ INSTANTIATE_TEST_SUITE_P( {.name = "l12"}, }, })), - [](auto&& info) { - std::string str; - if (info.param.has_free_flyer) { - str.append("FreeFlyer_"); - } - - str.append(std::to_string(size(info.param.joint_list))); - str.append("_Joints"); - - for (const auto& [name, type] : info.param.joint_list) { - str.append("_"); - str.append(name); - str.append("_"); - str.append(ToString(type)); - } - - return str; + [](const auto& info) { + std::stringstream stream; + PrintTo(info.param, &stream, {.as_param_name = true}); + return stream.str(); }); } // namespace diff --git a/tests/utils/robot_model.hpp b/tests/utils/robot_model.hpp index 8a15387e..231d1da3 100644 --- a/tests/utils/robot_model.hpp +++ b/tests/utils/robot_model.hpp @@ -178,7 +178,8 @@ inline auto MakeAllModelDescriptionsFor( /// Declaration of the PrintFormat struct ModelDescription::PrintFormat { - bool full_urdf = false; /*!< Print the full URDF string */ + bool as_param_name = false; /*!< Use the gtest param name paradigm */ + bool full_urdf = false; /*!< Print the full URDF string */ }; /** @@ -193,8 +194,6 @@ inline auto PrintTo(const ModelDescription &model, std::ostream *os, -> void { if (os == nullptr) return; - *os << "ModelDescription{"; - // Dumb function to retreive the robot name from the URDF constexpr auto GetRobotNameFromURDF = [](std::string_view urdf) -> std::optional { @@ -214,30 +213,47 @@ inline auto PrintTo(const ModelDescription &model, std::ostream *os, return res; }; - *os << ".urdf = "; - if (fmt.full_urdf) { - *os << std::quoted(model.urdf); - } else if (const auto robot_name = GetRobotNameFromURDF(model.urdf); - robot_name.has_value()) { - *os << *robot_name; - } else { - *os << "str{"; - *os << ".data() = @ " << (void const *)model.urdf.data() << ", "; - *os << ".size() = " << model.urdf.size() << ", "; + if (not fmt.as_param_name) { + *os << "ModelDescription{"; + + *os << ".urdf = "; + if (fmt.full_urdf) { + *os << std::quoted(model.urdf); + } else if (const auto robot_name = GetRobotNameFromURDF(model.urdf); + robot_name.has_value()) { + *os << *robot_name; + } else { + *os << "str{"; + *os << ".data() = @ " << (void const *)model.urdf.data() << ", "; + *os << ".size() = " << model.urdf.size() << ", "; + *os << "}"; + } + *os << ", "; + + *os << ".joint_list = [ "; + for (const auto &joint : model.joint_list) { + PrintTo(joint, os); + *os << ", "; + } + *os << "], "; + + *os << ".has_free_flyer = " << model.has_free_flyer; + *os << "}"; - } - *os << ", "; + } else { + // GTest Param name forbids with space etc... - *os << ".joint_list = [ "; - for (const auto &joint : model.joint_list) { - PrintTo(joint, os); - *os << ", "; - } - *os << "], "; + // TODO: Add URDF ? - *os << ".has_free_flyer = " << model.has_free_flyer; + if (model.has_free_flyer) { + *os << "FreeFlyer_"; + } - *os << "}"; + *os << model.joint_list.size() << "_Joints"; + for (const auto &[name, type] : model.joint_list) { + *os << "_" << name << "_" << ToString(type); + } + } } /** From 2f2b3e1390edd7e3a9c1239aa7d8a2e257d0602f Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 23 Jan 2025 10:29:25 +0100 Subject: [PATCH 11/61] [lfc] Fix URDF new line format --- tests/test_linear_feedback_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 9bbd21a1..7b2c699d 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -119,8 +119,7 @@ constexpr std::string_view dummy_urdf = " " " " " " - " " + " " " " " " " " From a6a947c2d57a0fb4194807f05b28f94a8bd98128 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 23 Jan 2025 16:48:23 +0100 Subject: [PATCH 12/61] [lfc] Use custom type instead of tuple to set the TestParams --- tests/test_linear_feedback_controller.cpp | 146 ++++++++++++++-------- 1 file changed, 94 insertions(+), 52 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 7b2c699d..9ef55ac3 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,11 +1,12 @@ #include using namespace std::literals::chrono_literals; -using std::chrono::duration_cast; -using std::chrono::milliseconds; #include using namespace std::literals::string_view_literals; +#include +#include + #include "utils/pd_controller.hpp" using tests::utils::Gains; // using tests::utils::References; @@ -25,18 +26,78 @@ using linear_feedback_controller::LinearFeedbackController; namespace { -using LinearFeedbackControllerTestParams = - std::tuple; +struct TestParams { + ModelDescription model; + Gains gains; + Duration pd_to_lf_duration; + + struct PrintFormat; +}; + +struct TestParams::PrintFormat { + ModelDescription::PrintFormat model = {}; + bool as_param_name = false; +}; + +auto PrintTo(const TestParams& params, std::ostream* os, + TestParams::PrintFormat fmt = {}) -> void { + using std::chrono::duration_cast; + using std::chrono::milliseconds; + + if (os == nullptr) return; + + if (not fmt.as_param_name) { + *os << "TestParams{"; + + *os << ".model = "; + PrintTo(params.model, os, fmt.model); + *os << ", "; + + *os << ".gains = "; + PrintTo(params.gains, os); + *os << ", "; + + *os << ".pd_to_lf_duration = " + << duration_cast(params.pd_to_lf_duration).count() + << "ms "; + + *os << "}"; + } else { + fmt.model.as_param_name = true; + PrintTo(params.model, os, fmt.model_fmt); + *os << "_" << duration_cast(params.pd_to_lf_duration).count() + << "ms"; + } +} + +auto MakeAllValidTestParamsFrom(const std::vector& models, + std::initializer_list durations) + -> std::vector { + std::vector out; + out.reserve(models.size() * durations.size()); + + for (const auto& model : models) { + const auto gains = Gains::Random(model.joint_list.size()); + for (const auto& duration : durations) { + out.emplace_back(TestParams{ + .model = model, + .gains = gains, + .pd_to_lf_duration = duration, + }); + } + } + + return out; +} -auto MakeValidParamsFrom(const LinearFeedbackControllerTestParams& test_params) +auto MakeValidParamsFrom(const TestParams& test_params) -> ControllerParameters { - const auto& [model, duration] = test_params; + const auto& [model, gains, duration] = test_params; ControllerParameters out; out.urdf = std::string{model.urdf}; out.robot_has_free_flyer = model.has_free_flyer; - { const auto joint_list = MakePairOfJointNamesFrom(model.joint_list); out.controlled_joint_names = std::move(joint_list.controlled); @@ -44,9 +105,6 @@ auto MakeValidParamsFrom(const LinearFeedbackControllerTestParams& test_params) } { - // FIXME: Size ? number of moving joints ? - const auto gains = Gains::Random(out.moving_joint_names.size()); - out.d_gains.resize(gains.d.size()); Eigen::Map(out.d_gains.data(), out.d_gains.size()) = gains.d; @@ -62,7 +120,7 @@ auto MakeValidParamsFrom(const LinearFeedbackControllerTestParams& test_params) } struct LinearFeedbackControllerTest - : public ::testing::TestWithParam {}; + : public ::testing::TestWithParam {}; TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); @@ -127,47 +185,31 @@ constexpr std::string_view dummy_urdf = INSTANTIATE_TEST_SUITE_P( DummyUrdf, LinearFeedbackControllerTest, - ::testing::Combine( - ::testing::ValuesIn(MakeAllModelDescriptionsFor( - dummy_urdf, - { - { - {.name = "l01"}, - }, - { - {.name = "l02", .type = JointType::Controlled}, - }, - { - {.name = "l01"}, - {.name = "l12"}, - }, - })), - ::testing::ValuesIn(std::vector{500ms, 1s})), + ::testing::ValuesIn(MakeAllValidTestParamsFrom( + MakeAllModelDescriptionsFor(dummy_urdf, + { + { + {.name = "l01"}, + }, + { + {.name = "l02", + .type = JointType::Controlled}, + }, + { + {.name = "l01"}, + {.name = "l12"}, + }, + }), + { + 500ms, + 1s, + })), [](const auto& info) { - // NOTE: Can't use structured binding inside GTest macros - const auto& model = std::get(info.param); - const auto& duration = std::get(info.param); - - std::string str; - if (model.has_free_flyer) { - str.append("FreeFlyer_"); - } - - str.append(std::to_string(size(model.joint_list))); - str.append("_Joints"); - - for (const auto& [name, type] : model.joint_list) { - str.append("_"); - str.append(name); - str.append("_"); - str.append(ToString(type)); - } - - str.append("_Duration_"); - str.append(std::to_string(duration_cast(duration).count())); - str.append("_ms"); - - return str; - }); + std::stringstream stream; + PrintTo(info.param, &stream, {.as_param_name = true}); + return stream.str(); + } + +); } // namespace From 1bcb70ba9ab6d45ba6507632cfb49600d30374ff Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 08:33:14 +0100 Subject: [PATCH 13/61] [lfc] Refactor some names --- tests/test_linear_feedback_controller.cpp | 28 ++++++----------------- 1 file changed, 7 insertions(+), 21 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 9ef55ac3..d45e6295 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -12,7 +12,6 @@ using tests::utils::Gains; // using tests::utils::References; #include "utils/robot_model.hpp" -using tests::utils::JointType; using tests::utils::MakeAllModelDescriptionsFor; using tests::utils::MakePairOfJointNamesFrom; using tests::utils::ModelDescription; @@ -64,7 +63,7 @@ auto PrintTo(const TestParams& params, std::ostream* os, *os << "}"; } else { fmt.model.as_param_name = true; - PrintTo(params.model, os, fmt.model_fmt); + PrintTo(params.model, os, fmt.model); *os << "_" << duration_cast(params.pd_to_lf_duration).count() << "ms"; } @@ -90,14 +89,13 @@ auto MakeAllValidTestParamsFrom(const std::vector& models, return out; } -auto MakeValidParamsFrom(const TestParams& test_params) - -> ControllerParameters { +auto MakeParamsFrom(const TestParams& test_params) -> ControllerParameters { const auto& [model, gains, duration] = test_params; ControllerParameters out; - out.urdf = std::string{model.urdf}; out.robot_has_free_flyer = model.has_free_flyer; + { const auto joint_list = MakePairOfJointNamesFrom(model.joint_list); out.controlled_joint_names = std::move(joint_list.controlled); @@ -154,7 +152,7 @@ TEST(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - EXPECT_TRUE(ctrl.load(MakeValidParamsFrom(GetParam()))); + EXPECT_TRUE(ctrl.load(MakeParamsFrom(GetParam()))); } constexpr std::string_view dummy_urdf = @@ -188,22 +186,10 @@ INSTANTIATE_TEST_SUITE_P( ::testing::ValuesIn(MakeAllValidTestParamsFrom( MakeAllModelDescriptionsFor(dummy_urdf, { - { - {.name = "l01"}, - }, - { - {.name = "l02", - .type = JointType::Controlled}, - }, - { - {.name = "l01"}, - {.name = "l12"}, - }, + {{.name = "l01"}}, + {{.name = "l01"}, {.name = "l12"}}, }), - { - 500ms, - 1s, - })), + {500ms, 1s})), [](const auto& info) { std::stringstream stream; PrintTo(info.param, &stream, {.as_param_name = true}); From b7f4b8008baea43d4a20301be66ef123f14bfedf Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 15:49:59 +0100 Subject: [PATCH 14/61] [lfc] Refactor in order to take ControllerParameters as test param --- tests/test_linear_feedback_controller.cpp | 140 ++++------------------ tests/utils/robot_model.hpp | 97 ++++++++++----- 2 files changed, 93 insertions(+), 144 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index d45e6295..8f2bf304 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -5,16 +5,10 @@ using namespace std::literals::chrono_literals; using namespace std::literals::string_view_literals; #include -#include -#include "utils/pd_controller.hpp" +#include "utils/linear_feedback_controller.hpp" using tests::utils::Gains; -// using tests::utils::References; - -#include "utils/robot_model.hpp" -using tests::utils::MakeAllModelDescriptionsFor; -using tests::utils::MakePairOfJointNamesFrom; -using tests::utils::ModelDescription; +using tests::utils::MakeAllControllerParametersFrom; #include "linear_feedback_controller/linear_feedback_controller.hpp" using linear_feedback_controller::ControllerParameters; @@ -25,100 +19,8 @@ using linear_feedback_controller::LinearFeedbackController; namespace { -struct TestParams { - ModelDescription model; - Gains gains; - Duration pd_to_lf_duration; - - struct PrintFormat; -}; - -struct TestParams::PrintFormat { - ModelDescription::PrintFormat model = {}; - bool as_param_name = false; -}; - -auto PrintTo(const TestParams& params, std::ostream* os, - TestParams::PrintFormat fmt = {}) -> void { - using std::chrono::duration_cast; - using std::chrono::milliseconds; - - if (os == nullptr) return; - - if (not fmt.as_param_name) { - *os << "TestParams{"; - - *os << ".model = "; - PrintTo(params.model, os, fmt.model); - *os << ", "; - - *os << ".gains = "; - PrintTo(params.gains, os); - *os << ", "; - - *os << ".pd_to_lf_duration = " - << duration_cast(params.pd_to_lf_duration).count() - << "ms "; - - *os << "}"; - } else { - fmt.model.as_param_name = true; - PrintTo(params.model, os, fmt.model); - *os << "_" << duration_cast(params.pd_to_lf_duration).count() - << "ms"; - } -} - -auto MakeAllValidTestParamsFrom(const std::vector& models, - std::initializer_list durations) - -> std::vector { - std::vector out; - out.reserve(models.size() * durations.size()); - - for (const auto& model : models) { - const auto gains = Gains::Random(model.joint_list.size()); - for (const auto& duration : durations) { - out.emplace_back(TestParams{ - .model = model, - .gains = gains, - .pd_to_lf_duration = duration, - }); - } - } - - return out; -} - -auto MakeParamsFrom(const TestParams& test_params) -> ControllerParameters { - const auto& [model, gains, duration] = test_params; - - ControllerParameters out; - out.urdf = std::string{model.urdf}; - out.robot_has_free_flyer = model.has_free_flyer; - - { - const auto joint_list = MakePairOfJointNamesFrom(model.joint_list); - out.controlled_joint_names = std::move(joint_list.controlled); - out.moving_joint_names = std::move(joint_list.moving); - } - - { - out.d_gains.resize(gains.d.size()); - Eigen::Map(out.d_gains.data(), out.d_gains.size()) = - gains.d; - - out.p_gains.resize(gains.p.size()); - Eigen::Map(out.p_gains.data(), out.p_gains.size()) = - gains.p; - } - - out.pd_to_lf_transition_duration = duration; - - return out; -} - struct LinearFeedbackControllerTest - : public ::testing::TestWithParam {}; + : public ::testing::TestWithParam {}; TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); @@ -152,7 +54,13 @@ TEST(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - EXPECT_TRUE(ctrl.load(MakeParamsFrom(GetParam()))); + EXPECT_TRUE(ctrl.load(GetParam())); +} + +TEST_P(LinearFeedbackControllerTest, SetInitialStateEmpty) { + auto ctrl = LinearFeedbackController{}; + ASSERT_TRUE(ctrl.load(GetParam())); + EXPECT_FALSE(ctrl.set_initial_state({}, {})); } constexpr std::string_view dummy_urdf = @@ -181,20 +89,20 @@ constexpr std::string_view dummy_urdf = " " ""; -INSTANTIATE_TEST_SUITE_P( - DummyUrdf, LinearFeedbackControllerTest, - ::testing::ValuesIn(MakeAllValidTestParamsFrom( - MakeAllModelDescriptionsFor(dummy_urdf, - { - {{.name = "l01"}}, - {{.name = "l01"}, {.name = "l12"}}, - }), - {500ms, 1s})), - [](const auto& info) { - std::stringstream stream; - PrintTo(info.param, &stream, {.as_param_name = true}); - return stream.str(); - } +INSTANTIATE_TEST_SUITE_P(DummyUrdf, LinearFeedbackControllerTest, + ::testing::ValuesIn(MakeAllControllerParametersFrom( + dummy_urdf, + { + {{.name = "l01"}}, + {{.name = "l01"}, {.name = "l12"}}, + }, + {500ms, 1s})), + [](const auto& info) { + std::stringstream stream; + PrintTo(info.param, &stream, + {.as_param_name = true}); + return stream.str(); + } ); diff --git a/tests/utils/robot_model.hpp b/tests/utils/robot_model.hpp index 231d1da3..f0869173 100644 --- a/tests/utils/robot_model.hpp +++ b/tests/utils/robot_model.hpp @@ -100,28 +100,26 @@ constexpr auto PrintTo(JointDescription joint, std::ostream *os) noexcept *os << "}"; } -/// Return type of MakePairOfJointNamesFrom struct JointNamesPair { std::vector controlled; std::vector moving; }; /** - * @brief Create the controlled/moving list based on a list of JointDescription + * @brief Create a pair of names for each JointType * - * @param[in] joint_list List of JointDescription + * @param[in] joint_desc_list List of JointDescription * - * @return JointNamesPair Containing both controlled and moving joint names + * @return JointNamesPair Pair of name list */ -inline auto MakePairOfJointNamesFrom( - const std::vector &joint_list) -> JointNamesPair { - const auto number_of_joints = size(joint_list); - +inline auto MakePairOfNamesFrom( + const std::vector &joint_desc_list) -> JointNamesPair { JointNamesPair out; - out.controlled.reserve(number_of_joints); - out.moving.reserve(number_of_joints); - for (const auto &joint : joint_list) { + out.controlled.reserve(joint_desc_list.size()); + out.moving.reserve(joint_desc_list.size()); + + for (const auto &joint : joint_desc_list) { if (IsControlled(joint.type)) { out.controlled.emplace_back(joint.name); } @@ -134,6 +132,47 @@ inline auto MakePairOfJointNamesFrom( return out; } +/** + * @brief Create a list of JointDescription from the given lists + * + * @param[in] controlled List of controlled joint names + * @param[in] moving List of moving joint names + */ +inline auto MakeJointDescriptionListFrom( + const std::vector &controlled, + const std::vector &moving) -> std::vector { + std::vector out; + + // The worst case would be that each names are differents + out.reserve(moving.size() + controlled.size()); + + for (std::string_view name : moving) { + out.emplace_back(JointDescription{ + .name = name, + .type = JointType::Moving, + }); + } + + for (std::string_view name : controlled) { + // If the name has already been pushed into out, update the type to Both + auto found = std::find_if(out.begin(), out.end(), + [name](const JointDescription &joint_desc) { + return joint_desc.name == name; + }); + + if (found != out.end()) { + found->type = JointType::Both; + } else { + out.emplace_back(JointDescription{ + .name = name, + .type = JointType::Controlled, + }); + } + } + + return out; +} + /// Global information about the model we wish to create struct ModelDescription { /// PrintFormat used by PrintTo to format a Model @@ -149,28 +188,30 @@ struct ModelDescription { * for each joint_list provided * * @param[in] urdf The common URDF used by every ModelDescription - * @param[in] all_joint_lists All arguments forwarded to the '.joint_list = ' - * constructor + * @param[in] all_joint_lists All arguments forwarded to the '.joint_list = + * ' constructor * - * @return std::vector> Containing all + * @return std::vector Containing all * ModelDescriptions constructed */ inline auto MakeAllModelDescriptionsFor( - std::string_view urdf, std::initializer_list> - all_joint_lists) noexcept + std::string_view urdf, + std::initializer_list> all_joint_lists) -> std::vector { std::vector out; - out.reserve(2 * all_joint_lists.size()); - for (auto has_free_flyer : {false, true}) { - for (const auto &joint_list : all_joint_lists) { - out.emplace_back(ModelDescription{ - .urdf = urdf, - .joint_list = joint_list, - .has_free_flyer = has_free_flyer, - }); - } + for (const auto &joint_list : all_joint_lists) { + out.emplace_back(ModelDescription{ + .urdf = urdf, + .joint_list = joint_list, + .has_free_flyer = false, + }); + out.emplace_back(ModelDescription{ + .urdf = urdf, + .joint_list = std::move(joint_list), + .has_free_flyer = true, + }); } return out; @@ -266,10 +307,10 @@ inline auto PrintTo(const ModelDescription &model, std::ostream *os, */ inline auto MakeRobotModelBuilderFrom(const ModelDescription &model) -> std::unique_ptr { - const auto joint_lists = MakePairOfJointNamesFrom(model.joint_list); + auto [controlled, moving] = MakePairOfNamesFrom(model.joint_list); auto rmb = std::make_unique(); - if (rmb->build_model(std::string{model.urdf}, joint_lists.moving, - joint_lists.controlled, model.has_free_flyer)) { + if (rmb->build_model(std::string{model.urdf}, std::move(moving), + std::move(controlled), model.has_free_flyer)) { return rmb; } else { return nullptr; From 066fc36d0c81876dcf56acb9b9213e568258bcc7 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 15:50:27 +0100 Subject: [PATCH 15/61] [lfc] Add missing header --- tests/utils/linear_feedback_controller.hpp | 111 +++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 tests/utils/linear_feedback_controller.hpp diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp new file mode 100644 index 00000000..563818f1 --- /dev/null +++ b/tests/utils/linear_feedback_controller.hpp @@ -0,0 +1,111 @@ +#pragma once + +#include // duration_cast/milliseconds + +#include "lf_controller.hpp" +#include "linear_feedback_controller/linear_feedback_controller.hpp" +#include "pd_controller.hpp" +#include "robot_model.hpp" + +namespace tests::utils { + +inline auto MakeAllControllerParametersFrom( + std::string_view urdf, + std::initializer_list> all_joint_lists, + std::initializer_list durations) + -> std::vector { + using linear_feedback_controller::ControllerParameters; + + const auto FromEigen = [](const Eigen::VectorXd& val) -> std::vector { + std::vector out; + out.resize(val.size()); + Eigen::Map(out.data(), out.size()) = val; + return out; + }; + + std::vector out; + + out.reserve(2 * all_joint_lists.size() * durations.size()); + for (const auto& joint_list : all_joint_lists) { + const auto joint_names = MakePairOfNamesFrom(joint_list); + + // FIXME : Controlled size ? + const auto gains = Gains::Random(joint_names.controlled.size()); + + for (const auto& duration : durations) { + for (auto has_free_flyer : {false, true}) { + out.emplace_back(ControllerParameters{ + .urdf = std::string{urdf}, + .moving_joint_names = joint_names.moving, + .p_gains = FromEigen(gains.p), + .d_gains = FromEigen(gains.d), + .controlled_joint_names = joint_names.controlled, + .robot_has_free_flyer = has_free_flyer, + .pd_to_lf_transition_duration = duration, + }); + } + } + } + + return out; +} + +/// PrintTo Format used by PrintTo +struct ControllerParametersPrintFormat { + tests::utils::ModelDescription::PrintFormat model = {}; + bool as_param_name = false; +}; + +} // namespace tests::utils + +// For ADL +namespace linear_feedback_controller { + +inline auto PrintTo(const ControllerParameters& params, std::ostream* os, + tests::utils::ControllerParametersPrintFormat fmt = {}) + -> void { + if (os == nullptr) return; + + using tests::utils::MakeJointDescriptionListFrom; + const auto model = tests::utils::ModelDescription{ + .urdf = params.urdf, + .joint_list = MakeJointDescriptionListFrom(params.controlled_joint_names, + params.moving_joint_names), + .has_free_flyer = params.robot_has_free_flyer, + }; + + constexpr auto ToEigen = [](const std::vector& v) { + return Eigen::Map(v.data(), v.size()); + }; + + const auto gains = tests::utils::Gains{ + .p = ToEigen(params.p_gains), + .d = ToEigen(params.d_gains), + }; + + using std::chrono::duration_cast; + using std::chrono::milliseconds; + const auto duration = + duration_cast(params.pd_to_lf_transition_duration).count(); + + if (not fmt.as_param_name) { + *os << "ControllerParameters{"; + + PrintTo(model, os, fmt.model); + *os << ", "; + + PrintTo(gains, os); + *os << ", "; + + *os << ".pd_to_lf_transition_duration = " << duration << "ms "; + + *os << "}"; + } else { + fmt.model.as_param_name = true; + PrintTo(model, os, fmt.model); + // TODO gains ? + *os << "_" << duration << "ms"; + } +} + +} // namespace linear_feedback_controller From 2377b52c1cf8b3a33bfe15ab080ea2c531e06416 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:18:23 +0100 Subject: [PATCH 16/61] [lfc] Use GTest EXPECT_PRED with matchers --- tests/test_linear_feedback_controller.cpp | 66 +++++++++++++++-------- 1 file changed, 45 insertions(+), 21 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 8f2bf304..4e4f73c9 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,14 +1,9 @@ -#include -using namespace std::literals::chrono_literals; - -#include -using namespace std::literals::string_view_literals; - #include +#include #include "utils/linear_feedback_controller.hpp" -using tests::utils::Gains; using tests::utils::MakeAllControllerParametersFrom; +using tests::utils::References; #include "linear_feedback_controller/linear_feedback_controller.hpp" using linear_feedback_controller::ControllerParameters; @@ -22,45 +17,73 @@ namespace { struct LinearFeedbackControllerTest : public ::testing::TestWithParam {}; +template +constexpr auto Not(Pred&& pred) { + return [&pred](auto&&... val) { + return not pred(std::forward(val)...); + }; +} + +constexpr auto CorrectlyLoads(LinearFeedbackController& ctrl) { + return [&](const ControllerParameters& params) { return ctrl.load(params); }; +} + +constexpr auto FailsToLoad(LinearFeedbackController& ctrl) { + return Not(CorrectlyLoads(ctrl)); +} + +constexpr auto CorrectlySetInitialState(LinearFeedbackController& ctrl) { + return [&](const References& refs) { + return ctrl.set_initial_state(refs.tau, refs.q); + }; +} + +constexpr auto FailsToSetInitialState(LinearFeedbackController& ctrl) { + return Not(CorrectlySetInitialState(ctrl)); +} + TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; - EXPECT_FALSE(ctrl.load({})); + EXPECT_PRED1(Not(CorrectlyLoads(ctrl)), ControllerParameters{}); } -TEST(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { +TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { auto ctrl = LinearFeedbackController{}; - auto params = ControllerParameters{}; - // TODO - EXPECT_FALSE(ctrl.load(params)); + auto no_urdf_param = GetParam(); + no_urdf_param.urdf.clear(); + EXPECT_PRED1(FailsToLoad(ctrl), no_urdf_param); } -TEST(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { +TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto ctrl = LinearFeedbackController{}; - auto params = ControllerParameters{}; + auto params = GetParam(); // TODO EXPECT_FALSE(ctrl.load(params)); } -TEST(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { +TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { auto ctrl = LinearFeedbackController{}; - auto params = ControllerParameters{}; - // TODO - EXPECT_FALSE(ctrl.load(params)); + + auto negative_duration_params = GetParam(); + negative_duration_params.pd_to_lf_transition_duration = + -negative_duration_params.pd_to_lf_transition_duration; + + EXPECT_PRED1(FailsToLoad(ctrl), negative_duration_params); } TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - EXPECT_TRUE(ctrl.load(GetParam())); + EXPECT_PRED1(CorrectlyLoads(ctrl), GetParam()); } TEST_P(LinearFeedbackControllerTest, SetInitialStateEmpty) { auto ctrl = LinearFeedbackController{}; - ASSERT_TRUE(ctrl.load(GetParam())); - EXPECT_FALSE(ctrl.set_initial_state({}, {})); + ASSERT_PRED1(CorrectlyLoads(ctrl), GetParam()); + EXPECT_PRED1(FailsToSetInitialState(ctrl), References{}); } constexpr std::string_view dummy_urdf = @@ -89,6 +112,7 @@ constexpr std::string_view dummy_urdf = " " ""; +using namespace std::literals::chrono_literals; INSTANTIATE_TEST_SUITE_P(DummyUrdf, LinearFeedbackControllerTest, ::testing::ValuesIn(MakeAllControllerParametersFrom( dummy_urdf, From 2233bb0376b27b78ec2f011818d558c4934adf61 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:23:20 +0100 Subject: [PATCH 17/61] [lfc] Fix missing FailsToLoad() call --- tests/test_linear_feedback_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 4e4f73c9..cc300497 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -48,7 +48,7 @@ TEST(LinearFeedbackControllerTest, Ctor) { TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(Not(CorrectlyLoads(ctrl)), ControllerParameters{}); + EXPECT_PRED1(FailsToLoad(ctrl), ControllerParameters{}); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { From 1bcf3ccad0e756d2e51a6a017f885f1642e8c83d Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:32:15 +0100 Subject: [PATCH 18/61] [lfc] Refactor Predicates names --- tests/test_linear_feedback_controller.cpp | 28 ++++++++--------------- 1 file changed, 10 insertions(+), 18 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index cc300497..634a6487 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -18,44 +18,36 @@ struct LinearFeedbackControllerTest : public ::testing::TestWithParam {}; template -constexpr auto Not(Pred&& pred) { +constexpr auto DoNot(Pred&& pred) { return [&pred](auto&&... val) { return not pred(std::forward(val)...); }; } -constexpr auto CorrectlyLoads(LinearFeedbackController& ctrl) { +constexpr auto Loads(LinearFeedbackController& ctrl) { return [&](const ControllerParameters& params) { return ctrl.load(params); }; } -constexpr auto FailsToLoad(LinearFeedbackController& ctrl) { - return Not(CorrectlyLoads(ctrl)); -} - -constexpr auto CorrectlySetInitialState(LinearFeedbackController& ctrl) { +constexpr auto SetInitialState(LinearFeedbackController& ctrl) { return [&](const References& refs) { return ctrl.set_initial_state(refs.tau, refs.q); }; } -constexpr auto FailsToSetInitialState(LinearFeedbackController& ctrl) { - return Not(CorrectlySetInitialState(ctrl)); -} - TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(FailsToLoad(ctrl), ControllerParameters{}); + EXPECT_PRED1(DoNot(Loads(ctrl)), ControllerParameters{}); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { auto ctrl = LinearFeedbackController{}; auto no_urdf_param = GetParam(); no_urdf_param.urdf.clear(); - EXPECT_PRED1(FailsToLoad(ctrl), no_urdf_param); + EXPECT_PRED1(DoNot(Loads(ctrl)), no_urdf_param); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { @@ -72,18 +64,18 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { negative_duration_params.pd_to_lf_transition_duration = -negative_duration_params.pd_to_lf_transition_duration; - EXPECT_PRED1(FailsToLoad(ctrl), negative_duration_params); + EXPECT_PRED1(DoNot(Loads(ctrl)), negative_duration_params); } TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(CorrectlyLoads(ctrl), GetParam()); + EXPECT_PRED1(Loads(ctrl), GetParam()); } -TEST_P(LinearFeedbackControllerTest, SetInitialStateEmpty) { +TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateEmpty) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(CorrectlyLoads(ctrl), GetParam()); - EXPECT_PRED1(FailsToSetInitialState(ctrl), References{}); + ASSERT_PRED1(Loads(ctrl), GetParam()); + EXPECT_PRED1(DoNot(SetInitialState(ctrl)), References{}); } constexpr std::string_view dummy_urdf = From 8e11682db83271c12033ce21b6125c74c989c8f9 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:38:51 +0100 Subject: [PATCH 19/61] [lfc] Add test to Refs size mismatches --- tests/test_linear_feedback_controller.cpp | 31 +++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 634a6487..3d7a4122 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -78,6 +78,37 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateEmpty) { EXPECT_PRED1(DoNot(SetInitialState(ctrl)), References{}); } +TEST_P(LinearFeedbackControllerTest, + DISABLED_SetInitialStateSizeMismatchInternal) { + auto ctrl = LinearFeedbackController{}; + ASSERT_PRED1(Loads(ctrl), GetParam()); + const auto good_refs = References::Random(GetParam().d_gains.size()); + + { + auto tau_bigger = good_refs; + tau_bigger.tau << tau_bigger.tau, 1.0; + EXPECT_PRED1(DoNot(SetInitialState(ctrl)), tau_bigger); + } + + { + auto tau_smaller = good_refs; + tau_smaller.tau.conservativeResize(tau_smaller.tau.size() - 1); + EXPECT_PRED1(DoNot(SetInitialState(ctrl)), tau_smaller); + } + + { + auto q_bigger = good_refs; + q_bigger.q << q_bigger.q, 1.0; + EXPECT_PRED1(DoNot(SetInitialState(ctrl)), q_bigger); + } + + { + auto q_smaller = good_refs; + q_smaller.q.conservativeResize(q_smaller.q.size() - 1); + EXPECT_PRED1(DoNot(SetInitialState(ctrl)), q_smaller); + } +} + constexpr std::string_view dummy_urdf = "" "" From 257878e6425d8723ff4d8fe3bd5b90c83cb5a04f Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:42:24 +0100 Subject: [PATCH 20/61] [lfc] Add valid SetInitialState tests --- tests/test_linear_feedback_controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 3d7a4122..9df61b2f 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -109,6 +109,13 @@ TEST_P(LinearFeedbackControllerTest, } } +TEST_P(LinearFeedbackControllerTest, SetInitialState) { + auto ctrl = LinearFeedbackController{}; + ASSERT_PRED1(Loads(ctrl), GetParam()); + EXPECT_PRED1(SetInitialState(ctrl), + References::Random(GetParam().d_gains.size())); +} + constexpr std::string_view dummy_urdf = "" "" From f9b2be0dbd2bf653978c6f9769d11e7e0ca4306c Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:44:00 +0100 Subject: [PATCH 21/61] [lfc] Rename SizeMismatchInternal to SizeMismatch --- tests/test_linear_feedback_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 9df61b2f..4edb0841 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -78,8 +78,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateEmpty) { EXPECT_PRED1(DoNot(SetInitialState(ctrl)), References{}); } -TEST_P(LinearFeedbackControllerTest, - DISABLED_SetInitialStateSizeMismatchInternal) { +TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { auto ctrl = LinearFeedbackController{}; ASSERT_PRED1(Loads(ctrl), GetParam()); const auto good_refs = References::Random(GetParam().d_gains.size()); From 919f5e05f3cb312917cf23362edd0bbe4a455e86 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 16:51:14 +0100 Subject: [PATCH 22/61] [lfc] Add Nan test to set_initial_state --- tests/test_linear_feedback_controller.cpp | 28 +++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 4edb0841..c38fa99d 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,6 +1,9 @@ #include #include +#include "utils/mutation.hpp" +using tests::utils::TemporaryMutate; + #include "utils/linear_feedback_controller.hpp" using tests::utils::MakeAllControllerParametersFrom; using tests::utils::References; @@ -108,6 +111,31 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { } } +#define MakeRefOf(val) std::make_tuple(std::ref((val)), std::string_view{#val}) + +TEST_P(LinearFeedbackControllerTest, SetInitialStateSpecialDouble) { + auto ctrl = LinearFeedbackController{}; + ASSERT_PRED1(Loads(ctrl), GetParam()); + auto refs = References::Random(GetParam().d_gains.size()); + + for (const auto& [ref, str] : { + MakeRefOf(refs.q(0)), + MakeRefOf(refs.q.tail<1>()[0]), + MakeRefOf(refs.tau(0)), + MakeRefOf(refs.tau.tail<1>()[0]), + }) { + for (auto tmp_value : { + std::numeric_limits::infinity(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::signaling_NaN(), + }) { + const auto mutation = TemporaryMutate(ref, tmp_value); + EXPECT_PRED1(DoNot(SetInitialState(ctrl)), refs) + << str << " = " << ref << " (was " << mutation.OldValue() << ")"; + } + } +} + TEST_P(LinearFeedbackControllerTest, SetInitialState) { auto ctrl = LinearFeedbackController{}; ASSERT_PRED1(Loads(ctrl), GetParam()); From bd6c71a44cb5d595ef8390b4335743cc519f7b8b Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 17:00:05 +0100 Subject: [PATCH 23/61] [lfc] Rename Loads -> Load + Add tempalte ComputeControl --- tests/test_linear_feedback_controller.cpp | 31 ++++++++++++++++------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index c38fa99d..2ce138ad 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -27,7 +27,7 @@ constexpr auto DoNot(Pred&& pred) { }; } -constexpr auto Loads(LinearFeedbackController& ctrl) { +constexpr auto Load(LinearFeedbackController& ctrl) { return [&](const ControllerParameters& params) { return ctrl.load(params); }; } @@ -37,20 +37,26 @@ constexpr auto SetInitialState(LinearFeedbackController& ctrl) { }; } +constexpr auto SuccesfullyInitialized(LinearFeedbackController& ctrl) { + return [&](const ControllerParameters& params, const References& refs) { + return ctrl.load(params) and ctrl.set_initial_state(refs.tau, refs.q); + }; +} + TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(DoNot(Loads(ctrl)), ControllerParameters{}); + EXPECT_PRED1(DoNot(Load(ctrl)), ControllerParameters{}); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { auto ctrl = LinearFeedbackController{}; auto no_urdf_param = GetParam(); no_urdf_param.urdf.clear(); - EXPECT_PRED1(DoNot(Loads(ctrl)), no_urdf_param); + EXPECT_PRED1(DoNot(Load(ctrl)), no_urdf_param); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { @@ -67,23 +73,23 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { negative_duration_params.pd_to_lf_transition_duration = -negative_duration_params.pd_to_lf_transition_duration; - EXPECT_PRED1(DoNot(Loads(ctrl)), negative_duration_params); + EXPECT_PRED1(DoNot(Load(ctrl)), negative_duration_params); } TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(Loads(ctrl), GetParam()); + EXPECT_PRED1(Load(ctrl), GetParam()); } TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateEmpty) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Loads(ctrl), GetParam()); + ASSERT_PRED1(Load(ctrl), GetParam()); EXPECT_PRED1(DoNot(SetInitialState(ctrl)), References{}); } TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Loads(ctrl), GetParam()); + ASSERT_PRED1(Load(ctrl), GetParam()); const auto good_refs = References::Random(GetParam().d_gains.size()); { @@ -115,7 +121,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { TEST_P(LinearFeedbackControllerTest, SetInitialStateSpecialDouble) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Loads(ctrl), GetParam()); + ASSERT_PRED1(Load(ctrl), GetParam()); auto refs = References::Random(GetParam().d_gains.size()); for (const auto& [ref, str] : { @@ -138,11 +144,18 @@ TEST_P(LinearFeedbackControllerTest, SetInitialStateSpecialDouble) { TEST_P(LinearFeedbackControllerTest, SetInitialState) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Loads(ctrl), GetParam()); + ASSERT_PRED1(Load(ctrl), GetParam()); EXPECT_PRED1(SetInitialState(ctrl), References::Random(GetParam().d_gains.size())); } +TEST_P(LinearFeedbackControllerTest, ComputeControl) { + auto ctrl = LinearFeedbackController{}; + ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), + References::Random(GetParam().d_gains.size())); + // TODO +} + constexpr std::string_view dummy_urdf = "" "" From c9b1b310dfec5baf13b7d9062ea96c63ba86c71f Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 24 Jan 2025 17:00:26 +0100 Subject: [PATCH 24/61] [utils] Remove useless static assert from mutation --- tests/utils/mutation.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tests/utils/mutation.hpp b/tests/utils/mutation.hpp index effddeda..665e8f19 100644 --- a/tests/utils/mutation.hpp +++ b/tests/utils/mutation.hpp @@ -39,9 +39,7 @@ struct [[nodiscard]] Mutation final { */ template constexpr Mutation(ValueType &value, T &&tmp) - : Mutation(std::addressof(value), std::forward(tmp)) { - static_assert(not std::is_const_v); - } + : Mutation(std::addressof(value), std::forward(tmp)) {} /// Move Ctor constexpr Mutation(Mutation &&other) { From c3761999a9c1626cb89a69c5c2ddecfe75b61c6e Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 08:53:14 +0100 Subject: [PATCH 25/61] [lfc] Disable SetInitialStateSpecialDouble test --- tests/test_linear_feedback_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 2ce138ad..917d1b7b 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -119,7 +119,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { #define MakeRefOf(val) std::make_tuple(std::ref((val)), std::string_view{#val}) -TEST_P(LinearFeedbackControllerTest, SetInitialStateSpecialDouble) { +TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSpecialDouble) { auto ctrl = LinearFeedbackController{}; ASSERT_PRED1(Load(ctrl), GetParam()); auto refs = References::Random(GetParam().d_gains.size()); From 216e6e37841d5e0bb606677c6ce3cf4bb2a0241e Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 09:21:34 +0100 Subject: [PATCH 26/61] [lfc] Add additional expectations in SetInitialState --- tests/test_linear_feedback_controller.cpp | 17 +++++++++++++++++ tests/utils/linear_feedback_controller.hpp | 1 - 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 917d1b7b..d58f19bd 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -4,6 +4,10 @@ #include "utils/mutation.hpp" using tests::utils::TemporaryMutate; +#include "utils/lf_controller.hpp" +using tests::utils::MakeValidRandomControlFor; +using tests::utils::MakeValidRandomSensorFor; + #include "utils/linear_feedback_controller.hpp" using tests::utils::MakeAllControllerParametersFrom; using tests::utils::References; @@ -147,12 +151,25 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { ASSERT_PRED1(Load(ctrl), GetParam()); EXPECT_PRED1(SetInitialState(ctrl), References::Random(GetParam().d_gains.size())); + + ASSERT_NE(ctrl.get_robot_model(), nullptr); + EXPECT_EQ(ctrl.get_robot_model()->get_moving_joint_names(), + GetParam().moving_joint_names); + + EXPECT_EQ(ctrl.get_robot_model()->get_robot_has_free_flyer(), + GetParam().robot_has_free_flyer); + + // Other verifications based on RMB ? ... } TEST_P(LinearFeedbackControllerTest, ComputeControl) { auto ctrl = LinearFeedbackController{}; ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), References::Random(GetParam().d_gains.size())); + + const auto control = MakeValidRandomControlFor(*ctrl.get_robot_model()); + const auto sensor = MakeValidRandomSensorFor(*ctrl.get_robot_model()); + // TODO } diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp index 563818f1..1532c2ba 100644 --- a/tests/utils/linear_feedback_controller.hpp +++ b/tests/utils/linear_feedback_controller.hpp @@ -2,7 +2,6 @@ #include // duration_cast/milliseconds -#include "lf_controller.hpp" #include "linear_feedback_controller/linear_feedback_controller.hpp" #include "pd_controller.hpp" #include "robot_model.hpp" From 6061fa499d743c271832383cc4a533b1873eaa02 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 09:24:17 +0100 Subject: [PATCH 27/61] [lfc] Add RMB ptr expectations on Load* tests --- tests/test_linear_feedback_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index d58f19bd..889ec99b 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -54,6 +54,7 @@ TEST(LinearFeedbackControllerTest, Ctor) { TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; EXPECT_PRED1(DoNot(Load(ctrl)), ControllerParameters{}); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { @@ -61,6 +62,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { auto no_urdf_param = GetParam(); no_urdf_param.urdf.clear(); EXPECT_PRED1(DoNot(Load(ctrl)), no_urdf_param); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { @@ -68,6 +70,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto params = GetParam(); // TODO EXPECT_FALSE(ctrl.load(params)); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { @@ -78,11 +81,13 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { -negative_duration_params.pd_to_lf_transition_duration; EXPECT_PRED1(DoNot(Load(ctrl)), negative_duration_params); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); } TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; EXPECT_PRED1(Load(ctrl), GetParam()); + EXPECT_NE(ctrl.get_robot_model(), nullptr); } TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateEmpty) { From fc369c0f13aa41d1dc6b8352b8b51d7993d3da37 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 10:33:40 +0100 Subject: [PATCH 28/61] [lfc] Refactor DoNot predicate into utils/core --- tests/test_linear_feedback_controller.cpp | 10 ++-- tests/utils/core.hpp | 61 +++++++++++++++++++++++ 2 files changed, 64 insertions(+), 7 deletions(-) create mode 100644 tests/utils/core.hpp diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 889ec99b..f59c6b10 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,6 +1,9 @@ #include #include +#include "utils/core.hpp" +using tests::utils::DoNot; + #include "utils/mutation.hpp" using tests::utils::TemporaryMutate; @@ -24,13 +27,6 @@ namespace { struct LinearFeedbackControllerTest : public ::testing::TestWithParam {}; -template -constexpr auto DoNot(Pred&& pred) { - return [&pred](auto&&... val) { - return not pred(std::forward(val)...); - }; -} - constexpr auto Load(LinearFeedbackController& ctrl) { return [&](const ControllerParameters& params) { return ctrl.load(params); }; } diff --git a/tests/utils/core.hpp b/tests/utils/core.hpp new file mode 100644 index 00000000..0db9ec2b --- /dev/null +++ b/tests/utils/core.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +namespace tests::utils { + +/** + * @return A predicate functor calling the underlying predicate and + * returning it's negation + * + * @param[in] pred A simple predicate taking any arguments and returning a bool + */ +template +constexpr auto DoNot(Pred&& pred) { + return [&](auto&&... arg) -> bool { + return not pred(std::forward(arg)...); + }; +} + +/** + * @return A predicate functor returning true if ANY one of the underlying + * predicates returns true + * + * @param[in] ...preds All predicates + * + * @warning Predicates will take the exact same arguments + */ +template +constexpr auto AnyOf(Preds&&... preds) { + return [&](const auto&... arg) -> bool { return (... or preds(arg...)); }; +} + +/** + * @return A predicate functor returning true if ALL the underlying predicates + * returns true + * + * @param[in] ...preds All predicates + * + * @warning Predicates will take the exact same arguments + */ +template +constexpr auto AllOf(Preds&&... preds) { + return [&](const auto&... arg) -> bool { return (... and preds(arg...)); }; +} + +/** + * @return A predicate functor returning the result of calling \a pred by + * filtering the input args based on the indices provided + * + * @param[in] ...preds All predicates + */ +template +constexpr auto WithArgs(Pred&& pred) { + return [&](auto&&... arg) { + auto t = std::forward_as_tuple(std::forward(arg)...); + return pred(std::forward(t))>(std::get(t))...); + }; +} + +} // namespace tests::utils From ae9d308a53e7d6b34b5009f44be8dac5e13c4fbb Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 13:04:08 +0100 Subject: [PATCH 29/61] [pd_controller] Refactor the expected computation of compute_control --- tests/test_pd_controller.cpp | 8 ++------ tests/utils/pd_controller.hpp | 19 +++++++++++++++++++ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/tests/test_pd_controller.cpp b/tests/test_pd_controller.cpp index 865a8667..987a3c4a 100644 --- a/tests/test_pd_controller.cpp +++ b/tests/test_pd_controller.cpp @@ -5,6 +5,7 @@ using tests::utils::TemporaryMutate; #include "utils/pd_controller.hpp" +using tests::utils::ComputeExpectedControlFrom; using tests::utils::Gains; using tests::utils::References; @@ -180,13 +181,8 @@ TEST(PdControllerTest, ComputeControl) { SCOPED_TRACE(trace_log.str()); - // clang-format off - // o = tau_r - (p * (q - q_r)) - (d * v) const Eigen::VectorXd expected_control = - (refs.tau.array() - - (gains.p.array() * (arg_q - refs.q).array()) - - (gains.d.array() * arg_v.array())); - // clang-format on + ComputeExpectedControlFrom(gains, refs, arg_q, arg_v); SetTo(pd_ctrl, gains); SetTo(pd_ctrl, refs); diff --git a/tests/utils/pd_controller.hpp b/tests/utils/pd_controller.hpp index a71ee4d6..1d7f2f9b 100644 --- a/tests/utils/pd_controller.hpp +++ b/tests/utils/pd_controller.hpp @@ -137,6 +137,25 @@ constexpr auto operator<<(std::ostream &os, const References &references) return os; } +/** + * @return The expected control vector that should return a PDController + * + * @param[in] gains Expected gains + * @param[in] refs Expected references + * @param[in] q Expected input states + * @param[in] v Expected input velocities + */ +inline auto ComputeExpectedControlFrom(const Gains &gains, + const References &refs, + const Eigen::VectorXd &q, + const Eigen::VectorXd &v) { + // clang-format off + // o = tau_r - (p * (q - q_r)) - (d * v) + return (refs.tau.array() + - (gains.p.array() * (q - refs.q).array()) + - (gains.d.array() * v.array())); + // clang-format on +} } // namespace tests::utils #endif // LINEAR_FEEDBACK_CONTROLLER_TESTS__PD_CONTROLLER_HPP_ From ce633621caa893556d604db9490bf087020b418d Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 13:14:04 +0100 Subject: [PATCH 30/61] [pd_controller] Rename Expected* function --- tests/test_pd_controller.cpp | 2 +- tests/utils/pd_controller.hpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/tests/test_pd_controller.cpp b/tests/test_pd_controller.cpp index 987a3c4a..b4c777d5 100644 --- a/tests/test_pd_controller.cpp +++ b/tests/test_pd_controller.cpp @@ -182,7 +182,7 @@ TEST(PdControllerTest, ComputeControl) { SCOPED_TRACE(trace_log.str()); const Eigen::VectorXd expected_control = - ComputeExpectedControlFrom(gains, refs, arg_q, arg_v); + ExpectedPDControlFrom(gains, refs, arg_q, arg_v); SetTo(pd_ctrl, gains); SetTo(pd_ctrl, refs); diff --git a/tests/utils/pd_controller.hpp b/tests/utils/pd_controller.hpp index 1d7f2f9b..30a4b29b 100644 --- a/tests/utils/pd_controller.hpp +++ b/tests/utils/pd_controller.hpp @@ -145,10 +145,9 @@ constexpr auto operator<<(std::ostream &os, const References &references) * @param[in] q Expected input states * @param[in] v Expected input velocities */ -inline auto ComputeExpectedControlFrom(const Gains &gains, - const References &refs, - const Eigen::VectorXd &q, - const Eigen::VectorXd &v) { +inline auto ExpectedPDControlFrom(const Gains &gains, const References &refs, + const Eigen::VectorXd &q, + const Eigen::VectorXd &v) { // clang-format off // o = tau_r - (p * (q - q_r)) - (d * v) return (refs.tau.array() From f90b3fc03f49109bb70b40a6e7e12add1de664b5 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 13:14:35 +0100 Subject: [PATCH 31/61] [lfc] Rename SizeMismatch PRED --- tests/test_linear_feedback_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index f59c6b10..d7a0cf7f 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -63,9 +63,9 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto ctrl = LinearFeedbackController{}; - auto params = GetParam(); + auto param_size_mismatch = GetParam(); // TODO - EXPECT_FALSE(ctrl.load(params)); + EXPECT_PRED1(DoNot(Load(ctrl)), param_size_mismatch); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } From 3658a320e16e4fb423c41601fee2f26fc3da0fbf Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 14:20:01 +0100 Subject: [PATCH 32/61] [pd_controller] Force the Eigen type return by ExpectedPD* function --- tests/utils/pd_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/utils/pd_controller.hpp b/tests/utils/pd_controller.hpp index 30a4b29b..c8c82a71 100644 --- a/tests/utils/pd_controller.hpp +++ b/tests/utils/pd_controller.hpp @@ -147,7 +147,7 @@ constexpr auto operator<<(std::ostream &os, const References &references) */ inline auto ExpectedPDControlFrom(const Gains &gains, const References &refs, const Eigen::VectorXd &q, - const Eigen::VectorXd &v) { + const Eigen::VectorXd &v) -> Eigen::VectorXd { // clang-format off // o = tau_r - (p * (q - q_r)) - (d * v) return (refs.tau.array() From 9ffe30faaf0cec093b0243743d1094fd3f363727 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 14:20:28 +0100 Subject: [PATCH 33/61] [pd_controller] Fix missing function rename --- tests/test_pd_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_pd_controller.cpp b/tests/test_pd_controller.cpp index b4c777d5..1f6e5be6 100644 --- a/tests/test_pd_controller.cpp +++ b/tests/test_pd_controller.cpp @@ -5,7 +5,7 @@ using tests::utils::TemporaryMutate; #include "utils/pd_controller.hpp" -using tests::utils::ComputeExpectedControlFrom; +using tests::utils::ExpectedPDControlFrom; using tests::utils::Gains; using tests::utils::References; From 194e4d7570535a868834607e57b2fe604428b428 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 14:20:44 +0100 Subject: [PATCH 34/61] [lfc] Implements ComputeControl test (still miss gravitation stuff) --- tests/test_linear_feedback_controller.cpp | 48 ++++++++++++++++++++--- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index d7a0cf7f..de2e590b 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -7,13 +7,22 @@ using tests::utils::DoNot; #include "utils/mutation.hpp" using tests::utils::TemporaryMutate; +#include "utils/pd_controller.hpp" +using tests::utils::ExpectedPDControlFrom; +using tests::utils::Gains; +using tests::utils::References; + +#include "utils/eigen_conversions.hpp" +using linear_feedback_controller_msgs::Eigen::Control; +using linear_feedback_controller_msgs::Eigen::Sensor; + #include "utils/lf_controller.hpp" +using tests::utils::ExpectedLFControlFrom; using tests::utils::MakeValidRandomControlFor; using tests::utils::MakeValidRandomSensorFor; #include "utils/linear_feedback_controller.hpp" using tests::utils::MakeAllControllerParametersFrom; -using tests::utils::References; #include "linear_feedback_controller/linear_feedback_controller.hpp" using linear_feedback_controller::ControllerParameters; @@ -22,6 +31,8 @@ using linear_feedback_controller::LinearFeedbackController; #include "gtest/gtest.h" +using namespace std::literals::chrono_literals; + namespace { struct LinearFeedbackControllerTest @@ -165,13 +176,41 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { TEST_P(LinearFeedbackControllerTest, ComputeControl) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), - References::Random(GetParam().d_gains.size())); + const auto refs = References::Random(GetParam().d_gains.size()); + + ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), refs); + + constexpr auto ToEigen = [](const std::vector& v) { + return Eigen::Map(v.data(), v.size()); + }; + + const auto gains = Gains{ + .p = ToEigen(GetParam().p_gains), + .d = ToEigen(GetParam().d_gains), + }; const auto control = MakeValidRandomControlFor(*ctrl.get_robot_model()); const auto sensor = MakeValidRandomSensorFor(*ctrl.get_robot_model()); - // TODO + // First call always calls PDController + const auto first_call = std::chrono::high_resolution_clock::now(); + EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), + ExpectedPDControlFrom(gains, refs, sensor.joint_state.position, + sensor.joint_state.velocity)); + + EXPECT_EQ( + ctrl.compute_control( + (first_call + GetParam().pd_to_lf_transition_duration + 1us), sensor, + control, false), + ExpectedLFControlFrom(sensor, control, GetParam().robot_has_free_flyer)); + + // This test that time::now() is not used inside the controller an only + // depends on the first_call + EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), + ExpectedPDControlFrom(gains, refs, sensor.joint_state.position, + sensor.joint_state.velocity)); + + // TODO: Gravity compensation ???? } constexpr std::string_view dummy_urdf = @@ -200,7 +239,6 @@ constexpr std::string_view dummy_urdf = " " ""; -using namespace std::literals::chrono_literals; INSTANTIATE_TEST_SUITE_P(DummyUrdf, LinearFeedbackControllerTest, ::testing::ValuesIn(MakeAllControllerParametersFrom( dummy_urdf, From a352cbc5023f64693a3809b1818245a143d28a2b Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 16:25:01 +0100 Subject: [PATCH 35/61] [lfc] Update ComputeControl test with weighted computation --- tests/test_linear_feedback_controller.cpp | 71 ++++++++++++++++------- 1 file changed, 51 insertions(+), 20 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index de2e590b..b9a82f82 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -12,10 +12,6 @@ using tests::utils::ExpectedPDControlFrom; using tests::utils::Gains; using tests::utils::References; -#include "utils/eigen_conversions.hpp" -using linear_feedback_controller_msgs::Eigen::Control; -using linear_feedback_controller_msgs::Eigen::Sensor; - #include "utils/lf_controller.hpp" using tests::utils::ExpectedLFControlFrom; using tests::utils::MakeValidRandomControlFor; @@ -54,6 +50,12 @@ constexpr auto SuccesfullyInitialized(LinearFeedbackController& ctrl) { }; } +constexpr auto AreAlmostEquals(double error_absolute = 1e-6) { + return [=](const auto& lhs, const auto& rhs) { + return ((lhs.array() - rhs.array()).abs() <= error_absolute).all(); + }; +} + TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } @@ -133,18 +135,19 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { } } -#define MakeRefOf(val) std::make_tuple(std::ref((val)), std::string_view{#val}) +#define MakeNamedRefOf(val) \ + std::make_tuple(std::string_view{#val}, std::ref((val))) TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSpecialDouble) { auto ctrl = LinearFeedbackController{}; ASSERT_PRED1(Load(ctrl), GetParam()); auto refs = References::Random(GetParam().d_gains.size()); - for (const auto& [ref, str] : { - MakeRefOf(refs.q(0)), - MakeRefOf(refs.q.tail<1>()[0]), - MakeRefOf(refs.tau(0)), - MakeRefOf(refs.tau.tail<1>()[0]), + for (const auto& [str, ref] : { + MakeNamedRefOf(refs.q(0)), + MakeNamedRefOf(refs.q.tail<1>()[0]), + MakeNamedRefOf(refs.tau(0)), + MakeNamedRefOf(refs.tau.tail<1>()[0]), }) { for (auto tmp_value : { std::numeric_limits::infinity(), @@ -174,6 +177,8 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { // Other verifications based on RMB ? ... } +#define MakeNamedValueOf(val) std::make_tuple(std::string_view{#val}, (val)) + TEST_P(LinearFeedbackControllerTest, ComputeControl) { auto ctrl = LinearFeedbackController{}; const auto refs = References::Random(GetParam().d_gains.size()); @@ -192,23 +197,49 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { const auto control = MakeValidRandomControlFor(*ctrl.get_robot_model()); const auto sensor = MakeValidRandomSensorFor(*ctrl.get_robot_model()); + const auto expected_pd_control = ExpectedPDControlFrom( + gains, refs, sensor.joint_state.position, sensor.joint_state.velocity); + + const auto expected_lf_control = + ExpectedLFControlFrom(sensor, control, GetParam().robot_has_free_flyer); + + constexpr auto ComputePercentOf = [](const auto& min, const auto& val, + const auto& max) { + return (val - min) / (max - min); + }; + + constexpr auto ApplyWeight = [](const auto& weight, const auto& pd, + const auto& lf) { + return (((1.0 - weight) * pd) + (weight * lf)); + }; + + const auto first_call = linear_feedback_controller::TimePoint{ + std::chrono::high_resolution_clock::now()}; + const auto transition = first_call + GetParam().pd_to_lf_transition_duration; + // First call always calls PDController - const auto first_call = std::chrono::high_resolution_clock::now(); EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), - ExpectedPDControlFrom(gains, refs, sensor.joint_state.position, - sensor.joint_state.velocity)); + expected_pd_control); - EXPECT_EQ( - ctrl.compute_control( - (first_call + GetParam().pd_to_lf_transition_duration + 1us), sensor, - control, false), - ExpectedLFControlFrom(sensor, control, GetParam().robot_has_free_flyer)); + EXPECT_EQ(ctrl.compute_control(transition + 1ms, sensor, control, false), + expected_lf_control); + + for (const auto& [str, when] : { + MakeNamedValueOf(first_call + 1ms), + MakeNamedValueOf(first_call + 5ms), + MakeNamedValueOf(transition - 1ms), + }) { + EXPECT_PRED2(AreAlmostEquals(), + ctrl.compute_control(when, sensor, control, false), + ApplyWeight(ComputePercentOf(first_call, when, transition), + expected_pd_control, expected_lf_control)) + << "when = " << std::quoted(str); + } // This test that time::now() is not used inside the controller an only // depends on the first_call EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), - ExpectedPDControlFrom(gains, refs, sensor.joint_state.position, - sensor.joint_state.velocity)); + expected_pd_control); // TODO: Gravity compensation ???? } From 3551eb6d2c8591a7026556d74c91b44c18acb0ad Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 16:39:55 +0100 Subject: [PATCH 36/61] [lfc] Minor update on the ComputeControl test --- tests/test_linear_feedback_controller.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index b9a82f82..969d7bdc 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -51,7 +51,7 @@ constexpr auto SuccesfullyInitialized(LinearFeedbackController& ctrl) { } constexpr auto AreAlmostEquals(double error_absolute = 1e-6) { - return [=](const auto& lhs, const auto& rhs) { + return [=](const auto& lhs, const auto& rhs) -> bool { return ((lhs.array() - rhs.array()).abs() <= error_absolute).all(); }; } @@ -213,20 +213,26 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { return (((1.0 - weight) * pd) + (weight * lf)); }; - const auto first_call = linear_feedback_controller::TimePoint{ - std::chrono::high_resolution_clock::now()}; - const auto transition = first_call + GetParam().pd_to_lf_transition_duration; + const linear_feedback_controller::TimePoint first_call = + std::chrono::high_resolution_clock::now(); + + const linear_feedback_controller::TimePoint transition = + first_call + GetParam().pd_to_lf_transition_duration; // First call always calls PDController EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), expected_pd_control); + // When duration expired, always calls LFController EXPECT_EQ(ctrl.compute_control(transition + 1ms, sensor, control, false), expected_lf_control); + // In between, compute both and apply a weight based on the time elapsed from + // the first call and the expected transition for (const auto& [str, when] : { MakeNamedValueOf(first_call + 1ms), MakeNamedValueOf(first_call + 5ms), + MakeNamedValueOf(transition - 50ms), MakeNamedValueOf(transition - 1ms), }) { EXPECT_PRED2(AreAlmostEquals(), @@ -283,8 +289,6 @@ INSTANTIATE_TEST_SUITE_P(DummyUrdf, LinearFeedbackControllerTest, PrintTo(info.param, &stream, {.as_param_name = true}); return stream.str(); - } - -); + }); } // namespace From 28e57ee665ccf6c585e21c975a44ea1a84339358 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 16:44:44 +0100 Subject: [PATCH 37/61] [lfc] Alias the time_point and use the clock from it --- tests/test_linear_feedback_controller.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 969d7bdc..c574d490 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -213,10 +213,9 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { return (((1.0 - weight) * pd) + (weight * lf)); }; - const linear_feedback_controller::TimePoint first_call = - std::chrono::high_resolution_clock::now(); - - const linear_feedback_controller::TimePoint transition = + using time_point = linear_feedback_controller::TimePoint; + const time_point first_call = time_point::clock::now(); + const time_point transition = first_call + GetParam().pd_to_lf_transition_duration; // First call always calls PDController From 6b49775dca95ca2e6c5d7cf01e79d6a5bbee9b77 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 27 Jan 2025 17:05:14 +0100 Subject: [PATCH 38/61] [lfc] Update LoadSizeMismatch test --- tests/test_linear_feedback_controller.cpp | 47 +++++++++++++++++++++-- 1 file changed, 43 insertions(+), 4 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index c574d490..b9c2401a 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -76,10 +76,49 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto ctrl = LinearFeedbackController{}; - auto param_size_mismatch = GetParam(); - // TODO - EXPECT_PRED1(DoNot(Load(ctrl)), param_size_mismatch); - EXPECT_EQ(ctrl.get_robot_model(), nullptr); + auto good_params = GetParam(); + + { + auto p_gains_too_big = good_params; + p_gains_too_big.p_gains.push_back(3.141592); + + EXPECT_PRED1(DoNot(Load(ctrl)), p_gains_too_big); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); + } + + { + auto p_gains_smaller = good_params; + p_gains_smaller.p_gains.pop_back(); + + EXPECT_PRED1(DoNot(Load(ctrl)), p_gains_smaller); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); + } + + { + auto d_gains_too_big = good_params; + d_gains_too_big.d_gains.push_back(3.141592); + + EXPECT_PRED1(DoNot(Load(ctrl)), d_gains_too_big); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); + } + + { + auto d_gains_smaller = good_params; + d_gains_smaller.d_gains.pop_back(); + + EXPECT_PRED1(DoNot(Load(ctrl)), d_gains_smaller); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); + } + + { + auto moving_joint_cleared = good_params; + moving_joint_cleared.moving_joint_names.clear(); + + EXPECT_PRED1(DoNot(Load(ctrl)), moving_joint_cleared); + EXPECT_EQ(ctrl.get_robot_model(), nullptr); + } + + // Others ?? ... } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { From 7569b837c8f775e62d3376df86de41ba4c67ec85 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 09:49:09 +0100 Subject: [PATCH 39/61] [utils] Replace `#pragma once` with include guards --- tests/utils/core.hpp | 4 +++- tests/utils/linear_feedback_controller.hpp | 5 ++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/tests/utils/core.hpp b/tests/utils/core.hpp index 0db9ec2b..e17efe30 100644 --- a/tests/utils/core.hpp +++ b/tests/utils/core.hpp @@ -1,4 +1,5 @@ -#pragma once +#ifndef LINEAR_FEEDBACK_CONTROLLER_TESTS__CORE_HPP_ +#define LINEAR_FEEDBACK_CONTROLLER_TESTS__CORE_HPP_ #include #include @@ -59,3 +60,4 @@ constexpr auto WithArgs(Pred&& pred) { } } // namespace tests::utils +#endif // LINEAR_FEEDBACK_CONTROLLER_TESTS__CORE_HPP_ diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp index 1532c2ba..6fa78cd1 100644 --- a/tests/utils/linear_feedback_controller.hpp +++ b/tests/utils/linear_feedback_controller.hpp @@ -1,4 +1,5 @@ -#pragma once +#ifndef LINEAR_FEEDBACK_CONTROLLER_TESTS__LINEAR_FEEDBACK_CONTROLLER_HPP_ +#define LINEAR_FEEDBACK_CONTROLLER_TESTS__LINEAR_FEEDBACK_CONTROLLER_HPP_ #include // duration_cast/milliseconds @@ -108,3 +109,5 @@ inline auto PrintTo(const ControllerParameters& params, std::ostream* os, } } // namespace linear_feedback_controller + +#endif // LINEAR_FEEDBACK_CONTROLLER_TESTS__LINEAR_FEEDBACK_CONTROLLER_HPP_ From e4bb0ec3e4a10a62186d358863c8ccdb13b69525 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 10:00:52 +0100 Subject: [PATCH 40/61] [lfc] Rename transition to pd_timeout --- tests/test_linear_feedback_controller.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index b9c2401a..26aea88a 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -221,7 +221,6 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { TEST_P(LinearFeedbackControllerTest, ComputeControl) { auto ctrl = LinearFeedbackController{}; const auto refs = References::Random(GetParam().d_gains.size()); - ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), refs); constexpr auto ToEigen = [](const std::vector& v) { @@ -242,7 +241,7 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { const auto expected_lf_control = ExpectedLFControlFrom(sensor, control, GetParam().robot_has_free_flyer); - constexpr auto ComputePercentOf = [](const auto& min, const auto& val, + constexpr auto ComputePercentOf = [](const auto& val, const auto& min, const auto& max) { return (val - min) / (max - min); }; @@ -254,7 +253,7 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { using time_point = linear_feedback_controller::TimePoint; const time_point first_call = time_point::clock::now(); - const time_point transition = + const time_point pd_timeout = first_call + GetParam().pd_to_lf_transition_duration; // First call always calls PDController @@ -262,7 +261,7 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { expected_pd_control); // When duration expired, always calls LFController - EXPECT_EQ(ctrl.compute_control(transition + 1ms, sensor, control, false), + EXPECT_EQ(ctrl.compute_control(pd_timeout + 1ms, sensor, control, false), expected_lf_control); // In between, compute both and apply a weight based on the time elapsed from @@ -270,12 +269,12 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { for (const auto& [str, when] : { MakeNamedValueOf(first_call + 1ms), MakeNamedValueOf(first_call + 5ms), - MakeNamedValueOf(transition - 50ms), - MakeNamedValueOf(transition - 1ms), + MakeNamedValueOf(pd_timeout - 50ms), + MakeNamedValueOf(pd_timeout - 1ms), }) { EXPECT_PRED2(AreAlmostEquals(), ctrl.compute_control(when, sensor, control, false), - ApplyWeight(ComputePercentOf(first_call, when, transition), + ApplyWeight(ComputePercentOf(when, first_call, pd_timeout), expected_pd_control, expected_lf_control)) << "when = " << std::quoted(str); } From 261d2c23df30c0a2a464e7be355a9cc59bddb2df Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 15:26:37 +0100 Subject: [PATCH 41/61] [lfc] Remove the Weights related lambdas --- tests/test_linear_feedback_controller.cpp | 65 ++++++++++++----------- 1 file changed, 35 insertions(+), 30 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 26aea88a..38dca8a1 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -239,17 +239,7 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { gains, refs, sensor.joint_state.position, sensor.joint_state.velocity); const auto expected_lf_control = - ExpectedLFControlFrom(sensor, control, GetParam().robot_has_free_flyer); - - constexpr auto ComputePercentOf = [](const auto& val, const auto& min, - const auto& max) { - return (val - min) / (max - min); - }; - - constexpr auto ApplyWeight = [](const auto& weight, const auto& pd, - const auto& lf) { - return (((1.0 - weight) * pd) + (weight * lf)); - }; + ExpectedLFControlFrom(sensor, control, *ctrl.get_robot_model()); using time_point = linear_feedback_controller::TimePoint; const time_point first_call = time_point::clock::now(); @@ -272,11 +262,14 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { MakeNamedValueOf(pd_timeout - 50ms), MakeNamedValueOf(pd_timeout - 1ms), }) { - EXPECT_PRED2(AreAlmostEquals(), - ctrl.compute_control(when, sensor, control, false), - ApplyWeight(ComputePercentOf(when, first_call, pd_timeout), - expected_pd_control, expected_lf_control)) - << "when = " << std::quoted(str); + const double ratio = + ((when - first_call) / (GetParam().pd_to_lf_transition_duration)); + + EXPECT_PRED2( + AreAlmostEquals(1e-6), + ctrl.compute_control(when, sensor, control, false), + (((1.0 - ratio) * expected_pd_control) + (ratio * expected_lf_control))) + << "when = " << std::quoted(str) << " | ratio = " << ratio; } // This test that time::now() is not used inside the controller an only @@ -311,21 +304,33 @@ constexpr std::string_view dummy_urdf = " " " " " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " ""; -INSTANTIATE_TEST_SUITE_P(DummyUrdf, LinearFeedbackControllerTest, - ::testing::ValuesIn(MakeAllControllerParametersFrom( - dummy_urdf, - { - {{.name = "l01"}}, - {{.name = "l01"}, {.name = "l12"}}, - }, - {500ms, 1s})), - [](const auto& info) { - std::stringstream stream; - PrintTo(info.param, &stream, - {.as_param_name = true}); - return stream.str(); - }); +INSTANTIATE_TEST_SUITE_P( + DummyUrdf, LinearFeedbackControllerTest, + ::testing::ValuesIn(MakeAllControllerParametersFrom( + dummy_urdf, + { + {{.name = "l01"}}, + {{.name = "l01"}, {.name = "l12"}}, + {{.name = "l01"}, {.name = "l23"}}, + {{.name = "l01"}, {.name = "l12"}, {.name = "l23"}}, + }, + {500ms, 1s})), + [](const auto& info) { + std::stringstream stream; + PrintTo(info.param, &stream, {.as_param_name = true}); + return stream.str(); + }); } // namespace From 45a202414056f17a03f83aa0c614978955cb0af2 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 15:43:48 +0100 Subject: [PATCH 42/61] [lfc] Remove core.hpp --- tests/test_linear_feedback_controller.cpp | 16 ++++-- tests/utils/core.hpp | 63 ----------------------- 2 files changed, 13 insertions(+), 66 deletions(-) delete mode 100644 tests/utils/core.hpp diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 38dca8a1..77434b10 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -1,9 +1,6 @@ #include #include -#include "utils/core.hpp" -using tests::utils::DoNot; - #include "utils/mutation.hpp" using tests::utils::TemporaryMutate; @@ -34,6 +31,19 @@ namespace { struct LinearFeedbackControllerTest : public ::testing::TestWithParam {}; +/** + * @return A predicate functor calling the underlying predicate and + * returning it's negation + * + * @param[in] pred A simple predicate taking any arguments and returning a bool + */ +template +constexpr auto DoNot(Pred&& pred) { + return [&](auto&&... arg) -> bool { + return not pred(std::forward(arg)...); + }; +} + constexpr auto Load(LinearFeedbackController& ctrl) { return [&](const ControllerParameters& params) { return ctrl.load(params); }; } diff --git a/tests/utils/core.hpp b/tests/utils/core.hpp deleted file mode 100644 index e17efe30..00000000 --- a/tests/utils/core.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef LINEAR_FEEDBACK_CONTROLLER_TESTS__CORE_HPP_ -#define LINEAR_FEEDBACK_CONTROLLER_TESTS__CORE_HPP_ - -#include -#include - -namespace tests::utils { - -/** - * @return A predicate functor calling the underlying predicate and - * returning it's negation - * - * @param[in] pred A simple predicate taking any arguments and returning a bool - */ -template -constexpr auto DoNot(Pred&& pred) { - return [&](auto&&... arg) -> bool { - return not pred(std::forward(arg)...); - }; -} - -/** - * @return A predicate functor returning true if ANY one of the underlying - * predicates returns true - * - * @param[in] ...preds All predicates - * - * @warning Predicates will take the exact same arguments - */ -template -constexpr auto AnyOf(Preds&&... preds) { - return [&](const auto&... arg) -> bool { return (... or preds(arg...)); }; -} - -/** - * @return A predicate functor returning true if ALL the underlying predicates - * returns true - * - * @param[in] ...preds All predicates - * - * @warning Predicates will take the exact same arguments - */ -template -constexpr auto AllOf(Preds&&... preds) { - return [&](const auto&... arg) -> bool { return (... and preds(arg...)); }; -} - -/** - * @return A predicate functor returning the result of calling \a pred by - * filtering the input args based on the indices provided - * - * @param[in] ...preds All predicates - */ -template -constexpr auto WithArgs(Pred&& pred) { - return [&](auto&&... arg) { - auto t = std::forward_as_tuple(std::forward(arg)...); - return pred(std::forward(t))>(std::get(t))...); - }; -} - -} // namespace tests::utils -#endif // LINEAR_FEEDBACK_CONTROLLER_TESTS__CORE_HPP_ From 403bf30b211b9714e65c85af3e55a4c500115cf3 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 15:44:09 +0100 Subject: [PATCH 43/61] [utils] Reorder the ExpectedLFController* function args --- tests/test_linear_feedback_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 77434b10..429c6f0f 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -249,7 +249,7 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { gains, refs, sensor.joint_state.position, sensor.joint_state.velocity); const auto expected_lf_control = - ExpectedLFControlFrom(sensor, control, *ctrl.get_robot_model()); + ExpectedLFControlFrom(*ctrl.get_robot_model(), sensor, control); using time_point = linear_feedback_controller::TimePoint; const time_point first_call = time_point::clock::now(); From ef1e61acc86b6424dea1666f7eb7a625e0473dce Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 15:45:46 +0100 Subject: [PATCH 44/61] [lfc] Use 5e-6 as double comparison espilon --- tests/test_linear_feedback_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 429c6f0f..87de3bff 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -60,9 +60,9 @@ constexpr auto SuccesfullyInitialized(LinearFeedbackController& ctrl) { }; } -constexpr auto AreAlmostEquals(double error_absolute = 1e-6) { +constexpr auto AreAlmostEquals(double abs_error) { return [=](const auto& lhs, const auto& rhs) -> bool { - return ((lhs.array() - rhs.array()).abs() <= error_absolute).all(); + return ((lhs.array() - rhs.array()).abs() <= abs_error).all(); }; } @@ -276,7 +276,7 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { ((when - first_call) / (GetParam().pd_to_lf_transition_duration)); EXPECT_PRED2( - AreAlmostEquals(1e-6), + AreAlmostEquals(5e-6), ctrl.compute_control(when, sensor, control, false), (((1.0 - ratio) * expected_pd_control) + (ratio * expected_lf_control))) << "when = " << std::quoted(str) << " | ratio = " << ratio; From 2bde3b280fc47c9b7a885633ac0446466df0778f Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 15:52:19 +0100 Subject: [PATCH 45/61] [lfc] Add doc to the functions used to create the predicates --- tests/test_linear_feedback_controller.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 87de3bff..47d1f705 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -44,22 +44,44 @@ constexpr auto DoNot(Pred&& pred) { }; } +/** + * @return A predicate that returns true when ctrl.load() succeed + * + * @param[in] ctrl A reference to the LFCController to load + */ constexpr auto Load(LinearFeedbackController& ctrl) { return [&](const ControllerParameters& params) { return ctrl.load(params); }; } +/** + * @return A predicate that returns true when ctrl.set_initial_state() succeed + * + * @param[in] ctrl A reference to the LFCController to set the initial state + */ constexpr auto SetInitialState(LinearFeedbackController& ctrl) { return [&](const References& refs) { return ctrl.set_initial_state(refs.tau, refs.q); }; } +/** + * @return A predicate that returns true when both ctrl.load() and + * ctrl.set_initial_state() succeed + * + * @param[in] ctrl A reference to the LFCController to initialized + */ constexpr auto SuccesfullyInitialized(LinearFeedbackController& ctrl) { return [&](const ControllerParameters& params, const References& refs) { return ctrl.load(params) and ctrl.set_initial_state(refs.tau, refs.q); }; } +/** + * @return A predicate used to compare 2 double Eigen::Matrix returning true + * when all elements follows |lhs - rhs| <= eps + * + * @param[in] abs_error The max absolute error allowed when comparing doubles + */ constexpr auto AreAlmostEquals(double abs_error) { return [=](const auto& lhs, const auto& rhs) -> bool { return ((lhs.array() - rhs.array()).abs() <= abs_error).all(); From 5d7048d72f9a0a6e9da950e48fd3ebf757d8c1a1 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 30 Jan 2025 15:57:54 +0100 Subject: [PATCH 46/61] [lfc] Rename ComputeControl to ComputeControlWithoutGravity --- tests/test_linear_feedback_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 47d1f705..7515b546 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -250,7 +250,7 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { #define MakeNamedValueOf(val) std::make_tuple(std::string_view{#val}, (val)) -TEST_P(LinearFeedbackControllerTest, ComputeControl) { +TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { auto ctrl = LinearFeedbackController{}; const auto refs = References::Random(GetParam().d_gains.size()); ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), refs); From eb847a2827334c773a902ae8d531ec353456d9d8 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 31 Jan 2025 09:50:39 +0100 Subject: [PATCH 47/61] [utils] Refactor MakePairOfNamesFrom as factory of JointNamesPair --- tests/utils/linear_feedback_controller.hpp | 10 ++--- tests/utils/robot_model.hpp | 48 +++++++++++----------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp index 6fa78cd1..af9f5b37 100644 --- a/tests/utils/linear_feedback_controller.hpp +++ b/tests/utils/linear_feedback_controller.hpp @@ -27,19 +27,19 @@ inline auto MakeAllControllerParametersFrom( out.reserve(2 * all_joint_lists.size() * durations.size()); for (const auto& joint_list : all_joint_lists) { - const auto joint_names = MakePairOfNamesFrom(joint_list); + const auto [controlled, moving] = JointNamesPair::From(joint_list); - // FIXME : Controlled size ? - const auto gains = Gains::Random(joint_names.controlled.size()); + // FIXME : Controlled or moving size ? + const auto gains = Gains::Random(controlled.size()); for (const auto& duration : durations) { for (auto has_free_flyer : {false, true}) { out.emplace_back(ControllerParameters{ .urdf = std::string{urdf}, - .moving_joint_names = joint_names.moving, + .moving_joint_names = moving, .p_gains = FromEigen(gains.p), .d_gains = FromEigen(gains.d), - .controlled_joint_names = joint_names.controlled, + .controlled_joint_names = controlled, .robot_has_free_flyer = has_free_flyer, .pd_to_lf_transition_duration = duration, }); diff --git a/tests/utils/robot_model.hpp b/tests/utils/robot_model.hpp index f0869173..1da9350b 100644 --- a/tests/utils/robot_model.hpp +++ b/tests/utils/robot_model.hpp @@ -103,34 +103,34 @@ constexpr auto PrintTo(JointDescription joint, std::ostream *os) noexcept struct JointNamesPair { std::vector controlled; std::vector moving; -}; -/** - * @brief Create a pair of names for each JointType - * - * @param[in] joint_desc_list List of JointDescription - * - * @return JointNamesPair Pair of name list - */ -inline auto MakePairOfNamesFrom( - const std::vector &joint_desc_list) -> JointNamesPair { - JointNamesPair out; - - out.controlled.reserve(joint_desc_list.size()); - out.moving.reserve(joint_desc_list.size()); + /** + * @brief Create a pair of names for each JointType + * + * @param[in] joint_desc_list List of JointDescription + * + * @return JointNamesPair Pair of name list + */ + static inline auto From(const std::vector &joint_desc_list) + -> JointNamesPair { + JointNamesPair out; + + out.controlled.reserve(joint_desc_list.size()); + out.moving.reserve(joint_desc_list.size()); + + for (const auto &joint : joint_desc_list) { + if (IsControlled(joint.type)) { + out.controlled.emplace_back(joint.name); + } - for (const auto &joint : joint_desc_list) { - if (IsControlled(joint.type)) { - out.controlled.emplace_back(joint.name); + if (IsMoving(joint.type)) { + out.moving.emplace_back(joint.name); + } } - if (IsMoving(joint.type)) { - out.moving.emplace_back(joint.name); - } + return out; } - - return out; -} +}; /** * @brief Create a list of JointDescription from the given lists @@ -307,7 +307,7 @@ inline auto PrintTo(const ModelDescription &model, std::ostream *os, */ inline auto MakeRobotModelBuilderFrom(const ModelDescription &model) -> std::unique_ptr { - auto [controlled, moving] = MakePairOfNamesFrom(model.joint_list); + auto [controlled, moving] = JointNamesPair::From(model.joint_list); auto rmb = std::make_unique(); if (rmb->build_model(std::string{model.urdf}, std::move(moving), std::move(controlled), model.has_free_flyer)) { From 90cafad06c5e6205a745dd72357dad855a65395e Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 31 Jan 2025 10:32:49 +0100 Subject: [PATCH 48/61] [lfc] Move define on top of the file --- tests/test_linear_feedback_controller.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 7515b546..43a995d2 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -26,6 +26,10 @@ using linear_feedback_controller::LinearFeedbackController; using namespace std::literals::chrono_literals; +#define MakeNamedValueOf(val) std::make_tuple(std::string_view{#val}, (val)) +#define MakeNamedRefOf(val) \ + std::make_tuple(std::string_view{#val}, std::ref((val))) + namespace { struct LinearFeedbackControllerTest @@ -206,9 +210,6 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { } } -#define MakeNamedRefOf(val) \ - std::make_tuple(std::string_view{#val}, std::ref((val))) - TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSpecialDouble) { auto ctrl = LinearFeedbackController{}; ASSERT_PRED1(Load(ctrl), GetParam()); @@ -248,8 +249,6 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { // Other verifications based on RMB ? ... } -#define MakeNamedValueOf(val) std::make_tuple(std::string_view{#val}, (val)) - TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { auto ctrl = LinearFeedbackController{}; const auto refs = References::Random(GetParam().d_gains.size()); From 39981c4950e880a4631ddf1b1245c0e203312a74 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 31 Jan 2025 16:03:18 +0100 Subject: [PATCH 49/61] [lfc] Use TimePoint directly --- tests/test_linear_feedback_controller.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 43a995d2..d7d42954 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -19,8 +19,8 @@ using tests::utils::MakeAllControllerParametersFrom; #include "linear_feedback_controller/linear_feedback_controller.hpp" using linear_feedback_controller::ControllerParameters; -using linear_feedback_controller::Duration; using linear_feedback_controller::LinearFeedbackController; +using linear_feedback_controller::TimePoint; #include "gtest/gtest.h" @@ -272,9 +272,8 @@ TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { const auto expected_lf_control = ExpectedLFControlFrom(*ctrl.get_robot_model(), sensor, control); - using time_point = linear_feedback_controller::TimePoint; - const time_point first_call = time_point::clock::now(); - const time_point pd_timeout = + const TimePoint first_call = TimePoint::clock::now(); + const TimePoint pd_timeout = first_call + GetParam().pd_to_lf_transition_duration; // First call always calls PDController From 3df7a5edb860e120181315373939bd4bcf15035f Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 31 Jan 2025 16:03:31 +0100 Subject: [PATCH 50/61] [lfc] Add templates for the missing tests --- tests/test_linear_feedback_controller.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index d7d42954..5a783850 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -168,6 +168,10 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { EXPECT_EQ(ctrl.get_robot_model(), nullptr); } +TEST_P(LinearFeedbackControllerTest, DISABLED_LoadMismatchSize) { + FAIL() << "Not implemented"; +} + TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; EXPECT_PRED1(Load(ctrl), GetParam()); @@ -249,6 +253,18 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { // Other verifications based on RMB ? ... } +TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongTimestamp) { + FAIL() << "Not implemented"; +} + +TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongSensor) { + FAIL() << "Not implemented"; +} + +TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongControl) { + FAIL() << "Not implemented"; +} + TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { auto ctrl = LinearFeedbackController{}; const auto refs = References::Random(GetParam().d_gains.size()); @@ -310,6 +326,10 @@ TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { // TODO: Gravity compensation ???? } +TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWithGravity) { + FAIL() << "Not implemented"; +} + constexpr std::string_view dummy_urdf = "" "" From 91c23922589b9553f53a91b832d9236681761bd9 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 31 Jan 2025 16:31:53 +0100 Subject: [PATCH 51/61] [lfc] Refactor all test data construction --- tests/test_linear_feedback_controller.cpp | 112 +++++++++++++++------- 1 file changed, 80 insertions(+), 32 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 5a783850..a3247c17 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -9,7 +9,12 @@ using tests::utils::ExpectedPDControlFrom; using tests::utils::Gains; using tests::utils::References; +#include "utils/eigen_conversions.hpp" +using linear_feedback_controller_msgs::Eigen::Control; +using linear_feedback_controller_msgs::Eigen::Sensor; + #include "utils/lf_controller.hpp" +using linear_feedback_controller::RobotModelBuilder; using tests::utils::ExpectedLFControlFrom; using tests::utils::MakeValidRandomControlFor; using tests::utils::MakeValidRandomSensorFor; @@ -32,9 +37,6 @@ using namespace std::literals::chrono_literals; namespace { -struct LinearFeedbackControllerTest - : public ::testing::TestWithParam {}; - /** * @return A predicate functor calling the underlying predicate and * returning it's negation @@ -92,6 +94,70 @@ constexpr auto AreAlmostEquals(double abs_error) { }; } +/// TODO +struct PDParams { + Gains gains; + References refs; + + static auto From(const ControllerParameters& params) -> PDParams { + constexpr auto ToEigen = [](const std::vector& v) { + return Eigen::Map(v.data(), v.size()); + }; + + PDParams out; + out.gains.p = ToEigen(params.p_gains); + out.gains.d = ToEigen(params.d_gains); + out.refs = References::Random(params.d_gains.size()); + return out; + } +}; + +/// TODO +struct Timestamps { + TimePoint first_call; + TimePoint pd_timeout; + + static auto From(const ControllerParameters& params) -> Timestamps { + Timestamps out; + out.first_call = TimePoint::clock::now(); + out.first_call = TimePoint::clock::now(); + out.pd_timeout = out.first_call + params.pd_to_lf_transition_duration; + return out; + } +}; + +/// TODO +struct ControllerInputs { + Sensor sensor; + Control control; + + static auto From(const RobotModelBuilder& model) -> ControllerInputs { + ControllerInputs out; + out.sensor = MakeValidRandomSensorFor(model); + out.control = MakeValidRandomControlFor(model); + return out; + } +}; + +/// TODO +struct Expectations { + Eigen::VectorXd pd; + Eigen::VectorXd lf; + + static auto From(const RobotModelBuilder& model, const Gains& gains, + const References& refs, const Sensor& sensor, + const Control& control) -> Expectations { + Expectations out; + out.pd = ExpectedPDControlFrom(gains, refs, sensor.joint_state.position, + sensor.joint_state.velocity); + out.lf = ExpectedLFControlFrom(model, sensor, control); + return out; + } +}; + +struct LinearFeedbackControllerTest + : public ::testing::TestWithParam {}; + TEST(LinearFeedbackControllerTest, Ctor) { EXPECT_NO_THROW({ auto ctrl = LinearFeedbackController{}; }); } @@ -267,38 +333,21 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongControl) { TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { auto ctrl = LinearFeedbackController{}; - const auto refs = References::Random(GetParam().d_gains.size()); + const auto [gains, refs] = PDParams::From(GetParam()); + const auto [first_call, pd_timeout] = Timestamps::From(GetParam()); ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), refs); - constexpr auto ToEigen = [](const std::vector& v) { - return Eigen::Map(v.data(), v.size()); - }; - - const auto gains = Gains{ - .p = ToEigen(GetParam().p_gains), - .d = ToEigen(GetParam().d_gains), - }; - - const auto control = MakeValidRandomControlFor(*ctrl.get_robot_model()); - const auto sensor = MakeValidRandomSensorFor(*ctrl.get_robot_model()); - - const auto expected_pd_control = ExpectedPDControlFrom( - gains, refs, sensor.joint_state.position, sensor.joint_state.velocity); - - const auto expected_lf_control = - ExpectedLFControlFrom(*ctrl.get_robot_model(), sensor, control); - - const TimePoint first_call = TimePoint::clock::now(); - const TimePoint pd_timeout = - first_call + GetParam().pd_to_lf_transition_duration; + const auto& model = *ctrl.get_robot_model(); + const auto [sensor, control] = ControllerInputs::From(model); + const auto expected = Expectations::From(model, gains, refs, sensor, control); // First call always calls PDController EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), - expected_pd_control); + expected.pd); // When duration expired, always calls LFController EXPECT_EQ(ctrl.compute_control(pd_timeout + 1ms, sensor, control, false), - expected_lf_control); + expected.lf); // In between, compute both and apply a weight based on the time elapsed from // the first call and the expected transition @@ -311,17 +360,16 @@ TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { const double ratio = ((when - first_call) / (GetParam().pd_to_lf_transition_duration)); - EXPECT_PRED2( - AreAlmostEquals(5e-6), - ctrl.compute_control(when, sensor, control, false), - (((1.0 - ratio) * expected_pd_control) + (ratio * expected_lf_control))) + EXPECT_PRED2(AreAlmostEquals(5e-6), + ctrl.compute_control(when, sensor, control, false), + (((1.0 - ratio) * expected.pd) + (ratio * expected.lf))) << "when = " << std::quoted(str) << " | ratio = " << ratio; } // This test that time::now() is not used inside the controller an only // depends on the first_call EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), - expected_pd_control); + expected.pd); // TODO: Gravity compensation ???? } From 8febf09bcb291def61b4b25900e404839b2b4732 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Fri, 31 Jan 2025 16:39:42 +0100 Subject: [PATCH 52/61] [lfc] Remove wrong input ComputeControl tests --- tests/test_linear_feedback_controller.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index a3247c17..fb0b1496 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -319,18 +319,6 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { // Other verifications based on RMB ? ... } -TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongTimestamp) { - FAIL() << "Not implemented"; -} - -TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongSensor) { - FAIL() << "Not implemented"; -} - -TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWrongControl) { - FAIL() << "Not implemented"; -} - TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { auto ctrl = LinearFeedbackController{}; const auto [gains, refs] = PDParams::From(GetParam()); From 6b7ea9f3fe991e7135cab89e9039519ae54ba1ee Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 3 Feb 2025 09:31:03 +0100 Subject: [PATCH 53/61] [lfc] Merge With/Without GRavity stuff into ComputeControl --- tests/test_linear_feedback_controller.cpp | 71 ++++++++++++----------- 1 file changed, 37 insertions(+), 34 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index fb0b1496..9d9ce9d0 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -319,7 +319,7 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { // Other verifications based on RMB ? ... } -TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { +TEST_P(LinearFeedbackControllerTest, ComputeControl) { auto ctrl = LinearFeedbackController{}; const auto [gains, refs] = PDParams::From(GetParam()); const auto [first_call, pd_timeout] = Timestamps::From(GetParam()); @@ -329,41 +329,44 @@ TEST_P(LinearFeedbackControllerTest, ComputeControlWithoutGravity) { const auto [sensor, control] = ControllerInputs::From(model); const auto expected = Expectations::From(model, gains, refs, sensor, control); - // First call always calls PDController - EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), - expected.pd); - - // When duration expired, always calls LFController - EXPECT_EQ(ctrl.compute_control(pd_timeout + 1ms, sensor, control, false), - expected.lf); - - // In between, compute both and apply a weight based on the time elapsed from - // the first call and the expected transition - for (const auto& [str, when] : { - MakeNamedValueOf(first_call + 1ms), - MakeNamedValueOf(first_call + 5ms), - MakeNamedValueOf(pd_timeout - 50ms), - MakeNamedValueOf(pd_timeout - 1ms), - }) { - const double ratio = - ((when - first_call) / (GetParam().pd_to_lf_transition_duration)); + for (auto gravity_compensation : {false, true}) { + SCOPED_TRACE(::testing::Message() + << "\n gravity_compensation = " << gravity_compensation); + + // First call always calls PDController + EXPECT_EQ( + ctrl.compute_control(first_call, sensor, control, gravity_compensation), + expected.pd); + + // When duration expired, always calls LFController + EXPECT_EQ(ctrl.compute_control(pd_timeout + 1ms, sensor, control, + gravity_compensation), + expected.lf); + + // In between, compute both and apply a weight based on the time elapsed + // from the first call and the expected transition + for (const auto& [str, when] : { + MakeNamedValueOf(first_call + 1ms), + MakeNamedValueOf(first_call + 5ms), + MakeNamedValueOf(pd_timeout - 50ms), + MakeNamedValueOf(pd_timeout - 1ms), + }) { + const double ratio = + ((when - first_call) / (GetParam().pd_to_lf_transition_duration)); + + EXPECT_PRED2( + AreAlmostEquals(5e-6), + ctrl.compute_control(when, sensor, control, gravity_compensation), + (((1.0 - ratio) * expected.pd) + (ratio * expected.lf))) + << "when = " << std::quoted(str) << " | ratio = " << ratio; + } - EXPECT_PRED2(AreAlmostEquals(5e-6), - ctrl.compute_control(when, sensor, control, false), - (((1.0 - ratio) * expected.pd) + (ratio * expected.lf))) - << "when = " << std::quoted(str) << " | ratio = " << ratio; + // This test that time::now() is not used inside the controller an only + // depends on the first_call + EXPECT_EQ( + ctrl.compute_control(first_call, sensor, control, gravity_compensation), + expected.pd); } - - // This test that time::now() is not used inside the controller an only - // depends on the first_call - EXPECT_EQ(ctrl.compute_control(first_call, sensor, control, false), - expected.pd); - - // TODO: Gravity compensation ???? -} - -TEST_P(LinearFeedbackControllerTest, DISABLED_ComputeControlWithGravity) { - FAIL() << "Not implemented"; } constexpr std::string_view dummy_urdf = From 79233e7dd3d24fec701fcd4b5b0a21db559bce63 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Mon, 3 Feb 2025 14:01:13 +0100 Subject: [PATCH 54/61] [lfc] Update gravity_compensation SCOPE_TRACE format --- tests/test_linear_feedback_controller.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 9d9ce9d0..40cc71ed 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -30,6 +30,7 @@ using linear_feedback_controller::TimePoint; #include "gtest/gtest.h" using namespace std::literals::chrono_literals; +using namespace std::literals::string_view_literals; #define MakeNamedValueOf(val) std::make_tuple(std::string_view{#val}, (val)) #define MakeNamedRefOf(val) \ @@ -329,9 +330,13 @@ TEST_P(LinearFeedbackControllerTest, ComputeControl) { const auto [sensor, control] = ControllerInputs::From(model); const auto expected = Expectations::From(model, gains, refs, sensor, control); - for (auto gravity_compensation : {false, true}) { + for (auto gravity_compensation : { + false, + true, + }) { SCOPED_TRACE(::testing::Message() - << "\n gravity_compensation = " << gravity_compensation); + << "\n gravity_compensation = " + << (gravity_compensation ? "TRUE"sv : "FALSE"sv)); // First call always calls PDController EXPECT_EQ( From fceb0e080b4032f4b5afa34f2f8e67a2c84d29b5 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Tue, 4 Feb 2025 13:33:21 +0100 Subject: [PATCH 55/61] [lfc] Remove PRED stuff (easier to understand for reviewers) --- tests/test_linear_feedback_controller.cpp | 108 ++++++---------------- 1 file changed, 28 insertions(+), 80 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 40cc71ed..b32ebc1d 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -38,51 +38,6 @@ using namespace std::literals::string_view_literals; namespace { -/** - * @return A predicate functor calling the underlying predicate and - * returning it's negation - * - * @param[in] pred A simple predicate taking any arguments and returning a bool - */ -template -constexpr auto DoNot(Pred&& pred) { - return [&](auto&&... arg) -> bool { - return not pred(std::forward(arg)...); - }; -} - -/** - * @return A predicate that returns true when ctrl.load() succeed - * - * @param[in] ctrl A reference to the LFCController to load - */ -constexpr auto Load(LinearFeedbackController& ctrl) { - return [&](const ControllerParameters& params) { return ctrl.load(params); }; -} - -/** - * @return A predicate that returns true when ctrl.set_initial_state() succeed - * - * @param[in] ctrl A reference to the LFCController to set the initial state - */ -constexpr auto SetInitialState(LinearFeedbackController& ctrl) { - return [&](const References& refs) { - return ctrl.set_initial_state(refs.tau, refs.q); - }; -} - -/** - * @return A predicate that returns true when both ctrl.load() and - * ctrl.set_initial_state() succeed - * - * @param[in] ctrl A reference to the LFCController to initialized - */ -constexpr auto SuccesfullyInitialized(LinearFeedbackController& ctrl) { - return [&](const ControllerParameters& params, const References& refs) { - return ctrl.load(params) and ctrl.set_initial_state(refs.tau, refs.q); - }; -} - /** * @return A predicate used to compare 2 double Eigen::Matrix returning true * when all elements follows |lhs - rhs| <= eps @@ -95,7 +50,6 @@ constexpr auto AreAlmostEquals(double abs_error) { }; } -/// TODO struct PDParams { Gains gains; References refs; @@ -113,7 +67,6 @@ struct PDParams { } }; -/// TODO struct Timestamps { TimePoint first_call; TimePoint pd_timeout; @@ -121,13 +74,11 @@ struct Timestamps { static auto From(const ControllerParameters& params) -> Timestamps { Timestamps out; out.first_call = TimePoint::clock::now(); - out.first_call = TimePoint::clock::now(); out.pd_timeout = out.first_call + params.pd_to_lf_transition_duration; return out; } }; -/// TODO struct ControllerInputs { Sensor sensor; Control control; @@ -140,7 +91,6 @@ struct ControllerInputs { } }; -/// TODO struct Expectations { Eigen::VectorXd pd; Eigen::VectorXd lf; @@ -165,7 +115,7 @@ TEST(LinearFeedbackControllerTest, Ctor) { TEST(LinearFeedbackControllerTest, DISABLED_LoadEmptyParams) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(DoNot(Load(ctrl)), ControllerParameters{}); + EXPECT_FALSE(ctrl.load(ControllerParameters{})); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } @@ -173,7 +123,8 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNoURDF) { auto ctrl = LinearFeedbackController{}; auto no_urdf_param = GetParam(); no_urdf_param.urdf.clear(); - EXPECT_PRED1(DoNot(Load(ctrl)), no_urdf_param); + + EXPECT_FALSE(ctrl.load(no_urdf_param)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } @@ -185,7 +136,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto p_gains_too_big = good_params; p_gains_too_big.p_gains.push_back(3.141592); - EXPECT_PRED1(DoNot(Load(ctrl)), p_gains_too_big); + EXPECT_FALSE(ctrl.load(p_gains_too_big)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } @@ -193,7 +144,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto p_gains_smaller = good_params; p_gains_smaller.p_gains.pop_back(); - EXPECT_PRED1(DoNot(Load(ctrl)), p_gains_smaller); + EXPECT_FALSE(ctrl.load(p_gains_smaller)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } @@ -201,7 +152,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto d_gains_too_big = good_params; d_gains_too_big.d_gains.push_back(3.141592); - EXPECT_PRED1(DoNot(Load(ctrl)), d_gains_too_big); + EXPECT_FALSE(ctrl.load(d_gains_too_big)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } @@ -209,7 +160,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto d_gains_smaller = good_params; d_gains_smaller.d_gains.pop_back(); - EXPECT_PRED1(DoNot(Load(ctrl)), d_gains_smaller); + EXPECT_FALSE(ctrl.load(d_gains_smaller)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } @@ -217,11 +168,9 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto moving_joint_cleared = good_params; moving_joint_cleared.moving_joint_names.clear(); - EXPECT_PRED1(DoNot(Load(ctrl)), moving_joint_cleared); + EXPECT_FALSE(ctrl.load(moving_joint_cleared)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } - - // Others ?? ... } TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { @@ -231,59 +180,57 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadNegativeDuration) { negative_duration_params.pd_to_lf_transition_duration = -negative_duration_params.pd_to_lf_transition_duration; - EXPECT_PRED1(DoNot(Load(ctrl)), negative_duration_params); + EXPECT_FALSE(ctrl.load(negative_duration_params)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } -TEST_P(LinearFeedbackControllerTest, DISABLED_LoadMismatchSize) { - FAIL() << "Not implemented"; -} - TEST_P(LinearFeedbackControllerTest, Load) { auto ctrl = LinearFeedbackController{}; - EXPECT_PRED1(Load(ctrl), GetParam()); + EXPECT_TRUE(ctrl.load(GetParam())); EXPECT_NE(ctrl.get_robot_model(), nullptr); } TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateEmpty) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Load(ctrl), GetParam()); - EXPECT_PRED1(DoNot(SetInitialState(ctrl)), References{}); + ASSERT_TRUE(ctrl.load(GetParam())); + EXPECT_FALSE(ctrl.set_initial_state({}, {})); } TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSizeMismatch) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Load(ctrl), GetParam()); + ASSERT_TRUE(ctrl.load(GetParam())); + const auto good_refs = References::Random(GetParam().d_gains.size()); { auto tau_bigger = good_refs; tau_bigger.tau << tau_bigger.tau, 1.0; - EXPECT_PRED1(DoNot(SetInitialState(ctrl)), tau_bigger); + EXPECT_FALSE(ctrl.set_initial_state(tau_bigger.tau, tau_bigger.q)); } { auto tau_smaller = good_refs; tau_smaller.tau.conservativeResize(tau_smaller.tau.size() - 1); - EXPECT_PRED1(DoNot(SetInitialState(ctrl)), tau_smaller); + EXPECT_FALSE(ctrl.set_initial_state(tau_smaller.tau, tau_smaller.q)); } { auto q_bigger = good_refs; q_bigger.q << q_bigger.q, 1.0; - EXPECT_PRED1(DoNot(SetInitialState(ctrl)), q_bigger); + EXPECT_FALSE(ctrl.set_initial_state(q_bigger.tau, q_bigger.q)); } { auto q_smaller = good_refs; q_smaller.q.conservativeResize(q_smaller.q.size() - 1); - EXPECT_PRED1(DoNot(SetInitialState(ctrl)), q_smaller); + EXPECT_FALSE(ctrl.set_initial_state(q_smaller.tau, q_smaller.q)); } } TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSpecialDouble) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Load(ctrl), GetParam()); + ASSERT_TRUE(ctrl.load(GetParam())); + auto refs = References::Random(GetParam().d_gains.size()); for (const auto& [str, ref] : { @@ -298,7 +245,7 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSpecialDouble) { std::numeric_limits::signaling_NaN(), }) { const auto mutation = TemporaryMutate(ref, tmp_value); - EXPECT_PRED1(DoNot(SetInitialState(ctrl)), refs) + EXPECT_FALSE(ctrl.set_initial_state(refs.tau, refs.q)) << str << " = " << ref << " (was " << mutation.OldValue() << ")"; } } @@ -306,9 +253,10 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_SetInitialStateSpecialDouble) { TEST_P(LinearFeedbackControllerTest, SetInitialState) { auto ctrl = LinearFeedbackController{}; - ASSERT_PRED1(Load(ctrl), GetParam()); - EXPECT_PRED1(SetInitialState(ctrl), - References::Random(GetParam().d_gains.size())); + ASSERT_TRUE(ctrl.load(GetParam())); + + const auto refs = References::Random(GetParam().d_gains.size()); + EXPECT_TRUE(ctrl.set_initial_state(refs.tau, refs.q)); ASSERT_NE(ctrl.get_robot_model(), nullptr); EXPECT_EQ(ctrl.get_robot_model()->get_moving_joint_names(), @@ -316,15 +264,15 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { EXPECT_EQ(ctrl.get_robot_model()->get_robot_has_free_flyer(), GetParam().robot_has_free_flyer); - - // Other verifications based on RMB ? ... } TEST_P(LinearFeedbackControllerTest, ComputeControl) { auto ctrl = LinearFeedbackController{}; const auto [gains, refs] = PDParams::From(GetParam()); + ASSERT_TRUE(ctrl.load(GetParam()) and + ctrl.set_initial_state(refs.tau, refs.q)); + const auto [first_call, pd_timeout] = Timestamps::From(GetParam()); - ASSERT_PRED2(SuccesfullyInitialized(ctrl), GetParam(), refs); const auto& model = *ctrl.get_robot_model(); const auto [sensor, control] = ControllerInputs::From(model); From 2128ac34bfd3fc1382859566277ecf15d4f6e70d Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 6 Feb 2025 08:57:42 +0100 Subject: [PATCH 56/61] [utils] Use vector ctor directly instead of .resize() call --- tests/utils/linear_feedback_controller.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp index af9f5b37..48614bc0 100644 --- a/tests/utils/linear_feedback_controller.hpp +++ b/tests/utils/linear_feedback_controller.hpp @@ -17,8 +17,7 @@ inline auto MakeAllControllerParametersFrom( using linear_feedback_controller::ControllerParameters; const auto FromEigen = [](const Eigen::VectorXd& val) -> std::vector { - std::vector out; - out.resize(val.size()); + std::vector out(val.size()); Eigen::Map(out.data(), out.size()) = val; return out; }; From df176054fa5e7e5fbbdcf62753234ff7bf233538 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 6 Feb 2025 08:58:29 +0100 Subject: [PATCH 57/61] [utils] Remove FIXME - Confirmed used of controlled joints for gains --- tests/utils/linear_feedback_controller.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp index 48614bc0..8d7dd777 100644 --- a/tests/utils/linear_feedback_controller.hpp +++ b/tests/utils/linear_feedback_controller.hpp @@ -28,9 +28,7 @@ inline auto MakeAllControllerParametersFrom( for (const auto& joint_list : all_joint_lists) { const auto [controlled, moving] = JointNamesPair::From(joint_list); - // FIXME : Controlled or moving size ? const auto gains = Gains::Random(controlled.size()); - for (const auto& duration : durations) { for (auto has_free_flyer : {false, true}) { out.emplace_back(ControllerParameters{ From e909f4dd8c665f7fce665ac1fdf895bd3584aeeb Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 6 Feb 2025 08:59:46 +0100 Subject: [PATCH 58/61] [utils] Use const within range-for loop --- tests/utils/linear_feedback_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/utils/linear_feedback_controller.hpp b/tests/utils/linear_feedback_controller.hpp index 8d7dd777..a9c41a19 100644 --- a/tests/utils/linear_feedback_controller.hpp +++ b/tests/utils/linear_feedback_controller.hpp @@ -30,7 +30,7 @@ inline auto MakeAllControllerParametersFrom( const auto gains = Gains::Random(controlled.size()); for (const auto& duration : durations) { - for (auto has_free_flyer : {false, true}) { + for (const auto has_free_flyer : {false, true}) { out.emplace_back(ControllerParameters{ .urdf = std::string{urdf}, .moving_joint_names = moving, From 68bbca2b00a78230bc2c8a8483ab22d7c6fed66c Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 6 Feb 2025 09:01:56 +0100 Subject: [PATCH 59/61] [lfc] Rename wrong variable name to be more explicits --- tests/test_linear_feedback_controller.cpp | 24 +++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index b32ebc1d..57d77c33 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -133,34 +133,34 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { auto good_params = GetParam(); { - auto p_gains_too_big = good_params; - p_gains_too_big.p_gains.push_back(3.141592); + auto p_gains_vector_too_big = good_params; + p_gains_vector_too_big.p_gains.push_back(3.141592); - EXPECT_FALSE(ctrl.load(p_gains_too_big)); + EXPECT_FALSE(ctrl.load(p_gains_vector_too_big)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } { - auto p_gains_smaller = good_params; - p_gains_smaller.p_gains.pop_back(); + auto p_gains_vector_smaller = good_params; + p_gains_vector_smaller.p_gains.pop_back(); - EXPECT_FALSE(ctrl.load(p_gains_smaller)); + EXPECT_FALSE(ctrl.load(p_gains_vector_smaller)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } { - auto d_gains_too_big = good_params; - d_gains_too_big.d_gains.push_back(3.141592); + auto d_gains_vector_too_big = good_params; + d_gains_vector_too_big.d_gains.push_back(3.141592); - EXPECT_FALSE(ctrl.load(d_gains_too_big)); + EXPECT_FALSE(ctrl.load(d_gains_vector_too_big)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } { - auto d_gains_smaller = good_params; - d_gains_smaller.d_gains.pop_back(); + auto d_gains_vector_smaller = good_params; + d_gains_vector_smaller.d_gains.pop_back(); - EXPECT_FALSE(ctrl.load(d_gains_smaller)); + EXPECT_FALSE(ctrl.load(d_gains_vector_smaller)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); } From 46ed05a970c774295ddf6d1f446f816f85e3baed Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 6 Feb 2025 09:41:35 +0100 Subject: [PATCH 60/61] [lfc] Remove side effect checks for SetInitialState --- tests/test_linear_feedback_controller.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index 57d77c33..bdeccbf3 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -257,13 +257,6 @@ TEST_P(LinearFeedbackControllerTest, SetInitialState) { const auto refs = References::Random(GetParam().d_gains.size()); EXPECT_TRUE(ctrl.set_initial_state(refs.tau, refs.q)); - - ASSERT_NE(ctrl.get_robot_model(), nullptr); - EXPECT_EQ(ctrl.get_robot_model()->get_moving_joint_names(), - GetParam().moving_joint_names); - - EXPECT_EQ(ctrl.get_robot_model()->get_robot_has_free_flyer(), - GetParam().robot_has_free_flyer); } TEST_P(LinearFeedbackControllerTest, ComputeControl) { From 82873e99b47e821ed6c4d61dab7ac0aa416b6a71 Mon Sep 17 00:00:00 2001 From: Arthur Valiente Date: Thu, 6 Feb 2025 12:35:13 +0100 Subject: [PATCH 61/61] [lfc] Use random number when appending to vectors --- tests/test_linear_feedback_controller.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_linear_feedback_controller.cpp b/tests/test_linear_feedback_controller.cpp index bdeccbf3..ec599f5a 100644 --- a/tests/test_linear_feedback_controller.cpp +++ b/tests/test_linear_feedback_controller.cpp @@ -134,7 +134,8 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { { auto p_gains_vector_too_big = good_params; - p_gains_vector_too_big.p_gains.push_back(3.141592); + p_gains_vector_too_big.p_gains.push_back( + Eigen::Vector::Random()[0]); EXPECT_FALSE(ctrl.load(p_gains_vector_too_big)); EXPECT_EQ(ctrl.get_robot_model(), nullptr); @@ -150,7 +151,8 @@ TEST_P(LinearFeedbackControllerTest, DISABLED_LoadSizeMismatch) { { auto d_gains_vector_too_big = good_params; - d_gains_vector_too_big.d_gains.push_back(3.141592); + d_gains_vector_too_big.d_gains.push_back( + Eigen::Vector::Random()[0]); EXPECT_FALSE(ctrl.load(d_gains_vector_too_big)); EXPECT_EQ(ctrl.get_robot_model(), nullptr);