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()
69void ompl::multilevel::QRRTStarImpl::setup()
72 calculateRewiringLowerBounds();
74void ompl::multilevel::QRRTStarImpl::clear()
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.
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.