Add SequenceConfigurationToConfigurationMetaPlanner

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<

include/aikido/planner/SequenceConfigurationToConfigurationMetaPlanner.hpp

@@ -0,0 +1,39 @@
+#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<PlannerPtr>& planners = std::vector<PlannerPtr>(),
+      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_

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

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<PlannerPtr>& planners,
+        common::RNG* rng)
+  : SequenceMetaPlanner(stateSpace, std::move(planners))
+  , ConfigurationToConfigurationPlanner(stateSpace, std::move(rng))
+{
+  for (auto planner : mPlanners)
+  {
+    auto castedPlanner
+        = std::dynamic_pointer_cast<ConfigurationToConfigurationPlannerPtr>(
+            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