-
Notifications
You must be signed in to change notification settings - Fork 30
/
Planner.cpp
143 lines (121 loc) · 4.62 KB
/
Planner.cpp
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
139
140
141
142
143
#include <aikido/planner/ompl/Planner.hpp>
#include <aikido/planner/ompl/GeometricStateSpace.hpp>
#include <aikido/constraint/ConjunctionConstraint.hpp>
namespace aikido {
namespace planner {
namespace ompl {
//=============================================================================
::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)
{
if (_stateSpace == nullptr) {
throw std::invalid_argument("StateSpace is nullptr.");
}
if (_interpolator == nullptr) {
throw std::invalid_argument("Interpolator is nullptr.");
}
if (_dmetric == nullptr) {
throw std::invalid_argument("DistanceMetric is nullptr.");
}
if (_sampler == nullptr) {
throw std::invalid_argument("Sampler is nullptr.");
}
if (_validityConstraint == nullptr) {
throw std::invalid_argument("ValidityConstraint is nullptr.");
}
if (_boundsConstraint == nullptr) {
throw std::invalid_argument("BoundsConstraint is nullptr.");
}
if (_boundsProjector == nullptr) {
throw std::invalid_argument("BoundsProjector is nullptr.");
}
if (_stateSpace != _interpolator->getStateSpace()) {
throw std::invalid_argument(
"StateSpace of interpolator not equal to planning StateSpace");
}
// Ensure distance metric and state space match
if (_stateSpace != _dmetric->getStateSpace()) {
throw std::invalid_argument(
"StateSpace of DistanceMetric not equal to planning StateSpace");
}
// Ensure sampleable constraint and state space match
if (_stateSpace != _sampler->getStateSpace()) {
throw std::invalid_argument(
"StateSpace of sampler not equal to planning StateSpace");
}
// Ensure bounds constraint and state space match
if (_stateSpace != _validityConstraint->getStateSpace()) {
throw std::invalid_argument(
"StateSpace of ValidityConstraint not equal to planning StateSpace");
}
// Ensure bounds constraint and state space match
if (_stateSpace != _boundsConstraint->getStateSpace()) {
throw std::invalid_argument(
"StateSpace of BoundsConstraint not equal to planning StateSpace");
}
// Ensure the projector state space and state space match
if (_stateSpace != _boundsProjector->getStateSpace()) {
throw std::invalid_argument(
"StateSpace of BoundsProjector not equal to planning StateSpace");
}
// Geometric State space
auto sspace = boost::make_shared<GeometricStateSpace>(
_stateSpace, std::move(_interpolator), std::move(_dmetric),
std::move(_sampler), _boundsConstraint,
std::move(_boundsProjector));
// Space Information
auto si = boost::make_shared<::ompl::base::SpaceInformation>(std::move(sspace));
// Validity checking
std::vector<constraint::TestableConstraintPtr> constraints{
std::move(_validityConstraint), std::move(_boundsConstraint)};
auto conjunctionConstraint =
std::make_shared<constraint::ConjunctionConstraint>(std::move(_stateSpace),
std::move(constraints));
::ompl::base::StateValidityCheckerPtr vchecker =
boost::make_shared<StateValidityChecker>(si, conjunctionConstraint);
si->setStateValidityChecker(vchecker);
return si;
}
//=============================================================================
trajectory::TrajectoryPtr planOMPL(
const ::ompl::base::PlannerPtr &_planner,
const ::ompl::base::ProblemDefinitionPtr &_pdef,
statespace::StateSpacePtr _sspace,
statespace::InterpolatorPtr _interpolator,
double _maxPlanTime)
{
// Planner
_planner->setProblemDefinition(_pdef);
_planner->setup();
auto solved = _planner->solve(_maxPlanTime);
auto returnTraj = boost::make_shared<trajectory::Interpolated>(
std::move(_sspace), std::move(_interpolator));
if (solved) {
// Get the path
auto path =
boost::dynamic_pointer_cast<::ompl::geometric::PathGeometric>(
_pdef->getSolutionPath());
if(!path){
throw std::invalid_argument(
"Path is not of type PathGeometric. Cannot convert to aikido "
"Trajectory");
}
for (size_t idx = 0; idx < path->getStateCount(); ++idx) {
const auto *st =
static_cast<GeometricStateSpace::StateType *>(
path->getState(idx));
// Arbitrary timing
returnTraj->addWaypoint(idx, st->mState);
}
}
return returnTraj;
}
}
}
}