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/GoalSampleableRegion.h"
00039
00040 void ompl::control::SyclopEST::setup(void)
00041 {
00042 Syclop::setup();
00043 sampler_ = si_->allocStateSampler();
00044 controlSampler_ = siC_->allocControlSampler();
00045 }
00046
00047 void ompl::control::SyclopEST::clear(void)
00048 {
00049 Syclop::clear();
00050 freeMemory();
00051 motions_.clear();
00052 }
00053
00054 void ompl::control::SyclopEST::getPlannerData(base::PlannerData& data) const
00055 {
00056 Planner::getPlannerData(data);
00057 if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00058 {
00059 const double delta = siC_->getPropagationStepSize();
00060
00061 for (std::vector<Motion*>::const_iterator i = motions_.begin(); i != motions_.end(); ++i)
00062 {
00063 const Motion* m = *i;
00064 if (m->parent)
00065 cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00066 else
00067 cpd->recordEdge(NULL, m->state, NULL, 0.);
00068 }
00069 }
00070 else
00071 {
00072 for (std::vector<Motion*>::const_iterator i = motions_.begin(); i != motions_.end(); ++i)
00073 {
00074 const Motion* m = *i;
00075 data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00076 }
00077 }
00078 }
00079
00080 ompl::control::Syclop::Motion* ompl::control::SyclopEST::addRoot(const base::State* s)
00081 {
00082 Motion* motion = new Motion(siC_);
00083 si_->copyState(motion->state, s);
00084 siC_->nullControl(motion->control);
00085 motions_.push_back(motion);
00086 return motion;
00087 }
00088
00089 void ompl::control::SyclopEST::selectAndExtend(Region& region, std::vector<Motion*>& newMotions)
00090 {
00091 Motion* treeMotion = region.motions[rng_.uniformInt(0, region.motions.size()-1)];
00092 Control* rctrl = siC_->allocControl();
00093 base::State* newState = si_->allocState();
00094
00095 controlSampler_->sample(rctrl, treeMotion->state);
00096 unsigned int duration = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
00097 duration = siC_->propagateWhileValid(treeMotion->state, rctrl, duration, newState);
00098
00099 if (duration >= siC_->getMinControlDuration())
00100 {
00101 Motion* motion = new Motion(siC_);
00102 si_->copyState(motion->state, newState);
00103 siC_->copyControl(motion->control, rctrl);
00104 motion->steps = duration;
00105 motion->parent = treeMotion;
00106 motions_.push_back(motion);
00107 newMotions.push_back(motion);
00108 }
00109
00110 siC_->freeControl(rctrl);
00111 si_->freeState(newState);
00112 }
00113
00114 void ompl::control::SyclopEST::freeMemory(void)
00115 {
00116 for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
00117 {
00118 Motion* m = *i;
00119 if (m->state)
00120 si_->freeState(m->state);
00121 if (m->control)
00122 siC_->freeControl(m->control);
00123 delete m;
00124 }
00125 }