diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 706c98efd0..b7e5612848 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -23,7 +23,7 @@ env: OsqpEigen_TAG: v0.7.0 tomlplusplus_TAG: v3.0.1 icub_models_TAG: v2.4.0 - UnicyclePlanner_TAG: d3f6c80afe21a9958da769c8dd8a2bbfee5ea922 + UnicyclePlanner_TAG: v0.7.0 telemetry_TAG: v1.2.0 bayes_filters_TAG: 53124b1d85fc00c8cd44d572a1e482d7e58c0f50 diff --git a/CHANGELOG.md b/CHANGELOG.md index 52448a2ecf..56758c9823 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,7 @@ All notable changes to this project are documented in this file. - Require `iDynTree v10.0.0` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/832) - Refactor `YarpRobotControl::setReferences` function to include optional current joint values and avoid to switch control mode in `YarpRobotControl::setReferences` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/833) - Set the gravity vector as an input argument of the `CentroidalMPC` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/823) +- Refactor the `Planners::UnicyclePlanner` to mimic the functionalitites of the planner deployed in [walking-controllers](https://github.com/robotology/walking-controllers) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/844) ### Fixed - Fix the barrier logic for threads synchronization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/811) diff --git a/bindings/python/Planners/CMakeLists.txt b/bindings/python/Planners/CMakeLists.txt index 9d68f2d625..d4ed471c4c 100644 --- a/bindings/python/Planners/CMakeLists.txt +++ b/bindings/python/Planners/CMakeLists.txt @@ -30,17 +30,3 @@ if(TARGET BipedalLocomotion::Planners) ) endif() - -if(TARGET BipedalLocomotion::Unicycle) - - set(H_PREFIX include/BipedalLocomotion/bindings/Planners) - - add_bipedal_locomotion_python_module( - NAME UnicycleBindings - SOURCES src/UnicyclePlanner.cpp src/UnicycleModule.cpp - HEADERS ${H_PREFIX}/UnicyclePlanner.h ${H_PREFIX}/UnicycleModule.h - LINK_LIBRARIES BipedalLocomotion::Unicycle - TESTS tests/test_unicycle_planner.py - ) - -endif() diff --git a/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicycleModule.h b/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicycleModule.h deleted file mode 100644 index e794d7cb3f..0000000000 --- a/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicycleModule.h +++ /dev/null @@ -1,26 +0,0 @@ -/** - * @file UnicycleModule.h - * @authors Giulio Romualdi - * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_MODULE_H -#define BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_MODULE_H - -#include - -namespace BipedalLocomotion -{ -namespace bindings -{ -namespace Planners -{ - -void CreateUnicycleModule(pybind11::module& module); - -} // namespace Planners -} // namespace bindings -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_MODULE_H diff --git a/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h b/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h deleted file mode 100644 index 9b6882e514..0000000000 --- a/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h +++ /dev/null @@ -1,18 +0,0 @@ -/** - * @file UnicyclePlanner.h - * @authors Diego Ferigo - * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H -#define BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H - -#include - -namespace BipedalLocomotion::bindings::Planners -{ -void CreateUnicyclePlanner(pybind11::module& module); -} // namespace BipedalLocomotion::bindings::Planners - -#endif // BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H diff --git a/bindings/python/Planners/src/UnicycleModule.cpp b/bindings/python/Planners/src/UnicycleModule.cpp deleted file mode 100644 index d4a5df0038..0000000000 --- a/bindings/python/Planners/src/UnicycleModule.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/** - * @file UnicycleModule.cpp - * @authors Giulio Romualdi, Diego Ferigo - * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include - -#include -#include - -namespace BipedalLocomotion -{ -namespace bindings -{ -namespace Planners -{ -void CreateUnicycleModule(pybind11::module& module) -{ - CreateUnicyclePlanner(module); -} -} // namespace Planners -} // namespace bindings -} // namespace BipedalLocomotion diff --git a/bindings/python/Planners/src/UnicyclePlanner.cpp b/bindings/python/Planners/src/UnicyclePlanner.cpp deleted file mode 100644 index ad52bb58cb..0000000000 --- a/bindings/python/Planners/src/UnicyclePlanner.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/** - * @file UnicyclePlanner.cpp - * @authors Diego Ferigo - * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace BipedalLocomotion::bindings::Planners -{ - -void CreateUnicyclePlanner(pybind11::module& module) -{ - namespace py = ::pybind11; - using namespace BipedalLocomotion::Planners; - using namespace BipedalLocomotion::System; - using namespace BipedalLocomotion::ParametersHandler; - - py::class_(module, "UnicycleKnot") - .def(py::init(), - py::arg("position") = std::make_tuple(0.0, 0.0), - py::arg("velocity") = std::make_tuple(0.0, 0.0), - py::arg("time") = 0.0) - .def_readwrite("time", &UnicycleKnot::time) - .def_readwrite("x", &UnicycleKnot::x) - .def_readwrite("y", &UnicycleKnot::y) - .def_readwrite("dx", &UnicycleKnot::dx) - .def_readwrite("dy", &UnicycleKnot::dy) - .def("__eq__", &UnicycleKnot::operator==, py::is_operator()) - .def("__repr__", [](const UnicycleKnot& k) { - return std::string("UnicycleKnot(") + // - "x=" + std::to_string(k.x) + ", " + // - "y=" + std::to_string(k.y) + ", " + // - "dx=" + std::to_string(k.dx) + ", " + // - "dy=" + std::to_string(k.dy) + ", " + // - "time=" + std::to_string(k.time) + // - ")"; - }); - - py::class_(module, "UnicyclePlannerInput") - .def(py::init&, - const double, - const std::optional&, - const std::optional&, - const double>(), - py::arg("knots") = std::vector{}, - py::arg("tf") = 0.0, - py::arg("initial_left_contact") = std::nullopt, - py::arg("initial_right_contact") = std::nullopt, - py::arg("t0") = 0.0) - .def_readwrite("t0", &UnicyclePlannerInput::t0) - .def_readwrite("tf", &UnicyclePlannerInput::tf) - .def_readwrite("initial_left_contact", &UnicyclePlannerInput::initialLeftContact) - .def_readwrite("initial_right_contact", &UnicyclePlannerInput::initialRightContact) - .def_readwrite("knots", &UnicyclePlannerInput::knots) - .def("__repr__", [](const UnicyclePlannerInput& i) { - auto printContact - = [](const std::optional& c) -> std::string { - return c ? "PlannedContact(" + c->name + ")" : "None"; - }; - return std::string("UnicyclePlannerInput(") + // - "t0=" + std::to_string(i.t0) + ", " + // - "tf=" + std::to_string(i.tf) + ", " + // - "knots=KnotList(" + std::to_string(i.knots.size()) + "), " + // - "initial_left_contact=" + printContact(i.initialLeftContact) + ", " + // - "initial_right_contact=" + printContact(i.initialRightContact) + // - ")"; - }); - - py::class_(module, "UnicyclePlannerOutput") - .def(py::init(), - py::arg("left") = Contacts::ContactList(), - py::arg("right") = Contacts::ContactList()) - .def_readwrite("left", &UnicyclePlannerOutput::left) - .def_readwrite("right", &UnicyclePlannerOutput::right) - .def("__repr__", [](const UnicyclePlannerOutput& o) { - auto printContactList = [](const Contacts::ContactList& l) -> std::string { - return "ContactList(" + std::to_string(l.size()) + ")"; - }; - return std::string("UnicyclePlannerOutput(") + // - "left=" + printContactList(o.left) + ", " + // - "right=" + printContactList(o.right) + // - ")"; - }); - - BipedalLocomotion::bindings::System::CreateAdvanceable( // - module, - "UnicyclePlanner"); - - py::class_>( // - module, - "UnicyclePlanner") - .def(py::init()); -} - -} // namespace BipedalLocomotion::bindings::Planners diff --git a/bindings/python/Planners/tests/test_unicycle_planner.py b/bindings/python/Planners/tests/test_unicycle_planner.py deleted file mode 100644 index d93621ce23..0000000000 --- a/bindings/python/Planners/tests/test_unicycle_planner.py +++ /dev/null @@ -1,156 +0,0 @@ -import pytest -pytestmark = pytest.mark.planners - -import bipedal_locomotion_framework.bindings as blf -import numpy as np - - -def test_unicycle_knot(): - - knot1 = blf.planners.UnicycleKnot() - - assert knot1.x == 0.0 - assert knot1.y == 0.0 - assert knot1.dx == 0.0 - assert knot1.dy == 0.0 - assert knot1.time == 0.0 - - knot2 = blf.planners.UnicycleKnot(position=(1.0, 2.0), - velocity=(0.1, 0.2), - time=0.5) - - assert knot2.x == 1.0 - assert knot2.y == 2.0 - assert knot2.dx == 0.1 - assert knot2.dy == 0.2 - assert knot2.time == 0.5 - - -def test_unicycle_planner_input(): - - unicycle_input1 = blf.planners.UnicyclePlannerInput() - - assert unicycle_input1.t0 == 0.0 - assert unicycle_input1.tf == 0.0 - assert unicycle_input1.knots == [] - - knot1 = blf.planners.UnicycleKnot( - position=(1.0, 0.0), velocity=(0.1, 0.2), time=0.5) - knot2 = blf.planners.UnicycleKnot( - position=(2.0, 1.0), velocity=(0.0, -0.2), time=1.5) - unicycle_input2 = blf.planners.UnicyclePlannerInput( - t0=1.0, tf=2.0, knots=[knot1, knot2]) - - assert unicycle_input2.t0 == 1.0 - assert unicycle_input2.tf == 2.0 - assert unicycle_input2.knots == [knot1, knot2] - - -def test_unicycle_planner_output(): - - unicycle_output1 = blf.planners.UnicyclePlannerOutput() - - assert unicycle_output1.left == blf.contacts.ContactList() - assert unicycle_output1.right == blf.contacts.ContactList() - - contact_left1 = blf.contacts.PlannedContact() - contact_left1.name = "ContactLeft1" - contact_left1.activation_time = 0.1 - contact_left1.deactivation_time = 0.5 - - contact_right1 = blf.contacts.PlannedContact() - contact_right1.name = "ContactRight1" - contact_right1.activation_time = 0.5 - contact_right1.deactivation_time = 2.5 - - left = blf.contacts.ContactList() - left.add_contact(contact_left1) - - right = blf.contacts.ContactList() - right.add_contact(contact_right1) - - unicycle_output2 = blf.planners.UnicyclePlannerOutput( - left=left, right=right) - - assert unicycle_output2.left == left - assert unicycle_output2.right == right - - -def test_unicycle_planner(capsys): - - parameters_handler = blf.parameters_handler.StdParametersHandler() - - # dt - parameters_handler.set_parameter_float("sampling_time", 0.010) - - # gains - parameters_handler.set_parameter_float("unicycleGain", 10.0) - parameters_handler.set_parameter_float("slowWhenTurningGain", 5.0) - - # reference - parameters_handler.set_parameter_vector_float("referencePosition", (0.1, 0.0)) - - # weights - parameters_handler.set_parameter_float("timeWeight", 2.5) - parameters_handler.set_parameter_float("positionWeight", 1.0) - - # duration - parameters_handler.set_parameter_float("minStepDuration", 0.8) - parameters_handler.set_parameter_float("maxStepDuration", 2.0) - parameters_handler.set_parameter_float("nominalDuration", 0.9) - - # step length - parameters_handler.set_parameter_float("minStepLength", 0.05) - parameters_handler.set_parameter_float("maxStepLength", 0.30) - - # feet distance - parameters_handler.set_parameter_float("minWidth", 0.14) - parameters_handler.set_parameter_float("nominalWidth", 0.16) - - # angle variation - parameters_handler.set_parameter_float("minAngleVariation", np.deg2rad(8.0)) - parameters_handler.set_parameter_float("maxAngleVariation", np.deg2rad(15.0)) - - # gait - parameters_handler.set_parameter_float("switchOverSwingRatio", 0.6) - - # Create the planner - unicycle = blf.planners.UnicyclePlanner() - -# with capsys.disabled(): -# assert not unicycle.set_input(input=blf.planners.UnicyclePlannerInput()) -# assert not unicycle.is_output_valid() -# assert not unicycle.advance() - - # Initialize the planner - assert unicycle.initialize(handler=parameters_handler) - - # Create the input - unicycle_input = blf.planners.UnicyclePlannerInput( - t0=0.0, tf=25.0, knots=[ - blf.planners.UnicycleKnot(position=(0.0, 0.0), velocity=(0.0, 0.0), time=0.0), - blf.planners.UnicycleKnot(position=(0.5, 0.0), velocity=(0.1, 0.0), time=10.0), - blf.planners.UnicycleKnot(position=(1.0, 0.25), velocity=(0.1, 0.1), time=20.0), - blf.planners.UnicycleKnot(position=(1.1, 0.25), velocity=(0.0, 0.0), time=25.0), - ]) - - # Set the input - assert unicycle.set_input(input=unicycle_input) - - # Compute the steps - assert unicycle.advance() - - # Get the output - unicycle_output = unicycle.get_output() - - # Check that steps have been computed - assert len(unicycle_output.left) > 0 - assert len(unicycle_output.right) > 0 - - for contact in unicycle_output.left: - print(contact) - - print() - - for contact in unicycle_output.right: - print(contact) diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index 706fa7b624..2d8b1ea565 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -53,10 +53,6 @@ #include @endcmakeiftarget BipedalLocomotion::Planners -@cmakeiftarget BipedalLocomotion::Planners && BipedalLocomotion::Unicycle -#include -@endcmakeiftarget BipedalLocomotion::Planners && BipedalLocomotion::Unicycle - @cmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::RobotInterfaceYarpImplementation #include @endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::RobotInterfaceYarpImplementation @@ -169,10 +165,6 @@ PYBIND11_MODULE(bindings, m) bindings::Planners::CreateModule(plannersModule); @endcmakeiftarget BipedalLocomotion::Planners - @cmakeiftarget BipedalLocomotion::Planners && BipedalLocomotion::Unicycle - bindings::Planners::CreateUnicycleModule(plannersModule); - @endcmakeiftarget BipedalLocomotion::Planners && BipedalLocomotion::Unicycle - @cmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::RobotInterfaceYarpImplementation py::module robotInterfaceModule = m.def_submodule("robot_interface"); bindings::RobotInterface::CreateModule(robotInterfaceModule); diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 35aed002c2..1784a4765f 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -26,10 +26,10 @@ if (FRAMEWORK_COMPILE_Unicycle) add_bipedal_locomotion_library( NAME Unicycle - PUBLIC_HEADERS ${H_PREFIX}/UnicyclePlanner.h - SOURCES src/UnicyclePlanner.cpp - PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts Eigen3::Eigen - PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging iDynTree::idyntree-core UnicyclePlanner + PUBLIC_HEADERS ${H_PREFIX}/UnicycleTrajectoryPlanner.h ${H_PREFIX}/UnicycleUtilities.h + SOURCES src/UnicycleTrajectoryPlanner.cpp src/UnicycleUtilities.cpp + PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts Eigen3::Eigen iDynTree::idyntree-modelio + PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging UnicyclePlanner BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::ManifConversions INSTALLATION_FOLDER Planners) endif() diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h deleted file mode 100644 index b98f2d75a3..0000000000 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h +++ /dev/null @@ -1,165 +0,0 @@ -/** - * @file UnicyclePlanner.h - * @authors Diego Ferigo, Stefano Dafarra - * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H -#define BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H - -#include "BipedalLocomotion/Contacts/ContactList.h" -#include "BipedalLocomotion/ParametersHandler/IParametersHandler.h" -#include "BipedalLocomotion/System/Advanceable.h" - -#include -#include - -#include -#include - -namespace BipedalLocomotion::Planners -{ -struct UnicycleKnot; -class UnicyclePlanner; -struct UnicyclePlannerInput; -struct UnicyclePlannerOutput; -} // namespace BipedalLocomotion::Planners - -struct BipedalLocomotion::Planners::UnicycleKnot -{ - UnicycleKnot(const double _x = 0.0, - const double _y = 0.0, - const double _dx = 0.0, - const double _dy = 0.0, - const double _t = 0.0) - : x(_x) - , y(_y) - , dx(_dx) - , dy(_dy) - , time(_t) - { - } - - UnicycleKnot(const Eigen::Vector2d& _position = {0, 0}, - const Eigen::Vector2d& _velocity = {0, 0}, - const double _time = 0.0) - : x(_position[0]) - , y(_position[1]) - , dx(_velocity[0]) - , dy(_velocity[1]) - , time(_time) - { - } - - bool operator==(const UnicycleKnot& rhs) - { - return this->x == rhs.x && this->y == rhs.y && this->dx == rhs.dx && this->dy == rhs.dy - && this->time == rhs.time; - } - - double x; ///< The knot x coordinates. - double y; ///< The knot y coordinates. - - double dx = 0.0; ///< The knot x velocity. - double dy = 0.0; ///< The knot y velocity. - - double time = 0.0; ///< The knot activation time. -}; - -struct BipedalLocomotion::Planners::UnicyclePlannerInput -{ - UnicyclePlannerInput(const std::vector& _knots, - const double _tf = 0.0, - const std::optional& _initialLeftContact = {}, - const std::optional& _initialRightContact = {}, - const double _t0 = 0.0) - : t0(_t0) - , tf(_tf) - , initialLeftContact(_initialLeftContact) - , initialRightContact(_initialRightContact) - , knots(_knots) - { - } - - double t0; ///< The beginning of the planner horizon. - double tf; ///< The end of the planner horizon. - - std::optional initialLeftContact; ///< Left contact initialization - std::optional initialRightContact; ///< Right contact initialization - - std::vector knots; ///< A list of knots. -}; - -struct BipedalLocomotion::Planners::UnicyclePlannerOutput -{ - UnicyclePlannerOutput(const Contacts::ContactList& _left = {}, - const Contacts::ContactList& _right = {}) - : left(_left) - , right(_right) - { - } - - Contacts::ContactList left; ///< The list of left foot contacts; - Contacts::ContactList right; ///< The list of right foot contacts; -}; - -class BipedalLocomotion::Planners::UnicyclePlanner final - : public System::Advanceable -{ -public: - UnicyclePlanner(); - - virtual ~UnicyclePlanner(); - - // clang-format off - - /** - * Initialize the planner. - * - * @note The following parameters are required by the class: - * - * | Name | Type | Default | Mandatory | Description | - * | :--------------------: | :------------: | :---------------: | :-------: | :------------------------------------------------: | - * | `sampling_time` | double | - | Yes | The sampling time of the planner | - * | `unicycleGain` | double | 10.0 | No | The main gain of the unicycle controller | - * | `slowWhenTurningGain` | double | 0.0 | No | The turnin gain of the unicycle controller | - * | `referencePosition` | list of double | (0.10, 0.00) | No | The reference position of the unicycle controller | - * | `timeWeight` | double | 1.0 | No | The time weight of the OC problem | - * | `positionWeight` | double | 1.0 | No | The position weight of the OC problem | - * | `minStepDuration` | double | - | Yes | The minimum duration of a step | - * | `maxStepDuration` | double | - | Yes | The maximum duration of a step | - * | `nominalDuration` | double | - | Yes | The nominal duration of a step | - * | `minStepLength` | double | - | Yes | The minimum length of a step | - * | `maxStepLength` | double | - | Yes | The maximum length of a step | - * | `minWidth` | double | - | Yes | The minimum feet distance | - * | `nominalWidth` | double | - | Yes | The nominal feet distance | - * | `minAngleVariation` | double | - | Yes | The minimum unicycle rotation | - * | `maxAngleVariation` | double | - | Yes | The maximum unicycle rotation | - * | `switchOverSwingRatio` | double | - | Yes | The ratio between single and double support phases | - * | `swingLeft` | bool | false | No | Perform the first step with the left foot | - * | `terminalStep` | bool | true | No | Add a terminal step at the end of the horizon | - * | `startAlwaysSameFoot` | bool | false | No | Restart with the default foot if still | - * | `left_foot_name` | string | left | No | Name of the left foot | - * | `right_foot_name` | string | right | No | Name of the right foot | - * - * @param handler Pointer to the parameter handler. - * @return True in case of success, false otherwise. - */ - bool initialize(std::weak_ptr handler) override; - // clang-format on - - const UnicyclePlannerOutput& getOutput() const override; - - bool isOutputValid() const override; - - bool setInput(const UnicyclePlannerInput& input) override; - - bool advance() override; - -private: - class Impl; - std::unique_ptr m_pImpl; -}; - -#endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h new file mode 100644 index 0000000000..d85c516606 --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -0,0 +1,265 @@ +/** + * @file UnicycleTrajectoryPlanner.h + * @authors Lorenzo Moretti, Diego Ferigo, Giulio Romualdi, Stefano Dafarra + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_TRAJECTORY_PLANNER_H +#define BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_TRAJECTORY_PLANNER_H + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +namespace BipedalLocomotion::Planners +{ +class UnicycleTrajectoryPlanner; +struct UnicycleTrajectoryPlannerInput; +struct UnicycleTrajectoryPlannerOutput; +struct UnicycleTrajectoryPlannerParameters; +} // namespace BipedalLocomotion::Planners + +struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerInput +{ + /** + * UnicycleTrajectoryPlannerInput implements the input to the planner. Depending on the type of + * unicycle controller used the plannerInput is a 2d-vector or a 3d-vector. For instance, if + * unicycle controller is set to UnicycleController::PERSON_FOLLOWING, the plannerInput is a + * vector of size 2 that represents a reference position (i.e., [x, y]). Instead, if it is set + * to UnicycleController::DIRECT, the plannerInput is a vector of size 3 that representes a + * velocity command (i.e., [xdot, ydot, wz]). + */ + Eigen::VectorXd plannerInput; /**< Input to the unicycle planner */ + + DCMInitialState dcmInitialState; /**< Initial state of the DCM trajectory generator */ + + struct COMInitialState + { + Eigen::Vector2d initialPlanarPosition; /**< Initial planar position of the CoM */ + Eigen::Vector2d initialPlanarVelocity; /**< Initial planar velocity of the CoM */ + }; + + COMInitialState comInitialState; /**< Initial state of the CoM */ + + bool isLeftLastSwinging; /**< True if the last foot that was swinging is the left one. False + otherwise. */ + + std::chrono::nanoseconds initTime; /**< Initial time of the trajectory */ + + manif::SE3d measuredTransform; /**< Measured transform of the last foot that touched + the floor */ + + static UnicycleTrajectoryPlannerInput generateDummyUnicycleTrajectoryPlannerInput(); +}; + +struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput +{ + struct COMTrajectory + { + std::vector position; + std::vector velocity; + std::vector acceleration; + }; + + struct DCMTrajectory + { + std::vector position; + std::vector velocity; + }; + + struct ContactStatus + { + std::vector leftFootInContact; /**< True if the left foot is in contact. False + otherwise. */ + std::vector rightFootInContact; /**< True if the right foot is in contact. False + otherwise. */ + std::vector UsedLeftAsFixed; /**< True if the left foot is the last that got in + contact. */ + }; + + struct Steps + { + std::deque leftSteps, rightSteps; + }; + + COMTrajectory comTrajectory; /**< CoM trajectory */ + + DCMTrajectory dcmTrajectory; /**< DCM trajectory */ + + ContactStatus contactStatus; /**< Contact status of the feet */ + + Steps steps; /**< List of steps and their phases */ + + std::vector mergePoints; /**< Indexes of the merge points of the trajectory */ +}; + +struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerParameters +{ + std::chrono::nanoseconds dt; /**< Sampling time of the planner */ + + std::chrono::nanoseconds plannerHorizon; /**< Time horizon of the planner */ + + double leftYawDeltaInRad; /**< Left foot cartesian offset in the yaw */ + + double rightYawDeltaInRad; /**< Right foot cartesian offset in the yaw */ + + Eigen::Vector2d referencePointDistance; /**< Reference position of the unicycle + controller */ + + double nominalWidth; /**< Nominal feet distance */ + + int leftContactFrameIndex; /**< Index of the left foot contact frame */ + + std::string leftContactFrameName; /**< Name of the left foot contact frame */ + + int rightContactFrameIndex; /**< Index of the right foot contact frame */ + + std::string rightContactFrameName; /**< Name of the right foot contact frame */ +}; + +/** + * UnicycleTrajectoryPlanner is a class that uses UnicycleGenerator of + * https://github.com/robotology/unicycle-footstep-planner) to generate reference trajectories for + * humanoid robots. Every time the advance() function is called, the planner generates a new + * trajectory, which spans the time horizon configured by the user. + * The getOutput() member function returns the generated trajectory, which includes the CoM, DCM, + * and footstep ones. The planner requires the user to set the robot model using the setRobotModel() + * member function, before invoking the initialize() member function, which configures the planner. + * As input, which is set using the setInput() member function, the planner requires an instance of + * the UnicycleTrajectoryPlannerInput struct. + */ +class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final + : public System::Advanceable +{ +public: + UnicycleTrajectoryPlanner(); + + virtual ~UnicycleTrajectoryPlanner(); + + /** + * Set the robot contact frames. + * It should be called after the initialize() function. + * It checks if the contact frames names parsed by the initialize() function exist. + * If yes, it sets the related contact frames indexes and returns true. + * Otherwise, it sets the Impl::FSM::State back to NotInitialized and returns false. + * @param model iDynTree::Model of the robot considered. + */ + bool setRobotContactFrames(const iDynTree::Model& model); + + // clang-format off + /** + * Initialize the planner. + * + * @note The following parameters are required by the class: + * + * | Name | Type | Default | Example | Description | + * | :-----------------------: | :---------------: | :---------------: | :-------------: | :-------------------------------------------------------------: | + * | `referencePosition` | list of 2 doubles | - | (0.1 0.0) | The reference position of the unicycle controller | + * | `controlType` | string | "direct" | - | The control mode used by the unicycle controller | + * | `unicycleGain` | double | 10.0 | - | The main gain of the unicycle controller | + * | `slowWhenTurningGain` | double | 2.0 | - | The turning gain of the unicycle controller | + * | `slowWhenBackwardFactor` | double | 0.4 | - | The backward gain of the unicycle controller | + * | `slowWhenSidewaysFactor` | double | 0.2 | - | The sideways gain of the unicycle controller | + * | `dt` | double | 0.002 | - | The sampling time of the planner | + * | `plannerHorizon` | double | 20.0 | - | The planner time horizon | + * | `positionWeight` | double | 1.0 | - | The position weight of the OC problem | + * | `timeWeight` | double | 2.5 | - | The time weight of the OC problem | + * | `maxStepLength` | double | 0.32 | - | The maximum length of a step | + * | `minStepLength` | double | 0.01 | - | The minimum length of a step | + * |`maxLengthBackwardFactor` | double | 0.8 | - | The factor of maximum backward walk | + * | `nominalWidth` | double | 0.20 | - | The nominal feet distance | + * | `minWidth` | double | 0.14 | - | The minimum feet distance | + * | `minStepDuration` | double | 0.65 | - | The minimum duration of a step | + * | `maxStepDuration` | double | 1.5 | - | The maximum duration of a step | + * | `nominalDuration` | double | 0.8 | - | The nominal duration of a step | + * | `maxAngleVariation` | double | 18.0 | - | The maximum unicycle rotation | + * | `minAngleVariation` | double | 5.0 | - | The minimum unicycle rotation | + * | `saturationFactors` | list of 2 doubles | - | (0.7 0.7) | Linear and Angular velocity conservative factors | + * | `leftYawDeltaInDeg` | double | 0.0 | - | Offset for the left foot rotation around the z axis | + * | `rightYawDeltaInDeg` | double | 0.0 | - | Offset for the right foot rotation around the z axis | + * | `swingLeft` | bool | false | - | Perform the first step with the left foot | + * | `startAlwaysSameFoot` | bool | false | - | Restart with the default foot if still | + * | `terminalStep` | bool | true | - | Add a terminal step at the end of the horizon | + * | `mergePointRatios` | list of 2 doubles | - | (0.4 0.4) | The ratios of the DS phase in which it is present a merge point | + * | `switchOverSwingRatio` | double | 0.2 | - | The ratio between single and double support phases | + * | `lastStepSwitchTime` | double | 0.3 | - | Time duration of double support phase in final step | + * | `isPauseActive` | bool | true | - | If true, the planner can pause, instead of make tiny steps. | + * | `comHeight` | double | 0.70 | - | CoM height in double support phase | + * | `comHeightDelta` | double | 0.01 | - | Delta to add to CoM heinght in Single support phases | + * | `leftZMPDelta` | list of 2 doubles | - | (0.0 0.0) | Local ZMP reference: delta wrt center frame of the foot | + * | `rightZMPDelta` | list of 2 doubles | - | (0.0 0.0) | Local ZMP reference: delta wrt center frame of the foot | + * | `lastStepDCMOffset` | double | 0.5 | - | Last Step DCM Offset. If 0, DCM coincides with stance foot ZMP | + * | `leftContactFrameName` | string | - | "l_sole" | Name of the left foot contact frame | + * | `rightContactFrameName` | string | - | "r_sole" | Name of the right foot contact frame | + * + * @param handler Pointer to the parameter handler. + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + // clang-format on + + /** + * Get the output of the planner. + * @return The output of the planner. + */ + const UnicycleTrajectoryPlannerOutput& getOutput() const override; + + /** + * Check if the output is valid. + * @return True if the output is valid, false otherwise. + */ + bool isOutputValid() const override; + + /** + * Set the input of the planner. + * @param input Input of the planner. + * @return True in case of success, false otherwise. + */ + bool setInput(const UnicycleTrajectoryPlannerInput& input) override; + + /** + * Advance the planner. + * @return True in case of success, false otherwise. + */ + bool advance() override; + + /** + * Get the contact phase list. + * It returns the contact phase list built with the footsteps generated by the planner. + * @return The contact phase list. + */ + Contacts::ContactPhaseList getContactPhaseList(); + +private: + class Impl; + std::unique_ptr m_pImpl; + + /** + * Set the unicycle controller mode based on the given string. + * @param unicycleControllerAsString unicycle controller mode as string. + * @param unicycleController unicycle controller mode. + * @return True in case of success, false otherwise. + */ + bool setUnicycleControllerFromString(const std::string& unicycleControllerAsString, + UnicycleController& unicycleController); + + /** + * Generate the first trajectory. + */ + bool generateFirstTrajectory(); +}; + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h new file mode 100644 index 0000000000..90bd36f675 --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h @@ -0,0 +1,37 @@ +/** + * @file UnicycleUtilities.h + * @authors Lorenzo Moretti,Giulio Romualdi, Stefano Dafarra + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_UTILITIES_H +#define BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_UTILITIES_H + +#include +#include + +namespace BipedalLocomotion::Planners::UnicycleUtilities +{ +/** + * It populates the contact list. + * @param initTime the initial time of the trajectory. + * @param dt the time step. + * @param inContact a vector containing the contact status. + * @param steps a deque containing the steps. + * @param contactFrameIndex the index of the contact frame. + * @param contactName the name of the contact. + * @param contactList the contact list to be populated. It should be empty or contain only the first + * step (i.e., the current one already in contact at time initTime). + */ +bool getContactList(const std::chrono::nanoseconds& initTime, + const std::chrono::nanoseconds& dt, + const std::vector& inContact, + const std::deque& steps, + const int& contactFrameIndex, + const std::string& contactName, + BipedalLocomotion::Contacts::ContactList& contactList); + +} // namespace BipedalLocomotion::Planners::UnicycleUtilities + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_UTILITIES_H diff --git a/src/Planners/src/UnicyclePlanner.cpp b/src/Planners/src/UnicyclePlanner.cpp deleted file mode 100644 index e3b0ad0709..0000000000 --- a/src/Planners/src/UnicyclePlanner.cpp +++ /dev/null @@ -1,709 +0,0 @@ -/** - * @file UnicyclePlanner.h - * @authors Diego Ferigo, Stefano Dafarra - * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -using namespace BipedalLocomotion; - -class Planners::UnicyclePlanner::Impl -{ -public: - struct - { - double planner; - } dt; - - struct - { - double t0; - double tf; - } horizon; - - struct - { - struct - { - double x = 0.10; - double y = 0.00; - } reference; - - struct - { - double unicycle = 10.0; - double slowWhenTurning = 0.0; - } gains; - } controller; - - struct - { - double time = 1.0; - double position = 1.0; - } weights; - - struct - { - double min; - double max; - double nominal; - } duration; - - struct - { - double min; - double max; - } stepLength; - - struct - { - double min; - double nominal; - } feetDistance; - - struct - { - double min; - double max; - } angleVariation; - - struct - { - double stancePhaseRatio; - bool startWithLeft = false; - bool terminalStep = true; - bool resetStartingFootIfStill = false; - } gait; - - struct - { - std::string left = "left"; - std::string right = "right"; - } names; - - struct - { - std::optional left; - std::optional right; - } initialContacts; - - UnicyclePlannerOutput outputRef; - std::optional output = std::nullopt; - - std::shared_ptr<::FootPrint> left; - std::shared_ptr<::FootPrint> right; - std::unique_ptr<::UnicyclePlanner> planner; -}; - -Planners::UnicyclePlanner::UnicyclePlanner() - : m_pImpl{std::make_unique()} -{ -} - -Planners::UnicyclePlanner::~UnicyclePlanner() = default; - -bool Planners::UnicyclePlanner::initialize( - std::weak_ptr handler) -{ - constexpr auto logPrefix = "[UnicyclePlanner::initialize]"; - - auto ptr = handler.lock(); - - if (ptr == nullptr) - { - log()->error("{} The handler has to point to an already initialized IParametersHandler.", - logPrefix); - return false; - } - - bool okPlanner = true; - m_pImpl->planner = std::make_unique<::UnicyclePlanner>(); - - // == - // dt - // == - - if (!ptr->getParameter("sampling_time", m_pImpl->dt.planner)) - { - log()->error("{} Unable to load the sampling time of the planner (sampling_time).", - logPrefix); - return false; - } - - okPlanner = okPlanner && m_pImpl->planner->setPlannerPeriod(m_pImpl->dt.planner); - okPlanner = okPlanner && m_pImpl->planner->setMaximumIntegratorStepSize(m_pImpl->dt.planner); - - // ========== - // controller - // ========== - - if (!ptr->getParameter("unicycleGain", m_pImpl->controller.gains.unicycle)) - { - log()->info("{} Using default unicycleGain={}.", - logPrefix, - m_pImpl->controller.gains.unicycle); - } - - if (!ptr->getParameter("slowWhenTurningGain", m_pImpl->controller.gains.slowWhenTurning)) - { - log()->info("{} Using default slowWhenTurningGain={}.", - logPrefix, - m_pImpl->controller.gains.slowWhenTurning); - } - - okPlanner = okPlanner - && m_pImpl->planner->setControllerGain(m_pImpl->controller.gains.unicycle); - okPlanner = okPlanner - && m_pImpl->planner->setSlowWhenTurnGain(m_pImpl->controller.gains.slowWhenTurning); - - std::vector reference; - - if (!(ptr->getParameter("referencePosition", reference) && reference.size() == 2)) - { - log()->info("{} Using default referencePosition=({}, {}).", - logPrefix, - m_pImpl->controller.reference.x, - m_pImpl->controller.reference.y); - } else - { - m_pImpl->controller.reference.x = reference[0]; - m_pImpl->controller.reference.y = reference[1]; - } - - okPlanner = okPlanner - && m_pImpl->planner->setDesiredPersonDistance(m_pImpl->controller.reference.x, - m_pImpl->controller.reference.y); - - // ======= - // weights - // ======= - - if (!ptr->getParameter("timeWeight", m_pImpl->weights.time)) - { - log()->info("{} Using default timeWeight={}.", logPrefix, m_pImpl->weights.time); - } - - if (!ptr->getParameter("positionWeight", m_pImpl->weights.position)) - { - log()->info("{} Using default positionWeight={}.", logPrefix, m_pImpl->weights.position); - } - - okPlanner - = okPlanner - && m_pImpl->planner->setCostWeights(m_pImpl->weights.position, m_pImpl->weights.time); - - // ======== - // duration - // ======== - - if (!ptr->getParameter("minStepDuration", m_pImpl->duration.min)) - { - log()->error("{} Unable to load the min step duration (minStepDuration).", logPrefix); - return false; - } - - if (!ptr->getParameter("maxStepDuration", m_pImpl->duration.max)) - { - log()->error("{} Unable to load the max step duration (maxStepDuration).", logPrefix); - return false; - } - - if (!ptr->getParameter("nominalDuration", m_pImpl->duration.nominal)) - { - log()->error("{} Unable to load the nominal step duration (nominalDuration).", logPrefix); - return false; - } - - okPlanner = okPlanner - && m_pImpl->planner->setStepTimings(m_pImpl->duration.min, - m_pImpl->duration.max, - m_pImpl->duration.nominal); - - // ========== - // stepLength - // ========== - - if (!ptr->getParameter("minStepLength", m_pImpl->stepLength.min)) - { - log()->error("{} Unable to load the min step length (minStepLength).", logPrefix); - return false; - } - - if (!ptr->getParameter("maxStepLength", m_pImpl->stepLength.max)) - { - log()->error("{} Unable to load the max step length (maxStepLength).", logPrefix); - return false; - } - - okPlanner = okPlanner && m_pImpl->planner->setMaxStepLength(m_pImpl->stepLength.max); - okPlanner = okPlanner && m_pImpl->planner->setMinimumStepLength(m_pImpl->stepLength.min); - - // ============ - // feetDistance - // ============ - - if (!ptr->getParameter("minWidth", m_pImpl->feetDistance.min)) - { - log()->error("{} Unable to load the min feet distance (minWidth).", logPrefix); - return false; - } - - if (!ptr->getParameter("nominalWidth", m_pImpl->feetDistance.nominal)) - { - log()->error("{} Unable to load the nominal feet distance (nominalWidth).", logPrefix); - return false; - } - - okPlanner = okPlanner - && m_pImpl->planner->setWidthSetting( // - m_pImpl->feetDistance.min, - m_pImpl->feetDistance.nominal); - - // ============== - // angleVariation - // ============== - - if (!ptr->getParameter("minAngleVariation", m_pImpl->angleVariation.min)) - { - log()->error("{} Unable to load the min foot angle variation (minAngleVariation).", - logPrefix); - return false; - } - - if (!ptr->getParameter("maxAngleVariation", m_pImpl->angleVariation.max)) - { - log()->error("{} Unable to load the max foot angle variation (maxAngleVariation).", - logPrefix); - return false; - } - - okPlanner = okPlanner && m_pImpl->planner->setMaxAngleVariation(m_pImpl->angleVariation.max); - okPlanner = okPlanner - && m_pImpl->planner->setMinimumAngleForNewSteps(m_pImpl->angleVariation.min); - - // ==== - // gait - // ==== - - if (!ptr->getParameter("switchOverSwingRatio", m_pImpl->gait.stancePhaseRatio)) - { - log()->error("{} Unable to load the stance phase ratio (switchOverSwingRatio).", logPrefix); - return false; - } - - if (m_pImpl->gait.stancePhaseRatio <= 0) - { - log()->error("{} The switchOverSwingRatio cannot be <= 0.", logPrefix); - return false; - } - - if (!ptr->getParameter("swingLeft", m_pImpl->gait.startWithLeft)) - { - log()->info("{} Using default swingLeft={}.", logPrefix, m_pImpl->gait.startWithLeft); - } - - if (!ptr->getParameter("terminalStep", m_pImpl->gait.terminalStep)) - { - log()->info("{} Using default terminalStep={}.", logPrefix, m_pImpl->gait.terminalStep); - } - - if (!ptr->getParameter("startAlwaysSameFoot", m_pImpl->gait.resetStartingFootIfStill)) - { - log()->info("{} Using default startAlwaysSameFoot={}.", - logPrefix, - m_pImpl->gait.resetStartingFootIfStill); - } - - m_pImpl->planner->startWithLeft(m_pImpl->gait.startWithLeft); - m_pImpl->planner->addTerminalStep(m_pImpl->gait.terminalStep); - m_pImpl->planner->resetStartingFootIfStill(m_pImpl->gait.resetStartingFootIfStill); - - // ===== - // names - // ===== - - if (!ptr->getParameter("left_foot_name", m_pImpl->names.left)) - { - log()->info("{} Using default left_foot_name={}.", logPrefix, m_pImpl->names.left); - } - - if (!ptr->getParameter("right_foot_name", m_pImpl->names.right)) - { - log()->info("{} Using default right_foot_name={}.", logPrefix, m_pImpl->names.right); - } - - // ============================= - // UnicyclePlanner configuration - // ============================= - - if (!okPlanner) - { - log()->error("{} Failed to configure UnicyclePlanner.", logPrefix); - return false; - } - - return true; -} - -const Planners::UnicyclePlannerOutput& Planners::UnicyclePlanner::getOutput() const -{ - constexpr auto logPrefix = "[UnicyclePlanner::getOutput]"; - - if (!this->isOutputValid()) - { - log()->warn("{} Returning an empty output.", logPrefix); - m_pImpl->outputRef = {}; - return m_pImpl->outputRef; - } - - m_pImpl->outputRef = *m_pImpl->output; - return m_pImpl->outputRef; -} - -bool Planners::UnicyclePlanner::isOutputValid() const -{ - constexpr auto logPrefix = "[UnicyclePlanner::isOutputValid]"; - - if (!m_pImpl->planner) - { - log()->error("{} The Unicycle planner has never been initialized.", logPrefix); - return false; - } - - if (!m_pImpl->left || !m_pImpl->right) - { - log()->error("{} The Unicycle planner never computed the foot steps.", logPrefix); - return false; - } - - if (!m_pImpl->output) - { - log()->error("{} The output has never been computed.", logPrefix); - return false; - } - - return true; -} - -bool Planners::UnicyclePlanner::setInput(const UnicyclePlannerInput& input) -{ - constexpr auto logPrefix = "[UnicyclePlanner::setInput]"; - - if (!m_pImpl->planner) - { - log()->error("{} The Unicycle planner has never been initialized.", logPrefix); - return false; - } - - auto getMaxKnotTime = [](const UnicyclePlannerInput& input) -> double { - double maxKnotTime = 0.0; - - for (const auto& knot : input.knots) - { - if (knot.time > maxKnotTime) - maxKnotTime = knot.time; - } - - return maxKnotTime; - }; - - if (input.tf < getMaxKnotTime(input)) - { - log()->error("{} The input contains a knot whose time is over the planner horizon.", - logPrefix); - return false; - } - - m_pImpl->planner->clearDesiredTrajectory(); - - for (const auto& knot : input.knots) - { - auto position = iDynTree::Vector2(); - position[0] = knot.x; - position[1] = knot.y; - - auto velocity = iDynTree::Vector2(); - velocity[0] = knot.dx; - velocity[1] = knot.dy; - - if (!m_pImpl->planner->addDesiredTrajectoryPoint(knot.time, position, velocity)) - { - m_pImpl->planner->clearDesiredTrajectory(); - log()->error("{} Failed to insert knot in the Unicycle planner.", logPrefix); - return false; - } - } - - m_pImpl->horizon.t0 = input.t0; - m_pImpl->horizon.tf = input.tf; - - m_pImpl->initialContacts.left = input.initialLeftContact; - m_pImpl->initialContacts.right = input.initialRightContact; - - return true; -} - -bool Planners::UnicyclePlanner::advance() -{ - constexpr auto logPrefix = "[UnicyclePlanner::advance]"; - - if (!m_pImpl->planner) - { - log()->error("{} The Unicycle planner has never been initialized.", logPrefix); - return false; - } - - // Lambda to clean up resources when returning false - auto cleanup = [&]() { - m_pImpl->left = nullptr; - m_pImpl->right = nullptr; - m_pImpl->output = std::nullopt; - }; - - // Cleanup first - cleanup(); - - // ================================== - // Plan contacts with UnicyclePlanner - // ================================== - - // Initialize the left FootPrint - m_pImpl->left = std::make_shared(); - m_pImpl->left->setFootName(m_pImpl->names.left); - - // Initialize the right FootPrint - m_pImpl->right = std::make_shared(); - m_pImpl->right->setFootName(m_pImpl->names.right); - - // Convert manif to iDynTree - auto toiDynTree = [](const manif::SE3d::Translation& translation) -> iDynTree::Vector2 { - iDynTree::Vector2 position; - position[0] = translation[0]; - position[1] = translation[1]; - return position; - }; - - // The initTime of the UnicyclePlanner cannot be smaller than - // the impact time of the last step. - // If an initial step configuration is passed, the initial time must be updated. - double initTime = m_pImpl->horizon.t0; - - // Process the initial left contact configuration - if (m_pImpl->initialContacts.left) - { - const auto& contact = m_pImpl->initialContacts.left; - - // Here we decompose the quaternion to YXZ intrinsic Euler angles (default in Eigen). - // The most reliable decomposition is ZXY extrinsic, that is equivalent since the two - // commute when the order is reversed. This decomposition, having Z as first rotation, - // should get the correct yaw in most cases. - const auto& euler - = contact->pose.quat().normalized().toRotationMatrix().eulerAngles(1, 0, 2); - - // Create the inital step - m_pImpl->left->addStep(toiDynTree(contact->pose.translation()), - euler[2], - std::chrono::duration(contact->activationTime).count()); - - const double impactTime = std::chrono::duration(contact->activationTime).count(); - initTime = impactTime > initTime ? impactTime : initTime; - } - - // Process the initial left contact configuration - if (m_pImpl->initialContacts.right) - { - const auto& contact = m_pImpl->initialContacts.right; - - // Here we decompose the quaternion to YXZ intrinsic Euler angles (default in Eigen). - // The most reliable decomposition is ZXY extrinsic, that is equivalent since the two - // commute when the order is reversed. This decomposition, having Z as first rotation, - // should get the correct yaw in most cases. - const auto& euler - = contact->pose.quat().normalized().toRotationMatrix().eulerAngles(1, 0, 2); - - // Create the inital step - m_pImpl->right->addStep(toiDynTree(contact->pose.translation()), - euler[2], - std::chrono::duration(contact->activationTime).count()); - - const double impactTime = std::chrono::duration(contact->activationTime).count(); - initTime = impactTime > initTime ? impactTime : initTime; - } - - if (!m_pImpl->planner->computeNewSteps(m_pImpl->left, - m_pImpl->right, - initTime, - m_pImpl->horizon.tf)) - { - cleanup(); - log()->error("{} Failed to compute new steps.", logPrefix); - return false; - } - - // =========================================== - // Compute step timings with UnicycleGenerator - // =========================================== - - // Create and configure the generator - auto generator = UnicycleGenerator(); - generator.setSwitchOverSwingRatio(m_pImpl->gait.stancePhaseRatio); - generator.setPauseConditions(m_pImpl->duration.max, m_pImpl->duration.nominal); - - // The last step will have an infinite deactivation time, the following option - // is necessary for the generator but it does not affect the advanceable output - generator.setTerminalHalfSwitchTime(1.0); - - // Due to how the generator works, the start time must be bigger than last impact time - const double startLeft = m_pImpl->left->getSteps().front().impactTime; - const double startRight = m_pImpl->right->getSteps().front().impactTime; - const double startTime = std::max(startLeft, startRight); - - // Compute the contact states using the generator - if (!generator.generateFromFootPrints(m_pImpl->left, - m_pImpl->right, - startTime, - m_pImpl->dt.planner)) - { - cleanup(); - log()->error("{} Failed to generate from footprints.", logPrefix); - return false; - } - - // Get the contact states over the horizon - std::vector leftStandingPeriod; - std::vector rightStandingPeriod; - generator.getFeetStandingPeriods(leftStandingPeriod, rightStandingPeriod); - - // Bind dt to catch it in the next lambda - const auto& dt = m_pImpl->dt.planner; - - // Lambda to convert Step to Contact, filling only the timings. - // Transforms will be included in a later stage. - auto convertStepsToContacts - = [dt](const std::vector& isFootInContactVector, - const StepList& steps) -> std::vector { - std::vector contacts; - - for (const auto& step : steps) - { - using namespace std::chrono_literals; - auto contact = Contacts::PlannedContact(); - contact.name = step.footName; - contact.activationTime - = std::chrono::duration_cast(step.impactTime * 1s); - contact.deactivationTime = contact.activationTime; - contacts.push_back(contact); - } - - size_t contactIdx = 0; - double activeTime = 0.0; - - for (size_t idx = 1; idx < isFootInContactVector.size(); ++idx) - { - // Get the active contact - auto& contact = contacts[contactIdx]; - - const bool thisState = isFootInContactVector[idx]; - const bool lastState = isFootInContactVector[idx - 1]; - - // Increase the active time - activeTime += dt; - - // During impact, reset the active time counter - if (lastState == 0 && thisState == 1) - activeTime = 0.0; - - // During lift, store the time in the active contact and get the new contact - if (lastState == 1 && thisState == 0) - { - using namespace std::chrono_literals; - contact.deactivationTime - = contact.activationTime - + std::chrono::duration_cast(activeTime * 1s); - contactIdx++; - } - } - - // The deactivation time of the last contact has not yet been processed - contacts.back().deactivationTime = std::chrono::nanoseconds::max(); - - return contacts; - }; - - // Convert Step objects to PlannedContact objects - std::vector leftContacts - = convertStepsToContacts(leftStandingPeriod, m_pImpl->left->getSteps()); - std::vector rightContacts - = convertStepsToContacts(rightStandingPeriod, m_pImpl->right->getSteps()); - - if (m_pImpl->left->getSteps().size() != leftContacts.size()) - { - cleanup(); - log()->error("{} Wrong number of converted steps for left foot.", logPrefix); - return false; - } - - if (m_pImpl->right->getSteps().size() != rightContacts.size()) - { - cleanup(); - log()->error("{} Wrong number of converted steps for right foot.", logPrefix); - return false; - } - - // ======================================= - // Convert Step transforms to Contact pose - // ======================================= - - // Lambda to fill the transforms of the PlannedContact objects - auto fillContactTransform = [](std::vector& contacts, - decltype(::FootPrint().getSteps())& steps) -> void { - assert(contacts.size() == steps.size()); - for (size_t i = 0; i < contacts.size(); ++i) - { - contacts[i].pose.quat(Eigen::AngleAxisd(steps[i].angle, Eigen::Vector3d::UnitZ())); - contacts[i].pose.translation({steps[i].position[0], steps[i].position[1], 0.0}); - } - }; - - // Fill the transforms - fillContactTransform(leftContacts, m_pImpl->left->getSteps()); - fillContactTransform(rightContacts, m_pImpl->right->getSteps()); - - // ================================ - // Create the output data structure - // ================================ - - // Lambda to convert vector of PlannedContact to ConctactList - auto convertToContactList - = [](const std::vector& contacts) -> Contacts::ContactList { - Contacts::ContactList list; - for (const auto& contact : contacts) - list.addContact(contact); - return list; - }; - - // Create the system's output - m_pImpl->output = std::make_optional(); - - m_pImpl->output->left = convertToContactList(leftContacts); - m_pImpl->output->right = convertToContactList(rightContacts); - - m_pImpl->output->left.setDefaultName(m_pImpl->names.left); - m_pImpl->output->right.setDefaultName(m_pImpl->names.right); - - return true; -} diff --git a/src/Planners/src/UnicycleTrajectoryPlanner.cpp b/src/Planners/src/UnicycleTrajectoryPlanner.cpp new file mode 100644 index 0000000000..ed33743a3b --- /dev/null +++ b/src/Planners/src/UnicycleTrajectoryPlanner.cpp @@ -0,0 +1,800 @@ +/** + * @file UnicycleTrajectoryPlanner.cpp + * @authors Lorenzo Moretti, Diego Ferigo, Giulio Romualdi, Stefano Dafarra + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +using namespace BipedalLocomotion; + +class Planners::UnicycleTrajectoryPlanner::Impl +{ + +public: + enum class FSM + { + NotInitialized, + Initialized, + Running, + }; + + FSM state{FSM::NotInitialized}; + + UnicycleTrajectoryPlannerOutput output; + + UnicycleTrajectoryPlannerInput input; + + UnicycleTrajectoryPlannerParameters parameters; + + UnicycleGenerator generator; + + std::mutex mutex; + + /* + The CoM model is the Linear Inverted Pendulum Model, described by the equations: + + | xd | | -w 0 0 0 | | x | | +w 0 0 0 | | Xdcm | + | yd | = | 0 -w 0 0 | * | y | + | 0 +w 0 0 | * | Ydcm | + | xdd | | 0 0 -w 0 | | xd | | 0 0 +w 0 | | Xdcmd | + | ydd | | 0 0 0 -w | | yd | | 0 0 0 +w | | Xdcmd | + + where: + {x,y} is the CoM planar position + + dcm is the Divergent Component of Motion + + w is the angular frequency of the Linear Inverted Pendulum, computed as sqrt(g/z), with z + being the CoM constant height + */ + struct COMSystem + { + std::shared_ptr + dynamics; + std::shared_ptr> + integrator; + }; + + COMSystem comSystem; + + std::chrono::nanoseconds initTime{std::chrono::nanoseconds::zero()}; /**< init time of the + trajectory generated by + the planner */ + struct COMHeightTrajectory + { + std::vector position, velocity, acceleration; + }; + + COMHeightTrajectory comHeightTrajectory; +}; + +BipedalLocomotion::Planners::UnicycleTrajectoryPlannerInput BipedalLocomotion::Planners:: + UnicycleTrajectoryPlannerInput::generateDummyUnicycleTrajectoryPlannerInput() +{ + UnicycleTrajectoryPlannerInput input; + + input.plannerInput = Eigen::VectorXd::Zero(3); + + iDynTree::Vector2 dcmInitialPosition, dcmInitialVelocity; + dcmInitialPosition.zero(); + dcmInitialVelocity.zero(); + input.dcmInitialState.initialPosition = dcmInitialPosition; + input.dcmInitialState.initialVelocity = dcmInitialVelocity; + + input.isLeftLastSwinging = false; + + input.initTime = std::chrono::nanoseconds::zero(); + + input.measuredTransform = manif::SE3d::Identity(); + input.measuredTransform.translation(Eigen::Vector3d(0.0, -0.1, 0.0)); + + return input; +} + +bool Planners::UnicycleTrajectoryPlanner::setUnicycleControllerFromString( + const std::string& unicycleControllerAsString, UnicycleController& unicycleController) +{ + if (unicycleControllerAsString == "personFollowing") + { + unicycleController = UnicycleController::PERSON_FOLLOWING; + } else if (unicycleControllerAsString == "direct") + { + unicycleController = UnicycleController::DIRECT; + } else + { + log()->error("[UnicycleTrajectoryPlanner::setUnicycleControllerFromString] Invalid " + "controller type."); + return false; + } + + return true; +} + +Planners::UnicycleTrajectoryPlanner::UnicycleTrajectoryPlanner() +{ + m_pImpl = std::make_unique(); +} + +Planners::UnicycleTrajectoryPlanner::~UnicycleTrajectoryPlanner() = default; + +bool Planners::UnicycleTrajectoryPlanner::setRobotContactFrames(const iDynTree::Model& model) +{ + + const auto logPrefix = "[UnicycleTrajectoryPlanner::setRobotContactFrames]"; + + if (m_pImpl->state == Impl::FSM::NotInitialized) + { + log()->error("{} The Unicycle planner has not been initialized. Initialize it first.", + logPrefix); + return false; + } + + iDynTree::KinDynComputations kinDyn; + + if (!kinDyn.loadRobotModel(model)) + { + log()->error("{} Unable to load the robot model.", logPrefix); + m_pImpl->state = Impl::FSM::NotInitialized; + return false; + } + + m_pImpl->parameters.leftContactFrameIndex + = kinDyn.model().getFrameIndex(m_pImpl->parameters.leftContactFrameName); + if (m_pImpl->parameters.leftContactFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} Unable to find the frame named {}.", + logPrefix, + m_pImpl->parameters.leftContactFrameName); + m_pImpl->state = Impl::FSM::NotInitialized; + return false; + } + + m_pImpl->parameters.rightContactFrameIndex + = kinDyn.model().getFrameIndex(m_pImpl->parameters.rightContactFrameName); + if (m_pImpl->parameters.rightContactFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} Unable to find the frame named {}.", + logPrefix, + m_pImpl->parameters.rightContactFrameName); + m_pImpl->state = Impl::FSM::NotInitialized; + return false; + } + + return true; +} + +bool Planners::UnicycleTrajectoryPlanner::initialize( + std::weak_ptr handler) +{ + constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::initialize]"; + + auto ptr = handler.lock(); + + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return false; + } + + // lambda function to parse parameters + auto loadParam = [ptr, logPrefix](const std::string& paramName, auto& param) -> bool { + if (!ptr->getParameter(paramName, param)) + { + log()->error("{} Unable to get the parameter named '{}'.", logPrefix, paramName); + return false; + } + return true; + }; + + // lambda function to parse parameters with fallback option + auto loadParamWithFallback = + [ptr, logPrefix](const std::string& paramName, auto& param, const auto& fallback) -> bool { + if (!ptr->getParameter(paramName, param)) + { + log()->info("{} Unable to find the parameter named '{}'. The default one with value " + "[{}] will be used.", + logPrefix, + paramName, + fallback); + param = fallback; + } + return true; + }; + + // initialize parameters + std::string unicycleControllerAsString; + + double unicycleGain; + double slowWhenTurningGain; + double slowWhenBackwardFactor; + double slowWhenSidewaysFactor; + + double positionWeight; + double timeWeight; + + std::string leftContactFrameName; + std::string rightContactFrameName; + + double maxStepLength; + double minStepLength; + double maxLengthBackwardFactor; + double minWidth; + double minStepDuration; + double maxStepDuration; + double nominalDuration; + double maxAngleVariation; + double minAngleVariation; + + Eigen::Vector2d saturationFactors; + + bool startWithLeft{true}; + bool startWithSameFoot{true}; + bool terminalStep{true}; + + Eigen::Vector2d mergePointRatios; + double switchOverSwingRatio; + double lastStepSwitchTime; + bool isPauseActive{true}; + + double comHeight; + double comHeightDelta; + Eigen::Vector2d leftZMPDelta; + Eigen::Vector2d rightZMPDelta; + double lastStepDCMOffset; + + // parse initialization parameters + bool ok = true; + + ok = ok && loadParam("referencePosition", m_pImpl->parameters.referencePointDistance); + ok = ok && loadParamWithFallback("controlType", unicycleControllerAsString, "direct"); + ok = ok && loadParamWithFallback("unicycleGain", unicycleGain, 10.0); + ok = ok && loadParamWithFallback("slowWhenTurningGain", slowWhenTurningGain, 2.0); + ok = ok && loadParamWithFallback("slowWhenBackwardFactor", slowWhenBackwardFactor, 0.4); + ok = ok && loadParamWithFallback("slowWhenSidewaysFactor", slowWhenSidewaysFactor, 0.2); + double dt, plannerHorizon; + ok = ok && loadParamWithFallback("dt", dt, 0.002); + m_pImpl->parameters.dt = std::chrono::nanoseconds(static_cast(dt * 1e9)); + ok = ok && loadParamWithFallback("plannerHorizon", plannerHorizon, 20.0); + m_pImpl->parameters.plannerHorizon + = std::chrono::nanoseconds(static_cast(plannerHorizon * 1e9)); + ok = ok && loadParamWithFallback("positionWeight", positionWeight, 1.0); + ok = ok && loadParamWithFallback("timeWeight", timeWeight, 2.5); + ok = ok && loadParamWithFallback("maxStepLength", maxStepLength, 0.32); + ok = ok && loadParamWithFallback("minStepLength", minStepLength, 0.01); + ok = ok && loadParamWithFallback("maxLengthBackwardFactor", maxLengthBackwardFactor, 0.8); + ok = ok && loadParamWithFallback("nominalWidth", m_pImpl->parameters.nominalWidth, 0.20); + ok = ok && loadParamWithFallback("minWidth", minWidth, 0.14); + ok = ok && loadParamWithFallback("minStepDuration", minStepDuration, 0.65); + ok = ok && loadParamWithFallback("maxStepDuration", maxStepDuration, 1.5); + ok = ok && loadParamWithFallback("nominalDuration", nominalDuration, 0.8); + ok = ok && loadParamWithFallback("maxAngleVariation", maxAngleVariation, 18.0); + ok = ok && loadParamWithFallback("minAngleVariation", minAngleVariation, 5.0); + ok = ok && loadParam("saturationFactors", saturationFactors); + ok = ok + && loadParamWithFallback("leftYawDeltaInDeg", m_pImpl->parameters.leftYawDeltaInRad, 0.0); + ok = ok + && loadParamWithFallback("rightYawDeltaInDeg", + m_pImpl->parameters.rightYawDeltaInRad, + 0.0); + m_pImpl->parameters.leftYawDeltaInRad + = iDynTree::deg2rad(m_pImpl->parameters.leftYawDeltaInRad); + m_pImpl->parameters.rightYawDeltaInRad + = iDynTree::deg2rad(m_pImpl->parameters.rightYawDeltaInRad); + ok = ok && loadParamWithFallback("swingLeft", startWithLeft, false); + ok = ok && loadParamWithFallback("startAlwaysSameFoot", startWithSameFoot, true); + ok = ok && loadParamWithFallback("terminalStep", terminalStep, true); + ok = ok && loadParam("mergePointRatios", mergePointRatios); + ok = ok && loadParamWithFallback("switchOverSwingRatio", switchOverSwingRatio, 0.2); + ok = ok && loadParamWithFallback("lastStepSwitchTime", lastStepSwitchTime, 0.3); + ok = ok && loadParamWithFallback("isPauseActive", isPauseActive, true); + ok = ok && loadParamWithFallback("comHeight", comHeight, 0.70); + ok = ok && loadParamWithFallback("comHeightDelta", comHeightDelta, 0.01); + ok = ok && loadParam("leftZMPDelta", leftZMPDelta); + ok = ok && loadParam("rightZMPDelta", rightZMPDelta); + ok = ok && loadParamWithFallback("lastStepDCMOffset", lastStepDCMOffset, 0.5); + ok = ok && loadParam("leftContactFrameName", m_pImpl->parameters.leftContactFrameName); + ok = ok && loadParam("rightContactFrameName", m_pImpl->parameters.rightContactFrameName); + + // try to configure the planner + auto unicyclePlanner = m_pImpl->generator.unicyclePlanner(); + + ok = ok + && unicyclePlanner->setDesiredPersonDistance(m_pImpl->parameters.referencePointDistance[0], + m_pImpl->parameters.referencePointDistance[1]); + ok = ok && unicyclePlanner->setPersonFollowingControllerGain(unicycleGain); + ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); + ok = ok && unicyclePlanner->setSlowWhenBackwardFactor(slowWhenBackwardFactor); + ok = ok && unicyclePlanner->setSlowWhenSidewaysFactor(slowWhenBackwardFactor); + ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength, maxLengthBackwardFactor); + ok = ok && unicyclePlanner->setMaximumIntegratorStepSize(m_pImpl->parameters.dt.count() * 1e-9); + ok = ok && unicyclePlanner->setWidthSetting(minWidth, m_pImpl->parameters.nominalWidth); + ok = ok && unicyclePlanner->setMaxAngleVariation(maxAngleVariation); + ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation); + ok = ok && unicyclePlanner->setCostWeights(positionWeight, timeWeight); + ok = ok && unicyclePlanner->setStepTimings(minStepDuration, maxStepDuration, nominalDuration); + ok = ok && unicyclePlanner->setPlannerPeriod(m_pImpl->parameters.dt.count() * 1e-9); + ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); + ok = ok + && unicyclePlanner->setSaturationsConservativeFactors(saturationFactors(0), + saturationFactors(1)); + unicyclePlanner->setLeftFootYawOffsetInRadians(m_pImpl->parameters.leftYawDeltaInRad); + unicyclePlanner->setRightFootYawOffsetInRadians(m_pImpl->parameters.rightYawDeltaInRad); + unicyclePlanner->addTerminalStep(terminalStep); + unicyclePlanner->startWithLeft(startWithLeft); + unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); + + UnicycleController unicycleController; + ok = ok && setUnicycleControllerFromString(unicycleControllerAsString, unicycleController); + ok = ok && unicyclePlanner->setUnicycleController(unicycleController); + + ok = ok && m_pImpl->generator.setSwitchOverSwingRatio(switchOverSwingRatio); + ok = ok && m_pImpl->generator.setTerminalHalfSwitchTime(lastStepSwitchTime); + ok = ok && m_pImpl->generator.setPauseConditions(maxStepDuration, nominalDuration); + ok = ok && m_pImpl->generator.setMergePointRatio(mergePointRatios[0], mergePointRatios[1]); + + m_pImpl->generator.setPauseActive(isPauseActive); + + auto comHeightGenerator = m_pImpl->generator.addCoMHeightTrajectoryGenerator(); + ok = ok && comHeightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); + + auto dcmGenerator = m_pImpl->generator.addDCMTrajectoryGenerator(); + iDynTree::Vector2 leftZMPDeltaVec{leftZMPDelta}; + iDynTree::Vector2 rightZMPDeltaVec{rightZMPDelta}; + dcmGenerator->setFootOriginOffset(leftZMPDeltaVec, rightZMPDeltaVec); + double omega = sqrt(BipedalLocomotion::Math::StandardAccelerationOfGravitation / comHeight); + dcmGenerator->setOmega(omega); + dcmGenerator->setFirstDCMTrajectoryMode(FirstDCMTrajectoryMode::FifthOrderPoly); + ok = ok && dcmGenerator->setLastStepDCMOffsetPercentage(lastStepDCMOffset); + + // initialize the COM system + m_pImpl->comSystem.dynamics + = std::make_shared(); + m_pImpl->comSystem.integrator + = std::make_shared>(); + + // Set dynamical system matrices + Eigen::Matrix4d A = -omega * Eigen::Matrix4d::Identity(); + Eigen::Matrix4d B = -A; + ok = ok && m_pImpl->comSystem.dynamics->setSystemMatrices(A, B); + // Set the initial state + ok = ok && m_pImpl->comSystem.dynamics->setState({Eigen::Vector4d::Zero()}); + // Set the dynamical system to the integrator + ok = ok && m_pImpl->comSystem.integrator->setDynamicalSystem(m_pImpl->comSystem.dynamics); + ok = ok && m_pImpl->comSystem.integrator->setIntegrationStep(m_pImpl->parameters.dt); + + // generateFirstTrajectory; + ok = ok && generateFirstTrajectory(); + + // debug information + auto leftSteps = m_pImpl->generator.getLeftFootPrint()->getSteps(); + + for (const auto& step : leftSteps) + { + BipedalLocomotion::log()->debug("Left step at initialization: position: {}, angle: {}, " + "impact time: {}", + step.position.toString(), + step.angle, + step.impactTime); + } + + auto rightSteps = m_pImpl->generator.getRightFootPrint()->getSteps(); + + for (const auto& step : rightSteps) + { + BipedalLocomotion::log()->debug("Right step at initialization: position: {}, angle: {}, " + "impact time: {}", + step.position.toString(), + step.angle, + step.impactTime); + } + + std::vector leftPhases, rightPhases; + m_pImpl->generator.getStepPhases(leftPhases, rightPhases); + + for (size_t i = 0; i < leftPhases.size(); i++) + { + BipedalLocomotion::log()->debug("Left phase at initialization: {}", + static_cast(leftPhases.at(i))); + } + + for (size_t i = 0; i < rightPhases.size(); i++) + { + BipedalLocomotion::log()->debug("Right phase at initialization: {}", + static_cast(rightPhases.at(i))); + } + + if (ok) + { + m_pImpl->state = Impl::FSM::Initialized; + } + + return ok; +} + +const Planners::UnicycleTrajectoryPlannerOutput& +Planners::UnicycleTrajectoryPlanner::getOutput() const +{ + constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::getOutput]"; + + std::lock_guard lock(m_pImpl->mutex); + + return m_pImpl->output; +} + +bool Planners::UnicycleTrajectoryPlanner::isOutputValid() const +{ + return m_pImpl->state == Impl::FSM::Running; +} + +bool Planners::UnicycleTrajectoryPlanner::setInput(const UnicycleTrajectoryPlannerInput& input) +{ + constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::setInput]"; + + if (m_pImpl->state == Impl::FSM::NotInitialized) + { + log()->error("{} The Unicycle planner has never been initialized.", logPrefix); + return false; + } + + m_pImpl->input = input; + + return true; +} + +bool Planners::UnicycleTrajectoryPlanner::advance() +{ + constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::advance]"; + + if (m_pImpl->state == Impl::FSM::NotInitialized) + { + log()->error("{} The Unicycle planner has never been initialized.", logPrefix); + return false; + } + + auto unicyclePlanner = m_pImpl->generator.unicyclePlanner(); + auto dcmGenerator = m_pImpl->generator.addDCMTrajectoryGenerator(); + + double initTime{m_pImpl->input.initTime.count() * 1e-9}; + m_pImpl->initTime = m_pImpl->input.initTime; + double dt{m_pImpl->parameters.dt.count() * 1e-9}; + + // check if it is not the first run + if (m_pImpl->state == Impl::FSM::Running) + { + bool correctLeft{!m_pImpl->input.isLeftLastSwinging}; + + // compute end time of trajectory + double endTime = initTime + m_pImpl->parameters.plannerHorizon.count() * 1e-9; + + // set desired point + Eigen::Vector2d desiredPointInRelativeFrame, desiredPointInAbsoluteFrame; + desiredPointInRelativeFrame(0) = m_pImpl->input.plannerInput(0); + desiredPointInRelativeFrame(0) = m_pImpl->input.plannerInput(1); + + // left foot + Eigen::Vector2d measuredPositionLeft; + double measuredAngleLeft; + double leftYawDeltaInRad; + measuredPositionLeft(0) = m_pImpl->input.measuredTransform.x(); + measuredPositionLeft(1) = m_pImpl->input.measuredTransform.y(); + measuredAngleLeft + = Conversions::toiDynTreeRot(m_pImpl->input.measuredTransform.asSO3()).asRPY()(2); + leftYawDeltaInRad = m_pImpl->parameters.leftYawDeltaInRad; + + // right foot + Eigen::Vector2d measuredPositionRight; + double measuredAngleRight; + double rightYawDeltaInRad; + measuredPositionRight(0) = m_pImpl->input.measuredTransform.x(); + measuredPositionRight(1) = m_pImpl->input.measuredTransform.y(); + measuredAngleRight + = Conversions::toiDynTreeRot(m_pImpl->input.measuredTransform.asSO3()).asRPY()(2); + rightYawDeltaInRad = m_pImpl->parameters.rightYawDeltaInRad; + + // get unicycle pose + double measuredAngle; + measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; + Eigen::Vector2d measuredPosition = correctLeft ? measuredPositionLeft + : measuredPositionRight; + Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition; + unicyclePositionFromStanceFoot(0) = 0.0; + + Eigen::Matrix2d unicycleRotation; + double unicycleAngle; + + if (correctLeft) + { + unicyclePositionFromStanceFoot(1) = -m_pImpl->parameters.nominalWidth / 2; + unicycleAngle = measuredAngleLeft - leftYawDeltaInRad; + footPosition = measuredPositionLeft; + } else + { + unicyclePositionFromStanceFoot(1) = m_pImpl->parameters.nominalWidth / 2; + unicycleAngle = measuredAngleRight - rightYawDeltaInRad; + footPosition = measuredPositionRight; + } + + double s_theta = std::sin(unicycleAngle); + double c_theta = std::cos(unicycleAngle); + + unicycleRotation(0, 0) = c_theta; + unicycleRotation(0, 1) = -s_theta; + unicycleRotation(1, 0) = s_theta; + unicycleRotation(1, 1) = c_theta; + + unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot + footPosition; + + // apply the homogeneous transformation w_H_{unicycle} + desiredPointInAbsoluteFrame + = unicycleRotation + * (m_pImpl->parameters.referencePointDistance + desiredPointInRelativeFrame) + + unicyclePosition; + + // clear the old trajectory + unicyclePlanner->clearPersonFollowingDesiredTrajectory(); + + // add new point + if (!unicyclePlanner + ->addPersonFollowingDesiredTrajectoryPoint(endTime, + iDynTree::Vector2( + desiredPointInAbsoluteFrame))) + { + log()->error("{} Error while setting the new reference.", logPrefix); + return false; + } + + // set the desired direct control + unicyclePlanner->setDesiredDirectControl(m_pImpl->input.plannerInput(0), + m_pImpl->input.plannerInput(1), + m_pImpl->input.plannerInput(2)); + + // set the initial state of the DCM trajectory generator + + if (!dcmGenerator->setDCMInitialState(m_pImpl->input.dcmInitialState)) + { + log()->error("{} Failed to set the initial state.", logPrefix); + return false; + } + + // generate the new trajectory + if (!(m_pImpl->generator.reGenerate(initTime, + dt, + endTime, + correctLeft, + iDynTree::Vector2(measuredPosition), + measuredAngle))) + { + log()->error("{} Failed in computing new trajectory.", logPrefix); + return false; + } + } + + // get the output + std::lock_guard lock(m_pImpl->mutex); + + // get the feet contact status + m_pImpl->generator.getFeetStandingPeriods(m_pImpl->output.contactStatus.leftFootInContact, + m_pImpl->output.contactStatus.rightFootInContact); + + m_pImpl->generator.getWhenUseLeftAsFixed(m_pImpl->output.contactStatus.UsedLeftAsFixed); + + // get the footsteps + m_pImpl->output.steps.leftSteps = m_pImpl->generator.getLeftFootPrint()->getSteps(); + m_pImpl->output.steps.rightSteps = m_pImpl->generator.getRightFootPrint()->getSteps(); + + // get the DCM trajectory + auto convertToEigen + = [](const std::vector& inputVect) -> std::vector { + std::vector outputVect; + outputVect.reserve(inputVect.size()); + + for (const auto& v : inputVect) + { + outputVect.push_back(iDynTree::toEigen(v)); + }; + + return outputVect; + }; + + m_pImpl->output.dcmTrajectory.position = convertToEigen(dcmGenerator->getDCMPosition()); + m_pImpl->output.dcmTrajectory.velocity = convertToEigen(dcmGenerator->getDCMVelocity()); + + // get the CoM planar trajectory + std::chrono::nanoseconds time = m_pImpl->input.initTime; + Eigen::Vector4d state; + state.head<2>() = m_pImpl->input.comInitialState.initialPlanarPosition; + state.tail<2>() = m_pImpl->input.comInitialState.initialPlanarVelocity; + m_pImpl->comSystem.dynamics->setState({state.head<4>()}); + using namespace BipedalLocomotion::GenericContainer::literals; + auto stateDerivative = BipedalLocomotion::GenericContainer::make_named_tuple( + BipedalLocomotion::GenericContainer::named_param<"dx"_h, Eigen::VectorXd>()); + Eigen::Vector4d controlInput; + + m_pImpl->output.comTrajectory.position.resize(m_pImpl->output.dcmTrajectory.position.size()); + m_pImpl->output.comTrajectory.velocity.resize(m_pImpl->output.dcmTrajectory.position.size()); + m_pImpl->output.comTrajectory.acceleration.resize( + m_pImpl->output.dcmTrajectory.position.size()); + + for (size_t i = 0; i < m_pImpl->output.dcmTrajectory.position.size(); i++) + { + // populate CoM planar position + m_pImpl->output.comTrajectory.position[i].head<2>() = state.head<2>(); + + // set the control input, u + controlInput << m_pImpl->output.dcmTrajectory.position.at(i), + m_pImpl->output.dcmTrajectory.velocity.at(i); + m_pImpl->comSystem.dynamics->setControlInput({controlInput}); + + // compute the state derivative xdot = Ax + Bu + m_pImpl->comSystem.dynamics->dynamics(time, stateDerivative); + + // populate CoM planar velocity and acceleration + m_pImpl->output.comTrajectory.acceleration[i].head<2>() + = stateDerivative.get_from_hash<"dx"_h>().tail<2>(); + m_pImpl->output.comTrajectory.velocity[i].head<2>() + = stateDerivative.get_from_hash<"dx"_h>().head<2>(); + + // advance the integrator for one step + m_pImpl->comSystem.integrator->oneStepIntegration(time, m_pImpl->parameters.dt); + state.head<4>() = std::get<0>(m_pImpl->comSystem.integrator->getSolution()); + + // update the system state + m_pImpl->comSystem.dynamics->setState({state}); + time += m_pImpl->parameters.dt; + } + + // get the CoM height trajectory + auto comHeightGenerator = m_pImpl->generator.addCoMHeightTrajectoryGenerator(); + comHeightGenerator->getCoMHeightTrajectory(m_pImpl->comHeightTrajectory.position); + comHeightGenerator->getCoMHeightVelocity(m_pImpl->comHeightTrajectory.velocity); + comHeightGenerator->getCoMHeightAccelerationProfile(m_pImpl->comHeightTrajectory.acceleration); + + // stack the CoM planar and the height trajectory + for (size_t i = 0; i < m_pImpl->output.comTrajectory.position.size(); i++) + { + m_pImpl->output.comTrajectory.position[i].z() = m_pImpl->comHeightTrajectory.position[i]; + m_pImpl->output.comTrajectory.velocity[i].z() = m_pImpl->comHeightTrajectory.velocity[i]; + m_pImpl->output.comTrajectory.acceleration[i].z() + = m_pImpl->comHeightTrajectory.acceleration[i]; + } + + // get the merge points + m_pImpl->generator.getMergePoints(m_pImpl->output.mergePoints); + + m_pImpl->state = Impl::FSM::Running; + + return true; +} + +bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajectory() +{ + + constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::generateFirstTrajectory]"; + + // clear the all trajectory + auto unicyclePlanner = m_pImpl->generator.unicyclePlanner(); + unicyclePlanner->clearPersonFollowingDesiredTrajectory(); + unicyclePlanner->setDesiredDirectControl(0.0, 0.0, 0.0); + + // clear left and right footsteps + m_pImpl->generator.getLeftFootPrint()->clearSteps(); + m_pImpl->generator.getRightFootPrint()->clearSteps(); + + // set initial and final times + double initTime = 0; + double endTime = initTime + m_pImpl->parameters.plannerHorizon.count() * 1e-9; + double dt = m_pImpl->parameters.dt.count() * 1e-9; + + // at the beginning ergoCub has to stop + Eigen::Vector2d m_personFollowingDesiredPoint; + m_personFollowingDesiredPoint(0) = m_pImpl->parameters.referencePointDistance(0); + m_personFollowingDesiredPoint(1) = m_pImpl->parameters.referencePointDistance(1); + + // add the initial point + if (!unicyclePlanner + ->addPersonFollowingDesiredTrajectoryPoint(initTime, + iDynTree::Vector2( + m_personFollowingDesiredPoint))) + { + log()->error("{} Error while setting the initial point.", logPrefix); + return false; + } + + // add the final point + if (!unicyclePlanner + ->addPersonFollowingDesiredTrajectoryPoint(endTime, + iDynTree::Vector2( + m_personFollowingDesiredPoint))) + { + log()->error("{} Error while setting the final point.", logPrefix); + return false; + } + + // generate the first trajectories + if (!m_pImpl->generator.generate(initTime, dt, endTime)) + { + + log()->error("{} Error while computing the first trajectories.", logPrefix); + + return false; + } + + return true; +} + +Contacts::ContactPhaseList +BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::getContactPhaseList() +{ + constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::getContactPhaseList]"; + + Contacts::ContactPhaseList contactPhaseList; + + if (isOutputValid() == false) + { + log()->error("{} The output is not valid. Returning an empty Contact Phase List.", + logPrefix); + return contactPhaseList; + } + + // get the contact phase lists + BipedalLocomotion::Contacts::ContactListMap ContactListMap; + std::vector leftStepPhases, rightStepPhases; + m_pImpl->generator.getStepPhases(leftStepPhases, rightStepPhases); + + BipedalLocomotion::Contacts::ContactList leftContactList, rightContactList; + + if (!Planners::UnicycleUtilities::getContactList(m_pImpl->initTime, + m_pImpl->parameters.dt, + m_pImpl->output.contactStatus.leftFootInContact, + m_pImpl->output.steps.leftSteps, + m_pImpl->parameters.leftContactFrameIndex, + "left_foot", + leftContactList)) + { + log()->error("{} Error while getting the left contact list. Returning an empty Contact " + "Phase List.", + logPrefix); + return contactPhaseList; + }; + + if (!Planners::UnicycleUtilities::getContactList(m_pImpl->initTime, + m_pImpl->parameters.dt, + m_pImpl->output.contactStatus + .rightFootInContact, + m_pImpl->output.steps.rightSteps, + m_pImpl->parameters.rightContactFrameIndex, + "right_foot", + rightContactList)) + { + log()->error("{} Error while getting the right contact list. Returning an empty Contact " + "Phase List.", + logPrefix); + return contactPhaseList; + }; + + ContactListMap["left_foot"] = leftContactList; + ContactListMap["right_foot"] = rightContactList; + contactPhaseList.setLists(ContactListMap); + + return contactPhaseList; +}; \ No newline at end of file diff --git a/src/Planners/src/UnicycleUtilities.cpp b/src/Planners/src/UnicycleUtilities.cpp new file mode 100644 index 0000000000..33cb14d68e --- /dev/null +++ b/src/Planners/src/UnicycleUtilities.cpp @@ -0,0 +1,102 @@ +/** + * @file UnicycleUtilities.cpp + * @authors Lorenzo Moretti, Giulio Romualdi, Stefano Dafarra + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ +#include +#include +#include + +#include +#include + +namespace BipedalLocomotion::Planners::UnicycleUtilities +{ + +bool getContactList(const std::chrono::nanoseconds& initTime, + const std::chrono::nanoseconds& dt, + const std::vector& inContact, + const std::deque& steps, + const int& contactFrameIndex, + const std::string& contactName, + BipedalLocomotion::Contacts::ContactList& contactList) +{ + + constexpr auto logPrefix = "[UnicycleUtilities::getContactList]"; + + if (contactList.size() > 1) + { + BipedalLocomotion::log()->error("{} The contact list has size greater than 1. Size should " + "be 0 or 1.", + logPrefix); + return false; + } + + size_t impactTimeIndex{0}; + auto stepIterator = steps.begin(); + + while (stepIterator != steps.end()) + { + auto step = *stepIterator; + + BipedalLocomotion::Contacts::PlannedContact contact{}; + + contact.index = contactFrameIndex; + contact.name = contactName; + + Eigen::Vector3d translation = Eigen::Vector3d::Zero(); + translation.head<2>() = iDynTree::toEigen(step.position); + manif::SO3d rotation{0, 0, step.angle}; + contact.pose = manif::SE3d(translation, rotation); + + std::chrono::nanoseconds impactTime{static_cast(step.impactTime * 1e9)}; + + contact.activationTime = impactTime; + contact.deactivationTime = std::chrono::nanoseconds::max(); + + impactTimeIndex = (impactTime <= initTime) ? 0 + : static_cast((impactTime - initTime) / dt); + + for (auto i = impactTimeIndex; i < inContact.size(); i++) + { + if (i > 0 && !inContact.at(i) && inContact.at(i - 1)) + { + contact.deactivationTime = initTime + dt * i; + break; + } + } + + if ((stepIterator == steps.begin()) && (contactList.size() == 1) && (impactTimeIndex == 0)) + { + // editing the first step if the contact list is not empty + // since the first contact, being the current active one, + // is already in the contact list + + if (!contactList.editContact(contactList.begin(), contact)) + { + BipedalLocomotion::log()->error("{} Error while editing the first contact of the " + "contact list.", + logPrefix); + + return false; + } + } else + { + if (!contactList.addContact(contact)) + { + BipedalLocomotion::log()->error("{} Error while adding contact to the contact " + "list.", + logPrefix); + + return false; + } + } + + stepIterator++; + }; + + return true; +}; + +} // namespace BipedalLocomotion::Planners::UnicycleUtilities diff --git a/src/Planners/tests/CMakeLists.txt b/src/Planners/tests/CMakeLists.txt index a22d7ebe28..338fd69eed 100644 --- a/src/Planners/tests/CMakeLists.txt +++ b/src/Planners/tests/CMakeLists.txt @@ -22,12 +22,31 @@ add_bipedal_test( SOURCES SwingFootPlannerTest.cpp LINKS BipedalLocomotion::Planners) -if (FRAMEWORK_COMPILE_Unicycle) + if (FRAMEWORK_COMPILE_Unicycle) + + include_directories(${CMAKE_CURRENT_BINARY_DIR}) + configure_file("${CMAKE_CURRENT_SOURCE_DIR}/FolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/FolderPath.h" @ONLY) + + # Get the urdf model of the robot + set(ERGOCUB_MODEL_EXPECTED_MD5 7d24f42cb415e660abc4bbc8a52d355f) + if (EXISTS "${CMAKE_CURRENT_BINARY_DIR}/model.urdf") + file(MD5 "${CMAKE_CURRENT_BINARY_DIR}/model.urdf" ERGOCUB_MODEL_CHECKSUM_VARIABLE) + string(COMPARE EQUAL ${ERGOCUB_MODEL_CHECKSUM_VARIABLE} ${ERGOCUB_MODEL_EXPECTED_MD5} ERGOCUB_MODEL_UPDATED) + else() + set(ERGOCUB_MODEL_UPDATED FALSE) + endif() + + if(NOT ${ERGOCUB_MODEL_UPDATED}) + message(STATUS "Fetching ergoCubSN000 model from icub-tech-iit/ergocub-software...") + file(DOWNLOAD https://raw.githubusercontent.com/icub-tech-iit/ergocub-software/f28733afcbbfcc99cbac13be530a6a072f832746/urdf/ergoCub/robots/ergoCubSN000/model.urdf + ${CMAKE_CURRENT_BINARY_DIR}/model.urdf + EXPECTED_MD5 ${ERGOCUB_MODEL_EXPECTED_MD5}) + endif() add_bipedal_test( - NAME UnicyclePlanner - SOURCES UnicyclePlannerTest.cpp - LINKS BipedalLocomotion::Planners BipedalLocomotion::Unicycle + NAME UnicycleTrajectoryPlanner + SOURCES UnicycleTrajectoryPlannerTest.cpp + LINKS BipedalLocomotion::Unicycle ) endif() diff --git a/src/Planners/tests/FolderPath.h.in b/src/Planners/tests/FolderPath.h.in new file mode 100644 index 0000000000..ea845e0bc0 --- /dev/null +++ b/src/Planners/tests/FolderPath.h.in @@ -0,0 +1,20 @@ +/** + * @file FolderPath.h(.in) + * @authors Lorenzo Moretti + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef CONFIG_FOLDERPATH_H_IN +#define CONFIG_FOLDERPATH_H_IN + +#define BINARY_DIR "@CMAKE_CURRENT_BINARY_DIR@" + +#include + +inline std::string getRobotModelPath() +{ + return std::string(BINARY_DIR) + "/model.urdf"; +} + +#endif // CONFIG_FOLDERPATH_H_IN diff --git a/src/Planners/tests/UnicyclePlannerTest.cpp b/src/Planners/tests/UnicyclePlannerTest.cpp deleted file mode 100644 index cb12997ae3..0000000000 --- a/src/Planners/tests/UnicyclePlannerTest.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/** - * @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 = 1e-2) -{ - 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 = false; - - 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"); - } - -} diff --git a/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp b/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp new file mode 100644 index 0000000000..e86e77bf45 --- /dev/null +++ b/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp @@ -0,0 +1,102 @@ +/** + * @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 + +#include +#include + +#include +#include +#include + +#include + +#include + +using namespace BipedalLocomotion::Planners; +using namespace BipedalLocomotion::ParametersHandler; + +void saveData(const BipedalLocomotion::Contacts::ContactPhaseList& contactPhaseList, + const std::string& filename) +{ + std::ofstream file(filename); + if (!file.is_open()) + { + BipedalLocomotion::log()->error("[saveData] Unable to open file {} for writing.", filename); + return; + } + + file << contactPhaseList.toString() << std::endl; + + file.close(); +} + +std::shared_ptr params() +{ + // Set the non-default parameters of the planner + std::shared_ptr handler = std::make_shared(); + + handler->setParameter("plannerHorizon", 20.0); + handler->setParameter("referencePosition", Eigen::Vector2d(0.1, 0.0)); + handler->setParameter("saturationFactors", Eigen::Vector2d(0.7, 0.7)); + handler->setParameter("leftZMPDelta", Eigen::Vector2d(0.0, 0.0)); + handler->setParameter("rightZMPDelta", Eigen::Vector2d(0.0, 0.0)); + handler->setParameter("mergePointRatios", Eigen::Vector2d(0.4, 0.4)); + handler->setParameter("leftContactFrameName", "l_sole"); + handler->setParameter("rightContactFrameName", "r_sole"); + + return handler; +} + +TEST_CASE("UnicyclePlannerTest") +{ + + auto jointsList + = std::vector{"l_hip_pitch", "l_hip_roll", "l_hip_yaw", + "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", + "r_knee", "r_ankle_pitch", "r_ankle_roll", + "torso_pitch", "torso_roll", "torso_yaw", + "neck_pitch", "neck_roll", "neck_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", + "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", + "r_shoulder_yaw", "r_elbow"}; + + iDynTree::ModelLoader ml; + REQUIRE(ml.loadReducedModelFromFile(getRobotModelPath(), jointsList)); + + const auto handler = params(); + + bool saveDataTofile = false; + + BipedalLocomotion::Planners::UnicycleTrajectoryPlanner planner; + + REQUIRE(planner.initialize(handler)); + + REQUIRE(planner.setRobotContactFrames(ml.model())); + + UnicycleTrajectoryPlannerInput input + = UnicycleTrajectoryPlannerInput::generateDummyUnicycleTrajectoryPlannerInput(); + + UnicycleTrajectoryPlannerOutput output; + + REQUIRE(planner.setInput(input)); + REQUIRE(planner.advance()); + REQUIRE(planner.isOutputValid()); + + output = planner.getOutput(); + + auto contactPhaseList = planner.getContactPhaseList(); + + REQUIRE(contactPhaseList.size() == 1); + + if (saveDataTofile) + { + saveData(contactPhaseList, "UnicycleTrajectoryPlannerTestOutput.txt"); + } +}