37 #include "ompl/control/planners/sst/SST.h" 38 #include "ompl/base/goals/GoalSampleableRegion.h" 39 #include "ompl/base/objectives/MinimaxObjective.h" 40 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h" 41 #include "ompl/base/objectives/PathLengthOptimizationObjective.h" 42 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h" 43 #include "ompl/tools/config/SelfConfig.h" 51 prevSolutionControls_.clear();
52 prevSolutionSteps_.clear();
60 ompl::control::SST::~SST()
69 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
70 nn_->setDistanceFunction([
this](
const Motion *a,
const Motion *b)
72 return distanceFunction(a, b);
75 witnesses_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
76 witnesses_->setDistanceFunction([
this](
const Motion *a,
const Motion *b)
78 return distanceFunction(a, b);
83 if (pdef_->hasOptimizationObjective())
85 opt_ = pdef_->getOptimizationObjective();
86 if (dynamic_cast<base::MaximizeMinClearanceObjective *>(opt_.get()) ||
87 dynamic_cast<base::MinimaxObjective *>(opt_.get()))
88 OMPL_WARN(
"%s: Asymptotic near-optimality has only been proven with Lipschitz continuous cost " 89 "functions w.r.t. state and control. This optimization objective will result in undefined " 95 OMPL_WARN(
"%s: No optimization object set. Using path length", getName().c_str());
96 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
97 pdef_->setOptimizationObjective(opt_);
101 prevSolutionCost_ = opt_->infiniteCost();
108 controlSampler_.reset();
114 prevSolutionCost_ = opt_->infiniteCost();
121 std::vector<Motion *> motions;
123 for (
auto &motion : motions)
126 si_->freeState(motion->state_);
127 if (motion->control_)
128 siC_->freeControl(motion->control_);
134 std::vector<Motion *> witnesses;
135 witnesses_->list(witnesses);
136 for (
auto &witness : witnesses)
141 for (
auto &i : prevSolution_)
146 prevSolution_.clear();
147 for (
auto &prevSolutionControl : prevSolutionControls_)
149 if (prevSolutionControl)
150 siC_->freeControl(prevSolutionControl);
152 prevSolutionControls_.clear();
153 prevSolutionSteps_.clear();
158 std::vector<Motion *> ret;
159 Motion *selected =
nullptr;
161 nn_->nearestR(sample, selectionRadius_, ret);
164 if (!i->inactive_ && opt_->isCostBetterThan(i->accCost_, bestCost))
166 bestCost = i->accCost_;
170 if (selected ==
nullptr)
173 while (selected ==
nullptr)
175 nn_->nearestK(sample, k, ret);
176 for (
unsigned int i = 0; i < ret.size() && selected ==
nullptr; i++)
177 if (!ret[i]->inactive_)
187 if (witnesses_->size() > 0)
189 auto *closest =
static_cast<Witness *
>(witnesses_->nearest(node));
190 if (distanceFunction(closest, node) > pruningRadius_)
193 closest->linkRep(node);
194 si_->copyState(closest->state_, node->
state_);
195 witnesses_->add(closest);
201 auto *closest =
new Witness(siC_);
202 closest->linkRep(node);
203 si_->copyState(closest->state_, node->
state_);
204 witnesses_->add(closest);
217 auto *motion =
new Motion(siC_);
218 si_->copyState(motion->state_, st);
219 siC_->nullControl(motion->control_);
221 motion->accCost_ = opt_->identityCost();
222 findClosestWitness(motion);
225 if (nn_->size() == 0)
227 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
232 sampler_ = si_->allocStateSampler();
233 if (!controlSampler_)
234 controlSampler_ = siC_->allocControlSampler();
236 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
238 Motion *solution =
nullptr;
239 Motion *approxsol =
nullptr;
240 double approxdif = std::numeric_limits<double>::infinity();
241 bool sufficientlyShort =
false;
243 auto *rmotion =
new Motion(siC_);
245 Control *rctrl = rmotion->control_;
248 unsigned iterations = 0;
253 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
254 goal_s->sampleGoal(rstate);
256 sampler_->sampleUniform(rstate);
259 Motion *nmotion = selectNode(rmotion);
262 controlSampler_->sample(rctrl);
263 unsigned int cd = rng_.uniformInt(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
264 unsigned int propCd = siC_->propagateWhileValid(nmotion->
state_, rctrl, cd, rstate);
269 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
270 Witness *closestWitness = findClosestWitness(rmotion);
272 if (closestWitness->
rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->
rep_->accCost_))
276 auto *motion =
new Motion(siC_);
277 motion->accCost_ = cost;
278 si_->copyState(motion->state_, rmotion->state_);
279 siC_->copyControl(motion->control_, rctrl);
281 motion->parent_ = nmotion;
283 closestWitness->linkRep(motion);
287 bool solv = goal->
isSatisfied(motion->state_, &dist);
288 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
293 for (
auto &i : prevSolution_)
296 prevSolution_.clear();
297 for (
auto &prevSolutionControl : prevSolutionControls_)
298 if (prevSolutionControl)
299 siC_->freeControl(prevSolutionControl);
300 prevSolutionControls_.clear();
301 prevSolutionSteps_.clear();
303 Motion *solTrav = solution;
304 while (solTrav->
parent_ !=
nullptr)
306 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
307 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->
control_));
308 prevSolutionSteps_.push_back(solTrav->
steps_);
311 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
312 prevSolutionCost_ = solution->accCost_;
314 OMPL_INFORM(
"Found solution with cost %.2f", solution->accCost_.value());
315 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
316 if (sufficientlyShort)
319 if (solution ==
nullptr && dist < approxdif)
324 for (
auto &i : prevSolution_)
327 prevSolution_.clear();
328 for (
auto &prevSolutionControl : prevSolutionControls_)
329 if (prevSolutionControl)
330 siC_->freeControl(prevSolutionControl);
331 prevSolutionControls_.clear();
332 prevSolutionSteps_.clear();
334 Motion *solTrav = approxsol;
335 while (solTrav->
parent_ !=
nullptr)
337 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
338 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->
control_));
339 prevSolutionSteps_.push_back(solTrav->
steps_);
342 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
345 if (oldRep != rmotion)
352 si_->freeState(oldRep->
state_);
354 siC_->freeControl(oldRep->
control_);
361 oldRep = oldRepParent;
370 bool approximate =
false;
371 if (solution ==
nullptr)
373 solution = approxsol;
377 if (solution !=
nullptr)
380 auto path(std::make_shared<PathControl>(si_));
381 for (
int i = prevSolution_.size() - 1; i >= 1; --i)
382 path->append(prevSolution_[i], prevSolutionControls_[i - 1],
383 prevSolutionSteps_[i - 1] * siC_->getPropagationStepSize());
384 path->append(prevSolution_[0]);
386 pdef_->addSolutionPath(path, approximate, approxdif, getName());
389 si_->freeState(xstate);
391 si_->freeState(rmotion->state_);
392 if (rmotion->control_)
393 siC_->freeControl(rmotion->control_);
396 OMPL_INFORM(
"%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
403 Planner::getPlannerData(data);
405 std::vector<Motion *> motions;
406 std::vector<Motion *> allMotions;
410 for (
auto &motion : motions)
412 if (motion->numChildren_ == 0)
414 allMotions.push_back(motion);
417 for (
unsigned i = 0; i < allMotions.size(); i++)
419 if (allMotions[i]->parent_ !=
nullptr)
421 allMotions.push_back(allMotions[i]->parent_);
425 double delta = siC_->getPropagationStepSize();
427 if (prevSolution_.size() != 0)
430 for (
auto m : allMotions)
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
double getGoalBias() const
Get the goal bias the planner is using.
unsigned numChildren_
Number of children.
Definition of an abstract control.
Motion * parent_
The parent motion in the exploration tree.
std::vector< base::State * > prevSolution_
The best solution we found so far.
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
bool inactive_
If inactive, this node is not considered for selection.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of goals.
Control * control_
The control contained by the motion.
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
SST(const SpaceInformationPtr &si)
Constructor.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
double getSelectionRadius() const
Get the selection radius the planner is using.
Motion * rep_
The node in the tree that is within the pruning radius.
Abstract definition of a goal region that can be sampled.
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
unsigned int steps_
The number of steps_ the control is applied for.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
A class to store the exit status of Planner::solve()
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
base::State * state_
The state contained by the motion.
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double getPruningRadius() const
Get the pruning radius the planner is using.
Representation of a motion.
void setGoalBias(double goalBias)
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
void freeMemory()
Free the memory allocated by this planner.
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.