2021-11-06 ompl - rrtconnect 源码分析

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_);
 }```