diff --git a/CMakeLists.txt b/CMakeLists.txt
index 46c60cc..812313c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.1)
 project(UnicyclePlanner
         LANGUAGES CXX
-        VERSION 0.4.3)
+        VERSION 0.5.0)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html
@@ -54,7 +54,9 @@ find_package(Eigen3 REQUIRED)
 
 set(UCPLANNER_SOURCES src/ControlledUnicycle.cpp
                       src/UnicyclePlanner.cpp
-                      src/UnicycleController.cpp
+                      src/UnicycleBaseController.cpp
+                      src/PersonFollowingController.cpp
+                      src/UnicycleDirectController.cpp
                       src/UnicycleOptimization.cpp
                       src/UnicycleFoot.cpp
                       src/FootPrint.cpp
@@ -69,7 +71,9 @@ set(UCPLANNER_SOURCES src/ControlledUnicycle.cpp
                       src/FreeSpaceEllipse.cpp)
 
 set(UCPLANNER_HEADERS include/ControlledUnicycle.h
-                      include/UnicycleController.h
+                      include/UnicycleBaseController.h
+                      include/PersonFollowingController.h
+                      include/UnicycleDirectController.h
                       include/UnicycleOptimization.h
                       include/UnicycleFoot.h
                       include/FootPrint.h
diff --git a/include/ControlledUnicycle.h b/include/ControlledUnicycle.h
index 5d9cab3..0214c13 100644
--- a/include/ControlledUnicycle.h
+++ b/include/ControlledUnicycle.h
@@ -9,20 +9,21 @@
 #define CONTROLLEDUNICYCLE_H
 
 #include <iDynTree/DynamicalSystem.h>
-#include <UnicycleController.h>
+#include <iDynTree/Controller.h>
 #include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/VectorFixSize.h>
 #include <memory>
 
 class ControlledUnicycle : public iDynTree::optimalcontrol::DynamicalSystem{
     iDynTree::VectorDynSize m_controllerOutput, m_initialState;
-    std::shared_ptr<UnicyleController> m_controller_ptr;
+    std::shared_ptr<iDynTree::optimalcontrol::Controller> m_controller_ptr;
 
 public:
 
     ControlledUnicycle();
 
     //the state is [x, theta], i.e. the 2D position of the cart and the angle wrt Z axis;
-    //the controller is [u;w]
+    //the controller is [u;w;v], i.e. forward speed, angular velocity, lateral speed.
 
     bool dynamics(const iDynTree::VectorDynSize &state, double time, iDynTree::VectorDynSize &stateDynamics) override;
 
@@ -30,7 +31,7 @@ class ControlledUnicycle : public iDynTree::optimalcontrol::DynamicalSystem{
 
     const iDynTree::VectorDynSize& initialState() const override;
 
-    bool setController(std::shared_ptr<UnicyleController> controller);
+    bool setController(std::shared_ptr<iDynTree::optimalcontrol::Controller> controller);
 };
 
 
diff --git a/include/UnicycleController.h b/include/PersonFollowingController.h
similarity index 69%
rename from include/UnicycleController.h
rename to include/PersonFollowingController.h
index e2c5866..1381010 100644
--- a/include/UnicycleController.h
+++ b/include/PersonFollowingController.h
@@ -5,16 +5,15 @@
  *
  */
 
-#ifndef UNICYCLECONTROLLER_H
-#define UNICYCLECONTROLLER_H
+#ifndef PERSONFOLLOWINGCONTROLLER_H
+#define PERSONFOLLOWINGCONTROLLER_H
 
 #include "FreeSpaceEllipse.h"
-#include <iDynTree/Controller.h>
+#include "UnicycleBaseController.h"
 #include <iDynTree/TimeRange.h>
 #include <iDynTree/Core/VectorFixSize.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
-#include <memory>
 #include <deque>
 
 typedef struct{
@@ -23,37 +22,31 @@ typedef struct{
     iDynTree::Vector2 yDotDesired;
 } TrajectoryPoint;
 
-class UnicyleController : public iDynTree::optimalcontrol::Controller{
+class PersonFollowingController : public UnicycleBaseController{
     iDynTree::Vector2 m_personDistance, m_y, m_personPosition, m_unicyclePosition;
     double m_personDistanceNorm;
     double m_theta;
     iDynTree::MatrixDynSize m_inverseB, m_R;
     std::deque<TrajectoryPoint> m_desiredTrajectory;
-    double m_gain, m_maxVelocity, m_maxAngularVelocity, m_time;
-    double m_slowWhenTurnGain;
-    double m_slowWhenBackwardFactor;
+    double m_gain, m_time;
     double m_semiMajorInnerEllipseOffset;
     double m_semiMinorInnerEllipseOffset;
     FreeSpaceEllipse m_outerEllipse, m_innerEllipse;
     double m_conservativeFactor;
 
-    double saturate(double input, double saturation);
-
-    double saturate(double input, double positiveSaturation, double negativeSaturation);
-
     void interpolateReferences(double time,
                                const std::deque<TrajectoryPoint>::reverse_iterator& point,
                                iDynTree::Vector2& yOutput, iDynTree::Vector2 &yDotOutput);
 
 public:
-    UnicyleController();
+    PersonFollowingController();
 
     //the state is [y, x, theta], i.e. the 2D position of the point to be followed, the 2D position of the cart and the angle wrt Z axis;
-    //the controller is [u;w]
+    //the controller is [u;w;v], i.e. forward speed, angular velocity, lateral speed.
 
-    bool doControl(iDynTree::VectorDynSize &controllerOutput) override;
+    virtual bool doUnicycleControl(double& forwardSpeed, double& angularVelocity, double& lateralVelocity) override;
 
-    bool setStateFeedback(const double t, const iDynTree::VectorDynSize &stateFeedback) override;
+    virtual bool setUnicycleStateFeedback(const double t, const iDynTree::Vector2& unicyclePosition, double unicycleOrientation) override;
 
     bool setPersonDistance(double xPosition, double yPosition);
 
@@ -63,12 +56,6 @@ class UnicyleController : public iDynTree::optimalcontrol::Controller{
 
     bool setGain(double controllerGain);
 
-    bool setSaturations(double maxVelocity, double maxAngularVelocity);
-
-    bool setSlowWhenTurnGain(double slowWhenTurnGain); //if >0 the unicycle progress more slowly when also turning.
-
-    bool setSlowWhenBackwardFactor(double slowWhenBackwardFactor); //if >0 the unicycle progress more slowly when going backward. It is a multiplicative gain
-
     bool setDesiredPoint(const TrajectoryPoint &desiredPoint);
 
     bool getDesiredPoint(double time, iDynTree::Vector2& yDesired, iDynTree::Vector2& yDotDesired);
@@ -90,4 +77,4 @@ class UnicyleController : public iDynTree::optimalcontrol::Controller{
     bool setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset);
 };
 
-#endif // UNICYCLECONTROLLER_H
+#endif // PERSONFOLLOWINGCONTROLLER_H
diff --git a/include/UnicycleBaseController.h b/include/UnicycleBaseController.h
new file mode 100644
index 0000000..7729c22
--- /dev/null
+++ b/include/UnicycleBaseController.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 20022 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Stefano Dafarra
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#ifndef UNICYCLEBASECONTROLLER_H
+#define UNICYCLEBASECONTROLLER_H
+
+#include <iDynTree/Controller.h>
+#include <iDynTree/Core/VectorFixSize.h>
+#include <iDynTree/Core/VectorDynSize.h>
+
+class UnicycleBaseController : public iDynTree::optimalcontrol::Controller{
+
+    double m_maxLinearVelocity, m_maxAngularVelocity;
+    double m_slowWhenTurnGain;
+    double m_slowWhenBackwardFactor;
+    double m_slowWhenSidewaysFactor;
+
+    double saturate(double input, double saturation);
+
+    double saturate(double input, double positiveSaturation, double negativeSaturation);
+
+public:
+
+    UnicycleBaseController();
+
+    virtual bool doUnicycleControl(double& forwardSpeed, double& angularVelocity, double& lateralVelocity) = 0;
+
+    virtual bool setUnicycleStateFeedback(const double t, const iDynTree::Vector2& unicyclePosition, double unicycleOrientation) = 0;
+
+    bool doControl(iDynTree::VectorDynSize &controllerOutput) final;
+
+    bool setStateFeedback(const double t, const iDynTree::VectorDynSize &stateFeedback) final;
+
+    bool setSaturations(double maxLinearVelocity, double maxAngularVelocity);
+
+    bool setSlowWhenTurnGain(double slowWhenTurnGain); //if >0 the unicycle progress more slowly when also turning.
+
+    bool setSlowWhenBackwardFactor(double slowWhenBackwardFactor); //if >0 the unicycle progress more slowly when going backward. It is a multiplicative gain
+
+    bool setSlowWhenSidewaysFactor(double slowWhenSidewaysFactor); //if >0 the unicycle progress more slowly when going backward. It is a multiplicative gain
+
+};
+
+#endif // UNICYCLEBASECONTROLLER_H
diff --git a/include/UnicycleDirectController.h b/include/UnicycleDirectController.h
new file mode 100644
index 0000000..423cb62
--- /dev/null
+++ b/include/UnicycleDirectController.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 20022 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Stefano Dafarra
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#ifndef UNICYCLEDIRECTCONTROLLER_H
+#define UNICYCLEDIRECTCONTROLLER_H
+
+#include "UnicycleBaseController.h"
+
+class UnicycleDirectController : public UnicycleBaseController
+{
+
+    double m_desiredForwardSpeed, m_desiredAngularVelocity, m_desiredLateralVelocity;
+    double m_time;
+    double m_deactivationEndTime;
+
+public:
+
+    UnicycleDirectController();
+
+    virtual bool doUnicycleControl(double& forwardSpeed, double& angularVelocity, double& lateralVelocity);
+
+    virtual bool setUnicycleStateFeedback(const double t, const iDynTree::Vector2& , double );
+
+    void setConstantControl(double forwardSpeed, double angularVelocity, double lateralVelocity);
+
+    void setInactiveUntil(double endTime);
+
+    double desiredLateralVelocity() const;
+
+};
+
+#endif // UNICYCLEDIRECTCONTROLLER_H
diff --git a/include/UnicyclePlanner.h b/include/UnicyclePlanner.h
index ecd7b42..23aa3d5 100644
--- a/include/UnicyclePlanner.h
+++ b/include/UnicyclePlanner.h
@@ -9,7 +9,8 @@
 #define UNICYCLEPLANNER_H
 
 #include "ControlledUnicycle.h"
-#include "UnicycleController.h"
+#include "PersonFollowingController.h"
+#include "UnicycleDirectController.h"
 #include "UnicycleOptimization.h"
 #include "UnicycleFoot.h"
 #include "FreeSpaceEllipse.h"
@@ -24,8 +25,16 @@ enum class FreeSpaceEllipseMethod
     REFERENCE_AND_FOOTSTEPS
 };
 
+enum class UnicycleController
+{
+    PERSON_FOLLOWING,
+    DIRECT
+};
+
 class UnicyclePlanner {
-    std::shared_ptr<UnicyleController> m_controller;
+    std::shared_ptr<PersonFollowingController> m_personFollowingController;
+    std::shared_ptr<UnicycleDirectController> m_directController;
+    UnicycleController m_currentController;
     std::shared_ptr<ControlledUnicycle> m_unicycle;
     iDynTree::optimalcontrol::integrators::ForwardEuler m_integrator;
     UnicycleOptimization m_unicycleProblem;
@@ -33,6 +42,7 @@ class UnicyclePlanner {
     bool m_addTerminalStep, m_startLeft, m_resetStartingFoot, m_firstStep;
     FreeSpaceEllipseMethod m_freeSpaceMethod;
     double m_leftYawOffset, m_rightYawOffset;
+    double m_linearVelocityConservativeFactor, m_angularVelocityConservativeFactor;
     std::mutex m_mutex;
 
     std::shared_ptr<UnicycleFoot> m_left, m_right;
@@ -57,20 +67,41 @@ class UnicyclePlanner {
     //Controller inputs
     bool setDesiredPersonDistance(double xPosition, double yPosition);
 
+    [[deprecated("Use setPersonFollowingControllerGain instead.")]]
     bool setControllerGain(double controllerGain); //optional
 
+    bool setPersonFollowingControllerGain(double controllerGain); //optional
+
     bool setSlowWhenTurnGain(double slowWhenTurnGain); //if >0 the unicycle progress more slowly when also turning.
 
     bool setSlowWhenBackwardFactor(double slowWhenBackwardFactor); //if >0 the unicycle progress more slowly when going backward. It is a multiplicative gain
 
-    bool addDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2& yDesired, const iDynTree::Vector2& yDotDesired); //If two points have the same initTime it is an undefined behavior. It keeps the desired values constant from initTime to the initTime of the next desired point (they are automatically ordered)
+    bool setSlowWhenSidewaysFactor(double slowWhenSidewaysFactor); //if >0 the unicycle progress more slowly when going sideways. It is a multiplicative gain
+
+    [[deprecated("Use addPersonFollowingDesiredTrajectoryPoint instead.")]]
+    bool addDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2& yDesired, const iDynTree::Vector2& yDotDesired);
+
+    bool addPersonFollowingDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2& yDesired, const iDynTree::Vector2& yDotDesired); //If two points have the same initTime it is an undefined behavior. It keeps the desired values constant from initTime to the initTime of the next desired point (they are automatically ordered)
 
+    [[deprecated("Use addPersonFollowingDesiredTrajectoryPoint instead.")]]
     bool addDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2& yDesired);// like the above but assumes zero velocity
 
+    bool addPersonFollowingDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2& yDesired);// like the above but assumes zero velocity
+
+    [[deprecated("Use clearPersonFollowingDesiredTrajectory instead.")]]
     void clearDesiredTrajectory();
 
+    void clearPersonFollowingDesiredTrajectory();
+
+    [[deprecated("Use clearPersonFollowingDesiredTrajectoryUpTo instead.")]]
     bool clearDesiredTrajectoryUpTo(double time);
 
+    bool clearPersonFollowingDesiredTrajectoryUpTo(double time);
+
+    void setDesiredDirectControl(double forwardVelocity, double angularVelocity, double lateralVelocity);
+
+    bool setSaturationsConservativeFactors(double linearVelocityConservativeFactor, double angularVelocityConservativeFactor);
+
     //Integrator inputs
     bool setMaximumIntegratorStepSize(double dT);
 
@@ -118,6 +149,8 @@ class UnicyclePlanner {
     bool setInnerFreeSpaceEllipseOffset(double offset);
 
     bool setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset);
+
+    bool setUnicycleController(UnicycleController controller);
 };
 
 #endif // UNICYCLEPLANNER_H
diff --git a/src/ControlledUnicycle.cpp b/src/ControlledUnicycle.cpp
index 16d5e4c..9c64c5b 100644
--- a/src/ControlledUnicycle.cpp
+++ b/src/ControlledUnicycle.cpp
@@ -6,23 +6,22 @@
  */
 
 #include "ControlledUnicycle.h"
-#include "UnicycleController.h"
 #include <cmath>
 #include <memory>
 #include <iostream>
 
 ControlledUnicycle::ControlledUnicycle()
-:iDynTree::optimalcontrol::DynamicalSystem(3, 2)
+:iDynTree::optimalcontrol::DynamicalSystem(3, 3)
 , m_controller_ptr(nullptr)
 {
     m_initialState.resize(3);
     m_initialState.zero();
-    m_controllerOutput.resize(2);
+    m_controllerOutput.resize(3);
     m_controllerOutput.zero();
 }
 
 //the state is [x, theta], i.e. the 2D position of the point to be followed, the 2D position of the cart and the angle wrt Z axis;
-//the controller is [u;w]
+//the controller is [u;w;v]
 
 bool ControlledUnicycle::dynamics(const iDynTree::VectorDynSize &state, double time, iDynTree::VectorDynSize &stateDynamics)
 {
@@ -53,8 +52,8 @@ bool ControlledUnicycle::dynamics(const iDynTree::VectorDynSize &state, double t
     double c_theta = std::cos(theta);
     double s_theta = std::sin(theta);
 
-    stateDynamics(0) = c_theta * m_controllerOutput(0);
-    stateDynamics(1) = s_theta * m_controllerOutput(0);
+    stateDynamics(0) = c_theta * m_controllerOutput(0) - s_theta * m_controllerOutput(2);
+    stateDynamics(1) = s_theta * m_controllerOutput(0) + c_theta * m_controllerOutput(2);
     stateDynamics(2) = m_controllerOutput(1);
 
     return true;
@@ -75,7 +74,7 @@ const iDynTree::VectorDynSize &ControlledUnicycle::initialState() const
     return m_initialState;
 }
 
-bool ControlledUnicycle::setController(std::shared_ptr<UnicyleController> controller)
+bool ControlledUnicycle::setController(std::shared_ptr<iDynTree::optimalcontrol::Controller> controller)
 {
     if (controller->controlSpaceSize() != controlSpaceSize()){
         std::cerr << "The controller dimension is not coherent with the controlSpaceSize." << std::endl;
diff --git a/src/UnicycleController.cpp b/src/PersonFollowingController.cpp
similarity index 70%
rename from src/UnicycleController.cpp
rename to src/PersonFollowingController.cpp
index c2f4198..057f857 100644
--- a/src/UnicycleController.cpp
+++ b/src/PersonFollowingController.cpp
@@ -5,31 +5,14 @@
  *
  */
 
-#include "UnicycleController.h"
+#include "PersonFollowingController.h"
 #include "iDynTree/Core/EigenHelpers.h"
 #include "Eigen/Core"
 #include <iostream>
 #include <cmath>
 #include <algorithm>
 
-double UnicyleController::saturate(double input, double saturation)
-{
-    return saturate(input, saturation, -saturation);
-}
-
-double UnicyleController::saturate(double input, double positiveSaturation, double negativeSaturation)
-{
-    if ((positiveSaturation < 0) || (negativeSaturation > positiveSaturation))
-        return input;
-
-    if (input > positiveSaturation)
-        return positiveSaturation;
-    else if (input < negativeSaturation)
-        return negativeSaturation;
-    else return input;
-}
-
-void UnicyleController::interpolateReferences(double time,
+void PersonFollowingController::interpolateReferences(double time,
                                               const std::deque<TrajectoryPoint>::reverse_iterator &point,
                                               iDynTree::Vector2 &yOutput, iDynTree::Vector2 &yDotOutput)
 {
@@ -41,15 +24,10 @@ void UnicyleController::interpolateReferences(double time,
 
 }
 
-UnicyleController::UnicyleController()
-    :iDynTree::optimalcontrol::Controller(2)
-    ,m_theta(0)
+PersonFollowingController::PersonFollowingController()
+    :m_theta(0)
     ,m_gain(10)
-    ,m_maxVelocity(-1) //negative value -> don't saturate
-    ,m_maxAngularVelocity(-1) //negative value -> don't saturate
     ,m_time(0)
-    ,m_slowWhenTurnGain(0.0)
-    ,m_slowWhenBackwardFactor(1.0)
     ,m_semiMajorInnerEllipseOffset(0.0)
     ,m_semiMinorInnerEllipseOffset(0.0)
     ,m_conservativeFactor(2.0)
@@ -77,11 +55,8 @@ UnicyleController::UnicyleController()
 
 }
 
-bool UnicyleController::doControl(iDynTree::VectorDynSize &controllerOutput)
+bool PersonFollowingController::doUnicycleControl(double &forwardSpeed, double &angularVelocity, double &lateralVelocity)
 {
-    if(controllerOutput.size() != this->controlSpaceSize())
-        controllerOutput.resize(this->controlSpaceSize());
-
     double s_theta = std::sin(m_theta);
     double c_theta =std::cos(m_theta);
     double dx = m_personDistance(0);
@@ -99,32 +74,30 @@ bool UnicyleController::doControl(iDynTree::VectorDynSize &controllerOutput)
         return false;
     }
 
-    iDynTree::toEigen(controllerOutput) = iDynTree::toEigen(m_inverseB) * (iDynTree::toEigen(yDotDesired) -
-                                          m_gain * (iDynTree::toEigen(m_y) - iDynTree::toEigen(yDesired)));
+    Eigen::Vector2d controlOutput = iDynTree::toEigen(m_inverseB) * (iDynTree::toEigen(yDotDesired) -
+                                    m_gain * (iDynTree::toEigen(m_y) - iDynTree::toEigen(yDesired)));
 
-    controllerOutput(1) = saturate(controllerOutput(1), m_maxAngularVelocity);
-    double velocitySaturation = m_maxVelocity / (1.0 + m_slowWhenTurnGain * (std::sqrt(controllerOutput(1) * controllerOutput(1))));
-    controllerOutput(0) = saturate(controllerOutput(0), velocitySaturation, -m_slowWhenBackwardFactor * velocitySaturation);
+    forwardSpeed = controlOutput(0);
+    angularVelocity = controlOutput(1);
+    lateralVelocity = 0.0;
 
     return true;
 }
 
-bool UnicyleController::setStateFeedback(const double t, const iDynTree::VectorDynSize &stateFeedback){
-    if (stateFeedback.size() != 3)
-        return false;
+bool PersonFollowingController::setUnicycleStateFeedback(const double t, const iDynTree::Vector2 &unicyclePosition, double unicycleOrientation)
+{
     m_time = t;
 
-    m_theta = stateFeedback(2);
+    m_theta = unicycleOrientation;
 
-    m_unicyclePosition(0) = stateFeedback(0);
-    m_unicyclePosition(1) = stateFeedback(1);
+    m_unicyclePosition = unicyclePosition;
 
     m_y = getPersonPosition(m_unicyclePosition, m_theta);
 
     return true;
 }
 
-bool UnicyleController::setPersonDistance(double xPosition, double yPosition)
+bool PersonFollowingController::setPersonDistance(double xPosition, double yPosition)
 {
     if(xPosition == 0){
         std::cerr << "The xPosition need to be greater than 0." << std::endl;
@@ -137,12 +110,12 @@ bool UnicyleController::setPersonDistance(double xPosition, double yPosition)
     return true;
 }
 
-const iDynTree::Vector2 &UnicyleController::getPersonDistance() const
+const iDynTree::Vector2 &PersonFollowingController::getPersonDistance() const
 {
     return m_personDistance;
 }
 
-const iDynTree::Vector2 &UnicyleController::getPersonPosition(const iDynTree::Vector2 &unicyclePosition, double unicycleAngle)
+const iDynTree::Vector2 &PersonFollowingController::getPersonPosition(const iDynTree::Vector2 &unicyclePosition, double unicycleAngle)
 {
     double c_theta = std::cos(unicycleAngle);
     double s_theta = std::sin(unicycleAngle);
@@ -157,7 +130,7 @@ const iDynTree::Vector2 &UnicyleController::getPersonPosition(const iDynTree::Ve
     return m_personPosition;
 }
 
-bool UnicyleController::setGain(double controllerGain)
+bool PersonFollowingController::setGain(double controllerGain)
 {
     if (controllerGain <= 0){
         std::cerr << "The controller gain is supposed to be positive." << std::endl;
@@ -167,42 +140,7 @@ bool UnicyleController::setGain(double controllerGain)
     return true;
 }
 
-bool UnicyleController::setSaturations(double maxVelocity, double maxAngularVelocity)
-{
-    if ((maxVelocity < 0)||(maxAngularVelocity < 0)){
-        std::cerr << "The saturations are on the absolute value, thus they need to be non-negative." << std::endl;
-        return false;
-    }
-
-    m_maxVelocity = maxVelocity;
-    m_maxAngularVelocity = maxAngularVelocity;
-
-    return true;
-}
-
-bool UnicyleController::setSlowWhenTurnGain(double slowWhenTurnGain)
-{
-    if (slowWhenTurnGain < 0){
-        std::cerr << "The slowWhenTurn gain is supposed to be non-negative." << std::endl;
-        return false;
-    }
-
-    m_slowWhenTurnGain = slowWhenTurnGain;
-    return true;
-}
-
-bool UnicyleController::setSlowWhenBackwardFactor(double slowWhenBackwardFactor)
-{
-    if (slowWhenBackwardFactor < 0){
-        std::cerr << "The slowWhenBackwardFactor multiplier is supposed to be non-negative." << std::endl;
-        return false;
-    }
-
-    m_slowWhenBackwardFactor = slowWhenBackwardFactor;
-    return true;
-}
-
-bool UnicyleController::setDesiredPoint(const TrajectoryPoint &desiredPoint)
+bool PersonFollowingController::setDesiredPoint(const TrajectoryPoint &desiredPoint)
 {
     if (desiredPoint.initTime < 0){
         std::cerr << "The initTime parameter is supposed to be non-negative." << std::endl;
@@ -216,7 +154,7 @@ bool UnicyleController::setDesiredPoint(const TrajectoryPoint &desiredPoint)
     return true;
 }
 
-bool UnicyleController::getDesiredTrajectoryInitialTime(double &firstTime)
+bool PersonFollowingController::getDesiredTrajectoryInitialTime(double &firstTime)
 {
     if (m_desiredTrajectory.empty()){
         std::cerr << "No trajectory loaded yet." << std::endl;
@@ -227,12 +165,12 @@ bool UnicyleController::getDesiredTrajectoryInitialTime(double &firstTime)
     return true;
 }
 
-void UnicyleController::clearDesiredTrajectory()
+void PersonFollowingController::clearDesiredTrajectory()
 {
     m_desiredTrajectory.clear();
 }
 
-bool UnicyleController::clearDesiredTrajectoryUpTo(double time)
+bool PersonFollowingController::clearDesiredTrajectoryUpTo(double time)
 {
     if (time < 0){
         std::cerr << "The time is expected to be non-negative" <<std::endl;
@@ -262,7 +200,7 @@ bool UnicyleController::clearDesiredTrajectoryUpTo(double time)
     return true;
 }
 
-bool UnicyleController::setFreeSpaceEllipse(const FreeSpaceEllipse &freeSpaceEllipse)
+bool PersonFollowingController::setFreeSpaceEllipse(const FreeSpaceEllipse &freeSpaceEllipse)
 {
     m_outerEllipse = freeSpaceEllipse;
     m_innerEllipse = freeSpaceEllipse;
@@ -286,7 +224,7 @@ bool UnicyleController::setFreeSpaceEllipse(const FreeSpaceEllipse &freeSpaceEll
     return true;
 }
 
-bool UnicyleController::setFreeSpaceEllipseConservativeFactor(double conservativeFactor)
+bool PersonFollowingController::setFreeSpaceEllipseConservativeFactor(double conservativeFactor)
 {
     if (conservativeFactor < 0)
     {
@@ -298,12 +236,12 @@ bool UnicyleController::setFreeSpaceEllipseConservativeFactor(double conservativ
     return true;
 }
 
-bool UnicyleController::setInnerFreeSpaceEllipseOffset(double offset)
+bool PersonFollowingController::setInnerFreeSpaceEllipseOffset(double offset)
 {
     return setInnerFreeSpaceEllipseOffsets(offset, offset);
 }
 
-bool UnicyleController::setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset)
+bool PersonFollowingController::setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset)
 {
     if (semiMajorAxisOffset < 0)
     {
@@ -322,7 +260,7 @@ bool UnicyleController::setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffs
     return setFreeSpaceEllipse(m_outerEllipse);
 }
 
-bool UnicyleController::getDesiredPoint(double time,
+bool PersonFollowingController::getDesiredPoint(double time,
                                         iDynTree::Vector2 &yDesired, iDynTree::Vector2 &yDotDesired)
 {
     if (time < 0){
@@ -361,7 +299,7 @@ bool UnicyleController::getDesiredPoint(double time,
 
 }
 
-bool UnicyleController::getDesiredPointInFreeSpaceEllipse(double time, const iDynTree::Vector2 &unicyclePosition, double unicycleAngle, iDynTree::Vector2 &yDesired, iDynTree::Vector2 &yDotDesired)
+bool PersonFollowingController::getDesiredPointInFreeSpaceEllipse(double time, const iDynTree::Vector2 &unicyclePosition, double unicycleAngle, iDynTree::Vector2 &yDesired, iDynTree::Vector2 &yDotDesired)
 {
     if (!getDesiredPoint(time, yDesired, yDotDesired))
     {
diff --git a/src/UnicycleBaseController.cpp b/src/UnicycleBaseController.cpp
new file mode 100644
index 0000000..b108609
--- /dev/null
+++ b/src/UnicycleBaseController.cpp
@@ -0,0 +1,118 @@
+/*
+ * Copyright (C) 20022 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Stefano Dafarra
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#include <UnicycleBaseController.h>
+#include <iostream>
+#include <cmath>
+
+double UnicycleBaseController::saturate(double input, double saturation)
+{
+    return saturate(input, saturation, -saturation);
+}
+
+double UnicycleBaseController::saturate(double input, double positiveSaturation, double negativeSaturation)
+{
+    if ((positiveSaturation < 0) || (negativeSaturation > positiveSaturation))
+        return input;
+
+    if (input > positiveSaturation)
+        return positiveSaturation;
+    else if (input < negativeSaturation)
+        return negativeSaturation;
+    else return input;
+}
+
+UnicycleBaseController::UnicycleBaseController()
+    : iDynTree::optimalcontrol::Controller(3)
+    ,m_maxLinearVelocity(-1) //negative value -> don't saturate
+    ,m_maxAngularVelocity(-1) //negative value -> don't saturate
+    ,m_slowWhenTurnGain(0.0)
+    ,m_slowWhenBackwardFactor(1.0)
+    ,m_slowWhenSidewaysFactor(1.0)
+{ }
+
+bool UnicycleBaseController::doControl(iDynTree::VectorDynSize &controllerOutput)
+{
+    if(controllerOutput.size() != this->controlSpaceSize())
+        controllerOutput.resize(this->controlSpaceSize());
+
+
+    if (!doUnicycleControl(controllerOutput(0), controllerOutput(1), controllerOutput(2)))
+    {
+        return false;
+    }
+
+    controllerOutput(1) = saturate(controllerOutput(1), m_maxAngularVelocity);
+    double velocitySaturation = m_maxLinearVelocity / (1.0 + m_slowWhenTurnGain * (std::sqrt(controllerOutput(1) * controllerOutput(1))));
+    controllerOutput(0) = saturate(controllerOutput(0), velocitySaturation, -m_slowWhenBackwardFactor * velocitySaturation);
+    controllerOutput(2) = saturate(controllerOutput(2), m_slowWhenSidewaysFactor * m_maxLinearVelocity);
+
+    return true;
+}
+
+bool UnicycleBaseController::setStateFeedback(const double t, const iDynTree::VectorDynSize &stateFeedback)
+{
+    if (stateFeedback.size() != 3)
+        return false;
+
+    iDynTree::Vector2 unicyclePosition;
+    double unicycleOrientation;
+
+    unicycleOrientation = stateFeedback(2);
+
+    unicyclePosition(0) = stateFeedback(0);
+    unicyclePosition(1) = stateFeedback(1);
+
+    return setUnicycleStateFeedback(t, unicyclePosition, unicycleOrientation);
+
+}
+
+bool UnicycleBaseController::setSaturations(double maxLinearVelocity, double maxAngularVelocity)
+{
+    if ((maxLinearVelocity < 0)||(maxAngularVelocity < 0)){
+        std::cerr << "The saturations are on the absolute value, thus they need to be non-negative." << std::endl;
+        return false;
+    }
+
+    m_maxLinearVelocity = maxLinearVelocity;
+    m_maxAngularVelocity = maxAngularVelocity;
+
+    return true;
+}
+
+bool UnicycleBaseController::setSlowWhenTurnGain(double slowWhenTurnGain)
+{
+    if (slowWhenTurnGain < 0){
+        std::cerr << "The slowWhenTurn gain is supposed to be non-negative." << std::endl;
+        return false;
+    }
+
+    m_slowWhenTurnGain = slowWhenTurnGain;
+    return true;
+}
+
+bool UnicycleBaseController::setSlowWhenBackwardFactor(double slowWhenBackwardFactor)
+{
+    if (slowWhenBackwardFactor < 0){
+        std::cerr << "The slowWhenBackwardFactor multiplier is supposed to be non-negative." << std::endl;
+        return false;
+    }
+
+    m_slowWhenBackwardFactor = slowWhenBackwardFactor;
+    return true;
+}
+
+bool UnicycleBaseController::setSlowWhenSidewaysFactor(double slowWhenSidewaysFactor)
+{
+    if (slowWhenSidewaysFactor < 0){
+        std::cerr << "The slowWhenSidewaysFactor multiplier is supposed to be non-negative." << std::endl;
+        return false;
+    }
+
+    m_slowWhenSidewaysFactor = slowWhenSidewaysFactor;
+    return true;
+}
diff --git a/src/UnicycleDirectController.cpp b/src/UnicycleDirectController.cpp
new file mode 100644
index 0000000..507fcd4
--- /dev/null
+++ b/src/UnicycleDirectController.cpp
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 20022 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Stefano Dafarra
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#include <UnicycleDirectController.h>
+
+
+UnicycleDirectController::UnicycleDirectController()
+    : m_desiredForwardSpeed(0.0)
+    , m_desiredAngularVelocity(0.0)
+    , m_desiredLateralVelocity(0.0)
+    , m_time(0.0)
+    , m_deactivationEndTime(0.0)
+{
+
+}
+
+bool UnicycleDirectController::doUnicycleControl(double &forwardSpeed, double &angularVelocity, double &lateralVelocity)
+{
+
+    if (m_time < m_deactivationEndTime)
+    {
+        forwardSpeed = 0.0;
+        angularVelocity = 0.0;
+        lateralVelocity = 0.0;
+
+        return true;
+    }
+
+    forwardSpeed = m_desiredForwardSpeed;
+    angularVelocity = m_desiredAngularVelocity;
+    lateralVelocity = m_desiredLateralVelocity;
+
+    return true;
+}
+
+bool UnicycleDirectController::setUnicycleStateFeedback(const double t, const iDynTree::Vector2 &, double)
+{
+    m_time = t;
+    return true;
+}
+
+void UnicycleDirectController::setConstantControl(double forwardSpeed, double angularVelocity, double lateralVelocity)
+{
+    m_desiredForwardSpeed = forwardSpeed;
+    m_desiredAngularVelocity = angularVelocity;
+    m_desiredLateralVelocity = lateralVelocity;
+}
+
+void UnicycleDirectController::setInactiveUntil(double endTime)
+{
+    m_deactivationEndTime = endTime;
+}
+
+double UnicycleDirectController::desiredLateralVelocity() const
+{
+    return m_desiredLateralVelocity;
+}
diff --git a/src/UnicyclePlanner.cpp b/src/UnicyclePlanner.cpp
index 0d3d604..437ca9f 100644
--- a/src/UnicyclePlanner.cpp
+++ b/src/UnicyclePlanner.cpp
@@ -23,12 +23,20 @@ bool UnicyclePlanner::getInitialStateFromFeet(double initTime)
 
     if ((m_left->numberOfSteps() == 0) && (m_right->numberOfSteps() == 0)){
 
-        if (!m_controller->getDesiredPoint(initTime, unicycleState.position, dummyVector))
-            return false;
+        if (m_currentController == UnicycleController::PERSON_FOLLOWING)
+        {
+            if (!m_personFollowingController->getDesiredPoint(initTime, unicycleState.position, dummyVector))
+                return false;
 
-        unicycleState.position(0) = unicycleState.position(0) - m_controller->getPersonDistance()(0);
-        unicycleState.position(1) = unicycleState.position(1) - m_controller->getPersonDistance()(1);
-        unicycleState.angle = 0;
+            unicycleState.position(0) = unicycleState.position(0) - m_personFollowingController->getPersonDistance()(0);
+            unicycleState.position(1) = unicycleState.position(1) - m_personFollowingController->getPersonDistance()(1);
+            unicycleState.angle = 0;
+        }
+        else
+        {
+            unicycleState.position.zero();
+            unicycleState.angle = 0.0;
+        }
 
         m_left->addStepFromUnicycle(unicycleState, initTime);
         m_right->addStepFromUnicycle(unicycleState, initTime);
@@ -106,11 +114,11 @@ bool UnicyclePlanner::getInitialStateFromFeet(double initTime)
 
         TrajectoryPoint initialPoint;
         initialPoint.initTime = initTime;
-        initialPoint.yDesired = m_controller->getPersonPosition(unicycleState.position, unicycleState.angle);
+        initialPoint.yDesired = m_personFollowingController->getPersonPosition(unicycleState.position, unicycleState.angle);
         dummyVector.zero();
         initialPoint.yDotDesired = dummyVector;
 
-        if (!(m_controller->setDesiredPoint(initialPoint))){
+        if (!(m_personFollowingController->setDesiredPoint(initialPoint))){
             std::cerr <<"Failed to set a the intial reference given the provided FootPrints." <<std::endl;
             return false;
         }
@@ -118,10 +126,12 @@ bool UnicyclePlanner::getInitialStateFromFeet(double initTime)
         if (m_firstStep) {
             initialPoint.initTime = initTime + m_nominalTime - m_minTime;
 
-            if (!(m_controller->setDesiredPoint(initialPoint))){
+            if (!(m_personFollowingController->setDesiredPoint(initialPoint))){
                 std::cerr <<"Failed to set a the intial dummy reference to have a slower first step." <<std::endl;
                 return false;
             }
+
+            m_directController->setInactiveUntil(initTime + m_nominalTime - m_minTime);
         }
     }
 
@@ -130,6 +140,14 @@ bool UnicyclePlanner::getInitialStateFromFeet(double initTime)
         return false;
     }
 
+    //Change first stepping foot according to the desired lateral velocity
+    if (m_firstStep && m_currentController == UnicycleController::DIRECT &&
+            std::abs(m_directController->desiredLateralVelocity()) > 0)
+    {
+        //If we want to move right, use the right as first stepping foot
+        m_swingLeft = m_directController->desiredLateralVelocity() > 0;
+    }
+
     return true;
 }
 
@@ -308,7 +326,9 @@ bool UnicyclePlanner::addTerminalStep(const UnicycleState &lastUnicycleState)
 }
 
 UnicyclePlanner::UnicyclePlanner()
-    :m_controller(std::make_shared<UnicyleController>())
+    :m_personFollowingController(std::make_shared<PersonFollowingController>())
+    ,m_directController(std::make_shared<UnicycleDirectController>())
+    ,m_currentController(UnicycleController::PERSON_FOLLOWING)
     ,m_unicycle(std::make_shared<ControlledUnicycle>())
     ,m_integrator(m_unicycle)
     ,m_initTime(0.0)
@@ -332,8 +352,10 @@ UnicyclePlanner::UnicyclePlanner()
     ,m_left(nullptr)
     ,m_right(nullptr)
     ,m_swingLeft(true)
+    , m_linearVelocityConservativeFactor(0.9)
+    , m_angularVelocityConservativeFactor(0.7)
 {
-    m_unicycle->setController(m_controller);
+    m_unicycle->setController(m_personFollowingController);
     m_integrator.setMaximumStepSize(0.01);
     m_unicycleProblem.setMaxLength(0.20);
     m_unicycleProblem.setMinWidth(0.08);
@@ -345,31 +367,50 @@ bool UnicyclePlanner::setDesiredPersonDistance(double xPosition, double yPositio
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setPersonDistance(xPosition, yPosition);
+    return m_personFollowingController->setPersonDistance(xPosition, yPosition);
 }
 
 bool UnicyclePlanner::setControllerGain(double controllerGain)
+{
+    return setPersonFollowingControllerGain(controllerGain);
+}
+
+bool UnicyclePlanner::setPersonFollowingControllerGain(double controllerGain)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setGain(controllerGain);
+    return m_personFollowingController->setGain(controllerGain);
 }
 
 bool UnicyclePlanner::setSlowWhenTurnGain(double slowWhenTurnGain)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setSlowWhenTurnGain(slowWhenTurnGain);
+    return m_personFollowingController->setSlowWhenTurnGain(slowWhenTurnGain) &&
+            m_directController->setSlowWhenTurnGain(slowWhenTurnGain);
 }
 
 bool UnicyclePlanner::setSlowWhenBackwardFactor(double slowWhenBackwardFactor)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setSlowWhenBackwardFactor(slowWhenBackwardFactor);
+    return m_personFollowingController->setSlowWhenBackwardFactor(slowWhenBackwardFactor) &&
+            m_directController->setSlowWhenBackwardFactor(slowWhenBackwardFactor);
+}
+
+bool UnicyclePlanner::setSlowWhenSidewaysFactor(double slowWhenSidewaysFactor)
+{
+    std::lock_guard<std::mutex> guard(m_mutex);
+
+    return m_personFollowingController->setSlowWhenSidewaysFactor(slowWhenSidewaysFactor) && m_directController->setSlowWhenSidewaysFactor(slowWhenSidewaysFactor);
 }
 
 bool UnicyclePlanner::addDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2 &yDesired)
+{
+    return addPersonFollowingDesiredTrajectoryPoint(initTime, yDesired);
+}
+
+bool UnicyclePlanner::addPersonFollowingDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2 &yDesired)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
@@ -377,10 +418,15 @@ bool UnicyclePlanner::addDesiredTrajectoryPoint(double initTime, const iDynTree:
     newPoint.yDesired = yDesired;
     newPoint.yDotDesired.zero();
     newPoint.initTime = initTime;
-    return m_controller->setDesiredPoint(newPoint);
+    return m_personFollowingController->setDesiredPoint(newPoint);
 }
 
 bool UnicyclePlanner::addDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2 &yDesired, const iDynTree::Vector2 &yDotDesired)
+{
+    return addPersonFollowingDesiredTrajectoryPoint(initTime, yDesired, yDotDesired);
+}
+
+bool UnicyclePlanner::addPersonFollowingDesiredTrajectoryPoint(double initTime, const iDynTree::Vector2 &yDesired, const iDynTree::Vector2 &yDotDesired)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
@@ -388,21 +434,60 @@ bool UnicyclePlanner::addDesiredTrajectoryPoint(double initTime, const iDynTree:
     newPoint.yDesired = yDesired;
     newPoint.yDotDesired = yDotDesired;
     newPoint.initTime = initTime;
-    return m_controller->setDesiredPoint(newPoint);
+    return m_personFollowingController->setDesiredPoint(newPoint);
 }
 
 void UnicyclePlanner::clearDesiredTrajectory()
+{
+    return clearPersonFollowingDesiredTrajectory();
+}
+
+void UnicyclePlanner::clearPersonFollowingDesiredTrajectory()
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    m_controller->clearDesiredTrajectory();
+    m_personFollowingController->clearDesiredTrajectory();
 }
 
 bool UnicyclePlanner::clearDesiredTrajectoryUpTo(double time)
+{
+    return clearPersonFollowingDesiredTrajectoryUpTo(time);
+}
+
+bool UnicyclePlanner::clearPersonFollowingDesiredTrajectoryUpTo(double time)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->clearDesiredTrajectoryUpTo(time);
+    return m_personFollowingController->clearDesiredTrajectoryUpTo(time);
+}
+
+void UnicyclePlanner::setDesiredDirectControl(double forwardVelocity, double angularVelocity, double lateralVelocity)
+{
+    std::lock_guard<std::mutex> guard(m_mutex);
+
+    m_directController->setConstantControl(forwardVelocity, angularVelocity, lateralVelocity);
+}
+
+bool UnicyclePlanner::setSaturationsConservativeFactors(double linearVelocityConservativeFactor, double angularVelocityConservativeFactor)
+{
+    std::lock_guard<std::mutex> guard(m_mutex);
+
+    if (linearVelocityConservativeFactor < 0.0 || linearVelocityConservativeFactor > 1.0)
+    {
+        std::cerr << " The parameter linearVelocityConservativeFactor is supposed to be between 0 and 1" << std::endl;
+        return false;
+    }
+
+    if (angularVelocityConservativeFactor < 0.0 || angularVelocityConservativeFactor > 1.0)
+    {
+        std::cerr << " The parameter angularVelocityConservativeFactor is supposed to be between 0 and 1" << std::endl;
+        return false;
+    }
+
+    m_linearVelocityConservativeFactor = linearVelocityConservativeFactor;
+    m_angularVelocityConservativeFactor = angularVelocityConservativeFactor;
+
+    return true;
 }
 
 bool UnicyclePlanner::setMaximumIntegratorStepSize(double dT)
@@ -590,9 +675,13 @@ bool UnicyclePlanner::computeNewSteps(std::shared_ptr< FootPrint > leftFoot, std
     m_left.reset(new UnicycleFoot(leftFoot));
     m_right.reset(new UnicycleFoot(rightFoot));
 
-    double maxVelocity = std::sqrt(std::pow(m_maxLength,2) - std::pow(m_nominalWidth,2))/m_minTime * 0.90;
-    double maxAngVelocity = m_maxAngle/m_minTime*0.70;
-    if (!m_controller->setSaturations(maxVelocity, maxAngVelocity))
+    double maxVelocity = std::sqrt(std::pow(m_maxLength,2) - std::pow(m_nominalWidth,2))/m_minTime * m_linearVelocityConservativeFactor;
+    double maxAngVelocity = m_maxAngle/m_minTime * m_angularVelocityConservativeFactor;
+
+    if (!m_personFollowingController->setSaturations(maxVelocity, maxAngVelocity))
+        return false;
+
+    if (!m_directController->setSaturations(maxVelocity, maxAngVelocity))
         return false;
 
     if (!initializePlanner(m_initTime)){
@@ -795,7 +884,7 @@ bool UnicyclePlanner::getPersonPosition(double time, iDynTree::Vector2& personPo
         std::cerr << "Time is out of range." << std::endl;
         return false;
     }
-    personPosition = m_controller->getPersonPosition(unicycleState.position, unicycleState.angle);
+    personPosition = m_personFollowingController->getPersonPosition(unicycleState.position, unicycleState.angle);
     return true;
 }
 
@@ -804,7 +893,7 @@ void UnicyclePlanner::setFreeSpaceEllipseMethod(FreeSpaceEllipseMethod method)
     std::lock_guard<std::mutex> guard(m_mutex);
 
     m_freeSpaceMethod = method;
-    m_controller->setFreeSpaceEllipse(FreeSpaceEllipse()); //Reset the ellipse
+    m_personFollowingController->setFreeSpaceEllipse(FreeSpaceEllipse()); //Reset the ellipse
     m_unicycleProblem.setFreeSpaceEllipse(FreeSpaceEllipse()); //Reset the ellipse
 }
 
@@ -815,7 +904,7 @@ bool UnicyclePlanner::setFreeSpaceEllipse(const FreeSpaceEllipse &freeSpaceEllip
     if (m_freeSpaceMethod == FreeSpaceEllipseMethod::REFERENCE_ONLY ||
             m_freeSpaceMethod == FreeSpaceEllipseMethod::REFERENCE_AND_FOOTSTEPS)
     {
-        if (!m_controller->setFreeSpaceEllipse(freeSpaceEllipse))
+        if (!m_personFollowingController->setFreeSpaceEllipse(freeSpaceEllipse))
         {
             return false;
         }
@@ -837,21 +926,40 @@ bool UnicyclePlanner::setFreeSpaceEllipseConservativeFactor(double conservativeF
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setFreeSpaceEllipseConservativeFactor(conservativeFactor);
+    return m_personFollowingController->setFreeSpaceEllipseConservativeFactor(conservativeFactor);
 }
 
 bool UnicyclePlanner::setInnerFreeSpaceEllipseOffset(double offset)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setInnerFreeSpaceEllipseOffset(offset);
+    return m_personFollowingController->setInnerFreeSpaceEllipseOffset(offset);
 }
 
 bool UnicyclePlanner::setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    return m_controller->setInnerFreeSpaceEllipseOffsets(semiMajorAxisOffset, semiMinorAxisOffset);
+    return m_personFollowingController->setInnerFreeSpaceEllipseOffsets(semiMajorAxisOffset, semiMinorAxisOffset);
+}
+
+bool UnicyclePlanner::setUnicycleController(UnicycleController controller)
+{
+    std::lock_guard<std::mutex> guard(m_mutex);
+
+    if (controller == UnicycleController::PERSON_FOLLOWING)
+    {
+        m_currentController = controller;
+        return m_unicycle->setController(m_personFollowingController);
+    }
+
+    if (controller == UnicycleController::DIRECT)
+    {
+        m_currentController = controller;
+        return m_unicycle->setController(m_directController);
+    }
+
+    return false;
 }
 
 
diff --git a/tests/DcmInterpolationTest.cpp b/tests/DcmInterpolationTest.cpp
index 67ffa79..d955305 100644
--- a/tests/DcmInterpolationTest.cpp
+++ b/tests/DcmInterpolationTest.cpp
@@ -23,7 +23,7 @@
 /**
  * Struct containing the necessary quantities for the trajectory planner
  */
-typedef struct
+struct Configuration
 {
     // timing quantities
     double plannerHorizon = 10.0, dT = 0.010;
@@ -64,7 +64,7 @@ typedef struct
     double lastStepDCMOffsetPercentage = 0.2;
 
     bool swingLeft = true;
-} Configuration;
+};
 
 
 /**
@@ -76,7 +76,7 @@ bool configurePlanner(std::shared_ptr<UnicyclePlanner> planner, const Configurat
     bool ok = true;
 
     ok = ok && planner->setDesiredPersonDistance(conf.referencePointDistanceX, conf.referencePointDistanceY);
-    ok = ok && planner->setControllerGain(conf.unicycleGain);
+    ok = ok && planner->setPersonFollowingControllerGain(conf.unicycleGain);
     ok = ok && planner->setMaximumIntegratorStepSize(conf.dT);
     ok = ok && planner->setMaxStepLength(conf.maxStepLength);
     ok = ok && planner->setMaxAngleVariation(conf.maxAngleVariation);
@@ -252,8 +252,8 @@ bool interpolationTest()
     double initTime = 0;
 
     // add desired initial and final position dor the unicycle
-    iDynTree::assertTrue(unicyclePlanner->addDesiredTrajectoryPoint(initTime, initPosition, initVelocity));
-    iDynTree::assertTrue(unicyclePlanner->addDesiredTrajectoryPoint(initTime + conf.plannerHorizon,
+    iDynTree::assertTrue(unicyclePlanner->addPersonFollowingDesiredTrajectoryPoint(initTime, initPosition, initVelocity));
+    iDynTree::assertTrue(unicyclePlanner->addPersonFollowingDesiredTrajectoryPoint(initTime + conf.plannerHorizon,
                                                             finalPosition, finalVelocity));
 
     // generate the reference footprints and the trajectory for the DCM
@@ -300,12 +300,12 @@ bool interpolationTest()
     iDynTree::assertTrue(unicyclePlanner->getPersonPosition(initTime, initPosition));
 
     // remove all desired trajectory point
-    unicyclePlanner->clearDesiredTrajectory();
+    unicyclePlanner->clearPersonFollowingDesiredTrajectory();
 
     // set new desired positin
     finalPosition(0) = initPosition(0) + 0.3;
     finalPosition(1) = initPosition(1) + 0.5;
-    iDynTree::assertTrue(unicyclePlanner->addDesiredTrajectoryPoint(initTime + conf.plannerHorizon, finalPosition));
+    iDynTree::assertTrue(unicyclePlanner->addPersonFollowingDesiredTrajectoryPoint(initTime + conf.plannerHorizon, finalPosition));
 
     iDynTree::assertTrue(dcmGenerator->setDCMInitialState(boundaryConditionAtMergePoint));
 
@@ -339,12 +339,12 @@ bool interpolationTest()
     iDynTree::assertTrue(unicyclePlanner->getPersonPosition(initTime, initPosition));
 
     // remove all desired trajectory point
-    unicyclePlanner->clearDesiredTrajectory();
+    unicyclePlanner->clearPersonFollowingDesiredTrajectory();
 
     // set new desired positin
     finalPosition(0) = initPosition(0) + 0.0;
     finalPosition(1) = initPosition(1) + 0.2;
-    iDynTree::assertTrue(unicyclePlanner->addDesiredTrajectoryPoint(initTime + conf.plannerHorizon, finalPosition));
+    iDynTree::assertTrue(unicyclePlanner->addPersonFollowingDesiredTrajectoryPoint(initTime + conf.plannerHorizon, finalPosition));
     iDynTree::assertTrue(dcmGenerator->setDCMInitialState(boundaryConditionAtMergePoint));
 
     // evaluate the new trajectory
diff --git a/tests/InterpolationTest.cpp b/tests/InterpolationTest.cpp
index b022fb4..a555293 100644
--- a/tests/InterpolationTest.cpp
+++ b/tests/InterpolationTest.cpp
@@ -16,7 +16,7 @@
 #include <ctime>
 
 
-typedef struct {
+struct Configuration{
     double initTime = 0.0, endTime = 10.0, dT = 0.01, K = 10, dX = 0.05, dY = 0.0;
     double maxL = 0.05, minL = 0.005, minW = 0.03, maxAngle = iDynTree::deg2rad(45), minAngle = iDynTree::deg2rad(5);
     double nominalW = 0.05, maxT = 8, minT = 2.9, nominalT = 4, timeWeight = 2.5, positionWeight = 1;
@@ -27,12 +27,12 @@ typedef struct {
     double comHeightDelta = 0.002;
     double switchOverSwing = 0.6;
     bool swingLeft = true;
-} Configuration;
+};
 
 bool configurePlanner(std::shared_ptr<UnicyclePlanner> planner,const Configuration &conf){
     bool ok = true;
     ok = ok && planner->setDesiredPersonDistance(conf.dX, conf.dY);
-    ok = ok && planner->setControllerGain(conf.K);
+    ok = ok && planner->setPersonFollowingControllerGain(conf.K);
     ok = ok && planner->setMaximumIntegratorStepSize(conf.dT);
     ok = ok && planner->setMaxStepLength(conf.maxL);
     ok = ok && planner->setWidthSetting(conf.minW, conf.nominalW);
@@ -77,7 +77,7 @@ bool configureGenerator(UnicycleGenerator& generator, const Configuration &conf)
 
 void printSteps(const std::deque<Step>& leftSteps, const std::deque<Step>& rightSteps){
     std::cerr << "Left foot "<< leftSteps.size() << " steps:"<< std::endl;
-    for (auto step : leftSteps){
+    for (auto& step : leftSteps){
         std::cerr << "Position "<< step.position.toString() << std::endl;
         std::cerr << "Angle "<< iDynTree::rad2deg(step.angle) << std::endl;
         std::cerr << "Time  "<< step.impactTime << std::endl;
@@ -85,7 +85,7 @@ void printSteps(const std::deque<Step>& leftSteps, const std::deque<Step>& right
 
 
     std::cerr << std::endl << "Right foot "<< rightSteps.size() << " steps:" << std::endl;
-    for (auto step : rightSteps){
+    for (auto& step : rightSteps){
         std::cerr << "Position "<< step.position.toString() << std::endl;
         std::cerr << "Angle "<< iDynTree::rad2deg(step.angle) << std::endl;
         std::cerr << "Time  "<< step.impactTime << std::endl;
@@ -171,7 +171,7 @@ void printTrajectories(UnicycleGenerator& interpolator, size_t& newMergePoint, I
 
     std::cerr << "--------------------------------------------->Left Trajectory." << std::endl;
     //print_iDynTree(lFootTrajectory);
-    for (auto pose : lFootTrajectory){
+    for (auto& pose : lFootTrajectory){
         posLeft << pose.getPosition()(0) << "    " << pose.getPosition()(1) << "    " <<
             "    " << pose.getPosition()(2)<<"    "<< std::endl;
     }
@@ -188,7 +188,7 @@ void printTrajectories(UnicycleGenerator& interpolator, size_t& newMergePoint, I
 
     std::cerr << "--------------------------------------------->Right Trajectory." << std::endl;
     //print_iDynTree(rFootTrajectory);
-    for (auto pose : rFootTrajectory){
+    for (auto& pose : rFootTrajectory){
         posRight << pose.getPosition()(0) << " " << pose.getPosition()(1) << " " <<
                     " " << pose.getPosition()(2)<<"    "<< std::endl;
     }
@@ -344,8 +344,8 @@ bool interpolationTest(){
 
     finalVelocity.zero();
 
-    iDynTree::assertTrue(unicycle.unicyclePlanner()->addDesiredTrajectoryPoint(conf.initTime, initPosition, initVelocity));
-    iDynTree::assertTrue(unicycle.unicyclePlanner()->addDesiredTrajectoryPoint(conf.endTime, finalPosition, finalVelocity));
+    iDynTree::assertTrue(unicycle.unicyclePlanner()->addPersonFollowingDesiredTrajectoryPoint(conf.initTime, initPosition, initVelocity));
+    iDynTree::assertTrue(unicycle.unicyclePlanner()->addPersonFollowingDesiredTrajectoryPoint(conf.endTime, finalPosition, finalVelocity));
 
     clock_t total = clock();
     //iDynTree::assertTrue(unicycle.computeNewSteps(leftFoot, rightFoot));
@@ -368,8 +368,8 @@ bool interpolationTest(){
     newInitTime = newMergePoint*conf.dT + conf.initTime;
     std::cerr << "New run at " << newInitTime << std::endl;
     iDynTree::assertTrue(unicycle.unicyclePlanner()->getPersonPosition(newInitTime, finalPosition));
-    unicycle.unicyclePlanner()->clearDesiredTrajectory();
-    iDynTree::assertTrue(unicycle.unicyclePlanner()->addDesiredTrajectoryPoint(conf.endTime + 10, finalPosition));
+    unicycle.unicyclePlanner()->clearPersonFollowingDesiredTrajectory();
+    iDynTree::assertTrue(unicycle.unicyclePlanner()->addPersonFollowingDesiredTrajectoryPoint(conf.endTime + 10, finalPosition));
 
     iDynTree::assertTrue(leftFoot->keepOnlyPresentStep(newInitTime));
 
diff --git a/tests/UnicycleTest.cpp b/tests/UnicycleTest.cpp
index 2269eff..4eb4a77 100644
--- a/tests/UnicycleTest.cpp
+++ b/tests/UnicycleTest.cpp
@@ -16,13 +16,13 @@
 #include <iostream>
 #include <ctime>
 
-typedef struct {
+struct Configuration {
     double initTime = 0.0, endTime = 50.0, dT = 0.01, K = 10, dX = 0.2, dY = 0.0;
     double maxL = 0.2, minL = 0.05, minW = 0.08, maxAngle = iDynTree::deg2rad(45), minAngle = iDynTree::deg2rad(5);
     double nominalW = 0.14, maxT = 10, minT = 3, nominalT = 4, timeWeight = 2.5, positionWeight = 1;
     bool swingLeft = true;
     double slowWhenTurnGain = 0.5;
-} Configuration;
+};
 
 bool printSteps(std::deque<Step> leftSteps, std::deque<Step> rightSteps){
     std::cerr << "Left foot "<< leftSteps.size() << " steps:"<< std::endl;
@@ -117,7 +117,7 @@ bool populateDesiredTrajectory(UnicyclePlanner& planner, double initTime, double
         yDotDes(0) = 0.01;
         yDes(1) = 0.5*std::sin(0.1*t);
         yDotDes(1) = 0.5*0.1*std::cos(0.1*t);
-        if(!planner.addDesiredTrajectoryPoint(t,yDes, yDotDes))
+        if(!planner.addPersonFollowingDesiredTrajectoryPoint(t,yDes, yDotDes))
             return false;
         t += dT;
     }
@@ -151,7 +151,7 @@ bool plannerTest(){
 
     //Initialization (some of these calls may be avoided)
     iDynTree::assertTrue(planner.setDesiredPersonDistance(conf.dX, conf.dY));
-    iDynTree::assertTrue(planner.setControllerGain(conf.K));
+    iDynTree::assertTrue(planner.setPersonFollowingControllerGain(conf.K));
     iDynTree::assertTrue(planner.setMaximumIntegratorStepSize(conf.dT));
     iDynTree::assertTrue(planner.setMaxStepLength(conf.maxL));
     iDynTree::assertTrue(planner.setWidthSetting(conf.minW, conf.nominalW));
@@ -200,12 +200,145 @@ bool plannerTest(){
     iDynTree::assertTrue(right->numberOfSteps() == 1);
     Step lastStep;
     iDynTree::assertTrue(right->getLastStep(lastStep));
-    planner.clearDesiredTrajectory();
+    planner.clearPersonFollowingDesiredTrajectory();
     iDynTree::Vector2 dummyVector, newDesired;
     dummyVector.zero();
     newDesired(0) = lastStep.position(0) + 0.5;
     newDesired(1) = lastStep.position(1) + 0.5;
-    iDynTree::assertTrue(planner.addDesiredTrajectoryPoint(lastStep.impactTime+10, newDesired, dummyVector));
+    iDynTree::assertTrue(planner.addPersonFollowingDesiredTrajectoryPoint(lastStep.impactTime+10, newDesired, dummyVector));
+
+    iDynTree::assertTrue(planner.computeNewSteps(left, right, lastStep.impactTime, lastStep.impactTime+10));
+
+    leftSteps = left->getSteps();
+    rightSteps = right->getSteps();
+
+    iDynTree::assertTrue(printSteps(leftSteps, rightSteps));
+
+    iDynTree::assertTrue(checkConstraints(leftSteps, rightSteps, conf));
+
+    return true;
+}
+
+bool directControlTest(){
+
+    Configuration conf;
+    conf.initTime = 0.0;
+    conf.endTime = 50.0;
+    conf.dT = 0.01;
+    conf.K = 10;
+    conf.dX = 0.2;
+    conf.dY = 0.0;
+    conf.maxL = 0.2;
+    conf.minL = 0.05;
+    conf.minW = 0.08;
+    conf.maxAngle = iDynTree::deg2rad(45);
+    conf.minAngle = iDynTree::deg2rad(5);
+    conf.nominalW = 0.14;
+    conf.maxT = 10;
+    conf.minT = 3;
+    conf.nominalT = 4;
+    conf.timeWeight = 2.5;
+    conf.positionWeight = 1;
+    conf.swingLeft = true;
+    conf.slowWhenTurnGain = 0.5;
+
+    UnicyclePlanner planner;
+
+    //Initialization (some of these calls may be avoided)
+    iDynTree::assertTrue(planner.setUnicycleController(UnicycleController::DIRECT));
+    iDynTree::assertTrue(planner.setMaximumIntegratorStepSize(conf.dT));
+    iDynTree::assertTrue(planner.setMaxStepLength(conf.maxL));
+    iDynTree::assertTrue(planner.setWidthSetting(conf.minW, conf.nominalW));
+    iDynTree::assertTrue(planner.setMaxAngleVariation(conf.maxAngle));
+    iDynTree::assertTrue(planner.setCostWeights(conf.positionWeight, conf.timeWeight));
+    iDynTree::assertTrue(planner.setStepTimings(conf.minT, conf.maxT, conf.nominalT));
+    iDynTree::assertTrue(planner.setPlannerPeriod(conf.dT));
+    iDynTree::assertTrue(planner.setMinimumAngleForNewSteps(conf.minAngle));
+    iDynTree::assertTrue(planner.setMinimumStepLength(conf.minL));
+    iDynTree::assertTrue(planner.setSlowWhenTurnGain(conf.slowWhenTurnGain));
+    iDynTree::assertTrue(planner.setSlowWhenSidewaysFactor(0.2));
+    iDynTree::assertTrue(planner.setSaturationsConservativeFactors(0.7, 0.7));
+
+    planner.addTerminalStep(true);
+    planner.startWithLeft(conf.swingLeft);
+
+    //Generate desired trajectory
+
+    planner.setDesiredDirectControl(10.0, 0.0, 0.0);
+
+    std::shared_ptr<FootPrint> left, right;
+    left = std::make_shared<FootPrint>();
+    right = std::make_shared<FootPrint>();
+    iDynTree::Vector2 initPosition;
+    initPosition(0) = 0.3;
+    initPosition(1) = -0.5;
+    //left->addStep(initPosition, iDynTree::deg2rad(15), 25); //fake initialization
+
+    clock_t start = clock();
+    iDynTree::assertTrue(planner.computeNewSteps(left, right, conf.initTime, conf.endTime));
+    std::cerr <<"Test Finished in " << (static_cast<double>(clock() - start) / CLOCKS_PER_SEC) << " seconds."<<std::endl;
+
+    StepList leftSteps = left->getSteps();
+    StepList rightSteps = right->getSteps();
+
+    std::cerr << "First test." << std::endl;
+    iDynTree::assertTrue(printSteps(leftSteps, rightSteps));
+    iDynTree::assertTrue(checkConstraints(leftSteps, rightSteps, conf));
+
+
+
+    std::cerr << std::endl << "------------------------------------------------------------------" << std::endl;
+    std::cerr << "Second test." << std::endl;
+
+    left->clearSteps();
+    iDynTree::assertTrue(right->dropPastSteps());
+    iDynTree::assertTrue(right->numberOfSteps() == 1);
+    Step lastStep;
+    iDynTree::assertTrue(right->getLastStep(lastStep));
+
+    planner.setDesiredDirectControl(0.0, 10.0, 0.0);
+
+
+    iDynTree::assertTrue(planner.computeNewSteps(left, right, lastStep.impactTime, lastStep.impactTime+10));
+
+    leftSteps = left->getSteps();
+    rightSteps = right->getSteps();
+
+    iDynTree::assertTrue(printSteps(leftSteps, rightSteps));
+
+    iDynTree::assertTrue(checkConstraints(leftSteps, rightSteps, conf));
+
+
+    std::cerr << std::endl << "------------------------------------------------------------------" << std::endl;
+    std::cerr << "Third test." << std::endl;
+
+    left->clearSteps();
+    iDynTree::assertTrue(right->dropPastSteps());
+    iDynTree::assertTrue(right->numberOfSteps() == 1);
+    iDynTree::assertTrue(right->getLastStep(lastStep));
+
+    planner.setDesiredDirectControl(0.0, 0.0, 10.0);
+
+
+    iDynTree::assertTrue(planner.computeNewSteps(left, right, lastStep.impactTime, lastStep.impactTime+10));
+
+    leftSteps = left->getSteps();
+    rightSteps = right->getSteps();
+
+    iDynTree::assertTrue(printSteps(leftSteps, rightSteps));
+
+    iDynTree::assertTrue(checkConstraints(leftSteps, rightSteps, conf));
+
+    std::cerr << std::endl << "------------------------------------------------------------------" << std::endl;
+    std::cerr << "Fourth test." << std::endl;
+
+    left->clearSteps();
+    iDynTree::assertTrue(right->dropPastSteps());
+    iDynTree::assertTrue(right->numberOfSteps() == 1);
+    iDynTree::assertTrue(right->getLastStep(lastStep));
+
+    planner.setDesiredDirectControl(0.0, 0.0, -10.0);
+
 
     iDynTree::assertTrue(planner.computeNewSteps(left, right, lastStep.impactTime, lastStep.impactTime+10));
 
@@ -221,5 +354,7 @@ bool plannerTest(){
 
 int main(int argc, char **argv) {
     iDynTree::assertTrue(plannerTest());
+    std::cerr << "----------------Direct Control Test -------------------" << std::endl;
+    iDynTree::assertTrue(directControlTest());
     return EXIT_SUCCESS;
 }