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