ConcreteRobot uses ranker for PlantoTSR

CHANGELOG.md

@@ -12,8 +12,7 @@
 * Distance
 
   * Added methods to rank configurations based on specified metric: [#423](https://github.com/personalrobotics/aikido/pull/423)
-  * Added weights as optinal paramter to rankers: [#484] (https://github.com/personalrobotics/aikido/pull/484)
-
+  * Added weights as optional parameter to rankers: [#484](https://github.com/personalrobotics/aikido/pull/484)
 
 * State Space
 
@@ -63,6 +62,7 @@
   * Fixed segmentation fault on 32-bit machines in vector-field planner: [#459](https://github.com/personalrobotics/aikido/pull/459)
   * Updated interface to OMPL planners to follow the style of the new refactored planning API: [#466](https://github.com/personalrobotics/aikido/pull/466)
   * Added convenience function for converting SO(2) trajectories to R1 trajectories, support for postprocessing SO(2) trajectories: [#496](https://github.com/personalrobotics/aikido/pull/496)
+  * Used ConfigurationRanker in TSR planners: [#503](https://github.com/personalrobotics/aikido/pull/503)
 
 * Robot
 

include/aikido/distance/ConfigurationRanker.hpp

@@ -36,7 +36,7 @@ class ConfigurationRanker
   /// \param[in, out] configurations Vector of configurations to rank.
   void rankConfigurations(
       std::vector<statespace::dart::MetaSkeletonStateSpace::ScopedState>&
-          configurations);
+          configurations) const;
 
 protected:
   /// Returns the cost of the configuration.

include/aikido/distance/NominalConfigurationRanker.hpp

@@ -6,6 +6,8 @@
 namespace aikido {
 namespace distance {
 
+AIKIDO_DECLARE_POINTERS(NominalConfigurationRanker)
+
 /// Ranks configurations by their distance from a nominal configuration.
 /// Configurations closer to the nominal configuration are ranked higher.
 class NominalConfigurationRanker : public ConfigurationRanker

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/distance/ConfigurationRanker.cpp

@@ -69,7 +69,7 @@ ConfigurationRanker::ConfigurationRanker(
 
 //==============================================================================
 void ConfigurationRanker::rankConfigurations(
-    std::vector<MetaSkeletonStateSpace::ScopedState>& configurations)
+    std::vector<MetaSkeletonStateSpace::ScopedState>& configurations) const
 {
   std::unordered_map<const MetaSkeletonStateSpace::State*, double> costs;
   costs.reserve(configurations.size());

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

@@ -4,13 +4,16 @@
 #include "aikido/common/RNG.hpp"
 #include "aikido/constraint/dart/InverseKinematicsSampleable.hpp"
 #include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
+#include "aikido/distance/NominalConfigurationRanker.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
 using aikido::common::cloneRNGFrom;
 using aikido::constraint::dart::InverseKinematicsSampleable;
 using aikido::constraint::dart::TSR;
 using aikido::constraint::dart::createSampleableBounds;
+using aikido::distance::ConstConfigurationRankerPtr;
+using aikido::distance::NominalConfigurationRanker;
 using aikido::statespace::dart::MetaSkeletonStateSaver;
 using aikido::statespace::dart::MetaSkeletonStateSpace;
 using ::dart::dynamics::BodyNode;
@@ -27,7 +30,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))
@@ -84,24 +87,59 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan(
   auto saver = MetaSkeletonStateSaver(mMetaSkeleton);
   DART_UNUSED(saver);
 
+  auto robot = mMetaSkeleton->getBodyNode(0)->getSkeleton();
+
+  std::vector<MetaSkeletonStateSpace::ScopedState> configurations;
+
+  // Use a ranker
+  ConstConfigurationRankerPtr configurationRanker(mConfigurationRanker);
+  if (!configurationRanker)
+  {
+    auto nominalState = mMetaSkeletonStateSpace->createState();
+    mMetaSkeletonStateSpace->copyState(startState, nominalState);
+    configurationRanker = std::make_shared<const NominalConfigurationRanker>(
+        mMetaSkeletonStateSpace,
+        mMetaSkeleton,
+        std::vector<double>(),
+        nominalState);
+  }
+
+  // Goal state
   auto goalState = mMetaSkeletonStateSpace->createState();
-  while (generator->canSample())
+
+  // Sample valid configurations first.
+  static const std::size_t maxSamples{100};
+  std::size_t samples = 0;
+  while (samples < maxSamples && generator->canSample())
   {
     // Sample from TSR
-    {
-      std::lock_guard<std::mutex> lock(skeleton->getMutex());
-      bool sampled = generator->sample(goalState);
-      if (!sampled)
-        continue;
-    }
+    std::lock_guard<std::mutex> lock(robot->getMutex());
+    bool sampled = generator->sample(goalState);
 
+    // Increment even if it's not a valid sample since this loop
+    // has to terminate even if none are valid.
+    ++samples;
+
+    if (!sampled)
+      continue;
+
+    configurations.emplace_back(goalState.clone());
+  }
+
+  if (configurations.empty())
+    return nullptr;
+
+  configurationRanker->rankConfigurations(configurations);
+
+  for (std::size_t i = 0; i < configurations.size(); ++i)
+  {
     // Create ConfigurationToConfiguration Problem.
     // NOTE: This is done here because the ConfigurationToConfiguration
     // problem stores a *cloned* scoped state of the passed state.
     auto delegateProblem = ConfigurationToConfiguration(
         mMetaSkeletonStateSpace,
         startState,
-        goalState,
+        configurations[i],
         problem.getConstraint());
 
     auto traj = mDelegate->plan(delegateProblem, result);

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);
 }
 
 //==============================================================================