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();
60ompl::control::SST::~SST()
70 nn_->setDistanceFunction([
this](
const Motion *a,
const Motion *b)
72 return distanceFunction(a, b);
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();
108 controlSampler_.reset();
115 prevSolutionCost_ = opt_->infiniteCost();
122 std::vector<Motion *> motions;
124 for (
auto &motion : motions)
127 si_->freeState(motion->state_);
128 if (motion->control_)
129 siC_->freeControl(motion->control_);
135 std::vector<Motion *> witnesses;
136 witnesses_->list(witnesses);
137 for (
auto &witness : witnesses)
142 for (
auto &i : prevSolution_)
147 prevSolution_.clear();
148 for (
auto &prevSolutionControl : prevSolutionControls_)
150 if (prevSolutionControl)
151 siC_->freeControl(prevSolutionControl);
153 prevSolutionControls_.clear();
154 prevSolutionSteps_.clear();
159 std::vector<Motion *> ret;
160 Motion *selected =
nullptr;
162 nn_->nearestR(sample, selectionRadius_, ret);
165 if (!i->inactive_ && opt_->isCostBetterThan(i->accCost_, bestCost))
167 bestCost = i->accCost_;
171 if (selected ==
nullptr)
174 while (selected ==
nullptr)
176 nn_->nearestK(sample, k, ret);
177 for (
unsigned int i = 0; i < ret.size() && selected ==
nullptr; i++)
178 if (!ret[i]->inactive_)
188 if (witnesses_->size() > 0)
190 auto *closest =
static_cast<Witness *
>(witnesses_->nearest(node));
191 if (distanceFunction(closest, node) > pruningRadius_)
194 closest->linkRep(node);
195 si_->copyState(closest->state_, node->
state_);
196 witnesses_->add(closest);
202 auto *closest =
new Witness(siC_);
203 closest->linkRep(node);
204 si_->copyState(closest->state_, node->
state_);
205 witnesses_->add(closest);
218 auto *motion =
new Motion(siC_);
219 si_->copyState(motion->state_, st);
220 siC_->nullControl(motion->control_);
222 motion->accCost_ = opt_->identityCost();
223 findClosestWitness(motion);
226 if (nn_->size() == 0)
228 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
233 sampler_ = si_->allocStateSampler();
234 if (!controlSampler_)
235 controlSampler_ = siC_->allocControlSampler();
239 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
241 Motion *solution =
nullptr;
242 Motion *approxsol =
nullptr;
243 double approxdif = std::numeric_limits<double>::infinity();
244 bool sufficientlyShort =
false;
246 auto *rmotion =
new Motion(siC_);
248 Control *rctrl = rmotion->control_;
251 unsigned iterations = 0;
256 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
257 goal_s->sampleGoal(rstate);
259 sampler_->sampleUniform(rstate);
262 Motion *nmotion = selectNode(rmotion);
265 controlSampler_->sample(rctrl);
266 unsigned int cd = rng_.uniformInt(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
267 unsigned int propCd = siC_->propagateWhileValid(nmotion->
state_, rctrl, cd, rstate);
272 base::Cost incCostControl = opt_->controlCost(rctrl, cd);
273 base::Cost incCost = opt_->combineCosts(incCostMotion, incCostControl);
274 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
275 Witness *closestWitness = findClosestWitness(rmotion);
277 if (closestWitness->
rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->
rep_->accCost_))
281 auto *motion =
new Motion(siC_);
282 motion->accCost_ = cost;
283 si_->copyState(motion->state_, rmotion->state_);
284 siC_->copyControl(motion->control_, rctrl);
286 motion->parent_ = nmotion;
288 closestWitness->linkRep(motion);
292 bool solv = goal->
isSatisfied(motion->state_, &dist);
293 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
298 for (
auto &i : prevSolution_)
301 prevSolution_.clear();
302 for (
auto &prevSolutionControl : prevSolutionControls_)
303 if (prevSolutionControl)
304 siC_->freeControl(prevSolutionControl);
305 prevSolutionControls_.clear();
306 prevSolutionSteps_.clear();
308 Motion *solTrav = solution;
309 while (solTrav->
parent_ !=
nullptr)
311 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
312 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->
control_));
313 prevSolutionSteps_.push_back(solTrav->
steps_);
316 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
317 prevSolutionCost_ = solution->accCost_;
320 if (intermediateSolutionCallback)
323 std::vector<const base::State *> prevSolutionConst(prevSolution_.begin(), prevSolution_.end());
324 intermediateSolutionCallback(
this, prevSolutionConst, prevSolutionCost_);
326 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
327 if (sufficientlyShort)
330 if (solution ==
nullptr && dist < approxdif)
335 for (
auto &i : prevSolution_)
338 prevSolution_.clear();
339 for (
auto &prevSolutionControl : prevSolutionControls_)
340 if (prevSolutionControl)
341 siC_->freeControl(prevSolutionControl);
342 prevSolutionControls_.clear();
343 prevSolutionSteps_.clear();
345 Motion *solTrav = approxsol;
346 while (solTrav->
parent_ !=
nullptr)
348 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
349 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->
control_));
350 prevSolutionSteps_.push_back(solTrav->
steps_);
353 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
356 if (oldRep != rmotion)
364 si_->freeState(oldRep->
state_);
366 siC_->freeControl(oldRep->
control_);
373 oldRep = oldRepParent;
382 bool approximate =
false;
383 if (solution ==
nullptr)
385 solution = approxsol;
389 if (solution !=
nullptr)
392 auto path(std::make_shared<PathControl>(si_));
393 for (
int i = prevSolution_.size() - 1; i >= 1; --i)
394 path->append(prevSolution_[i], prevSolutionControls_[i - 1],
395 prevSolutionSteps_[i - 1] * siC_->getPropagationStepSize());
396 path->append(prevSolution_[0]);
398 pdef_->addSolutionPath(path, approximate, approxdif, getName());
401 si_->freeState(xstate);
403 si_->freeState(rmotion->state_);
404 if (rmotion->control_)
405 siC_->freeControl(rmotion->control_);
408 OMPL_INFORM(
"%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
410 return {solved, approximate};
415 Planner::getPlannerData(data);
417 std::vector<Motion *> motions;
418 std::vector<Motion *> allMotions;
422 for (
auto &motion : motions)
424 if (motion->numChildren_ == 0)
426 allMotions.push_back(motion);
429 for (
unsigned i = 0; i < allMotions.size(); i++)
431 if (allMotions[i]->parent_ !=
nullptr)
433 allMotions.push_back(allMotions[i]->parent_);
437 double delta = siC_->getPropagationStepSize();
439 if (prevSolution_.size() != 0)
442 for (
auto m : allMotions)
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...
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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.
Definition of an abstract control.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Representation of a motion.
base::State * state_
The state contained by the motion.
unsigned numChildren_
Number of children.
Control * control_
The control contained by the motion.
unsigned int steps_
The number of steps_ the control is applied for.
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.
void setGoalBias(double goalBias)
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.
SST(const SpaceInformationPtr &si)
Constructor.
double getGoalBias() const
Get the goal bias the planner is using.
void freeMemory()
Free the memory allocated by this planner.
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.
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
double getSelectionRadius() const
Get the selection radius the planner is using.
double getPruningRadius() const
Get the pruning radius the planner is using.
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
std::vector< base::State * > prevSolution_
The best solution we found so far.
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.
#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 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.