From d946452eb43df2370a4765543e02a8f16d2f76cc Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 19:39:49 -0800 Subject: [PATCH 1/5] Add SequenceConfigurationToConfigurationMetaPlanner --- .../ConfigurationToConfigurationPlanner.hpp | 2 + ...onfigurationToConfigurationMetaPlanner.hpp | 39 +++++++++++++++++++ src/planner/CMakeLists.txt | 3 +- ...onfigurationToConfigurationMetaPlanner.cpp | 33 ++++++++++++++++ 4 files changed, 76 insertions(+), 1 deletion(-) create mode 100644 include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp create mode 100644 src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp diff --git a/include/aikido/planner/ConfigurationToConfigurationPlanner.hpp b/include/aikido/planner/ConfigurationToConfigurationPlanner.hpp index 4a23a0dec9a..6d1e3425413 100644 --- a/include/aikido/planner/ConfigurationToConfigurationPlanner.hpp +++ b/include/aikido/planner/ConfigurationToConfigurationPlanner.hpp @@ -8,6 +8,8 @@ namespace aikido { namespace planner { +AIKIDO_DECLARE_POINTERS(ConfigurationToConfigurationPlanner) + /// Base planner class for ConfigurationToConfiguration planning problem. class ConfigurationToConfigurationPlanner : public SingleProblemPlanner< diff --git a/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp new file mode 100644 index 00000000000..258491c16c8 --- /dev/null +++ b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp @@ -0,0 +1,39 @@ +#ifndef AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_HPP_ +#define AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_HPP_ + +#include "aikido/planner/SequenceMetaPlanner.hpp" +#include "aikido/planner/ConfigurationToConfigurationPlanner.hpp" + +namespace aikido { +namespace planner { + +/// A meta planner that solves ConfigurationToConfiguration using the sub planners one-by-one +/// sequentially and returns the first successfully planned trajectory. +class SequenceConfigurationToConfigurationMetaPlanner : + public SequenceMetaPlanner, public ConfigurationToConfigurationPlanner +{ +public: + + /// Constructs given list of planners. + /// + /// \param[in] stateSpace State space that this planner associated with. + /// \param[in] planners Planners to be used by this planner. + /// \param[in] rng RNG that planner uses. If nullptr, a default is created. + /// \throw If any of \c planners are not ConfigurationToConfigurationPlanner. + SequenceConfigurationToConfigurationMetaPlanner( + statespace::ConstStateSpacePtr stateSpace, + const std::vector& planners = std::vector(), + common::RNG* rng = nullptr); + + // Documentation inherited. + // trajectory::TrajectoryPtr plan( + // const Problem& problem, Result* result = nullptr) override; + trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr) + override; +}; + +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_HPP_ diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index 8358a7b6a6a..dd6c2c51aa3 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -8,9 +8,10 @@ set(sources PlanningResult.cpp Problem.cpp RankedMetaPlanner.cpp + SequenceConfigurationToConfigurationMetaPlanner.cpp + SequenceMetaPlanner.cpp SnapConfigurationToConfigurationPlanner.cpp SnapPlanner.cpp - SequenceMetaPlanner.cpp World.cpp WorldStateSaver.cpp dart/ConfigurationToConfiguration.cpp diff --git a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp new file mode 100644 index 00000000000..d885c6a43c5 --- /dev/null +++ b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp @@ -0,0 +1,33 @@ +#include "aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp" + +namespace aikido { +namespace planner { + +//============================================================================== +SequenceConfigurationToConfigurationMetaPlanner::SequenceConfigurationToConfigurationMetaPlanner( + statespace::ConstStateSpacePtr stateSpace, + const std::vector& planners, + common::RNG* rng) + : SequenceMetaPlanner(stateSpace, std::move(planners)) + , ConfigurationToConfigurationPlanner(stateSpace, std::move(rng)) +{ + for (auto planner : mPlanners) + { + auto castedPlanner = std::dynamic_pointer_cast(planner); + + if (!castedPlanner) + throw std::invalid_argument("One of the planners is not ConfigurationToConfigurationPlanner."); + } +} + + +// //============================================================================== +trajectory::TrajectoryPtr SequenceConfigurationToConfigurationMetaPlanner::plan( + const SolvableProblem& problem, Result* result) +{ + return SequenceMetaPlanner::plan(problem, result); +} + + +} // namespace planner +} // namespace aikido From 9eda36ce8a30d5d109268aeb051004878ce0dc02 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 19:49:27 -0800 Subject: [PATCH 2/5] Clang format --- ...ConfigurationToConfigurationMetaPlanner.hpp | 18 +++++++++--------- ...ConfigurationToConfigurationMetaPlanner.cpp | 18 ++++++++++-------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp index 258491c16c8..7d36d256a46 100644 --- a/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp +++ b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp @@ -1,19 +1,20 @@ #ifndef AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_HPP_ #define AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_HPP_ -#include "aikido/planner/SequenceMetaPlanner.hpp" #include "aikido/planner/ConfigurationToConfigurationPlanner.hpp" +#include "aikido/planner/SequenceMetaPlanner.hpp" namespace aikido { namespace planner { -/// A meta planner that solves ConfigurationToConfiguration using the sub planners one-by-one -/// sequentially and returns the first successfully planned trajectory. -class SequenceConfigurationToConfigurationMetaPlanner : - public SequenceMetaPlanner, public ConfigurationToConfigurationPlanner +/// A meta planner that solves ConfigurationToConfiguration using the sub +/// planners one-by-one sequentially and returns the first successfully planned +/// trajectory. +class SequenceConfigurationToConfigurationMetaPlanner + : public SequenceMetaPlanner + , public ConfigurationToConfigurationPlanner { public: - /// Constructs given list of planners. /// /// \param[in] stateSpace State space that this planner associated with. @@ -28,9 +29,8 @@ class SequenceConfigurationToConfigurationMetaPlanner : // Documentation inherited. // trajectory::TrajectoryPtr plan( // const Problem& problem, Result* result = nullptr) override; - trajectory::TrajectoryPtr plan( - const SolvableProblem& problem, Result* result = nullptr) - override; + trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr) override; }; } // namespace planner diff --git a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp index d885c6a43c5..e4e5fd70edf 100644 --- a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp +++ b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp @@ -4,23 +4,26 @@ namespace aikido { namespace planner { //============================================================================== -SequenceConfigurationToConfigurationMetaPlanner::SequenceConfigurationToConfigurationMetaPlanner( - statespace::ConstStateSpacePtr stateSpace, - const std::vector& planners, - common::RNG* rng) +SequenceConfigurationToConfigurationMetaPlanner:: + SequenceConfigurationToConfigurationMetaPlanner( + statespace::ConstStateSpacePtr stateSpace, + const std::vector& planners, + common::RNG* rng) : SequenceMetaPlanner(stateSpace, std::move(planners)) , ConfigurationToConfigurationPlanner(stateSpace, std::move(rng)) { for (auto planner : mPlanners) { - auto castedPlanner = std::dynamic_pointer_cast(planner); + auto castedPlanner + = std::dynamic_pointer_cast( + planner); if (!castedPlanner) - throw std::invalid_argument("One of the planners is not ConfigurationToConfigurationPlanner."); + throw std::invalid_argument( + "One of the planners is not ConfigurationToConfigurationPlanner."); } } - // //============================================================================== trajectory::TrajectoryPtr SequenceConfigurationToConfigurationMetaPlanner::plan( const SolvableProblem& problem, Result* result) @@ -28,6 +31,5 @@ trajectory::TrajectoryPtr SequenceConfigurationToConfigurationMetaPlanner::plan( return SequenceMetaPlanner::plan(problem, result); } - } // namespace planner } // namespace aikido From 72acf6954b1ef9b0ff979b0466d0829e6b6c0b32 Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Tue, 3 Mar 2020 15:39:51 -0800 Subject: [PATCH 3/5] Update src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp Co-Authored-By: Brian Hou --- src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp index e4e5fd70edf..e7e14647035 100644 --- a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp +++ b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp @@ -24,7 +24,7 @@ SequenceConfigurationToConfigurationMetaPlanner:: } } -// //============================================================================== +//============================================================================== trajectory::TrajectoryPtr SequenceConfigurationToConfigurationMetaPlanner::plan( const SolvableProblem& problem, Result* result) { From 2636ff054b23129f35984ab8381d0a53a377613f Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Tue, 3 Mar 2020 15:39:59 -0800 Subject: [PATCH 4/5] Update src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp Co-Authored-By: Brian Hou --- src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp index e7e14647035..e70d6cf4800 100644 --- a/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp +++ b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp @@ -12,7 +12,7 @@ SequenceConfigurationToConfigurationMetaPlanner:: : SequenceMetaPlanner(stateSpace, std::move(planners)) , ConfigurationToConfigurationPlanner(stateSpace, std::move(rng)) { - for (auto planner : mPlanners) + for (const auto& planner : mPlanners) { auto castedPlanner = std::dynamic_pointer_cast( From f7de8251933197c173f7b6f83f86719aafe63fe9 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 3 Mar 2020 15:42:11 -0800 Subject: [PATCH 5/5] Remove commented out lines --- .../planner/SequenceConfigurationToConfigurationMetaPlanner.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp index 7d36d256a46..7c353ea3488 100644 --- a/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp +++ b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp @@ -27,8 +27,6 @@ class SequenceConfigurationToConfigurationMetaPlanner common::RNG* rng = nullptr); // Documentation inherited. - // trajectory::TrajectoryPtr plan( - // const Problem& problem, Result* result = nullptr) override; trajectory::TrajectoryPtr plan( const SolvableProblem& problem, Result* result = nullptr) override; };