37 #include "ompl/control/planners/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
55 ompl::control::RRT::~RRT(
void)
64 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
72 controlSampler_.reset();
76 lastGoalMotion_ = NULL;
83 std::vector<Motion*> motions;
85 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
87 if (motions[i]->state)
88 si_->freeState(motions[i]->state);
89 if (motions[i]->control)
90 siC_->freeControl(motions[i]->control);
105 si_->copyState(motion->
state, st);
106 siC_->nullControl(motion->
control);
110 if (nn_->size() == 0)
112 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
117 sampler_ = si_->allocStateSampler();
118 if (!controlSampler_)
119 controlSampler_ = siC_->allocDirectedControlSampler();
121 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), nn_->size());
125 double approxdif = std::numeric_limits<double>::infinity();
135 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
138 sampler_->sampleUniform(rstate);
141 Motion *nmotion = nn_->nearest(rmotion);
144 unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->
control, nmotion->
state, rmotion->
state);
146 if (addIntermediateStates_)
149 std::vector<base::State *> pstates;
150 cd = siC_->propagateWhileValid(nmotion->
state, rctrl, cd, pstates,
true);
152 if (cd >= siC_->getMinControlDuration())
154 Motion *lastmotion = nmotion;
157 for ( ; p < pstates.size(); ++p)
161 motion->
state = pstates[p];
163 motion->
control = siC_->allocControl();
164 siC_->copyControl(motion->
control, rctrl);
166 motion->
parent = lastmotion;
177 if (dist < approxdif)
185 while (++p < pstates.size())
186 si_->freeState(pstates[p]);
191 for (
size_t p = 0 ; p < pstates.size(); ++p)
192 si_->freeState(pstates[p]);
196 if (cd >= siC_->getMinControlDuration())
200 si_->copyState(motion->
state, rmotion->
state);
201 siC_->copyControl(motion->
control, rctrl);
214 if (dist < approxdif)
224 bool approximate =
false;
225 if (solution == NULL)
227 solution = approxsol;
231 if (solution != NULL)
233 lastGoalMotion_ = solution;
236 std::vector<Motion*> mpath;
237 while (solution != NULL)
239 mpath.push_back(solution);
240 solution = solution->parent;
245 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
246 if (mpath[i]->parent)
247 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
249 path->
append(mpath[i]->state);
251 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
255 si_->freeState(rmotion->
state);
257 siC_->freeControl(rmotion->
control);
259 si_->freeState(xstate);
261 OMPL_INFORM(
"%s: Created %u states", getName().c_str(), nn_->size());
268 Planner::getPlannerData(data);
270 std::vector<Motion*> motions;
274 double delta = siC_->getPropagationStepSize();
279 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
281 const Motion* m = motions[i];
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
Definition of an abstract control.
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...
Abstract definition of goals.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continue solving for some amount of time. Return true if solution was found.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::State * state
The state contained by the motion.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
RRT(const SpaceInformationPtr &si)
Constructor.
Definition of a control path.
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself...
double getGoalBias(void) const
Get the goal bias the planner is using.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
Motion * parent
The parent motion in the exploration tree.
Abstract definition of a goal region that can be sampled.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Representation of a motion.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Control * control
The control contained by the motion.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
A class to store the exit status of Planner::solve()
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...
void freeMemory(void)
Free the memory allocated by this planner.
bool getIntermediateStates(void) const
Return true if the intermediate states generated along motions are to be added to the tree itself...
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 of an abstract state.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
virtual void clear(void)
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
void setGoalBias(double goalBias)
virtual bool hasControls(void) const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
unsigned int steps
The number of steps the control is applied for.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.