37#include "ompl/geometric/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/tools/config/SelfConfig.h"
60ompl::geometric::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();
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_);
102 OMPL_WARN(
"%s: No optimization object set. Using path length", getName().c_str());
103 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
105 prevSolutionCost_ = opt_->infiniteCost();
118 prevSolutionCost_ = opt_->infiniteCost();
125 std::vector<Motion *> motions;
127 for (
auto &motion : motions)
130 si_->freeState(motion->state_);
136 std::vector<Motion *> witnesses;
137 witnesses_->list(witnesses);
138 for (
auto &witness : witnesses)
141 si_->freeState(witness->state_);
146 for (
auto &i : prevSolution_)
151 prevSolution_.clear();
156 std::vector<Motion *> ret;
157 Motion *selected =
nullptr;
159 nn_->nearestR(sample, selectionRadius_, ret);
162 if (!i->inactive_ && opt_->isCostBetterThan(i->accCost_, bestCost))
164 bestCost = i->accCost_;
168 if (selected ==
nullptr)
171 while (selected ==
nullptr)
173 nn_->nearestK(sample, k, ret);
174 for (
unsigned int i = 0; i < ret.size() && selected ==
nullptr; i++)
175 if (!ret[i]->inactive_)
185 if (witnesses_->size() > 0)
187 auto *closest =
static_cast<Witness *
>(witnesses_->nearest(node));
188 if (distanceFunction(closest, node) > pruningRadius_)
191 closest->linkRep(node);
192 si_->copyState(closest->state_, node->
state_);
193 witnesses_->add(closest);
199 auto *closest =
new Witness(si_);
200 closest->linkRep(node);
201 si_->copyState(closest->state_, node->
state_);
202 witnesses_->add(closest);
211 sampler_->sampleUniform(xstate);
214 double step = rng_.uniformReal(0, maxDistance_);
217 double d = si_->distance(m->
state_, xstate);
218 si_->getStateSpace()->interpolate(m->
state_, xstate, step / d, xstate);
219 si_->enforceBounds(xstate);
232 auto *motion =
new Motion(si_);
233 si_->copyState(motion->state_, st);
235 motion->accCost_ = opt_->identityCost();
236 findClosestWitness(motion);
239 if (nn_->size() == 0)
241 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
246 sampler_ = si_->allocStateSampler();
250 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
252 Motion *solution =
nullptr;
253 Motion *approxsol =
nullptr;
254 double approxdif = std::numeric_limits<double>::infinity();
255 bool sufficientlyShort =
false;
256 auto *rmotion =
new Motion(si_);
260 unsigned iterations = 0;
265 bool attemptToReachGoal = (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample());
266 if (attemptToReachGoal)
267 goal_s->sampleGoal(rstate);
269 sampler_->sampleUniform(rstate);
272 Motion *nmotion = selectNode(rmotion);
275 double d = si_->distance(nmotion->
state_, rstate);
277 attemptToReachGoal = rng_.uniform01() < .5;
278 if (attemptToReachGoal)
280 if (d > maxDistance_)
282 si_->getStateSpace()->interpolate(nmotion->
state_, rstate, maxDistance_ / d, xstate);
288 dstate = monteCarloProp(nmotion);
291 si_->copyState(rstate, dstate);
293 if (si_->checkMotion(nmotion->
state_, rstate))
296 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
297 Witness *closestWitness = findClosestWitness(rmotion);
299 if (closestWitness->
rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->
rep_->accCost_))
303 auto *motion =
new Motion(si_);
304 motion->accCost_ = cost;
305 si_->copyState(motion->state_, rstate);
307 if (!attemptToReachGoal)
308 si_->freeState(dstate);
309 motion->parent_ = nmotion;
311 closestWitness->linkRep(motion);
315 bool solv = goal->
isSatisfied(motion->state_, &dist);
316 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
321 for (
auto &i : prevSolution_)
324 prevSolution_.clear();
325 Motion *solTrav = solution;
326 while (solTrav !=
nullptr)
328 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
331 prevSolutionCost_ = solution->accCost_;
334 if (intermediateSolutionCallback)
337 std::vector<const base::State *> prevSolutionConst(prevSolution_.begin(), prevSolution_.end());
338 intermediateSolutionCallback(
this, prevSolutionConst, prevSolutionCost_);
340 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
341 if (sufficientlyShort)
346 if (solution ==
nullptr && dist < approxdif)
351 for (
auto &i : prevSolution_)
356 prevSolution_.clear();
357 Motion *solTrav = approxsol;
358 while (solTrav !=
nullptr)
360 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
365 if (oldRep != rmotion)
373 si_->freeState(oldRep->
state_);
379 oldRep = oldRepParent;
388 bool approximate =
false;
389 if (solution ==
nullptr)
391 solution = approxsol;
395 if (solution !=
nullptr)
398 auto path(std::make_shared<PathGeometric>(si_));
399 for (
int i = prevSolution_.size() - 1; i >= 0; --i)
400 path->append(prevSolution_[i]);
402 pdef_->addSolutionPath(path, approximate, approxdif, getName());
405 si_->freeState(xstate);
407 si_->freeState(rmotion->state_);
408 rmotion->state_ =
nullptr;
411 OMPL_INFORM(
"%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
413 return {solved, approximate};
418 Planner::getPlannerData(data);
420 std::vector<Motion *> motions;
421 std::vector<Motion *> allMotions;
425 for (
auto &motion : motions)
426 if (motion->numChildren_ == 0)
427 allMotions.push_back(motion);
428 for (
unsigned i = 0; i < allMotions.size(); i++)
429 if (allMotions[i]->getParent() !=
nullptr)
430 allMotions.push_back(allMotions[i]->getParent());
432 if (prevSolution_.size() != 0)
435 for (
auto &allMotion : allMotions)
437 if (allMotion->getParent() ==
nullptr)
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
double value() const
The value of the cost.
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Objective for attempting to maximize the minimum clearance along a path.
The cost of a path is defined as the worst state cost over the entire path. This objective attempts t...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition of an abstract state.
Representation of a motion.
unsigned numChildren_
Number of children.
base::State * state_
The state contained by the motion.
Motion * parent_
The parent motion in the exploration tree.
bool inactive_
If inactive, this node is not considered for selection.
Motion * rep_
The node in the tree that is within the pruning radius.
double getPruningRadius() const
Get the pruning radius the planner is using.
base::Cost prevSolutionCost_
The best solution cost we found so far.
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
double getSelectionRadius() const
Get the selection radius the planner is using.
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
void setGoalBias(double goalBias)
double getRange() const
Get the range the planner is using.
double getGoalBias() const
Get the goal bias the planner is using.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
void freeMemory()
Free the memory allocated by this planner.
SST(const base::SpaceInformationPtr &si)
Constructor.
std::vector< base::State * > prevSolution_
The best solution we found so far.
base::State * monteCarloProp(Motion *m)
Randomly propagate a new edge.
void setRange(double distance)
Set the range the planner is supposed to use.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.