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"
60 ompl::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_);
101 prevSolutionCost_ = opt_->infiniteCost();
114 prevSolutionCost_ = opt_->infiniteCost();
121 std::vector<Motion *> motions;
123 for (
auto &motion : motions)
126 si_->freeState(motion->state_);
132 std::vector<Motion *> witnesses;
133 witnesses_->list(witnesses);
134 for (
auto &witness : witnesses)
137 si_->freeState(witness->state_);
142 for (
auto &i : prevSolution_)
147 prevSolution_.clear();
152 std::vector<Motion *> ret;
153 Motion *selected =
nullptr;
155 nn_->nearestR(sample, selectionRadius_, ret);
158 if (!i->inactive_ && opt_->isCostBetterThan(i->accCost_, bestCost))
160 bestCost = i->accCost_;
164 if (selected ==
nullptr)
167 while (selected ==
nullptr)
169 nn_->nearestK(sample, k, ret);
170 for (
unsigned int i = 0; i < ret.size() && selected ==
nullptr; i++)
171 if (!ret[i]->inactive_)
181 if (witnesses_->size() > 0)
183 auto *closest =
static_cast<Witness *
>(witnesses_->nearest(node));
184 if (distanceFunction(closest, node) > pruningRadius_)
187 closest->linkRep(node);
188 si_->copyState(closest->state_, node->
state_);
189 witnesses_->add(closest);
195 auto *closest =
new Witness(si_);
196 closest->linkRep(node);
197 si_->copyState(closest->state_, node->
state_);
198 witnesses_->add(closest);
207 sampler_->sampleUniform(xstate);
210 double step = rng_.uniformReal(0, maxDistance_);
213 double d = si_->distance(m->
state_, xstate);
214 si_->getStateSpace()->interpolate(m->
state_, xstate, step / d, xstate);
215 si_->enforceBounds(xstate);
228 auto *motion =
new Motion(si_);
229 si_->copyState(motion->state_, st);
231 motion->accCost_ = opt_->identityCost();
232 findClosestWitness(motion);
235 if (nn_->size() == 0)
237 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
242 sampler_ = si_->allocStateSampler();
244 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
246 Motion *solution =
nullptr;
247 Motion *approxsol =
nullptr;
248 double approxdif = std::numeric_limits<double>::infinity();
249 bool sufficientlyShort =
false;
250 auto *rmotion =
new Motion(si_);
254 unsigned iterations = 0;
259 bool attemptToReachGoal = (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample());
260 if (attemptToReachGoal)
261 goal_s->sampleGoal(rstate);
263 sampler_->sampleUniform(rstate);
266 Motion *nmotion = selectNode(rmotion);
269 double d = si_->distance(nmotion->
state_, rstate);
271 attemptToReachGoal = rng_.uniform01() < .5;
272 if (attemptToReachGoal)
274 if (d > maxDistance_)
276 si_->getStateSpace()->interpolate(nmotion->
state_, rstate, maxDistance_ / d, xstate);
282 dstate = monteCarloProp(nmotion);
285 si_->copyState(rstate, dstate);
287 if (si_->checkMotion(nmotion->
state_, rstate))
290 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
291 Witness *closestWitness = findClosestWitness(rmotion);
293 if (closestWitness->
rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->
rep_->accCost_))
297 auto *motion =
new Motion(si_);
298 motion->accCost_ = cost;
299 si_->copyState(motion->state_, rstate);
301 if (!attemptToReachGoal)
302 si_->freeState(dstate);
303 motion->parent_ = nmotion;
305 closestWitness->linkRep(motion);
309 bool solv = goal->isSatisfied(motion->state_, &dist);
310 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
315 for (
auto &i : prevSolution_)
318 prevSolution_.clear();
319 Motion *solTrav = solution;
320 while (solTrav !=
nullptr)
322 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
325 prevSolutionCost_ = solution->accCost_;
328 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
329 if (sufficientlyShort)
334 if (solution ==
nullptr && dist < approxdif)
339 for (
auto &i : prevSolution_)
344 prevSolution_.clear();
345 Motion *solTrav = approxsol;
346 while (solTrav !=
nullptr)
348 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
353 if (oldRep != rmotion)
361 si_->freeState(oldRep->
state_);
367 oldRep = oldRepParent;
376 bool approximate =
false;
377 if (solution ==
nullptr)
379 solution = approxsol;
383 if (solution !=
nullptr)
386 auto path(std::make_shared<PathGeometric>(si_));
387 for (
int i = prevSolution_.size() - 1; i >= 0; --i)
388 path->append(prevSolution_[i]);
390 pdef_->addSolutionPath(path, approximate, approxdif, getName());
393 si_->freeState(xstate);
395 si_->freeState(rmotion->state_);
396 rmotion->state_ =
nullptr;
399 OMPL_INFORM(
"%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
401 return {solved, approximate};
406 Planner::getPlannerData(data);
408 std::vector<Motion *> motions;
409 std::vector<Motion *> allMotions;
413 for (
auto &motion : motions)
414 if (motion->numChildren_ == 0)
415 allMotions.push_back(motion);
416 for (
unsigned i = 0; i < allMotions.size(); i++)
417 if (allMotions[i]->getParent() !=
nullptr)
418 allMotions.push_back(allMotions[i]->getParent());
420 if (prevSolution_.size() != 0)
423 for (
auto &allMotion : allMotions)
425 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.
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.
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.