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)
69 if (data.hasControls())
84 Motion* motion =
new Motion(siC_);
85 si_->copyState(motion->state, s);
86 siC_->nullControl(motion->control);
87 motions_.push_back(motion);
91 void ompl::control::SyclopEST::selectAndExtend(Region& region, std::vector<Motion*>& newMotions)
93 Motion* treeMotion = region.motions[rng_.uniformInt(0, region.motions.size()-1)];
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())
103 Motion* motion =
new Motion(siC_);
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);
126 siC_->freeControl(m->control);