diff --git a/CHANGELOG.md b/CHANGELOG.md index 905653aeb8..6c7e25e09d 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/include/aikido/distance/ConfigurationRanker.hpp b/include/aikido/distance/ConfigurationRanker.hpp index 8e308f40de..5e4fb2c284 100644 --- a/include/aikido/distance/ConfigurationRanker.hpp +++ b/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 weights = std::vector()); /// Destructor virtual ~ConfigurationRanker() = default; diff --git a/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp b/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp index c84ec38cc2..4025a84446 100644 --- a/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp +++ b/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 weights = std::vector()); 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) diff --git a/include/aikido/distance/NominalConfigurationRanker.hpp b/include/aikido/distance/NominalConfigurationRanker.hpp index c4a2ee943f..46762cf5f3 100644 --- a/include/aikido/distance/NominalConfigurationRanker.hpp +++ b/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 weights = std::vector(), const statespace::CartesianProduct::State* nominalConfiguration = nullptr); diff --git a/include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp b/include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp index ccd664b0aa..e5cc98590b 100644 --- a/include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp +++ b/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 planner, - ::dart::dynamics::MetaSkeletonPtr metaSkeleton); + ::dart::dynamics::MetaSkeletonPtr metaSkeleton, + distance::ConfigurationRankerPtr configurationRanker = nullptr); // Documentation inherited. virtual trajectory::TrajectoryPtr plan( diff --git a/include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp b/include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp index 7639ca2b15..1bb0712c8c 100644 --- a/include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp +++ b/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 diff --git a/src/distance/ConfigurationRanker.cpp b/src/distance/ConfigurationRanker.cpp index 09d15dd66d..a5fa10aa0e 100644 --- a/src/distance/ConfigurationRanker.cpp +++ b/src/distance/ConfigurationRanker.cpp @@ -1,3 +1,5 @@ +#include + #include "aikido/distance/ConfigurationRanker.hpp" namespace aikido { @@ -5,12 +7,14 @@ 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 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( - 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( + std::const_pointer_cast(mMetaSkeletonStateSpace)); + + std::vector> 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( + std::move(_sspace), std::move(metrics)); } //============================================================================== diff --git a/src/distance/JointAvoidanceConfigurationRanker.cpp b/src/distance/JointAvoidanceConfigurationRanker.cpp index 0740eaa175..2d90a4230b 100644 --- a/src/distance/JointAvoidanceConfigurationRanker.cpp +++ b/src/distance/JointAvoidanceConfigurationRanker.cpp @@ -9,11 +9,18 @@ using ::dart::dynamics::ConstMetaSkeletonPtr; //============================================================================== JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker( ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, - ConstMetaSkeletonPtr metaSkeleton) + ConstMetaSkeletonPtr metaSkeleton, + std::vector 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(); diff --git a/src/distance/NominalConfigurationRanker.cpp b/src/distance/NominalConfigurationRanker.cpp index f2f14e9974..b1ffeba05e 100644 --- a/src/distance/NominalConfigurationRanker.cpp +++ b/src/distance/NominalConfigurationRanker.cpp @@ -10,9 +10,10 @@ using ::dart::dynamics::ConstMetaSkeletonPtr; NominalConfigurationRanker::NominalConfigurationRanker( ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, ConstMetaSkeletonPtr metaSkeleton, + std::vector weights, const statespace::CartesianProduct::State* nominalConfiguration) : ConfigurationRanker( - std::move(metaSkeletonStateSpace), std::move(metaSkeleton)) + std::move(metaSkeletonStateSpace), std::move(metaSkeleton), weights) , mNominalConfiguration(nominalConfiguration) { if (!mNominalConfiguration) diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp index 1c5e8f64cc..ff0746323e 100644 --- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp @@ -26,12 +26,13 @@ namespace dart { ConfigurationToConfiguration_to_ConfigurationToTSR:: ConfigurationToConfiguration_to_ConfigurationToTSR( std::shared_ptr planner, - ::dart::dynamics::MetaSkeletonPtr metaSkeleton) + ::dart::dynamics::MetaSkeletonPtr metaSkeleton, + distance::ConfigurationRankerPtr configurationRanker) : PlannerAdapter( std::move(planner), std::move(metaSkeleton)) { - // Do nothing + mConfigurationRanker = std::move(configurationRanker); } //============================================================================== diff --git a/src/planner/dart/ConfigurationToTSRPlanner.cpp b/src/planner/dart/ConfigurationToTSRPlanner.cpp index 8130174271..9c9edfec18 100644 --- a/src/planner/dart/ConfigurationToTSRPlanner.cpp +++ b/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( std::move(stateSpace), std::move(metaSkeleton)) + , mConfigurationRanker(std::move(configurationRanker)) { // Do nothing } diff --git a/tests/distance/test_JointAvoidanceConfigurationRanker.cpp b/tests/distance/test_JointAvoidanceConfigurationRanker.cpp index 79692ab5f4..b2cceb822e 100644 --- a/tests/distance/test_JointAvoidanceConfigurationRanker.cpp +++ b/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 negativeWeights{-1, 0}; + EXPECT_THROW( + JointAvoidanceConfigurationRanker( + mStateSpace, mManipulator, negativeWeights), + std::invalid_argument); + + std::vector wrongDimensionWeights{1}; + EXPECT_THROW( + JointAvoidanceConfigurationRanker( + mStateSpace, mManipulator, wrongDimensionWeights), + std::invalid_argument); + + JointAvoidanceConfigurationRanker rankerOne(mStateSpace, mManipulator); + DART_UNUSED(rankerOne); + + std::vector 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 jointPositions{Eigen::Vector2d(0.3, 0.1), + Eigen::Vector2d(0.1, 0.4), + Eigen::Vector2d(0.2, 0.1)}; + + std::vector 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 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); + } +} diff --git a/tests/distance/test_NominalConfigurationRanker.cpp b/tests/distance/test_NominalConfigurationRanker.cpp index 313d9a3dc5..b3b22906b4 100644 --- a/tests/distance/test_NominalConfigurationRanker.cpp +++ b/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 negativeWeights{-1, 0}; + EXPECT_THROW( + NominalConfigurationRanker( + mStateSpace, mManipulator, negativeWeights, nullptr), + std::invalid_argument); + + std::vector wrongDimensionWeights{1}; + EXPECT_THROW( + NominalConfigurationRanker( + mStateSpace, mManipulator, wrongDimensionWeights, nullptr), + std::invalid_argument); + + NominalConfigurationRanker rankerOne(mStateSpace, mManipulator); + DART_UNUSED(rankerOne); + + std::vector 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 jointPositions{Eigen::Vector2d(0.2, 0.5), + Eigen::Vector2d(0.1, 0.6), + Eigen::Vector2d(0.5, 0.1)}; + + std::vector 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 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); + } +}