Add weights to Rankers

CHANGELOG.md

@@ -12,6 +12,8 @@
 * 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)
+
 
 * State Space
 

include/aikido/distance/ConfigurationRanker.hpp

@@ -10,6 +10,8 @@
 namespace aikido {
 namespace distance {
 
+AIKIDO_DECLARE_POINTERS(ConfigurationRanker)
+
 /// ConfigurationRanker is a base class for ranking configurations.
 /// The rule for evaluating the costs of configurations to rank them
 /// is specified by the concrete classes.
@@ -20,9 +22,12 @@ class ConfigurationRanker
   ///
   /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
   /// \param[in] metaSkeleton Metaskeleton of the robot.
+  /// \param[in] weights Weights over the joints to compute distance.
+  /// Defaults to unit vector.
   ConfigurationRanker(
       statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
-      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton);
+      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+      std::vector<double> weights = std::vector<double>());
 
   /// Destructor
   virtual ~ConfigurationRanker() = default;

include/aikido/distance/JointAvoidanceConfigurationRanker.hpp

@@ -16,11 +16,17 @@ class JointAvoidanceConfigurationRanker : public ConfigurationRanker
   ///
   /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
   /// \param[in] metaSkeleton Metaskeleton of the robot.
+  /// \param[in] weights Weights over joints to compute distance.
+  /// Defaults to unit vector.
   JointAvoidanceConfigurationRanker(
       statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
-      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton);
+      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+      std::vector<double> weights = std::vector<double>());
 
 protected:
+  /// Set limits appropriately to account for infinite limits.
+  void setupJointLimits();
+
   /// Returns cost as negative of distance from position limits.
   double evaluateConfiguration(
       const statespace::dart::MetaSkeletonStateSpace::State* solution)

include/aikido/distance/NominalConfigurationRanker.hpp

@@ -15,11 +15,14 @@ class NominalConfigurationRanker : public ConfigurationRanker
   ///
   /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
   /// \param[in] metaSkeleton Metaskeleton of the robot.
+  /// \param[in] weights Weights over the joints to compute distance.
+  /// Defaults to unit vector.
   /// \param[in] nominalConfiguration Nominal configuration. The current
   /// configuration of \c metaSkeleton is considered if set to \c nullptr.
   NominalConfigurationRanker(
       statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
       ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+      std::vector<double> weights = std::vector<double>(),
       const statespace::CartesianProduct::State* nominalConfiguration
       = nullptr);
 

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp

@@ -22,10 +22,12 @@ class ConfigurationToConfiguration_to_ConfigurationToTSR
   /// \param[in] planner Non-DART ConfigurationToConfigurationPlanner planner to
   /// convert.
   /// \param[in] metaSkeleton MetaSkeleton for adapted planner to operate on.
+  /// \param[in] configurationRanker Ranker to rank configurations.
   ConfigurationToConfiguration_to_ConfigurationToTSR(
       std::shared_ptr<aikido::planner::ConfigurationToConfigurationPlanner>
           planner,
-      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+      distance::ConfigurationRankerPtr configurationRanker = nullptr);
 
   // Documentation inherited.
   virtual trajectory::TrajectoryPtr plan(

include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp

@@ -1,6 +1,7 @@
 #ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRPLANNER_HPP_
 #define AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRPLANNER_HPP_
 
+#include "aikido/distance/ConfigurationRanker.hpp"
 #include "aikido/planner/dart/ConfigurationToTSR.hpp"
 #include "aikido/planner/dart/SingleProblemPlanner.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
@@ -26,9 +27,11 @@ class ConfigurationToTSRPlanner
   ///
   /// \param[in] stateSpace State space that this planner associated with.
   /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  /// \param[in] configurationRanker Ranker to rank configurations.
   ConfigurationToTSRPlanner(
       statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
-      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+      distance::ConfigurationRankerPtr configurationRanker = nullptr);
 
   /// Solves \c problem returning the result to \c result.
   ///
@@ -38,6 +41,9 @@ class ConfigurationToTSRPlanner
       const SolvableProblem& problem, Result* result = nullptr)
       = 0;
   // Note: SolvableProblem is defined in SingleProblemPlanner.
+
+protected:
+  distance::ConfigurationRankerPtr mConfigurationRanker;
 };
 
 } // namespace dart

src/distance/ConfigurationRanker.cpp

