diff --git a/include/aikido/planner/ConfigurationToConfigurationPlanner.hpp b/include/aikido/planner/ConfigurationToConfigurationPlanner.hpp index 4a23a0dec9..6d1e342541 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 0000000000..7c353ea348 --- /dev/null +++ b/include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp @@ -0,0 +1,37 @@ +#ifndef AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_HPP_ +#define AIKIDO_PLANNER_SEQUENCECONFIGURATIONTOCONFIGURATIONMETAPLANNER_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 +{ +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 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 cc3720d04a..c940544f5b 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -9,9 +9,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 0000000000..e70d6cf480 --- /dev/null +++ b/src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp @@ -0,0 +1,35 @@ +#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 (const 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