From 600eec3a7b69366fcde5d8a4f131e4f1e1aadbf8 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Thu, 7 Feb 2019 20:30:28 -0800 Subject: [PATCH 01/10] ConcreteRobot uses ranker for PlantoTSR --- include/aikido/robot/ConcreteRobot.hpp | 5 +- include/aikido/robot/util.hpp | 5 +- src/robot/ConcreteRobot.cpp | 6 +- src/robot/util.cpp | 95 +++++++++++++++++--------- 4 files changed, 73 insertions(+), 38 deletions(-) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 7d4cca90ec..a29dbcabe1 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/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,7 @@ 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 resulting configurations. /// \return Trajectory to a sample in TSR, or nullptr if planning fails. aikido::trajectory::TrajectoryPtr planToTSR( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, @@ -199,7 +201,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::ConfigurationRankerPtr& ranker = nullptr); /// TODO: Replace this with Problem interface. /// Returns a Trajectory that moves the configuration of the metakeleton such diff --git a/include/aikido/robot/util.hpp b/include/aikido/robot/util.hpp index 15b25f01fe..470e033c74 100644 --- a/include/aikido/robot/util.hpp +++ b/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,7 @@ 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 resulting configurations. /// \return Trajectory to a sample in TSR, or nullptr if planning fails. trajectory::TrajectoryPtr planToTSR( const statespace::dart::MetaSkeletonStateSpacePtr& space, @@ -142,7 +144,8 @@ trajectory::TrajectoryPtr planToTSR( const constraint::TestablePtr& collisionTestable, common::RNG* rng, double timelimit, - std::size_t maxNumTrials); + std::size_t maxNumTrials, + const distance::ConfigurationRankerPtr& 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 diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 9db1d01551..01ce8284bc 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/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::ConfigurationRankerPtr& ranker) { auto collisionConstraint = getFullCollisionConstraint(stateSpace, metaSkeleton, collisionFree); @@ -414,7 +415,8 @@ TrajectoryPtr ConcreteRobot::planToTSR( collisionConstraint, cloneRNG().get(), timelimit, - maxNumTrials); + maxNumTrials, + ranker); } //============================================================================== diff --git a/src/robot/util.cpp b/src/robot/util.cpp index fa85ee0786..b535fb5b5c 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -1,4 +1,6 @@ #include "aikido/robot/util.hpp" + +#include #include #include #include @@ -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( + auto snapPlanner = std::make_shared( space, std::make_shared(space)); + + std::vector configurations; + + // Use a ranker + ConfigurationRankerPtr configurationRanker(ranker); + if (!ranker) + { + auto nominalState = space->createState(); + space->copyState(startState, nominalState); + configurationRanker = std::make_shared( + space, metaSkeleton, std::vector(), nominalState); + } + while (snapSamples < maxSnapSamples && generator->canSample()) { // Sample from TSR - { - std::lock_guard lock(robot->getMutex()); - bool sampled = generator->sample(goalState); - if (!sampled) - continue; + std::lock_guard 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 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 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( From 2775b7eacb99d62ec24027cd105fa74576b95455 Mon Sep 17 00:00:00 2001 From: Aditya Vamsikrishna Mandalika Date: Tue, 12 Feb 2019 11:09:41 -0800 Subject: [PATCH 02/10] Update include/aikido/robot/ConcreteRobot.hpp Co-Authored-By: gilwoolee --- include/aikido/robot/ConcreteRobot.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index a29dbcabe1..059b64202e 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -192,7 +192,7 @@ 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 resulting configurations. + /// \param[in] ranker Ranker to rank the sampled configurations. /// \return Trajectory to a sample in TSR, or nullptr if planning fails. aikido::trajectory::TrajectoryPtr planToTSR( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, From ca3238e4adc41bf10a5bb632bd6b8484e9b0034a Mon Sep 17 00:00:00 2001 From: Aditya Vamsikrishna Mandalika Date: Tue, 12 Feb 2019 11:09:47 -0800 Subject: [PATCH 03/10] Update include/aikido/robot/util.hpp Co-Authored-By: gilwoolee --- include/aikido/robot/util.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aikido/robot/util.hpp b/include/aikido/robot/util.hpp index 470e033c74..278563b388 100644 --- a/include/aikido/robot/util.hpp +++ b/include/aikido/robot/util.hpp @@ -134,7 +134,7 @@ 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 resulting configurations. +/// \param[in] ranker Ranker to rank the sampled configurations. /// \return Trajectory to a sample in TSR, or nullptr if planning fails. trajectory::TrajectoryPtr planToTSR( const statespace::dart::MetaSkeletonStateSpacePtr& space, From fac84625aa8fc1a6893a1d581d29dc0a3c52c81d Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Wed, 13 Feb 2019 17:58:27 -0800 Subject: [PATCH 04/10] Address PR comments --- .../ConfigurationToConfiguration_to_ConfigurationToTSR.hpp | 4 +++- include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp | 4 ++-- include/aikido/robot/ConcreteRobot.hpp | 5 +++-- include/aikido/robot/util.hpp | 5 +++-- .../ConfigurationToConfiguration_to_ConfigurationToTSR.cpp | 2 +- src/planner/dart/ConfigurationToTSRPlanner.cpp | 2 +- src/robot/ConcreteRobot.cpp | 2 +- 7 files changed, 14 insertions(+), 10 deletions(-) diff --git a/include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp b/include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp index e5cc98590b..4bc70a7875 100644 --- a/include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp +++ b/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 planner, ::dart::dynamics::MetaSkeletonPtr metaSkeleton, - distance::ConfigurationRankerPtr configurationRanker = nullptr); + distance::ConstConfigurationRankerPtr 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 1bb0712c8c..088ab8c0ef 100644 --- a/include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp +++ b/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 diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 059b64202e..db4e1c6740 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -192,7 +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. + /// \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, @@ -202,7 +203,7 @@ class ConcreteRobot : public Robot const aikido::constraint::dart::CollisionFreePtr& collisionFree, double timelimit, std::size_t maxNumTrials, - const distance::ConfigurationRankerPtr& ranker = nullptr); + const distance::ConstConfigurationRankerPtr& ranker = nullptr); /// TODO: Replace this with Problem interface. /// Returns a Trajectory that moves the configuration of the metakeleton such diff --git a/include/aikido/robot/util.hpp b/include/aikido/robot/util.hpp index 278563b388..8c31735e84 100644 --- a/include/aikido/robot/util.hpp +++ b/include/aikido/robot/util.hpp @@ -134,7 +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. +/// \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, @@ -145,7 +146,7 @@ trajectory::TrajectoryPtr planToTSR( common::RNG* rng, double timelimit, std::size_t maxNumTrials, - const distance::ConfigurationRankerPtr& ranker = nullptr); + 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 diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp index ff0746323e..0156213ebe 100644 --- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp @@ -27,7 +27,7 @@ ConfigurationToConfiguration_to_ConfigurationToTSR:: ConfigurationToConfiguration_to_ConfigurationToTSR( std::shared_ptr planner, ::dart::dynamics::MetaSkeletonPtr metaSkeleton, - distance::ConfigurationRankerPtr configurationRanker) + distance::ConstConfigurationRankerPtr configurationRanker) : PlannerAdapter( std::move(planner), std::move(metaSkeleton)) diff --git a/src/planner/dart/ConfigurationToTSRPlanner.cpp b/src/planner/dart/ConfigurationToTSRPlanner.cpp index 9c9edfec18..40b89da4ed 100644 --- a/src/planner/dart/ConfigurationToTSRPlanner.cpp +++ b/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( std::move(stateSpace), std::move(metaSkeleton)) , mConfigurationRanker(std::move(configurationRanker)) diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 01ce8284bc..bb9790538e 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -402,7 +402,7 @@ TrajectoryPtr ConcreteRobot::planToTSR( const CollisionFreePtr& collisionFree, double timelimit, std::size_t maxNumTrials, - const distance::ConfigurationRankerPtr& ranker) + const distance::ConstConfigurationRankerPtr& ranker) { auto collisionConstraint = getFullCollisionConstraint(stateSpace, metaSkeleton, collisionFree); From 6f830bce4d39baea2a8ebf4b0e16cd036569504e Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Wed, 13 Feb 2019 21:30:48 -0800 Subject: [PATCH 05/10] Update src/robot/util.cpp Co-Authored-By: gilwoolee --- src/robot/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/robot/util.cpp b/src/robot/util.cpp index b535fb5b5c..73d13940fc 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -260,7 +260,7 @@ trajectory::TrajectoryPtr planToTSR( space->setState(metaSkeleton.get(), startState); } - if (configurations.size() == 0) + if (configurations.empty()) return nullptr; configurationRanker->rankConfigurations(configurations); From 00584f49f5b66ad0c43eb70da9401af0acb679b0 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Wed, 13 Feb 2019 21:36:21 -0800 Subject: [PATCH 06/10] Address PR comments --- src/robot/util.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/robot/util.cpp b/src/robot/util.cpp index b535fb5b5c..74747e331d 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -274,10 +274,7 @@ trajectory::TrajectoryPtr planToTSR( auto traj = snapPlanner->plan(problem, &pResult); if (traj) - { - space->setState(metaSkeleton.get(), startState); return traj; - } } // Start the timer From 41032c73c79495cd2323fd07f660d6e795cf42fe Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Thu, 14 Feb 2019 13:29:30 -0800 Subject: [PATCH 07/10] Update CHANGELOG.md --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b6db27b8b5..791fa456a4 100644 --- a/CHANGELOG.md +++ b/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 @@ -62,6 +61,7 @@ * Added a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443) * 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) + * TSR planners use ConfigurationRanker: [#503](https://github.com/personalrobotics/aikido/pull/503) * Robot From eaa38cd7f279af2a06302ed690c97809d2489e6b Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 14 Feb 2019 17:06:50 -0800 Subject: [PATCH 08/10] Update CHANGELOG.md Co-Authored-By: gilwoolee --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 791fa456a4..ba1c9a55d2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -61,7 +61,7 @@ * Added a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443) * 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) - * TSR planners use ConfigurationRanker: [#503](https://github.com/personalrobotics/aikido/pull/503) + * Used ConfigurationRanker in TSR planners: [#503](https://github.com/personalrobotics/aikido/pull/503) * Robot From 7200224f47659e6c267d258337e72974c3e15f81 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Thu, 14 Feb 2019 23:40:31 -0800 Subject: [PATCH 09/10] Use ConfigurationRanker in ConfigurationToTSR planner, make ConfigurationRanker's rank method const, address PR comments --- .../aikido/distance/ConfigurationRanker.hpp | 2 +- .../distance/NominalConfigurationRanker.hpp | 2 + src/distance/ConfigurationRanker.cpp | 2 +- ...nToConfiguration_to_ConfigurationToTSR.cpp | 52 ++++++++++++++++--- src/robot/util.cpp | 6 --- 5 files changed, 48 insertions(+), 16 deletions(-) diff --git a/include/aikido/distance/ConfigurationRanker.hpp b/include/aikido/distance/ConfigurationRanker.hpp index 5e4fb2c284..25c6f3dfb4 100644 --- a/include/aikido/distance/ConfigurationRanker.hpp +++ b/include/aikido/distance/ConfigurationRanker.hpp @@ -36,7 +36,7 @@ class ConfigurationRanker /// \param[in, out] configurations Vector of configurations to rank. void rankConfigurations( std::vector& - configurations); + configurations) const; protected: /// Returns the cost of the configuration. diff --git a/include/aikido/distance/NominalConfigurationRanker.hpp b/include/aikido/distance/NominalConfigurationRanker.hpp index 46762cf5f3..4b2b4af36e 100644 --- a/include/aikido/distance/NominalConfigurationRanker.hpp +++ b/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 diff --git a/src/distance/ConfigurationRanker.cpp b/src/distance/ConfigurationRanker.cpp index 47e6af4b44..301aef72a3 100644 --- a/src/distance/ConfigurationRanker.cpp +++ b/src/distance/ConfigurationRanker.cpp @@ -69,7 +69,7 @@ ConfigurationRanker::ConfigurationRanker( //============================================================================== void ConfigurationRanker::rankConfigurations( - std::vector& configurations) + std::vector& configurations) const { std::unordered_map costs; costs.reserve(configurations.size()); diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp index 0156213ebe..69dd97abbe 100644 --- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp @@ -4,6 +4,7 @@ #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" @@ -11,6 +12,8 @@ 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; @@ -84,24 +87,57 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan( auto saver = MetaSkeletonStateSaver(mMetaSkeleton); DART_UNUSED(saver); + auto robot = mMetaSkeleton->getBodyNode(0)->getSkeleton(); + + std::vector configurations; + + // Use a ranker + ConstConfigurationRankerPtr configurationRanker(mConfigurationRanker); + if (!configurationRanker) + { + auto nominalState = mMetaSkeletonStateSpace->createState(); + mMetaSkeletonStateSpace->copyState(startState, nominalState); + configurationRanker = std::make_shared( + mMetaSkeletonStateSpace, mMetaSkeleton, + std::vector(), 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 lock(skeleton->getMutex()); - bool sampled = generator->sample(goalState); - if (!sampled) - continue; - } + std::lock_guard 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); diff --git a/src/robot/util.cpp b/src/robot/util.cpp index 5fbd29ac54..768fe34834 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -254,12 +254,6 @@ trajectory::TrajectoryPtr planToTSR( configurations.emplace_back(goalState.clone()); } - { - // Set to start state - std::lock_guard lock(robot->getMutex()); - space->setState(metaSkeleton.get(), startState); - } - if (configurations.empty()) return nullptr; From f7174ce0934739b3df8d49145714e66905de2073 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Fri, 15 Feb 2019 21:00:26 -0800 Subject: [PATCH 10/10] Clang format --- .../ConfigurationToConfiguration_to_ConfigurationToTSR.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp index 69dd97abbe..94bb4d4c13 100644 --- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp @@ -98,8 +98,10 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan( auto nominalState = mMetaSkeletonStateSpace->createState(); mMetaSkeletonStateSpace->copyState(startState, nominalState); configurationRanker = std::make_shared( - mMetaSkeletonStateSpace, mMetaSkeleton, - std::vector(), nominalState); + mMetaSkeletonStateSpace, + mMetaSkeleton, + std::vector(), + nominalState); } // Goal state