Aikido
Planner.hpp File Reference
#include <chrono>
#include <utility>
#include <ompl/base/Planner.h>
#include <ompl/base/ProblemDefinition.h>
#include <ompl/base/ScopedState.h>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/goals/GoalRegion.h>
#include <ompl/geometric/PathSimplifier.h>
#include "aikido/constraint/Projectable.hpp"
#include "aikido/constraint/Sampleable.hpp"
#include "aikido/constraint/Testable.hpp"
#include "aikido/distance/DistanceMetric.hpp"
#include "aikido/planner/ompl/BackwardCompatibility.hpp"
#include "aikido/planner/ompl/GeometricStateSpace.hpp"
#include "aikido/statespace/Interpolator.hpp"
#include "aikido/statespace/StateSpace.hpp"
#include "aikido/trajectory/Interpolated.hpp"
#include "detail/Planner-impl.hpp"

Go to the source code of this file.

Namespaces

 aikido
 Format of serialized trajectory in YAML.
 
 aikido::planner
 
 aikido::planner::ompl
 

Functions

template<class PlannerType >
trajectory::InterpolatedPtr aikido::planner::ompl::planOMPL (const statespace::StateSpace::State *_start, const statespace::StateSpace::State *_goal, statespace::ConstStateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxPlanTime, double _maxDistanceBtwValidityChecks)
 Use the template OMPL Planner type to plan a trajectory that moves from the start to the goal point. More...
 
template<class PlannerType >
trajectory::InterpolatedPtr aikido::planner::ompl::planOMPL (const statespace::StateSpace::State *_start, constraint::TestablePtr _goalTestable, constraint::SampleablePtr _goalSampler, statespace::ConstStateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxPlanTime, double _maxDistanceBtwValidityChecks)
 Use the template OMPL Planner type to plan a trajectory that moves from the start to a goal region. More...
 
trajectory::InterpolatedPtr aikido::planner::ompl::planCRRT (const statespace::StateSpace::State *_start, constraint::TestablePtr _goalTestable, constraint::SampleablePtr _goalSampler, constraint::ProjectablePtr _trajConstraint, statespace::ConstStateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxPlanTime, double _maxExtensionDistance, double _maxDistanceBtwProjections, double _minStepsize)
 Use the CRRT planner to plan a trajectory that moves from the start to a goal region while respecting a constraint. More...
 
trajectory::InterpolatedPtr aikido::planner::ompl::planCRRTConnect (const statespace::StateSpace::State *_start, constraint::TestablePtr _goalTestable, constraint::SampleablePtr _goalSampler, constraint::ProjectablePtr _trajConstraint, statespace::ConstStateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxPlanTime, double _maxExtensionDistance, double _maxDistanceBtwProjections, double _minStepsize, double _minTreeConnectionDistance)
 Use the CRRT planner to plan a trajectory that moves from the start to a goal region while respecting a constraint. More...
 
::ompl::base::SpaceInformationPtr aikido::planner::ompl::getSpaceInformation (statespace::ConstStateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxDistanceBtwValidityChecks)
 Generate an OMPL SpaceInformation from aikido components. More...
 
ompl_shared_ptr<::ompl::base::GoalRegion > aikido::planner::ompl::getGoalRegion (::ompl::base::SpaceInformationPtr _si, constraint::TestablePtr _goalTestable, constraint::SampleablePtr _goalSampler)
 Create an OMPL GoalRegion from a Testable and Sampler that describe the goal region. More...
 
trajectory::InterpolatedPtr aikido::planner::ompl::planOMPL (const ::ompl::base::PlannerPtr &_planner, const ::ompl::base::ProblemDefinitionPtr &_pdef, statespace::ConstStateSpacePtr _sspace, statespace::InterpolatorPtr _interpolator, double _maxPlanTime)
 Use the template OMPL Planner type to plan in a custom OMPL Space Information and problem definition and return an aikido Trajector Returns nullptr on planning failure. More...
 
std::pair< std::unique_ptr< trajectory::Interpolated >, bool > aikido::planner::ompl::simplifyOMPL (statespace::StateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxDistanceBtwValidityChecks, double _timeout, std::size_t _maxEmptySteps, trajectory::InterpolatedPtr _originalTraj)
 Take in an aikido trajectory and simplify it using OMPL methods. More...
 
::ompl::geometric::PathGeometric aikido::planner::ompl::toOMPLTrajectory (const trajectory::InterpolatedPtr &_interpolatedTraj, ::ompl::base::SpaceInformationPtr _si)
 Take an interpolated trajectory and convert it into OMPL geometric path. More...
 
std::unique_ptr< trajectory::Interpolated > aikido::planner::ompl::toInterpolatedTrajectory (const ::ompl::geometric::PathGeometric &_path, statespace::InterpolatorPtr _interpolator)
 Take an OMPL geometric path and convert it into an interpolated trajectory. More...