RRTConnect 算法原理
rrt算法综述
太多了先不找了,分析代码去。。。
参考论文
rrtconnect 开创性论文
- RRTConnect.h
#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
#define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
#include "ompl/datastructures/NearestNeighbors.h"
#include "ompl/geometric/planners/PlannerIncludes.h"
namespace ompl
{
namespace geometric
{
class RRTConnect : public base::Planner //继承planner基类
{
public:
RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates = false);
~RRTConnect() override;
void getPlannerData(base::PlannerData &data) const override;
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
void clear() override;
bool getIntermediateStates() const
{
return addIntermediateStates_;
}
void setIntermediateStates(bool addIntermediateStates)
{
addIntermediateStates_ = addIntermediateStates;
}
void setRange(double distance)
{
maxDistance_ = distance;
}
double getRange() const
{
return maxDistance_;
}
template <template <typename T> class NN>
void setNearestNeighbors()
{
if ((tStart_ && tStart_->size() != 0) || (tGoal_ && tGoal_->size() != 0))
OMPL_WARN("Calling setNearestNeighbors will clear all states.");
clear();
tStart_ = std::make_shared<NN<Motion *>>();
tGoal_ = std::make_shared<NN<Motion *>>();
setup();
}
void setup() override;
protected:
class Motion // 连接状态空间采样点的动作,有根状态,当前状态和父动作
{
public:
Motion() = default;
Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
{
}
~Motion() = default;
const base::State *root{nullptr};
base::State *state{nullptr};
Motion *parent{nullptr};
};
using TreeData = std::shared_ptr<NearestNeighbors<Motion *>>;
struct TreeGrowingInfo
{
base::State *xstate;
Motion *xmotion;
bool start;
};
enum GrowState //采样状态的标志符,陷阱、继续和到达三种状态
{
TRAPPED,
ADVANCED,
REACHED
};
void freeMemory();
double distanceFunction(const Motion *a, const Motion *b) const //计算采样点之间的距离,也是代价,或者说评估路径优劣的标准建立基础,根据不同的距离计算方式,可以有目的性地选择路径生长方向
{
return si_->distance(a->state, b->state);//修改这一句为所需
}
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion);//让树生长,采样随机
base::StateSamplerPtr sampler_;//指向默认的采样器
TreeData tStart_;//树的起点
TreeData tGoal_;//树的目标
double maxDistance_{0.};//每一步树枝生长的最大距离?
bool addIntermediateStates_;//是否插入中间状态?
RNG rng_;
std::pair<base::State *, base::State *> connectionPoint_;
double distanceBetweenTrees_;//起点树和终点树的距离,rrtconnect两树分别从起点终点相对生长,距离为零时直接连接两树各自的路径,形成最终路径
};
}
}
#endif
RRTConnect.cpp
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
#include "ompl/geometric/planners/rrt/RRTConnect.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/util/String.h"
ompl::geometric::RRTConnect::RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates)
: base::Planner(si, addIntermediateStates ? "RRTConnectIntermediate" : "RRTConnect")
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
specs_.directed = true;
Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange, "0.:1.:10000.");//设置采样点可以延伸的距离
Planner::declareParam<bool>("intermediate_states", this, &RRTConnect::setIntermediateStates,
&RRTConnect::getIntermediateStates, "0,1");//是否可以插入中间点
connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);//初始化一个连接点
distanceBetweenTrees_ = std::numeric_limits<double>::infinity();//初始化两树距离为无限大,开始生长之后计算距离,向值不断减小的方向生长?
addIntermediateStates_ = addIntermediateStates;//创建rrtconnectplanner时设定参数
}
ompl::geometric::RRTConnect::~RRTConnect() //析构函数,释放内存
{
freeMemory();
}
void ompl::geometric::RRTConnect::setup()//开始solve之前会setup planner,初始化一些默认设置
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!tStart_)
tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
if (!tGoal_)
tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
tStart_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });//设定两采样点之间的距离计算函数,以起点为根的树
tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });//设定两采样点之间的距离计算函数,以终点为根的树
}
void ompl::geometric::RRTConnect::freeMemory()
{
std::vector<Motion *> motions;
if (tStart_)
{
tStart_->list(motions);
for (auto &motion : motions)
{
if (motion->state != nullptr)
si_->freeState(motion->state);
delete motion;
}
}
if (tGoal_)
{
tGoal_->list(motions);
for (auto &motion : motions)
{
if (motion->state != nullptr)
si_->freeState(motion->state);
delete motion;
}
}
}
void ompl::geometric::RRTConnect::clear()//清空规划算法设置,相当于未设置任何自定义参数
{
Planner::clear();
sampler_.reset();
freeMemory();
if (tStart_)
tStart_->clear();
if (tGoal_)
tGoal_->clear();
connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
}
//tree 要生长的树
// tgi 生长信息,包括节点和动作
// rmotion 动作
ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion)
{
//延伸树枝
/* find closest state in the tree */
Motion *nmotion = tree->nearest(rmotion); //本树上距离采样到的新运动rmotion最近的点,新指针nmotion指向树上这个运动
/* assume we can reach the state we go towards */
bool reach = true;
/* find state to add */
base::State *dstate = rmotion->state;//rmotion 内含的构型(状态)
double d = si_->distance(nmotion->state, rmotion->state); //(距离计算方式,需要自己改,这里是构型空间/关节空间距离)
if (d > maxDistance_)
{
//距离大于一次生长的最大距离,则插入中间点,以maxdistance/d这个0-1参数归一化存储到xstate中?
si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
/* Check if we have moved at all. Due to some stranger state spaces (e.g., the constrained state spaces),
* interpolate can fail and no progress is made. Without this check, the algorithm gets stuck in a loop as it
* thinks it is making progress, when none is actually occurring. */
if (si_->equalStates(nmotion->state, tgi.xstate))//插入完发现相同状态树没长?局部陷阱?
return TRAPPED;
dstate = tgi.xstate;//xstate (不论是否插入中间点)就是要生长到的点的状态
reach = false;
}
// 是否可以达到,树上的状态和要连接的新状态是否可行,进行isvalid 有效性检测,是statespace的方法,构型空间有效性判断,碰撞检测在此进行!需要改写自己的碰撞检测需求。
bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) :
si_->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
if (!validMotion) //连接不上新的状态,局部陷阱了
return TRAPPED;
if (addIntermediateStates_)
{
const base::State *astate = tgi.start ? nmotion->state : dstate;
const base::State *bstate = tgi.start ? dstate : nmotion->state;
std::vector<base::State *> states;
const unsigned int count = si_->getStateSpace()->validSegmentCount(astate, bstate);
if (si_->getMotionStates(astate, bstate, states, count, true, true))
si_->freeState(states[0]);
for (std::size_t i = 1; i < states.size(); ++i)
{
auto *motion = new Motion;
motion->state = states[i];
motion->parent = nmotion;
motion->root = nmotion->root;
tree->add(motion);
nmotion = motion;
}
tgi.xmotion = nmotion;
}
else
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tree->add(motion);
tgi.xmotion = motion;
}
return reach ? REACHED : ADVANCED;
}
ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
if (goal == nullptr)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);
}
if (tStart_->size() == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
(int)(tStart_->size() + tGoal_->size()));
TreeGrowingInfo tgi;
tgi.xstate = si_->allocState();
Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
bool startTree = true;
bool solved = false;
while (!ptc)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
tgi.start = startTree;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st != nullptr)
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);
}
if (tGoal_->size() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
break;
}
}
/* sample random state */
sampler_->sampleUniform(rstate);
GrowState gs = growTree(tree, tgi, rmotion);
if (gs != TRAPPED)
{
/* remember which motion was just added */
Motion *addedMotion = tgi.xmotion;
/* attempt to connect trees */
/* if reached, it means we used rstate directly, no need to copy again */
if (gs != REACHED)
si_->copyState(rstate, tgi.xstate);
GrowState gsc = ADVANCED;
tgi.start = startTree;
while (gsc == ADVANCED)
gsc = growTree(otherTree, tgi, rmotion);
/* update distance between trees */
const double newDist = tree->getDistanceFunction()(addedMotion, otherTree->nearest(addedMotion));
if (newDist < distanceBetweenTrees_)
{
distanceBetweenTrees_ = newDist;
// OMPL_INFORM("Estimated distance to go: %f", distanceBetweenTrees_);
}
Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
/* if we connected the trees in a valid way (start and goal pair is valid)*/
if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// it must be the case that either the start tree or the goal tree has made some progress
// so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
// on the solution path
if (startMotion->parent != nullptr)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;
connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
/* construct the solution path */
Motion *solution = startMotion;
std::vector<Motion *> mpath1;
while (solution != nullptr)
{
mpath1.push_back(solution);
solution = solution->parent;
}
solution = goalMotion;
std::vector<Motion *> mpath2;
while (solution != nullptr)
{
mpath2.push_back(solution);
solution = solution->parent;
}
auto path(std::make_shared<PathGeometric>(si_));
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1; i >= 0; --i)
path->append(mpath1[i]->state);
for (auto &i : mpath2)
path->append(i->state);
pdef_->addSolutionPath(path, false, 0.0, getName());
solved = true;
break;
}
else
{
// We didn't reach the goal, but if we were extending the start
// tree, then we can mark/improve the approximate path so far.
if (!startTree)
{
// We were working from the startTree.
double dist = 0.0;
goal->isSatisfied(tgi.xmotion->state, &dist);
if (dist < approxdif)
{
approxdif = dist;
approxsol = tgi.xmotion;
}
}
}
}
}
si_->freeState(tgi.xstate);
si_->freeState(rstate);
delete rmotion;
OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
tStart_->size(), tGoal_->size());
if (approxsol && !solved)
{
/* construct the solution path */
std::vector<Motion *> mpath;
while (approxsol != nullptr)
{
mpath.push_back(approxsol);
approxsol = approxsol->parent;
}
auto path(std::make_shared<PathGeometric>(si_));
for (int i = mpath.size() - 1; i >= 0; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(path, true, approxdif, getName());
return base::PlannerStatus::APPROXIMATE_SOLUTION;
}
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion *> motions;
if (tStart_)
tStart_->list(motions);
for (auto &motion : motions)
{
if (motion->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
else
{
data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
}
}
motions.clear();
if (tGoal_)
tGoal_->list(motions);
for (auto &motion : motions)
{
if (motion->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
else
{
// The edges in the goal tree are reversed to be consistent with start tree
data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
}
}
// Add the edge connecting the two trees
data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
// Add some info.
data.properties["approx goal distance REAL"] = ompl::toString(distanceBetweenTrees_);
}```