diff --git a/CHANGELOG.md b/CHANGELOG.md index a67fbc27e1..82854ef4c7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ All notable changes to this project are documented in this file. - Add the test for the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/862) - Implement low-pass filter for estimated friction torques in `JointTorqueControlDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/892) - Add `blf-motor-current-tracking.py` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/894) +- Add the possibility to initialize the base position and the feet pose in the `unicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/887) ### Changed diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h index 359efe0dd5..002db1b757 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h @@ -243,6 +243,17 @@ class ContactList */ const_iterator getPresentContact(const std::chrono::nanoseconds& time) const; + /** + * @brief Get the contact given the time. + * + * It returns the contact with the highest deactivation time lower than time. + * If no contacts have a deactivation time lower than time, it returns an iterator to the end. + * @param time The present time. + * @return an iterator to the last contact having an activation time lower than time. + * If no contact satisfies this condition, it returns a pointer to the end. + */ + const_iterator getPreviousContact(const std::chrono::nanoseconds& time) const; + /** * @brief Clear all the steps, except the one returned by getPresentContact * @param time The present time. diff --git a/src/Contacts/src/ContactList.cpp b/src/Contacts/src/ContactList.cpp index 99d26f8289..9125e8ff16 100644 --- a/src/Contacts/src/ContactList.cpp +++ b/src/Contacts/src/ContactList.cpp @@ -264,6 +264,27 @@ ContactList::const_iterator ContactList::getNextContact(const std::chrono::nanos return nextContact; } +ContactList::const_iterator +ContactList::getPreviousContact(const std::chrono::nanoseconds& time) const +{ + // With the reverse iterator we find the last step such that the deactivation time is smaller + // than time + ContactList::const_reverse_iterator presentReverse + = std::find_if(rbegin(), rend(), [time](const PlannedContact& a) -> bool { + return a.deactivationTime < time; + }); + + if (presentReverse == rend()) + { + // No contact has activation time lower than the specified time. + return end(); + } + + return --(presentReverse.base()); // This is to convert a reverse iterator to a forward + // iterator. The -- is because base() returns a forward + // iterator to the next element. +} + bool ContactList::keepOnlyPresentContact(const std::chrono::nanoseconds& time) { ContactList::const_iterator dropPoint = getPresentContact(time); diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h index 55f012cab5..91bf82596b 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h @@ -116,6 +116,32 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final */ bool initialize(std::weak_ptr handler) override; + /** + * @brief Initialize the planner. + * @param handler Pointer to the parameter handler. + * @param initialBasePosition Initial position of the base. + * @param leftToRightTransform Transformation Matrix between the left and right foot. + */ + bool initialize(std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition, + const manif::SE3d& leftToRightTransform); + + /** + * @brief Initialize the planner. + * @param handler Pointer to the parameter handler. + * @param leftToRightTransform Transformation Matrix between the left and right foot. + */ + bool initialize(std::weak_ptr handler, + const manif::SE3d& leftToRightTransform); + + /** + * @brief Initialize the planner. + * @param handler Pointer to the parameter handler. + * @param initialBasePosition Initial position of the base. + */ + bool initialize(std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition); + /** * @brief Get the output of the planner. * @return Output of the planner. diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h index 2d2f64d7fe..d64c4e3f4d 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -143,6 +143,8 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerParameters int rightContactFrameIndex; /**< Index of the right foot contact frame */ std::string rightContactFrameName; /**< Name of the right foot contact frame */ + + bool swingLeft; /**< Perform the first step with the left foot */ }; /** @@ -238,6 +240,32 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final bool initialize(std::weak_ptr handler) override; // clang-format on + /** + * @brief Initialize the planner. + * @param handler Pointer to the parameter handler. + * @param initialBasePosition Initial position of the base. + * @param leftToRightTransform Transformation Matrix between the left and right foot. + */ + bool initialize(std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition, + const manif::SE3d& leftToRightTransform); + + /** + * @brief Initialize the planner. + * @param handler Pointer to the parameter handler. + * @param leftToRightTransform Transformation Matrix between the left and right foot. + */ + bool initialize(std::weak_ptr handler, + const manif::SE3d& leftToRightTransform); + + /** + * @brief Initialize the planner. + * @param handler Pointer to the parameter handler. + * @param initialBasePosition Initial position of the base. + */ + bool initialize(std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition); + /** * Get the output of the planner. * @return The output of the planner. @@ -291,8 +319,12 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final /** * Generate the first trajectory. + * @param initialBasePosition Initial position of the base. + * @param leftToRightTransform Transformation Matrix between the left and right foot. */ - bool generateFirstTrajectory(); + bool generateFirstTrajectory(const Eigen::Ref& initialBasePosition + = Eigen::Vector3d::Zero(), + const manif::SE3d& leftToRightTransform = manif::SE3d::Identity()); }; #endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index 30e76a78e4..aaa0b30c45 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -118,7 +118,6 @@ class Planners::UnicycleTrajectoryGenerator::Impl * @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); }; @@ -182,8 +181,30 @@ bool Planners::UnicycleTrajectoryGenerator::setRobotContactFrames(const iDynTree return true; } +bool Planners::UnicycleTrajectoryGenerator::initialize( + std::weak_ptr handler, + const manif::SE3d& leftToRightTransform) +{ + return initialize(handler, Eigen::Vector3d::Zero(), leftToRightTransform); +} + +bool Planners::UnicycleTrajectoryGenerator::initialize( + std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition) +{ + return initialize(handler, initialBasePosition, manif::SE3d::Identity()); +} + bool Planners::UnicycleTrajectoryGenerator::initialize( std::weak_ptr handler) +{ + return initialize(handler, Eigen::Vector3d::Zero(), manif::SE3d::Identity()); +} + +bool Planners::UnicycleTrajectoryGenerator::initialize( + std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition, + const manif::SE3d& leftToRightTransform) { constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::initialize]"; @@ -250,7 +271,10 @@ bool Planners::UnicycleTrajectoryGenerator::initialize( m_pImpl->newTrajectoryRequired = false; // Initialize the blf unicycle planner - ok = ok && m_pImpl->unicycleTrajectoryPlanner.initialize(ptr); + ok = ok + && m_pImpl->unicycleTrajectoryPlanner.initialize(ptr, + initialBasePosition, + leftToRightTransform); // Initialize contact frames std::string leftContactFrameName, rightContactFrameName; @@ -323,11 +347,9 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const 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); } @@ -808,7 +830,6 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::generateFirstTraj void Planners::UnicycleTrajectoryGenerator::Impl::resetContactList( const std::chrono::nanoseconds& time, - const std::chrono::nanoseconds& minStepDuration, const Contacts::ContactList& previousContactList, Contacts::ContactList& currentContactList) { @@ -825,7 +846,7 @@ void Planners::UnicycleTrajectoryGenerator::Impl::resetContactList( // If not, try to get the last active if (activeContact == previousContactList.end()) { - activeContact = previousContactList.getActiveContact(time - minStepDuration / 2); + activeContact = previousContactList.getPreviousContact(time); } // if any active contact found, add it to the current contact list diff --git a/src/Planners/src/UnicycleTrajectoryPlanner.cpp b/src/Planners/src/UnicycleTrajectoryPlanner.cpp index c583dce347..66bea056e8 100644 --- a/src/Planners/src/UnicycleTrajectoryPlanner.cpp +++ b/src/Planners/src/UnicycleTrajectoryPlanner.cpp @@ -17,8 +17,10 @@ #include #include + #include #include +#include #include #include @@ -206,8 +208,30 @@ int Planners::UnicycleTrajectoryPlanner::getRightContactFrameIndex() const return m_pImpl->parameters.rightContactFrameIndex; } +bool Planners::UnicycleTrajectoryPlanner::initialize( + std::weak_ptr handler, + const manif::SE3d& leftToRightTransform) +{ + return initialize(handler, Eigen::Vector3d::Zero(), leftToRightTransform); +} + +bool Planners::UnicycleTrajectoryPlanner::initialize( + std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition) +{ + return initialize(handler, initialBasePosition, manif::SE3d::Identity()); +} + bool Planners::UnicycleTrajectoryPlanner::initialize( std::weak_ptr handler) +{ + return initialize(handler, Eigen::Vector3d::Zero(), manif::SE3d::Identity()); +} + +bool Planners::UnicycleTrajectoryPlanner::initialize( + std::weak_ptr handler, + const Eigen::Vector3d& initialBasePosition, + const manif::SE3d& leftToRightTransform) { constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::initialize]"; @@ -270,7 +294,6 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( Eigen::Vector2d saturationFactors; - bool startWithLeft{true}; bool startWithSameFoot{true}; bool terminalStep{true}; @@ -325,7 +348,7 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( = 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("swingLeft", m_pImpl->parameters.swingLeft, false); ok = ok && loadParamWithFallback("startAlwaysSameFoot", startWithSameFoot, true); ok = ok && loadParamWithFallback("terminalStep", terminalStep, true); ok = ok && loadParam("mergePointRatios", mergePointRatios); @@ -365,7 +388,7 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( unicyclePlanner->setLeftFootYawOffsetInRadians(m_pImpl->parameters.leftYawDeltaInRad); unicyclePlanner->setRightFootYawOffsetInRadians(m_pImpl->parameters.rightYawDeltaInRad); unicyclePlanner->addTerminalStep(terminalStep); - unicyclePlanner->startWithLeft(startWithLeft); + unicyclePlanner->startWithLeft(m_pImpl->parameters.swingLeft); unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); UnicycleController unicycleController; @@ -426,7 +449,16 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( feetCubicSplineGenerator->setStepHeight(stepHeight); // generateFirstTrajectory; - ok = ok && generateFirstTrajectory(); + if (leftToRightTransform == manif::SE3d::Identity()) + { + // if the transform is the identity, the nominal width is used as distance between the feet + manif::SE3d tmpTransform = manif::SE3d::Identity(); + tmpTransform.translation(Eigen::Vector3d(0.0, -m_pImpl->parameters.nominalWidth, 0.0)); + ok = ok && generateFirstTrajectory(initialBasePosition, tmpTransform); + } else + { + ok = ok && generateFirstTrajectory(initialBasePosition, leftToRightTransform); + } // debug information auto leftSteps = m_pImpl->generator.getLeftFootPrint()->getSteps(); @@ -767,7 +799,9 @@ bool Planners::UnicycleTrajectoryPlanner::advance() return true; } -bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajectory() +bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajectory( + const Eigen::Ref& initialBasePosition, + const manif::SE3d& leftToRightTransform) { constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::generateFirstTrajectory]"; @@ -788,8 +822,10 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajec // 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); + m_personFollowingDesiredPoint(0) + = m_pImpl->parameters.referencePointDistance(0) + initialBasePosition(0); + m_personFollowingDesiredPoint(1) + = m_pImpl->parameters.referencePointDistance(1) + initialBasePosition(1); // add the initial point if (!unicyclePlanner @@ -811,12 +847,52 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajec return false; } + // add real position of the feet + std::shared_ptr left, right; + + left = m_pImpl->generator.getLeftFootPrint(); + left->clearSteps(); + right = m_pImpl->generator.getRightFootPrint(); + right->clearSteps(); + + Eigen::Vector2d leftPosition, rightPosition; + double leftAngle, rightAngle; + + // if the standing foot is the right -> the unicycle starts parallel to the right foot + if (m_pImpl->parameters.swingLeft) + { + rightPosition(0) = initialBasePosition(0); + rightPosition(1) + = initialBasePosition(1) - leftToRightTransform.inverse().translation()(1) / 2; + rightAngle = 0; + right->addStep(iDynTree::Vector2(rightPosition), rightAngle, 0.0); + + leftPosition(0) = initialBasePosition(0) + leftToRightTransform.inverse().translation()(0); + leftPosition(1) + = initialBasePosition(1) + leftToRightTransform.inverse().translation()(1) / 2; + iDynTree::Rotation leftToRightRotation + = iDynTree::Rotation(leftToRightTransform.rotation()); + leftAngle = leftToRightRotation.asRPY()(2); + left->addStep(iDynTree::Vector2(leftPosition), leftAngle, 0.0); + } else + { + leftPosition(0) = initialBasePosition(0); + leftPosition(1) = initialBasePosition(1) - leftToRightTransform.translation()(1) / 2; + leftAngle = 0; + left->addStep(iDynTree::Vector2(leftPosition), leftAngle, 0.0); + + rightPosition(0) = initialBasePosition(0) + leftToRightTransform.translation()(0); + rightPosition(1) = initialBasePosition(1) + leftToRightTransform.translation()(1) / 2; + iDynTree::Rotation leftToRightRotation + = iDynTree::Rotation(leftToRightTransform.rotation()); + rightAngle = leftToRightRotation.asRPY()(2); + right->addStep(iDynTree::Vector2(rightPosition), rightAngle, 0.0); + } + // generate the first trajectories if (!m_pImpl->generator.generate(initTime, dt, endTime)) { - log()->error("{} Error while computing the first trajectories.", logPrefix); - return false; } diff --git a/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp b/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp index ef92bed209..ae3c1beb06 100644 --- a/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp +++ b/src/Planners/tests/UnicycleTrajectoryGeneratorTest.cpp @@ -32,10 +32,13 @@ std::shared_ptr params() handler->setParameter("leftContactFrameName", "l_sole"); handler->setParameter("rightContactFrameName", "r_sole"); + // Override some default parameters + handler->setParameter("nominalWidth", 0.2); + return handler; } -TEST_CASE("UnicycleTrajectoryGeneratorTest") +TEST_CASE("Simple run", "[UnicycleTrajectoryGenerator]") { auto jointsList @@ -73,3 +76,135 @@ TEST_CASE("UnicycleTrajectoryGeneratorTest") REQUIRE(output.contactPhaseList.size() == 1); } + +TEST_CASE("Initialization with Feet Realitve Pose", "[UnicycleTrajectoryGenerator]") +{ + + 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; + + auto leftToRightTransform = manif::SE3d::Identity(); + double nominalWidth{}; + handler->getParameter("nominalWidth", nominalWidth); + leftToRightTransform.translation(Eigen::Vector3d(0.0, -nominalWidth, 0.0)); + + REQUIRE(unicycleTrajectoryGenerator.initialize(handler, leftToRightTransform)); + + 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); +} + +TEST_CASE("Initialization with Base Position", "[UnicycleTrajectoryGenerator]") +{ + + 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; + + Eigen::Vector3d basePosition = Eigen::Vector3d(0.0, 0.0, 0.7); + + REQUIRE(unicycleTrajectoryGenerator.initialize(handler, basePosition)); + + 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); +} + +TEST_CASE("Initialization with Base Position and Feet Relative Pose", + "[UnicycleTrajectoryGenerator]") +{ + + 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; + + auto leftToRightTransform = manif::SE3d::Identity(); + double nominalWidth{}; + handler->getParameter("nominalWidth", nominalWidth); + leftToRightTransform.translation(Eigen::Vector3d(0.0, -nominalWidth, 0.0)); + + Eigen::Vector3d basePosition = Eigen::Vector3d(0.0, 0.0, 0.7); + + REQUIRE(unicycleTrajectoryGenerator.initialize(handler, basePosition, leftToRightTransform)); + + 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); +}