Aikido
aikido::planner::ompl Namespace Reference

Classes

class  CRRT
 Implements a constrained RRT planner. More...
 
class  CRRTConnect
 Implements a bi-direction constrained RRT planner. More...
 
class  GeometricStateSpace
 Wraps an aikido StateSpace into a space recognized by OMPL. More...
 
class  GoalRegion
 Exposes a Testable/Sampleable constraint pair as a goal to OMPL planners. More...
 
class  MotionValidator
 Implement an OMPL MotionValidator. More...
 
class  OMPLConfigurationToConfigurationPlanner
 Creates an OMPL Planner. More...
 
class  StateSampler
 Wraps an aikido::constraint::SampleGenerator in a ompl::base::StateSampler. More...
 
class  StateValidityChecker
 Expose a set of aikido::constraint::Testable class as a StateValidityChecker to the OMPL framework. More...
 

Typedefs

template<class T >
using ompl_shared_ptr = boost::shared_ptr< T >
 
template<class T >
using ompl_weak_ptr = boost::weak_ptr< T >
 
using GeometricStateSpacePtr = std::shared_ptr< GeometricStateSpace >
 
using ConstGeometricStateSpacePtr = std::shared_ptr< const GeometricStateSpace >
 
using WeakGeometricStateSpacePtr = std::weak_ptr< GeometricStateSpace >
 
using WeakConstGeometricStateSpacePtr = std::weak_ptr< const GeometricStateSpace >
 
using UniqueGeometricStateSpacePtr = std::unique_ptr< GeometricStateSpace >
 
using UniqueConstGeometricStateSpacePtr = std::unique_ptr< const GeometricStateSpace >
 

Functions

template<class T , class... Args>
ompl_shared_ptr< T > ompl_make_shared (Args &&... args)
 
template<class T , class U >
ompl_shared_ptr< T > ompl_dynamic_pointer_cast (const ompl_shared_ptr< U > &r)
 
template<class T , class U >
ompl_shared_ptr< T > ompl_static_pointer_cast (const ompl_shared_ptr< U > &r)
 
template<class F , class... Args>
auto ompl_bind (F &&f, Args &&... args) -> decltype(boost::bind(std::forward< F >(f), std::forward< Args >(args)...))
 
::ompl::base::SpaceInformationPtr createSpaceInformation (statespace::dart::MetaSkeletonStateSpacePtr _stateSpace, constraint::TestablePtr _validityConstraint, double _maxDistanceBtwValidityChecks, std::unique_ptr< common::RNG > _rng)
 Create a SpaceInformation by constructing default bounds constraints and metrics for the given robot. More...
 
template<class PlannerType >
trajectory::InterpolatedPtr 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 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 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 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 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 > 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 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 > 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 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::InterpolatedtoInterpolatedTrajectory (const ::ompl::geometric::PathGeometric &_path, statespace::InterpolatorPtr _interpolator)
 Take an OMPL geometric path and convert it into an interpolated trajectory. More...
 

Variables

constexpr double EQUALITY_EPSILON = 1e-7
 The maximum distance between two states for them to be considered equal. More...
 

Typedef Documentation

◆ ConstGeometricStateSpacePtr

◆ GeometricStateSpacePtr

◆ ompl_shared_ptr

template<class T >
using aikido::planner::ompl::ompl_shared_ptr = typedef boost::shared_ptr<T>

◆ ompl_weak_ptr

template<class T >
using aikido::planner::ompl::ompl_weak_ptr = typedef boost::weak_ptr<T>

◆ UniqueConstGeometricStateSpacePtr

◆ UniqueGeometricStateSpacePtr

◆ WeakConstGeometricStateSpacePtr

◆ WeakGeometricStateSpacePtr

Function Documentation

◆ createSpaceInformation()

::ompl::base::SpaceInformationPtr aikido::planner::ompl::createSpaceInformation ( statespace::dart::MetaSkeletonStateSpacePtr  _stateSpace,
constraint::TestablePtr  _validityConstraint,
double  _maxDistanceBtwValidityChecks,
std::unique_ptr< common::RNG _rng 
)

Create a SpaceInformation by constructing default bounds constraints and metrics for the given robot.

Parameters
_stateSpaceThe dart MetaSkeletonStateSpace to plan in
_validityConstraintA constraint describing a set of checks that should be done before marking a state as valid. This constraint should include collision checks. It does not need to include joint limits or boundary checks. These will be added by the function.
_maxDistanceBtwValidityChecksThe maximum distance (under dmetric) between validity checking two successive points on a tree extension
_rngA random number generator to be used by state samplers

