Fix bugs in CRRTs. (#574)

* First show at fixing CRRT bugs.

* Respond to code review from AVK.

* Tweak a comment.

* Check manifold res in correct place.

* Update CHANGELOG.

* Tweak changelog.

* Flip an inequality, run make format.

* Clamp connection radius in CRRTConnect instead of checking connection
motion.

* Rename mStepSize -> mProjectionResolution.

* Expose slack factor.

* Rename slack factor to be less scary.

* Undo rename of mMaxStepsize.

* Rename slack factor again.

* Remove name tag.

Co-authored-by: Brian Hou <[email protected]>

CHANGELOG.md

@@ -2,6 +2,10 @@
 
 ### 0.5.0 (2020-XX-XX)
 
+* Planner
+
+  * Fixed key bugs in CRRTs: [#574](https://github.com/personalrobotics/aikido/pull/574)
+
 ### 0.4.0 (2020-08-27)
 
 * Planner

include/aikido/planner/ompl/CRRT.hpp

@@ -95,6 +95,20 @@ class CRRT : public ::ompl::base::Planner
   /// larger than this distance.
   double getProjectionResolution() const;
 
+  /// Set the slack factor for the resolution of the final path *after*
+  /// projection. During tree extension, any projection that would cause the
+  /// final resolution of the path to exceed this factor times
+  /// `mMaxStepsize` is rejected.
+  /// \param _slackFactor The slack factor enforced for the resolution of the
+  /// final path.
+  void setMaxProjectedStepsizeSlackFactor(double _slackFactor);
+
+  /// Get the slack factor for the resolution of the final path *after*
+  /// projection. During tree extension, any projection that would cause the
+  /// final resolution of the path to exceed this factor times
+  /// `mMaxStepsize` is rejected.
+  double getMaxProjectedStepsizeSlackFactor() const;
+
   /// Set the minimum distance between two states for them to be considered
   /// "equivalent". This is used during extension to determine if a projection
   /// is near enough the previous projection to say progress is no longer being
@@ -209,6 +223,11 @@ class CRRT : public ::ompl::base::Planner
   /// The maximum length of a step before projecting
   double mMaxStepsize;
 
+  /// We multiply this with `mMaxStepsize` to get the maximum length of a step
+  /// *after* projecting: in other words, the resolution of the final path on
+  /// the constraint manifold.
+  double mMaxProjectedStepsizeSlackFactor;
+
   /// The minumum step size along the constraint. Used to determine
   /// when projection is no longer making progress during an extension.
   double mMinStepsize;

include/aikido/planner/ompl/CRRTConnect.hpp

@@ -73,7 +73,9 @@ class CRRTConnect : public CRRT
   /// The goal tree
   TreeData mGoalTree;
 
-  /// Max distance between two trees to consider them connected
+  /// Max distance between two trees to consider them connected. Note that this
+  /// must be less than the resolution used to collision-check, since we assume
+  /// the connection motion is collision-free
   double mConnectionRadius;
 
   /// The pair of states in each tree connected during planning.  Use for

src/planner/ompl/CRRT.cpp

@@ -24,6 +24,7 @@ CRRT::CRRT(
   , mLastGoalMotion(nullptr)
   , mCons(nullptr)
   , mMaxStepsize(0.1)
+  , mMaxProjectedStepsizeSlackFactor(2.0)
   , mMinStepsize(1e-4)
 {
 
@@ -48,6 +49,12 @@ CRRT::CRRT(
       &CRRT::setProjectionResolution,
       &CRRT::getProjectionResolution,
       "0.:1.:10000.");
+  Planner::declareParam<double>(
+      "max_projected_stepsize_slack_factor",
+      this,
+      &CRRT::setMaxProjectedStepsizeSlackFactor,
+      &CRRT::getMaxProjectedStepsizeSlackFactor,
+      "0.:1.:10000.");
   Planner::declareParam<double>(
       "min_step",
       this,
@@ -152,6 +159,18 @@ double CRRT::getProjectionResolution() const
   return mMaxStepsize;
 }
 
+//==============================================================================
+void CRRT::setMaxProjectedStepsizeSlackFactor(double _slackFactor)
+{
+  mMaxProjectedStepsizeSlackFactor = _slackFactor;
+}
+
+//==============================================================================
+double CRRT::getMaxProjectedStepsizeSlackFactor() const
+{
+  return mMaxProjectedStepsizeSlackFactor;
+}
+
 //==============================================================================
 void CRRT::setMinStateDifference(double _mindist)
 {
@@ -335,7 +354,8 @@ CRRT::Motion* CRRT::constrainedExtend(
   while (ptc == false)
   {
 
-    if (distToTarget == 0 || distToTarget - prevDistToTarget >= -mMinStepsize)
+    if (si_->equalStates(cmotion->state, gstate)
+        || prevDistToTarget - distToTarget <= mMinStepsize)
     {
       // reached target or not making progress
       break;
@@ -358,6 +378,17 @@ CRRT::Motion* CRRT::constrainedExtend(
       }
     }
 
+    // NOTE: This check makes sure the resolution of the path on the constraint
+    // manifold and the resolution used for interpolation (*before* projection)
+    // do not differ wildly. In other words, after projection the distance
+    // between the nearest-neighbor and the projected state should lie within a
+    // scalar multiple of `mMaxStepsize`.
+    double manifoldResolution = si_->distance(xstate, cmotion->state);
+    if (manifoldResolution > mMaxProjectedStepsizeSlackFactor * mMaxStepsize)
+    {
+      break;
+    }
+
     if (si_->checkMotion(cmotion->state, xstate))
     {
       // Add the motion to the tree

src/planner/ompl/CRRTConnect.cpp

@@ -87,6 +87,18 @@ void CRRTConnect::clear()
 //==============================================================================
 void CRRTConnect::setConnectionRadius(double radius)
 {
+  if (radius > si_->getStateValidityCheckingResolution())
+  {
+    radius = si_->getStateValidityCheckingResolution();
+
+    ::ompl::msg::log(
+        __FILE__,
+        __LINE__,
+        ::ompl::msg::LOG_WARN,
+        "Passed connection radius was too large. Clamped to: %f \n",
+        radius);
+  }
+
   mConnectionRadius = radius;
 }