Allow the unicycle to move sideways

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

include/ControlledUnicycle.h

@@ -9,28 +9,29 @@
 #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;
 
  bool setInitialState(const iDynTree::Vector2& unicyclePosition, double angle);
 
  const iDynTree::VectorDynSize& initialState() const override;
 
- bool setController(std::shared_ptr<UnicyleController> controller);
+ bool setController(std::shared_ptr<iDynTree::optimalcontrol::Controller> controller);
 };
 
 

include/UnicycleController.h → 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

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

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

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,15 +25,24 @@ 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;
  double m_initTime, m_endTime, m_minTime, m_maxTime, m_nominalTime, m_dT, m_minAngle, m_nominalWidth, m_maxLength, m_minLength, m_maxAngle;
  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

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;