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