BiTRRT.cpp
45 ompl::geometric::BiTRRT::BiTRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "BiTRRT")
50 Planner::declareParam<double>("range", this, &BiTRRT::setRange, &BiTRRT::getRange, "0.:1.:10000.");
57 Planner::declareParam<double>("init_temperature", this, &BiTRRT::setInitTemperature, &BiTRRT::getInitTemperature);
62 Planner::declareParam<double>("cost_threshold", this, &BiTRRT::setCostThreshold, &BiTRRT::getCostThreshold);
144 OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.",
166 ompl::geometric::BiTRRT::Motion *ompl::geometric::BiTRRT::addMotion(const base::State *state, TreeData &tree,
175 if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
177 if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
228 ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion *nearest, TreeData &tree,
241 si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
243 si_->getStateSpace()->interpolate(toMotion->state, nearest->state, 1.0 - maxDistance_ / d, toMotion->state);
269 ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion *toMotion, TreeData &tree,
339 ompl::base::PlannerStatus ompl::geometric::BiTRRT::solve(const base::PlannerTerminationCondition &ptc)
467 OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
485 data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
499 data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
505 data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
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: BiTRRT.cpp:339
Motion * addMotion(const base::State *state, TreeData &tree, Motion *parent=nullptr)
Add a state to the given tree. The motion created is returned.
Definition: BiTRRT.cpp:166
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BiTRRT.cpp:115
bool minExpansionControl(double dist)
Use frontier node ratio to filter nodes that do not add new information to the search tree...
Definition: BiTRRT.cpp:212
The planner failed to find a solution.
Definition: PlannerStatus.h:62
bool transitionTest(const base::Cost &motionCost)
Transition test that filters transitions based on the motion cost. If the motion cost is near or belo...
Definition: BiTRRT.cpp:185
GrowResult extendTree(Motion *toMotion, TreeData &tree, Motion *&result)
Extend tree toward the state in rmotion. Store the result of the extension, if any, in result.
Definition: BiTRRT.cpp:269
A shared pointer wrapper for ompl::base::StateSampler.
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
double getFrontierThreshold() const
Get the distance between a new state and the nearest neighbor that qualifies a state as being a front...
Definition: BiTRRT.h:149
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:63
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
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
The nearest-neighbors data structure that contains the entire the tree of motions generated during pl...
Definition: BiTRRT.h:215
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
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: BiTRRT.cpp:472
void setFrontierNodeRatio(double frontierNodeRatio)
Set the ratio between adding non-frontier nodes to frontier nodes. For example: .1 is one non-frontie...
Definition: BiTRRT.h:157
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:47
double getInitTemperature() const
Get the initial temperature at the start of planning.
Definition: BiTRRT.h:135
double getFrontierNodeRatio() const
Get the ratio between adding non-frontier nodes to frontier nodes.
Definition: BiTRRT.h:164
The planner found an exact solution.
Definition: PlannerStatus.h:66
const base::State * root
Pointer to the root of the tree this motion is contained in.
Definition: BiTRRT.h:207
void setTempChangeFactor(double factor)
Set the factor by which the temperature is increased after a failed transition test. This value should be in the range (0, 1], typically close to zero (default is 0.1). This value is an exponential (e^factor) that is multiplied with the current temperature.
Definition: BiTRRT.h:99
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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
void setInitTemperature(double initTemperature)
Set the initial temperature at the start of planning. Should be high to allow for initial exploration...
Definition: BiTRRT.h:129
A shared pointer wrapper for ompl::base::SpaceInformation.
double getTempChangeFactor() const
Get the factor by which the temperature is increased after a failed transition.
Definition: BiTRRT.h:106
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
*double getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:122
void setFrontierThreshold(double frontierThreshold)
Set the distance between a new state and the nearest neighbor that qualifies a state as being a front...
Definition: BiTRRT.h:142
Representation of a motion in the search tree.
Definition: BiTRRT.h:183
static const double COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION
For cost-based planners it has been observed that smaller ranges are typically suitable. The same range computation strategy is used for all planners, but for cost planners an additional factor (smaller than 1) is multiplied in.
Definition: MagicConstants.h:79
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setRange(double distance)
Set the maximum possible length of any one motion in the search tree. Very short/long motions may inh...
Definition: BiTRRT.h:83
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
void setCostThreshold(double maxCost)
Set the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:114
bool connectTrees(Motion *nmotion, TreeData &tree, Motion *xmotion)
Attempt to connect tree to nmotion, which is in the other tree. xmotion is scratch space and will be ...
Definition: BiTRRT.cpp:277
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BiTRRT.cpp:97