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