37 #include "ompl/control/planners/syclop/SyclopEST.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
43 sampler_ =
si_->allocStateSampler();
53 lastGoalMotion_ = NULL;
58 Planner::getPlannerData(data);
60 double delta = siC_->getPropagationStepSize();
65 for (
size_t i = 0; i < motions_.size(); ++i)
67 if (motions_[i]->parent)
85 si_->copyState(motion->
state, s);
86 siC_->nullControl(motion->
control);
87 motions_.push_back(motion);
94 Control* rctrl = siC_->allocControl();
97 controlSampler_->sample(rctrl, treeMotion->
state);
98 unsigned int duration = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
99 duration = siC_->propagateWhileValid(treeMotion->
state, rctrl, duration, newState);
101 if (duration >= siC_->getMinControlDuration())
104 si_->copyState(motion->
state, newState);
105 siC_->copyControl(motion->
control, rctrl);
106 motion->
steps = duration;
107 motion->
parent = treeMotion;
108 motions_.push_back(motion);
109 newMotions.push_back(motion);
111 lastGoalMotion_ = motion;
114 siC_->freeControl(rctrl);
115 si_->freeState(newState);
120 for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
124 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.
Definition of an abstract control.
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...
std::vector< Motion * > motions
The tree motions contained in this region.
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
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.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
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 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 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.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
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...
unsigned int steps
The number of steps for which the control is applied.