-
Notifications
You must be signed in to change notification settings - Fork 30
/
Planner-impl.hpp
103 lines (88 loc) · 3.69 KB
/
Planner-impl.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
#include "../GeometricStateSpace.hpp"
#include "../StateValidityChecker.hpp"
#include "../GoalRegion.hpp"
#include "../../../trajectory/Interpolated.hpp"
#include <ompl/geometric/PathGeometric.h>
namespace aikido {
namespace planner {
namespace ompl {
//=============================================================================
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)
{
// Create a SpaceInformation. This function will ensure state space matching
auto si = getSpaceInformation(
_stateSpace, _interpolator, std::move(_dmetric), std::move(_sampler),
std::move(_validityConstraint), std::move(_boundsConstraint),
std::move(_boundsProjector));
// Start and states
auto pdef = boost::make_shared<::ompl::base::ProblemDefinition>(si);
auto sspace = boost::static_pointer_cast<GeometricStateSpace>(
si->getStateSpace());
auto start = sspace->allocState(_start);
auto goal = sspace->allocState(_goal);
// ProblemDefinition clones states and keeps them internally
pdef->setStartAndGoalStates(start, goal);
sspace->freeState(start);
sspace->freeState(goal);
auto planner = boost::make_shared<PlannerType>(si);
return planOMPL(planner, pdef, std::move(_stateSpace),
std::move(_interpolator), _maxPlanTime);
}
//=============================================================================
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)
{
if (_goalTestable == nullptr) {
throw std::invalid_argument("Testable goal is nullptr.");
}
if (_goalSampler == nullptr) {
throw std::invalid_argument("Sampleable goal is nullptr.");
}
if (_goalTestable->getStateSpace() != _stateSpace) {
throw std::invalid_argument("Testable goal does not match StateSpace");
}
if (_goalSampler->getStateSpace() != _stateSpace) {
throw std::invalid_argument("Sampleable goal does not match StateSpace");
}
auto si = getSpaceInformation(
_stateSpace, _interpolator, std::move(_dmetric), std::move(_sampler),
std::move(_validityConstraint), std::move(_boundsConstraint),
std::move(_boundsProjector));
// Set the start and goal
auto pdef = boost::make_shared<::ompl::base::ProblemDefinition>(si);
auto sspace = boost::static_pointer_cast<GeometricStateSpace>(
si->getStateSpace());
auto start = sspace->allocState(_start);
pdef->addStartState(start); // copies
sspace->freeState(start);
auto goalRegion = boost::make_shared<GoalRegion>(
si, std::move(_goalTestable), _goalSampler->createSampleGenerator());
pdef->setGoal(goalRegion);
auto planner = boost::make_shared<PlannerType>(si);
return planOMPL(planner, pdef, std::move(_stateSpace),
std::move(_interpolator), _maxPlanTime);
}
}
}
}