Merge pull request #51 from robotology/ellipseImprovements

Ellipse improvements

.github/workflows/conda-forge-ci.yml

@@ -15,7 +15,7 @@ jobs:
  strategy:
  matrix:
  build_type: [Release]
- os: [ubuntu-latest, windows-latest, macOS-latest]
+ os: [ubuntu-latest, windows-2019, macOS-latest]
  fail-fast: false
 
  steps:

CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.1)
 project(UnicyclePlanner
  LANGUAGES CXX
- VERSION 0.4.1)
+ VERSION 0.4.2)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

include/FreeSpaceEllipse.h

@@ -66,11 +66,13 @@ class FreeSpaceEllipse
  // If the ellipsoid has never been set, it returns true by default
  bool isPointInside(const iDynTree::Vector2& testPoint) const;
 
- iDynTree::Vector2 projectPointInsideEllipse(const iDynTree::Vector2& testPoint) const;
+ iDynTree::Vector2 projectPointInsideEllipse(const iDynTree::Vector2& testPoint, const iDynTree::Vector2 &projectionPoint) const;
 
  bool getIntersectionsWithLine(const iDynTree::Vector2& linePoint1, const iDynTree::Vector2& linePoint2,
  iDynTree::Vector2& intersection1, iDynTree::Vector2& intersection2) const;
 
+ bool getClosestIntersectionsWithLine(const iDynTree::Vector2& linePoint1, const iDynTree::Vector2& linePoint2, iDynTree::Vector2& intersection) const;
+
  iDynTree::Vector2 getTangentVector(const iDynTree::Vector2& intersectionPoint) const;
 
  std::string printInfo() const;

include/UnicycleController.h

@@ -32,7 +32,8 @@ class UnicyleController : public iDynTree::optimalcontrol::Controller{
  double m_gain, m_maxVelocity, m_maxAngularVelocity, m_time;
  double m_slowWhenTurnGain;
  double m_slowWhenBackwardFactor;
- double m_innerEllipseOffset;
+ double m_semiMajorInnerEllipseOffset;
+ double m_semiMinorInnerEllipseOffset;
  FreeSpaceEllipse m_outerEllipse, m_innerEllipse;
  double m_conservativeFactor;
 
@@ -85,6 +86,8 @@ class UnicyleController : public iDynTree::optimalcontrol::Controller{
  bool setFreeSpaceEllipseConservativeFactor(double conservativeFactor);
 
  bool setInnerFreeSpaceEllipseOffset(double offset);
+
+ bool setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset);
 };
 
 #endif // UNICYCLECONTROLLER_H

include/UnicyclePlanner.h

@@ -116,6 +116,8 @@ class UnicyclePlanner {
  bool setFreeSpaceEllipseConservativeFactor(double conservativeFactor); //Used only to saturate the references
 
  bool setInnerFreeSpaceEllipseOffset(double offset);
+
+ bool setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset);
 };
 
 #endif // UNICYCLEPLANNER_H

src/FreeSpaceEllipse.cpp

@@ -175,7 +175,7 @@ FreeSpaceEllipse::FreeSpaceEllipse()
  clear();
 }
 
