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