◆ getGoalRegion()

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.

Parameters
_siInformation about the planning space
_goalTestableA Testable constraint that can determine if a given state is a goal state
_goalSamplerA Sampleable capable of sampling states that satisfy _goalTestable

◆ getSpaceInformation()

::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.

Parameters
_stateSpaceThe StateSpace that the SpaceInformation operates on
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space.
_dmetricA valid distance metric defined on the StateSpace
_samplerA Sampleable that can sample states from the StateSpace. Warning: Many OMPL planners internally assume this sampler samples uniformly. Care should be taken when using a non-uniform sampler.
_validityConstraintA constraint used to test validity during planning. This should include collision checking and any other constraints that must be satisfied for a state to be considered valid.
_boundsConstraintA constraint used to determine whether states encountered during planning fall within any bounds specified on the StateSpace. In addition to the _validityConstraint, this must also be satsified for a state to be considered valid.
_boundsProjectorA Projectable that projects a state back within valid bounds defined on the StateSpace
_maxDistanceBtwValidityChecksThe maximum distance (under dmetric) between validity checking two successive points on a tree extension

◆ ompl_bind()

template<class F , class... Args>
auto aikido::planner::ompl::ompl_bind ( F &&  f,
Args &&...  args 
) -> decltype(boost::bind(std::forward<F>(f), std::forward<Args>(args)...))

◆ ompl_dynamic_pointer_cast()

template<class T , class U >
ompl_shared_ptr<T> aikido::planner::ompl::ompl_dynamic_pointer_cast ( const ompl_shared_ptr< U > &  r)

◆ ompl_make_shared()

template<class T , class... Args>
ompl_shared_ptr<T> aikido::planner::ompl::ompl_make_shared ( Args &&...  args)

◆ ompl_static_pointer_cast()

template<class T , class U >
ompl_shared_ptr<T> aikido::planner::ompl::ompl_static_pointer_cast ( const ompl_shared_ptr< U > &  r)

◆ planCRRT()

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.

Parameters
_startThe start state
_goalTestableA Testable constraint that can determine if a given state is a goal state
_goalSamplerA Sampleable capable of sampling states that satisfy _goalTestable
_trajConstraintThe constraint to satisfy along the trajectory
_stateSpaceThe StateSpace that the planner must plan within
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space.
_dmetricA valid distance metric defined on the StateSpace
_samplerA Sampleable that can sample states from the StateSpace. Warning: Many OMPL planners internally assume this sampler samples uniformly. Care should be taken when using a non-uniform sampler.
_validityConstraintA constraint used to test validity during planning. This should include collision checking and any other constraints that must be satisfied for a state to be considered valid.
_boundsConstraintA constraint used to determine whether states encountered during planning fall within any bounds specified on the StateSpace. In addition to the _validityConstraint, this must also be satsified for a state to be considered valid.
_boundsProjectorA Projectable that projects a state back within valid bounds defined on the StateSpace
_maxPlanTimeThe maximum time to allow the planner to search for a solution
_maxExtensionDistanceThe maximum distance to extend the tree on a single extension
_maxDistanceBtwProjectionsThe maximum distance (under dmetric) between projecting and validity checking two successive points on a tree extension
_minStepsizeThe minimum distance between two states for the them to be considered "different"

◆ planCRRTConnect()

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.

Parameters
_startThe start state
_goalTestableA Testable constraint that can determine if a given state is a goal state
_goalSamplerA Sampleable capable of sampling states that satisfy _goalTestable
_trajConstraintThe constraint to satisfy along the trajectory
_stateSpaceThe StateSpace that the planner must plan within
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space.
_dmetricA valid distance metric defined on the StateSpace
_samplerA Sampleable that can sample states from the StateSpace. Warning: Many OMPL planners internally assume this sampler samples uniformly. Care should be taken when using a non-uniform sampler.
_validityConstraintA constraint used to test validity during planning. This should include collision checking and any other constraints that must be satisfied for a state to be considered valid.
_boundsConstraintA constraint used to determine whether states encountered during planning fall within any bounds specified on the StateSpace. In addition to the _validityConstraint, this must also be satsified for a state to be considered valid.
_boundsProjectorA Projectable that projects a state back within valid bounds defined on the StateSpace
_maxPlanTimeThe maximum time to allow the planner to search for a solution
_maxExtensionDistanceThe maximum distance to extend the tree on a single extension
_maxDistanceBtwProjectionsThe maximum distance (under dmetric) between projecting and validity checking two successive points on a tree extension
_minTreeConnectionDistanceThe minumum distance between the start and goal tree to consider them connected
_minStepsizeThe minimum distance between two states for the them to be considered "different"

