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