Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

rrt.C

Go to the documentation of this file.
00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
00006 // All Rights Reserved.
00007 // 
00008 // Permission to use, copy, and distribute this software and its 
00009 // documentation is hereby granted free of charge, provided that 
00010 // (1) it is not a component of a commercial product, and 
00011 // (2) this notice appears in all copies of the software and
00012 //     related documentation. 
00013 // 
00014 // Iowa State University and the author make no representations
00015 // about the suitability or fitness of this software for any purpose.  
00016 // It is provided "as is" without express or implied warranty.
00017 //----------------------------------------------------------------------
00018 
00019 #include <math.h>
00020 #include <stdio.h>
00021 
00022 #include "rrt.h"
00023 #include "defs.h"
00024 
00025 
00026 // *********************************************************************
00027 // *********************************************************************
00028 // CLASS:     RRT base class
00029 // 
00030 // *********************************************************************
00031 // *********************************************************************
00032 
00033 RRT::RRT(Problem *problem): IncrementalPlanner(problem) {
00034 
00035   UseANN = false;
00036 
00037   READ_PARAMETER_OR_DEFAULT(ConnectTimeLimit,INFINITY);
00038 
00039   Reset();
00040 }
00041 
00042 
00043 
00044 void RRT::Reset() {
00045   IncrementalPlanner::Reset();
00046 
00047   NumNodes = 1000;
00048 
00049   SatisfiedCount = 0;
00050   GoalDist = P->Metric(P->InitialState,P->GoalState);
00051   BestState = P->InitialState;
00052 
00053 #ifdef USE_ANN
00054   MAG = MultiANN(&G);
00055   MAG2 = MultiANN(&G2);
00056 #endif
00057 
00058 }
00059 
00060 
00061 // Return the best new state in nx_best
00062 // success will be false if no action is collision free
00063 MSLVector RRT::SelectInput(const MSLVector &x1, const MSLVector &x2, 
00064                            MSLVector &nx_best, bool &success,
00065                            bool forward = true)
00066 {
00067   MSLVector u_best,nx;
00068   list<MSLVector>::iterator u;
00069   double d,d_min;
00070   success = false;
00071   d_min = (forward) ? P->Metric(x1,x2) : P->Metric(x2,x1);
00072   list<MSLVector> il = P->GetInputs(x1);
00073 
00074   if (Holonomic) { // Just do interpolation
00075     u_best = P->InterpolateState(x1,x2,0.1) - x1;
00076     u_best = u_best.norm(); // Normalize the direction
00077     nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
00078     SatisfiedCount++;
00079     if (P->Satisfied(nx_best))
00080       success = true;
00081   }
00082   else {  // Nonholonomic (the more general case -- look at Inputs)
00083     forall(u,il) {
00084       if (forward)
00085         nx = P->Integrate(x1,*u,PlannerDeltaT);
00086       else
00087         nx = P->Integrate(x1,*u,-PlannerDeltaT);
00088       
00089       d  = (forward) ? P->Metric(nx,x2): P->Metric(x2,nx);
00090       
00091       SatisfiedCount++;
00092       
00093       if ((d < d_min)&&(x1 != nx)) { 
00094         if (P->Satisfied(nx)) {
00095           d_min = d; u_best = *u; nx_best = nx; success = true;
00096         }
00097       }
00098     }
00099   }
00100 
00101   //cout << "u_best: " << u_best << "\n";
00102   //cout << "nx_best: " << nx_best << "\n";
00103   //cout << "success: " << success << "\n";
00104   //success = true;
00105 
00106   return u_best;
00107 }
00108 
00109 
00110 
00111 
00112 MSLNode* RRT::SelectNode(const MSLVector &x, MSLTree* t,
00113                          bool forward = true) {
00114   double d,d_min;
00115   MSLNode *n_best;
00116   list<MSLNode*>::iterator n;
00117 
00118   d_min = INFINITY; d = 0.0;
00119 
00120 #ifdef USE_ANN
00121   if (!UseANN) {
00122 #endif
00123     list<MSLNode*> nl;
00124     nl = t->Nodes();
00125     forall(n,nl) {
00126       d = (forward) ? P->Metric((*n)->State(),x) : P->Metric(x,(*n)->State());
00127       if (d < d_min) {
00128         d_min = d; n_best = (*n); 
00129       }
00130     } 
00131 #ifdef USE_ANN
00132   }
00133   else {
00134     if (&g == &G)
00135       n_best = MAG.SelectNode(x);
00136     else
00137       n_best = MAG2.SelectNode(x);
00138   }
00139 #endif
00140 
00141   //cout << "n_best: " << (*n_best) << "\n";
00142   
00143   return n_best;
00144 }
00145 
00146 
00147 
00148 bool RRT::Extend(const MSLVector &x, 
00149                  MSLTree *t,
00150                  MSLNode *&nn, bool forward = true) {
00151   MSLNode *n_best;
00152   MSLVector nx,u_best;
00153   bool success;
00154   
00155   n_best = SelectNode(x,t,forward);
00156   u_best = SelectInput(n_best->State(),x,nx,success,forward);
00157   // nx gets next state
00158   if (success) {   // If a collision-free input was found
00159     // Extend the tree
00160     nn = t->Extend(n_best, nx, u_best, PlannerDeltaT);
00161 
00162     //cout << "n_best: " << n_best << "\n";
00163     //cout << "New node: " << nn << "\n";
00164   }
00165 
00166   return success;
00167 }
00168 
00169 
00170 
00171 // This function essentially iterates Extend until it has to stop
00172 // The same action is used for every iteration
00173 bool RRT::Connect(const MSLVector &x, 
00174                   MSLTree *t,
00175                   MSLNode *&nn, bool forward = true) {
00176   MSLNode *nn_prev,*n_best;
00177   MSLVector nx,nx_prev,u_best;
00178   bool success;
00179   double d,d_prev,clock;
00180   int steps;
00181   
00182   n_best = SelectNode(x,t,forward);
00183   u_best = SelectInput(n_best->State(),x,nx,success,forward); 
00184   steps = 0;
00185            // nx gets next state
00186   if (success) {   // If a collision-free input was found
00187     d = P->Metric(nx,x); d_prev = d;
00188     nx_prev = nx; // Initialize
00189     nn = n_best;
00190     clock = PlannerDeltaT;
00191     while ((P->Satisfied(nx))&&
00192            (clock <= ConnectTimeLimit)&&
00193            (d <= d_prev))
00194       {
00195         SatisfiedCount++;
00196         steps++; // Number of steps made in connecting
00197         nx_prev = nx;
00198         d_prev = d; nn_prev = nn;
00199         // Uncomment line below to select best action each time
00200         //u_best = SelectInput(g.inf(nn),x,nx,success,forward); 
00201         if (Holonomic) { 
00202             nx = P->Integrate(nx_prev,u_best,PlannerDeltaT);
00203         }
00204         else { // Nonholonomic
00205           if (forward)
00206             nx = P->Integrate(nx_prev,u_best,PlannerDeltaT);
00207           else
00208             nx = P->Integrate(nx_prev,u_best,-PlannerDeltaT);
00209         }
00210         d = P->Metric(nx,x);
00211         clock += PlannerDeltaT;
00212         // Uncomment the subsequent two lines to
00213         //   make each intermediate node added
00214         //nn = g.new_node(nx_prev); // Make a new node
00215         //g.new_edge(nn_prev,nn,u_best);
00216       }
00217     nn = t->Extend(n_best, nx_prev, u_best, steps*PlannerDeltaT);
00218 
00219   }
00220 
00221   return success;
00222 }
00223 
00224 
00225 
00226 bool RRT::Plan()
00227 {
00228   int i;
00229   double d;
00230   MSLNode *n,*nn,*n_goal;
00231   MSLVector nx,u_best;
00232   list<MSLNode*> path;
00233 
00234   // Keep track of time
00235   float t = used_time();
00236 
00237   // Make the root node of G
00238   if (!T)
00239     T = new MSLTree(P->InitialState);
00240 
00241   nn = T->Root();
00242 
00243   i = 0;
00244   n = SelectNode(P->GoalState,T);
00245   n_goal = n;
00246 
00247   GoalDist = P->Metric(n->State(),P->GoalState);
00248   while ((i < NumNodes)&&(!GapSatisfied(n_goal->State(),P->GoalState))) {
00249     if (Extend(ChooseState(),T,nn)) { 
00250       d = P->Metric(nn->State(),P->GoalState);
00251       if (d < GoalDist) {  // Decrease if goal closer
00252         GoalDist = d;
00253         BestState = nn->State();
00254         n_goal = nn;
00255         //cout << "GoalDist " << GoalDist << "\n";
00256       }
00257     }
00258     i++;
00259   }
00260 
00261   CumulativePlanningTime += ((double)used_time(t));
00262   cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 
00263 
00264   // Get the solution path
00265   if (GapSatisfied(n_goal->State(),P->GoalState)) {
00266     cout << "Success\n";
00267     path = T->PathToRoot(n_goal);
00268     path.reverse();
00269     RecordSolution(path); // Write to Path and Policy
00270     return true;
00271   }
00272   else {
00273     cout << "Failure\n";
00274     return false;
00275   }
00276 }
00277 
00278 
00279 
00280 MSLVector RRT::ChooseState() {
00281   return RandomState();
00282 }
00283 
00284 
00285 
00286 
00287 // *********************************************************************
00288 // *********************************************************************
00289 // CLASS:     RRTGoalBias
00290 //
00291 // Occasionally pick the goal state instead of a random sample. 
00292 // *********************************************************************
00293 // *********************************************************************
00294 
00295 
00296 RRTGoalBias::RRTGoalBias(Problem *p):RRT(p) {
00297   READ_PARAMETER_OR_DEFAULT(GoalProb,0.05);
00298 }
00299 
00300 
00301 
00302 MSLVector RRTGoalBias::ChooseState()
00303 {
00304   double rv;
00305 
00306   R >> rv;
00307   if (rv > GoalProb)
00308     return RandomState();
00309   else
00310     return P->GoalState;
00311 }
00312 
00313 
00314 
00315 // *********************************************************************
00316 // *********************************************************************
00317 // CLASS:     RRTCon
00318 // 
00319 // Make the regular RRT greedy by attempting to connect all the way
00320 // to the random sample, or go as far as possible before colliding.
00321 // *********************************************************************
00322 // *********************************************************************
00323 
00324 
00325 // Build an RRT that tries to go as far as it can for each edge
00326 RRTCon::RRTCon(Problem *p):RRTGoalBias(p) {
00327   READ_PARAMETER_OR_DEFAULT(GoalProb,0.0);
00328 }
00329 
00330 
00331 
00332 
00333 bool RRTCon::Plan()
00334 {
00335   int i;
00336   double d;
00337   MSLNode *n,*nn,*n_goal;
00338   MSLVector nx,u_best;
00339   list<MSLNode*> path;
00340 
00341   // Keep track of time
00342   float t = used_time();
00343 
00344   // Make the root node of G
00345   if (!T)
00346     T = new MSLTree(P->InitialState);
00347 
00348 
00349   i = 0;
00350   n = SelectNode(P->GoalState,T);
00351   n_goal = n;
00352 
00353   GoalDist = P->Metric(n->State(),P->GoalState);
00354   while ((i < NumNodes)&&(!GapSatisfied(n_goal->State(),P->GoalState))) {
00355     if (Connect(ChooseState(),T,nn)) { 
00356       d = P->Metric(nn->State(),P->GoalState);
00357       if (d < GoalDist) {  // Decrease if goal closer
00358         GoalDist = d;
00359         BestState = nn->State();
00360         n_goal = nn;
00361         //cout << "GoalDist " << GoalDist << "\n";
00362       }
00363     }
00364     i++;
00365   }
00366 
00367   CumulativePlanningTime += ((double)used_time(t));
00368   cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 
00369 
00370   // Get the solution path
00371   if (GapSatisfied(n_goal->State(),P->GoalState)) {
00372     cout << "Successful Path Found\n";
00373     path = T->PathToRoot(n_goal);
00374     path.reverse();
00375     RecordSolution(path); // Write to Path and Policy
00376     return true;
00377   }
00378   else {
00379     cout << "Failure to connect after " << 
00380       T->Size() << " nodes\n";
00381     cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00382     
00383     return false;
00384   }
00385 }
00386 
00387 
00388 // *********************************************************************
00389 // *********************************************************************
00390 // CLASS:     RRTDual
00391 // 
00392 // The dual-tree planner used in LaValle, Kuffner, ICRA 1999.  Each 
00393 // tree is extended toward a randomly-sampled point.  RRTExtExt is
00394 // generally better.
00395 // *********************************************************************
00396 // *********************************************************************
00397 
00398 RRTDual::RRTDual(Problem *p):RRT(p) {
00399 }
00400 
00401 
00402 void RRTDual::RecoverSolution(MSLNode *n1, MSLNode *n2) {
00403   list<MSLNode*> path,path2;
00404 
00405   path = T->PathToRoot(n1);
00406   path.reverse();
00407 
00408   path2 = T2->PathToRoot(n2); 
00409   
00410   //cout << "Path: " << path;
00411   //cout << "Path2: " << path2;
00412 
00413   RecordSolution(path,path2); // Write to Path and Policy
00414 }
00415 
00416 
00417 
00418 bool RRTDual::Plan()
00419 {
00420   int i;
00421   MSLVector rx,u_best,nx;
00422   MSLNode *nn,*nn2;
00423   bool connected;
00424 
00425   connected = false;
00426 
00427   float t = used_time();
00428 
00429   //int total = 0;
00430   //for (int j=0; j < 100; j++) {
00431 
00432   if (!T)
00433     T = new MSLTree(P->InitialState);
00434   if (!T2)
00435     T2 = new MSLTree(P->GoalState);
00436  
00437   nn = T->Root();
00438   nn2 = T2->Root();
00439 
00440   i = 0;
00441   connected = false;
00442   while ((i < NumNodes) && (!connected)) {
00443     rx = ChooseState();
00444     Extend(rx,T,nn);
00445     Extend(rx,T2,nn2,false);  // false means reverse-time integrate
00446 
00447     if (GapSatisfied(nn->State(),nn2->State())) {
00448       cout << "CONNECTED!!  MSLNodes: " << 
00449         T->Size() + T2->Size() << "\n";
00450       connected = true;
00451       RecoverSolution(nn,nn2);
00452     }
00453 
00454     i++;
00455   }
00456   
00457   if (!connected)
00458     cout << "Failure to connect after " << 
00459       T->Size()+T2->Size() 
00460          << " nodes\n";
00461   
00462   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00463   
00464   //total += i;
00465   // }
00466   //cout << "Avg: " << total/100 << "\n";
00467 
00468   CumulativePlanningTime += ((double)used_time(t));
00469   cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 
00470   
00471   return connected;
00472 }
00473 
00474 
00475 
00476 
00477 
00478 // *********************************************************************
00479 // *********************************************************************
00480 // CLASS:     RRTExtExt
00481 // 
00482 // A dual-tree planner in which computation is balanced between 
00483 // exploring and connecting trees to each other.  Greedier verions
00484 // of this are RRTExtCon and RRTConCon.
00485 // *********************************************************************
00486 // *********************************************************************
00487 
00488 RRTExtExt::RRTExtExt(Problem *p):RRTDual(p) {
00489 }
00490 
00491 
00492 bool RRTExtExt::Plan()
00493 {
00494   int i;
00495   MSLVector u_best,nx,nx2;
00496   MSLNode *nn,*nn2;
00497   bool connected;
00498 
00499   connected = false;
00500 
00501   float t = used_time();
00502 
00503   //int total = 0;
00504   //for (int j=0; j < 100; j++) {
00505   
00506   if (!T)
00507     T = new MSLTree(P->InitialState);
00508   if (!T2)
00509     T2 = new MSLTree(P->GoalState);
00510 
00511   nn = T->Root();
00512   nn2 = T2->Root();
00513 
00514   i = 0;
00515   connected = false;
00516   while ((i < NumNodes) && (!connected)) {
00517     if (Extend(ChooseState(),T,nn)) {
00518       if (Extend(nn->State(),T2,nn2,false)) {
00519         i++;
00520         if (GapSatisfied(nn->State(),nn2->State())) {
00521           cout << "CONNECTED!!  MSLNodes: " << 
00522             T->Size() + T2->Size() << "\n";
00523           connected = true;
00524           RecoverSolution(nn,nn2);
00525         }
00526       }
00527     }
00528     i++;
00529 
00530     if ((!connected) && (Extend(ChooseState(),T2,nn,false))) {
00531       if (Extend(nn->State(),T,nn2)) {
00532         i++;
00533         if (GapSatisfied(nn2->State(),nn->State())) {
00534           cout << "CONNECTED!!  MSLNodes: " << 
00535             T->Size() + T2->Size();
00536           connected = true;
00537           RecoverSolution(nn2,nn);
00538         }
00539       }
00540     }
00541   }
00542 
00543   i++;
00544 
00545   if (!connected)
00546     cout << "Failure to connect after " << 
00547       T->Size() + T2->Size()
00548          << " nodes\n";
00549 
00550   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00551 
00552   //total += i;
00553   // }
00554   //cout << "Avg: " << total/100 << "\n";
00555   
00556   CumulativePlanningTime += ((double)used_time(t));
00557   cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 
00558 
00559   return connected;
00560 }
00561 
00562 
00563 
00564 
00565 // *********************************************************************
00566 // *********************************************************************
00567 // CLASS:     RRTGoalZoom
00568 // 
00569 // Bias sampling towards a region that contains the goal.  The 
00570 // region shrinks around the goal as the RRT comes nearer.  This
00571 // planner is based on a class project at Iowa State by Jun Qu in 1999.
00572 // *********************************************************************
00573 // *********************************************************************
00574 
00575 RRTGoalZoom::RRTGoalZoom(Problem *p):RRT(p) {
00576   GoalProb = 0.05;
00577   ZoomProb = 0.5;
00578   ZoomFactor = 2.0;
00579 }
00580 
00581 
00582 // Zoom using a square box
00583 MSLVector RRTGoalZoom::ChooseState()
00584 {
00585   double rv,r,diff;
00586   MSLVector zx;
00587   int i;
00588 
00589   R >> rv;
00590   diff = 0.0;
00591   zx = P->LowerState;  // Initialize to anything
00592 
00593   if (rv < GoalProb)
00594     return P->GoalState;
00595   else {
00596     if (rv < (ZoomProb+GoalProb)) {
00597       for (i = 0; i < P->GoalState.dim(); i++) {
00598         if (fabs(P->GoalState[i] - BestState[i]) > diff)
00599           diff = fabs(P->GoalState[i] - BestState[i]);
00600       }
00601       for (i = 0; i < P->GoalState.dim(); i++) {
00602         R >> r;
00603         zx[i] += P->GoalState[i] - diff + 2.0*r*ZoomFactor*diff;
00604       }
00605       return zx;
00606     }
00607     else
00608       return RandomState();
00609   }
00610   
00611 }
00612 
00613 
00614 // *********************************************************************
00615 // *********************************************************************
00616 // CLASS:     RRTPolar
00617 // 
00618 // Instead of random sampling, attempt to gradually bias samples
00619 // toward the goal.
00620 // *********************************************************************
00621 // *********************************************************************
00622 
00623 RRTPolar::RRTPolar(Problem *p):RRT(p) {
00624   // RadiusExp = 1.0/(P->InitialState.dim() - 1);
00625   RadiusExp = 1.0;
00626 }
00627 
00628 
00629 
00630 
00631 
00632 
00633 // This implementation ignores C-space topology, VERY BAD!
00634 MSLVector RRTPolar::ChooseState()
00635 {
00636   double r,w;
00637   MSLVector zx;
00638   int i,j;
00639   bool success;
00640   
00641   w = 0.0;
00642   zx = P->GoalState;  // Initialize to anything
00643 
00644   success = false;
00645 
00646   while (!success) {
00647     for (i = 0; i < P->GoalState.dim(); i++) {
00648       // Generate sample from N(0,1)
00649       zx[i] = 0.0;
00650       for (j = 0; j < 12; j++) {
00651         R >> r; zx[i] += r;
00652       }
00653       zx[i] -= 6.0;
00654       w += zx[i]*zx[i];
00655     }
00656     
00657     w = sqrt(w);
00658     
00659     //cout << "RadiusExp: " << RadiusExp;
00660     
00661     R >> r;  // Radius
00662     r = pow(r,RadiusExp);
00663     for (i = 0; i < P->GoalState.dim(); i++) {
00664       zx[i] = (P->UpperState[i] - P->LowerState[i])*
00665         sqrt(P->GoalState.dim())*r*zx[i]/w + 
00666         P->GoalState[i];
00667     }
00668     
00669     // Check if sample is within bounds
00670     success = true;
00671     for (i = 0; i < P->GoalState.dim(); i++) {
00672       if ((zx[i] >= P->UpperState[i])||(zx[i] <= P->LowerState[i]))
00673         success = false;
00674     }
00675   }
00676 
00677   return zx;
00678 }
00679 
00680 
00681 
00682 MSLVector RRTPolar::SelectInput(const MSLVector &x1, const MSLVector &x2, 
00683                                  MSLVector &nx_best, bool &success)
00684 {
00685   MSLVector u_best,nx;
00686   list<MSLVector>::iterator u;
00687   double d,dg,dmax,d_min;
00688   success = false;
00689   d_min = INFINITY;
00690   dg  = P->Metric(x1,x2);
00691   dmax  = P->Metric(P->LowerState,P->UpperState);
00692   list<MSLVector> il = P->GetInputs(x1);
00693   forall(u,il) {
00694     //nx = P->Integrate(x1,u,PlannerDeltaT*sqrt(dg/dmax));  // Slow down near goal
00695     nx = P->Integrate(x1,*u,PlannerDeltaT);
00696     d  = P->Metric(nx,x2);
00697     if ((d < d_min)&&(P->Satisfied(nx))&&(x1 != nx)) 
00698       {
00699         d_min = d; u_best = *u; nx_best = nx; success = true;
00700       }
00701   }
00702   
00703   //cout << "u_best: " << u_best << "\n");
00704 
00705   return u_best;
00706 }
00707 
00708 
00709 
00710 // *********************************************************************
00711 // *********************************************************************
00712 // CLASS:     RRTHull
00713 // 
00714 // A simple exploration of what happens to the RRT in a large disc.
00715 // Only works for 2DPoint model!!!!!
00716 // *********************************************************************
00717 // *********************************************************************
00718 
00719 RRTHull::RRTHull(Problem *p):RRT(p) {
00720   Radius = 100000000.0;
00721 }
00722 
00723 
00724 
00725 MSLVector RRTHull::ChooseState() {
00726   double theta;
00727   MSLVector v(2);
00728 
00729   R >> theta;  theta *= 2.0*PI;
00730 
00731   v[0] = Radius*cos(theta);
00732   v[1] = Radius*sin(theta);
00733 
00734   return v;
00735 }
00736 
00737 
00738 
00739 // *********************************************************************
00740 // *********************************************************************
00741 // CLASS:     RRTExtCon
00742 // 
00743 // The planner presented in Kuffner, LaValle, 2000.  There are two
00744 // trees, and the attempt to connect them is greedy.
00745 // *********************************************************************
00746 // *********************************************************************
00747 
00748 RRTExtCon::RRTExtCon(Problem *p):RRTDual(p) {
00749 }
00750 
00751 
00752 
00753 bool RRTExtCon::Plan()
00754 {
00755   int i;
00756   MSLVector nx,nx_prev;
00757   MSLNode *nn,*nn2;
00758   bool connected;
00759 
00760   connected = false;
00761 
00762   float t = used_time();
00763 
00764   if (!T)
00765     T = new MSLTree(P->InitialState);
00766   if (!T2)
00767     T2 = new MSLTree(P->GoalState);
00768 
00769   nn = T->Root();
00770   nn2 = T2->Root();
00771 
00772   i = 0;
00773   connected = false;
00774   while ((i < NumNodes) && (!connected)) {
00775     if (Extend(ChooseState(),T,nn)) { 
00776       // Update the goal RRT
00777       if (Connect(nn->State(),T2,nn2,false)) {
00778         if (GapSatisfied(nn->State(),nn2->State())) {
00779           cout << "CONNECTED!!  MSLNodes: " << 
00780             T->Size() + T2->Size()
00781                << "\n";
00782           connected = true;
00783           RecoverSolution(nn,nn2);
00784         }
00785       }
00786     }
00787     i++;
00788 
00789     if ((!connected)&&(Extend(ChooseState(),T2,nn,false))) { 
00790       // Update the initial RRT
00791       if (Connect(nn->State(),T,nn2)) { 
00792         if (GapSatisfied(nn->State(),nn2->State())) {
00793           cout << "CONNECTED!!  MSLNodes: " << 
00794             T->Size() + T2->Size() << "\n";
00795           connected = true;
00796           RecoverSolution(nn2,nn);
00797         }
00798       }
00799     }
00800     i++;
00801   }
00802 
00803   if (!connected)
00804     cout << "Failure to connect after " << T->Size() + T2->Size()
00805          << " nodes\n";
00806 
00807   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00808 
00809   CumulativePlanningTime += ((double)used_time(t));
00810   cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 
00811 
00812   return connected;
00813 }
00814 
00815 
00816 
00817 // *********************************************************************
00818 // *********************************************************************
00819 // CLASS:     RRTConCon
00820 // 
00821 // One the best dual-tree planners.  Be greedy in growing the trees
00822 // and in trying to connect them to each other.  This is usually the
00823 // fastest planner, but there is often a price for being greedy...
00824 // *********************************************************************
00825 // *********************************************************************
00826 
00827 RRTConCon::RRTConCon(Problem *p):RRTDual(p) {
00828 }
00829 
00830 
00831 
00832 bool RRTConCon::Plan()
00833 {
00834   int i;
00835   MSLVector nx,nx_prev;
00836   MSLNode *nn,*nn2;
00837   bool connected;
00838 
00839   connected = false;
00840 
00841   // Keep track of time
00842   float t = used_time();
00843 
00844   if (!T)
00845     T = new MSLTree(P->InitialState);
00846   if (!T2)
00847     T2 = new MSLTree(P->GoalState);
00848  
00849   nn = T->Root();
00850   nn2 = T2->Root();
00851 
00852   i = 0;
00853   connected = false;
00854 
00855   while ((i < NumNodes) && (!connected)) {
00856     if (Connect(ChooseState(),T,nn)) { 
00857       // Update the goal RRT
00858       //cout << "nn: " << nn->State() << "  nn2: " << nn2->State() << "\n";
00859       if (Connect(nn->State(),T2,nn2,false)) {
00860         if (GapSatisfied(nn->State(),nn2->State())) {
00861           cout << "CONNECTED!!  MSLNodes: " << 
00862             T->Size()+T2->Size() << "\n";
00863           connected = true;
00864           RecoverSolution(nn,nn2); // Defined in RRTDual
00865         }
00866       }
00867     }
00868     i++;
00869 
00870     if ((!connected)&&(Connect(ChooseState(),T2,nn,false))) { 
00871       // Update the initial RRT
00872       if (Connect(nn->State(),T,nn2)) { 
00873         if (GapSatisfied(nn->State(),nn2->State())) {
00874           cout << "CONNECTED!!  MSLNodes: " << 
00875             T->Size()+T2->Size() << "\n";
00876           connected = true;
00877           RecoverSolution(nn2,nn);
00878         }
00879       }
00880     }
00881     i++;
00882   }
00883 
00884   if (!connected)
00885     cout << "Failure to connect after " << 
00886       T->Size()+T2->Size() << " nodes\n";
00887 
00888   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00889 
00890   CumulativePlanningTime += ((double)used_time(t));
00891   cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 
00892 
00893   return connected;
00894 }
00895 
00896 
00897 
00898 // *********************************************************************
00899 // *********************************************************************
00900 // CLASS:     RandomTree
00901 // 
00902 // Technically not an RRT.  Pick a vertex and input at random.
00903 // *********************************************************************
00904 // *********************************************************************
00905 
00906 RandomTree::RandomTree(Problem *p):RRT(p) {
00907 }
00908 
00909 
00910 MSLNode* RandomTree::SelectNode(const MSLVector &x, MSLTree *t,
00911                                 bool forward = true) {
00912   cout << "WARNING: RESULT NOT CORRECT!!!\n";
00913   cout << "NEED TO FIX RandomTree::SelectNode\n";
00914 
00915   //return g.choose_node();
00916   return t->Root();
00917 }
00918 
00919 
00920 MSLVector RandomTree::SelectInput(const MSLVector &x1, const MSLVector &x2, 
00921                                   MSLVector &nx_best, bool &success,
00922                                   bool forward = true) {
00923   double r;
00924   list<MSLVector> il;
00925   int k;
00926   MSLVector u_best;
00927 
00928   il = P->GetInputs(x1);
00929   R >> r;
00930 
00931   k = (int) (r * il.size());
00932 
00933   cout << "WARNING: RESULT NOT CORRECT!!!\n";
00934   cout << "NEED TO FIX RandomTree::SelectInput\n";
00935   //u_best = il.inf(il[k]);
00936   u_best = il.front();
00937 
00938   nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
00939   SatisfiedCount++;
00940 
00941   return u_best;
00942   
00943 }
00944 
00945 
00946 
00947 
00948 
00949 
00950 
00951 
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.