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/RRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <limits>
00042
00043 void ompl::geometric::RRT::clear(void)
00044 {
00045 Planner::clear();
00046 sampler_.reset();
00047 freeMemory();
00048 if (nn_)
00049 nn_->clear();
00050 }
00051
00052 void ompl::geometric::RRT::setup(void)
00053 {
00054 Planner::setup();
00055 SelfConfig sc(si_, getName());
00056 sc.configurePlannerRange(maxDistance_);
00057
00058 if (!nn_)
00059 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00060 nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
00061 }
00062
00063 void ompl::geometric::RRT::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::RRT::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 nn_->add(motion);
00089 }
00090
00091 if (nn_->size() == 0)
00092 {
00093 msg_.error("There are no valid initial states!");
00094 return false;
00095 }
00096
00097 if (!sampler_)
00098 sampler_ = si_->allocStateSampler();
00099
00100 msg_.inform("Starting with %u states", nn_->size());
00101
00102 Motion *solution = NULL;
00103 Motion *approxsol = NULL;
00104 double approxdif = std::numeric_limits<double>::infinity();
00105 Motion *rmotion = new Motion(si_);
00106 base::State *rstate = rmotion->state;
00107 base::State *xstate = si_->allocState();
00108
00109 while (ptc() == false)
00110 {
00111
00112
00113 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00114 goal_s->sampleGoal(rstate);
00115 else
00116 sampler_->sampleUniform(rstate);
00117
00118
00119 Motion *nmotion = nn_->nearest(rmotion);
00120 base::State *dstate = rstate;
00121
00122
00123 double d = si_->distance(nmotion->state, rstate);
00124 if (d > maxDistance_)
00125 {
00126 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00127 dstate = xstate;
00128 }
00129
00130 if (si_->checkMotion(nmotion->state, dstate))
00131 {
00132
00133 Motion *motion = new Motion(si_);
00134 si_->copyState(motion->state, dstate);
00135 motion->parent = nmotion;
00136
00137 nn_->add(motion);
00138 double dist = 0.0;
00139 bool solved = goal->isSatisfied(motion->state, &dist);
00140 if (solved)
00141 {
00142 approxdif = dist;
00143 solution = motion;
00144 break;
00145 }
00146 if (dist < approxdif)
00147 {
00148 approxdif = dist;
00149 approxsol = motion;
00150 }
00151 }
00152 }
00153
00154 bool approximate = false;
00155 if (solution == NULL)
00156 {
00157 solution = approxsol;
00158 approximate = true;
00159 }
00160
00161 if (solution != NULL)
00162 {
00163
00164 std::vector<Motion*> mpath;
00165 while (solution != NULL)
00166 {
00167 mpath.push_back(solution);
00168 solution = solution->parent;
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 goal->setDifference(approxdif);
00176 goal->setSolutionPath(base::PathPtr(path), approximate);
00177
00178 if (approximate)
00179 msg_.warn("Found approximate solution");
00180 }
00181
00182 si_->freeState(xstate);
00183 if (rmotion->state)
00184 si_->freeState(rmotion->state);
00185 delete rmotion;
00186
00187 msg_.inform("Created %u states", nn_->size());
00188
00189 return goal->isAchieved();
00190 }
00191
00192 void ompl::geometric::RRT::getPlannerData(base::PlannerData &data) const
00193 {
00194 Planner::getPlannerData(data);
00195
00196 std::vector<Motion*> motions;
00197 if (nn_)
00198 nn_->list(motions);
00199
00200 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00201 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00202 }