37 #include "ompl/control/planners/syclop/SyclopRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
44 sampler_ =
si_->allocStateSampler();
50 if (!nn_ && !regionalNN_)
52 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(
si_->getStateSpace()));
63 lastGoalMotion_ = NULL;
68 Planner::getPlannerData(data);
69 std::vector<Motion*> motions;
72 double delta = siC_->getPropagationStepSize();
77 for (
size_t i = 0; i < motions.size(); ++i)
79 if (motions[i]->parent)
97 si_->copyState(motion->
state, s);
98 siC_->nullControl(motion->
control);
109 std::vector<double> coord(decomp_->getDimension());
110 decomp_->sampleFromRegion(region.
index, rng_, coord);
111 decomp_->sampleFullState(sampler, coord, rmotion->
state);
118 std::vector<unsigned int> searchRegions;
119 decomp_->getNeighbors(region.
index, searchRegions);
120 searchRegions.push_back(region.
index);
122 std::vector<Motion*> motions;
123 for (std::vector<unsigned int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
125 const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
126 motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
129 std::vector<Motion*>::const_iterator i = motions.begin();
131 double minDistance = distanceFunction(rmotion, nmotion);
133 while (i != motions.end())
136 const double dist = distanceFunction(rmotion, m);
137 if (dist < minDistance)
148 nmotion = nn_->nearest(rmotion);
151 unsigned int duration = controlSampler_->sampleTo(rmotion->
control, nmotion->
control, nmotion->
state, rmotion->
state);
152 if (duration >= siC_->getMinControlDuration())
154 rmotion->
steps = duration;
155 rmotion->
parent = nmotion;
156 newMotions.push_back(rmotion);
159 lastGoalMotion_ = rmotion;
163 si_->freeState(rmotion->
state);
164 siC_->freeControl(rmotion->
control);
173 std::vector<Motion*> motions;
175 for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
179 si_->freeState(m->
state);
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
base::State * state
The state contained by the motion.
int index
The index of the graph node corresponding to this region.
A boost shared pointer wrapper for ompl::base::StateSampler.
Representation of a region in the Decomposition assigned to Syclop.
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...
virtual Syclop::Motion * addRoot(const base::State *s)
Add State s as a new root in the low-level tree, and return the Motion corresponding to s...
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Representation of a motion.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Control * control
The control contained by the motion.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void freeMemory(void)
Free the memory allocated by this planner.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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...
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 void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
virtual void selectAndExtend(Region ®ion, std::vector< Motion * > &newMotions)
Select a Motion from the given Region, and extend the tree from the Motion. Add any new motions creat...
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual bool hasControls(void) const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
const Motion * parent
The parent motion in the tree.
SpaceInformationPtr si_
The space information for which planning is done.
unsigned int steps
The number of steps for which the control is applied.