From a9b3bc93d1a8985ba99283584ba41887e77eaebe Mon Sep 17 00:00:00 2001 From: Alan Zhou Date: Mon, 24 Apr 2023 08:45:36 -0400 Subject: [PATCH] adding warm-starting, more info for the cost convergence criteria, and some other debugging info around informed sampling. very much WIP --- .../src/PathLengthDirectInfSampler.cpp | 6 + .../CostConvergenceTerminationCondition.h | 3 + .../CostConvergenceTerminationCondition.cpp | 17 +- src/ompl/geometric/planners/rrt/RRT.h | 1 + src/ompl/geometric/planners/rrt/RRTConnect.h | 10 + src/ompl/geometric/planners/rrt/RRTstar.h | 123 +++++++----- .../geometric/planners/rrt/SimpleMotion.h | 54 +++++ .../geometric/planners/rrt/src/RRTstar.cpp | 189 ++++++++++-------- src/ompl/tools/thunder/Thunder.h | 12 +- src/ompl/tools/thunder/src/Thunder.cpp | 25 ++- 10 files changed, 301 insertions(+), 139 deletions(-) create mode 100644 src/ompl/geometric/planners/rrt/SimpleMotion.h diff --git a/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp b/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp index 8215f3a156..8996116d99 100644 --- a/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp +++ b/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp @@ -371,6 +371,7 @@ namespace ompl } else // We have a solution { + // OMPL_INFORM("DirectedInfSampler: have solution! maxCost: %f", maxCost.value()); // Update the definitions of the PHSs updatePhsDefinitions(maxCost); @@ -381,11 +382,13 @@ namespace ompl // Check if the average measure is greater than half the domain's measure. Half is an arbitrary number. if (informedSubSpace_->getMeasure() < summedMeasure_ / static_cast(listPhsPtrs_.size())) { + // OMPL_DEBUG("Measure is big. %f < %f", informedSubSpace_->getMeasure(), summedMeasure_ / static_cast(listPhsPtrs_.size())); // The measure is large, sample from the entire world and keep if it's in any PHS foundSample = sampleBoundsRejectPhs(statePtr, iters); } else { + OMPL_DEBUG("measure is small enough %f >= %f that we sample the PHS", informedSubSpace_->getMeasure(), summedMeasure_ / static_cast(listPhsPtrs_.size())); // The measure is sufficiently small that we will directly sample the PHSes, with the weighting // given by their relative measures foundSample = samplePhsRejectBounds(statePtr, iters); @@ -417,6 +420,9 @@ namespace ompl // Increment the provided counter ++(*iters); } + // if(foundSample){ + // OMPL_DEBUG("found sample in %u iters", *iters); + // } // successful? return foundSample; diff --git a/src/ompl/base/terminationconditions/CostConvergenceTerminationCondition.h b/src/ompl/base/terminationconditions/CostConvergenceTerminationCondition.h index efe54652af..9ba37e6f63 100644 --- a/src/ompl/base/terminationconditions/CostConvergenceTerminationCondition.h +++ b/src/ompl/base/terminationconditions/CostConvergenceTerminationCondition.h @@ -65,6 +65,9 @@ namespace ompl ProblemDefinitionPtr pdef_; /// Cumulative moving average of solutions found so far. double averageCost_{0.}; + /// Gradient of the cost + double cost_dot_ {0.}; + double cost_ddot_ {0.0}; /// Number of solutions found so far. size_t solutions_{0}; diff --git a/src/ompl/base/terminationconditions/src/CostConvergenceTerminationCondition.cpp b/src/ompl/base/terminationconditions/src/CostConvergenceTerminationCondition.cpp index d8f3731b84..95d9889254 100644 --- a/src/ompl/base/terminationconditions/src/CostConvergenceTerminationCondition.cpp +++ b/src/ompl/base/terminationconditions/src/CostConvergenceTerminationCondition.cpp @@ -56,11 +56,24 @@ void ompl::base::CostConvergenceTerminationCondition::processNewSolution(const o double newCost = ((solutions - 1) * averageCost_ + solutionCost.value()) / solutions; double costLowerThreshold = (1. - epsilon_) * averageCost_; double costUpperThreshold = (1. + epsilon_) * averageCost_; + double old_cost_dot {cost_dot_}; + double cost_delta {averageCost_ - newCost}; averageCost_ = newCost; + if (cost_delta > 0) { + cost_dot_ = ((solutions - 1) * cost_dot_ + (cost_delta)) / solutions; + cost_ddot_ = ((solutions - 1) * cost_ddot_ + (old_cost_dot - cost_dot_)) / solutions; + } + + + + OMPL_DEBUG("CCTC: %u: avg_cost(%u): %f; upper/lower: %f/%f; gradient: %f; cost_ddot: %f", solutions_, solutions, averageCost_, costUpperThreshold, costLowerThreshold, cost_dot_, cost_ddot_); - if (solutions == solutionsWindow_ && + if (solutions_ > 3 * solutionsWindow_ && + solutions == solutionsWindow_ && averageCost_ > costLowerThreshold && - averageCost_ < costUpperThreshold) + averageCost_ < costUpperThreshold && + cost_dot_ > 0 && + cost_dot_ < epsilon_) { OMPL_DEBUG("CostConvergenceTerminationCondition: Cost of optimizing planner converged after %lu solutions", solutions_); terminate(); diff --git a/src/ompl/geometric/planners/rrt/RRT.h b/src/ompl/geometric/planners/rrt/RRT.h index cc9db54f61..2e24f2da1b 100644 --- a/src/ompl/geometric/planners/rrt/RRT.h +++ b/src/ompl/geometric/planners/rrt/RRT.h @@ -174,6 +174,7 @@ namespace ompl /** \brief State sampler */ base::StateSamplerPtr sampler_; + // base::ValidStateSamplerPtr sampler_; /** \brief A nearest-neighbors datastructure containing the tree of motions */ std::shared_ptr> nn_; diff --git a/src/ompl/geometric/planners/rrt/RRTConnect.h b/src/ompl/geometric/planners/rrt/RRTConnect.h index 871c9e4a48..2a1aa22c74 100644 --- a/src/ompl/geometric/planners/rrt/RRTConnect.h +++ b/src/ompl/geometric/planners/rrt/RRTConnect.h @@ -39,6 +39,7 @@ #include "ompl/datastructures/NearestNeighbors.h" #include "ompl/geometric/planners/PlannerIncludes.h" +#include "ompl/geometric/planners/rrt/SimpleMotion.h" namespace ompl { @@ -114,6 +115,15 @@ namespace ompl setup(); } + // std::shared_ptr> getNearestNeighbors() { + // return nn_; + // } + + // void copyNearestNeighbors(std::shared_ptr> nn) { + // *nn_ = *nn; + // setup(); + // } + void setup() override; protected: diff --git a/src/ompl/geometric/planners/rrt/RRTstar.h b/src/ompl/geometric/planners/rrt/RRTstar.h index 6af9847fbd..89955a640f 100644 --- a/src/ompl/geometric/planners/rrt/RRTstar.h +++ b/src/ompl/geometric/planners/rrt/RRTstar.h @@ -40,6 +40,7 @@ #include "ompl/geometric/planners/PlannerIncludes.h" #include "ompl/base/OptimizationObjective.h" #include "ompl/datastructures/NearestNeighbors.h" +#include "ompl/geometric/planners/rrt/SimpleMotion.h" #include #include @@ -65,7 +66,7 @@ namespace ompl the algorithm terminates before the elapsed time. @par External documentation S. Karaman and E. Frazzoli, Sampling-based - Algorithms for Optimal Motion Planning, International Journal of Robotics + Algorithms for Optimal SimpleMotion Planning, International Journal of Robotics Research, Vol 30, No 7, 2011. https://arxiv.org/abs/1105.1186 */ @@ -144,10 +145,71 @@ namespace ompl if (nn_ && nn_->size() != 0) OMPL_WARN("Calling setNearestNeighbors will clear all states."); clear(); - nn_ = std::make_shared>(); + nn_ = std::make_shared>(); setup(); } + // NearestNeighbors getNearestNeighbors() { + // return static_cast>(*nn_); + // } + + // void copyNearestNeighbors(std::shared_ptr> nn) { + // *nn_ = static_cast>(*nn); + // setup(); + // } + void addPotentialSolutionPath(PathGeometric soln_path) { + // std::cerr << "adding " << soln_path.getStateCount() << "states to the nn." << std::endl; + for(unsigned int i{0}; igetOptimizationObjective(); + std::cerr << "no space optimizer!" << std::endl; + } + if(!nn_) { + std::cerr << "no nearest neighbor object!" << std::endl; + } + // std::cerr << "we have si, pdef, opt, and nn. ready to rock." << std::endl; + base::State *dstate {soln_path.getState(i)}; + if(!dstate){ + std::cerr << "failed to copy the state for " << i << std::endl; + } + // create a motion + auto *motion = new SimpleMotion(si_); + + si_->copyState(motion->state, dstate); + if(i==0){ + // std::cerr << "i=" << i << ", therefore setting to identity cost" << std::endl; + motion->cost = opt_->identityCost(); + } else { + // std::cerr << "i=" << i << ", finding nearest neighbor." << std::endl; + SimpleMotion *nmotion = nn_->nearest(motion); + if(!nmotion) { + std::cerr << "failed for find a nearest neighbor nmotion." << std::endl; + } + motion->incCost = opt_->motionCost(nmotion->state, motion->state); + motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost); + motion->parent = nmotion; + } + if(i == (soln_path.getStateCount()-1)) { + double distanceFromGoal; + base::Goal *goal = pdef_->getGoal().get(); + if (goal->isSatisfied(motion->state, &distanceFromGoal)) + { + motion->inGoal = true; + goalSimpleMotions_.push_back(motion); + bestGoalSimpleMotion_ = goalSimpleMotions_.front(); + bestCost_ = bestGoalSimpleMotion_->cost; + } + } + nn_->add(motion); + } + } + /** \brief Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cost. The planner then goes through this list, starting with the lowest @@ -329,38 +391,6 @@ namespace ompl } protected: - /** \brief Representation of a motion */ - class Motion - { - public: - /** \brief Constructor that allocates memory for the state. This constructor automatically allocates - * memory for \e state, \e cost, and \e incCost */ - Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr), inGoal(false) - { - } - - ~Motion() = default; - - /** \brief The state contained by the motion */ - base::State *state; - - /** \brief The parent motion in the exploration tree */ - Motion *parent; - - /** \brief Set to true if this vertex is in the goal region */ - bool inGoal; - - /** \brief The cost up to this motion */ - base::Cost cost; - - /** \brief The incremental cost of this motion's parent to this motion (this is stored to save distance - * computations in the updateChildCosts() method) */ - base::Cost incCost; - - /** \brief The set of motions descending from the current motion */ - std::vector children; - }; - /** \brief Create the samplers */ void allocSampler(); @@ -386,19 +416,19 @@ namespace ompl }; /** \brief Compute distance between motions (actually distance between contained states) */ - double distanceFunction(const Motion *a, const Motion *b) const + double distanceFunction(const SimpleMotion *a, const SimpleMotion *b) const { return si_->distance(a->state, b->state); } /** \brief Gets the neighbours of a given motion, using either k-nearest of radius as appropriate. */ - void getNeighbors(Motion *motion, std::vector &nbh) const; + void getNeighbors(SimpleMotion *motion, std::vector &nbh) const; /** \brief Removes the given motion from the parent's child list */ - void removeFromParent(Motion *m); + void removeFromParent(SimpleMotion *m); /** \brief Updates the cost of the children of this node if the cost up to this node has changed */ - void updateChildCosts(Motion *m); + void updateChildCosts(SimpleMotion *m); /** \brief Prunes all those states which estimated total cost is higher than pruneTreeCost. Returns the number of motions pruned. Depends on the parameter set by @@ -410,26 +440,27 @@ namespace ompl (\e setAdmissibleCostToCome()) is true, a heuristic estimate of the cost to come is used; otherwise, the current cost to come to the motion is used (which may overestimate the cost through the motion). */ - base::Cost solutionHeuristic(const Motion *motion) const; + base::Cost solutionHeuristic(const SimpleMotion *motion) const; /** \brief Add the children of a vertex to the given list. */ - void addChildrenToList(std::queue> *motionList, Motion *motion); + void addChildrenToList(std::queue> *motionList, SimpleMotion *motion); /** \brief Check whether the given motion passes the specified cost threshold, meaning it will be \e kept * during pruning */ - bool keepCondition(const Motion *motion, const base::Cost &threshold) const; + bool keepCondition(const SimpleMotion *motion, const base::Cost &threshold) const; /** \brief Calculate the k_RRG* and r_RRG* terms */ void calculateRewiringLowerBounds(); /** \brief State sampler */ base::StateSamplerPtr sampler_; + // base::ValidStateSamplerPtr sampler_; /** \brief An informed sampler */ base::InformedSamplerPtr infSampler_; /** \brief A nearest-neighbors datastructure containing the tree of motions */ - std::shared_ptr> nn_; + std::shared_ptr> nn_; /** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is * available) */ @@ -461,10 +492,10 @@ namespace ompl base::OptimizationObjectivePtr opt_; /** \brief The best goal motion. */ - Motion *bestGoalMotion_{nullptr}; + SimpleMotion *bestGoalSimpleMotion_{nullptr}; /** \brief A list of states in the tree that satisfy the goal condition */ - std::vector goalMotions_; + std::vector goalSimpleMotions_; /** \brief The status of the tree pruning option. */ bool useTreePruning_{false}; @@ -496,8 +527,8 @@ namespace ompl /** \brief The size of the batches. */ unsigned int batchSize_{1u}; - /** \brief Stores the start states as Motions. */ - std::vector startMotions_; + /** \brief Stores the start states as SimpleMotions. */ + std::vector startSimpleMotions_; /** \brief Best cost found so far by algorithm */ base::Cost bestCost_{std::numeric_limits::quiet_NaN()}; diff --git a/src/ompl/geometric/planners/rrt/SimpleMotion.h b/src/ompl/geometric/planners/rrt/SimpleMotion.h new file mode 100644 index 0000000000..31c9293472 --- /dev/null +++ b/src/ompl/geometric/planners/rrt/SimpleMotion.h @@ -0,0 +1,54 @@ +#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_SIMPLE_MOTION_ +#define OMPL_GEOMETRIC_PLANNERS_RRT_SIMPLE_MOTION_ + +#include "ompl/geometric/planners/PlannerIncludes.h" +#include "ompl/base/OptimizationObjective.h" +#include "ompl/datastructures/NearestNeighbors.h" +#include "ompl/geometric/planners/rrt/SimpleMotion.h" + +#include +#include +#include +#include +#include +#include + +namespace ompl +{ + namespace geometric + { + /** \brief Representation of a motion */ + class SimpleMotion + { + public: + /** \brief Constructor that allocates memory for the state. This constructor automatically allocates + * memory for \e state, \e cost, and \e incCost */ + SimpleMotion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr), inGoal(false) + { + } + + ~SimpleMotion() = default; + + /** \brief The state contained by the motion */ + base::State *state; + + /** \brief The parent motion in the exploration tree */ + SimpleMotion *parent; + + /** \brief Set to true if this vertex is in the goal region */ + bool inGoal; + + /** \brief The cost up to this motion */ + base::Cost cost; + + /** \brief The incremental cost of this motion's parent to this motion (this is stored to save distance + * computations in the updateChildCosts() method) */ + base::Cost incCost; + + /** \brief The set of motions descending from the current motion */ + std::vector children; + }; + } +} + +#endif \ No newline at end of file diff --git a/src/ompl/geometric/planners/rrt/src/RRTstar.cpp b/src/ompl/geometric/planners/rrt/src/RRTstar.cpp index 427da59ed0..cb517fd115 100644 --- a/src/ompl/geometric/planners/rrt/src/RRTstar.cpp +++ b/src/ompl/geometric/planners/rrt/src/RRTstar.cpp @@ -102,8 +102,8 @@ void ompl::geometric::RRTstar::setup() } if (!nn_) - nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors(this)); - nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); }); + nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors(this)); + nn_->setDistanceFunction([this](const SimpleMotion *a, const SimpleMotion *b) { return distanceFunction(a, b); }); // Setup optimization objective // @@ -152,9 +152,9 @@ void ompl::geometric::RRTstar::clear() if (nn_) nn_->clear(); - bestGoalMotion_ = nullptr; - goalMotions_.clear(); - startMotions_.clear(); + bestGoalSimpleMotion_ = nullptr; + goalSimpleMotions_.clear(); + startSimpleMotions_.clear(); iterations_ = 0; bestCost_ = base::Cost(std::numeric_limits::quiet_NaN()); @@ -176,11 +176,11 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer // There are, add them while (const base::State *st = pis_.nextStart()) { - auto *motion = new Motion(si_); + auto *motion = new SimpleMotion(si_); si_->copyState(motion->state, st); motion->cost = opt_->identityCost(); nn_->add(motion); - startMotions_.push_back(motion); + startSimpleMotions_.push_back(motion); } // And assure that, if we're using an informed sampler, it's reset @@ -200,7 +200,8 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer allocSampler(); } - OMPL_INFORM("%s: Started planning with %u states. Seeking a solution better than %.5f.", getName().c_str(), nn_->size(), opt_->getCostThreshold().value()); + OMPL_INFORM("%s: Started planning with %u states. Seeking a solution better than threshold %.5f.", getName().c_str(), nn_->size(), opt_->getCostThreshold().value()); + if ((useTreePruning_ || useRejectionSampling_ || useInformedSampling_ || useNewStateRejection_) && !si_->getStateSpace()->isMetricSpace()) @@ -211,16 +212,16 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback(); - Motion *approxGoalMotion = nullptr; + SimpleMotion *approxGoalSimpleMotion = nullptr; double approxDist {0.0}; const bool returnApproxSol {pdef_->getReturnApproximateSolutions()}; if (returnApproxSol) approxDist = std::numeric_limits::infinity(); - auto *rmotion = new Motion(si_); + auto *rmotion = new SimpleMotion(si_); base::State *rstate = rmotion->state; base::State *xstate = si_->allocState(); - std::vector nbh; + std::vector nbh; std::vector costs; std::vector incCosts; @@ -230,7 +231,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer unsigned int rewireTest = 0; unsigned int statesGenerated = 0; - if (bestGoalMotion_) + if (bestGoalSimpleMotion_) OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(), bestCost_.value()); @@ -248,27 +249,38 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer while (ptc == false) { + // OMPL_DEBUG("starting iter %u", iterations_); iterations_++; // sample random state (with goal biasing) // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal // states. - if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && - goal_s->canSample()) - goal_s->sampleGoal(rstate); + if (goal_s && goalSimpleMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && + goal_s->canSample()) { + // OMPL_DEBUG("goal sampling at iter %u", iterations_); + goal_s->sampleGoal(rstate); + } + else { // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this // loop and return to try again - if (!sampleUniform(rstate)) - continue; + // OMPL_DEBUG("attempting a sample at iter %u", iterations_); + if (!sampleUniform(rstate)){ + OMPL_WARN("FAILED to get a sample at iter %u", iterations_); + continue; + } + } // find closest state in the tree - Motion *nmotion = nn_->nearest(rmotion); + SimpleMotion *nmotion = nn_->nearest(rmotion); - if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate)) - continue; + if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate)) { + OMPL_DEBUG("found an equal state at iter %u. continuing.", iterations_); + continue; + } + base::State *dstate = rstate; @@ -284,7 +296,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer if (si_->checkMotion(nmotion->state, dstate)) { // create a motion - auto *motion = new Motion(si_); + auto *motion = new SimpleMotion(si_); si_->copyState(motion->state, dstate); motion->parent = nmotion; motion->incCost = opt_->motionCost(nmotion->state, motion->state); @@ -396,6 +408,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer { if (opt_->isCostBetterThan(solutionHeuristic(motion), bestCost_)) { + // OMPL_DEBUG("useNewStateRejection_: cost is better at iter %u! adding motion with cost %f", iterations_, motion->cost.value()); nn_->add(motion); motion->parent->children.push_back(motion); } @@ -411,6 +424,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer // add motion to the tree nn_->add(motion); motion->parent->children.push_back(motion); + // OMPL_DEBUG("cost is better at iter %u! adding motion with cost %f", iterations_, motion->cost.value()); } bool checkForSolution = false; @@ -424,6 +438,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer else nbhIncCost = opt_->motionCost(motion->state, nbh[i]->state); base::Cost nbhNewCost = opt_->combineCosts(motion->cost, nbhIncCost); + // OMPL_DEBUG("Cost: %f must be better than %f", nbhNewCost.value(), nbh[i]->cost.value()); if (opt_->isCostBetterThan(nbhNewCost, nbh[i]->cost)) { bool motionValid; @@ -453,47 +468,52 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer updateChildCosts(nbh[i]); checkForSolution = true; + // OMPL_DEBUG("We rewired on iter %u with cost %f", iterations_, nbhNewCost.value()); } } } } - // Add the new motion to the goalMotion_ list, if it satisfies the goal + // Add the new motion to the goalSimpleMotion_ list, if it satisfies the goal double distanceFromGoal; if (goal->isSatisfied(motion->state, &distanceFromGoal)) { motion->inGoal = true; - goalMotions_.push_back(motion); + goalSimpleMotions_.push_back(motion); checkForSolution = true; + OMPL_INFORM("We added a motion to our goalSimpleMotions_: %u on iter %u", goalSimpleMotions_.size(), iterations_); } // Checking for solution or iterative improvement if (checkForSolution) { bool updatedSolution = false; - if (!bestGoalMotion_ && !goalMotions_.empty()) + if (!bestGoalSimpleMotion_ && !goalSimpleMotions_.empty()) { // We have found our first solution, store it as the best. We only add one // vertex at a time, so there can only be one goal vertex at this moment. - bestGoalMotion_ = goalMotions_.front(); - bestCost_ = bestGoalMotion_->cost; + bestGoalSimpleMotion_ = goalSimpleMotions_.front(); + bestCost_ = bestGoalSimpleMotion_->cost; updatedSolution = true; - OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u " - "vertices in the graph)", - getName().c_str(), bestCost_.value(), iterations_, nn_->size()); + // OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u " + // "vertices in the graph)", + // getName().c_str(), bestCost_.value(), iterations_, nn_->size()); } else { + // OMPL_INFORM("%s: Found an ANOTHER solution with a cost of %.2f in %u iterations (%u " + // "vertices in the graph)", + // getName().c_str(), bestCost_.value(), iterations_, nn_->size()); // We already have a solution, iterate through the list of goal vertices // and see if there's any improvement. - for (auto &goalMotion : goalMotions_) + for (auto &goalSimpleMotion : goalSimpleMotions_) { // Is this goal motion better than the (current) best? - if (opt_->isCostBetterThan(goalMotion->cost, bestCost_)) + if (opt_->isCostBetterThan(goalSimpleMotion->cost, bestCost_)) { - bestGoalMotion_ = goalMotion; - bestCost_ = bestGoalMotion_->cost; + bestGoalSimpleMotion_ = goalSimpleMotion; + bestCost_ = bestGoalSimpleMotion_->cost; updatedSolution = true; // Check if it satisfies the optimization objective, if it does, break the for loop @@ -515,8 +535,8 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer if (intermediateSolutionCallback) { std::vector spath; - Motion *intermediate_solution = - bestGoalMotion_->parent; // Do not include goal state to simplify code. + SimpleMotion *intermediate_solution = + bestGoalSimpleMotion_->parent; // Do not include goal state to simplify code. // Push back until we find the start, but not the start itself while (intermediate_solution->parent != nullptr) @@ -531,29 +551,29 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer } // Checking for approximate solution (closest state found to the goal) - if (goalMotions_.size() == 0 && distanceFromGoal < approxDist) + if (goalSimpleMotions_.size() == 0 && distanceFromGoal < approxDist) { - approxGoalMotion = motion; + approxGoalSimpleMotion = motion; approxDist = distanceFromGoal; } } // terminate if a sufficient solution is found - if (bestGoalMotion_ && opt_->isSatisfied(bestCost_)) + if (bestGoalSimpleMotion_ && opt_->isSatisfied(bestCost_)) break; } // Add our solution (if it exists) - Motion *newSolution = nullptr; - if (bestGoalMotion_) + SimpleMotion *newSolution = nullptr; + if (bestGoalSimpleMotion_) { // We have an exact solution - newSolution = bestGoalMotion_; + newSolution = bestGoalSimpleMotion_; } - else if (approxGoalMotion) + else if (approxGoalSimpleMotion) { // We don't have a solution, but we do have an approximate solution - newSolution = approxGoalMotion; + newSolution = approxGoalSimpleMotion; } // No else, we have nothing @@ -562,12 +582,12 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer { ptc.terminate(); // construct the solution path - std::vector mpath; - Motion *iterMotion = newSolution; - while (iterMotion != nullptr) + std::vector mpath; + SimpleMotion *iterSimpleMotion = newSolution; + while (iterSimpleMotion != nullptr) { - mpath.push_back(iterMotion); - iterMotion = iterMotion->parent; + mpath.push_back(iterSimpleMotion); + iterSimpleMotion = iterSimpleMotion->parent; } // set the solution path @@ -580,7 +600,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer psol.setPlannerName(getName()); // If we don't have a goal motion, the solution is approximate - if (!bestGoalMotion_) + if (!bestGoalSimpleMotion_) psol.setApproximate(approxDist); // Does the solution satisfy the optimization objective? @@ -596,13 +616,13 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost " "%.3f", - getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value()); + getName().c_str(), statesGenerated, rewireTest, goalSimpleMotions_.size(), bestCost_.value()); - // We've added a solution if newSolution == true, and it is an approximate solution if bestGoalMotion_ == false - return {newSolution != nullptr, bestGoalMotion_ == nullptr}; + // We've added a solution if newSolution == true, and it is an approximate solution if bestGoalSimpleMotion_ == false + return {newSolution != nullptr, bestGoalSimpleMotion_ == nullptr}; } -void ompl::geometric::RRTstar::getNeighbors(Motion *motion, std::vector &nbh) const +void ompl::geometric::RRTstar::getNeighbors(SimpleMotion *motion, std::vector &nbh) const { auto cardDbl = static_cast(nn_->size() + 1u); if (useKNearest_) @@ -619,7 +639,7 @@ void ompl::geometric::RRTstar::getNeighbors(Motion *motion, std::vectorparent->children.begin(); it != m->parent->children.end(); ++it) { @@ -631,7 +651,7 @@ void ompl::geometric::RRTstar::removeFromParent(Motion *m) } } -void ompl::geometric::RRTstar::updateChildCosts(Motion *m) +void ompl::geometric::RRTstar::updateChildCosts(SimpleMotion *m) { for (std::size_t i = 0; i < m->children.size(); ++i) { @@ -644,7 +664,7 @@ void ompl::geometric::RRTstar::freeMemory() { if (nn_) { - std::vector motions; + std::vector motions; nn_->list(motions); for (auto &motion : motions) { @@ -659,12 +679,12 @@ void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const { Planner::getPlannerData(data); - std::vector motions; + std::vector motions; if (nn_) nn_->list(motions); - if (bestGoalMotion_) - data.addGoalVertex(base::PlannerDataVertex(bestGoalMotion_->state)); + if (bestGoalSimpleMotion_) + data.addGoalVertex(base::PlannerDataVertex(bestGoalSimpleMotion_->state)); for (auto &motion : motions) { @@ -699,34 +719,34 @@ int ompl::geometric::RRTstar::pruneTree(const base::Cost &pruneTreeCost) // motion is found that is kept. // To avoid making an intermediate copy of the NN structure, we process the tree by descending down from the // start(s). - // In the first pass, all Motions with a cost below pruneTreeCost, or Motion's with children with costs below + // In the first pass, all SimpleMotions with a cost below pruneTreeCost, or SimpleMotion's with children with costs below // pruneTreeCost are added to the replacement NN structure, - // while all other Motions are stored as either a 'leaf' or 'chain' Motion. After all the leaves are + // while all other SimpleMotions are stored as either a 'leaf' or 'chain' SimpleMotion. After all the leaves are // disconnected and deleted, we check - // if any of the the chain Motions are now leaves, and repeat that process until done. + // if any of the the chain SimpleMotions are now leaves, and repeat that process until done. // This avoids (1) copying the NN structure into an intermediate variable and (2) the use of the expensive // NN::remove() method. // Variable - // The queue of Motions to process: - std::queue> motionQueue; + // The queue of SimpleMotions to process: + std::queue> motionQueue; // The list of leaves to prune - std::queue> leavesToPrune; + std::queue> leavesToPrune; // The list of chain vertices to recheck after pruning - std::list chainsToRecheck; + std::list chainsToRecheck; // Clear the NN structure: nn_->clear(); // Put all the starts into the NN structure and their children into the queue: // We do this so that start states are never pruned. - for (auto &startMotion : startMotions_) + for (auto &startSimpleMotion : startSimpleMotions_) { // Add to the NN - nn_->add(startMotion); + nn_->add(startSimpleMotion); // Add their children to the queue: - addChildrenToList(&motionQueue, startMotion); + addChildrenToList(&motionQueue, startSimpleMotion); } while (motionQueue.empty() == false) @@ -772,7 +792,7 @@ int ompl::geometric::RRTstar::pruneTree(const base::Cost &pruneTreeCost) else { // No, we aren't. This doesn't mean we won't though - // Move this Motion to the temporary list + // Move this SimpleMotion to the temporary list chainsToRecheck.push_back(motionQueue.front()); } @@ -790,7 +810,7 @@ int ompl::geometric::RRTstar::pruneTree(const base::Cost &pruneTreeCost) motionQueue.pop(); } - // We now have a list of Motions to definitely remove, and a list of Motions to recheck + // We now have a list of SimpleMotions to definitely remove, and a list of SimpleMotions to recheck // Iteratively check the two lists until there is nothing to to remove while (leavesToPrune.empty() == false) { @@ -801,13 +821,13 @@ int ompl::geometric::RRTstar::pruneTree(const base::Cost &pruneTreeCost) if (leavesToPrune.front()->inGoal == true) { // Warn if pruning the _best_ goal - if (leavesToPrune.front() == bestGoalMotion_) + if (leavesToPrune.front() == bestGoalSimpleMotion_) { OMPL_ERROR("%s: Pruning the best goal.", getName().c_str()); } // Remove it - goalMotions_.erase(std::remove(goalMotions_.begin(), goalMotions_.end(), leavesToPrune.front()), - goalMotions_.end()); + goalSimpleMotions_.erase(std::remove(goalSimpleMotions_.begin(), goalSimpleMotions_.end(), leavesToPrune.front()), + goalSimpleMotions_.end()); } // Remove the leaf from its parent @@ -831,7 +851,7 @@ int ompl::geometric::RRTstar::pruneTree(const base::Cost &pruneTreeCost) auto mIter = chainsToRecheck.begin(); while (mIter != chainsToRecheck.end()) { - // Is the Motion a leaf? + // Is the SimpleMotion a leaf? if ((*mIter)->children.empty() == true) { // It is, add to the removal queue @@ -874,7 +894,7 @@ int ompl::geometric::RRTstar::pruneTree(const base::Cost &pruneTreeCost) return numPruned; } -void ompl::geometric::RRTstar::addChildrenToList(std::queue> *motionList, Motion *motion) +void ompl::geometric::RRTstar::addChildrenToList(std::queue> *motionList, SimpleMotion *motion) { for (auto &child : motion->children) { @@ -882,20 +902,20 @@ void ompl::geometric::RRTstar::addChildrenToList(std::queueisCostBetterThan(threshold, solutionHeuristic(motion)); } -ompl::base::Cost ompl::geometric::RRTstar::solutionHeuristic(const Motion *motion) const +ompl::base::Cost ompl::geometric::RRTstar::solutionHeuristic(const SimpleMotion *motion) const { base::Cost costToCome; if (useAdmissibleCostToCome_) @@ -904,10 +924,10 @@ ompl::base::Cost ompl::geometric::RRTstar::solutionHeuristic(const Motion *motio costToCome = opt_->infiniteCost(); // Find the min from each start - for (auto &startMotion : startMotions_) + for (auto &startSimpleMotion : startSimpleMotions_) { costToCome = opt_->betterCost( - costToCome, opt_->motionCost(startMotion->state, + costToCome, opt_->motionCost(startSimpleMotion->state, motion->state)); // lower-bounding cost from the start to the state } } @@ -1115,6 +1135,7 @@ void ompl::geometric::RRTstar::allocSampler() { // We are using a regular sampler sampler_ = si_->allocStateSampler(); + // sampler_ = si_->allocValidStateSampler(); } // Wrap into a sorted sampler @@ -1138,11 +1159,11 @@ bool ompl::geometric::RRTstar::sampleUniform(base::State *statePtr) } else { - // Simply return a state from the regular sampler + // // Simply return a state from the regular sampler sampler_->sampleUniform(statePtr); - // Always true return true; + // return sampler_->sample(statePtr); } } diff --git a/src/ompl/tools/thunder/Thunder.h b/src/ompl/tools/thunder/Thunder.h index 2f1357d372..9e9c02409b 100644 --- a/src/ompl/tools/thunder/Thunder.h +++ b/src/ompl/tools/thunder/Thunder.h @@ -65,7 +65,8 @@ namespace ompl enum thunderPlanner { PLANNER_CFOREST, - PLANNER_RRTCONNECT + PLANNER_RRTCONNECT, + PLANNER_INFORMEDRRTSTAR, }; namespace tools { @@ -111,6 +112,7 @@ namespace ompl double SparseD_{}; size_t n_parallel_plans_{0}; size_t cforest_n_threads_{0}; + std::shared_ptr seed_soln_path_ {}; public: /** \brief Display debug data about potential available solutions */ @@ -190,6 +192,14 @@ namespace ompl planner_type_ = thunderPlanner::PLANNER_RRTCONNECT; } + void setInformedRRTstar() { + planner_type_ = thunderPlanner::PLANNER_INFORMEDRRTSTAR; + } + + void addPotentialSolutionPath(std::shared_ptr seed_soln_path) { + seed_soln_path_ = seed_soln_path; + } + /** \brief Set the number of threads to use for planning. */ void setNumParallelPlans(const size_t n_parallel_plans) { n_parallel_plans_ = n_parallel_plans; diff --git a/src/ompl/tools/thunder/src/Thunder.cpp b/src/ompl/tools/thunder/src/Thunder.cpp index 6aabb1e671..231503f94d 100644 --- a/src/ompl/tools/thunder/src/Thunder.cpp +++ b/src/ompl/tools/thunder/src/Thunder.cpp @@ -98,6 +98,7 @@ void ompl::tools::Thunder::setup() planner_vec_ = planner_vec; } // set up planner ptr vector based on planner type + size_t k {}; for (auto &planner : planner_vec_) { if (!planner) @@ -112,15 +113,27 @@ void ompl::tools::Thunder::setup() auto cforest_planner {std::make_shared(si_)}; cforest_planner->setNumThreads(cforest_n_threads_); planner = cforest_planner; - } - else - { + } else if (planner_type_ == thunderPlanner::PLANNER_INFORMEDRRTSTAR) { + auto rrt_planner {std::make_shared(si_)}; + if(seed_soln_path_) { + rrt_planner->setProblemDefinition(pdef_); + std::cerr << "set pdef for planner k=" << k << std::endl; + rrt_planner->setup(); + std::cerr << "ran setup for planner k=" << k << std::endl; + rrt_planner->addPotentialSolutionPath(*seed_soln_path_); + } + planner = rrt_planner; + } else { planner = std::make_shared(si_); } - planner->setProblemDefinition(pdef_); - if (!planner->isSetup()) - planner->setup(); + + if (!planner->isSetup()) { + planner->setProblemDefinition(pdef_); + planner->setup(); + } + } + k++; } // Setup planning from experience planner