Skip to content
Open
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
namespace aikido {
namespace planner {

AIKIDO_DECLARE_POINTERS(ConfigurationToConfigurationPlanner)

/// Base planner class for ConfigurationToConfiguration planning problem.
class ConfigurationToConfigurationPlanner
: public SingleProblemPlanner<
Expand Down
Original file line number Diff line number Diff line change
@@ -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<PlannerPtr>& planners = std::vector<PlannerPtr>(),
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_
3 changes: 2 additions & 1 deletion src/planner/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
35 changes: 35 additions & 0 deletions src/planner/SequenceConfigurationToConfigurationMetaPlanner.cpp
Original file line number Diff line number Diff line change
@@ -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 (const 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