From 1835d079e8c231235747d727730c07f81353846d Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Mon, 31 May 2021 18:00:00 +0200
Subject: [PATCH 01/15] Update CMakeLists.txt style

---
 src/Planners/CMakeLists.txt | 36 ++++++++++++++++++++++++++++++------
 1 file changed, 30 insertions(+), 6 deletions(-)

diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt
index ba76e8d999..e5ca25cb40 100644
--- a/src/Planners/CMakeLists.txt
+++ b/src/Planners/CMakeLists.txt
@@ -7,12 +7,36 @@ if (FRAMEWORK_COMPILE_Planners)
   set(H_PREFIX include/BipedalLocomotion/Planners)
 
   add_bipedal_locomotion_library(
-    NAME                   Planners
-    PUBLIC_HEADERS         ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h ${H_PREFIX}/Spline.h ${H_PREFIX}/QuinticSpline.h ${H_PREFIX}/SO3Planner.h ${H_PREFIX}/SO3Planner.tpp ${H_PREFIX}/SwingFootPlanner.h ${H_PREFIX}/CubicSpline.h
-    SOURCES                src/ConvexHullHelper.cpp  src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp src/QuinticSpline.cpp src/SwingFootPlanner.cpp src/CubicSpline.cpp
-    PUBLIC_LINK_LIBRARIES  Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts
-    PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r casadi BipedalLocomotion::Math BipedalLocomotion::TextLogging
-    INSTALLATION_FOLDER    Planners)
+    NAME Planners
+    PUBLIC_HEADERS
+    ${H_PREFIX}/ConvexHullHelper.h
+    ${H_PREFIX}/DCMPlanner.h
+    ${H_PREFIX}/TimeVaryingDCMPlanner.h
+    ${H_PREFIX}/Spline.h
+    ${H_PREFIX}/QuinticSpline.h
+    ${H_PREFIX}/SO3Planner.h
+    ${H_PREFIX}/SO3Planner.tpp
+    ${H_PREFIX}/SwingFootPlanner.h
+    ${H_PREFIX}/CubicSpline.h
+    SOURCES
+    src/ConvexHullHelper.cpp
+    src/DCMPlanner.cpp
+    src/TimeVaryingDCMPlanner.cpp
+    src/QuinticSpline.cpp
+    src/SwingFootPlanner.cpp
+    src/CubicSpline.cpp
+    PUBLIC_LINK_LIBRARIES
+    Eigen3::Eigen
+    BipedalLocomotion::ParametersHandler
+    BipedalLocomotion::System
+    BipedalLocomotion::Contacts
+    PRIVATE_LINK_LIBRARIES
+    Qhull::qhullcpp
+    Qhull::qhullstatic_r
+    casadi
+    BipedalLocomotion::Math
+    BipedalLocomotion::TextLogging
+    INSTALLATION_FOLDER Planners)
 
   add_subdirectory(tests)
 

From 38b39ea17b7feac9a145b62f5eb01d6bbcafaafb Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Fri, 6 Aug 2021 15:10:56 +0200
Subject: [PATCH 02/15] Add UnicyclePlanner class

---
 ...pedalLocomotionFrameworkDependencies.cmake |   7 +
 src/Planners/CMakeLists.txt                   |  24 +
 .../Planners/UnicyclePlanner.h                | 122 ++++
 src/Planners/src/UnicyclePlanner.cpp          | 600 ++++++++++++++++++
 4 files changed, 753 insertions(+)
 create mode 100644 src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
 create mode 100644 src/Planners/src/UnicyclePlanner.cpp

diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake
index 0aac6767d4..b5ea709ff9 100644
--- a/cmake/BipedalLocomotionFrameworkDependencies.cmake
+++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake
@@ -83,6 +83,9 @@ checkandset_dependency(Catch2)
 find_package(VALGRIND QUIET)
 checkandset_dependency(VALGRIND)
 
+find_package(UnicyclePlanner QUIET)
+checkandset_dependency(UnicyclePlanner)
+
 ##########################      Components       ##############################
 framework_dependent_option(FRAMEWORK_COMPILE_tests
   "Compile tests?" ON
@@ -128,6 +131,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_System
   "Compile System library?" ON
   "" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_Unicycle
+    "Compile the Unicycle Planner library?" ON
+    "FRAMEWORK_USE_UnicyclePlanner;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_Planners;FRAMEWORK_COMPILE_Contact" OFF)
+
 framework_dependent_option(FRAMEWORK_COMPILE_ContinuousDynamicalSystem
   "Compile System ContinuousDynamicalSystem?" ON
   "FRAMEWORK_COMPILE_ContactModels;FRAMEWORK_COMPILE_Math" OFF)
diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt
index e5ca25cb40..742cf9e1a0 100644
--- a/src/Planners/CMakeLists.txt
+++ b/src/Planners/CMakeLists.txt
@@ -34,6 +34,7 @@ if (FRAMEWORK_COMPILE_Planners)
     Qhull::qhullcpp
     Qhull::qhullstatic_r
     casadi
+    iDynTree::idyntree-core
     BipedalLocomotion::Math
     BipedalLocomotion::TextLogging
     INSTALLATION_FOLDER Planners)
@@ -41,3 +42,26 @@ if (FRAMEWORK_COMPILE_Planners)
   add_subdirectory(tests)
 
 endif()
