39#include <ompl/multilevel/planners/qrrt/QRRTStarImpl.h>
40#include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
41#include <ompl/tools/config/SelfConfig.h>
42#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
43#include <boost/foreach.hpp>
44#include <boost/math/constants/constants.hpp>
45#include "ompl/util/GeometricEquations.h"
48#define foreach BOOST_FOREACH
50ompl::multilevel::QRRTStarImpl::QRRTStarImpl(
const base::SpaceInformationPtr &si, BundleSpace *parent_)
53 setName(
"QRRTStarImpl" + std::to_string(id_));
54 Planner::declareParam<double>(
"useKNearest_",
this, &ompl::multilevel::QRRTStarImpl::setKNearest,
55 &ompl::multilevel::QRRTStarImpl::getKNearest,
"0,1");
57 symmetric_ = getBundle()->getStateSpace()->hasSymmetricInterpolate();
59 setImportance(
"exponential");
61 setGraphSampler(
"randomvertex");
62 setMetric(
"geodesic");
65ompl::multilevel::QRRTStarImpl::~QRRTStarImpl()
72 calculateRewiringLowerBounds();
77 goalConfigurations_.clear();
91 sampleBundleGoalBias(
xRandom_->state);
102 std::vector<Configuration *> nearestNbh;
103 getNearestNeighbors(q_new, nearestNbh);
106 q_new->
lineCost = getOptimizationObjectivePtr()->motionCost(q_nearest->state, q_new->state);
107 q_new->
cost = getOptimizationObjectivePtr()->combineCosts(q_nearest->
cost, q_new->
lineCost);
108 q_new->
parent = q_nearest;
113 std::vector<int> validNeighbor(nearestNbh.size(), 0);
116 std::vector<ompl::base::Cost> lineCosts;
120 lineCosts.resize(nearestNbh.size());
123 for (
unsigned int i = 0; i < nearestNbh.size(); i++)
127 if (q_near == q_nearest)
129 validNeighbor.at(i) = 1;
132 lineCosts[i] = cost_nearest;
136 validNeighbor.at(i) = 0;
138 ompl::base::Cost line_cost = getOptimizationObjectivePtr()->motionCost(q_near->state, q_new->state);
139 ompl::base::Cost new_cost = getOptimizationObjectivePtr()->combineCosts(q_near->
cost, line_cost);
143 lineCosts[i] = line_cost;
146 if (getOptimizationObjectivePtr()->isCostBetterThan(new_cost, q_new->
cost))
152 q_new->
cost = new_cost;
154 validNeighbor.at(i) = 1;
157 validNeighbor.at(i) = -1;
164 bool checkForSolution =
false;
167 for (
unsigned int i = 0; i < nearestNbh.size(); i++)
171 if (q_near != q_new->
parent)
177 line_cost = lineCosts[i];
181 line_cost = getOptimizationObjectivePtr()->motionCost(q_new->state, q_near->state);
183 base::Cost new_cost = getOptimizationObjectivePtr()->combineCosts(q_new->
cost, line_cost);
187 if (getOptimizationObjectivePtr()->isCostBetterThan(new_cost, q_near->
cost))
189 bool valid = (validNeighbor.at(i) == 1);
191 if (validNeighbor.at(i) == 0)
194 getBundle()->checkMotion(q_new->state, q_near->state));
204 removeFromParent(q_near);
208 q_near->
cost = new_cost;
213 updateChildCosts(q_near);
214 checkForSolution =
true;
222 bool satisfied =
pdef_->getGoal()->isSatisfied(q_new->state, &dist);
226 checkForSolution =
true;
229 if (checkForSolution)
231 bool updatedSolution =
false;
236 updatedSolution =
true;
244 if (getOptimizationObjectivePtr()->isCostBetterThan(qk->
cost,
bestCost_))
248 updatedSolution =
true;
261void ompl::multilevel::QRRTStarImpl::updateChildCosts(Configuration *q)
263 for (std::size_t i = 0; i < q->
children.size(); ++i)
265 q->
children.at(i)->cost = getOptimizationObjectivePtr()->combineCosts(q->
cost, q->
children.at(i)->lineCost);
266 updateChildCosts(q->
children.at(i));
270void ompl::multilevel::QRRTStarImpl::removeFromParent(Configuration *q)
282bool ompl::multilevel::QRRTStarImpl::getSolution(base::PathPtr &solution)
286 solutionPath_ = std::make_shared<geometric::PathGeometric>(getBundle());
288 Configuration *intermediate_node = qGoal_;
289 while (intermediate_node !=
nullptr)
291 std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->append(intermediate_node->state);
292 intermediate_node = intermediate_node->
parent;
294 std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->reverse();
295 solution = solutionPath_;
304void ompl::multilevel::QRRTStarImpl::getNearestNeighbors(Configuration *x, std::vector<Configuration *> &nearest)
306 auto cardDbl =
static_cast<double>(nearestDatastructure_->size() + 1u);
310 unsigned int k = std::ceil(k_rrt_Constant_ *
log(cardDbl));
311 nearestDatastructure_->nearestK(x, k, nearest);
315 double r = std::min(maxDistance_, r_rrt_Constant_ * std::pow(
log(cardDbl) / cardDbl, 1 / d_));
316 nearestDatastructure_->nearestR(x, r, nearest);
320void ompl::multilevel::QRRTStarImpl::calculateRewiringLowerBounds()
322 d_ = (double)getBundle()->getStateDimension();
323 double e = boost::math::constants::e<double>();
325 k_rrt_Constant_ = std::pow(2, d_ + 1) * e * (1.0 + 1.0 / d_);
328 std::pow(2 * (1.0 + 1.0 / d_) * (getBundle()->getSpaceMeasure() /
unitNBallMeasure(d_)), 1.0 / d_);
331void ompl::multilevel::QRRTStarImpl::getPlannerData(base::PlannerData &data)
const
333 multilevel::PlannerDataVertexAnnotated pstart(qStart_->state);
334 pstart.setLevel(getLevel());
335 data.addStartVertex(pstart);
339 multilevel::PlannerDataVertexAnnotated pgoal(qGoal_->state);
340 pgoal.setLevel(getLevel());
341 data.addGoalVertex(pgoal);
344 std::vector<Configuration *> motions;
345 if (nearestDatastructure_)
346 nearestDatastructure_->list(motions);
348 foreach (
const Configuration *q, motions)
352 multilevel::PlannerDataVertexAnnotated p1(q->
parent->state);
353 multilevel::PlannerDataVertexAnnotated p2(q->state);
354 p1.setLevel(getLevel());
355 p2.setLevel(getLevel());
356 data.addEdge(p1, p2);
360 OMPL_DEBUG(
"Tree (level %d) has %d/%d vertices/edges", getLevel(), motions.size(), motions.size() - 1);
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
ProblemDefinitionPtr pdef_
The user set problem definition.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
A configuration in Bundle-space.
Configuration * parent
parent index for {qrrt*}
base::Cost cost
cost to reach until current vertex in {qrrt*}
base::Cost lineCost
same as rrt*, connection cost with parent {qrrt*}
std::vector< Configuration * > children
The set of motions descending from the current motion {qrrt*}.
double maxDistance_
Maximum expansion step.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for Bundle space configurations.
Configuration * qGoal_
The (best) goal configuration.
Configuration * xRandom_
Temporary random configuration.
Configuration * steerTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.
std::vector< Configuration * > goalConfigurations_
List of configurations that satisfy the goal condition.
base::Cost bestCost_
Best cost found so far by algorithm.
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
virtual double distance(const Configuration *a, const Configuration *b) const
Distance between two configurations using the current metric.
virtual bool checkMotion(const Configuration *a, const Configuration *b) const
Check if we can move from configuration a to configuration b using the current metric.
bool findSection() override
Call algorithm to solve the find section problem.
bool hasSolution_
If there exists a solution.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
bool firstRun_
Variable to check if this bundle space planner has been run at.
unsigned int getLevel() const
Level in hierarchy of Bundle-spaces.
bool symmetric_
true if cost from a to b is same as b to a
virtual void grow() override
One iteration of RRT with adjusted sampling function.
bool useKNearest_
option to use k nn or radius
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.