From fd416e0b57abd91e8b8708ad5bdbdc8fee90c713 Mon Sep 17 00:00:00 2001 From: mebbaid Date: Sat, 2 Mar 2024 21:43:52 +0100 Subject: [PATCH 1/5] add UnicyclePlanner unit test --- src/Planners/tests/CMakeLists.txt | 10 ++ src/Planners/tests/UnicyclePlannerTest.cpp | 115 +++++++++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 src/Planners/tests/UnicyclePlannerTest.cpp diff --git a/src/Planners/tests/CMakeLists.txt b/src/Planners/tests/CMakeLists.txt index 3aafc5feb2..a22d7ebe28 100644 --- a/src/Planners/tests/CMakeLists.txt +++ b/src/Planners/tests/CMakeLists.txt @@ -21,3 +21,13 @@ add_bipedal_test( NAME SwingFootPlanner SOURCES SwingFootPlannerTest.cpp LINKS BipedalLocomotion::Planners) + +if (FRAMEWORK_COMPILE_Unicycle) + + add_bipedal_test( + NAME UnicyclePlanner + SOURCES UnicyclePlannerTest.cpp + LINKS BipedalLocomotion::Planners BipedalLocomotion::Unicycle + ) + +endif() diff --git a/src/Planners/tests/UnicyclePlannerTest.cpp b/src/Planners/tests/UnicyclePlannerTest.cpp new file mode 100644 index 0000000000..346b47a69b --- /dev/null +++ b/src/Planners/tests/UnicyclePlannerTest.cpp @@ -0,0 +1,115 @@ +/** + * @file UnicyclePlannerTest.cpp + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ +#include + +#include + +// Catch2 +#include + +#include +#include + +using namespace BipedalLocomotion::Planners; +using namespace BipedalLocomotion::ParametersHandler; + + +#include + + +bool approxEqual(const manif::SE3d& lhs, const manif::SE3d& rhs, const double& translationTol = 1e-1, const double& rotationTol = 1e2) +{ + Eigen::Matrix3d rotationDiffMat = (lhs.rotation().matrix() * rhs.rotation().inverse().matrix()).eval(); + Eigen::AngleAxisd rotationDiffAA(rotationDiffMat); + return (lhs.translation() - rhs.translation()).norm() < translationTol && rotationDiffAA.angle() < rotationTol; +} + +void saveData(const UnicyclePlannerOutput& output, const std::string& filename) +{ + std::ofstream file(filename); + if (!file.is_open()) { + std::cerr << "Error: Unable to open file " << filename << " for writing." << std::endl; + return; + } + + file << "Left_foot_position_x Left_foot_position_y Left_foot_position_z Left_foot_quat_x Left_foot_quat_y Left_foot_quat_z Left_foot_quat_w" + "Right_foot_position_x Right_foot_position_y Right_foot_position_z Right_foot_quat_x Right_foot_quat_y Right_foot_quat_z Right_foot_quat_w \n"; + + for (size_t i = 0; i < output.left.size(); ++i) + { + const auto& left_pose = output.left[i].pose; + const auto& right_pose = output.right[i].pose; + + file << left_pose.coeffs().transpose() << " " + << right_pose.coeffs().transpose() << "\n"; + } + + file.close(); +} + + + + +std::shared_ptr params(const double& dT) +{ + // Set the non-default parameters of the planner + std::shared_ptr handler = std::make_shared(); + + handler->setParameter("sampling_time", dT); + handler->setParameter("minStepDuration", 0.7); + handler->setParameter("maxStepDuration", 1.31); + handler->setParameter("nominalDuration", 1.0); + handler->setParameter("maxStepLength", 0.5); + handler->setParameter("minStepLength", 0.01); + handler->setParameter("minWidth", 0.12); + handler->setParameter("nominalWidth", 0.17); + handler->setParameter("minAngleVariation", 5.0); + handler->setParameter("maxAngleVariation", 18.0); + handler->setParameter("switchOverSwingRatio", 0.3); + handler->setParameter("positionWeight", 100.0); + + return handler; +} + +TEST_CASE("UnicyclePlannerTest") +{ + const double dT = 0.01; + const auto handler = params(dT); + + bool saveDataTofile = true; + + UnicyclePlanner planner; + + REQUIRE(planner.initialize(handler)); + + UnicyclePlannerInput input( + {UnicycleKnot({0.0, 0.0}, {0.0, 0.0}, 0.0), + UnicycleKnot({0.5, 0.0}, {0.0, 0.0}, 1.0), + UnicycleKnot({1.0, 0.0}, {0.0, 0.0}, 1.5)}, + 20.0, + std::nullopt, + std::nullopt, + 0.0); + UnicyclePlannerOutput output; + + REQUIRE(planner.setInput(input)); + REQUIRE(planner.advance()); + output = planner.getOutput(); + + REQUIRE(output.left.size() > 2); + REQUIRE(output.left.size() == output.right.size() ); + + // require last contact of output.left to have x pos = last UnicycleKnot - referencePosition + + const auto& lastLeftContact = output.left.rbegin()->pose; + REQUIRE(approxEqual(lastLeftContact, manif::SE3d({input.knots.back().x - 0.1, input.knots.back().y, 0.0}, Eigen::Quaterniond::Identity()))); + + if (saveDataTofile) + { + saveData(output, "UnicyclePlannerTestOutput.txt"); + } + +} \ No newline at end of file From 438d9536623a28f41e256243d10ea47da67af605 Mon Sep 17 00:00:00 2001 From: mebbaid Date: Sat, 2 Mar 2024 21:52:11 +0100 Subject: [PATCH 2/5] updated changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7c294d31b2..19c4bfda26 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ All notable changes to this project are documented in this file. - Set submodel states from IMUs in RDE and add friction torques as measurement (https://github.com/ami-iit/bipedal-locomotion-framework/pull/793) - Add streaming of arm fts in YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/803) - 🤖 [ `ergoCubGazeboV1_1`] Add configuration files to log data with the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/806, https://github.com/ami-iit/bipedal-locomotion-framework/pull/808) +- Added a unit test code for the `UnicyclePlanner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/815) ### Changed - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810) From 87c54e16d22c7e8477431a3edc4252bc647f5976 Mon Sep 17 00:00:00 2001 From: Mohamed Elobaid <36491318+mebbaid@users.noreply.github.com> Date: Mon, 4 Mar 2024 09:51:52 +0100 Subject: [PATCH 3/5] Update src/Planners/tests/UnicyclePlannerTest.cpp Disable the saving of the data by default Co-authored-by: Stefano Dafarra --- src/Planners/tests/UnicyclePlannerTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Planners/tests/UnicyclePlannerTest.cpp b/src/Planners/tests/UnicyclePlannerTest.cpp index 346b47a69b..46676a30eb 100644 --- a/src/Planners/tests/UnicyclePlannerTest.cpp +++ b/src/Planners/tests/UnicyclePlannerTest.cpp @@ -79,7 +79,7 @@ TEST_CASE("UnicyclePlannerTest") const double dT = 0.01; const auto handler = params(dT); - bool saveDataTofile = true; + bool saveDataTofile = false; UnicyclePlanner planner; From a1ec8847bd65325896859a633155d9dfe4a74cc7 Mon Sep 17 00:00:00 2001 From: Mohamed Elobaid <36491318+mebbaid@users.noreply.github.com> Date: Mon, 4 Mar 2024 09:52:15 +0100 Subject: [PATCH 4/5] Update src/Planners/tests/UnicyclePlannerTest.cpp Formatting update Co-authored-by: Stefano Dafarra --- src/Planners/tests/UnicyclePlannerTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Planners/tests/UnicyclePlannerTest.cpp b/src/Planners/tests/UnicyclePlannerTest.cpp index 46676a30eb..6f6b36c0a6 100644 --- a/src/Planners/tests/UnicyclePlannerTest.cpp +++ b/src/Planners/tests/UnicyclePlannerTest.cpp @@ -112,4 +112,4 @@ TEST_CASE("UnicyclePlannerTest") saveData(output, "UnicyclePlannerTestOutput.txt"); } -} \ No newline at end of file +} From dca0fdfbe1cda299bbc47df795d7d0d654852206 Mon Sep 17 00:00:00 2001 From: Mohamed Elobaid <36491318+mebbaid@users.noreply.github.com> Date: Mon, 4 Mar 2024 09:58:09 +0100 Subject: [PATCH 5/5] Update UnicyclePlannerTest.cpp Correct tolerance for rotation difference --- src/Planners/tests/UnicyclePlannerTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Planners/tests/UnicyclePlannerTest.cpp b/src/Planners/tests/UnicyclePlannerTest.cpp index 6f6b36c0a6..cb12997ae3 100644 --- a/src/Planners/tests/UnicyclePlannerTest.cpp +++ b/src/Planners/tests/UnicyclePlannerTest.cpp @@ -20,7 +20,7 @@ using namespace BipedalLocomotion::ParametersHandler; #include -bool approxEqual(const manif::SE3d& lhs, const manif::SE3d& rhs, const double& translationTol = 1e-1, const double& rotationTol = 1e2) +bool approxEqual(const manif::SE3d& lhs, const manif::SE3d& rhs, const double& translationTol = 1e-1, const double& rotationTol = 1e-2) { Eigen::Matrix3d rotationDiffMat = (lhs.rotation().matrix() * rhs.rotation().inverse().matrix()).eval(); Eigen::AngleAxisd rotationDiffAA(rotationDiffMat);