@@ -1,16 +1,20 @@
+#include <dart/common/StlHelpers.hpp>
+
 #include "aikido/distance/ConfigurationRanker.hpp"
 
 namespace aikido {
 namespace distance {
 
 using statespace::dart::ConstMetaSkeletonStateSpacePtr;
 using statespace::dart::MetaSkeletonStateSpace;
+using dart::common::make_unique;
 using ::dart::dynamics::ConstMetaSkeletonPtr;
 
 //==============================================================================
 ConfigurationRanker::ConfigurationRanker(
     ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
-    ConstMetaSkeletonPtr metaSkeleton)
+    ConstMetaSkeletonPtr metaSkeleton,
+    std::vector<double> weights)
   : mMetaSkeletonStateSpace(std::move(metaSkeletonStateSpace))
   , mMetaSkeleton(std::move(metaSkeleton))
 {
@@ -20,9 +24,47 @@ ConfigurationRanker::ConfigurationRanker(
   if (!mMetaSkeleton)
     throw std::invalid_argument("MetaSkeleton is nullptr.");
 
-  mDistanceMetric = createDistanceMetricFor(
-      std::dynamic_pointer_cast<const statespace::CartesianProduct>(
-          mMetaSkeletonStateSpace));
+  if (weights.size() != 0)
+  {
+    if (weights.size() != mMetaSkeletonStateSpace->getDimension())
+    {
+      std::stringstream ss;
+      ss << "Size of weights should match "
+        "the dimension of MetaSkeletonStateSpace.";
+      throw std::invalid_argument(ss.str());
+    }
+
+    for (std::size_t i = 0; i < weights.size(); ++i)
+    {
+      if (weights[i] < 0)
+        throw std::invalid_argument("Weights should all be non-negative.");
+    }
+  }
+  else
+  {
+    weights.resize(mMetaSkeletonStateSpace->getDimension());
+    for (std::size_t i = 0; i < weights.size(); ++i)
+    {
+      weights[i] = 1;
+    }
+  }
+
+  // Create a temporary statespace to setup distance metric with weights.
+  auto _sspace = std::dynamic_pointer_cast<statespace::CartesianProduct>(
+      std::const_pointer_cast<MetaSkeletonStateSpace>(mMetaSkeletonStateSpace));
+
+  std::vector<std::pair<DistanceMetricPtr, double>> metrics;
+  metrics.reserve(_sspace->getNumSubspaces());
+
+  for (std::size_t i = 0; i < _sspace->getNumSubspaces(); ++i)
+  {
+    auto subspace = _sspace->getSubspace<>(i);
+    auto metric = createDistanceMetric(std::move(subspace));
+    metrics.emplace_back(std::make_pair(std::move(metric), weights[i]));
+  }
+
+  mDistanceMetric = make_unique<CartesianProductWeighted>(
+      std::move(_sspace), std::move(metrics));
 }
 
 //==============================================================================

src/distance/JointAvoidanceConfigurationRanker.cpp

@@ -9,11 +9,18 @@ using ::dart::dynamics::ConstMetaSkeletonPtr;
 //==============================================================================
 JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker(
     ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
-    ConstMetaSkeletonPtr metaSkeleton)
+    ConstMetaSkeletonPtr metaSkeleton,
+    std::vector<double> weights)
   : ConfigurationRanker(
-        std::move(metaSkeletonStateSpace), std::move(metaSkeleton))
+        std::move(metaSkeletonStateSpace), std::move(metaSkeleton), weights)
   , mLowerLimitsState(mMetaSkeletonStateSpace->createState())
   , mUpperLimitsState(mMetaSkeletonStateSpace->createState())
