RRT.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
00002 #include "Roadmap.h"
00003 #include "RoadmapNode.h"
00004 #include "ConfigurationSpace.h"
00005 #include "RRT.h"
00006 
00007 using namespace PathEngine;
00008 
00009 static bool debug=false;
00010 //static bool debug=true;
00011 
00012 RRT::RRT(PathPlanner* plan) : Algorithm(plan), extraConnectionCheckFunc_(NULL)
00013 {
00014     // set default properties
00015     properties_["max-trials"] = "10000";
00016     properties_["eps"] = "0.1";
00017 
00018     Tstart_ = Ta_ = roadmap_;
00019     Tgoal_  = Tb_ = RoadmapPtr(new Roadmap(planner_));
00020     TlastExtended_ = RoadmapPtr();
00021 
00022     extendFromStart_ = true;
00023     extendFromGoal_ = false;
00024 
00025     ignoreCollisionAtStart_ = false;
00026     ignoreCollisionAtGoal_ = false;
00027 }
00028 
00029 RRT::~RRT() 
00030 {
00031 }
00032 
00033 int RRT::extend(RoadmapPtr tree, Configuration& qRand, bool reverse) {
00034     if (debug) std::cout << "RRT::extend("<< qRand << ", " << reverse << ")" 
00035                          << std::endl;
00036     TlastExtended_ = tree;
00037 
00038     RoadmapNodePtr minNode;
00039     double min;
00040     tree->findNearestNode(qRand, minNode, min);
00041     if (debug) std::cout << "nearest : pos = (" << minNode->position() 
00042                          << "), d = " << min << std::endl;
00043 
00044     if (minNode != NULL) {
00045         Mobility* mobility = planner_->getMobility();
00046 
00047         if (min > eps_){
00048             Configuration qRandOrg = qRand;
00049             qRand = mobility->interpolate(minNode->position(), qRand, eps_/min);
00050             if (debug) std::cout << "qRand = (" << qRand << ")" << std::endl;
00051             if (mobility->distance(minNode->position(), qRand) > min){
00052                 std::cout << "distance didn't decrease" << std::endl;
00053                 std::cout << "qRandOrg : (" << qRandOrg << "), d = " << min
00054                           << std::endl;
00055                 std::cout << "qRand    : (" << qRand << "), d = " 
00056                           << mobility->distance(minNode->position(), qRand)
00057                           << std::endl;
00058                 getchar();
00059             }
00060         }
00061 
00062         if (planner_->checkCollision(qRand)) return Trapped;
00063 
00064         if (reverse){
00065             if (mobility->isReachable(qRand, minNode->position())){
00066                 RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
00067                 tree->addNode(newNode);
00068                 tree->addEdge(newNode, minNode);
00069                 if (min <= eps_) {
00070                     if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
00071                     return Reached;
00072                 }
00073                 else {
00074                     if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
00075                     return Advanced;
00076                 }
00077             } else {
00078                 if (debug) std::cout << "trapped" << std::endl;
00079                 return Trapped;
00080             }
00081         }else{
00082             if (mobility->isReachable(minNode->position(), qRand)){
00083                 RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
00084                 tree->addNode(newNode);
00085                 tree->addEdge(minNode, newNode);
00086                 if (min <= eps_) {
00087                     if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
00088                     return Reached;
00089                 }
00090                 else {
00091                     if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
00092                     return Advanced;
00093                 }
00094             } else {
00095                 if (debug) std::cout << "trapped" << std::endl;
00096                 return Trapped;
00097             }
00098         }
00099     }
00100 
00101     return Trapped;
00102 }
00103 
00104 int RRT::connect(RoadmapPtr tree,const Configuration &qNew, bool reverse) {
00105     if (debug) std::cout << "RRT::connect(" << qNew << ")" << std::endl;
00106 
00107     int ret = Reached;
00108     Configuration q = qNew;
00109 
00110     do {
00111         ret = extend(tree, q, reverse);
00112         q = qNew;
00113     } while (ret == Advanced);
00114     return ret;
00115 }
00116 
00117 void RRT::extractPath() {
00118     extractPath(path_);
00119 }
00120 
00121 void RRT::extractPath(std::vector<Configuration>& o_path) {
00122     //std::cout << "RRT::path" << std::endl;
00123     RoadmapNodePtr startMidNode = Tstart_->lastAddedNode();
00124     RoadmapNodePtr goalMidNode  = Tgoal_ ->lastAddedNode();
00125 
00126     o_path.clear();
00127     if (!startMidNode || !goalMidNode) return;
00128 
00129     RoadmapNodePtr node;
00130 
00131     if (extendFromStart_){
00132         node = startMidNode;
00133         do {
00134             o_path.insert(o_path.begin(), node->position());
00135             node = node->parent(0);
00136         } while (node != NULL);
00137     }
00138 
00139     if (extendFromGoal_){
00140         // note: If trees are extend from both of start and goal,
00141         //       goalMidNode == startMidNode.
00142         node = extendFromStart_ ? goalMidNode->child(0) : goalMidNode;
00143         do {
00144             o_path.push_back(node->position());
00145             node = node->child(0);
00146         } while (node != NULL);
00147     }
00148 #if 0
00149     startMidNode->children_.push_back(goalMidNode);
00150     goalMidNode->parent_ = startMidNode;
00151 #endif
00152 }
00153 
00154 bool RRT::extendOneStep()
00155 {
00156     Configuration qNew = planner_->getConfigurationSpace()->random();
00157     if (extendFromStart_ && extendFromGoal_){
00158         
00159         if (extend(Ta_, qNew, Tb_ == Tstart_) != Trapped) {
00160             if (connect(Tb_, qNew, Ta_ == Tstart_) == Reached) {
00161                 if (extraConnectionCheckFunc_){
00162                     return extraConnectionCheckFunc_(Tstart_->lastAddedNode()->position());
00163                 }else{
00164                     return true;
00165                 }
00166             }
00167         }
00168         swapTrees();
00169     }else if (extendFromStart_ && !extendFromGoal_){
00170         if (extend(Tstart_, qNew) != Trapped) {
00171             Configuration p = goal_;
00172             int ret;
00173             do {
00174                 ret = extend(Tstart_, p);
00175                 p = goal_;
00176             }while (ret == Advanced);
00177             if (ret == Reached) return true;
00178         }
00179     }else if (!extendFromStart_ && extendFromGoal_){
00180         std::cout << "this case is not implemented" << std::endl;
00181         return false;
00182     }
00183     return false;
00184 }
00185 
00186 bool RRT::calcPath() 
00187 {
00188     if (verbose_) std::cout << "RRT::calcPath" << std::endl;
00189 
00190     // 回数
00191     times_ = atoi(properties_["max-trials"].c_str());
00192 
00193     // eps
00194     eps_ = atof(properties_["eps"].c_str());
00195 
00196     if (verbose_){
00197         std::cout << "times:" << times_ << std::endl;
00198         std::cout << "eps:" << eps_ << std::endl;
00199     }
00200 
00201     RoadmapNodePtr startNode = RoadmapNodePtr(new RoadmapNode(start_));
00202     RoadmapNodePtr goalNode  = RoadmapNodePtr(new RoadmapNode(goal_));
00203 
00204     Tstart_->addNode(startNode);
00205     Tgoal_ ->addNode(goalNode);
00206 
00207     bool isSucceed = false;
00208   
00209     for (int i=0; i<times_; i++) {
00210         //if (!isRunning_) break;
00211         if (verbose_){
00212             printf("%5d/%5dtrials : %5d/%5dnodes\r", i+1, times_, Tstart_->nNodes(),Tgoal_->nNodes());
00213             fflush(stdout);
00214         }
00215         if ((isSucceed = extendOneStep())) break;
00216     }
00217   
00218     extractPath();
00219     Tgoal_->integrate(Tstart_);
00220 
00221     if (verbose_) {
00222         std::cout << std::endl << "fin.(calcPath), retval = " << isSucceed << std::endl;
00223     }
00224     return isSucceed;
00225 }
00226 
00227 void RRT::swapTrees()
00228 {
00229     RoadmapPtr tmp = Ta_;
00230     Ta_ = Tb_;
00231     Tb_ = tmp;
00232 }
00233 
00234 void RRT::setForwardTree(RoadmapPtr tree) { 
00235     Tstart_ = Ta_ = tree;
00236 }
00237 
00238 void RRT::setBackwardTree(RoadmapPtr tree) { 
00239     Tgoal_ = Tb_ = tree;
00240 }
00241 
00242 void RRT::setExtraConnectionCheckFunc(extraConnectionCheckFunc i_func)
00243 {
00244     extraConnectionCheckFunc_ = i_func;
00245 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56