From 1ae413dcdb994eca4156d006921061a794154c83 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 15 Jan 2019 19:54:36 -0800 Subject: [PATCH 1/5] Weighted Configuration Ranker --- .../aikido/distance/ConfigurationRanker.hpp | 12 ++++ .../JointAvoidanceConfigurationRanker.hpp | 13 +++++ .../distance/NominalConfigurationRanker.hpp | 14 +++++ ...nToConfiguration_to_ConfigurationToTSR.hpp | 4 +- .../dart/ConfigurationToTSRPlanner.hpp | 8 ++- src/distance/ConfigurationRanker.cpp | 46 ++++++++++++++++ .../JointAvoidanceConfigurationRanker.cpp | 19 +++++++ src/distance/NominalConfigurationRanker.cpp | 16 ++++++ ...nToConfiguration_to_ConfigurationToTSR.cpp | 5 +- .../dart/ConfigurationToTSRPlanner.cpp | 4 +- ...test_JointAvoidanceConfigurationRanker.cpp | 50 ++++++++++++++++- .../test_NominalConfigurationRanker.cpp | 55 ++++++++++++++++++- 12 files changed, 237 insertions(+), 9 deletions(-) diff --git a/include/aikido/distance/ConfigurationRanker.hpp b/include/aikido/distance/ConfigurationRanker.hpp index 8e308f40de..22204c5e96 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. @@ -24,6 +26,16 @@ class ConfigurationRanker statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton); + /// Constructor + /// + /// \param[in] metaSkeletonStateSpace Statespace of the skeleton. + /// \param[in] metaSkeleton Metaskeleton of the robot. + /// \param[in] weights Weights over the joints to compute distance. + ConfigurationRanker( + statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, + ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton, + std::vector weights); + /// Destructor virtual ~ConfigurationRanker() = default; diff --git a/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp b/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp index c84ec38cc2..3a8c288d0d 100644 --- a/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp +++ b/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp @@ -20,7 +20,20 @@ class JointAvoidanceConfigurationRanker : public ConfigurationRanker statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton); + /// Constructor + /// + /// \param[in] metaSkeletonStateSpace Statespace of the skeleton. + /// \param[in] metaSkeleton Metaskeleton of the robot. + /// \param[in] weights Weights over joints to compute distance. + JointAvoidanceConfigurationRanker( + statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, + ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton, + std::vector weights); + 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..8fb596280c 100644 --- a/include/aikido/distance/NominalConfigurationRanker.hpp +++ b/include/aikido/distance/NominalConfigurationRanker.hpp @@ -23,6 +23,20 @@ class NominalConfigurationRanker : public ConfigurationRanker const statespace::CartesianProduct::State* nominalConfiguration = nullptr); + /// Constructor + /// + /// \param[in] metaSkeletonStateSpace Statespace of the skeleton. + /// \param[in] metaSkeleton Metaskeleton of the robot. + /// \param[in] weights Weights over the joints to compute distance. + /// \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, + const statespace::CartesianProduct::State* nominalConfiguration + = nullptr); + protected: /// Returns cost as distance from the Nominal Configuration. double evaluateConfiguration( 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..6dab8543be 100644 --- a/src/distance/ConfigurationRanker.cpp +++ b/src/distance/ConfigurationRanker.cpp @@ -1,3 +1,5 @@ +#include + #include "aikido/distance/ConfigurationRanker.hpp" namespace aikido { @@ -5,6 +7,7 @@ namespace distance { using statespace::dart::ConstMetaSkeletonStateSpacePtr; using statespace::dart::MetaSkeletonStateSpace; +using dart::common::make_unique; using ::dart::dynamics::ConstMetaSkeletonPtr; //============================================================================== @@ -25,6 +28,49 @@ ConfigurationRanker::ConfigurationRanker( mMetaSkeletonStateSpace)); } +//============================================================================== +ConfigurationRanker::ConfigurationRanker( + ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, + ConstMetaSkeletonPtr metaSkeleton, + std::vector weights) + : mMetaSkeletonStateSpace(std::move(metaSkeletonStateSpace)) + , mMetaSkeleton(std::move(metaSkeleton)) +{ + if (!mMetaSkeletonStateSpace) + throw std::invalid_argument("MetaSkeletonStateSpace is nullptr."); + + if (!mMetaSkeleton) + throw std::invalid_argument("MetaSkeleton is nullptr."); + + if (mMetaSkeletonStateSpace->getDimension() != weights.size()) + throw std::invalid_argument( + "Weights should have the same dimension as the " + "MetaSkeletonStateSpace."); + + for (std::size_t i = 0; i < weights.size(); ++i) + { + if (weights[i] < 0) + throw std::invalid_argument("Weights should all be non-negative."); + } + + // 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)); +} + //============================================================================== void ConfigurationRanker::rankConfigurations( std::vector& configurations) diff --git a/src/distance/JointAvoidanceConfigurationRanker.cpp b/src/distance/JointAvoidanceConfigurationRanker.cpp index 0740eaa175..99b7c0f136 100644 --- a/src/distance/JointAvoidanceConfigurationRanker.cpp +++ b/src/distance/JointAvoidanceConfigurationRanker.cpp @@ -14,6 +14,25 @@ JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker( std::move(metaSkeletonStateSpace), std::move(metaSkeleton)) , mLowerLimitsState(mMetaSkeletonStateSpace->createState()) , mUpperLimitsState(mMetaSkeletonStateSpace->createState()) +{ + setupJointLimits(); +} + +//============================================================================== +JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker( + ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, + ConstMetaSkeletonPtr metaSkeleton, + std::vector weights) + : ConfigurationRanker( + 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..13ffa347b6 100644 --- a/src/distance/NominalConfigurationRanker.cpp +++ b/src/distance/NominalConfigurationRanker.cpp @@ -21,6 +21,22 @@ NominalConfigurationRanker::NominalConfigurationRanker( mMetaSkeleton.get()); } +//============================================================================== +NominalConfigurationRanker::NominalConfigurationRanker( + ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, + ConstMetaSkeletonPtr metaSkeleton, + std::vector weights, + const statespace::CartesianProduct::State* nominalConfiguration) + : ConfigurationRanker( + std::move(metaSkeletonStateSpace), std::move(metaSkeleton), weights) + , mNominalConfiguration(nominalConfiguration) +{ + if (!mNominalConfiguration) + mNominalConfiguration + = mMetaSkeletonStateSpace->getScopedStateFromMetaSkeleton( + mMetaSkeleton.get()); +} + //============================================================================== double NominalConfigurationRanker::evaluateConfiguration( const statespace::dart::MetaSkeletonStateSpace::State* solution) const 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..4dde2fcc23 100644 --- a/tests/distance/test_NominalConfigurationRanker.cpp +++ b/tests/distance/test_NominalConfigurationRanker.cpp @@ -74,8 +74,25 @@ TEST_F(NominalConfigurationRankerTest, Constructor) NominalConfigurationRanker(mStateSpace, nullptr, 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, nullptr); + DART_UNUSED(rankerOne); + + std::vector goodWeights{1, 2}; + NominalConfigurationRanker rankerTwo( + mStateSpace, mManipulator, goodWeights, nullptr); + DART_UNUSED(rankerTwo); } TEST_F(NominalConfigurationRankerTest, OrderTest) @@ -109,3 +126,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); + } +} From 4687d411f3f310c52d10aa00245750073742fb7c Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 15 Jan 2019 20:01:04 -0800 Subject: [PATCH 2/5] Update to a version with PR comments addressed --- .../aikido/distance/ConfigurationRanker.hpp | 15 ++----- .../JointAvoidanceConfigurationRanker.hpp | 11 +---- .../distance/NominalConfigurationRanker.hpp | 15 +------ src/distance/ConfigurationRanker.cpp | 40 +++++++------------ .../JointAvoidanceConfigurationRanker.cpp | 12 ------ src/distance/NominalConfigurationRanker.cpp | 15 ------- 6 files changed, 22 insertions(+), 86 deletions(-) diff --git a/include/aikido/distance/ConfigurationRanker.hpp b/include/aikido/distance/ConfigurationRanker.hpp index 22204c5e96..47d094dae4 100644 --- a/include/aikido/distance/ConfigurationRanker.hpp +++ b/include/aikido/distance/ConfigurationRanker.hpp @@ -7,34 +7,27 @@ #include +AIKIDO_DECLARE_POINTERS(ConfigurationRanker) + 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. class ConfigurationRanker { public: - /// Constructor - /// - /// \param[in] metaSkeletonStateSpace Statespace of the skeleton. - /// \param[in] metaSkeleton Metaskeleton of the robot. - ConfigurationRanker( - statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, - ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton); - /// Constructor /// /// \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, - std::vector weights); + std::vector weights = std::vector()); /// Destructor virtual ~ConfigurationRanker() = default; diff --git a/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp b/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp index 3a8c288d0d..4025a84446 100644 --- a/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp +++ b/include/aikido/distance/JointAvoidanceConfigurationRanker.hpp @@ -12,23 +12,16 @@ namespace distance { class JointAvoidanceConfigurationRanker : public ConfigurationRanker { public: - /// Constructor - /// - /// \param[in] metaSkeletonStateSpace Statespace of the skeleton. - /// \param[in] metaSkeleton Metaskeleton of the robot. - JointAvoidanceConfigurationRanker( - statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, - ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton); - /// Constructor /// /// \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, - std::vector weights); + std::vector weights = std::vector()); protected: /// Set limits appropriately to account for infinite limits. diff --git a/include/aikido/distance/NominalConfigurationRanker.hpp b/include/aikido/distance/NominalConfigurationRanker.hpp index 8fb596280c..46762cf5f3 100644 --- a/include/aikido/distance/NominalConfigurationRanker.hpp +++ b/include/aikido/distance/NominalConfigurationRanker.hpp @@ -11,29 +11,18 @@ namespace distance { class NominalConfigurationRanker : public ConfigurationRanker { public: - /// Constructor - /// - /// \param[in] metaSkeletonStateSpace Statespace of the skeleton. - /// \param[in] metaSkeleton Metaskeleton of the robot. - /// \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, - const statespace::CartesianProduct::State* nominalConfiguration - = nullptr); - /// Constructor /// /// \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 weights = std::vector(), const statespace::CartesianProduct::State* nominalConfiguration = nullptr); diff --git a/src/distance/ConfigurationRanker.cpp b/src/distance/ConfigurationRanker.cpp index 6dab8543be..9af4fdf567 100644 --- a/src/distance/ConfigurationRanker.cpp +++ b/src/distance/ConfigurationRanker.cpp @@ -10,24 +10,6 @@ using statespace::dart::MetaSkeletonStateSpace; using dart::common::make_unique; using ::dart::dynamics::ConstMetaSkeletonPtr; -//============================================================================== -ConfigurationRanker::ConfigurationRanker( - ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, - ConstMetaSkeletonPtr metaSkeleton) - : mMetaSkeletonStateSpace(std::move(metaSkeletonStateSpace)) - , mMetaSkeleton(std::move(metaSkeleton)) -{ - if (!mMetaSkeletonStateSpace) - throw std::invalid_argument("MetaSkeletonStateSpace is nullptr."); - - if (!mMetaSkeleton) - throw std::invalid_argument("MetaSkeleton is nullptr."); - - mDistanceMetric = createDistanceMetricFor( - std::dynamic_pointer_cast( - mMetaSkeletonStateSpace)); -} - //============================================================================== ConfigurationRanker::ConfigurationRanker( ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, @@ -42,15 +24,21 @@ ConfigurationRanker::ConfigurationRanker( if (!mMetaSkeleton) throw std::invalid_argument("MetaSkeleton is nullptr."); - if (mMetaSkeletonStateSpace->getDimension() != weights.size()) - throw std::invalid_argument( - "Weights should have the same dimension as the " - "MetaSkeletonStateSpace."); - - for (std::size_t i = 0; i < weights.size(); ++i) + if (weights.size() != 0) + { + 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 { - if (weights[i] < 0) - throw std::invalid_argument("Weights should all be non-negative."); + 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. diff --git a/src/distance/JointAvoidanceConfigurationRanker.cpp b/src/distance/JointAvoidanceConfigurationRanker.cpp index 99b7c0f136..2d90a4230b 100644 --- a/src/distance/JointAvoidanceConfigurationRanker.cpp +++ b/src/distance/JointAvoidanceConfigurationRanker.cpp @@ -6,18 +6,6 @@ namespace distance { using statespace::dart::ConstMetaSkeletonStateSpacePtr; using ::dart::dynamics::ConstMetaSkeletonPtr; -//============================================================================== -JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker( - ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, - ConstMetaSkeletonPtr metaSkeleton) - : ConfigurationRanker( - std::move(metaSkeletonStateSpace), std::move(metaSkeleton)) - , mLowerLimitsState(mMetaSkeletonStateSpace->createState()) - , mUpperLimitsState(mMetaSkeletonStateSpace->createState()) -{ - setupJointLimits(); -} - //============================================================================== JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker( ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, diff --git a/src/distance/NominalConfigurationRanker.cpp b/src/distance/NominalConfigurationRanker.cpp index 13ffa347b6..b1ffeba05e 100644 --- a/src/distance/NominalConfigurationRanker.cpp +++ b/src/distance/NominalConfigurationRanker.cpp @@ -6,21 +6,6 @@ namespace distance { using statespace::dart::ConstMetaSkeletonStateSpacePtr; using ::dart::dynamics::ConstMetaSkeletonPtr; -//============================================================================== -NominalConfigurationRanker::NominalConfigurationRanker( - ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, - ConstMetaSkeletonPtr metaSkeleton, - const statespace::CartesianProduct::State* nominalConfiguration) - : ConfigurationRanker( - std::move(metaSkeletonStateSpace), std::move(metaSkeleton)) - , mNominalConfiguration(nominalConfiguration) -{ - if (!mNominalConfiguration) - mNominalConfiguration - = mMetaSkeletonStateSpace->getScopedStateFromMetaSkeleton( - mMetaSkeleton.get()); -} - //============================================================================== NominalConfigurationRanker::NominalConfigurationRanker( ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, From b316bacf5cb8ba09b0a7c17c947c2514ffddb263 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 15 Jan 2019 20:04:56 -0800 Subject: [PATCH 3/5] Fix pointer declaration namespace --- include/aikido/distance/ConfigurationRanker.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/aikido/distance/ConfigurationRanker.hpp b/include/aikido/distance/ConfigurationRanker.hpp index 47d094dae4..5e4fb2c284 100644 --- a/include/aikido/distance/ConfigurationRanker.hpp +++ b/include/aikido/distance/ConfigurationRanker.hpp @@ -7,11 +7,11 @@ #include -AIKIDO_DECLARE_POINTERS(ConfigurationRanker) - 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. From afaac409bc7c2fd8806acc759f83c373083c6d7d Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Wed, 23 Jan 2019 00:21:04 -0800 Subject: [PATCH 4/5] Add dimension check for ConfigurationRanker weights and fix tests for NominalConfigurationRanker --- src/distance/ConfigurationRanker.cpp | 8 ++++++++ tests/distance/test_NominalConfigurationRanker.cpp | 9 ++++----- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/distance/ConfigurationRanker.cpp b/src/distance/ConfigurationRanker.cpp index 9af4fdf567..a5fa10aa0e 100644 --- a/src/distance/ConfigurationRanker.cpp +++ b/src/distance/ConfigurationRanker.cpp @@ -26,6 +26,14 @@ ConfigurationRanker::ConfigurationRanker( 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) diff --git a/tests/distance/test_NominalConfigurationRanker.cpp b/tests/distance/test_NominalConfigurationRanker.cpp index 4dde2fcc23..b3b22906b4 100644 --- a/tests/distance/test_NominalConfigurationRanker.cpp +++ b/tests/distance/test_NominalConfigurationRanker.cpp @@ -67,11 +67,11 @@ 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); std::vector negativeWeights{-1, 0}; @@ -86,7 +86,7 @@ TEST_F(NominalConfigurationRankerTest, Constructor) mStateSpace, mManipulator, wrongDimensionWeights, nullptr), std::invalid_argument); - NominalConfigurationRanker rankerOne(mStateSpace, mManipulator, nullptr); + NominalConfigurationRanker rankerOne(mStateSpace, mManipulator); DART_UNUSED(rankerOne); std::vector goodWeights{1, 2}; @@ -112,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); From 53fb108752056bdab5737fc36ec0cd6d65446c49 Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Thu, 24 Jan 2019 10:22:26 -0800 Subject: [PATCH 5/5] Update CHANGELOG.md --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) 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