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/control/planners/rrt/RRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 #include <limits>
00041
00042 void ompl::control::RRT::setup(void)
00043 {
00044 base::Planner::setup();
00045 if (!nn_)
00046 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00047 nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
00048 }
00049
00050 void ompl::control::RRT::clear(void)
00051 {
00052 Planner::clear();
00053 sampler_.reset();
00054 controlSampler_.reset();
00055 freeMemory();
00056 if (nn_)
00057 nn_->clear();
00058 }
00059
00060 void ompl::control::RRT::freeMemory(void)
00061 {
00062 if (nn_)
00063 {
00064 std::vector<Motion*> motions;
00065 nn_->list(motions);
00066 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00067 {
00068 if (motions[i]->state)
00069 si_->freeState(motions[i]->state);
00070 if (motions[i]->control)
00071 siC_->freeControl(motions[i]->control);
00072 delete motions[i];
00073 }
00074 }
00075 }
00076
00077 bool ompl::control::RRT::solve(const base::PlannerTerminationCondition &ptc)
00078 {
00079 checkValidity();
00080 base::Goal *goal = pdef_->getGoal().get();
00081 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00082
00083 while (const base::State *st = pis_.nextStart())
00084 {
00085 Motion *motion = new Motion(siC_);
00086 si_->copyState(motion->state, st);
00087 siC_->nullControl(motion->control);
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 if (!controlSampler_)
00100 controlSampler_ = siC_->allocControlSampler();
00101
00102 msg_.inform("Starting with %u states", nn_->size());
00103
00104 Motion *solution = NULL;
00105 Motion *approxsol = NULL;
00106 double approxdif = std::numeric_limits<double>::infinity();
00107
00108 Motion *rmotion = new Motion(siC_);
00109 base::State *rstate = rmotion->state;
00110 Control *rctrl = rmotion->control;
00111 base::State *xstate = si_->allocState();
00112
00113 while (ptc() == false)
00114 {
00115
00116 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00117 goal_s->sampleGoal(rstate);
00118 else
00119 sampler_->sampleUniform(rstate);
00120
00121
00122 Motion *nmotion = nn_->nearest(rmotion);
00123
00124
00125 unsigned int cd = controlSampler_->sampleTo(rctrl, siC_->getMinControlDuration(), siC_->getMaxControlDuration(),
00126 nmotion->control, nmotion->state, rmotion->state);
00127 cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, xstate);
00128
00129 if (cd >= siC_->getMinControlDuration())
00130 {
00131
00132 Motion *motion = new Motion(siC_);
00133 si_->copyState(motion->state, xstate);
00134 siC_->copyControl(motion->control, rctrl);
00135 motion->steps = cd;
00136 motion->parent = nmotion;
00137
00138 nn_->add(motion);
00139 double dist = 0.0;
00140 bool solved = goal->isSatisfied(motion->state, &dist);
00141 if (solved)
00142 {
00143 approxdif = dist;
00144 solution = motion;
00145 break;
00146 }
00147 if (dist < approxdif)
00148 {
00149 approxdif = dist;
00150 approxsol = motion;
00151 }
00152 }
00153 }
00154
00155 bool approximate = false;
00156 if (solution == NULL)
00157 {
00158 solution = approxsol;
00159 approximate = true;
00160 }
00161
00162 if (solution != NULL)
00163 {
00164
00165 std::vector<Motion*> mpath;
00166 while (solution != NULL)
00167 {
00168 mpath.push_back(solution);
00169 solution = solution->parent;
00170 }
00171
00172
00173 PathControl *path = new PathControl(si_);
00174 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00175 {
00176 path->states.push_back(si_->cloneState(mpath[i]->state));
00177 if (mpath[i]->parent)
00178 {
00179 path->controls.push_back(siC_->cloneControl(mpath[i]->control));
00180 path->controlDurations.push_back(mpath[i]->steps * siC_->getPropagationStepSize());
00181 }
00182 }
00183 goal->setDifference(approxdif);
00184 goal->setSolutionPath(base::PathPtr(path), approximate);
00185
00186 if (approximate)
00187 msg_.warn("Found approximate solution");
00188 }
00189
00190 if (rmotion->state)
00191 si_->freeState(rmotion->state);
00192 if (rmotion->control)
00193 siC_->freeControl(rmotion->control);
00194 delete rmotion;
00195 si_->freeState(xstate);
00196
00197 msg_.inform("Created %u states", nn_->size());
00198
00199 return goal->isAchieved();
00200 }
00201
00202 void ompl::control::RRT::getPlannerData(base::PlannerData &data) const
00203 {
00204 Planner::getPlannerData(data);
00205
00206 std::vector<Motion*> motions;
00207 if (nn_)
00208 nn_->list(motions);
00209
00210 if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00211 {
00212 double delta = siC_->getPropagationStepSize();
00213
00214 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00215 {
00216 const Motion* m = motions[i];
00217 if (m->parent)
00218 cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00219 else
00220 cpd->recordEdge(NULL, m->state, NULL, 0.);
00221 }
00222 }
00223 else
00224 {
00225 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00226 {
00227 const Motion* m = motions[i];
00228 data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00229 }
00230 }
00231 }