ConcreteRobot uses ranker for PlantoTSR

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp

@@ -23,11 +23,13 @@ class ConfigurationToConfiguration_to_ConfigurationToTSR
   /// convert.
   /// \param[in] metaSkeleton MetaSkeleton for adapted planner to operate on.
   /// \param[in] configurationRanker Ranker to rank configurations.
+  /// \param[in] ranker Ranker to rank the sampled configurations. If nullptr,
+  /// NominalConfigurationRanker is used with the current metaSkeleton pose.
   ConfigurationToConfiguration_to_ConfigurationToTSR(
       std::shared_ptr<aikido::planner::ConfigurationToConfigurationPlanner>
           planner,
       ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
-      distance::ConfigurationRankerPtr configurationRanker = nullptr);
+      distance::ConstConfigurationRankerPtr configurationRanker = nullptr);
 
   // Documentation inherited.
   virtual trajectory::TrajectoryPtr plan(

include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp

@@ -31,7 +31,7 @@ class ConfigurationToTSRPlanner
   ConfigurationToTSRPlanner(
       statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
       ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
-      distance::ConfigurationRankerPtr configurationRanker = nullptr);
+      distance::ConstConfigurationRankerPtr configurationRanker = nullptr);
 
   /// Solves \c problem returning the result to \c result.
   ///
@@ -43,7 +43,7 @@ class ConfigurationToTSRPlanner
   // Note: SolvableProblem is defined in SingleProblemPlanner.
 
 protected:
-  distance::ConfigurationRankerPtr mConfigurationRanker;
+  distance::ConstConfigurationRankerPtr mConfigurationRanker;
 };
 
 } // namespace dart

include/aikido/robot/ConcreteRobot.hpp

@@ -11,6 +11,7 @@
 #include "aikido/constraint/dart/CollisionFree.hpp"
 #include "aikido/constraint/dart/TSR.hpp"
 #include "aikido/control/TrajectoryExecutor.hpp"
+#include "aikido/distance/ConfigurationRanker.hpp"
 #include "aikido/planner/parabolic/ParabolicSmoother.hpp"
 #include "aikido/planner/parabolic/ParabolicTimer.hpp"
 #include "aikido/robot/Robot.hpp"
@@ -191,6 +192,8 @@ class ConcreteRobot : public Robot
   /// \param[in] collisionFree Testable constraint to check for collision.
   /// \param[in] timelimit Max time (seconds) to spend per planning to each IK
   /// \param[in] maxNumTrials Max numer of trials to plan.
+  /// \param[in] ranker Ranker to rank the sampled configurations. If nullptr,
+  /// NominalConfigurationRanker is used with the current metaSkeleton pose.
   /// \return Trajectory to a sample in TSR, or nullptr if planning fails.
   aikido::trajectory::TrajectoryPtr planToTSR(
       const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
@@ -199,7 +202,8 @@ class ConcreteRobot : public Robot
       const aikido::constraint::dart::TSRPtr& tsr,
       const aikido::constraint::dart::CollisionFreePtr& collisionFree,
       double timelimit,
-      std::size_t maxNumTrials);
+      std::size_t maxNumTrials,
+      const distance::ConstConfigurationRankerPtr& ranker = nullptr);
 
   /// TODO: Replace this with Problem interface.
   /// Returns a Trajectory that moves the configuration of the metakeleton such

include/aikido/robot/util.hpp

@@ -9,6 +9,7 @@
 #include "aikido/constraint/dart/CollisionFree.hpp"
 #include "aikido/constraint/dart/TSR.hpp"
 #include "aikido/control/TrajectoryExecutor.hpp"
+#include "aikido/distance/ConfigurationRanker.hpp"
 #include "aikido/io/yaml.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
