Go to the documentation of this file.00001
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
00011
00012 RRT::RRT(PathPlanner* plan) : Algorithm(plan), extraConnectionCheckFunc_(NULL)
00013 {
00014
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
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
00141
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
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
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 }