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/rrt/RRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 #include <limits>
00041
00042 ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT")
00043 {
00044 specs_.approximateSolutions = true;
00045 siC_ = si.get();
00046 addIntermediateStates_ = false;
00047
00048 goalBias_ = 0.05;
00049
00050 Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias);
00051 Planner::declareParam<bool>("intermediate_states", this, &RRT::setIntermediateStates, &RRT::getIntermediateStates);
00052 }
00053
00054 ompl::control::RRT::~RRT(void)
00055 {
00056 freeMemory();
00057 }
00058
00059 void ompl::control::RRT::setup(void)
00060 {
00061 base::Planner::setup();
00062 if (!nn_)
00063 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00064 nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
00065 }
00066
00067 void ompl::control::RRT::clear(void)
00068 {
00069 Planner::clear();
00070 sampler_.reset();
00071 controlSampler_.reset();
00072 freeMemory();
00073 if (nn_)
00074 nn_->clear();
00075 }
00076
00077 void ompl::control::RRT::freeMemory(void)
00078 {
00079 if (nn_)
00080 {
00081 std::vector<Motion*> motions;
00082 nn_->list(motions);
00083 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00084 {
00085 if (motions[i]->state)
00086 si_->freeState(motions[i]->state);
00087 if (motions[i]->control)
00088 siC_->freeControl(motions[i]->control);
00089 delete motions[i];
00090 }
00091 }
00092 }
00093
00094 bool ompl::control::RRT::solve(const base::PlannerTerminationCondition &ptc)
00095 {
00096 checkValidity();
00097 base::Goal *goal = pdef_->getGoal().get();
00098 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00099
00100 while (const base::State *st = pis_.nextStart())
00101 {
00102 Motion *motion = new Motion(siC_);
00103 si_->copyState(motion->state, st);
00104 siC_->nullControl(motion->control);
00105 nn_->add(motion);
00106 }
00107
00108 if (nn_->size() == 0)
00109 {
00110 msg_.error("There are no valid initial states!");
00111 return false;
00112 }
00113
00114 if (!sampler_)
00115 sampler_ = si_->allocStateSampler();
00116 if (!controlSampler_)
00117 controlSampler_ = siC_->allocDirectedControlSampler();
00118
00119 msg_.inform("Starting with %u states", nn_->size());
00120
00121 Motion *solution = NULL;
00122 Motion *approxsol = NULL;
00123 double approxdif = std::numeric_limits<double>::infinity();
00124
00125 Motion *rmotion = new Motion(siC_);
00126 base::State *rstate = rmotion->state;
00127 Control *rctrl = rmotion->control;
00128 base::State *xstate = si_->allocState();
00129
00130 while (ptc() == false)
00131 {
00132
00133 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00134 goal_s->sampleGoal(rstate);
00135 else
00136 sampler_->sampleUniform(rstate);
00137
00138
00139 Motion *nmotion = nn_->nearest(rmotion);
00140
00141
00142 unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state);
00143
00144 if (addIntermediateStates_)
00145 {
00146
00147 std::vector<base::State *> pstates;
00148 cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true);
00149
00150 if (cd >= siC_->getMinControlDuration())
00151 {
00152 Motion *lastmotion = nmotion;
00153 bool solved = false;
00154 size_t p = 0;
00155 for ( ; p < pstates.size(); ++p)
00156 {
00157
00158 Motion *motion = new Motion();
00159 motion->state = pstates[p];
00160
00161 motion->control = siC_->allocControl();
00162 siC_->copyControl(motion->control, rctrl);
00163 motion->steps = 1;
00164 motion->parent = lastmotion;
00165 lastmotion = motion;
00166 nn_->add(motion);
00167 double dist = 0.0;
00168 solved = goal->isSatisfied(motion->state, &dist);
00169 if (solved)
00170 {
00171 approxdif = dist;
00172 solution = motion;
00173 break;
00174 }
00175 if (dist < approxdif)
00176 {
00177 approxdif = dist;
00178 approxsol = motion;
00179 }
00180 }
00181
00182
00183 while (++p < pstates.size())
00184 si_->freeState(pstates[p]);
00185 if (solved)
00186 break;
00187 }
00188 else
00189 for (size_t p = 0 ; p < pstates.size(); ++p)
00190 si_->freeState(pstates[p]);
00191 }
00192 else
00193 {
00194 cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, xstate);
00195
00196 if (cd >= siC_->getMinControlDuration())
00197 {
00198
00199 Motion *motion = new Motion(siC_);
00200 si_->copyState(motion->state, xstate);
00201 siC_->copyControl(motion->control, rctrl);
00202 motion->steps = cd;
00203 motion->parent = nmotion;
00204
00205 nn_->add(motion);
00206 double dist = 0.0;
00207 bool solv = goal->isSatisfied(motion->state, &dist);
00208 if (solv)
00209 {
00210 approxdif = dist;
00211 solution = motion;
00212 break;
00213 }
00214 if (dist < approxdif)
00215 {
00216 approxdif = dist;
00217 approxsol = motion;
00218 }
00219 }
00220 }
00221 }
00222
00223 bool solved = false;
00224 bool approximate = false;
00225 if (solution == NULL)
00226 {
00227 solution = approxsol;
00228 approximate = true;
00229 }
00230
00231 if (solution != NULL)
00232 {
00233
00234 std::vector<Motion*> mpath;
00235 while (solution != NULL)
00236 {
00237 mpath.push_back(solution);
00238 solution = solution->parent;
00239 }
00240
00241
00242 PathControl *path = new PathControl(si_);
00243 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00244 if (mpath[i]->parent)
00245 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
00246 else
00247 path->append(mpath[i]->state);
00248 solved = true;
00249 goal->addSolutionPath(base::PathPtr(path), approximate, approxdif);
00250 }
00251
00252 if (rmotion->state)
00253 si_->freeState(rmotion->state);
00254 if (rmotion->control)
00255 siC_->freeControl(rmotion->control);
00256 delete rmotion;
00257 si_->freeState(xstate);
00258
00259 msg_.inform("Created %u states", nn_->size());
00260
00261 return solved;
00262 }
00263
00264 void ompl::control::RRT::getPlannerData(base::PlannerData &data) const
00265 {
00266 Planner::getPlannerData(data);
00267
00268 std::vector<Motion*> motions;
00269 if (nn_)
00270 nn_->list(motions);
00271
00272 if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00273 {
00274 double delta = siC_->getPropagationStepSize();
00275
00276 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00277 {
00278 const Motion* m = motions[i];
00279 if (m->parent)
00280 cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00281 else
00282 cpd->recordEdge(NULL, m->state, NULL, 0.);
00283 }
00284 }
00285 else
00286 {
00287 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00288 {
00289 const Motion* m = motions[i];
00290 data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00291 }
00292 }
00293 }