@@ -133,6 +134,8 @@ trajectory::TrajectoryPtr planToConfigurations(
 /// \param[in] rng Random number generator
 /// \param[in] timelimit Max time (seconds) to spend per planning to each IK
 /// \param[in] maxNumTrials Number of retries before failure.
+/// \param[in] ranker Ranker to rank the sampled configurations. If nullptr,
+/// NominalConfigurationRanker is used with the current metaSkeleton pose.
 /// \return Trajectory to a sample in TSR, or nullptr if planning fails.
 trajectory::TrajectoryPtr planToTSR(
     const statespace::dart::MetaSkeletonStateSpacePtr& space,
@@ -142,7 +145,8 @@ trajectory::TrajectoryPtr planToTSR(
     const constraint::TestablePtr& collisionTestable,
     common::RNG* rng,
     double timelimit,
-    std::size_t maxNumTrials);
+    std::size_t maxNumTrials,
+    const distance::ConstConfigurationRankerPtr& ranker = nullptr);
 
 /// Returns a Trajectory that moves the configuration of the metakeleton such
 /// that the specified bodynode is set to a sample in a goal TSR and

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

@@ -27,7 +27,7 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::
     ConfigurationToConfiguration_to_ConfigurationToTSR(
         std::shared_ptr<planner::ConfigurationToConfigurationPlanner> planner,
         ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
-        distance::ConfigurationRankerPtr configurationRanker)
+        distance::ConstConfigurationRankerPtr configurationRanker)
   : PlannerAdapter<planner::ConfigurationToConfigurationPlanner,
                    ConfigurationToTSRPlanner>(
         std::move(planner), std::move(metaSkeleton))

src/planner/dart/ConfigurationToTSRPlanner.cpp

@@ -8,7 +8,7 @@ namespace dart {
 ConfigurationToTSRPlanner::ConfigurationToTSRPlanner(
     statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
     ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
-    distance::ConfigurationRankerPtr configurationRanker)
+    distance::ConstConfigurationRankerPtr configurationRanker)
   : dart::SingleProblemPlanner<ConfigurationToTSRPlanner, ConfigurationToTSR>(
         std::move(stateSpace), std::move(metaSkeleton))
   , mConfigurationRanker(std::move(configurationRanker))

src/robot/ConcreteRobot.cpp

@@ -401,7 +401,8 @@ TrajectoryPtr ConcreteRobot::planToTSR(
     const TSRPtr& tsr,
     const CollisionFreePtr& collisionFree,
     double timelimit,
-    std::size_t maxNumTrials)
+    std::size_t maxNumTrials,
+    const distance::ConstConfigurationRankerPtr& ranker)
 {
   auto collisionConstraint
       = getFullCollisionConstraint(stateSpace, metaSkeleton, collisionFree);
@@ -414,7 +415,8 @@ TrajectoryPtr ConcreteRobot::planToTSR(
       collisionConstraint,
       cloneRNG().get(),
       timelimit,
-      maxNumTrials);
+      maxNumTrials,
+      ranker);
 }
 
 //==============================================================================

src/robot/util.cpp

@@ -1,4 +1,6 @@
 #include "aikido/robot/util.hpp"
+
+#include <algorithm>
 #include <dart/common/Console.hpp>
 #include <dart/common/StlHelpers.hpp>
 #include <dart/common/Timer.hpp>
@@ -13,6 +15,7 @@
 #include "aikido/constraint/dart/FrameTestable.hpp"
 #include "aikido/constraint/dart/InverseKinematicsSampleable.hpp"
 #include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
+#include "aikido/distance/NominalConfigurationRanker.hpp"
 #include "aikido/distance/defaults.hpp"
 #include "aikido/planner/PlanningResult.hpp"
 #include "aikido/planner/SnapConfigurationToConfigurationPlanner.hpp"