◆ planOMPL() [1/3]

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.

Parameters
_plannerPoints to some OMPL planner.
_pdefThe ProblemDefintion. This contains start and goal conditions for the planner.
_sspaceThe aikido StateSpace to plan against. Used for constructing the return trajectory.
_interpolatorAn aikido interpolator that can be used with the _stateSpace.
_maxPlanTimeThe maximum time to allow the planner to search for a solution

◆ planOMPL() [2/3]

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.

Returns nullptr on planning failure.

Parameters
_startThe start state
_goalThe goal state
_stateSpaceThe StateSpace that the planner must plan within
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space.
_dmetricA valid distance metric defined on the StateSpace
_samplerA Sampleable that can sample states from the StateSpace. Warning: Many OMPL planners internally assume this sampler samples uniformly. Care should be taken when using a non-uniform sampler.
_validityConstraintA constraint used to test validity during planning. This should include collision checking and any other constraints that must be satisfied for a state to be considered valid.
_boundsConstraintA constraint used to determine whether states encountered during planning fall within any bounds specified on the StateSpace. In addition to the _validityConstraint, this must also be satsified for a state to be considered valid.
_boundsProjectorA Projectable that projects a state back within valid bounds defined on the StateSpace
_maxPlanTimeThe maximum time to allow the planner to search for a solution
_maxDistanceBtwValidityChecksThe maximum distance (under dmetric) between validity checking two successive points on a tree extension

◆ planOMPL() [3/3]

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.

Returns nullptr on planning failure.

Parameters
_startThe start state
_goalTestableA Testable constraint that can determine if a given state is a goal state
_goalSamplerA Sampleable capable of sampling states that satisfy _goalTestable
_stateSpaceThe StateSpace that the planner must plan within
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space.
_dmetricA valid distance metric defined on the StateSpace
_samplerA Sampleable that can sample states from the StateSpace. Warning: Many OMPL planners internally assume this sampler samples uniformly. Care should be taken when using a non-uniform sampler.
_validityConstraintA constraint used to test validity during planning. This should include collision checking and any other constraints that must be satisfied for a state to be considered valid.
_boundsConstraintA constraint used to determine whether states encountered during planning fall within any bounds specified on the StateSpace. In addition to the _validityConstraint, this must also be satsified for a state to be considered valid.
_boundsProjectorA Projectable that projects a state back within valid bounds defined on the StateSpace
_maxPlanTimeThe maximum time to allow the planner to search for a solution
_maxDistanceBtwValidityChecksThe maximum distance (under dmetric) between validity checking two successive points on a tree extension

◆ simplifyOMPL()

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.

Parameters
_stateSpaceThe StateSpace that the planner must plan within
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space.
_dmetricA valid distance metric defined on the StateSpace
_samplerA Sampleable that can sample states from the StateSpace. Warning: Many OMPL planners internally assume this sampler samples uniformly. Care should be taken when using a non-uniform sampler.
_validityConstraintA constraint used to test validity during planning. This should include collision checking and any other constraints that must be satisfied for a state to be considered valid.
_boundsConstraintA constraint used to determine whether states encountered during planning fall within any bounds specified on the StateSpace. In addition to the _validityConstraint, this must also be satsified for a state to be considered valid.
_boundsProjectorA Projectable that projects a state back within valid bounds defined on the StateSpace
_maxDistanceBtwValidityChecksThe maximum distance (under dmetric) between validity checking two successive points on a tree extension
_timeoutTimeout, in seconds, after which the simplifier terminates to return possibly shortened path
_maxEmptyStepsMaximum number of consecutive failed attempts at shortening before simplification terminates. Default 0, equal to number of states in the path
_originalTrajThe untimed trajectory obtained from the planner, needs simplifying.

◆ toInterpolatedTrajectory()

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.

Parameters
_pathThe OMPL geometric path
_interpolatorAn Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. returns the corresponding interpolated trajectory

◆ toOMPLTrajectory()

::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.

Parameters
_interpolatedTrajthe interpolated trajectory to be converted
_siInformation about the planning space returns the corresponding OMPL geometric path

Variable Documentation

◆ EQUALITY_EPSILON

constexpr double aikido::planner::ompl::EQUALITY_EPSILON = 1e-7
constexpr

The maximum distance between two states for them to be considered equal.