+{
+  setupJointLimits();
+}
+
+//==============================================================================
+void JointAvoidanceConfigurationRanker::setupJointLimits()
 {
   auto lowerLimits = mMetaSkeleton->getPositionLowerLimits();
   auto upperLimits = mMetaSkeleton->getPositionUpperLimits();

src/distance/NominalConfigurationRanker.cpp

@@ -10,9 +10,10 @@ using ::dart::dynamics::ConstMetaSkeletonPtr;
 NominalConfigurationRanker::NominalConfigurationRanker(
     ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
     ConstMetaSkeletonPtr metaSkeleton,
+    std::vector<double> weights,
     const statespace::CartesianProduct::State* nominalConfiguration)
   : ConfigurationRanker(
-        std::move(metaSkeletonStateSpace), std::move(metaSkeleton))
+        std::move(metaSkeletonStateSpace), std::move(metaSkeleton), weights)
   , mNominalConfiguration(nominalConfiguration)
 {
   if (!mNominalConfiguration)

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

@@ -26,12 +26,13 @@ namespace dart {
 ConfigurationToConfiguration_to_ConfigurationToTSR::
     ConfigurationToConfiguration_to_ConfigurationToTSR(
         std::shared_ptr<planner::ConfigurationToConfigurationPlanner> planner,
-        ::dart::dynamics::MetaSkeletonPtr metaSkeleton)
+        ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+        distance::ConfigurationRankerPtr configurationRanker)
   : PlannerAdapter<planner::ConfigurationToConfigurationPlanner,
                    ConfigurationToTSRPlanner>(
         std::move(planner), std::move(metaSkeleton))
 {
-  // Do nothing
+  mConfigurationRanker = std::move(configurationRanker);
 }
 
 //==============================================================================

src/planner/dart/ConfigurationToTSRPlanner.cpp

@@ -7,9 +7,11 @@ namespace dart {
 //==============================================================================
 ConfigurationToTSRPlanner::ConfigurationToTSRPlanner(
     statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
-    ::dart::dynamics::MetaSkeletonPtr metaSkeleton)
+    ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+    distance::ConfigurationRankerPtr configurationRanker)
   : dart::SingleProblemPlanner<ConfigurationToTSRPlanner, ConfigurationToTSR>(
         std::move(stateSpace), std::move(metaSkeleton))
+  , mConfigurationRanker(std::move(configurationRanker))
 {
   // Do nothing
 }

tests/distance/test_JointAvoidanceConfigurationRanker.cpp

@@ -80,8 +80,25 @@ TEST_F(JointAvoidanceConfigurationRankerTest, Constructor)
       JointAvoidanceConfigurationRanker(mStateSpace, nullptr),
       std::invalid_argument);
 
-  JointAvoidanceConfigurationRanker ranker(mStateSpace, mManipulator);
-  DART_UNUSED(ranker);
+  std::vector<double> negativeWeights{-1, 0};
+  EXPECT_THROW(
+      JointAvoidanceConfigurationRanker(
+          mStateSpace, mManipulator, negativeWeights),
+      std::invalid_argument);
+
+  std::vector<double> wrongDimensionWeights{1};
+  EXPECT_THROW(
+      JointAvoidanceConfigurationRanker(
+          mStateSpace, mManipulator, wrongDimensionWeights),
+      std::invalid_argument);
+
+  JointAvoidanceConfigurationRanker rankerOne(mStateSpace, mManipulator);
+  DART_UNUSED(rankerOne);
+
+  std::vector<double> goodWeights{1, 2};
+  JointAvoidanceConfigurationRanker rankerTwo(
+      mStateSpace, mManipulator, goodWeights);
+  DART_UNUSED(rankerTwo);
 }
 
 TEST_F(JointAvoidanceConfigurationRankerTest, OrderTest)
@@ -111,3 +128,32 @@ TEST_F(JointAvoidanceConfigurationRankerTest, OrderTest)
     EXPECT_EIGEN_EQUAL(rankedState, jointPositions[i], EPS);
   }
 }
+
+TEST_F(JointAvoidanceConfigurationRankerTest, WeightedOrderTest)
+{
+  std::vector<Eigen::Vector2d> jointPositions{Eigen::Vector2d(0.3, 0.1),
+                                              Eigen::Vector2d(0.1, 0.4),
+                                              Eigen::Vector2d(0.2, 0.1)};
+
+  std::vector<aikido::statespace::CartesianProduct::ScopedState> states;
+  for (std::size_t i = 0; i < jointPositions.size(); ++i)
+  {
+    auto state = mStateSpace->createState();
+    mStateSpace->convertPositionsToState(jointPositions[i], state);
+    states.emplace_back(state.clone());
+  }
+
+  std::vector<double> weights{10, 1};
+  JointAvoidanceConfigurationRanker ranker(mStateSpace, mManipulator, weights);
+  ranker.rankConfigurations(states);
+
+  Eigen::VectorXd rankedState(2);
+  jointPositions[0] = Eigen::Vector2d(0.3, 0.1);
+  jointPositions[1] = Eigen::Vector2d(0.2, 0.1);
+  jointPositions[2] = Eigen::Vector2d(0.1, 0.4);
+  for (std::size_t i = 0; i < states.size(); ++i)
+  {
+    mStateSpace->convertStateToPositions(states[i], rankedState);
+    EXPECT_EIGEN_EQUAL(rankedState, jointPositions[i], EPS);
+  }
+}

