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/NearestNeighborsSqrtApprox.h"
00040 #include <limits>
00041
00042 void ompl::control::RRT::setup(void)
00043 {
00044 base::Planner::setup();
00045 if (!nn_)
00046 nn_.reset(new NearestNeighborsSqrtApprox<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 if (!goal)
00084 {
00085 msg_.error("Goal undefined");
00086 return false;
00087 }
00088
00089 while (const base::State *st = pis_.nextStart())
00090 {
00091 Motion *motion = new Motion(siC_);
00092 si_->copyState(motion->state, st);
00093 siC_->nullControl(motion->control);
00094 nn_->add(motion);
00095 }
00096
00097 if (nn_->size() == 0)
00098 {
00099 msg_.error("There are no valid initial states!");
00100 return false;
00101 }
00102
00103 if (!sampler_)
00104 sampler_ = si_->allocManifoldStateSampler();
00105 if (!controlSampler_)
00106 controlSampler_ = siC_->allocControlSampler();
00107
00108 msg_.inform("Starting with %u states", nn_->size());
00109
00110 Motion *solution = NULL;
00111 Motion *approxsol = NULL;
00112 double approxdif = std::numeric_limits<double>::infinity();
00113
00114 Motion *rmotion = new Motion(siC_);
00115 base::State *rstate = rmotion->state;
00116 Control *rctrl = rmotion->control;
00117 base::State *xstate = si_->allocState();
00118
00119 while (ptc() == false)
00120 {
00121
00122 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00123 goal_s->sampleGoal(rstate);
00124 else
00125 sampler_->sampleUniform(rstate);
00126
00127
00128 Motion *nmotion = nn_->nearest(rmotion);
00129
00130
00131 controlSampler_->sampleNext(rctrl, nmotion->control, nmotion->state);
00132 unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
00133 cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, xstate);
00134
00135 if (cd >= siC_->getMinControlDuration())
00136 {
00137
00138 Motion *motion = new Motion(siC_);
00139 si_->copyState(motion->state, xstate);
00140 siC_->copyControl(motion->control, rctrl);
00141 motion->steps = cd;
00142 motion->parent = nmotion;
00143
00144 nn_->add(motion);
00145 double dist = 0.0;
00146 bool solved = goal->isSatisfied(motion->state, &dist);
00147 if (solved)
00148 {
00149 approxdif = dist;
00150 solution = motion;
00151 break;
00152 }
00153 if (dist < approxdif)
00154 {
00155 approxdif = dist;
00156 approxsol = motion;
00157 }
00158 }
00159 }
00160
00161 bool approximate = false;
00162 if (solution == NULL)
00163 {
00164 solution = approxsol;
00165 approximate = true;
00166 }
00167
00168 if (solution != NULL)
00169 {
00170
00171 std::vector<Motion*> mpath;
00172 while (solution != NULL)
00173 {
00174 mpath.push_back(solution);
00175 solution = solution->parent;
00176 }
00177
00178
00179 PathControl *path = new PathControl(si_);
00180 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00181 {
00182 path->states.push_back(si_->cloneState(mpath[i]->state));
00183 if (mpath[i]->parent)
00184 {
00185 path->controls.push_back(siC_->cloneControl(mpath[i]->control));
00186 path->controlDurations.push_back(mpath[i]->steps * siC_->getPropagationStepSize());
00187 }
00188 }
00189 goal->setDifference(approxdif);
00190 goal->setSolutionPath(base::PathPtr(path), approximate);
00191
00192 if (approximate)
00193 msg_.warn("Found approximate solution");
00194 }
00195
00196 if (rmotion->state)
00197 si_->freeState(rmotion->state);
00198 if (rmotion->control)
00199 siC_->freeControl(rmotion->control);
00200 delete rmotion;
00201 si_->freeState(xstate);
00202
00203 msg_.inform("Created %u states", nn_->size());
00204
00205 return goal->isAchieved();
00206 }
00207
00208 void ompl::control::RRT::getPlannerData(base::PlannerData &data) const
00209 {
00210 Planner::getPlannerData(data);
00211
00212 std::vector<Motion*> motions;
00213 if (nn_)
00214 nn_->list(motions);
00215
00216 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00217 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00218 }