Program Listing for File PDST.h
↰ Return to documentation for file (src/ompl/geometric/planners/pdst/PDST.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Jonathan Sobieski, Mark Moll */
#ifndef OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
#define OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
#include <utility>
#include "ompl/base/Planner.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/geometric/PathGeometric.h"
#include "ompl/base/PlannerData.h"
#include "ompl/datastructures/BinaryHeap.h"
namespace ompl
{
namespace geometric
{
class PDST : public base::Planner
{
public:
PDST(const base::SpaceInformationPtr &si);
~PDST() override;
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
void clear() override;
void setup() override;
void getPlannerData(base::PlannerData &data) const override;
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
{
projectionEvaluator_ = projectionEvaluator;
}
void setProjectionEvaluator(const std::string &name)
{
projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
}
const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
{
return projectionEvaluator_;
}
void setGoalBias(double goalBias)
{
goalBias_ = goalBias;
}
double getGoalBias() const
{
return goalBias_;
}
protected:
struct Cell;
struct Motion;
struct MotionCompare
{
bool operator()(Motion *p1, Motion *p2) const
{
// lowest priority means highest score
return p1->score() < p2->score();
}
};
struct Motion
{
public:
Motion(base::State *startState, base::State *endState, double priority, Motion *parent)
: startState_(startState)
, endState_(endState)
, priority_(priority)
, parent_(parent)
, cell_(nullptr)
, heapElement_(nullptr)
, isSplit_(false)
{
}
Motion(base::State *state)
: startState_(state)
, endState_(state)
, priority_(0.)
, parent_(nullptr)
, cell_(nullptr)
, heapElement_(nullptr)
, isSplit_(false)
{
}
double score() const
{
return priority_ / cell_->volume_;
}
void updatePriority()
{
priority_ = priority_ * 2.0 + 1.0;
}
Motion *ancestor() const
{
auto *m = const_cast<Motion *>(this);
while ((m->parent_ != nullptr) && m->parent_->endState_ == m->startState_)
m = m->parent_;
return m;
}
ompl::base::State *startState_;
ompl::base::State *endState_;
double priority_;
Motion *parent_;
Cell *cell_;
ompl::BinaryHeap<Motion *, MotionCompare>::Element *heapElement_;
bool isSplit_;
};
struct Cell
{
Cell(double volume, ompl::base::RealVectorBounds bounds, unsigned int splitDimension = 0)
: volume_(volume)
, splitDimension_(splitDimension)
, splitValue_(0.0)
, left_(nullptr)
, right_(nullptr)
, bounds_(std::move(bounds))
{
}
~Cell()
{
if (left_ != nullptr)
{
delete left_;
delete right_;
}
}
void subdivide(unsigned int spaceDimension);
Cell *stab(const Eigen::Ref<Eigen::VectorXd> &projection) const
{
auto *containingCell = const_cast<Cell *>(this);
while (containingCell->left_ != nullptr)
{
if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
containingCell = containingCell->left_;
else
containingCell = containingCell->right_;
}
return containingCell;
}
void addMotion(Motion *motion)
{
motions_.push_back(motion);
motion->cell_ = this;
}
unsigned int size() const
{
unsigned int sz = 1;
if (left_ != nullptr)
sz += left_->size() + right_->size();
return sz;
}
double volume_;
unsigned int splitDimension_;
double splitValue_;
Cell *left_;
Cell *right_;
ompl::base::RealVectorBounds bounds_;
std::vector<Motion *> motions_;
};
void addMotion(Motion *motion, Cell *bsp, base::State * /*state*/, Eigen::Ref<Eigen::VectorXd> /*proj*/);
void updateHeapElement(Motion *motion)
{
if (motion->heapElement_ != nullptr)
priorityQueue_.update(motion->heapElement_);
else
motion->heapElement_ = priorityQueue_.insert(motion);
}
Motion *propagateFrom(Motion *motion, base::State * /*start*/, base::State * /*rnd*/);
void freeMemory();
ompl::base::StateSamplerPtr sampler_;
// Random number generator
RNG rng_;
std::vector<Motion *> startMotions_;
ompl::BinaryHeap<Motion *, MotionCompare> priorityQueue_;
Cell *bsp_{nullptr};
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_;
double goalBias_{0.05};
ompl::base::GoalSampleableRegion *goalSampler_{nullptr};
unsigned int iteration_{1};
Motion *lastGoalMotion_{nullptr};
};
} // namespace geometric
} // namespace ompl
#endif