-
Notifications
You must be signed in to change notification settings - Fork 30
/
Planner.hpp
138 lines (128 loc) · 7.07 KB
/
Planner.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#ifndef AIKIDO_OMPL_OMPLPLANNER_HPP_
#define AIKIDO_OMPL_OMPLPLANNER_HPP_
#include "../../distance/DistanceMetric.hpp"
#include "../../statespace/StateSpace.hpp"
#include "../../statespace/Interpolator.hpp"
#include "../../constraint/TestableConstraint.hpp"
#include "../../constraint/Sampleable.hpp"
#include "../../constraint/Projectable.hpp"
#include "../../trajectory/Trajectory.hpp"
#include <ompl/base/Planner.h>
#include <ompl/base/ProblemDefinition.h>
#include <ompl/base/SpaceInformation.h>
#include <boost/make_shared.hpp>
namespace aikido {
namespace planner {
namespace ompl {
/// Use the template OMPL Planner type to plan a trajectory that moves from the
/// start to the goal point.
/// \param _start The start state
/// \param _goal The goal state
/// \param _statespace The StateSpace that the planner must plan within
/// \param _interpolator An Interpolator defined on the StateSpace. This is used
/// to interpolate between two points within the space.
/// \param _dmetric A valid distance metric defined on the StateSpace
/// \param _sampler A SampleableConstraint 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.
/// \param _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.
/// \param _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.
/// \param _boundsProjector A Projectable that projects a state back within
/// valid bounds defined on the StateSpace
/// \param _maxPlanTime The maximum time to allow the planner to search for a
/// solution
template <class PlannerType>
trajectory::TrajectoryPtr planOMPL(
const statespace::StateSpace::State *_start,
const statespace::StateSpace::State *_goal,
statespace::StateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleableConstraintPtr _sampler,
constraint::TestableConstraintPtr _validityConstraint,
constraint::TestableConstraintPtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector, double _maxPlanTime);
/// Use the template OMPL Planner type to plan a trajectory that moves from the
/// start to a goal region.
/// \param _start The start state
/// \param _goalTestable A Testable constraint that can determine if a given state is a goal state
/// \param _goalSamplers A Sampleable capable of sampling states that satisfy _goalTestable
/// \param _statespace The StateSpace that the planner must plan within
/// \param _interpolator An Interpolator defined on the StateSpace. This is used
/// to interpolate between two points within the space.
/// \param _dmetric A valid distance metric defined on the StateSpace
/// \param _sampler A SampleableConstraint 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.
/// \param _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.
/// \param _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.
/// \param _boundsProjector A Projectable that projects a state back within
/// valid bounds defined on the StateSpace
/// \param _maxPlanTime The maximum time to allow the planner to search for a
/// solution
template <class PlannerType>
trajectory::TrajectoryPtr planOMPL(
const statespace::StateSpace::State *_start,
constraint::TestableConstraintPtr _goalTestable,
constraint::SampleableConstraintPtr _goalSampler,
statespace::StateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleableConstraintPtr _sampler,
constraint::TestableConstraintPtr _validityConstraint,
constraint::TestableConstraintPtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector,
double _maxPlanTime);
/// Generate an OMPL SpaceInformation from aikido components
/// \param _statespace The StateSpace that the SpaceInformation operates on
/// \param _interpolator An Interpolator defined on the StateSpace. This is used
/// to interpolate between two points within the space.
/// \param _dmetric A valid distance metric defined on the StateSpace
/// \param _sampler A SampleableConstraint 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.
/// \param _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.
/// \param _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.
/// \param _boundsProjector A Projectable that projects a state back within
/// valid bounds defined on the StateSpace
::ompl::base::SpaceInformationPtr getSpaceInformation(
statespace::StateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleableConstraintPtr _sampler,
constraint::TestableConstraintPtr _validityConstraint,
constraint::TestableConstraintPtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector);
/// Use the template OMPL Planner type to plan in a custom OMPL Space
/// Information and problem definition and return an aikido Trajector
/// \param _si The SpaceInformation used by the planner
/// \param _pdef The ProblemDefintion. This contains start and goal conditions for the planner.
/// \param _sspace The aikido StateSpace to plan against. Used for constructing the return trajectory.
/// \param _interpolator An aikido interpolator that can be used with the _stateSpace.
/// \param _maxPlanTime The maximum time to allow the planner to search for a
/// solution
trajectory::TrajectoryPtr planOMPL(const ::ompl::base::PlannerPtr &_planner,
const ::ompl::base::ProblemDefinitionPtr &_pdef,
statespace::StateSpacePtr _sspace,
statespace::InterpolatorPtr _interpolator,
double _maxPlanTime);
} // namespace ompl
} // namespace planner
} // namespace aikido
#include "detail/Planner-impl.hpp"
#endif