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