+
+if (FRAMEWORK_COMPILE_Unicycle)
+
+  set(H_PREFIX include/BipedalLocomotion/Planners)
+
+  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
+    INSTALLATION_FOLDER Planners)
+
+endif()
diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
new file mode 100644
index 0000000000..f7ec4a9bc5
--- /dev/null
+++ b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
@@ -0,0 +1,122 @@
+/**
+ * @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 GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#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 <Eigen/Core>
+#include <Eigen/Dense>
+
+#include <memory>
+
+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;
+    double y;
+
+    double dx = 0.0;
+    double dy = 0.0;
+
+    double time = 0.0;
+};
+
+struct BipedalLocomotion::Planners::UnicyclePlannerInput
+{
+    UnicyclePlannerInput(const std::vector<UnicycleKnot>& _knots,
+                         const double _tf = 0.0,
+                         const double _t0 = 0.0)
+        : t0(_t0)
+        , tf(_tf)
+        , knots(_knots)
+    {
+    }
+
+    double t0;
+    double tf;
+
+    std::vector<UnicycleKnot> knots;
+};
+
+struct BipedalLocomotion::Planners::UnicyclePlannerOutput
+{
+    UnicyclePlannerOutput(const Contacts::ContactList& _left = {},
+                          const Contacts::ContactList& _right = {})
+        : left(_left)
+        , right(_right)
+    {
+    }
+
+    Contacts::ContactList left;
+    Contacts::ContactList right;
+};
+
+class BipedalLocomotion::Planners::UnicyclePlanner final
+    : public System::Advanceable<UnicyclePlannerInput, UnicyclePlannerOutput>
+{
+public:
+    UnicyclePlanner();
+
+    virtual ~UnicyclePlanner();
+
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
+
+    const UnicyclePlannerOutput& getOutput() const override;
+
+    bool isOutputValid() const override;
+
+    bool setInput(const UnicyclePlannerInput& input) override;
+
+    bool advance() override;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> m_pImpl;
+};
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H
diff --git a/src/Planners/src/UnicyclePlanner.cpp b/src/Planners/src/UnicyclePlanner.cpp
new file mode 100644
index 0000000000..52847731d7
--- /dev/null
+++ b/src/Planners/src/UnicyclePlanner.cpp
@@ -0,0 +1,600 @@
+/**
+ * @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 GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include "BipedalLocomotion/Planners/UnicyclePlanner.h"
+#include "BipedalLocomotion/TextLogging/Logger.h"
+
+#include <FootPrint.h>
+#include <UnicycleGenerator.h>
+#include <UnicyclePlanner.h>
+#include <iDynTree/Core/VectorFixSize.h>
+
+#include <cassert>
+#include <limits>
+#include <optional>
+
+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;
+        double lastStepSwitchTime;
+        bool startWithLeft = false;
+        bool terminalStep = true;
+        bool resetStartingFootIfStill = false;
+    } gait;
+
+    struct
+    {
+        std::string left = "left";
+        std::string right = "right";
+    } names;
+
+    UnicyclePlannerOutput outputRef;
+    std::optional<UnicyclePlannerOutput> 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<Impl>()}
+{
+}
+
+Planners::UnicyclePlanner::~UnicyclePlanner() = default;
+
+bool Planners::UnicyclePlanner::initialize(
+    std::weak_ptr<const ParametersHandler::IParametersHandler> 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<double> 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("lastStepSwitchTime", m_pImpl->gait.lastStepSwitchTime))
+    {
+        m_pImpl->gait.lastStepSwitchTime = m_pImpl->duration.nominal;
+        log()->info("{} Using default lastStepSwitchTime={}.",
+                    logPrefix,
+                    m_pImpl->gait.lastStepSwitchTime);
+    }
+
+    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);
+        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;
+
+    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;
+    }
+
+    m_pImpl->output = std::nullopt;
+
+    // Initialize the left FootPrint
+    m_pImpl->left = std::make_shared<FootPrint>();
+    m_pImpl->left->setFootName(m_pImpl->names.left);
+
+    // Initialize the right FootPrint
+    m_pImpl->right = std::make_shared<FootPrint>();
+    m_pImpl->right->setFootName(m_pImpl->names.right);
+
+    // Compute the FootPrints
+    if (!m_pImpl->planner->computeNewSteps(m_pImpl->left,
+                                           m_pImpl->right,
+                                           m_pImpl->horizon.t0,
+                                           m_pImpl->horizon.tf))
+    {
+        m_pImpl->left = nullptr;
+        m_pImpl->right = nullptr;
+        log()->error("{} Failed to compute new steps.", logPrefix);
+        return false;
+    }
+
+    // Create and configure the generator
+    auto generator = UnicycleGenerator();
+    generator.setSwitchOverSwingRatio(m_pImpl->gait.stancePhaseRatio);
+    double lastStepSwitchTime = 0.8;
+    generator.setTerminalHalfSwitchTime(lastStepSwitchTime);
+    generator.setPauseConditions(m_pImpl->duration.max, m_pImpl->duration.nominal);
+
+    // Compute the contact states using the generator
+    if (!generator.generateFromFootPrints(m_pImpl->left,
+                                          m_pImpl->right,
+                                          m_pImpl->horizon.t0,
+                                          m_pImpl->dt.planner))
+    {
+        log()->error("{} Failed to generate from footprints.", logPrefix);
+        return false;
+    }
+
+    // Get the contact states over the horizon
+    std::vector<bool> leftStandingPeriod;
+    std::vector<bool> rightStandingPeriod;
+    generator.getFeetStandingPeriods(leftStandingPeriod, rightStandingPeriod);
+
+    // Lambda to convert the vector with contact states to a vector of PlannedContact.
+    // It just processes timings, we fill the transform later.
+    auto processStandingPeriod
+        = [](const std::vector<bool>& isFootInContactVector,
+             const double dt,
+             const std::string& contactName) -> std::vector<Contacts::PlannedContact> {
+        std::vector<Contacts::PlannedContact> contacts;
+
+        // Loop over all the horizon
+        for (size_t i = 1; i < isFootInContactVector.size(); ++i)
+        {
+            const bool thisState = isFootInContactVector[i];
+            const bool lastState = isFootInContactVector[i - 1];
+            const bool aboutToLand = lastState == 0 && thisState == 1;
+
+            // Create a new contact
+            if (i == 1 || aboutToLand)
+            {
+                auto contact = Contacts::PlannedContact();
+                contact.name = contactName;
+                contact.activationTime = dt * i;
+                contact.deactivationTime = contact.activationTime;
+                contacts.push_back(contact);
+            }
+
+            // Adjust the first contact activation
+            if (i == 1)
+                contacts.back().activationTime -= dt;
+
+            // Upate the contact duration
+            if (thisState)
+                contacts.back().deactivationTime += dt;
+        }
+
+        return contacts;
+    };
+
+    // Get the feet contacts with only timings
+    std::vector<Contacts::PlannedContact> leftContacts
+        = processStandingPeriod(leftStandingPeriod, m_pImpl->dt.planner, m_pImpl->names.left);
+    std::vector<Contacts::PlannedContact> rightContacts
+        = processStandingPeriod(rightStandingPeriod, m_pImpl->dt.planner, m_pImpl->names.right);
+
+    if (m_pImpl->left->getSteps().size() != leftContacts.size())
+    {
+        log()->error("{} Wrong number of converted steps for left foot.", logPrefix);
+        return false;
+    }
+
+    if (m_pImpl->right->getSteps().size() != rightContacts.size())
+    {
+        log()->error("{} Wrong number of converted steps for left foot.", logPrefix);
+        return false;
+    }
+
+    // Lambda to fill the transforms of the PlannedContact objects
+    auto fillContactTransform = [](std::vector<Contacts::PlannedContact>& contacts,
+                                   decltype(::FootPrint().getSteps())& steps) {
+        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());
+
+    // Lambda to convert vector of PlannedContact to ConctactList
+    auto convertToContactList
+        = [](const std::vector<Contacts::PlannedContact>& 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<UnicyclePlannerOutput>();
+
+    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;
+}

From 56d92318d230124dcf052398ad80677f6d4ee66c Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Tue, 1 Jun 2021 12:59:41 +0200
Subject: [PATCH 03/15] Update CMakeLists.txt style

---
 bindings/python/Planners/CMakeLists.txt | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/bindings/python/Planners/CMakeLists.txt b/bindings/python/Planners/CMakeLists.txt
index 7bdd64ef33..28a2fec229 100644
--- a/bindings/python/Planners/CMakeLists.txt
+++ b/bindings/python/Planners/CMakeLists.txt
@@ -8,10 +8,24 @@ if(FRAMEWORK_COMPILE_Planners)
 
   add_bipedal_locomotion_python_module(
     NAME PlannersBindings
-    SOURCES src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp src/QuinticSpline.cpp src/SwingFootPlanner.cpp src/Module.cpp
-    HEADERS ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h ${H_PREFIX}/QuinticSpline.h ${H_PREFIX}/SwingFootPlanner.h ${H_PREFIX}/Module.h
-    LINK_LIBRARIES BipedalLocomotion::Planners
-    TESTS tests/test_quintic_spline.py tests/test_swing_foot_planner.py tests/test_time_verying_dcm_planner.py
+    SOURCES
+    src/DCMPlanner.cpp
+    src/TimeVaryingDCMPlanner.cpp
+    src/QuinticSpline.cpp
+    src/SwingFootPlanner.cpp
+    src/Module.cpp
+    HEADERS
+    ${H_PREFIX}/DCMPlanner.h
+    ${H_PREFIX}/TimeVaryingDCMPlanner.h
+    ${H_PREFIX}/QuinticSpline.h
+    ${H_PREFIX}/SwingFootPlanner.h
+    ${H_PREFIX}/Module.h
+    LINK_LIBRARIES
+    BipedalLocomotion::Planners
+    TESTS
+    tests/test_quintic_spline.py
+    tests/test_swing_foot_planner.py
+    tests/test_time_verying_dcm_planner.py
     )
 
 endif()

From e1ea778bc3fb423085ff2ec124ef48978176d890 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Wed, 4 Aug 2021 15:36:51 +0200
Subject: [PATCH 04/15] Add UnicyclePlanner Python bindings

---
 bindings/python/Planners/CMakeLists.txt       |  7 +-
 .../bindings/Planners/UnicyclePlanner.h       | 18 +++++
 bindings/python/Planners/src/Module.cpp       |  2 +
 .../python/Planners/src/UnicyclePlanner.cpp   | 75 +++++++++++++++++++
 4 files changed, 101 insertions(+), 1 deletion(-)
 create mode 100644 bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h
 create mode 100644 bindings/python/Planners/src/UnicyclePlanner.cpp

diff --git a/bindings/python/Planners/CMakeLists.txt b/bindings/python/Planners/CMakeLists.txt
index 28a2fec229..8b538b8aa5 100644
--- a/bindings/python/Planners/CMakeLists.txt
+++ b/bindings/python/Planners/CMakeLists.txt
@@ -2,7 +2,7 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-if(FRAMEWORK_COMPILE_Planners)
+if(FRAMEWORK_COMPILE_Planners AND FRAMEWORK_COMPILE_Unicycle)
 
   set(H_PREFIX include/BipedalLocomotion/bindings/Planners)
 
@@ -13,15 +13,20 @@ if(FRAMEWORK_COMPILE_Planners)
     src/TimeVaryingDCMPlanner.cpp
     src/QuinticSpline.cpp
     src/SwingFootPlanner.cpp
+    src/UnicyclePlanner.cpp
     src/Module.cpp
     HEADERS
     ${H_PREFIX}/DCMPlanner.h
     ${H_PREFIX}/TimeVaryingDCMPlanner.h
     ${H_PREFIX}/QuinticSpline.h
     ${H_PREFIX}/SwingFootPlanner.h
+    ${H_PREFIX}/UnicyclePlanner.h
     ${H_PREFIX}/Module.h
     LINK_LIBRARIES
+    BipedalLocomotion::System
     BipedalLocomotion::Planners
+    BipedalLocomotion::Unicycle
+    BipedalLocomotion::Contacts
     TESTS
     tests/test_quintic_spline.py
     tests/test_swing_foot_planner.py
diff --git a/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h b/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h
new file mode 100644
index 0000000000..55068cfb46
--- /dev/null
+++ b/bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h
@@ -0,0 +1,18 @@
+/**
+ * @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 GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H
+
+#include <pybind11/pybind11.h>
+
+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/Module.cpp b/bindings/python/Planners/src/Module.cpp
index e3575fa0de..43a367bfb0 100644
--- a/bindings/python/Planners/src/Module.cpp
+++ b/bindings/python/Planners/src/Module.cpp
@@ -12,6 +12,7 @@
 #include <BipedalLocomotion/bindings/Planners/QuinticSpline.h>
 #include <BipedalLocomotion/bindings/Planners/SwingFootPlanner.h>
 #include <BipedalLocomotion/bindings/Planners/TimeVaryingDCMPlanner.h>
+#include <BipedalLocomotion/bindings/Planners/UnicyclePlanner.h>
 
 namespace BipedalLocomotion
 {
@@ -27,6 +28,7 @@ void CreateModule(pybind11::module& module)
     CreateDCMPlanner(module);
     CreateTimeVaryingDCMPlanner(module);
     CreateSwingFootPlanner(module);
+    CreateUnicyclePlanner(module);
 }
 } // namespace Planners
 } // namespace bindings
diff --git a/bindings/python/Planners/src/UnicyclePlanner.cpp b/bindings/python/Planners/src/UnicyclePlanner.cpp
new file mode 100644
index 0000000000..06ce7299ab
--- /dev/null
+++ b/bindings/python/Planners/src/UnicyclePlanner.cpp
@@ -0,0 +1,75 @@
+/**
+ * @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 GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include "BipedalLocomotion/Planners/UnicyclePlanner.h"
+#include "BipedalLocomotion/Contacts/ContactList.h"
+#include "BipedalLocomotion/ParametersHandler/IParametersHandler.h"
+#include "BipedalLocomotion/System/Advanceable.h"
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+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_<UnicycleKnot>(module, "UnicycleKnot")
+        .def(py::init<const Eigen::Vector2d&, const Eigen::Vector2d&, double>(),
+             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());
+
+    py::class_<UnicyclePlannerInput>(module, "UnicyclePlannerInput")
+        .def(py::init<const std::vector<UnicycleKnot>&, const double, const double>(),
+             py::arg("knots") = std::vector<UnicycleKnot>{},
+             py::arg("tf") = 0.0,
+             py::arg("t0") = 0.0)
+        .def_readwrite("t0", &UnicyclePlannerInput::t0)
+        .def_readwrite("tf", &UnicyclePlannerInput::tf)
+        .def_readwrite("knots", &UnicyclePlannerInput::knots);
+
+    py::class_<UnicyclePlannerOutput>(module, "UnicyclePlannerOutput")
+        .def(py::init<const Contacts::ContactList&, const Contacts::ContactList&>(),
+             py::arg("left") = Contacts::ContactList(),
+             py::arg("right") = Contacts::ContactList())
+        .def_readwrite("left", &UnicyclePlannerOutput::left)
+        .def_readwrite("right", &UnicyclePlannerOutput::right);
+
+    py::class_<Advanceable<UnicyclePlannerInput, UnicyclePlannerOutput>>( //
+        module,
+        "UnicyclePlannerAdvanceable");
+
+    py::class_<UnicyclePlanner, Advanceable<UnicyclePlannerInput, UnicyclePlannerOutput>>( //
+        module,
+        "UnicyclePlanner")
+        .def(py::init())
+        .def(
+            "initialize",
+            [](UnicyclePlanner& impl, std::shared_ptr<const IParametersHandler> handler) -> bool {
+                return impl.initialize(handler);
+            },
+            py::arg("handler"))
+        .def("get_output", &UnicyclePlanner::getOutput)
+        .def("is_output_valid", &UnicyclePlanner::isOutputValid)
+        .def("set_input", &UnicyclePlanner::setInput, py::arg("input"))
+        .def("advance", &UnicyclePlanner::advance);
+}
+
+} // namespace BipedalLocomotion::bindings::Planners

From 50762b2e1a205ff3772425dc187de2974272a0f1 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Mon, 14 Jun 2021 11:10:59 +0200
Subject: [PATCH 05/15] Add UnicyclePlanner Python test

---
 bindings/python/Planners/CMakeLists.txt       |   1 +
 .../Planners/tests/test_unicycle_planner.py   | 156 ++++++++++++++++++
 2 files changed, 157 insertions(+)
 create mode 100644 bindings/python/Planners/tests/test_unicycle_planner.py

diff --git a/bindings/python/Planners/CMakeLists.txt b/bindings/python/Planners/CMakeLists.txt
index 8b538b8aa5..5331ef02ff 100644
--- a/bindings/python/Planners/CMakeLists.txt
+++ b/bindings/python/Planners/CMakeLists.txt
@@ -31,6 +31,7 @@ if(FRAMEWORK_COMPILE_Planners AND FRAMEWORK_COMPILE_Unicycle)
     tests/test_quintic_spline.py
     tests/test_swing_foot_planner.py
     tests/test_time_verying_dcm_planner.py
+    tests/test_unicycle_planner.py
     )
 
 endif()
diff --git a/bindings/python/Planners/tests/test_unicycle_planner.py b/bindings/python/Planners/tests/test_unicycle_planner.py
new file mode 100644
index 0000000000..d93621ce23
--- /dev/null
+++ b/bindings/python/Planners/tests/test_unicycle_planner.py
@@ -0,0 +1,156 @@
+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)

From 023ebaf9f09d76fd00fdaa3b972f2d15428acf2f Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Tue, 1 Jun 2021 18:12:03 +0200
Subject: [PATCH 06/15] Add equality operator to Python's ContactList

---
 bindings/python/Contacts/src/Contacts.cpp | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/bindings/python/Contacts/src/Contacts.cpp b/bindings/python/Contacts/src/Contacts.cpp
index d56a6c81f3..82dfce2116 100644
--- a/bindings/python/Contacts/src/Contacts.cpp
+++ b/bindings/python/Contacts/src/Contacts.cpp
@@ -132,7 +132,18 @@ void CreateContactList(pybind11::module& module)
                  return "ContactList(" + std::to_string(list.size()) + ")";
              })
         .def("__reverse__",
-             [](const ContactList& l) { return py::make_iterator(l.crbegin(), l.crend()); });
+             [](const ContactList& l) { return py::make_iterator(l.crbegin(), l.crend()); })
+        .def(
+            "__eq__",
+            [](const ContactList& lhs, const ContactList& rhs) -> bool {
+                for (std::size_t i = 0; i < lhs.size(); ++i)
+                {
+                    if (!(lhs[i] == rhs[i]))
+                        return false;
+                }
+                return lhs.size() == rhs.size();
+            },
+            py::is_operator());
 }
 
 void CreateContactPhase(pybind11::module& module)
@@ -184,6 +195,6 @@ void CreateContactListJsonParser(pybind11::module& module)
                py::arg("filename"));
 }
 
-} // namespace contatcs
+} // namespace Contacts
 } // namespace bindings
 } // namespace BipedalLocomotion

From ee4cf78cd54add264eaba62d8f9e39e8b974e0a8 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Wed, 16 Jun 2021 11:56:16 +0200
Subject: [PATCH 07/15] Install unicycle-footstep-planner in CI

---
 .github/workflows/ci.yml             | 26 +++++++++++++++++++++++++-
 .github/workflows/conda-forge-ci.yml |  2 +-
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 9edf1dc68d..5041691a2c 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -22,6 +22,7 @@ env:
   osqp_TAG: v0.6.2
   OsqpEigen_TAG: v0.6.3
   tomlplusplus_TAG: 4f21332bdd7555bbf9add1aafe82909a2f8aa52b~
+  UnicyclePlanner_TAG: d3f6c80afe21a9958da769c8dd8a2bbfee5ea922
 
   # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
   VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg
@@ -126,7 +127,7 @@ jobs:
       with:
         path: ${{ github.workspace }}/install/deps
         # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45
-        key: source-deps-${{ runner.os }}-${{runner.temp}}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}-tomlplusplus-${{ env.tomlplusplus_TAG }}
+        key: source-deps-${{ runner.os }}-${{runner.temp}}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}-tomlplusplus-${{ env.tomlplusplus_TAG }}-unicycle-${{UnicyclePlanner_TAG}}
 
 
     - name: Source-based Dependencies [Windows]
@@ -271,6 +272,19 @@ jobs:
                     -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
+        # UnicycleFootstepPlanner
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/unicycle-footstep-planner
+        cd unicycle-footstep-planner
+        git checkout ${UnicyclePlanner_TAG}
+        mkdir build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+                     -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \
+                     -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+                    -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
     - name: Source-based Dependencies [Ubuntu/macOS]
       if: steps.cache-source-deps.outputs.cache-hit != 'true' && (startsWith(matrix.os, 'ubuntu') || matrix.os == 'macos-latest')
       shell: bash
@@ -377,6 +391,16 @@ jobs:
         cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
+        # UnicycleFootstepPlanner
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/unicycle-footstep-planner
+        cd unicycle-footstep-planner
+        git checkout ${UnicyclePlanner_TAG}
+        mkdir build
+        cd build
+        cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
     - name: Source-based Dependencies [Ubuntu]
       if: steps.cache-source-deps.outputs.cache-hit != 'true' && startsWith(matrix.os, 'ubuntu')
       shell: bash
diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml
index 8b3da88c5b..bc60b0c2ff 100644
--- a/.github/workflows/conda-forge-ci.yml
+++ b/.github/workflows/conda-forge-ci.yml
@@ -35,7 +35,7 @@ jobs:
         # Compilation related dependencies
         mamba install cmake compilers make ninja pkg-config
         # Actual dependencies
-        mamba install idyntree yarp libmatio matio-cpp lie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog catch2 nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl tomlplusplus
+        mamba install idyntree yarp libmatio matio-cpp lie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog catch2 nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl tomlplusplus unicycle-footstep-planner
 
     - name: Linux-only Dependencies [Linux]
       if: contains(matrix.os, 'ubuntu')

From 3f5cbc09f346061ba45f9ddb09e90c3dd4fdf20f Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Mon, 14 Jun 2021 11:02:20 +0200
Subject: [PATCH 08/15] Print Contact in scientific notation, if needed

---
 bindings/python/Contacts/src/Contacts.cpp | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/bindings/python/Contacts/src/Contacts.cpp b/bindings/python/Contacts/src/Contacts.cpp
index 82dfce2116..aad74be81f 100644
--- a/bindings/python/Contacts/src/Contacts.cpp
+++ b/bindings/python/Contacts/src/Contacts.cpp
@@ -9,11 +9,13 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
+#include <iomanip>
+
 #include <BipedalLocomotion/Contacts/Contact.h>
 #include <BipedalLocomotion/Contacts/ContactList.h>
+#include <BipedalLocomotion/Contacts/ContactListJsonParser.h>
 #include <BipedalLocomotion/Contacts/ContactPhase.h>
 #include <BipedalLocomotion/Contacts/ContactPhaseList.h>
-#include <BipedalLocomotion/Contacts/ContactListJsonParser.h>
 
 #include <BipedalLocomotion/bindings/Contacts/Contacts.h>
 
@@ -26,8 +28,6 @@ namespace Contacts
 
 std::string toString(const BipedalLocomotion::Contacts::PlannedContact& contact)
 {
-    std::string description;
-
     const Eigen::IOFormat FormatEigenVector //
         (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
 
@@ -38,12 +38,13 @@ std::string toString(const BipedalLocomotion::Contacts::PlannedContact& contact)
     pose << "SE3 (position = " << position.format(FormatEigenVector)
          << ", quaternion = " << quaternion.format(FormatEigenVector) << ")";
 
-    description = "Contact (name = " + contact.name + ", pose = " + pose.str()
-                  + ", activation_time = " + std::to_string(contact.activationTime)
-                  + ", deactivation_time = " + std::to_string(contact.deactivationTime)
-                  + ", type = " + std::to_string(static_cast<int>(contact.type)) + ")";
+    std::stringstream description;
+    description << "Contact (name = " << contact.name << ", pose = " << pose.str()
+                << std::setprecision(7) << ", activation_time = " << contact.activationTime
+                << ", deactivation_time = " << contact.deactivationTime
+                << ", type = " << static_cast<int>(contact.type) << ")";
 
-    return description;
+    return description.str();
 }
 
 void CreateContact(pybind11::module& module)

From 7e1a3a37fabc902a01985909593c047a6f691c93 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Wed, 4 Aug 2021 16:08:14 +0200
Subject: [PATCH 09/15] Add documentation

---
 .../Planners/UnicyclePlanner.h                | 56 +++++++++++++++----
 1 file changed, 46 insertions(+), 10 deletions(-)

diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
index f7ec4a9bc5..4c3cac59a8 100644
--- a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
+++ b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
@@ -57,13 +57,13 @@ struct BipedalLocomotion::Planners::UnicycleKnot
                && this->time == rhs.time;
     }
 
-    double x;
-    double y;
+    double x; ///< The knot x coordinates.
+    double y; ///< The knot y coordinates.
 
-    double dx = 0.0;
-    double dy = 0.0;
+    double dx = 0.0; ///< The knot x velocity.
+    double dy = 0.0; ///< The knot y velocity.
 
-    double time = 0.0;
+    double time = 0.0; ///< The knot activation time.
 };
 
 struct BipedalLocomotion::Planners::UnicyclePlannerInput
@@ -77,10 +77,10 @@ struct BipedalLocomotion::Planners::UnicyclePlannerInput
     {
     }
 
-    double t0;
-    double tf;
+    double t0; ///< The beginning of the planner horizon.
+    double tf; ///< The end of the planner horizon.
 
-    std::vector<UnicycleKnot> knots;
+    std::vector<UnicycleKnot> knots; ///< A list of knots.
 };
 
 struct BipedalLocomotion::Planners::UnicyclePlannerOutput
@@ -92,8 +92,8 @@ struct BipedalLocomotion::Planners::UnicyclePlannerOutput
     {
     }
 
-    Contacts::ContactList left;
-    Contacts::ContactList right;
+    Contacts::ContactList left; ///< The list of left foot contacts;
+    Contacts::ContactList right; ///< The list of right foot contacts;
 };
 
 class BipedalLocomotion::Planners::UnicyclePlanner final
@@ -104,7 +104,43 @@ class BipedalLocomotion::Planners::UnicyclePlanner final
 
     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 |
+     * |  `lastStepSwitchTime`  |     double     | `nominalDuration` |    No     |        The switching time of the last step         |
+     * |      `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<const ParametersHandler::IParametersHandler> handler) override;
+    // clang-format on
 
     const UnicyclePlannerOutput& getOutput() const override;
 

From e9c785a958ecee8560b3242d03aae4631e43c194 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Tue, 3 Aug 2021 14:54:27 +0200
Subject: [PATCH 10/15] Fix warning on non-virtual Advanceable destructor

---
 src/System/include/BipedalLocomotion/System/Advanceable.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/System/include/BipedalLocomotion/System/Advanceable.h b/src/System/include/BipedalLocomotion/System/Advanceable.h
index 983922b0b3..3349a1a0d9 100644
--- a/src/System/include/BipedalLocomotion/System/Advanceable.h
+++ b/src/System/include/BipedalLocomotion/System/Advanceable.h
@@ -36,6 +36,8 @@ template <class _Input, class _Output> class BipedalLocomotion::System::Advancea
     using Input = _Input;
     using Output = _Output;
 
+    virtual ~Advanceable() = default;
+
     /**
      * @brief Initialize the advanceable
      * @note the default implementation does nothing.

From 9e52d7a2eec01e7f5179dc0c09438e518d6adce4 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Thu, 5 Aug 2021 10:08:34 +0200
Subject: [PATCH 11/15] Update CHANGELOG.md

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 590052602a..c37a9fa347 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,6 +7,7 @@ All notable changes to this project are documented in this file.
 - Implement `ContactPhaseList::getPresentPhase()` method (https://github.com/dic-iit/bipedal-locomotion-framework/pull/396)
 - Add a synchronization mechanism for the `AdvanceableRunner` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/403)
 - Add the possibility to use spdlog with YARP (https://github.com/ami-iit/bipedal-locomotion-framework/pull/408)
+- Add new Advanceable exposing `UnicyclePlanner` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/320)
 
 ### Changed
 - Add `name` parameter to the `AdvanceableRunner` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/406)

From 658811650282a9d8831cc670d2cb436f7e0d5c64 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Fri, 10 Sep 2021 11:57:37 +0200
Subject: [PATCH 12/15] Allow passing initial contacts state

---
 .../python/Planners/src/UnicyclePlanner.cpp   |  10 +-
 .../Planners/UnicyclePlanner.h                |   9 +-
 src/Planners/src/UnicyclePlanner.cpp          | 200 +++++++++++++-----
 3 files changed, 168 insertions(+), 51 deletions(-)

diff --git a/bindings/python/Planners/src/UnicyclePlanner.cpp b/bindings/python/Planners/src/UnicyclePlanner.cpp
index 06ce7299ab..55950579ff 100644
--- a/bindings/python/Planners/src/UnicyclePlanner.cpp
+++ b/bindings/python/Planners/src/UnicyclePlanner.cpp
@@ -37,12 +37,20 @@ void CreateUnicyclePlanner(pybind11::module& module)
         .def("__eq__", &UnicycleKnot::operator==, py::is_operator());
 
     py::class_<UnicyclePlannerInput>(module, "UnicyclePlannerInput")
-        .def(py::init<const std::vector<UnicycleKnot>&, const double, const double>(),
+        .def(py::init<const std::vector<UnicycleKnot>&,
+                      const double,
+                      const std::optional<Contacts::PlannedContact>&,
+                      const std::optional<Contacts::PlannedContact>&,
+                      const double>(),
              py::arg("knots") = std::vector<UnicycleKnot>{},
              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);
 
     py::class_<UnicyclePlannerOutput>(module, "UnicyclePlannerOutput")
diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
index 4c3cac59a8..0327311215 100644
--- a/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
+++ b/src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h
@@ -16,6 +16,7 @@
 #include <Eigen/Dense>
 
 #include <memory>
+#include <optional>
 
 namespace BipedalLocomotion::Planners
 {
@@ -70,9 +71,13 @@ struct BipedalLocomotion::Planners::UnicyclePlannerInput
 {
     UnicyclePlannerInput(const std::vector<UnicycleKnot>& _knots,
                          const double _tf = 0.0,
+                         const std::optional<Contacts::PlannedContact>& _initialLeftContact = {},
+                         const std::optional<Contacts::PlannedContact>& _initialRightContact = {},
                          const double _t0 = 0.0)
         : t0(_t0)
         , tf(_tf)
+        , initialLeftContact(_initialLeftContact)
+        , initialRightContact(_initialRightContact)
         , knots(_knots)
     {
     }
@@ -80,6 +85,9 @@ struct BipedalLocomotion::Planners::UnicyclePlannerInput
     double t0; ///< The beginning of the planner horizon.
     double tf; ///< The end of the planner horizon.
 
+    std::optional<Contacts::PlannedContact> initialLeftContact; ///< Left contact initialization
+    std::optional<Contacts::PlannedContact> initialRightContact; ///< Right contact initialization
+
     std::vector<UnicycleKnot> knots; ///< A list of knots.
 };
 
@@ -129,7 +137,6 @@ class BipedalLocomotion::Planners::UnicyclePlanner final
      * |  `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 |
-     * |  `lastStepSwitchTime`  |     double     | `nominalDuration` |    No     |        The switching time of the last step         |
      * |      `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       |
diff --git a/src/Planners/src/UnicyclePlanner.cpp b/src/Planners/src/UnicyclePlanner.cpp
index 52847731d7..52336ab4d9 100644
--- a/src/Planners/src/UnicyclePlanner.cpp
+++ b/src/Planners/src/UnicyclePlanner.cpp
@@ -11,11 +11,12 @@
 #include <FootPrint.h>
 #include <UnicycleGenerator.h>
 #include <UnicyclePlanner.h>
+#include <iDynTree/Core/EigenHelpers.h>
 #include <iDynTree/Core/VectorFixSize.h>
+#include <manif/SE3.h>
 
 #include <cassert>
 #include <limits>
-#include <optional>
 
 using namespace BipedalLocomotion;
 
@@ -82,7 +83,6 @@ class Planners::UnicyclePlanner::Impl
     struct
     {
         double stancePhaseRatio;
-        double lastStepSwitchTime;
         bool startWithLeft = false;
         bool terminalStep = true;
         bool resetStartingFootIfStill = false;
@@ -94,6 +94,12 @@ class Planners::UnicyclePlanner::Impl
         std::string right = "right";
     } names;
 
+    struct
+    {
+        std::optional<Contacts::PlannedContact> left;
+        std::optional<Contacts::PlannedContact> right;
+    } initialContacts;
+
     UnicyclePlannerOutput outputRef;
     std::optional<UnicyclePlannerOutput> output = std::nullopt;
 
@@ -304,14 +310,6 @@ bool Planners::UnicyclePlanner::initialize(
         return false;
     }
 
-    if (!ptr->getParameter("lastStepSwitchTime", m_pImpl->gait.lastStepSwitchTime))
-    {
-        m_pImpl->gait.lastStepSwitchTime = m_pImpl->duration.nominal;
-        log()->info("{} Using default lastStepSwitchTime={}.",
-                    logPrefix,
-                    m_pImpl->gait.lastStepSwitchTime);
-    }
-
     if (!ptr->getParameter("swingLeft", m_pImpl->gait.startWithLeft))
     {
         log()->info("{} Using default swingLeft={}.", logPrefix, m_pImpl->gait.startWithLeft);
@@ -451,6 +449,9 @@ bool Planners::UnicyclePlanner::setInput(const UnicyclePlannerInput& input)
     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;
 }
 
@@ -464,7 +465,19 @@ bool Planners::UnicyclePlanner::advance()
         return false;
     }
 
-    m_pImpl->output = std::nullopt;
+    // 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<FootPrint>();
@@ -474,31 +487,96 @@ bool Planners::UnicyclePlanner::advance()
     m_pImpl->right = std::make_shared<FootPrint>();
     m_pImpl->right->setFootName(m_pImpl->names.right);
 
-    // Compute the FootPrints
+    // 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],
+                               contact->activationTime);
+
+        const double impactTime = contact->activationTime;
+        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],
+                                contact->activationTime);
+
+        const double impactTime = contact->activationTime;
+        initTime = impactTime > initTime ? impactTime : initTime;
+    }
+
     if (!m_pImpl->planner->computeNewSteps(m_pImpl->left,
                                            m_pImpl->right,
-                                           m_pImpl->horizon.t0,
+                                           initTime,
                                            m_pImpl->horizon.tf))
     {
-        m_pImpl->left = nullptr;
-        m_pImpl->right = nullptr;
+        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);
-    double lastStepSwitchTime = 0.8;
-    generator.setTerminalHalfSwitchTime(lastStepSwitchTime);
     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,
-                                          m_pImpl->horizon.t0,
+                                          startTime,
                                           m_pImpl->dt.planner))
     {
+        cleanup();
         log()->error("{} Failed to generate from footprints.", logPrefix);
         return false;
     }
@@ -508,64 +586,84 @@ bool Planners::UnicyclePlanner::advance()
     std::vector<bool> rightStandingPeriod;
     generator.getFeetStandingPeriods(leftStandingPeriod, rightStandingPeriod);
 
-    // Lambda to convert the vector with contact states to a vector of PlannedContact.
-    // It just processes timings, we fill the transform later.
-    auto processStandingPeriod
-        = [](const std::vector<bool>& isFootInContactVector,
-             const double dt,
-             const std::string& contactName) -> std::vector<Contacts::PlannedContact> {
+    // 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<bool>& isFootInContactVector,
+               const StepList& steps) -> std::vector<Contacts::PlannedContact> {
         std::vector<Contacts::PlannedContact> contacts;
 
-        // Loop over all the horizon
-        for (size_t i = 1; i < isFootInContactVector.size(); ++i)
+        for (const auto& step : steps)
         {
-            const bool thisState = isFootInContactVector[i];
-            const bool lastState = isFootInContactVector[i - 1];
-            const bool aboutToLand = lastState == 0 && thisState == 1;
+            auto contact = Contacts::PlannedContact();
+            contact.name = step.footName;
+            contact.activationTime = step.impactTime;
+            contact.deactivationTime = contact.activationTime;
+            contacts.push_back(contact);
+        }
 
-            // Create a new contact
-            if (i == 1 || aboutToLand)
-            {
-                auto contact = Contacts::PlannedContact();
-                contact.name = contactName;
-                contact.activationTime = dt * i;
-                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];
 
-            // Adjust the first contact activation
-            if (i == 1)
-                contacts.back().activationTime -= dt;
+            const bool thisState = isFootInContactVector[idx];
+            const bool lastState = isFootInContactVector[idx - 1];
 
-            // Upate the contact duration
-            if (thisState)
-                contacts.back().deactivationTime += dt;
+            // 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)
+            {
+                contact.deactivationTime = contact.activationTime + activeTime;
+                contactIdx++;
+            }
         }
 
+        // The deactivation time of the last contact has not yet been processed
+        contacts.back().deactivationTime = std::numeric_limits<float>().max();
+
         return contacts;
     };
 
-    // Get the feet contacts with only timings
+    // Convert Step objects to PlannedContact objects
     std::vector<Contacts::PlannedContact> leftContacts
-        = processStandingPeriod(leftStandingPeriod, m_pImpl->dt.planner, m_pImpl->names.left);
+        = convertStepsToContacts(leftStandingPeriod, m_pImpl->left->getSteps());
     std::vector<Contacts::PlannedContact> rightContacts
-        = processStandingPeriod(rightStandingPeriod, m_pImpl->dt.planner, m_pImpl->names.right);
+        = 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())
     {
-        log()->error("{} Wrong number of converted steps for left foot.", logPrefix);
+        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::PlannedContact>& contacts,
-                                   decltype(::FootPrint().getSteps())& steps) {
+                                   decltype(::FootPrint().getSteps())& steps) -> void {
         assert(contacts.size() == steps.size());
         for (size_t i = 0; i < contacts.size(); ++i)
         {
@@ -578,6 +676,10 @@ bool Planners::UnicyclePlanner::advance()
     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::PlannedContact>& contacts) -> Contacts::ContactList {

From 09233ad088cb3b0ba069d963f646654b899383fa Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Thu, 26 Aug 2021 17:31:44 +0200
Subject: [PATCH 13/15] Fix cleaning up when output is not valid

---
 src/Planners/src/UnicyclePlanner.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/Planners/src/UnicyclePlanner.cpp b/src/Planners/src/UnicyclePlanner.cpp
index 52336ab4d9..9836f72185 100644
--- a/src/Planners/src/UnicyclePlanner.cpp
+++ b/src/Planners/src/UnicyclePlanner.cpp
@@ -365,6 +365,7 @@ const Planners::UnicyclePlannerOutput& Planners::UnicyclePlanner::getOutput() co
     if (!this->isOutputValid())
     {
         log()->warn("{} Returning an empty output.", logPrefix);
+        m_pImpl->outputRef = {};
         return m_pImpl->outputRef;
     }
 

From 6285b8c0eadab7cfad1a96f6c663d1e5b2034263 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Thu, 26 Aug 2021 17:33:12 +0200
Subject: [PATCH 14/15] Update gitignore

---
 .gitignore | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.gitignore b/.gitignore
index 0b9ab1fdeb..d5aaa3ef7f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,5 @@
-# build folder
-build/*
+# build folders
+build*
 
 # emacs
 *~

From 331c9199aca85fdbd77127ea27d04588c021c66c Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Thu, 16 Sep 2021 12:24:13 +0200
Subject: [PATCH 15/15] Add __repr__ methods to pretty print objects in Python

---
 .../python/Planners/src/UnicyclePlanner.cpp   | 37 +++++++++++++++++--
 1 file changed, 34 insertions(+), 3 deletions(-)

diff --git a/bindings/python/Planners/src/UnicyclePlanner.cpp b/bindings/python/Planners/src/UnicyclePlanner.cpp
index 55950579ff..6e6b0803f3 100644
--- a/bindings/python/Planners/src/UnicyclePlanner.cpp
+++ b/bindings/python/Planners/src/UnicyclePlanner.cpp
@@ -34,7 +34,16 @@ void CreateUnicyclePlanner(pybind11::module& module)
         .def_readwrite("y", &UnicycleKnot::y)
         .def_readwrite("dx", &UnicycleKnot::dx)
         .def_readwrite("dy", &UnicycleKnot::dy)
-        .def("__eq__", &UnicycleKnot::operator==, py::is_operator());
+        .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_<UnicyclePlannerInput>(module, "UnicyclePlannerInput")
         .def(py::init<const std::vector<UnicycleKnot>&,
@@ -51,14 +60,36 @@ void CreateUnicyclePlanner(pybind11::module& module)
         .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_readwrite("knots", &UnicyclePlannerInput::knots)
+        .def("__repr__", [](const UnicyclePlannerInput& i) {
+            auto printContact
+                = [](const std::optional<Contacts::PlannedContact>& 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_<UnicyclePlannerOutput>(module, "UnicyclePlannerOutput")
         .def(py::init<const Contacts::ContactList&, const Contacts::ContactList&>(),
              py::arg("left") = Contacts::ContactList(),
              py::arg("right") = Contacts::ContactList())
         .def_readwrite("left", &UnicyclePlannerOutput::left)
-        .def_readwrite("right", &UnicyclePlannerOutput::right);
+        .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) + //
+                   ")";
+        });
 
     py::class_<Advanceable<UnicyclePlannerInput, UnicyclePlannerOutput>>( //
         module,