tests/distance/test_NominalConfigurationRanker.cpp

@@ -67,15 +67,32 @@ class NominalConfigurationRankerTest : public ::testing::Test
 TEST_F(NominalConfigurationRankerTest, Constructor)
 {
   EXPECT_THROW(
-      NominalConfigurationRanker(nullptr, mManipulator, nullptr),
+      NominalConfigurationRanker(nullptr, mManipulator),
       std::invalid_argument);
 
   EXPECT_THROW(
-      NominalConfigurationRanker(mStateSpace, nullptr, nullptr),
+      NominalConfigurationRanker(mStateSpace, nullptr),
       std::invalid_argument);
 
-  NominalConfigurationRanker ranker(mStateSpace, mManipulator, nullptr);
-  DART_UNUSED(ranker);
+  std::vector<double> negativeWeights{-1, 0};
+  EXPECT_THROW(
+      NominalConfigurationRanker(
+          mStateSpace, mManipulator, negativeWeights, nullptr),
+      std::invalid_argument);
+
+  std::vector<double> wrongDimensionWeights{1};
+  EXPECT_THROW(
+      NominalConfigurationRanker(
+          mStateSpace, mManipulator, wrongDimensionWeights, nullptr),
+      std::invalid_argument);
+
+  NominalConfigurationRanker rankerOne(mStateSpace, mManipulator);
+  DART_UNUSED(rankerOne);
+
+  std::vector<double> goodWeights{1, 2};
+  NominalConfigurationRanker rankerTwo(
+      mStateSpace, mManipulator, goodWeights, nullptr);
+  DART_UNUSED(rankerTwo);
 }
 
 TEST_F(NominalConfigurationRankerTest, OrderTest)
@@ -95,8 +112,7 @@ TEST_F(NominalConfigurationRankerTest, OrderTest)
   mManipulator->setPositions(Eigen::Vector2d(0.0, 0.0));
   NominalConfigurationRanker ranker(
       mStateSpace,
-      mManipulator,
-      mStateSpace->getScopedStateFromMetaSkeleton(mManipulator.get()));
+      mManipulator);
   ranker.rankConfigurations(states);
 
   Eigen::VectorXd rankedState(2);
@@ -109,3 +125,37 @@ TEST_F(NominalConfigurationRankerTest, OrderTest)
     EXPECT_EIGEN_EQUAL(rankedState, jointPositions[i], EPS);
   }
 }
+
+TEST_F(NominalConfigurationRankerTest, WeightedOrderTest)
+{
+  std::vector<Eigen::Vector2d> jointPositions{Eigen::Vector2d(0.2, 0.5),
+                                              Eigen::Vector2d(0.1, 0.6),
+                                              Eigen::Vector2d(0.5, 0.1)};
+
+  std::vector<aikido::statespace::CartesianProduct::ScopedState> states;
+  for (std::size_t i = 0; i < jointPositions.size(); ++i)
+  {
+    auto state = mStateSpace->createState();
+    mStateSpace->convertPositionsToState(jointPositions[i], state);
+    states.emplace_back(state.clone());
+  }
+
+  mManipulator->setPositions(Eigen::Vector2d(0.0, 0.0));
+  std::vector<double> weights{10, 1};
+  NominalConfigurationRanker ranker(
+      mStateSpace,
+      mManipulator,
+      weights,
+      mStateSpace->getScopedStateFromMetaSkeleton(mManipulator.get()));
+  ranker.rankConfigurations(states);
+
+  Eigen::VectorXd rankedState(2);
+  jointPositions[0] = Eigen::Vector2d(0.1, 0.6);
+  jointPositions[1] = Eigen::Vector2d(0.2, 0.5);
+  jointPositions[2] = Eigen::Vector2d(0.5, 0.1);
+  for (std::size_t i = 0; i < states.size(); ++i)
+  {
+    mStateSpace->convertStateToPositions(states[i], rankedState);
+    EXPECT_EIGEN_EQUAL(rankedState, jointPositions[i], EPS);
+  }
+}