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