Loading...
Searching...
No Matches
STRRTstar.cpp
41 : Planner(si, "SpaceTimeRRT"), sampler_(&(*si), startMotion_, goalMotions_, newBatchGoalMotions_, sampleOldBatch_)
43 if (std::dynamic_pointer_cast<ompl::base::SpaceTimeStateSpace>(si->getStateSpace()) == nullptr) {
49 Planner::declareParam<double>("range", this, &STRRTstar::setRange, &STRRTstar::getRange, "0.:1.:10000.");
68 tStart_->setDistanceFunction([this](const base::Motion *a, const base::Motion *b) { return distanceFunction(a, b); });
69 tGoal_->setDistanceFunction([this](const base::Motion *a, const base::Motion *b) { return distanceFunction(a, b); });
71 if (si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->getTimeComponent()->isBounded())
74 si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->getTimeComponent()->getMaxTimeBound();
119ompl::base::PlannerStatus ompl::geometric::STRRTstar::solve(const ompl::base::PlannerTerminationCondition &ptc)
141 OMPL_ERROR("%s: base::Motion planning start tree could not be initialized!", getName().c_str());
172 int numBatchSamples = static_cast<int>(tStart_->size() + tGoal_->size()); // number of samples in the current batch
190 OMPL_INFORM("%s: Starting planning with time bound factor %.2f", getName().c_str(), newBatchTimeBoundFactor);
213 goalMotions_.insert(goalMotions_.end(), newBatchGoalMotions_.begin(), newBatchGoalMotions_.end());
221 (firstBatch || isTimeBounded_ || !sampleUniformForUnboundedTime_ || rng_.uniform01() <= oldBatchSampleProb);
234 goalState = nextGoal(static_cast<int>(batchSize), oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
255 goalState = nextGoal(static_cast<int>(batchSize), oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
264 else if (forceGoalSample || newBatchGoalMotions_.size() < (unsigned long)(newBatchGoalSamples / goalStateSampleRatio_))
286 std::min(minimumTime_, si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(
357 // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
368 const double newDist = tree->getDistanceFunction()(addedMotion, otherTree->nearest(addedMotion));
382 // continue to look for solutions with the narrower time bound until the termination condition is met
407 OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
432 ompl::base::OptimizationObjectivePtr optimizationObjective = std::make_shared<ompl::base::MinimizeArrivalTime>(si_);
444 // If connect, advance from single nearest neighbor until the random state is reached or trapped
478 a->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
480 b->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
487 auto rt = rmotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
491 nmotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
522 si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
523 /* Check if we have moved at all. Due to some stranger state spaces (e.g., the constrained state spaces),
549void ompl::geometric::STRRTstar::increaseTimeBound(bool hasSameBounds, double &oldBatchTimeBoundFactor,
553 oldBatchTimeBoundFactor = hasSameBounds ? newBatchTimeBoundFactor * timeBoundFactorIncrease_ : newBatchTimeBoundFactor;
560 OMPL_INFORM("%s: Increased time bound factor to %.2f", getName().c_str(), newBatchTimeBoundFactor);
565 const ompl::base::ReportIntermediateSolutionFn &intermediateSolutionCallback, const ompl::base::PlannerTerminationCondition& ptc)
579 goalMotion->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
587 goalMotions_.insert(goalMotions_.end(), newBatchGoalMotions_.begin(), newBatchGoalMotions_.end());
625 bestTime_ = reachedGaol->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
663 double deltaT = si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(
725 return a->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position <
726 b->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
730 double t = m->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
850void ompl::geometric::STRRTstar::removeInvalidGoals(const std::vector<base::Motion *> &invalidGoals)
928 data.addEdge(data.vertexIndex(motion->connectionPoint->state), data.vertexIndex(motion->state));
997 addedMotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
999 addedMotion->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
1004 otherMotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
1006 otherMotion->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
1007 // rewire, if goal time is improved and the otherMotion node can be connected to the added node
1008 if (otherNodeT < nodeT && goalT < otherGoalT && si_->checkMotion(otherMotion->state, addedMotion->state))
1059bool ompl::geometric::STRRTstar::sampleGoalTime(ompl::base::State *goal, double oldBatchTimeBoundFactor,
1064 si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(startMotion_->state, goal);
1085 goal->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position = time;
1092 static ompl::base::PlannerTerminationCondition ptc = ompl::base::plannerNonTerminatingCondition();
1102ompl::base::State *ompl::geometric::STRRTstar::nextGoal(const ompl::base::PlannerTerminationCondition &ptc, int n,
1107 const ompl::base::GoalSampleableRegion *goal = pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION) ?
1153 OMPL_ERROR("%s: The optimum approximation factor needs to be between 0 and 1.", getName().c_str());
1220 OMPL_ERROR("%s: Initial Time Bound Factor Increase needs to be higher than 1.", getName().c_str());
A nearest neighbors datastructure that uses linear search.
Definition NearestNeighborsLinear.h:57
virtual void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const =0
Get the nearest neighbors of a point, within a specified radius.
const DistanceFunction & getDistanceFunction() const
Get the distance function used.
Definition NearestNeighbors.h:64
virtual std::size_t size() const =0
Get the number of elements in the datastructure.
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const =0
Get the k-nearest neighbors of a point.
virtual _T nearest(const _T &data) const =0
Get the nearest neighbor of a point.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition State.h:95
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of a goal region that can be sampled.
Definition GoalSampleableRegion.h:48
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
Definition Goal.h:102
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition ConditionalStateSampler.h:63
A shared pointer wrapper for ompl::base::OptimizationObjective.
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,...
Definition PlannerData.h:175
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
Definition PlannerData.cpp:315
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...
Definition PlannerData.cpp:413
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...
Definition PlannerData.cpp:422
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...
Definition PlannerData.cpp:432
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition PlannerData.h:410
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
bool eval() const
The implementation of some termination condition. By default, this just calls fn_()
Definition PlannerTerminationCondition.cpp:177
A shared pointer wrapper for ompl::base::SpaceInformation.
A state space consisting of a space and a time component.
Definition SpaceTimeStateSpace.h:51
double timeToCoverDistance(const ompl::base::State *state1, const ompl::base::State *state2) const
The time to get from state1 to state2 with respect to vMax.
Definition SpaceTimeStateSpace.cpp:101
static void addDescendants(base::Motion *m, const TreeData &tree)
Adds given all descendants of the given motion to given tree and checks whether one of the added moti...
Definition STRRTstar.cpp:954
void setOptimumApproxFactor(double optimumApproxFactor)
Set the Optimum Approximation factor. This allows the planner to converge more quickly,...
Definition STRRTstar.cpp:1149
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, std::vector< base::Motion * > &nbh, bool connect)
Attempt to grow a tree towards a random state for the neighborhood of the random state....
Definition STRRTstar.cpp:440
static void removeFromParent(base::Motion *m)
Removes the given motion from the parent's child list.
Definition STRRTstar.cpp:935
GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion)
Grow a tree towards a random state for a single nearest state.
Definition STRRTstar.cpp:509
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition STRRTstar.cpp:119
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition STRRTstar.cpp:1045
void setSampleUniformForUnboundedTime(bool uniform)
Whether the state space is sampled uniformly or centered at lower time values.
Definition STRRTstar.cpp:1225
double getOptimumApproxFactor() const
The Optimum Approximation factor (0 - 1).
Definition STRRTstar.cpp:1144
void setTimeBoundFactorIncrease(double f)
The value by which the time bound factor is multiplied in each increase step.
Definition STRRTstar.cpp:1207
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition STRRTstar.h:205
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition STRRTstar.cpp:869
base::Motion * pruneGoalTree()
Prune the goal tree after a solution was found. Return the goal motion, that is connected to the star...
Definition STRRTstar.cpp:710
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 STRRTstar.cpp:893
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition STRRTstar.cpp:58
bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
Samples the time component of a goal state dependant on its space component. Returns false,...
Definition STRRTstar.cpp:1059
void getNeighbors(TreeData &tree, base::Motion *motion, std::vector< base::Motion * > &nbh) const
Gets the neighbours of a given motion, using either k-nearest or radius_ as appropriate.
Definition STRRTstar.cpp:970
base::State * nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
N tries to sample a goal.
Definition STRRTstar.cpp:1089
void removeInvalidGoals(const std::vector< base::Motion * > &invalidGoals)
Remove invalid goal states from the goal set.
Definition STRRTstar.cpp:850
void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)
Definition STRRTstar.cpp:549
static base::Motion * computeSolutionMotion(base::Motion *motion)
Find the solution (connecting) motion for a motion that is indirectly connected.
Definition STRRTstar.cpp:829
std::shared_ptr< ompl::NearestNeighbors< base::Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition STRRTstar.h:130
void setRange(double distance)
Set the range the planner is supposed to use.
Definition STRRTstar.cpp:1134
unsigned int getBatchSize() const
The number of samples before the time bound is increased.
Definition STRRTstar.cpp:1193
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition SelfConfig.cpp:225
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
Definition ProblemDefinition.h:144
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
Definition PlannerTerminationCondition.cpp:182
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition GeometricEquations.cpp:55
Representation of a solution to a planning problem.
Definition ProblemDefinition.h:70
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition ProblemDefinition.h:105
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
Definition ProblemDefinition.h:97
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition Planner.h:199
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition Planner.h:211
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition PlannerStatus.h:60
Information attached to growing a tree of motions (used internally)
Definition STRRTstar.h:134