Aikido
|
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::Interpolated > | toInterpolatedTrajectory (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... | |
using aikido::planner::ompl::ConstGeometricStateSpacePtr = typedef std::shared_ptr< const GeometricStateSpace > |
using aikido::planner::ompl::GeometricStateSpacePtr = typedef std::shared_ptr< GeometricStateSpace > |
using aikido::planner::ompl::ompl_shared_ptr = typedef boost::shared_ptr<T> |
using aikido::planner::ompl::ompl_weak_ptr = typedef boost::weak_ptr<T> |
using aikido::planner::ompl::UniqueConstGeometricStateSpacePtr = typedef std::unique_ptr< const GeometricStateSpace > |
using aikido::planner::ompl::UniqueGeometricStateSpacePtr = typedef std::unique_ptr< GeometricStateSpace > |
using aikido::planner::ompl::WeakConstGeometricStateSpacePtr = typedef std::weak_ptr< const GeometricStateSpace > |
using aikido::planner::ompl::WeakGeometricStateSpacePtr = typedef std::weak_ptr< GeometricStateSpace > |
::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.
_stateSpace | The dart MetaSkeletonStateSpace to plan in |
_validityConstraint | A 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. |
_maxDistanceBtwValidityChecks | The maximum distance (under dmetric) between validity checking two successive points on a tree extension |
_rng | A random number generator to be used by state samplers |
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.
_si | Information about the planning space |
_goalTestable | A Testable constraint that can determine if a given state is a goal state |
_goalSampler | A Sampleable capable of sampling states that satisfy _goalTestable |
::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.
_stateSpace | The StateSpace that the SpaceInformation operates on |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. |
_dmetric | A valid distance metric defined on the StateSpace |
_sampler | A 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. |
_validityConstraint | A 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. |
_boundsConstraint | A 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. |
_boundsProjector | A Projectable that projects a state back within valid bounds defined on the StateSpace |
_maxDistanceBtwValidityChecks | The maximum distance (under dmetric) between validity checking two successive points on a tree extension |
auto aikido::planner::ompl::ompl_bind | ( | F && | f, |
Args &&... | args | ||
) | -> decltype(boost::bind(std::forward<F>(f), std::forward<Args>(args)...)) |
ompl_shared_ptr<T> aikido::planner::ompl::ompl_dynamic_pointer_cast | ( | const ompl_shared_ptr< U > & | r | ) |
ompl_shared_ptr<T> aikido::planner::ompl::ompl_make_shared | ( | Args &&... | args | ) |
ompl_shared_ptr<T> aikido::planner::ompl::ompl_static_pointer_cast | ( | const ompl_shared_ptr< U > & | r | ) |
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.
_start | The start state |
_goalTestable | A Testable constraint that can determine if a given state is a goal state |
_goalSampler | A Sampleable capable of sampling states that satisfy _goalTestable |
_trajConstraint | The constraint to satisfy along the trajectory |
_stateSpace | The StateSpace that the planner must plan within |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. |
_dmetric | A valid distance metric defined on the StateSpace |
_sampler | A 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. |
_validityConstraint | A 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. |
_boundsConstraint | A 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. |
_boundsProjector | A Projectable that projects a state back within valid bounds defined on the StateSpace |
_maxPlanTime | The maximum time to allow the planner to search for a solution |
_maxExtensionDistance | The maximum distance to extend the tree on a single extension |
_maxDistanceBtwProjections | The maximum distance (under dmetric) between projecting and validity checking two successive points on a tree extension |
_minStepsize | The minimum distance between two states for the them to be considered "different" |
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.
_start | The start state |
_goalTestable | A Testable constraint that can determine if a given state is a goal state |
_goalSampler | A Sampleable capable of sampling states that satisfy _goalTestable |
_trajConstraint | The constraint to satisfy along the trajectory |
_stateSpace | The StateSpace that the planner must plan within |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. |
_dmetric | A valid distance metric defined on the StateSpace |
_sampler | A 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. |
_validityConstraint | A 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. |
_boundsConstraint | A 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. |
_boundsProjector | A Projectable that projects a state back within valid bounds defined on the StateSpace |
_maxPlanTime | The maximum time to allow the planner to search for a solution |
_maxExtensionDistance | The maximum distance to extend the tree on a single extension |
_maxDistanceBtwProjections | The maximum distance (under dmetric) between projecting and validity checking two successive points on a tree extension |
_minTreeConnectionDistance | The minumum distance between the start and goal tree to consider them connected |
_minStepsize | The minimum distance between two states for the them to be considered "different" |
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.
_planner | Points to some OMPL planner. |
_pdef | The ProblemDefintion. This contains start and goal conditions for the planner. |
_sspace | The aikido StateSpace to plan against. Used for constructing the return trajectory. |
_interpolator | An aikido interpolator that can be used with the _stateSpace. |
_maxPlanTime | The maximum time to allow the planner to search for a solution |
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.
_start | The start state |
_goal | The goal state |
_stateSpace | The StateSpace that the planner must plan within |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. |
_dmetric | A valid distance metric defined on the StateSpace |
_sampler | A 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. |
_validityConstraint | A 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. |
_boundsConstraint | A 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. |
_boundsProjector | A Projectable that projects a state back within valid bounds defined on the StateSpace |
_maxPlanTime | The maximum time to allow the planner to search for a solution |
_maxDistanceBtwValidityChecks | The maximum distance (under dmetric) between validity checking two successive points on a tree extension |
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.
_start | The start state |
_goalTestable | A Testable constraint that can determine if a given state is a goal state |
_goalSampler | A Sampleable capable of sampling states that satisfy _goalTestable |
_stateSpace | The StateSpace that the planner must plan within |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. |
_dmetric | A valid distance metric defined on the StateSpace |
_sampler | A 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. |
_validityConstraint | A 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. |
_boundsConstraint | A 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. |
_boundsProjector | A Projectable that projects a state back within valid bounds defined on the StateSpace |
_maxPlanTime | The maximum time to allow the planner to search for a solution |
_maxDistanceBtwValidityChecks | The maximum distance (under dmetric) between validity checking two successive points on a tree extension |
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.
_stateSpace | The StateSpace that the planner must plan within |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. |
_dmetric | A valid distance metric defined on the StateSpace |
_sampler | A 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. |
_validityConstraint | A 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. |
_boundsConstraint | A 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. |
_boundsProjector | A Projectable that projects a state back within valid bounds defined on the StateSpace |
_maxDistanceBtwValidityChecks | The maximum distance (under dmetric) between validity checking two successive points on a tree extension |
_timeout | Timeout, in seconds, after which the simplifier terminates to return possibly shortened path |
_maxEmptySteps | Maximum number of consecutive failed attempts at shortening before simplification terminates. Default 0, equal to number of states in the path |
_originalTraj | The untimed trajectory obtained from the planner, needs simplifying. |
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.
_path | The OMPL geometric path |
_interpolator | An Interpolator defined on the StateSpace. This is used to interpolate between two points within the space. returns the corresponding interpolated trajectory |
::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.
_interpolatedTraj | the interpolated trajectory to be converted |
_si | Information about the planning space returns the corresponding OMPL geometric path |
|
constexpr |
The maximum distance between two states for them to be considered equal.