-iDynTree::Vector2 FreeSpaceEllipse::projectPointInsideEllipse(const iDynTree::Vector2 &testPoint) const
+iDynTree::Vector2 FreeSpaceEllipse::projectPointInsideEllipse(const iDynTree::Vector2 &testPoint, const iDynTree::Vector2& projectionPoint) const
 {
  if (!m_isSet)
  {
@@ -189,15 +189,20 @@ iDynTree::Vector2 FreeSpaceEllipse::projectPointInsideEllipse(const iDynTree::Ve
  return testPoint;
  }
 
- iDynTree::Vector2 normalizedGenerators;
+ iDynTree::Vector2 intersection;
+ if (!getClosestIntersectionsWithLine(projectionPoint, testPoint, intersection))
+ {
+ iDynTree::Vector2 normalizedGenerators;
 
- iDynTree::toEigen(normalizedGenerators) = iDynTree::toEigen(generators) / module;
+  iDynTree::toEigen(normalizedGenerators) = iDynTree::toEigen(generators) / module;
 
- iDynTree::Vector2 output;
+  iDynTree::Vector2 output;
 
- iDynTree::toEigen(output) = iDynTree::toEigen(m_C) * iDynTree::toEigen(normalizedGenerators) + iDynTree::toEigen(m_d);
+ iDynTree::toEigen(output) = iDynTree::toEigen(m_C) * iDynTree::toEigen(normalizedGenerators) + iDynTree::toEigen(m_d);
+ return output;
+ }
 
- return output;
+ return intersection;
 }
 
 bool FreeSpaceEllipse::getIntersectionsWithLine(const iDynTree::Vector2 &linePoint1, const iDynTree::Vector2 &linePoint2,
@@ -231,7 +236,7 @@ bool FreeSpaceEllipse::getIntersectionsWithLine(const iDynTree::Vector2 &linePoi
  intersection1InCircle.zero();
  intersection2InCircle.zero();
 
- if (std::abs(alpha) < 1e-15) // The line is horizontal, y = -gamma / beta. There is an intersection is y <= 1
+ if (std::abs(alpha) < 1e-15) // The line is horizontal, y = -gamma / beta. There is an intersection if |y| <= 1
  {
 
  intersection1InCircle(1) = -gamma / beta;
@@ -248,7 +253,7 @@ bool FreeSpaceEllipse::getIntersectionsWithLine(const iDynTree::Vector2 &linePoi
  }
  }
 
- else if (std::abs(beta) < 1e-15) // The line is vertical, x = -gamma / alpha. There is an intersection is x <= 1
+ else if (std::abs(beta) < 1e-15) // The line is vertical, x = -gamma / alpha. There is an intersection if |x| <= 1
  {
 
  intersection1InCircle(0) = -gamma / alpha;
@@ -296,6 +301,28 @@ bool FreeSpaceEllipse::getIntersectionsWithLine(const iDynTree::Vector2 &linePoi
  return true;
 }
 
+bool FreeSpaceEllipse::getClosestIntersectionsWithLine(const iDynTree::Vector2 &linePoint1, const iDynTree::Vector2 &linePoint2, iDynTree::Vector2 &intersection) const
+{
+ iDynTree::Vector2 intersection1, intersection2;
+
+ if (!getIntersectionsWithLine(linePoint1, linePoint2, intersection1, intersection2))
+ {
+ return false;
+ }
+
+ if ((iDynTree::toEigen(linePoint2) - iDynTree::toEigen(intersection1)).norm() <
+ (iDynTree::toEigen(linePoint2) - iDynTree::toEigen(intersection2)).norm())
+ {
+ intersection = intersection1;
+ }
+ else
+ {
+ intersection = intersection2;
+ }
+
+ return true;
+}
+
 iDynTree::Vector2 FreeSpaceEllipse::getTangentVector(const iDynTree::Vector2 &intersectionPoint) const
 {
  Eigen::Matrix2d R_pi_2; //The rotation matrix of 90 deg, to get the perpendicular to a vector;

src/UnicycleController.cpp

@@ -50,7 +50,8 @@ UnicyleController::UnicyleController()
  ,m_time(0)
  ,m_slowWhenTurnGain(0.0)
  ,m_slowWhenBackwardFactor(1.0)
- ,m_innerEllipseOffset(0.0)
+ ,m_semiMajorInnerEllipseOffset(0.0)
+ ,m_semiMinorInnerEllipseOffset(0.0)
  ,m_conservativeFactor(2.0)
 {
  m_personDistance(0) = 0.2;
@@ -267,8 +268,8 @@ bool UnicyleController::setFreeSpaceEllipse(const FreeSpaceEllipse &freeSpaceEll
  m_innerEllipse = freeSpaceEllipse;
  if (m_outerEllipse.isSet())
  {
- double innerEllipseSemiMajorAxis = m_outerEllipse.semiMajorAxis() - m_innerEllipseOffset;
- double innerEllipseSemiMinorAxis = m_outerEllipse.semiMajorAxis() - m_innerEllipseOffset;
+ double innerEllipseSemiMajorAxis = m_outerEllipse.semiMajorAxis() - m_semiMajorInnerEllipseOffset;
+ double innerEllipseSemiMinorAxis = m_outerEllipse.semiMinorAxis() - m_semiMinorInnerEllipseOffset;
 
  if ((innerEllipseSemiMajorAxis <= 0.0) || (innerEllipseSemiMinorAxis <= 0.0))
  {
@@ -299,13 +300,25 @@ bool UnicyleController::setFreeSpaceEllipseConservativeFactor(double conservativ
 
 bool UnicyleController::setInnerFreeSpaceEllipseOffset(double offset)
 {
- if (offset < 0)
+ return setInnerFreeSpaceEllipseOffsets(offset, offset);
+}
+
+bool UnicyleController::setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset)
+{
+ if (semiMajorAxisOffset < 0)
  {
- std::cerr << "The inner free space ellipse offset is expected to be non-negative" << std::endl;
+ std::cerr << "The inner free space ellipse semi major axis offset is expected to be non-negative" << std::endl;
  return false;
  }
 
- m_innerEllipseOffset = offset;
+ if (semiMinorAxisOffset < 0)
+ {
+ std::cerr << "The inner free space ellipse semi minor axis offset is expected to be non-negative" << std::endl;
+ return false;
+ }
+
+ m_semiMajorInnerEllipseOffset = semiMajorAxisOffset;
+ m_semiMinorInnerEllipseOffset = semiMinorAxisOffset;
  return setFreeSpaceEllipse(m_outerEllipse);
 }
 
@@ -355,48 +368,43 @@ bool UnicyleController::getDesiredPointInFreeSpaceEllipse(double time, const iDy
  return false;
  }
 
+ if (m_innerEllipse.isSet() && m_innerEllipse.isPointInside(yDesired))
+ {
+ return true; //premature exit if the point is already inside the inner ellipse
+ }
+
+ Eigen::Vector2d desiredFromOuter, desiredFromInner;
+ desiredFromInner = desiredFromOuter = iDynTree::toEigen(yDesired);
+ double blendingFactor = 0.0;
+
  if (m_outerEllipse.isSet())
  {
- Eigen::Vector2d desiredFromOuter, desiredFromInner, saturatedInput;
- desiredFromOuter = iDynTree::toEigen(m_outerEllipse.projectPointInsideEllipse(yDesired));
-  saturatedInput = desiredFromOuter;
+ desiredFromOuter = iDynTree::toEigen(m_outerEllipse.projectPointInsideEllipse(yDesired, unicyclePosition));
+ desiredFromInner = desiredFromOuter;
+ }
 
-  if (m_innerEllipse.isSet())
-  {
-  desiredFromInner = iDynTree::toEigen(m_innerEllipse.projectPointInsideEllipse(yDesired));
+ if (m_innerEllipse.isSet())
+ {
+ desiredFromInner = iDynTree::toEigen(m_innerEllipse.projectPointInsideEllipse(yDesired, unicyclePosition));
 
- iDynTree::Vector2 intersection1, intersection2;
+ iDynTree::Vector2 closestIntersection;
+ iDynTree::Vector2 personPosition = getPersonPosition(unicyclePosition, unicycleAngle);
 
- //Compute the intersections between inner ellipse and the line passing between the center of the unicycle and the person
- iDynTree::Vector2 personPosition = getPersonPosition(unicyclePosition, unicycleAngle);
- if (m_innerEllipse.getIntersectionsWithLine(unicyclePosition, personPosition, intersection1, intersection2))
+ //Compute the intersections between inner ellipse and the line passing between the center of the unicycle and the person
+ if (m_outerEllipse.getClosestIntersectionsWithLine(unicyclePosition, personPosition, closestIntersection))
+ {
+ Eigen::Vector2d ellipseTangentVector = iDynTree::toEigen(m_outerEllipse.getTangentVector(closestIntersection));
+ Eigen::Vector2d desiredMotionVector = iDynTree::toEigen(yDesired) - iDynTree::toEigen(personPosition);
+ double desiredMotionVectorModule = desiredMotionVector.norm();
+ if (desiredMotionVectorModule > 1e-4)
  {
- //If we are here there is at least one intersection point. We get the closest
- iDynTree::Vector2 closestIntersection;
- if ((iDynTree::toEigen(personPosition) - iDynTree::toEigen(intersection1)).norm() <
- (iDynTree::toEigen(personPosition) - iDynTree::toEigen(intersection2)).norm())
- {
- closestIntersection = intersection1;
- }
- else
- {
- closestIntersection = intersection2;
- }
-
- Eigen::Vector2d unicycleVector = iDynTree::toEigen(personPosition) - iDynTree::toEigen(unicyclePosition);
-
- Eigen::Vector2d ellipseTangentVector = iDynTree::toEigen(m_innerEllipse.getTangentVector(closestIntersection));
-
- double blendingFactor = std::abs(unicycleVector.transpose() * ellipseTangentVector) / m_personDistanceNorm; //1 if the unicycle is parallel to the tangent, 0 if perpendicular
- blendingFactor = std::tanh(m_conservativeFactor * blendingFactor);
-
- saturatedInput = blendingFactor * desiredFromInner + (1.0 - blendingFactor) * desiredFromOuter; //If the unicycle is perpendicular to the ellipse, we use the large one
+ blendingFactor = std::abs(desiredMotionVector.transpose() * ellipseTangentVector) / desiredMotionVectorModule; //1 if the desired motion vector is parallel to the tangent, 0 if perpendicular
  }
  }
-
- iDynTree::toEigen(yDesired) = saturatedInput;
-
  }
 
+ blendingFactor = std::tanh(m_conservativeFactor * blendingFactor);
+ iDynTree::toEigen(yDesired) = blendingFactor * desiredFromInner + (1.0 - blendingFactor) * desiredFromOuter;
+
  return true;
 }

src/UnicyclePlanner.cpp

@@ -840,4 +840,11 @@ bool UnicyclePlanner::setInnerFreeSpaceEllipseOffset(double offset)
  return m_controller->setInnerFreeSpaceEllipseOffset(offset);
 }
 
+bool UnicyclePlanner::setInnerFreeSpaceEllipseOffsets(double semiMajorAxisOffset, double semiMinorAxisOffset)
+{
+ std::lock_guard<std::mutex> guard(m_mutex);
+
+ return m_controller->setInnerFreeSpaceEllipseOffsets(semiMajorAxisOffset, semiMinorAxisOffset);
+}
+