Extend planner

hrplib/hrpPlanner/Configuration.cpp

@@ -35,3 +35,21 @@ unsigned int Configuration::size() const
 {
     return m_values.size();
 }
+
+bool Configuration::operator!=(const Configuration& cfg)
+{
+    if (size() != cfg.size()) return true;
+    for (unsigned int i=0; i<size(); i++){
+        if (value(i) != cfg.value(i)) return true;
+    }
+    return false;
+}
+
+bool Configuration::operator==(const Configuration& cfg)
+{
+    if (size() != cfg.size()) return false;
+    for (unsigned int i=0; i<size(); i++){
+        if (value(i) != cfg.value(i)) return false;
+    }
+    return true;
+}

hrplib/hrpPlanner/Configuration.h

@@ -37,6 +37,18 @@ namespace PathEngine {
          */
         unsigned int size() const;
 
+        /**
+           @brief compare configurations
+           @return true if not equal, false otherwise
+         */
+        bool operator!=(const Configuration& cfg);
+
+        /**
+           @brief compare configurations
+           @return true if equal, false otherwise
+         */
+        bool operator==(const Configuration& cfg);
+
     private:    
         std::vector<double> m_values;
     };

hrplib/hrpPlanner/RRT.cpp

@@ -9,14 +9,15 @@ using namespace PathEngine;
 static bool debug=false;
 //static bool debug=true;
 
-RRT::RRT(PathPlanner* plan) : Algorithm(plan)
+RRT::RRT(PathPlanner* plan) : Algorithm(plan), extraConnectionCheckFunc_(NULL)
 {
     // set default properties
     properties_["max-trials"] = "10000";
     properties_["eps"] = "0.1";
 
     Tstart_ = Ta_ = roadmap_;
     Tgoal_  = Tb_ = RoadmapPtr(new Roadmap(planner_));
+    TlastExtended_ = RoadmapPtr();
 
     extendFromStart_ = true;
     extendFromGoal_ = false;
@@ -32,6 +33,7 @@ RRT::~RRT()
 int RRT::extend(RoadmapPtr tree, Configuration& qRand, bool reverse) {
     if (debug) std::cout << "RRT::extend("<< qRand << ", " << reverse << ")" 
                          << std::endl;
+    TlastExtended_ = tree;
 
     RoadmapNodePtr minNode;
     double min;
@@ -154,7 +156,12 @@ bool RRT::extendOneStep()
 
         if (extend(Ta_, qNew, Tb_ == Tstart_) != Trapped) {
             if (connect(Tb_, qNew, Ta_ == Tstart_) == Reached) {
-                return true;
+                if (extraConnectionCheckFunc_){
+                    return extraConnectionCheckFunc_(Tstart_->lastAddedNode()->position(),
+                                                      Tgoal_->lastAddedNode()->position());
+                }else{
+                    return true;
+                }
             }
         }
         swapTrees();
@@ -230,3 +237,8 @@ void RRT::setForwardTree(RoadmapPtr tree) {
 void RRT::setBackwardTree(RoadmapPtr tree) { 
     Tgoal_ = Tb_ = tree;
 }
+
+void RRT::setExtraConnectionCheckFunc(extraConnectionCheckFunc i_func)
+{
+    extraConnectionCheckFunc_ = i_func;
+}

hrplib/hrpPlanner/RRT.h

@@ -17,6 +17,12 @@ namespace PathEngine {
    */
   class RRT
     : public Algorithm {
+  public:
+    /**
+     * スタートからのツリーとゴールからのツリーが接続できたかの追加チェックを行うユーザ関数。通常のチェックで接続できたとみなされるスタート側のコンフィギュレーションとゴール側のコンフィギュレーションを引数にとり、接続可能である場合にはtrueを、それ以外の場合はfalseを返す。
+     */
+    typedef boost::function2<bool, const Configuration &, const Configuration &> extraConnectionCheckFunc;
+
   private:
     /**
      * extend()の結果
@@ -37,6 +43,11 @@ namespace PathEngine {
      */
     RoadmapPtr Tgoal_;
 
+    /**
+     * @brief a tree extened in the last phase
+     */
+    RoadmapPtr TlastExtended_;
+
     /**
      * @brief ツリーの交換のために用いる変数。どちらか一方がTstart_をもう一方がTgoal_を指す
      */
@@ -82,6 +93,11 @@ namespace PathEngine {
      */
     bool extendFromGoal_;
 
+    /**
+     * スタートからのツリーとゴールからのツリーが接続できたかの追加チェックを行うユーザ関数
+     */
+    extraConnectionCheckFunc extraConnectionCheckFunc_;
+
   public:
     /**
      * @brief コンストラクタ
@@ -139,6 +155,12 @@ namespace PathEngine {
      */
     RoadmapPtr getBackwardTree() { return Tgoal_; }
 
+    /**
+     * @brief get a tree extened in the last phase
+     * @return a tree extened in the last phase
+     */
+    RoadmapPtr getLastExtendedTree() { return TlastExtended_; }
+
     /**
      * @brief ゴールからのツリーを設定する
      * @param tree ゴールからのツリー
@@ -156,6 +178,12 @@ namespace PathEngine {
      * @param e サンプリングしたコンフィギュレーションに向かって伸ばす距離
      */
     void epsilon(double e) { eps_ = e; }
+
+    /**
+     * @brief スタートからのツリーとゴールからのツリーが接続できたかの追加チェックを行うユーザ関数を設定する
+     * @param i_func ユーザ関数
+     */
+    void setExtraConnectionCheckFunc(extraConnectionCheckFunc i_func);
   };
 };