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/SyclopRRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040
00041 void ompl::control::SyclopRRT::setup(void)
00042 {
00043 Syclop::setup();
00044 sampler_ = si_->allocStateSampler();
00045 controlSampler_ = siC_->allocDirectedControlSampler();
00046
00047
00048
00049 if (!nn_ && !regionalNN_)
00050 {
00051 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00052 nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
00053 }
00054 }
00055
00056 void ompl::control::SyclopRRT::clear(void)
00057 {
00058 Syclop::clear();
00059 freeMemory();
00060 if (nn_)
00061 nn_->clear();
00062 }
00063
00064 void ompl::control::SyclopRRT::getPlannerData(base::PlannerData& data) const
00065 {
00066 Planner::getPlannerData(data);
00067 std::vector<Motion*> motions;
00068 if (nn_)
00069 nn_->list(motions);
00070 if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00071 {
00072 const double delta = siC_->getPropagationStepSize();
00073
00074 for (std::vector<Motion*>::const_iterator i = motions.begin(); i != motions.end(); ++i)
00075 {
00076 const Motion* m = *i;
00077 if (m->parent)
00078 cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00079 else
00080 cpd->recordEdge(NULL, m->state, NULL, 0.);
00081 }
00082 }
00083 else
00084 {
00085 for (std::vector<Motion*>::const_iterator i = motions.begin(); i != motions.end(); ++i)
00086 {
00087 const Motion* m = *i;
00088 data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00089 }
00090 }
00091 }
00092
00093 ompl::control::Syclop::Motion* ompl::control::SyclopRRT::addRoot(const base::State* s)
00094 {
00095 Motion* motion = new Motion(siC_);
00096 si_->copyState(motion->state, s);
00097 siC_->nullControl(motion->control);
00098
00099 if (nn_)
00100 nn_->add(motion);
00101 return motion;
00102 }
00103
00104 void ompl::control::SyclopRRT::selectAndExtend(Region& region, std::vector<Motion*>& newMotions)
00105 {
00106 Motion* rmotion = new Motion(siC_);
00107 base::StateSamplerPtr sampler(si_->allocStateSampler());
00108 decomp_->sampleFromRegion(region.index, sampler, rmotion->state);
00109
00110 Motion* nmotion;
00111 if (regionalNN_)
00112 {
00113
00114
00115 std::vector<int> searchRegions;
00116 decomp_->getNeighbors(region.index, searchRegions);
00117 searchRegions.push_back(region.index);
00118
00119 std::vector<Motion*> motions;
00120 for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
00121 {
00122 const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
00123 motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
00124 }
00125
00126 std::vector<Motion*>::const_iterator i = motions.begin();
00127 nmotion = *i;
00128 double minDistance = distanceFunction(rmotion, nmotion);
00129 ++i;
00130 while (i != motions.end())
00131 {
00132 Motion* m = *i;
00133 const double dist = distanceFunction(rmotion, m);
00134 if (dist < minDistance)
00135 {
00136 nmotion = m;
00137 minDistance = dist;
00138 }
00139 ++i;
00140 }
00141 }
00142 else
00143 {
00144 assert (nn_);
00145 nmotion = nn_->nearest(rmotion);
00146 }
00147
00148 base::State* newState = si_->allocState();
00149
00150 unsigned int duration = controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
00151
00152 duration = siC_->propagateWhileValid(nmotion->state, rmotion->control, duration, newState);
00153
00154 if (duration >= siC_->getMinControlDuration())
00155 {
00156 Motion* motion = new Motion(siC_);
00157 si_->copyState(motion->state, newState);
00158 siC_->copyControl(motion->control, rmotion->control);
00159 motion->steps = duration;
00160 motion->parent = nmotion;
00161 newMotions.push_back(motion);
00162 if (nn_)
00163 nn_->add(motion);
00164 }
00165
00166 si_->freeState(rmotion->state);
00167 siC_->freeControl(rmotion->control);
00168 delete rmotion;
00169 si_->freeState(newState);
00170 }
00171
00172 void ompl::control::SyclopRRT::freeMemory(void)
00173 {
00174 if (nn_)
00175 {
00176 std::vector<Motion*> motions;
00177 nn_->list(motions);
00178 for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
00179 {
00180 Motion* m = *i;
00181 if (m->state)
00182 si_->freeState(m->state);
00183 if (m->control)
00184 siC_->freeControl(m->control);
00185 delete m;
00186 }
00187 }
00188 }