00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/control/planners/syclop/SyclopEST.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039
00040 void ompl::control::SyclopEST::setup(void)
00041 {
00042 Syclop::setup();
00043 sampler_ = si_->allocStateSampler();
00044 controlSampler_ = siC_->allocControlSampler();
00045 lastGoalMotion_ = NULL;
00046 }
00047
00048 void ompl::control::SyclopEST::clear(void)
00049 {
00050 Syclop::clear();
00051 freeMemory();
00052 motions_.clear();
00053 lastGoalMotion_ = NULL;
00054 }
00055
00056 void ompl::control::SyclopEST::getPlannerData(base::PlannerData& data) const
00057 {
00058 Planner::getPlannerData(data);
00059
00060 double delta = siC_->getPropagationStepSize();
00061
00062 if (lastGoalMotion_)
00063 data.addGoalVertex(lastGoalMotion_->state);
00064
00065 for (size_t i = 0; i < motions_.size(); ++i)
00066 {
00067 if (motions_[i]->parent)
00068 {
00069 if (data.hasControls())
00070 data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
00071 base::PlannerDataVertex(motions_[i]->state),
00072 control::PlannerDataEdgeControl (motions_[i]->control, motions_[i]->steps * delta));
00073 else
00074 data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
00075 base::PlannerDataVertex(motions_[i]->state));
00076 }
00077 else
00078 data.addStartVertex (base::PlannerDataVertex(motions_[i]->state));
00079 }
00080 }
00081
00082 ompl::control::Syclop::Motion* ompl::control::SyclopEST::addRoot(const base::State* s)
00083 {
00084 Motion* motion = new Motion(siC_);
00085 si_->copyState(motion->state, s);
00086 siC_->nullControl(motion->control);
00087 motions_.push_back(motion);
00088 return motion;
00089 }
00090
00091 void ompl::control::SyclopEST::selectAndExtend(Region& region, std::vector<Motion*>& newMotions)
00092 {
00093 Motion* treeMotion = region.motions[rng_.uniformInt(0, region.motions.size()-1)];
00094 Control* rctrl = siC_->allocControl();
00095 base::State* newState = si_->allocState();
00096
00097 controlSampler_->sample(rctrl, treeMotion->state);
00098 unsigned int duration = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
00099 duration = siC_->propagateWhileValid(treeMotion->state, rctrl, duration, newState);
00100
00101 if (duration >= siC_->getMinControlDuration())
00102 {
00103 Motion* motion = new Motion(siC_);
00104 si_->copyState(motion->state, newState);
00105 siC_->copyControl(motion->control, rctrl);
00106 motion->steps = duration;
00107 motion->parent = treeMotion;
00108 motions_.push_back(motion);
00109 newMotions.push_back(motion);
00110
00111 lastGoalMotion_ = motion;
00112 }
00113
00114 siC_->freeControl(rctrl);
00115 si_->freeState(newState);
00116 }
00117
00118 void ompl::control::SyclopEST::freeMemory(void)
00119 {
00120 for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
00121 {
00122 Motion* m = *i;
00123 if (m->state)
00124 si_->freeState(m->state);
00125 if (m->control)
00126 siC_->freeControl(m->control);
00127 delete m;
00128 }
00129 }