Loading...
Searching...
No Matches
SST.cpp
52 Planner::declareParam<double>("goal_bias", this, &SST::setGoalBias, &SST::getGoalBias, "0.:.05:1.");
53 Planner::declareParam<double>("selection_radius", this, &SST::setSelectionRadius, &SST::getSelectionRadius, "0.:.1:"
55 Planner::declareParam<double>("pruning_radius", this, &SST::setPruningRadius, &SST::getPruningRadius, "0.:.1:100");
57 addPlannerProgressProperty("best cost REAL", [this] { return std::to_string(this->prevSolutionCost_.value()); });
224ompl::base::PlannerStatus ompl::geometric::SST::solve(const base::PlannerTerminationCondition &ptc)
248 const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
250 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
299 if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->rep_->accCost_))
411 OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(), iterations);
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 bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Objective for attempting to maximize the minimum clearance along a path.
Definition MaximizeMinClearanceObjective.h:48
The cost of a path is defined as the worst state cost over the entire path. This objective attempts t...
Definition MinimaxObjective.h:50
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 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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition Planner.h:403
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition Planner.cpp:92
Definition SST.h:207
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
Definition SST.cpp:183
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SST.cpp:65
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Definition SST.cpp:224
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
Definition SST.cpp:154
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition SST.cpp:108
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition SST.h:254
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 SST.cpp:416
std::shared_ptr< NearestNeighbors< Motion * > > witnesses_
A nearest-neighbors datastructure containing the tree of witness motions.
Definition SST.h:257
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
Definition SST.h:121
double selectionRadius_
The radius for determining the node selected for extension.
Definition SST.h:267
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition SST.h:261
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition SST.h:245
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition ConstrainedSpaceInformation.h:55
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
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49