Loading...
Searching...
No Matches
SST.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, Rutgers the State University of New Jersey, New Brunswick
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of Rutgers University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Authors: Zakary Littlefield */
36
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"
44#include <limits>
45
46ompl::control::SST::SST(const SpaceInformationPtr &si) : base::Planner(si, "SST")
47{
49 siC_ = si.get();
50 prevSolution_.clear();
51 prevSolutionControls_.clear();
52 prevSolutionSteps_.clear();
53
54 Planner::declareParam<double>("goal_bias", this, &SST::setGoalBias, &SST::getGoalBias, "0.:.05:1.");
55 Planner::declareParam<double>("selection_radius", this, &SST::setSelectionRadius, &SST::getSelectionRadius, "0.:.1:"
56 "100");
57 Planner::declareParam<double>("pruning_radius", this, &SST::setPruningRadius, &SST::getPruningRadius, "0.:.1:100");
58}
59
60ompl::control::SST::~SST()
61{
62 freeMemory();
63}
64
66{
68 if (!nn_)
70 nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
71 {
72 return distanceFunction(a, b);
73 });
74 if (!witnesses_)
76 witnesses_->setDistanceFunction([this](const Motion *a, const Motion *b)
77 {
78 return distanceFunction(a, b);
79 });
80
81 if (pdef_)
82 {
83 if (pdef_->hasOptimizationObjective())
84 {
85 opt_ = pdef_->getOptimizationObjective();
86 if (dynamic_cast<base::MaximizeMinClearanceObjective *>(opt_.get()) ||
87 dynamic_cast<base::MinimaxObjective *>(opt_.get()))
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 "
90 "behavior",
91 getName().c_str());
92 }
93 else
94 {
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_);
98 }
99 }
100
101 prevSolutionCost_ = opt_->infiniteCost();
102}
103
105{
106 Planner::clear();
107 sampler_.reset();
108 controlSampler_.reset();
109 freeMemory();
110 if (nn_)
111 nn_->clear();
112 if (witnesses_)
113 witnesses_->clear();
114 if (opt_)
115 prevSolutionCost_ = opt_->infiniteCost();
116}
117
119{
120 if (nn_)
121 {
122 std::vector<Motion *> motions;
123 nn_->list(motions);
124 for (auto &motion : motions)
125 {
126 if (motion->state_)
127 si_->freeState(motion->state_);
128 if (motion->control_)
129 siC_->freeControl(motion->control_);
130 delete motion;
131 }
132 }
133 if (witnesses_)
134 {
135 std::vector<Motion *> witnesses;
136 witnesses_->list(witnesses);
137 for (auto &witness : witnesses)
138 {
139 delete witness;
140 }
141 }
142 for (auto &i : prevSolution_)
143 {
144 if (i)
145 si_->freeState(i);
146 }
147 prevSolution_.clear();
148 for (auto &prevSolutionControl : prevSolutionControls_)
149 {
150 if (prevSolutionControl)
151 siC_->freeControl(prevSolutionControl);
152 }
153 prevSolutionControls_.clear();
154 prevSolutionSteps_.clear();
155}
156
158{
159 std::vector<Motion *> ret;
160 Motion *selected = nullptr;
161 base::Cost bestCost = opt_->infiniteCost();
162 nn_->nearestR(sample, selectionRadius_, ret);
163 for (auto &i : ret)
164 {
165 if (!i->inactive_ && opt_->isCostBetterThan(i->accCost_, bestCost))
166 {
167 bestCost = i->accCost_;
168 selected = i;
169 }
170 }
171 if (selected == nullptr)
172 {
173 int k = 1;
174 while (selected == nullptr)
175 {
176 nn_->nearestK(sample, k, ret);
177 for (unsigned int i = 0; i < ret.size() && selected == nullptr; i++)
178 if (!ret[i]->inactive_)
179 selected = ret[i];
180 k += 5;
181 }
182 }
183 return selected;
184}
185
187{
188 if (witnesses_->size() > 0)
189 {
190 auto *closest = static_cast<Witness *>(witnesses_->nearest(node));
191 if (distanceFunction(closest, node) > pruningRadius_)
192 {
193 closest = new Witness(siC_);
194 closest->linkRep(node);
195 si_->copyState(closest->state_, node->state_);
196 witnesses_->add(closest);
197 }
198 return closest;
199 }
200 else
201 {
202 auto *closest = new Witness(siC_);
203 closest->linkRep(node);
204 si_->copyState(closest->state_, node->state_);
205 witnesses_->add(closest);
206 return closest;
207 }
208}
209
211{
212 checkValidity();
213 base::Goal *goal = pdef_->getGoal().get();
214 auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
215
216 while (const base::State *st = pis_.nextStart())
217 {
218 auto *motion = new Motion(siC_);
219 si_->copyState(motion->state_, st);
220 siC_->nullControl(motion->control_);
221 nn_->add(motion);
222 motion->accCost_ = opt_->identityCost();
223 findClosestWitness(motion);
224 }
225
226 if (nn_->size() == 0)
227 {
228 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
230 }
231
232 if (!sampler_)
233 sampler_ = si_->allocStateSampler();
234 if (!controlSampler_)
235 controlSampler_ = siC_->allocControlSampler();
236
237 const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
238
239 OMPL_INFORM("%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
240
241 Motion *solution = nullptr;
242 Motion *approxsol = nullptr;
243 double approxdif = std::numeric_limits<double>::infinity();
244 bool sufficientlyShort = false;
245
246 auto *rmotion = new Motion(siC_);
247 base::State *rstate = rmotion->state_;
248 Control *rctrl = rmotion->control_;
249 base::State *xstate = si_->allocState();
250
251 unsigned iterations = 0;
252
253 while (ptc == false)
254 {
255 /* sample random state (with goal biasing) */
256 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
257 goal_s->sampleGoal(rstate);
258 else
259 sampler_->sampleUniform(rstate);
260
261 /* find closest state in the tree */
262 Motion *nmotion = selectNode(rmotion);
263
264 /* sample a random control that attempts to go towards the random state, and also sample a control duration */
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);
268
269 if (propCd == cd)
270 {
271 base::Cost incCostMotion = opt_->motionCost(nmotion->state_, 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);
276
277 if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->rep_->accCost_))
278 {
279 Motion *oldRep = closestWitness->rep_;
280 /* create a motion */
281 auto *motion = new Motion(siC_);
282 motion->accCost_ = cost;
283 si_->copyState(motion->state_, rmotion->state_);
284 siC_->copyControl(motion->control_, rctrl);
285 motion->steps_ = cd;
286 motion->parent_ = nmotion;
287 nmotion->numChildren_++;
288 closestWitness->linkRep(motion);
289
290 nn_->add(motion);
291 double dist = 0.0;
292 bool solv = goal->isSatisfied(motion->state_, &dist);
293 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
294 {
295 approxdif = dist;
296 solution = motion;
297
298 for (auto &i : prevSolution_)
299 if (i)
300 si_->freeState(i);
301 prevSolution_.clear();
302 for (auto &prevSolutionControl : prevSolutionControls_)
303 if (prevSolutionControl)
304 siC_->freeControl(prevSolutionControl);
305 prevSolutionControls_.clear();
306 prevSolutionSteps_.clear();
307
308 Motion *solTrav = solution;
309 while (solTrav->parent_ != nullptr)
310 {
311 prevSolution_.push_back(si_->cloneState(solTrav->state_));
312 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_));
313 prevSolutionSteps_.push_back(solTrav->steps_);
314 solTrav = solTrav->parent_;
315 }
316 prevSolution_.push_back(si_->cloneState(solTrav->state_));
317 prevSolutionCost_ = solution->accCost_;
318
319 OMPL_INFORM("Found solution with cost %.2f", solution->accCost_.value());
320 if (intermediateSolutionCallback)
321 {
322 // the callback requires a vector with const elements -> create a copy
323 std::vector<const base::State *> prevSolutionConst(prevSolution_.begin(), prevSolution_.end());
324 intermediateSolutionCallback(this, prevSolutionConst, prevSolutionCost_);
325 }
326 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
327 if (sufficientlyShort)
328 break;
329 }
330 if (solution == nullptr && dist < approxdif)
331 {
332 approxdif = dist;
333 approxsol = motion;
334
335 for (auto &i : prevSolution_)
336 if (i)
337 si_->freeState(i);
338 prevSolution_.clear();
339 for (auto &prevSolutionControl : prevSolutionControls_)
340 if (prevSolutionControl)
341 siC_->freeControl(prevSolutionControl);
342 prevSolutionControls_.clear();
343 prevSolutionSteps_.clear();
344
345 Motion *solTrav = approxsol;
346 while (solTrav->parent_ != nullptr)
347 {
348 prevSolution_.push_back(si_->cloneState(solTrav->state_));
349 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_));
350 prevSolutionSteps_.push_back(solTrav->steps_);
351 solTrav = solTrav->parent_;
352 }
353 prevSolution_.push_back(si_->cloneState(solTrav->state_));
354 }
355
356 if (oldRep != rmotion)
357 {
358 while (oldRep->inactive_ && oldRep->numChildren_ == 0)
359 {
360 oldRep->inactive_ = true;
361 nn_->remove(oldRep);
362
363 if (oldRep->state_)
364 si_->freeState(oldRep->state_);
365 if (oldRep->control_)
366 siC_->freeControl(oldRep->control_);
367
368 oldRep->state_ = nullptr;
369 oldRep->control_ = nullptr;
370 oldRep->parent_->numChildren_--;
371 Motion *oldRepParent = oldRep->parent_;
372 delete oldRep;
373 oldRep = oldRepParent;
374 }
375 }
376 }
377 }
378 iterations++;
379 }
380
381 bool solved = false;
382 bool approximate = false;
383 if (solution == nullptr)
384 {
385 solution = approxsol;
386 approximate = true;
387 }
388
389 if (solution != nullptr)
390 {
391 /* set the solution path */
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]);
397 solved = true;
398 pdef_->addSolutionPath(path, approximate, approxdif, getName());
399 }
400
401 si_->freeState(xstate);
402 if (rmotion->state_)
403 si_->freeState(rmotion->state_);
404 if (rmotion->control_)
405 siC_->freeControl(rmotion->control_);
406 delete rmotion;
407
408 OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
409
410 return {solved, approximate};
411}
412
414{
415 Planner::getPlannerData(data);
416
417 std::vector<Motion *> motions;
418 std::vector<Motion *> allMotions;
419 if (nn_)
420 nn_->list(motions);
421
422 for (auto &motion : motions)
423 {
424 if (motion->numChildren_ == 0)
425 {
426 allMotions.push_back(motion);
427 }
428 }
429 for (unsigned i = 0; i < allMotions.size(); i++)
430 {
431 if (allMotions[i]->parent_ != nullptr)
432 {
433 allMotions.push_back(allMotions[i]->parent_);
434 }
435 }
436
437 double delta = siC_->getPropagationStepSize();
438
439 if (prevSolution_.size() != 0)
440 data.addGoalVertex(base::PlannerDataVertex(prevSolution_[0]));
441
442 for (auto m : allMotions)
443 {
444 if (m->parent_)
445 {
446 if (data.hasControls())
447 data.addEdge(base::PlannerDataVertex(m->parent_->state_), base::PlannerDataVertex(m->state_),
448 control::PlannerDataEdgeControl(m->control_, m->steps_ * delta));
449 else
450 data.addEdge(base::PlannerDataVertex(m->parent_->state_), base::PlannerDataVertex(m->state_));
451 }
452 else
454 }
455}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition Goal.h:63
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...
Definition PlannerData.h:59
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)
Definition Planner.h:422
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition Planner.cpp:92
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
Definition of an abstract control.
Definition Control.h:48
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition PlannerData.h:61
Representation of a motion.
Definition SST.h:155
base::State * state_
The state contained by the motion.
Definition SST.h:179
unsigned numChildren_
Number of children.
Definition SST.h:191
Control * control_
The control contained by the motion.
Definition SST.h:182
unsigned int steps_
The number of steps_ the control is applied for.
Definition SST.h:185
Motion * parent_
The parent motion in the exploration tree.
Definition SST.h:188
bool inactive_
If inactive, this node is not considered for selection.
Definition SST.h:194
Motion * rep_
The node in the tree that is within the pruning radius.
Definition SST.h:220
void setGoalBias(double goalBias)
Definition SST.h:86
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition SST.cpp:413
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
Definition SST.h:105
SST(const SpaceInformationPtr &si)
Constructor.
Definition SST.cpp:46
double getGoalBias() const
Get the goal bias the planner is using.
Definition SST.h:92
void freeMemory()
Free the memory allocated by this planner.
Definition SST.cpp:118
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SST.cpp:65
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Definition SST.cpp:210
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition SST.h:245
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
Definition SST.cpp:157
double getSelectionRadius() const
Get the selection radius the planner is using.
Definition SST.h:111
double getPruningRadius() const
Get the pruning radius the planner is using.
Definition SST.h:132
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
Definition SST.h:126
std::vector< base::State * > prevSolution_
The best solution we found so far.
Definition SST.h:267
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition SST.cpp:104
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
Definition SST.cpp:186
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
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.
Definition Planner.h:195
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.