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