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" 58 ompl::geometric::SST::~SST()
67 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
68 nn_->setDistanceFunction([
this](
const Motion *a,
const Motion *b)
70 return distanceFunction(a, b);
73 witnesses_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
74 witnesses_->setDistanceFunction([
this](
const Motion *a,
const Motion *b)
76 return distanceFunction(a, b);
81 if (pdef_->hasOptimizationObjective())
83 opt_ = pdef_->getOptimizationObjective();
84 if (dynamic_cast<base::MaximizeMinClearanceObjective *>(opt_.get()) ||
85 dynamic_cast<base::MinimaxObjective *>(opt_.get()))
86 OMPL_WARN(
"%s: Asymptotic near-optimality has only been proven with Lipschitz continuous cost " 87 "functions w.r.t. state and control. This optimization objective will result in undefined " 93 OMPL_WARN(
"%s: No optimization object set. Using path length", getName().c_str());
94 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
95 pdef_->setOptimizationObjective(opt_);
99 prevSolutionCost_ = opt_->infiniteCost();
111 prevSolutionCost_ = opt_->infiniteCost();
118 std::vector<Motion *> motions;
120 for (
auto &motion : motions)
123 si_->freeState(motion->state_);
129 std::vector<Motion *> witnesses;
130 witnesses_->list(witnesses);
131 for (
auto &witness : witnesses)
134 si_->freeState(witness->state_);
139 for (
auto &i : prevSolution_)
144 prevSolution_.clear();
149 std::vector<Motion *> ret;
150 Motion *selected =
nullptr;
152 nn_->nearestR(sample, selectionRadius_, ret);
155 if (!i->inactive_ && opt_->isCostBetterThan(i->accCost_, bestCost))
157 bestCost = i->accCost_;
161 if (selected ==
nullptr)
164 while (selected ==
nullptr)
166 nn_->nearestK(sample, k, ret);
167 for (
unsigned int i = 0; i < ret.size() && selected ==
nullptr; i++)
168 if (!ret[i]->inactive_)
178 if (witnesses_->size() > 0)
180 auto *closest =
static_cast<Witness *
>(witnesses_->nearest(node));
181 if (distanceFunction(closest, node) > pruningRadius_)
184 closest->linkRep(node);
185 si_->copyState(closest->state_, node->
state_);
186 witnesses_->add(closest);
192 auto *closest =
new Witness(si_);
193 closest->linkRep(node);
194 si_->copyState(closest->state_, node->
state_);
195 witnesses_->add(closest);
204 sampler_->sampleUniform(xstate);
207 double step = rng_.uniformReal(0, maxDistance_);
210 double d = si_->distance(m->
state_, xstate);
211 si_->getStateSpace()->interpolate(m->
state_, xstate, step / d, xstate);
224 auto *motion =
new Motion(si_);
225 si_->copyState(motion->state_, st);
227 motion->accCost_ = opt_->identityCost();
228 findClosestWitness(motion);
231 if (nn_->size() == 0)
233 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
238 sampler_ = si_->allocStateSampler();
240 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
242 Motion *solution =
nullptr;
243 Motion *approxsol =
nullptr;
244 double approxdif = std::numeric_limits<double>::infinity();
245 bool sufficientlyShort =
false;
246 auto *rmotion =
new Motion(si_);
250 unsigned iterations = 0;
255 bool attemptToReachGoal = (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample());
256 if (attemptToReachGoal)
257 goal_s->sampleGoal(rstate);
259 sampler_->sampleUniform(rstate);
262 Motion *nmotion = selectNode(rmotion);
265 double d = si_->distance(nmotion->
state_, rstate);
267 attemptToReachGoal = rng_.uniform01() < .5;
268 if (attemptToReachGoal)
270 if (d > maxDistance_)
272 si_->getStateSpace()->interpolate(nmotion->
state_, rstate, maxDistance_ / d, xstate);
278 dstate = monteCarloProp(nmotion);
281 si_->copyState(rstate, dstate);
283 if (si_->checkMotion(nmotion->
state_, rstate))
286 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
287 Witness *closestWitness = findClosestWitness(rmotion);
289 if (closestWitness->
rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->
rep_->accCost_))
293 auto *motion =
new Motion(si_);
294 motion->accCost_ = cost;
295 si_->copyState(motion->state_, rstate);
297 if (!attemptToReachGoal)
298 si_->freeState(dstate);
299 motion->parent_ = nmotion;
301 closestWitness->linkRep(motion);
305 bool solv = goal->
isSatisfied(motion->state_, &dist);
306 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
311 for (
auto &i : prevSolution_)
314 prevSolution_.clear();
315 Motion *solTrav = solution;
316 while (solTrav !=
nullptr)
318 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
321 prevSolutionCost_ = solution->accCost_;
323 OMPL_INFORM(
"Found solution with cost %.2f", solution->accCost_.value());
324 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
325 if (sufficientlyShort)
330 if (solution ==
nullptr && dist < approxdif)
335 for (
auto &i : prevSolution_)
340 prevSolution_.clear();
341 Motion *solTrav = approxsol;
342 while (solTrav !=
nullptr)
344 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
349 if (oldRep != rmotion)
356 si_->freeState(oldRep->
state_);
361 oldRep = oldRepParent;
370 bool approximate =
false;
371 if (solution ==
nullptr)
373 solution = approxsol;
377 if (solution !=
nullptr)
380 auto path(std::make_shared<PathGeometric>(si_));
381 for (
int i = prevSolution_.size() - 1; i >= 0; --i)
382 path->append(prevSolution_[i]);
384 pdef_->addSolutionPath(path, approximate, approxdif, getName());
387 si_->freeState(xstate);
389 si_->freeState(rmotion->state_);
390 rmotion->state_ =
nullptr;
393 OMPL_INFORM(
"%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
400 Planner::getPlannerData(data);
402 std::vector<Motion *> motions;
403 std::vector<Motion *> allMotions;
407 for (
auto &motion : motions)
408 if (motion->numChildren_ == 0)
409 allMotions.push_back(motion);
410 for (
unsigned i = 0; i < allMotions.size(); i++)
411 if (allMotions[i]->getParent() !=
nullptr)
412 allMotions.push_back(allMotions[i]->getParent());
414 if (prevSolution_.size() != 0)
417 for (
auto &allMotion : allMotions)
419 if (allMotion->getParent() ==
nullptr)
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Motion * rep_
The node in the tree that is within the pruning radius.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void freeMemory()
Free the memory allocated by this planner.
Motion * parent_
The parent motion in the exploration tree.
SST(const base::SpaceInformationPtr &si)
Constructor.
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
bool inactive_
If inactive, this node is not considered for selection.
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
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.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned numChildren_
Number of children.
void setGoalBias(double goalBias)
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
double getPruningRadius() const
Get the pruning radius the planner is using.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Representation of a motion.
Invalid start state or no start state specified.
Abstract definition of a goal region that can be sampled.
base::State * monteCarloProp(Motion *m)
Randomly propagate a new edge.
std::vector< base::State * > prevSolution_
The best solution we found so far.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
double getSelectionRadius() const
Get the selection radius the planner is using.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
double getRange() const
Get the range the planner is using.
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.
void setRange(double distance)
Set the range the planner is supposed to use.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
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 setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
double getGoalBias() const
Get the goal bias the planner is using.
base::State * state_
The state contained by the motion.
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.