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_)
69 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
70 nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
71 {
72 return distanceFunction(a, b);
73 });
74 if (!witnesses_)
75 witnesses_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
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 OMPL_INFORM("%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
238
239 Motion *solution = nullptr;
240 Motion *approxsol = nullptr;
241 double approxdif = std::numeric_limits<double>::infinity();
242 bool sufficientlyShort = false;
243
244 auto *rmotion = new Motion(siC_);
245 base::State *rstate = rmotion->state_;
246 Control *rctrl = rmotion->control_;
247 base::State *xstate = si_->allocState();
248
249 unsigned iterations = 0;
250
251 while (ptc == false)
252 {
253 /* sample random state (with goal biasing) */
254 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
255 goal_s->sampleGoal(rstate);
256 else
257 sampler_->sampleUniform(rstate);
258
259 /* find closest state in the tree */
260 Motion *nmotion = selectNode(rmotion);
261
262 /* sample a random control that attempts to go towards the random state, and also sample a control duration */
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);
266
267 if (propCd == cd)
268 {
269 base::Cost incCost = opt_->motionCost(nmotion->state_, rstate);
270 base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
271 Witness *closestWitness = findClosestWitness(rmotion);
272
273 if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->rep_->accCost_))
274 {
275 Motion *oldRep = closestWitness->rep_;
276 /* create a motion */
277 auto *motion = new Motion(siC_);
278 motion->accCost_ = cost;
279 si_->copyState(motion->state_, rmotion->state_);
280 siC_->copyControl(motion->control_, rctrl);
281 motion->steps_ = cd;
282 motion->parent_ = nmotion;
283 nmotion->numChildren_++;
284 closestWitness->linkRep(motion);
285
286 nn_->add(motion);
287 double dist = 0.0;
288 bool solv = goal->isSatisfied(motion->state_, &dist);
289 if (solv && opt_->isCostBetterThan(motion->accCost_, prevSolutionCost_))
290 {
291 approxdif = dist;
292 solution = motion;
293
294 for (auto &i : prevSolution_)
295 if (i)
296 si_->freeState(i);
297 prevSolution_.clear();
298 for (auto &prevSolutionControl : prevSolutionControls_)
299 if (prevSolutionControl)
300 siC_->freeControl(prevSolutionControl);
301 prevSolutionControls_.clear();
302 prevSolutionSteps_.clear();
303
304 Motion *solTrav = solution;
305 while (solTrav->parent_ != nullptr)
306 {
307 prevSolution_.push_back(si_->cloneState(solTrav->state_));
308 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_));
309 prevSolutionSteps_.push_back(solTrav->steps_);
310 solTrav = solTrav->parent_;
311 }
312 prevSolution_.push_back(si_->cloneState(solTrav->state_));
313 prevSolutionCost_ = solution->accCost_;
314
315 OMPL_INFORM("Found solution with cost %.2f", solution->accCost_.value());
316 sufficientlyShort = opt_->isSatisfied(solution->accCost_);
317 if (sufficientlyShort)
318 break;
319 }
320 if (solution == nullptr && dist < approxdif)
321 {
322 approxdif = dist;
323 approxsol = motion;
324
325 for (auto &i : prevSolution_)
326 if (i)
327 si_->freeState(i);
328 prevSolution_.clear();
329 for (auto &prevSolutionControl : prevSolutionControls_)
330 if (prevSolutionControl)
331 siC_->freeControl(prevSolutionControl);
332 prevSolutionControls_.clear();
333 prevSolutionSteps_.clear();
334
335 Motion *solTrav = approxsol;
336 while (solTrav->parent_ != nullptr)
337 {
338 prevSolution_.push_back(si_->cloneState(solTrav->state_));
339 prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_));
340 prevSolutionSteps_.push_back(solTrav->steps_);
341 solTrav = solTrav->parent_;
342 }
343 prevSolution_.push_back(si_->cloneState(solTrav->state_));
344 }
345
346 if (oldRep != rmotion)
347 {
348 while (oldRep->inactive_ && oldRep->numChildren_ == 0)
349 {
350 oldRep->inactive_ = true;
351 nn_->remove(oldRep);
352
353 if (oldRep->state_)
354 si_->freeState(oldRep->state_);
355 if (oldRep->control_)
356 siC_->freeControl(oldRep->control_);
357
358 oldRep->state_ = nullptr;
359 oldRep->control_ = nullptr;
360 oldRep->parent_->numChildren_--;
361 Motion *oldRepParent = oldRep->parent_;
362 delete oldRep;
363 oldRep = oldRepParent;
364 }
365 }
366 }
367 }
368 iterations++;
369 }
370
371 bool solved = false;
372 bool approximate = false;
373 if (solution == nullptr)
374 {
375 solution = approxsol;
376 approximate = true;
377 }
378
379 if (solution != nullptr)
380 {
381 /* set the solution path */
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]);
387 solved = true;
388 pdef_->addSolutionPath(path, approximate, approxdif, getName());
389 }
390
391 si_->freeState(xstate);
392 if (rmotion->state_)
393 si_->freeState(rmotion->state_);
394 if (rmotion->control_)
395 siC_->freeControl(rmotion->control_);
396 delete rmotion;
397
398 OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
399
400 return {solved, approximate};
401}
402
404{
405 Planner::getPlannerData(data);
406
407 std::vector<Motion *> motions;
408 std::vector<Motion *> allMotions;
409 if (nn_)
410 nn_->list(motions);
411
412 for (auto &motion : motions)
413 {
414 if (motion->numChildren_ == 0)
415 {
416 allMotions.push_back(motion);
417 }
418 }
419 for (unsigned i = 0; i < allMotions.size(); i++)
420 {
421 if (allMotions[i]->parent_ != nullptr)
422 {
423 allMotions.push_back(allMotions[i]->parent_);
424 }
425 }
426
427 double delta = siC_->getPropagationStepSize();
428
429 if (prevSolution_.size() != 0)
430 data.addGoalVertex(base::PlannerDataVertex(prevSolution_[0]));
431
432 for (auto m : allMotions)
433 {
434 if (m->parent_)
435 {
436 if (data.hasControls())
437 data.addEdge(base::PlannerDataVertex(m->parent_->state_), base::PlannerDataVertex(m->state_),
438 control::PlannerDataEdgeControl(m->control_, m->steps_ * delta));
439 else
440 data.addEdge(base::PlannerDataVertex(m->parent_->state_), base::PlannerDataVertex(m->state_));
441 }
442 else
444 }
445}
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:429
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:403
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
#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
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition Planner.h:202
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.