@@ -32,6 +35,8 @@ namespace aikido {
 namespace robot {
 namespace util {
 
+using common::cloneRNGFrom;
+using common::RNG;
 using constraint::dart::CollisionFreePtr;
 using constraint::dart::InverseKinematicsSampleable;
 using constraint::dart::TSR;
@@ -41,6 +46,11 @@ using constraint::dart::createProjectableBounds;
 using constraint::dart::createSampleableBounds;
 using constraint::dart::createTestableBounds;
 using distance::createDistanceMetric;
+using distance::ConfigurationRankerPtr;
+using distance::NominalConfigurationRanker;
+using planner::ConfigurationToConfiguration;
+using planner::SnapConfigurationToConfigurationPlanner;
+using planner::ompl::OMPLConfigurationToConfigurationPlanner;
 using statespace::GeodesicInterpolator;
 using statespace::dart::MetaSkeletonStateSpacePtr;
 using statespace::dart::MetaSkeletonStateSaver;
@@ -50,11 +60,6 @@ using trajectory::TrajectoryPtr;
 using trajectory::Interpolated;
 using trajectory::InterpolatedPtr;
 using trajectory::SplinePtr;
-using common::cloneRNGFrom;
-using common::RNG;
-using planner::ConfigurationToConfiguration;
-using planner::SnapConfigurationToConfigurationPlanner;
-using planner::ompl::OMPLConfigurationToConfigurationPlanner;
 
 using dart::collision::FCLCollisionDetector;
 using dart::dynamics::BodyNodePtr;
@@ -170,7 +175,8 @@ trajectory::TrajectoryPtr planToTSR(
     const TestablePtr& collisionTestable,
     RNG* rng,
     double timelimit,
-    std::size_t maxNumTrials)
+    std::size_t maxNumTrials,
+    const distance::ConfigurationRankerPtr& ranker)
 {
   // Create an IK solver with metaSkeleton dofs.
   auto ik = InverseKinematics::create(bn);
@@ -217,49 +223,70 @@ trajectory::TrajectoryPtr planToTSR(
 
   auto robot = metaSkeleton->getBodyNode(0)->getSkeleton();
   SnapConfigurationToConfigurationPlanner::Result pResult;
-
-  auto planner = std::make_shared<SnapConfigurationToConfigurationPlanner>(
+  auto snapPlanner = std::make_shared<SnapConfigurationToConfigurationPlanner>(
       space, std::make_shared<GeodesicInterpolator>(space));
+
+  std::vector<MetaSkeletonStateSpace::ScopedState> configurations;
+
+  // Use a ranker
+  ConfigurationRankerPtr configurationRanker(ranker);
+  if (!ranker)
+  {
+    auto nominalState = space->createState();
+    space->copyState(startState, nominalState);
+    configurationRanker = std::make_shared<NominalConfigurationRanker>(
+        space, metaSkeleton, std::vector<double>(), nominalState);
+  }
+
   while (snapSamples < maxSnapSamples && generator->canSample())
   {
     // Sample from TSR
-    {
-      std::lock_guard<std::mutex> lock(robot->getMutex());
-      bool sampled = generator->sample(goalState);
-      if (!sampled)
-        continue;
+    std::lock_guard<std::mutex> lock(robot->getMutex());
+    bool sampled = generator->sample(goalState);
 
-      // Set to start state
-      space->setState(metaSkeleton.get(), startState);
-    }
+    // Increment even if it's not a valid sample since this loop
+    // has to terminate even if none are valid.
     ++snapSamples;
 
-    // Create ConfigurationToConfiguration Problem.
-    // NOTE: This is done here because the ConfigurationToConfiguration
-    // problem stores a *cloned* scoped state of the passed state.
+    if (!sampled)
+      continue;
+
+    configurations.emplace_back(goalState.clone());
+  }
+
+  {
+    // Set to start state
+    std::lock_guard<std::mutex> lock(robot->getMutex());
+    space->setState(metaSkeleton.get(), startState);
+  }
+
+  if (configurations.size() == 0)
+    return nullptr;
+
+  configurationRanker->rankConfigurations(configurations);
+
+  // Try snap planner first
+  for (std::size_t i = 0; i < configurations.size(); ++i)
+  {
     auto problem = ConfigurationToConfiguration(
-        space, startState, goalState, collisionTestable);
-    auto traj = planner->plan(problem, &pResult);
+        space, startState, configurations[i], collisionTestable);
+
+    auto traj = snapPlanner->plan(problem, &pResult);
 
     if (traj)
+    {
+      space->setState(metaSkeleton.get(), startState);
       return traj;
+    }
   }
 
   // Start the timer
   dart::common::Timer timer;
   timer.start();
-  while (timer.getElapsedTime() < timelimit && generator->canSample())
+  for (std::size_t i = 0; i < configurations.size(); ++i)
   {
-    // Sample from TSR
-    {
-      std::lock_guard<std::mutex> lock(robot->getMutex());
-      bool sampled = generator->sample(goalState);
-      if (!sampled)
-        continue;
-
-      // Set to start state
-      space->setState(metaSkeleton.get(), startState);
-    }
+    auto problem = ConfigurationToConfiguration(
+        space, startState, configurations[i], collisionTestable);
 
     auto traj = planToConfiguration(
         space,
@@ -272,7 +299,6 @@ trajectory::TrajectoryPtr planToTSR(
     if (traj)
       return traj;
   }
-
   return nullptr;
 }
 
@@ -401,7 +427,8 @@ trajectory::TrajectoryPtr planToEndEffectorOffset(
   auto startState = space->createState();
   space->getState(metaSkeleton.get(), startState);
 
-  auto minDistance = distance - vfParameters.negativeDistanceTolerance;
+  auto minDistance
+      = std::max(0.0, distance - vfParameters.negativeDistanceTolerance);
   auto maxDistance = distance + vfParameters.positiveDistanceTolerance;
 
   auto traj = planner::vectorfield::planToEndEffectorOffset(