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();
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();
237 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
239 Motion *solution =
nullptr;
240 Motion *approxsol =
nullptr;
241 double approxdif = std::numeric_limits<double>::infinity();
242 bool sufficientlyShort =
false;
244 auto *rmotion =
new Motion(siC_);
246 Control *rctrl = rmotion->control_;
249 unsigned iterations = 0;
254 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
255 goal_s->sampleGoal(rstate);
257 sampler_->sampleUniform(rstate);
260 Motion *nmotion = selectNode(rmotion);
263 controlSampler_->sample(rctrl);
264 unsigned int cd = rng_.uniformInt(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
265 unsigned int propCd = siC_->propagateWhileValid(nmotion->
state_, rctrl, cd, rstate);
270 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
271 Witness *closestWitness = findClosestWitness(rmotion);
273 if (closestWitness->
rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->
rep_->accCost_))
277 auto *motion =
new Motion(siC_);
278 motion->accCost_ = cost;
279 si_->copyState(motion->state_, rmotion->state_);
280 siC_->copyControl(motion->control_, rctrl);
282 motion->parent_ = nmotion;
284 closestWitness->linkRep(motion);
288 bool solv = goal->
isSatisfied(motion->state_, &dist);
289 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
294 for (
auto &i : prevSolution_)
297 prevSolution_.clear();
298 for (
auto &prevSolutionControl : prevSolutionControls_)
299 if (prevSolutionControl)
300 siC_->freeControl(prevSolutionControl);
301 prevSolutionControls_.clear();
302 prevSolutionSteps_.clear();
304 Motion *solTrav = solution;
305 while (solTrav->
parent_ !=
nullptr)
307 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
308 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->
control_));
309 prevSolutionSteps_.push_back(solTrav->
steps_);
312 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
313 prevSolutionCost_ = solution->accCost_;
316 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
317 if (sufficientlyShort)
320 if (solution ==
nullptr && dist < approxdif)
325 for (
auto &i : prevSolution_)
328 prevSolution_.clear();
329 for (
auto &prevSolutionControl : prevSolutionControls_)
330 if (prevSolutionControl)
331 siC_->freeControl(prevSolutionControl);
332 prevSolutionControls_.clear();
333 prevSolutionSteps_.clear();
335 Motion *solTrav = approxsol;
336 while (solTrav->
parent_ !=
nullptr)
338 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
339 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->
control_));
340 prevSolutionSteps_.push_back(solTrav->
steps_);
343 prevSolution_.push_back(si_->cloneState(solTrav->
state_));
346 if (oldRep != rmotion)
354 si_->freeState(oldRep->
state_);
356 siC_->freeControl(oldRep->
control_);
363 oldRep = oldRepParent;
372 bool approximate =
false;
373 if (solution ==
nullptr)
375 solution = approxsol;
379 if (solution !=
nullptr)
382 auto path(std::make_shared<PathControl>(si_));
383 for (
int i = prevSolution_.size() - 1; i >= 1; --i)
384 path->append(prevSolution_[i], prevSolutionControls_[i - 1],
385 prevSolutionSteps_[i - 1] * siC_->getPropagationStepSize());
386 path->append(prevSolution_[0]);
388 pdef_->addSolutionPath(path, approximate, approxdif, getName());
391 si_->freeState(xstate);
393 si_->freeState(rmotion->state_);
394 if (rmotion->control_)
395 siC_->freeControl(rmotion->control_);
398 OMPL_INFORM(
"%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
400 return {solved, approximate};
405 Planner::getPlannerData(data);
407 std::vector<Motion *> motions;
408 std::vector<Motion *> allMotions;
412 for (
auto &motion : motions)
414 if (motion->numChildren_ == 0)
416 allMotions.push_back(motion);
419 for (
unsigned i = 0; i < allMotions.size(); i++)
421 if (allMotions[i]->parent_ !=
nullptr)
423 allMotions.push_back(allMotions[i]->parent_);
427 double delta = siC_->getPropagationStepSize();
429 if (prevSolution_.size() != 0)
432 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.
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.