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/geometric/planners/rrt/LazyRRT.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <cassert>
00042
00043 ompl::geometric::LazyRRT::LazyRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "LazyRRT")
00044 {
00045 specs_.directed = true;
00046 goalBias_ = 0.05;
00047 maxDistance_ = 0.0;
00048 lastGoalMotion_ = NULL;
00049
00050 Planner::declareParam<double>("range", this, &LazyRRT::setRange, &LazyRRT::getRange);
00051 Planner::declareParam<double>("goal_bias", this, &LazyRRT::setGoalBias, &LazyRRT::getGoalBias);
00052 }
00053
00054 ompl::geometric::LazyRRT::~LazyRRT(void)
00055 {
00056 freeMemory();
00057 }
00058
00059 void ompl::geometric::LazyRRT::setup(void)
00060 {
00061 Planner::setup();
00062 tools::SelfConfig sc(si_, getName());
00063 sc.configurePlannerRange(maxDistance_);
00064
00065 if (!nn_)
00066 nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
00067 nn_->setDistanceFunction(boost::bind(&LazyRRT::distanceFunction, this, _1, _2));
00068 }
00069
00070 void ompl::geometric::LazyRRT::clear(void)
00071 {
00072 Planner::clear();
00073 sampler_.reset();
00074 freeMemory();
00075 if (nn_)
00076 nn_->clear();
00077 lastGoalMotion_ = NULL;
00078 }
00079
00080 void ompl::geometric::LazyRRT::freeMemory(void)
00081 {
00082 if (nn_)
00083 {
00084 std::vector<Motion*> motions;
00085 nn_->list(motions);
00086 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00087 {
00088 if (motions[i]->state)
00089 si_->freeState(motions[i]->state);
00090 delete motions[i];
00091 }
00092 }
00093 }
00094
00095 ompl::base::PlannerStatus ompl::geometric::LazyRRT::solve(const base::PlannerTerminationCondition &ptc)
00096 {
00097 checkValidity();
00098 base::Goal *goal = pdef_->getGoal().get();
00099 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00100
00101 while (const base::State *st = pis_.nextStart())
00102 {
00103 Motion *motion = new Motion(si_);
00104 si_->copyState(motion->state, st);
00105 motion->valid = true;
00106 nn_->add(motion);
00107 }
00108
00109 if (nn_->size() == 0)
00110 {
00111 logError("There are no valid initial states!");
00112 return base::PlannerStatus::INVALID_START;
00113 }
00114
00115 if (!sampler_)
00116 sampler_ = si_->allocStateSampler();
00117
00118 logInform("Starting with %u states", nn_->size());
00119
00120 Motion *solution = NULL;
00121 double distsol = -1.0;
00122 Motion *rmotion = new Motion(si_);
00123 base::State *rstate = rmotion->state;
00124 base::State *xstate = si_->allocState();
00125
00126 bool solutionFound = false;
00127
00128 while (ptc() == false && !solutionFound)
00129 {
00130
00131 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00132 goal_s->sampleGoal(rstate);
00133 else
00134 sampler_->sampleUniform(rstate);
00135
00136
00137 Motion *nmotion = nn_->nearest(rmotion);
00138 assert(nmotion != rmotion);
00139 base::State *dstate = rstate;
00140
00141
00142 double d = si_->distance(nmotion->state, rstate);
00143 if (d > maxDistance_)
00144 {
00145 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00146 dstate = xstate;
00147 }
00148
00149
00150 Motion *motion = new Motion(si_);
00151 si_->copyState(motion->state, dstate);
00152 motion->parent = nmotion;
00153 nmotion->children.push_back(motion);
00154 nn_->add(motion);
00155
00156 double dist = 0.0;
00157 if (goal->isSatisfied(motion->state, &dist))
00158 {
00159 distsol = dist;
00160 solution = motion;
00161 solutionFound = true;
00162
00163 lastGoalMotion_ = solution;
00164
00165
00166
00167 std::vector<Motion*> mpath;
00168 while (solution != NULL)
00169 {
00170 mpath.push_back(solution);
00171 solution = solution->parent;
00172 }
00173
00174
00175 for (int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
00176 if (!mpath[i]->valid)
00177 {
00178 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
00179 mpath[i]->valid = true;
00180 else
00181 {
00182 removeMotion(mpath[i]);
00183 solutionFound = false;
00184 }
00185 }
00186
00187 if (solutionFound)
00188 {
00189
00190 PathGeometric *path = new PathGeometric(si_);
00191 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00192 path->append(mpath[i]->state);
00193
00194 pdef_->addSolutionPath(base::PathPtr(path), false, distsol);
00195 }
00196 }
00197 }
00198
00199 si_->freeState(xstate);
00200 si_->freeState(rstate);
00201 delete rmotion;
00202
00203 logInform("Created %u states", nn_->size());
00204
00205 return solutionFound ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00206 }
00207
00208 void ompl::geometric::LazyRRT::removeMotion(Motion *motion)
00209 {
00210 nn_->remove(motion);
00211
00212
00213
00214 if (motion->parent)
00215 {
00216 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00217 if (motion->parent->children[i] == motion)
00218 {
00219 motion->parent->children.erase(motion->parent->children.begin() + i);
00220 break;
00221 }
00222 }
00223
00224
00225 for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00226 {
00227 motion->children[i]->parent = NULL;
00228 removeMotion(motion->children[i]);
00229 }
00230
00231 if (motion->state)
00232 si_->freeState(motion->state);
00233 delete motion;
00234 }
00235
00236 void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
00237 {
00238 Planner::getPlannerData(data);
00239
00240 std::vector<Motion*> motions;
00241 if (nn_)
00242 nn_->list(motions);
00243
00244 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state, 1));
00245
00246 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00247 {
00248 if (motions[i]->parent == NULL)
00249 data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
00250 else
00251 data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : NULL),
00252 base::PlannerDataVertex(motions[i]->state));
00253
00254 data.tagState(motions[i]->state, motions[i]->valid ? 1 : 0);
00255 }
00256 }