From c83ffc29610921d6f59fbeb8244bc74d1b315d37 Mon Sep 17 00:00:00 2001 From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com> Date: Fri, 19 Jul 2024 10:29:03 +0200 Subject: [PATCH] Add Unicycle Trajectory Generator in Planner component (#845) --- CHANGELOG.md | 1 + CMakeLists.txt | 2 + examples/CMakeLists.txt | 1 + .../CMakeLists.txt | 31 + .../FolderPath.h.in | 20 + examples/UnicycleTrajectoryGenerator/main.cpp | 306 +++++++ src/Planners/CMakeLists.txt | 4 +- .../Planners/UnicycleTrajectoryGenerator.h | 161 ++++ .../Planners/UnicycleTrajectoryPlanner.h | 33 + .../Planners/UnicycleUtilities.h | 89 ++ .../src/UnicycleTrajectoryGenerator.cpp | 842 ++++++++++++++++++ .../src/UnicycleTrajectoryPlanner.cpp | 93 +- src/Planners/src/UnicycleUtilities.cpp | 75 ++ src/Planners/tests/CMakeLists.txt | 7 + .../tests/UnicycleTrajectoryGeneratorTest.cpp | 75 ++ .../tests/UnicycleTrajectoryPlannerTest.cpp | 33 +- 16 files changed, 1736 insertions(+), 37 deletions(-) create mode 100644 examples/CMakeLists.txt create mode 100644 examples/UnicycleTrajectoryGenerator/CMakeLists.txt create mode 100644 examples/UnicycleTrajectoryGenerator/FolderPath.h.in create mode 100644 examples/UnicycleTrajectoryGenerator/main.cpp create mode 100644 src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h create mode 100644 src/Planners/src/UnicycleTrajectoryGenerator.cpp create mode 100644 src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index bc5150371e..ce1117548c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ All notable changes to this project are documented in this file. - Add the possibility to set the number of threads used by onnxruntime in `MANN` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/836) - Implement `ButterworthLowPassFilter` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/838) - Implement `Conversions::toiDynTreeRot` function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/842) +- Add the `Planners::UnicycleTrajectoryGenerator` to mimic the functionalities of the unicycle planner deployed in [walking-controllers](https://github.com/robotology/walking-controllers) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/845) - Create python bindings of `VectorsCollection` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/854) - Added a simple motor control example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/855) - Add `setControlModeAsync` function to set motor control mode in an asynchronous process (https://github.com/ami-iit/bipedal-locomotion-framework/pull/860) diff --git a/CMakeLists.txt b/CMakeLists.txt index 260436e910..1b23916cbd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,3 +138,5 @@ endif() add_subdirectory(bindings) add_subdirectory(utilities) + +add_subdirectory(examples) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000000..a45244e810 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(UnicycleTrajectoryGenerator) diff --git a/examples/UnicycleTrajectoryGenerator/CMakeLists.txt b/examples/UnicycleTrajectoryGenerator/CMakeLists.txt new file mode 100644 index 0000000000..ecd636049b --- /dev/null +++ b/examples/UnicycleTrajectoryGenerator/CMakeLists.txt @@ -0,0 +1,31 @@ +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_executable(UnicycleTrajectoryGeneratorExample main.cpp) + + target_link_libraries(UnicycleTrajectoryGeneratorExample + BipedalLocomotion::Unicycle + BipedalLocomotion::Planners + matioCpp::matioCpp + ${YARP_LIBRARIES} +) +endif() + diff --git a/examples/UnicycleTrajectoryGenerator/FolderPath.h.in b/examples/UnicycleTrajectoryGenerator/FolderPath.h.in new file mode 100644 index 0000000000..ea845e0bc0 --- /dev/null +++ b/examples/UnicycleTrajectoryGenerator/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/examples/UnicycleTrajectoryGenerator/main.cpp b/examples/UnicycleTrajectoryGenerator/main.cpp new file mode 100644 index 0000000000..8740c20b96 --- /dev/null +++ b/examples/UnicycleTrajectoryGenerator/main.cpp @@ -0,0 +1,306 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +using namespace BipedalLocomotion; + +// define some parameters of the unicycle trajectory generator +std::shared_ptr getUnicycleParametersHandler(double dt) +{ + std::shared_ptr handler + = std::make_shared(); + + handler->setParameter("dt", dt); + handler->setParameter("planner_advance_time_in_s", 0.18); + 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; +} + +// define some parameters of the swing foot planner +std::shared_ptr +getSwingFootPlannerParametersHandler(double dt) +{ + std::shared_ptr handler + = std::make_shared(); + + auto dtChrono = std::chrono::milliseconds(static_cast(dt * 1000)); + + handler->setParameter("sampling_time", dtChrono); + handler->setParameter("step_height", 0.035); + handler->setParameter("foot_apex_time", 0.5); + handler->setParameter("foot_landing_velocity", 0.0); + handler->setParameter("foot_landing_acceleration", 0.0); + + return handler; +} + +int main(int argc, char* argv[]) +{ + constexpr auto errorPrefix = "[main]"; + + bool saveResults = false; + if (argc > 1) + { + if (std::string(argv[1]) == "--save") + { + // results are saved in a mat file located in the directory + // where the executable is run + saveResults = true; + } + } + + // matio file + matioCpp::File resultFile; + if (saveResults) + { + resultFile = matioCpp::File::Create("UnicycleTrajectoryGeneratorResults.mat"); + } + std::vector positionLeftFoot; + std::vector positionRightFoot; + std::vector positionCOM; + std::vector velocityCOM; + std::vector DCMposition; + std::vector DCMvelocity; + std::vector time; + size_t i = 0; + + // set the Bipedal Locomotion clock factory + BipedalLocomotion::System::ClockBuilder::setFactory( + std::make_shared()); + + // load robot model + 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; + ml.loadReducedModelFromFile(getRobotModelPath(), jointsList); + + // sample time [s] of the unicycle trajectory generator + auto dt = 0.002; + std::chrono::milliseconds dtChrono = std::chrono::milliseconds(static_cast(dt * 1000)); + + // initialize the unicyle trajectory generator + Planners::UnicycleTrajectoryGenerator unicycleTrajectoryGenerator; + unicycleTrajectoryGenerator.initialize(getUnicycleParametersHandler(dt)); + unicycleTrajectoryGenerator.setRobotContactFrames(ml.model()); + + // initialize the input of the unicycle trajectory generator + Planners::UnicycleTrajectoryGeneratorInput input + = Planners::UnicycleTrajectoryGeneratorInput::generateDummyUnicycleTrajectoryGeneratorInput(); + + // initialize the output of the unicycle trajectory generator + Planners::UnicycleTrajectoryGeneratorOutput output; + + // initialize the swing foot planner + Planners::SwingFootPlanner leftFootPlanner; + leftFootPlanner.initialize(getSwingFootPlannerParametersHandler(dt)); + leftFootPlanner.setTime(std::chrono::nanoseconds::zero()); + Planners::SwingFootPlanner rightFootPlanner; + rightFootPlanner.initialize(getSwingFootPlannerParametersHandler(dt)); + rightFootPlanner.setTime(std::chrono::nanoseconds::zero()); + + // initialize loop variables + bool isFirstIteration = true; + manif::SE3d w_H_left; + manif::SE3d w_H_right; + + // initial time of the simulation + auto timeStart = BipedalLocomotion::clock().now(); + + // final time of the simulation + auto timeEnd = timeStart + std::chrono::seconds(20); + + while (BipedalLocomotion::clock().now() < timeEnd) + { + auto currentTime = BipedalLocomotion::clock().now(); + log()->info("[main] Current time: {}", + std::chrono::duration_cast(currentTime - timeStart)); + + // set linear velocity in x-direction + if ((currentTime - timeStart) < (timeEnd - timeStart) / 6) + { + input.plannerInput[0] = 0.0; + } else if ((currentTime - timeStart) > 4 * (timeEnd - timeStart) / 6) + { + input.plannerInput[0] = 0.0; + } else + { + input.plannerInput[0] = 0.1; + } + + if (!isFirstIteration) + { + // print the feet pose + log()->info("[main] Left foot position: {}", w_H_left.translation().transpose()); + log()->info("[main] Right foot position: {}", w_H_right.translation().transpose()); + } + + unicycleTrajectoryGenerator.setInput(input); + + if (!unicycleTrajectoryGenerator.advance()) + { + log()->error("[main] Unable to advance the unicycle trajectory generator."); + return EXIT_FAILURE; + } + + output = unicycleTrajectoryGenerator.getOutput(); + + if (!unicycleTrajectoryGenerator.isOutputValid()) + { + log()->error("[main] The output of the unicycle trajectory generator is not valid."); + return EXIT_FAILURE; + } + + // set the contact list of the swing foot planners + if (!leftFootPlanner.setContactList(output.contactPhaseList.lists().at("left_foot"))) + { + log()->error("[main] Unable to set the contact list of the left foot planner."); + return EXIT_FAILURE; + } + if (!rightFootPlanner.setContactList(output.contactPhaseList.lists().at("right_foot"))) + { + log()->error("[main] Unable to set the contact list of the right foot planner."); + return EXIT_FAILURE; + } + + // advance the swing foot planners + if (!leftFootPlanner.advance()) + { + log()->error("[main] Unable to advance the left foot planner."); + return EXIT_FAILURE; + } + if (!rightFootPlanner.advance()) + { + log()->error("[main] Unable to advance the right foot planner."); + return EXIT_FAILURE; + } + + // get the feet pose from the swing foot planners + w_H_left = leftFootPlanner.getOutput().transform; + w_H_right = rightFootPlanner.getOutput().transform; + + if (saveResults) + { + positionLeftFoot.push_back(w_H_left.translation()); + positionRightFoot.push_back(w_H_right.translation()); + positionCOM.push_back(output.comTrajectory.position.front()); + velocityCOM.push_back(output.comTrajectory.velocity.front()); + DCMposition.push_back(output.dcmTrajectory.position.front()); + DCMvelocity.push_back(output.dcmTrajectory.velocity.front()); + + i++; + time.push_back( + std::chrono::duration_cast(currentTime - timeStart) + .count()); + } + // get the DCM trajectory from the unicycle trajectory generator + auto dcmPosition = output.dcmTrajectory.position; + auto dcmVelocity = output.dcmTrajectory.velocity; + + Eigen::VectorXd Xdcm; + Xdcm.resize(dcmPosition.size()); + + Eigen::VectorXd Ydcm; + Ydcm.resize(dcmPosition.size()); + + for (size_t i = 0; i < dcmPosition.size(); i++) + { + Xdcm(i) = dcmPosition[i][0]; + } + + for (size_t i = 0; i < dcmPosition.size(); i++) + { + Ydcm(i) = dcmPosition[i][1]; + } + + // log()->info("[main] DCM x: {}", Xdcm.transpose()); + // log()->info("[main] DCM y: {}", Ydcm.transpose()); + + BipedalLocomotion::clock().sleepUntil(currentTime + dtChrono); + + isFirstIteration = false; + } + + // save the results to a mat file + if (saveResults) + { + auto toMatioTimeVector = matioCpp::make_variable("time", time); + resultFile.write(toMatioTimeVector); + + Eigen::Matrix3Xd footPositionMatrix(3, positionLeftFoot.size()); + for (size_t i = 0; i < positionLeftFoot.size(); i++) + { + footPositionMatrix.col(i) = positionLeftFoot[i]; + } + auto toMatioEigenVec = matioCpp::make_variable("LeftFootPosition", footPositionMatrix); + resultFile.write(toMatioEigenVec); + + for (size_t i = 0; i < positionRightFoot.size(); i++) + { + footPositionMatrix.col(i) = positionRightFoot[i]; + } + toMatioEigenVec = matioCpp::make_variable("RightFootPosition", footPositionMatrix); + resultFile.write(toMatioEigenVec); + + Eigen::Matrix3Xd comPositionMatrix(3, positionCOM.size()); + for (size_t i = 0; i < positionCOM.size(); i++) + { + comPositionMatrix.col(i) = positionCOM[i]; + } + + toMatioEigenVec = matioCpp::make_variable("COMPosition", comPositionMatrix); + resultFile.write(toMatioEigenVec); + + Eigen::Matrix3Xd comVelocityMatrix(3, velocityCOM.size()); + for (size_t i = 0; i < velocityCOM.size(); i++) + { + comVelocityMatrix.col(i) = velocityCOM[i]; + } + toMatioEigenVec = matioCpp::make_variable("COMVelocity", comVelocityMatrix); + resultFile.write(toMatioEigenVec); + + Eigen::Matrix2Xd dcmPositionMatrix(2, DCMposition.size()); + for (size_t i = 0; i < DCMposition.size(); i++) + { + dcmPositionMatrix.col(i) = DCMposition[i]; + } + toMatioEigenVec = matioCpp::make_variable("DCMPosition", dcmPositionMatrix); + resultFile.write(toMatioEigenVec); + + Eigen::Matrix2Xd dcmVelocityMatrix(2, DCMvelocity.size()); + for (size_t i = 0; i < DCMvelocity.size(); i++) + { + dcmVelocityMatrix.col(i) = DCMvelocity[i]; + } + toMatioEigenVec = matioCpp::make_variable("DCMVelocity", dcmVelocityMatrix); + resultFile.write(toMatioEigenVec); + } + + return EXIT_SUCCESS; +} diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 1784a4765f..ae05a602ce 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -26,8 +26,8 @@ if (FRAMEWORK_COMPILE_Unicycle) add_bipedal_locomotion_library( NAME Unicycle - PUBLIC_HEADERS ${H_PREFIX}/UnicycleTrajectoryPlanner.h ${H_PREFIX}/UnicycleUtilities.h - SOURCES src/UnicycleTrajectoryPlanner.cpp src/UnicycleUtilities.cpp + PUBLIC_HEADERS ${H_PREFIX}/UnicycleTrajectoryPlanner.h ${H_PREFIX}/UnicycleTrajectoryGenerator.h ${H_PREFIX}/UnicycleUtilities.h + SOURCES src/UnicycleTrajectoryPlanner.cpp src/UnicycleTrajectoryGenerator.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) diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h new file mode 100644 index 0000000000..55f012cab5 --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h @@ -0,0 +1,161 @@ +/** + * @file UnicycleTrajectoryGenerator.h + * @authors Lorenzo Moretti, Stefano Dafarra, Giulio Romualdi + * @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_TRAJECTORY_GENERATOR_H +#define BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_TRAJECTORY_GENERATOR_H + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +namespace BipedalLocomotion::Planners +{ +class UnicycleTrajectoryGenerator; +struct UnicycleTrajectoryGeneratorInput; +struct UnicycleTrajectoryGeneratorOutput; +struct UnicycleTrajectoryGeneratorParameters; +} // namespace BipedalLocomotion::Planners + +struct BipedalLocomotion::Planners::UnicycleTrajectoryGeneratorInput +{ + /** + * 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 */ + + static UnicycleTrajectoryGeneratorInput generateDummyUnicycleTrajectoryGeneratorInput(); +}; + +struct BipedalLocomotion::Planners::UnicycleTrajectoryGeneratorOutput + : public BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput +{ + bool isValid = true; /**< True if the output is valid, false otherwise. */ + BipedalLocomotion::Contacts::ContactPhaseList contactPhaseList; /**< Contact phase list */ +}; + +struct BipedalLocomotion::Planners::UnicycleTrajectoryGeneratorParameters +{ + std::chrono::nanoseconds dt; /**< Sampling time of the planner */ + size_t plannerAdvanceTimeSteps; /**< Number of time steps that the planner should be called in + advance */ + int leftContactFrameIndex; /**< Index of the left contact frame */ + int rightContactFrameIndex; /**< Index of the right contact frame */ +}; + +/** + * UnicycleTrajectoryGenerator is a class that generates reference trajectories for + * humanoid robots. + * Every time that the advance() member function is called, the Generator: + * 1. First checks if it is time to generate a new trajectory. In this case, it deploys the + BipedalLocomotion::Planners::UnicycleTrajectoryPlanner to plan this new trajectory. + * 2. Then, it checks if it is time to merge the current trajectory with the last one + computed by the UnicycleTrajectoryPlanner. In this case, it merges the two trajectories, + which become the current one. + * 3. Finally, it unrolls the current trajectory (i.e., it advances it over time). + * The getOutput() member function returns the current trajectory, which includes the CoM, DCM, + * and footstep ones. + * The Generator requires the user to set the robot model using the setRobotModel() member function, + * before invoking the initialize() member function, which configures the Generator. + * As input, which is set by the setInput() member function, the Generator requires an instance of + * the UnicycleTrajectoryGeneratorInput struct. + */ +class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final + : public System::Advanceable +{ +public: + UnicycleTrajectoryGenerator(); + + virtual ~UnicycleTrajectoryGenerator(); + + /** + * 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 | + * | :-------------------------: | :------------: | :---------------: | :-------------: | :-------------------------------------------------------: | + * | `planner_advance_time_in_s` | double | 0.08 | - | The time in advance at which the planner is called | + * | `dt` | double | 0.002 | - | The sampling time of the trajectory generator | + * | `leftContactFrameName` | string | - | "l_sole" | Name of the left foot contact frame | + * | `rightContactFrameName` | string | - | "r_sole" | Name of the right foot contact frame | + * + * Implicitely, the class needs also all the parameters required by the Bipedalocotion::Planner::UnicyclePlanner class. + // clang-format on + + * @param handler Pointer to the parameter handler. + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + + /** + * @brief Get the output of the planner. + * @return Output of the planner. + */ + const UnicycleTrajectoryGeneratorOutput& getOutput() const override; + + /** + * @brief Check if the output is valid. + * @return True if the output is valid, false otherwise. + */ + bool isOutputValid() const override; + + /** + * @brief Set the input of the planner. + * @param input Input of the planner. + * @return True in case of success, false otherwise. + */ + bool setInput(const UnicycleTrajectoryGeneratorInput& input) override; + + /** + * @brief Advance the planner. + * @return True in case of success, false otherwise. + */ + bool advance() override; + + /** + * @brief Get the sampling time of the planner. + * @return Sampling time of the planner. + */ + std::chrono::nanoseconds getSamplingTime() const; + +private: + class Impl; + std::unique_ptr m_pImpl; + + /** + * @brief Generate the first trajectory at initialization phase. + * @return True in case of success, false otherwise. + */ + bool generateFirstTrajectory(); +}; + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_TRAJECTORY_GENERATOR_H diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h index d85c516606..2d2f64d7fe 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -95,6 +95,15 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput std::deque leftSteps, rightSteps; }; + struct FootTrajectory + { + std::vector transform; /**< Foot transform */ + std::vector mixedVelocity; /**< Spatial velocity in mixed + representation */ + std::vector mixedAcceleration; /**< Spatial acceleration in mixed + representation */ + }; + COMTrajectory comTrajectory; /**< CoM trajectory */ DCMTrajectory dcmTrajectory; /**< DCM trajectory */ @@ -103,6 +112,10 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput Steps steps; /**< List of steps and their phases */ + FootTrajectory leftFootTrajectory; /**< Left foot trajectory */ + + FootTrajectory rightFootTrajectory; /**< Right foot trajectory */ + std::vector mergePoints; /**< Indexes of the merge points of the trajectory */ }; @@ -112,6 +125,8 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerParameters std::chrono::nanoseconds plannerHorizon; /**< Time horizon of the planner */ + std::chrono::nanoseconds minStepDuration; /**< Time minimum time duration of a step */ + double leftYawDeltaInRad; /**< Left foot cartesian offset in the yaw */ double rightYawDeltaInRad; /**< Right foot cartesian offset in the yaw */ @@ -159,6 +174,18 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final */ bool setRobotContactFrames(const iDynTree::Model& model); + /** + * Get the index of the right foot contact frame. + * @return The index of the right foot contact frame. + */ + int getRightContactFrameIndex() const; + + /** + * Get the index of the left foot contact frame. + * @return The index of the left foot contact frame. + */ + int getLeftContactFrameIndex() const; + // clang-format off /** * Initialize the planner. @@ -243,6 +270,12 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final */ Contacts::ContactPhaseList getContactPhaseList(); + /** + * Get the minimum time duration of a step. + * @return minimum time duration of a step. + */ + std::chrono::nanoseconds getMinStepDuration() const; + private: class Impl; std::unique_ptr m_pImpl; diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h index 90bd36f675..6e33e81831 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h @@ -9,7 +9,11 @@ #define BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_UTILITIES_H #include +#include #include +#include +#include +#include namespace BipedalLocomotion::Planners::UnicycleUtilities { @@ -32,6 +36,91 @@ bool getContactList(const std::chrono::nanoseconds& initTime, const std::string& contactName, BipedalLocomotion::Contacts::ContactList& contactList); +/** + * Merge two steps sequences, and stores the merged output in the second one. + * The new steps which has an impact time lower than the current time are discarded. + * @param newSteps the new list of steps. + * @param currentSteps the cuurent list of steps. + * @param currentTime the current time. + */ +void mergeSteps(const std::deque& newSteps, + std::deque& currentSteps, + const std::chrono::nanoseconds& currentTime); + +/** + * It appends a vector to a deque. + * @param input the input vector. + * @param output the output deque. + * @param initPoint the initial point where the vector has to be appended. + * @return true if the operation is successful, false otherwise. + */ +template +bool appendVectorToDeque(const std::vector& input, + std::deque& output, + const size_t& initPoint) +{ + if (initPoint > output.size()) + { + BipedalLocomotion::log()->error("[Utilities::appendVectorToDeque] The init point has to " + "be less or equal to the size of the output deque."); + return false; + } + + // resize the deque + output.resize(input.size() + initPoint); + + // Advances the iterator it by initPoint positions + typename std::deque::iterator it = output.begin(); + std::advance(it, initPoint); + + // copy the vector into the deque from the initPoint position + std::copy(input.begin(), input.end(), it); + + return true; +} + +/** + * It populates a vector from a deque. + * @param deque the input deque. + * @param vector the output vector. + */ +template +void populateVectorFromDeque(const std::deque& deque, std::vector& vector) +{ + + vector.clear(); + + vector.insert(vector.end(), deque.begin(), deque.end()); +} + +namespace Conversions +{ + +void convertToBLF(const iDynTree::Transform& input, manif::SE3d& output); + +void convertToBLF(const iDynTree::Twist& input, manif::SE3d::Tangent& output); + +void convertToBLF(const iDynTree::Twist& input, manif::SE3d::Tangent& output); + +void convertToBLF(const iDynTree::VectorDynSize& input, Eigen::VectorXd& output); + +void convertToBLF(const iDynTree::Vector2& input, Eigen::Vector2d& output); + +void convertToBLF(const iDynTree::Vector3& input, Eigen::Vector3d& output); + +template +void convertVector(const std::vector& inputVect, std::vector& outputVect) +{ + + outputVect.resize(inputVect.size()); + + for (size_t i = 0; i < inputVect.size(); ++i) + { + convertToBLF(inputVect[i], outputVect[i]); + } +} + +} // namespace Conversions } // namespace BipedalLocomotion::Planners::UnicycleUtilities #endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_UTILITIES_H diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp new file mode 100644 index 0000000000..30e76a78e4 --- /dev/null +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -0,0 +1,842 @@ +/** + * @file UnicycleTrajectoryGenerator.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 +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion; + +class Planners::UnicycleTrajectoryGenerator::Impl +{ + +public: + enum class FSM + { + NotInitialized, + Initialized, + Running, + }; + + FSM state{FSM::NotInitialized}; + + UnicycleTrajectoryGeneratorOutput output; + + UnicycleTrajectoryGeneratorInput input; + + UnicycleTrajectoryGeneratorParameters parameters; + + bool newTrajectoryRequired; /**< True if a new trajectory is required. False otherwise. */ + + size_t newTrajectoryMergeCounter; /**< It is a counter that informs when the new + trajectory will be merged. + The new trajectory gets merged at the + (newTrajectoryMergeCounter - 2) cycle. */ + + std::chrono::nanoseconds time; /**< Current time */ + + std::mutex mutex; + + enum class unicycleTrajectoryPlannerState + { + Called, + Returned, + Running, + }; + + unicycleTrajectoryPlannerState unicyclePlannerState{unicycleTrajectoryPlannerState::Returned}; + + std::future unicyclePlannerOutputFuture; + + struct Trajectory + { + std::deque dcmPosition; + std::deque dcmVelocity; + std::deque comPosition; + std::deque comVelocity; + std::deque comAcceleration; + std::deque leftFootinContact; + std::deque rightFootinContact; + std::deque isLeftFootLastSwinging; + std::deque mergePoints; + std::deque leftSteps; + std::deque rightSteps; + std::deque leftFootTransform; + std::deque rightFootTransform; + std::deque leftFootMixedVelocity; + std::deque rightFootMixedVelocity; + std::deque leftFootMixedAcceleration; + std::deque rightFootMixedAcceleration; + }; + + Trajectory trajectory; + + BipedalLocomotion::Planners::UnicycleTrajectoryPlanner unicycleTrajectoryPlanner; + + /** + * ask for a new trajectory to the unicycle trajectory planner + */ + bool askNewTrajectory(const std::chrono::nanoseconds& initTime, + const manif::SE3d& measuredTransform); + + /** + * merge the current trajectory with the new one computed by the unicycle trajectory planner + */ + bool mergeTrajectories(const size_t& mergePoint); + + /** + * advance the current trajectory + */ + bool advanceTrajectory(); + + /* + * @brief It resets to an empty contact list. Then, if it exists, the + * current active or the last active contact found in is added to + * . + * @param time The current time. + * @param minStepDuration The minimum time duration of a step. + * @param previousContactList The previous contact list, computed at last iteration. + * @param currentContactList The current contact list, being generated. + */ + static void resetContactList(const std::chrono::nanoseconds& time, + const std::chrono::nanoseconds& minStepDuration, + const Contacts::ContactList& previousContactList, + Contacts::ContactList& currentContactList); +}; + +Planners::UnicycleTrajectoryGenerator::UnicycleTrajectoryGenerator() +{ + m_pImpl = std::make_unique(); +} + +Planners::UnicycleTrajectoryGenerator::~UnicycleTrajectoryGenerator() +{ + + Planners::UnicycleTrajectoryGenerator::Impl::unicycleTrajectoryPlannerState unicyclePlannerState; + + { + std::lock_guard lock(m_pImpl->mutex); + unicyclePlannerState = m_pImpl->unicyclePlannerState; + } // The mutex is automatically unlocked here + + if (unicyclePlannerState != Impl::unicycleTrajectoryPlannerState::Returned) + { + m_pImpl->unicyclePlannerOutputFuture.wait(); + } +} + +Planners::UnicycleTrajectoryGeneratorInput +Planners::UnicycleTrajectoryGeneratorInput::generateDummyUnicycleTrajectoryGeneratorInput() +{ + UnicycleTrajectoryGeneratorInput input; + + input.plannerInput = Eigen::VectorXd::Zero(3); + + return input; +} + +bool Planners::UnicycleTrajectoryGenerator::setRobotContactFrames(const iDynTree::Model& model) +{ + + const auto logPrefix = "[UnicycleTrajectoryGenerator::setRobotContactFrames]"; + + if (m_pImpl->state == Impl::FSM::NotInitialized) + { + log()->error("{} The Unicycle planner has not been initialized. Initialize it first.", + logPrefix); + return false; + } + + if (!m_pImpl->unicycleTrajectoryPlanner.setRobotContactFrames(model)) + { + log()->error("{} Unable to set the robot contact frames.", logPrefix); + m_pImpl->state = Impl::FSM::NotInitialized; + return false; + } + + m_pImpl->parameters.leftContactFrameIndex + = m_pImpl->unicycleTrajectoryPlanner.getLeftContactFrameIndex(); + + m_pImpl->parameters.rightContactFrameIndex + = m_pImpl->unicycleTrajectoryPlanner.getRightContactFrameIndex(); + + return true; +} + +bool Planners::UnicycleTrajectoryGenerator::initialize( + std::weak_ptr handler) +{ + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::initialize]"; + + bool ok{true}; + + 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; + }; + + double dt, plannerAdvanceTimeInS; + ok = ok && loadParamWithFallback("dt", dt, 0.002); + m_pImpl->parameters.dt = std::chrono::nanoseconds(static_cast(dt * 1e9)); + + ok = ok && loadParamWithFallback("planner_advance_time_in_s", plannerAdvanceTimeInS, 0.18); + + m_pImpl->parameters.plannerAdvanceTimeSteps + = std::round(plannerAdvanceTimeInS / dt) + 2; // The additional 2 + // steps are because + // the trajectory from + // the planner is + // requested two steps + // in advance wrt the + // merge point + + // Initialize the time + m_pImpl->time = std::chrono::nanoseconds::zero(); + + // Initialize the merge points + m_pImpl->trajectory.mergePoints.insert(m_pImpl->trajectory.mergePoints.begin(), 0); + + // Initialize the new trajectory merge counter + m_pImpl->newTrajectoryMergeCounter = -1; + + // Initialize the new trajectory required flag + m_pImpl->newTrajectoryRequired = false; + + // Initialize the blf unicycle planner + ok = ok && m_pImpl->unicycleTrajectoryPlanner.initialize(ptr); + + // Initialize contact frames + std::string leftContactFrameName, rightContactFrameName; + ok = ok && loadParam("leftContactFrameName", leftContactFrameName); + ok = ok && loadParam("rightContactFrameName", rightContactFrameName); + + // Initialize first trajectory + ok = ok && generateFirstTrajectory(); + ok = ok && m_pImpl->mergeTrajectories(0); + + if (ok) + { + m_pImpl->state = Impl::FSM::Initialized; + } + + return true; +} + +const Planners::UnicycleTrajectoryGeneratorOutput& +Planners::UnicycleTrajectoryGenerator::getOutput() const +{ + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::getOutput]"; + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmPosition, + m_pImpl->output.dcmTrajectory.position); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmVelocity, + m_pImpl->output.dcmTrajectory.velocity); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.comPosition, + m_pImpl->output.comTrajectory.position); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.comVelocity, + m_pImpl->output.comTrajectory.velocity); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.comAcceleration, + m_pImpl->output.comTrajectory.acceleration); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.leftFootTransform, + m_pImpl->output.leftFootTrajectory + .transform); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.rightFootTransform, + m_pImpl->output.rightFootTrajectory + .transform); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.leftFootMixedVelocity, + m_pImpl->output.leftFootTrajectory + .mixedVelocity); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.rightFootMixedVelocity, + m_pImpl->output.rightFootTrajectory + .mixedVelocity); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory + .leftFootMixedAcceleration, + m_pImpl->output.leftFootTrajectory + .mixedAcceleration); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory + .rightFootMixedAcceleration, + m_pImpl->output.rightFootTrajectory + .mixedAcceleration); + + // instatiate variables for the contact phase lists + BipedalLocomotion::Contacts::ContactListMap contactListMap; + BipedalLocomotion::Contacts::ContactList leftContactList, rightContactList; + + // reset the contact lists + if (!m_pImpl->output.contactPhaseList.lists().empty()) + { + m_pImpl->resetContactList(m_pImpl->time - m_pImpl->parameters.dt, + m_pImpl->unicycleTrajectoryPlanner.getMinStepDuration(), + m_pImpl->output.contactPhaseList.lists().at("left_foot"), + leftContactList); + m_pImpl->resetContactList(m_pImpl->time - m_pImpl->parameters.dt, + m_pImpl->unicycleTrajectoryPlanner.getMinStepDuration(), + m_pImpl->output.contactPhaseList.lists().at("right_foot"), + rightContactList); + } + + // get the left contact phase list + std::vector leftFootInContact; + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.leftFootinContact, + leftFootInContact); + + if (!Planners::UnicycleUtilities::getContactList(m_pImpl->time, + m_pImpl->parameters.dt, + leftFootInContact, + m_pImpl->trajectory.leftSteps, + m_pImpl->parameters.leftContactFrameIndex, + "left_foot", + leftContactList)) + { + + log()->error("{} Unable to get the contact list for the left foot.", logPrefix); + m_pImpl->output.isValid = false; + }; + + contactListMap["left_foot"] = leftContactList; + + // get the right contact phase list + std::vector rightFootInContact; + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.rightFootinContact, + rightFootInContact); + + if (!Planners::UnicycleUtilities::getContactList(m_pImpl->time, + m_pImpl->parameters.dt, + rightFootInContact, + m_pImpl->trajectory.rightSteps, + m_pImpl->parameters.rightContactFrameIndex, + "right_foot", + rightContactList)) + { + log()->error("{} Unable to get the contact list for the right foot.", logPrefix); + m_pImpl->output.isValid = false; + }; + + contactListMap["right_foot"] = rightContactList; + + m_pImpl->output.contactPhaseList.setLists(contactListMap); + + return m_pImpl->output; +} + +bool Planners::UnicycleTrajectoryGenerator::isOutputValid() const +{ + return (m_pImpl->state == Impl::FSM::Running) && (m_pImpl->output.isValid); +} + +bool Planners::UnicycleTrajectoryGenerator::setInput(const UnicycleTrajectoryGeneratorInput& input) + +{ + + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::setInput]"; + + if (m_pImpl->state == Impl::FSM::NotInitialized) + { + log()->error("{} The Unicycle planner has never been initialized.", logPrefix); + return false; + } + + // check if the input is valid + if (input.plannerInput.size() > 3) + { + log()->error("{} The planner input has to be a vector of size 2 or 3.", logPrefix); + return false; + } + + m_pImpl->input = input; + + // the trajectory was already finished the new trajectory will be attached as soon as possible + if (m_pImpl->trajectory.mergePoints.empty()) + { + if (!(m_pImpl->trajectory.leftFootinContact.front() + && m_pImpl->trajectory.rightFootinContact.front())) + { + log()->error("{} The trajectory has already finished but the system is not in double " + "support.", + logPrefix); + return false; + } + + if (m_pImpl->newTrajectoryRequired) + return true; + + // Since the evaluation of a new trajectory takes time the new trajectory will be merged + // after x cycles + m_pImpl->newTrajectoryMergeCounter = m_pImpl->parameters.plannerAdvanceTimeSteps; + } + + // the trajectory was not finished the new trajectory will be attached at the next merge point + else + { + // Searches for the first merge point that is at least m_plannerAdvanceTimeSteps steps away + auto firstMergePointAvailable + = std::find_if(m_pImpl->trajectory.mergePoints.begin(), + m_pImpl->trajectory.mergePoints.end(), + [this](size_t input) { + return input >= this->m_pImpl->parameters.plannerAdvanceTimeSteps; + }); + + if (firstMergePointAvailable != m_pImpl->trajectory.mergePoints.end()) + { + if (m_pImpl->newTrajectoryRequired) + return true; + + m_pImpl->newTrajectoryMergeCounter = *firstMergePointAvailable; + } else + { + if (m_pImpl->newTrajectoryRequired) + return true; + + m_pImpl->newTrajectoryMergeCounter = m_pImpl->parameters.plannerAdvanceTimeSteps; + } + } + + m_pImpl->newTrajectoryRequired = true; + + return true; +} + +bool Planners::UnicycleTrajectoryGenerator::advance() +{ + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::advance]"; + + if (m_pImpl->state == Impl::FSM::NotInitialized) + { + log()->error("{} The Unicycle Trajectory Generator has never been initialized.", logPrefix); + return false; + } + + // if a new trajectory is required check if its the time to evaluate the new trajectory or + // the time to attach new one + if (m_pImpl->newTrajectoryRequired) + { + // when we are near to the merge point the new trajectory is evaluated + if (m_pImpl->newTrajectoryMergeCounter == m_pImpl->parameters.plannerAdvanceTimeSteps) + { + + std::chrono::nanoseconds initTimeTrajectory + = m_pImpl->time + m_pImpl->newTrajectoryMergeCounter * m_pImpl->parameters.dt; + + manif::SE3d measuredTransform = m_pImpl->trajectory.isLeftFootLastSwinging.front() + ? m_pImpl->trajectory.rightFootTransform.at( + m_pImpl->newTrajectoryMergeCounter) + : m_pImpl->trajectory.leftFootTransform.at( + m_pImpl->newTrajectoryMergeCounter); + + // ask for a new trajectory (and spawn an asynchronous thread to compute it) + { + std::lock_guard lock(m_pImpl->mutex); + + if (m_pImpl->unicyclePlannerState == Impl::unicycleTrajectoryPlannerState::Running) + { + log()->error("{} The unicycle planner is still running.", logPrefix); + return false; + } + m_pImpl->unicyclePlannerState = Impl::unicycleTrajectoryPlannerState::Called; + } // The mutex is automatically unlocked here + + if (!m_pImpl->askNewTrajectory(initTimeTrajectory, measuredTransform)) + { + log()->error("{} Unable to ask for a new trajectory.", logPrefix); + return false; + } + } + + if (m_pImpl->newTrajectoryMergeCounter == 2) + { + if (!m_pImpl->mergeTrajectories(m_pImpl->newTrajectoryMergeCounter)) + { + log()->error("{} Error while updating trajectories. They were not computed yet.", + logPrefix); + + return false; + } + m_pImpl->newTrajectoryRequired = false; + } + + m_pImpl->newTrajectoryMergeCounter--; + } + + // advance time + m_pImpl->time += m_pImpl->parameters.dt; + + // advance the trajectory + m_pImpl->advanceTrajectory(); + + m_pImpl->state = Impl::FSM::Running; + + return true; +} + +bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::askNewTrajectory( + const std::chrono::nanoseconds& initTime, const manif::SE3d& measuredTransform) +{ + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::Impl::askForNewTrajectory]"; + + log()->debug("{} Asking for a new trajectory.", logPrefix); + + auto mergePoint = newTrajectoryMergeCounter; + + if (mergePoint >= trajectory.dcmPosition.size()) + { + log()->error("{} The mergePoint has to be lower than the trajectory size.", logPrefix); + return false; + } + + // lambda function that computes the new trajectory + auto computeNewTrajectory = [this]() -> bool { + { + std::lock_guard lock(mutex); + unicyclePlannerState = unicycleTrajectoryPlannerState::Running; + } // The mutex is automatically unlocked here + + // advance the planner + bool ok = this->unicycleTrajectoryPlanner.advance(); + + { + std::lock_guard lock(mutex); + unicyclePlannerState = unicycleTrajectoryPlannerState::Returned; + } // The mutex is automatically unlocked here + + return ok; + }; + + // create the input for the unicycle planner + UnicycleTrajectoryPlannerInput unicycleTrajectoryPlannerInput; + unicycleTrajectoryPlannerInput.plannerInput = input.plannerInput; + unicycleTrajectoryPlannerInput.initTime = initTime; + unicycleTrajectoryPlannerInput.isLeftLastSwinging = trajectory.isLeftFootLastSwinging.front(); + unicycleTrajectoryPlannerInput.measuredTransform = measuredTransform; + unicycleTrajectoryPlannerInput.dcmInitialState.initialPosition + = trajectory.dcmPosition[mergePoint]; + unicycleTrajectoryPlannerInput.dcmInitialState.initialVelocity + = trajectory.dcmVelocity[mergePoint]; + unicycleTrajectoryPlannerInput.comInitialState.initialPlanarPosition + = trajectory.comPosition[mergePoint].head<2>(); + unicycleTrajectoryPlannerInput.comInitialState.initialPlanarVelocity + = trajectory.comVelocity[mergePoint].head<2>(); + + // set the input + this->unicycleTrajectoryPlanner.setInput(unicycleTrajectoryPlannerInput); + + // create a new asynchronous thread to compute the new trajectory + unicyclePlannerOutputFuture = std::async(std::launch::async, computeNewTrajectory); + + return true; +} + +bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajectories( + const size_t& mergePoint) +{ + + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::Impl::mergeTrajectories]"; + + log()->debug("{} Merging trajectories at mergepoint {}", logPrefix, mergePoint); + + // if unicycle trajectory generator has been initialized, check if the unicycle planner has + // finished the computation + if (!(state == Impl::FSM::NotInitialized)) + { + if (unicyclePlannerState != unicycleTrajectoryPlannerState::Returned) + { + log()->error("{} The unicycle planner is still running.", logPrefix); + return false; + } + + if (unicyclePlannerOutputFuture.valid()) + { + if (!unicyclePlannerOutputFuture.get()) + { + log()->error("{} Unable to advance the unicycle planner.", logPrefix); + return false; + } + } else + { + log()->error("{} The trajectory is not valid at time {} [s].", logPrefix, time); + return false; + } + } + + // get the output of the unicycle planner and append it to deques + + // get dcm position + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .dcmTrajectory.position, + trajectory.dcmPosition, + mergePoint); + // get dcm velocity + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .dcmTrajectory.velocity, + trajectory.dcmVelocity, + mergePoint); + // get feet contact status + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .contactStatus.UsedLeftAsFixed, + trajectory.isLeftFootLastSwinging, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .contactStatus.leftFootInContact, + trajectory.leftFootinContact, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .contactStatus.rightFootInContact, + trajectory.rightFootinContact, + mergePoint); + // get com trajectory + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .comTrajectory.position, + trajectory.comPosition, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .comTrajectory.velocity, + trajectory.comVelocity, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .comTrajectory.acceleration, + trajectory.comAcceleration, + mergePoint); + // get feet cubic spline trajectory + + // get left foot cubic spline + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .leftFootTrajectory.transform, + trajectory.leftFootTransform, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .leftFootTrajectory.mixedVelocity, + trajectory.leftFootMixedVelocity, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .leftFootTrajectory.mixedAcceleration, + trajectory.leftFootMixedAcceleration, + mergePoint); + + // get right foot cubic spline + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .rightFootTrajectory.transform, + trajectory.rightFootTransform, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .rightFootTrajectory.mixedVelocity, + trajectory.rightFootMixedVelocity, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .rightFootTrajectory.mixedAcceleration, + trajectory.rightFootMixedAcceleration, + mergePoint); + + // get steps + auto newLeftSteps = unicycleTrajectoryPlanner.getOutput().steps.leftSteps; + auto newRightSteps = unicycleTrajectoryPlanner.getOutput().steps.rightSteps; + + // merge steps + BipedalLocomotion::Planners::UnicycleUtilities::mergeSteps(newLeftSteps, + trajectory.leftSteps, + time); + BipedalLocomotion::Planners::UnicycleUtilities::mergeSteps(newRightSteps, + trajectory.rightSteps, + time); + + // get merge points + std::vector mergePoints; + mergePoints = unicycleTrajectoryPlanner.getOutput().mergePoints; + + // update mergePoints of new trajectory based on where the new trajectory is merged + for (auto& element : mergePoints) + { + element += mergePoint; + } + trajectory.mergePoints.assign(mergePoints.begin(), mergePoints.end()); + + // pop the first merge point + trajectory.mergePoints.pop_front(); + + return true; +} + +bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::advanceTrajectory() +{ + + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::Impl::advanceTrajectory]"; + + // check if vector is not initialized + if (trajectory.leftFootinContact.empty() || trajectory.rightFootinContact.empty() + || trajectory.isLeftFootLastSwinging.empty() || trajectory.dcmPosition.empty() + || trajectory.dcmVelocity.empty() || trajectory.comPosition.empty() + || trajectory.comVelocity.empty() || trajectory.comAcceleration.empty()) + + { + log()->error(" {} Cannot advance empty trajectory signals.", logPrefix); + return false; + } + + // lambda function to advance the trajectory by 1 step + auto trajectoryStepLambda = [](auto& inputDeque) { + inputDeque.pop_front(); + inputDeque.push_back(inputDeque.back()); + }; + + // advance trajectories + bool rightWasInContact = trajectory.rightFootinContact.front(); + trajectoryStepLambda(trajectory.rightFootinContact); + bool leftWasInContact = trajectory.leftFootinContact.front(); + trajectoryStepLambda(trajectory.leftFootinContact); + trajectoryStepLambda(trajectory.isLeftFootLastSwinging); + trajectoryStepLambda(trajectory.dcmPosition); + trajectoryStepLambda(trajectory.dcmVelocity); + trajectoryStepLambda(trajectory.comPosition); + trajectoryStepLambda(trajectory.comVelocity); + trajectoryStepLambda(trajectory.comAcceleration); + trajectoryStepLambda(trajectory.leftFootTransform); + trajectoryStepLambda(trajectory.leftFootMixedVelocity); + trajectoryStepLambda(trajectory.leftFootMixedAcceleration); + trajectoryStepLambda(trajectory.rightFootTransform); + trajectoryStepLambda(trajectory.rightFootMixedVelocity); + trajectoryStepLambda(trajectory.rightFootMixedAcceleration); + + // at each sampling time the merge points are decreased by one. + // If the first merge point is equal to 0 it will be dropped. + // A new trajectory will be merged at the first merge point or if the deque is empty + // as soon as possible. + if (!trajectory.mergePoints.empty()) + { + for (auto& mergePoint : trajectory.mergePoints) + mergePoint--; + + if (trajectory.mergePoints[0] == 0) + trajectory.mergePoints.pop_front(); + } + + // if the left foot is leaving the contact, the step is dropped + if (leftWasInContact && !trajectory.leftFootinContact.front()) + { + trajectory.leftSteps.pop_front(); + } + + // if the right foot is leaving the contact, the step is dropped + if (rightWasInContact && !trajectory.rightFootinContact.front()) + { + trajectory.rightSteps.pop_front(); + } + + return true; +} + +bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::generateFirstTrajectory() +{ + + constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::generateFirstTrajectory]"; + + UnicycleTrajectoryPlannerInput unicycleTrajectoryPlannerInput; + unicycleTrajectoryPlannerInput.initTime = m_pImpl->time; + unicycleTrajectoryPlannerInput.comInitialState.initialPlanarPosition = Eigen::Vector2d::Zero(); + unicycleTrajectoryPlannerInput.comInitialState.initialPlanarVelocity = Eigen::Vector2d::Zero(); + + log()->debug("{} Generating the first trajectory.", logPrefix); + + if (!m_pImpl->unicycleTrajectoryPlanner.setInput(unicycleTrajectoryPlannerInput)) + { + log()->error("{} Unable to set the input for the unicycle planner.", logPrefix); + return false; + } + + if (!m_pImpl->unicycleTrajectoryPlanner.advance()) + { + log()->error("{} Unable to advance the unicycle planner.", logPrefix); + return false; + } + + return true; +} + +void Planners::UnicycleTrajectoryGenerator::Impl::resetContactList( + const std::chrono::nanoseconds& time, + const std::chrono::nanoseconds& minStepDuration, + const Contacts::ContactList& previousContactList, + Contacts::ContactList& currentContactList) +{ + currentContactList.clear(); + + if (previousContactList.size() == 0) + { + return; + } + + auto activeContact = previousContactList.getActiveContact(time); + + // Is contact present at the current time? + // If not, try to get the last active + if (activeContact == previousContactList.end()) + { + activeContact = previousContactList.getActiveContact(time - minStepDuration / 2); + } + + // if any active contact found, add it to the current contact list + if (!(activeContact == previousContactList.end())) + { + currentContactList.addContact(*activeContact); + } +} + +std::chrono::nanoseconds +BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::getSamplingTime() const +{ + return m_pImpl->parameters.dt; +}; diff --git a/src/Planners/src/UnicycleTrajectoryPlanner.cpp b/src/Planners/src/UnicycleTrajectoryPlanner.cpp index ed33743a3b..c583dce347 100644 --- a/src/Planners/src/UnicycleTrajectoryPlanner.cpp +++ b/src/Planners/src/UnicycleTrajectoryPlanner.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -85,6 +86,19 @@ class Planners::UnicycleTrajectoryPlanner::Impl }; COMHeightTrajectory comHeightTrajectory; + + struct FootTrajectory + { + + std::vector transform; /**< Foot transform */ + std::vector mixedVelocity; /**< Spatial velocity in mixed + representation */ + std::vector mixedAcceleration; /**< Spatial acceleration in mixed + representation */ + }; + + FootTrajectory rightFootTrajectory; + FootTrajectory leftFootTrajectory; }; BipedalLocomotion::Planners::UnicycleTrajectoryPlannerInput BipedalLocomotion::Planners:: @@ -182,6 +196,16 @@ bool Planners::UnicycleTrajectoryPlanner::setRobotContactFrames(const iDynTree:: return true; } +int Planners::UnicycleTrajectoryPlanner::getLeftContactFrameIndex() const +{ + return m_pImpl->parameters.leftContactFrameIndex; +} + +int Planners::UnicycleTrajectoryPlanner::getRightContactFrameIndex() const +{ + return m_pImpl->parameters.rightContactFrameIndex; +} + bool Planners::UnicycleTrajectoryPlanner::initialize( std::weak_ptr handler) { @@ -284,6 +308,8 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( ok = ok && loadParamWithFallback("nominalWidth", m_pImpl->parameters.nominalWidth, 0.20); ok = ok && loadParamWithFallback("minWidth", minWidth, 0.14); ok = ok && loadParamWithFallback("minStepDuration", minStepDuration, 0.65); + m_pImpl->parameters.minStepDuration + = std::chrono::nanoseconds(static_cast(minStepDuration * 1e9)); ok = ok && loadParamWithFallback("maxStepDuration", maxStepDuration, 1.5); ok = ok && loadParamWithFallback("nominalDuration", nominalDuration, 0.8); ok = ok && loadParamWithFallback("maxAngleVariation", maxAngleVariation, 18.0); @@ -382,6 +408,23 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( ok = ok && m_pImpl->comSystem.integrator->setDynamicalSystem(m_pImpl->comSystem.dynamics); ok = ok && m_pImpl->comSystem.integrator->setIntegrationStep(m_pImpl->parameters.dt); + // initialize the feet cubic spline generator + auto feetCubicSplineGenerator = m_pImpl->generator.addFeetCubicSplineGenerator(); + + double stepHeight; + double stepLandingVelocity; + double footApexTime; + double pitchDelta; + ok = ok && loadParamWithFallback("stepHeight", stepHeight, 0.05); + ok = ok && loadParamWithFallback("stepLandingVelocity", stepLandingVelocity, -0.1); + ok = ok && loadParamWithFallback("footApexTime", footApexTime, 0.5); + ok = ok && loadParamWithFallback("pitchDelta", pitchDelta, 0.0); + + feetCubicSplineGenerator->setFootApexTime(footApexTime); + feetCubicSplineGenerator->setFootLandingVelocity(stepLandingVelocity); + feetCubicSplineGenerator->setPitchDelta(pitchDelta); + feetCubicSplineGenerator->setStepHeight(stepHeight); + // generateFirstTrajectory; ok = ok && generateFirstTrajectory(); @@ -616,8 +659,10 @@ bool Planners::UnicycleTrajectoryPlanner::advance() return outputVect; }; - m_pImpl->output.dcmTrajectory.position = convertToEigen(dcmGenerator->getDCMPosition()); - m_pImpl->output.dcmTrajectory.velocity = convertToEigen(dcmGenerator->getDCMVelocity()); + Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMPosition(), + m_pImpl->output.dcmTrajectory.position); + Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMVelocity(), + m_pImpl->output.dcmTrajectory.velocity); // get the CoM planar trajectory std::chrono::nanoseconds time = m_pImpl->input.initTime; @@ -678,6 +723,42 @@ bool Planners::UnicycleTrajectoryPlanner::advance() = m_pImpl->comHeightTrajectory.acceleration[i]; } + // get the feet trajectories + auto feetCubicSplineGenerator = m_pImpl->generator.addFeetCubicSplineGenerator(); + + feetCubicSplineGenerator->getFeetTrajectories(m_pImpl->leftFootTrajectory.transform, + m_pImpl->rightFootTrajectory.transform); + feetCubicSplineGenerator + ->getFeetTwistsInMixedRepresentation(m_pImpl->leftFootTrajectory.mixedVelocity, + m_pImpl->rightFootTrajectory.mixedVelocity); + feetCubicSplineGenerator + ->getFeetAccelerationInMixedRepresentation(m_pImpl->leftFootTrajectory.mixedAcceleration, + m_pImpl->rightFootTrajectory.mixedAcceleration); + + Planners::UnicycleUtilities::Conversions::convertVector(m_pImpl->leftFootTrajectory.transform, + m_pImpl->output.leftFootTrajectory + .transform); + Planners::UnicycleUtilities::Conversions::convertVector(m_pImpl->rightFootTrajectory.transform, + m_pImpl->output.rightFootTrajectory + .transform); + + Planners::UnicycleUtilities::Conversions::convertVector(m_pImpl->leftFootTrajectory + .mixedVelocity, + m_pImpl->output.leftFootTrajectory + .mixedVelocity); + Planners::UnicycleUtilities::Conversions::convertVector(m_pImpl->rightFootTrajectory + .mixedVelocity, + m_pImpl->output.rightFootTrajectory + .mixedVelocity); + Planners::UnicycleUtilities::Conversions::convertVector(m_pImpl->leftFootTrajectory + .mixedAcceleration, + m_pImpl->output.leftFootTrajectory + .mixedAcceleration); + Planners::UnicycleUtilities::Conversions::convertVector(m_pImpl->rightFootTrajectory + .mixedAcceleration, + m_pImpl->output.rightFootTrajectory + .mixedAcceleration); + // get the merge points m_pImpl->generator.getMergePoints(m_pImpl->output.mergePoints); @@ -797,4 +878,10 @@ BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::getContactPhaseList() contactPhaseList.setLists(ContactListMap); return contactPhaseList; -}; \ No newline at end of file +}; + +std::chrono::nanoseconds +BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::getMinStepDuration() const +{ + return m_pImpl->parameters.minStepDuration; +} diff --git a/src/Planners/src/UnicycleUtilities.cpp b/src/Planners/src/UnicycleUtilities.cpp index 33cb14d68e..7294c587c8 100644 --- a/src/Planners/src/UnicycleUtilities.cpp +++ b/src/Planners/src/UnicycleUtilities.cpp @@ -5,11 +5,15 @@ * distributed under the terms of the BSD-3-Clause license. */ #include +#include #include #include #include +#include #include +#include + namespace BipedalLocomotion::Planners::UnicycleUtilities { @@ -99,4 +103,75 @@ bool getContactList(const std::chrono::nanoseconds& initTime, return true; }; +void mergeSteps(const std::deque& newSteps, + std::deque& currentSteps, + const std::chrono::nanoseconds& currentTime) +{ + // lambda function to check if a step (impact time) is later than the current time + auto isLater = [currentTime](const Step& step) -> bool { + return step.impactTime >= static_cast(currentTime.count() * 1e-9); + }; + + // find the first new step with impact time greater than the current time + auto itNew = std::find_if(newSteps.begin(), newSteps.end(), isLater); + + // find the first current step with impact time greater than the current time. + // from this point on, the current steps are discarded, and the new steps are added. + auto itCurrent = std::find_if(currentSteps.begin(), currentSteps.end(), isLater); + Step firstDeletedStep; + if (itCurrent != currentSteps.end()) + { + firstDeletedStep = *itCurrent; + } + + // erase all current steps which are after the current time + currentSteps.erase(itCurrent, currentSteps.end()); + + // if the are new valid steps, append them to the current steps + if (itNew != newSteps.end()) + { + // append new steps to the current steps + currentSteps.insert(currentSteps.end(), itNew, newSteps.end()); + } + + // Check if merged steps are empty. In this case use firstDeletedStep. + if (currentSteps.empty()) + { + currentSteps.push_back(firstDeletedStep); + } +} + +namespace Conversions +{ +void convertToBLF(const iDynTree::Transform& input, manif::SE3d& output) +{ + output.asSO3() = iDynTree::toEigen(input.getRotation().asQuaternion()); + output.translation(iDynTree::toEigen(input.getPosition())); +} + +void convertToBLF(const iDynTree::Twist& input, manif::SE3d::Tangent& output) +{ + output = iDynTree::toEigen(input); +} + +void convertToBLF(const iDynTree::SpatialAcc& input, manif::SE3d::Tangent& output) +{ + output = iDynTree::toEigen(input); +} + +void convertToBLF(const iDynTree::VectorDynSize& input, Eigen::VectorXd& output) +{ + output = iDynTree::toEigen(input); +} + +void convertToBLF(const iDynTree::Vector2& input, Eigen::Vector2d& output) +{ + output = iDynTree::toEigen(input); +} + +void convertToBLF(const iDynTree::Vector3& input, Eigen::Vector3d& output) +{ + output = iDynTree::toEigen(input); +} +} // namespace Conversions } // namespace BipedalLocomotion::Planners::UnicycleUtilities diff --git a/src/Planners/tests/CMakeLists.txt b/src/Planners/tests/CMakeLists.txt index 338fd69eed..1e30562060 100644 --- a/src/Planners/tests/CMakeLists.txt +++ b/src/Planners/tests/CMakeLists.txt @@ -49,4 +49,11 @@ add_bipedal_test( LINKS BipedalLocomotion::Unicycle ) + add_bipedal_test( + NAME UnicycleTrajectoryGenerator + SOURCES UnicycleTrajectoryGeneratorTest.cpp + + LINKS BipedalLocomotion::Unicycle + ) + endif() diff --git a/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp b/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp new file mode 100644 index 0000000000..ef92bed209 --- /dev/null +++ b/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp @@ -0,0 +1,75 @@ +/** + * @file UnicycleTrajectoryGeneratorTest.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 + +using namespace BipedalLocomotion::Planners; +using namespace BipedalLocomotion::ParametersHandler; + +std::shared_ptr params() +{ + // Set the non-default parameters of the planner + std::shared_ptr handler = std::make_shared(); + + 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("UnicycleTrajectoryGeneratorTest") +{ + + 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(); + + BipedalLocomotion::Planners::UnicycleTrajectoryGenerator unicycleTrajectoryGenerator; + + REQUIRE(unicycleTrajectoryGenerator.initialize(handler)); + + REQUIRE(unicycleTrajectoryGenerator.setRobotContactFrames(ml.model())); + + UnicycleTrajectoryGeneratorInput input + = UnicycleTrajectoryGeneratorInput::generateDummyUnicycleTrajectoryGeneratorInput(); + + UnicycleTrajectoryGeneratorOutput output; + + REQUIRE(unicycleTrajectoryGenerator.setInput(input)); + REQUIRE(unicycleTrajectoryGenerator.advance()); + REQUIRE(unicycleTrajectoryGenerator.isOutputValid()); + + output = unicycleTrajectoryGenerator.getOutput(); + + REQUIRE(output.contactPhaseList.size() == 1); +} diff --git a/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp b/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp index e86e77bf45..6e3391e271 100644 --- a/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp +++ b/src/Planners/tests/UnicycleTrajectoryPlannerTest.cpp @@ -14,28 +14,11 @@ #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 @@ -72,8 +55,6 @@ TEST_CASE("UnicyclePlannerTest") const auto handler = params(); - bool saveDataTofile = false; - BipedalLocomotion::Planners::UnicycleTrajectoryPlanner planner; REQUIRE(planner.initialize(handler)); @@ -83,20 +64,8 @@ TEST_CASE("UnicyclePlannerTest") 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"); - } + REQUIRE(planner.getContactPhaseList().size() == 1); }