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 #include <LEDA/REDEFINE_NAMES.h>
00026 
00027 
00028 // *********************************************************************
00029 // *********************************************************************
00030 // CLASS:     RRT base class
00031 // 
00032 // *********************************************************************
00033 // *********************************************************************
00034 
00035 RRT::RRT(Problem *problem): Planner(problem) {
00036 
00037   UseANN = false;
00038   NumNodes = 1000;
00039 
00040   if (is_file(P->FilePath+"ConnectTimeLimit")) {
00041     file_istream fin(P->FilePath + "ConnectTimeLimit");
00042     fin >> ConnectTimeLimit;
00043     fin.close();
00044   }
00045   else {
00046     ConnectTimeLimit = INFINITY; // Go "all the way" in Connect
00047   }
00048 
00049   Reset();
00050 }
00051 
00052 
00053 
00054 void RRT::Reset() {
00055   Planner::Reset();
00056 
00057   SatisfiedCount = 0;
00058   GoalDist = P->Metric(P->InitialState,P->GoalState);
00059   BestState = P->InitialState;
00060 
00061   GEdgeTime.init(G,PlannerDeltaT);
00062   G2EdgeTime.init(G2,PlannerDeltaT);
00063 
00064 #ifdef USE_ANN
00065   MAG = MultiANN(&G);
00066   MAG2 = MultiANN(&G2);
00067 #endif
00068 
00069 }
00070 
00071 
00072 // Return the best new state in nx_best
00073 // success will be false if no action is collision free
00074 vector RRT::SelectBestInput(const vector &x1, const vector &x2, 
00075                             vector &nx_best, bool &success,
00076                             bool forward = true)
00077 {
00078   vector u,u_best,nx;
00079   double d,d_min;
00080   success = false;
00081   d_min = (forward) ? P->Metric(x1,x2) : P->Metric(x2,x1);
00082   list<vector> il = P->GetInputs(x1);
00083 
00084   if (Holonomic) { // Just do interpolation
00085     u_best = P->InterpolateState(x1,x2,0.1) - x1;
00086     u_best = u_best.norm(); // Normalize the direction
00087     nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
00088     SatisfiedCount++;
00089     if (P->Satisfied(nx_best))
00090       success = true;
00091   }
00092   else {  // Nonholonomic (the more general case -- look at Inputs)
00093     forall(u,il) {
00094       if (forward)
00095         nx = P->Integrate(x1,u,PlannerDeltaT);
00096       else
00097         nx = P->Integrate(x1,u,-PlannerDeltaT);
00098       
00099       d  = (forward) ? P->Metric(nx,x2): P->Metric(x2,nx);
00100       
00101       SatisfiedCount++;
00102       
00103       if ((d < d_min)&&(x1 != nx)) { 
00104         if (P->Satisfied(nx)) {
00105           d_min = d; u_best = u; nx_best = nx; success = true;
00106         }
00107       }
00108     }
00109   }
00110 
00111   // Uncomment below for random inputs!
00112   //int n = il.length();
00113   //R >> d;
00114   //u_best = il.inf(il.get_item((int) (d*n)));
00115   //nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
00116     
00117   //cout << "u_best: " << u_best << "\n";
00118 
00119   return u_best;
00120 }
00121 
00122 
00123 
00124 vector RRT::SelectRandomInput(const vector &x1, const vector &x2, 
00125                             vector &nx_best, bool &success,
00126                             bool forward = true)
00127 {
00128   vector u,u_best;
00129   double d;
00130   success = false;
00131   int i,n;
00132   
00133   i = 0;
00134   list<vector> il = P->GetInputs(x1);
00135   n = il.length();
00136   while ((i < n)&&(!success)) {
00137     R >> d;
00138     u_best = il.inf(il.get_item((int) (d*n)));
00139     nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
00140     if (P->Satisfied(nx_best))
00141       success = true;
00142     i++;
00143   }
00144 
00145   return u_best;
00146 }
00147 
00148 
00149 
00150 node RRT::NearestNeighbor(const vector &x, const GRAPH<vector,vector> &g,
00151                           bool forward = true) {
00152   double d,d_min;
00153   node n,n_best;
00154   
00155   n_best = n = g.first_node(); // Keeps the warnings away
00156   d_min = INFINITY; d = 0.0;
00157 
00158 #ifdef USE_ANN
00159   if (!UseANN) {
00160 #endif
00161     forall_nodes(n,g) {
00162       d = (forward) ? P->Metric(g[n],x) : P->Metric(x,g[n]);
00163       if (d < d_min) {
00164         d_min = d; n_best = n; 
00165       }
00166     } 
00167 #ifdef USE_ANN
00168   }
00169   else {
00170     if (&g == &G)
00171       n_best = MAG.NearestNeighbor(x);
00172     else
00173       n_best = MAG2.NearestNeighbor(x);
00174   }
00175 #endif
00176 
00177   //cout << "n_best: " << n_best << "info: " << g.inf(n_best) << "\n";
00178   
00179   //n_best =  g.choose_node(); // Pick a node at random
00180 
00181   return n_best;
00182 }
00183 
00184 
00185 
00186 bool RRT::Extend(const vector &x, 
00187                  GRAPH<vector,vector> &g,
00188                  node &nn, bool forward = true) {
00189   node n_best;
00190   vector nx,u_best;
00191   bool success;
00192   
00193   n_best = NearestNeighbor(x,g,forward);
00194   u_best = SelectBestInput(g.inf(n_best),x,nx,success,forward);
00195   // nx gets next state
00196   if (success) {   // If a collision-free input was found
00197     nn = g.new_node(nx); // Make a new node with state nx
00198     g.new_edge(n_best,nn,u_best);
00199     //cout << "New node: " << g.inf(nn) << "   " << "u:" << u_best << "\n";
00200   }
00201 
00202   return success;
00203 }
00204 
00205 
00206 
00207 // This function essentially iterates Extend until it has to stop
00208 // The same action is used for every iteration
00209 bool RRT::Connect(const vector &x, 
00210                   GRAPH<vector,vector> &g,
00211                   node &nn, bool forward = true) {
00212   node nn_prev,n_best;
00213   vector nx,nx_prev,u_best;
00214   bool success;
00215   double d,d_prev,clock;
00216   int steps;
00217   
00218   n_best = NearestNeighbor(x,g,forward);
00219   u_best = SelectBestInput(g.inf(n_best),x,nx,success,forward); 
00220   steps = 0;
00221            // nx gets next state
00222   if (success) {   // If a collision-free input was found
00223     d = P->Metric(nx,x); d_prev = d;
00224     nx_prev = nx; // Initialize
00225     nn = n_best;
00226     clock = PlannerDeltaT;
00227     while ((P->Satisfied(nx))&&
00228            (clock <= ConnectTimeLimit)&&
00229            (d <= d_prev))
00230       {
00231         SatisfiedCount++;
00232         steps++; // Number of steps made in connecting
00233         nx_prev = nx;
00234         d_prev = d; nn_prev = nn;
00235         // Uncomment line below to select best action each time
00236         //u_best = SelectBestInput(g.inf(nn),x,nx,success,forward); 
00237         if (Holonomic) { 
00238             nx = P->Integrate(nx_prev,u_best,PlannerDeltaT);
00239         }
00240         else { // Nonholonomic
00241           if (forward)
00242             nx = P->Integrate(nx_prev,u_best,PlannerDeltaT);
00243           else
00244             nx = P->Integrate(nx_prev,u_best,-PlannerDeltaT);
00245         }
00246         d = P->Metric(nx,x);
00247         clock += PlannerDeltaT;
00248         // Uncomment the subsequent two lines to
00249         //   make each intermediate node added
00250         //nn = g.new_node(nx_prev); // Make a new node
00251         //g.new_edge(nn_prev,nn,u_best);
00252       }
00253     nn = g.new_node(nx_prev); // Make a new node
00254     g.new_edge(n_best,nn,u_best);
00255     if (&g == &G)
00256       GEdgeTime[nn] = steps*PlannerDeltaT;
00257     else
00258       G2EdgeTime[nn] = steps*PlannerDeltaT;
00259   }
00260 
00261   return success;
00262 }
00263 
00264 
00265 // Like RRT::Extend, except try ALL inputs
00266 bool RRT::Project(const vector &x, 
00267                  GRAPH<vector,vector> &g,
00268                  node &nn, bool forward = true) {
00269   node n_best;
00270   vector nx,x1,u;
00271   bool success;
00272   
00273   n_best = NearestNeighbor(x,g,forward);
00274   x1 = g.inf(n_best);
00275   list<vector> il = P->GetInputs(x1);
00276   success = false;
00277   forall(u,il) {
00278     if (forward)
00279       nx = P->Integrate(x1,u,PlannerDeltaT);
00280     else
00281       nx = P->Integrate(x1,u,-PlannerDeltaT); 
00282     if (P->Satisfied(nx)) {   // If a collision-free input was found
00283       nn = g.new_node(nx); // Make a new node with state nx
00284       g.new_edge(n_best,nn,u);
00285       success = true;
00286       //cout << "New node: " << g.inf(nn) << "   " << "u:" << u << "\n";
00287     }
00288   } 
00289 
00290   return success;
00291 }
00292 
00293 
00294 
00295 // Edges of G are input vectors, and vertices are states
00296 // Note that this method also adds to an existing RRT
00297 void RRT::Construct()
00298 {
00299   int i;
00300   vector u_best,nx;
00301   node nn;
00302   double d;
00303 
00304   // Make the root node of G
00305   if (G.number_of_nodes() == 0) {
00306     nn = G.new_node(P->InitialState);
00307     GEdgeTime[nn] = 0.0;
00308   }
00309 
00310   for (i=0; i < NumNodes; i++) {
00311     if (Extend(ChooseState(),G,nn)) { 
00312       d = P->Metric(G.inf(nn),P->GoalState);
00313       if (d < GoalDist)  {  // Decrease if goal closer
00314         GoalDist = d;
00315         BestState = G.inf(nn);
00316       }
00317       if (G.number_of_nodes() % 1000 == 0)
00318         cout << G.number_of_nodes() << " nodes in the RRT.\n";
00319     }
00320   }
00321 
00322 }
00323 
00324 
00325 bool RRT::Plan()
00326 {
00327   int i;
00328   double d;
00329   node n,nn,n_goal;
00330   vector nx,u_best;
00331   double ptime;
00332   list<node> path;
00333 
00334   // Make the root node of G
00335   if (G.number_of_nodes() == 0) {
00336     n = G.new_node(P->InitialState);
00337     GEdgeTime[n] = 0.0;
00338   }
00339 
00340   i = 0;
00341   n = NearestNeighbor(P->GoalState,G);
00342   n_goal = n;
00343 
00344   GoalDist = P->Metric(G.inf(n),P->GoalState);
00345   while ((i < NumNodes)&&(!GapSatisfied(G.inf(n_goal),P->GoalState))) {
00346     if (Extend(ChooseState(),G,nn)) { 
00347       d = P->Metric(G.inf(nn),P->GoalState);
00348       if (d < GoalDist) {  // Decrease if goal closer
00349         GoalDist = d;
00350         BestState = G.inf(nn);
00351         n_goal = nn;
00352         //cout << "GoalDist " << GoalDist << "\n";
00353       }
00354     }
00355     i++;
00356   }
00357 
00358   // Get the solution path
00359   if (GapSatisfied(G.inf(n_goal),P->GoalState)) {
00360     cout << "Successful Path Found\n";
00361     path = PathToLeaf(n_goal,G);
00362     // Make the correct times
00363     ptime = 0.0; TimeList.clear();
00364     forall(n,path) {
00365       ptime += GEdgeTime[n];
00366       TimeList.push_back(ptime);
00367     }
00368     RecordSolution(path); // Write to Path and Policy
00369     return true;
00370   }
00371   else
00372     return false;
00373 }
00374 
00375 
00376 
00377 vector RRT::ChooseState() {
00378   return RandomState();
00379 }
00380 
00381 
00382 // The algorithm actually starts at the leaf and stops at the root
00383 list<node> RRT::PathToLeaf(const node &n, const GRAPH<vector,vector> &g) 
00384 {
00385    list<node> path;
00386    node ni;
00387    int i;
00388 
00389    path.clear();
00390    path.push(n); 
00391 
00392    i = 0;
00393    ni = n;
00394    while ((ni != g.first_node())&&(i < g.number_of_nodes())) {
00395      path.push(g.source(g.first_in_edge(ni)));
00396      ni = g.source(g.first_in_edge(ni));
00397      i++;
00398    }
00399 
00400    return path;
00401 }
00402 
00403 
00404 
00405 // *********************************************************************
00406 // *********************************************************************
00407 // CLASS:     RRTGoalBias
00408 //
00409 // Occasionally pick the goal state instead of a random sample. 
00410 // *********************************************************************
00411 // *********************************************************************
00412 
00413 
00414 RRTGoalBias::RRTGoalBias(Problem *p):RRT(p) {
00415   if (is_file(P->FilePath+"GoalProb")) {
00416     file_istream fin(P->FilePath + "GoalProb");
00417     fin >> GoalProb;
00418     fin.close();
00419   }
00420   else {
00421     GoalProb = 0.05;
00422   }
00423 }
00424 
00425 
00426 
00427 vector RRTGoalBias::ChooseState()
00428 {
00429   double rv;
00430 
00431   R >> rv;
00432   if (rv > GoalProb)
00433     return RandomState();
00434   else
00435     return P->GoalState;
00436 }
00437 
00438 
00439 // *********************************************************************
00440 // *********************************************************************
00441 // CLASS:     RRTGoalPull
00442 // 
00443 // *********************************************************************
00444 // *********************************************************************
00445 
00446 
00447 RRTGoalPull::RRTGoalPull(Problem *p):RRT(p) {
00448 }
00449 
00450 
00451 
00452 void RRTGoalPull::Construct()
00453 {
00454   int i;
00455   vector u_best,nx;
00456   node nn,nn2;
00457   bool success;
00458 
00459   // Make the root node of G
00460   if (G.number_of_nodes() == 0) {
00461     nn = G.new_node(P->InitialState);
00462     GEdgeTime[nn] = 0.0;
00463   }
00464 
00465   for (i=0; i < NumNodes; i++) {
00466     if (Extend(ChooseState(),G,nn)) { 
00467       u_best = SelectBestInput(G.inf(nn),P->GoalState,nx,success); // nx gets next state
00468       if (success) {   // If a collision-free input was found
00469         nn2 = G.new_node(nx); // Make a new node with state nx
00470         G.new_edge(nn,nn2,u_best);
00471       }
00472     }
00473   }
00474 }
00475 
00476 
00477 // *********************************************************************
00478 // *********************************************************************
00479 // CLASS:     RRTCon
00480 // 
00481 // Make the regular RRT greedy by attempting to connect all the way
00482 // to the random sample, or go as far as possible before colliding.
00483 // *********************************************************************
00484 // *********************************************************************
00485 
00486 
00487 // Build an RRT that tries to go as far as it can for each edge
00488 RRTCon::RRTCon(Problem *p):RRTGoalBias(p) {
00489   if (!is_file(P->FilePath+"GoalProb")) {
00490     GoalProb = 0.0; // No bias by default
00491   }
00492 }
00493 
00494 
00495 void RRTCon::Construct()
00496 {
00497   int i;
00498   vector u_best,nx,nx_prev;
00499   node nn;
00500 
00501 
00502   // Make the root node of G
00503   if (G.number_of_nodes() == 0) {
00504     nn = G.new_node(P->InitialState);
00505     GEdgeTime[nn] = 0.0;
00506   }
00507 
00508   for (i=0; i < NumNodes; i++) {
00509     Connect(ChooseState(),G,nn);
00510   }
00511 }
00512 
00513 
00514 
00515 bool RRTCon::Plan()
00516 {
00517   int i;
00518   double d;
00519   node n,nn,n_goal;
00520   vector nx,u_best;
00521   double ptime;
00522   list<node> path;
00523 
00524   // Make the root node of G
00525   if (G.number_of_nodes() == 0) {
00526     n = G.new_node(P->InitialState);
00527     GEdgeTime[n] = 0.0;
00528   }
00529 
00530   i = 0;
00531   n = NearestNeighbor(P->GoalState,G);
00532   n_goal = n;
00533 
00534   GoalDist = P->Metric(G.inf(n),P->GoalState);
00535   while ((i < NumNodes)&&(!GapSatisfied(G.inf(n_goal),P->GoalState))) {
00536     if (Connect(ChooseState(),G,nn)) { 
00537       d = P->Metric(G.inf(nn),P->GoalState);
00538       if (d < GoalDist) {  // Decrease if goal closer
00539         GoalDist = d;
00540         BestState = G.inf(nn);
00541         n_goal = nn;
00542         //cout << "GoalDist " << GoalDist << "\n";
00543       }
00544     }
00545     i++;
00546   }
00547 
00548   // Get the solution path
00549   if (GapSatisfied(G.inf(n_goal),P->GoalState)) {
00550     cout << "Successful Path Found\n";
00551     path = PathToLeaf(n_goal,G);
00552     // Make the correct times
00553     ptime = 0.0; TimeList.clear();
00554     forall(n,path) {
00555       ptime += GEdgeTime[n];
00556       TimeList.push_back(ptime);
00557     }
00558     RecordSolution(path); // Write to Path and Policy
00559     return true;
00560   }
00561   else
00562     return false;
00563 
00564 }
00565 
00566 
00567 // *********************************************************************
00568 // *********************************************************************
00569 // CLASS:     RRTDual
00570 // 
00571 // The dual-tree planner used in LaValle, Kuffner, ICRA 1999.  Each 
00572 // tree is extended toward a randomly-sampled point.  RRTExtExt is
00573 // generally better.
00574 // *********************************************************************
00575 // *********************************************************************
00576 
00577 RRTDual::RRTDual(Problem *p):RRT(p) {
00578 }
00579 
00580 
00581 void RRTDual::RecoverSolution(const node &n1, const node &n2) {
00582   list<node> path,path2;
00583   double ptime;
00584   node n;
00585 
00586   path = PathToLeaf(n1,G);
00587   // Figure out forward timings
00588   ptime = 0.0; TimeList.clear();
00589   forall(n,path) {
00590     ptime += GEdgeTime[n];
00591     TimeList.push_back(ptime);
00592   }
00593   
00594   path2 = PathToLeaf(n2,G2); path2.reverse();
00595   // Figure out backwards timings
00596   ptime += PlannerDeltaT; // Add a time step for the gap
00597   forall(n,path2)  { // Compute total time in G2 part of path
00598     TimeList.push_back(ptime);
00599     ptime += G2EdgeTime[n];
00600   }
00601   
00602   //cout << "Path: " << path;
00603   //cout << "Path2: " << path2;
00604   //cout << "\n TimeList: " << TimeList << " L: " << TimeList.length();
00605 
00606   RecordSolution(path,path2); // Write to Path and Policy
00607 }
00608 
00609 
00610 
00611 bool RRTDual::Plan()
00612 {
00613   int i;
00614   vector rx,u_best,nx;
00615   node nn,nn2;
00616   bool connected;
00617   list<node> path1;
00618 
00619   connected = false;
00620 
00621   float t = used_time();
00622 
00623   //int total = 0;
00624   //for (int j=0; j < 100; j++) {
00625 
00626   //G.clear();
00627   if (G.number_of_nodes() == 0) {
00628     nn = G.new_node(P->InitialState);
00629     GEdgeTime[nn] = 0.0;
00630   }
00631 
00632   //G2.clear();
00633   if (G2.number_of_nodes() == 0) {
00634     nn = G2.new_node(P->GoalState);
00635     G2EdgeTime[nn] = 0.0;
00636   }
00637 
00638   nn = G.first_node();
00639   nn2 = G2.first_node();
00640 
00641   i = 0;
00642   connected = false;
00643   while ((i < NumNodes) && (!connected)) {
00644     rx = ChooseState();
00645     Extend(rx,G,nn);
00646     Extend(rx,G2,nn2,false);  // false means reverse-time integrate
00647 
00648     if (GapSatisfied(G.inf(nn),G2.inf(nn2))) {
00649       cout << "CONNECTED!!  Nodes: " << 
00650         G.number_of_nodes() + G2.number_of_nodes() << "\n";
00651       connected = true;
00652       RecoverSolution(nn,nn2);
00653     }
00654 
00655     i++;
00656   }
00657 
00658   if (!connected)
00659     cout << "Failure to connect after " << G.number_of_nodes()+G2.number_of_nodes() 
00660          << " nodes\n";
00661 
00662   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00663 
00664   //total += i;
00665   // }
00666   //cout << "Avg: " << total/100 << "\n";
00667 
00668   cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
00669 
00670   return connected;
00671 }
00672 
00673 
00674 
00675 
00676 
00677 // *********************************************************************
00678 // *********************************************************************
00679 // CLASS:     RRTExtExt
00680 // 
00681 // A dual-tree planner in which computation is balanced between 
00682 // exploring and connecting trees to each other.  Greedier verions
00683 // of this are RRTExtCon and RRTConCon.
00684 // *********************************************************************
00685 // *********************************************************************
00686 
00687 RRTExtExt::RRTExtExt(Problem *p):RRTDual(p) {
00688 }
00689 
00690 
00691 bool RRTExtExt::Plan()
00692 {
00693   int i;
00694   vector u_best,nx,nx2;
00695   node nn,nn2;
00696   bool connected;
00697   list<node> path1;
00698 
00699   connected = false;
00700 
00701   float t = used_time();
00702 
00703   //int total = 0;
00704   //for (int j=0; j < 100; j++) {
00705   
00706   //G.clear();
00707   if (G.number_of_nodes() == 0) {
00708     nn = G.new_node(P->InitialState);
00709     GEdgeTime[nn] = 0.0;
00710   }
00711   //G2.clear();
00712   if (G2.number_of_nodes() == 0) {
00713     nn = G2.new_node(P->GoalState);
00714     G2EdgeTime[nn] = 0.0;
00715   }
00716 
00717   nn = G.first_node();
00718   nn2 = G2.first_node();
00719 
00720   i = 0;
00721   connected = false;
00722   while ((i < NumNodes) && (!connected)) {
00723     if (Extend(ChooseState(),G,nn)) {
00724       if (Extend(G.inf(nn),G2,nn2,false)) {
00725         i++;
00726         if (GapSatisfied(G.inf(nn),G2.inf(nn2))) {
00727           cout << "CONNECTED!!  Nodes: " << 
00728             G.number_of_nodes()+G2.number_of_nodes() 
00729                << "\n";
00730           connected = true;
00731           RecoverSolution(nn,nn2);
00732         }
00733       }
00734     }
00735     i++;
00736 
00737     if ((!connected) && (Extend(ChooseState(),G2,nn,false))) {
00738       if (Extend(G.inf(nn),G,nn2)) {
00739         i++;
00740         if (GapSatisfied(G.inf(nn2),G2.inf(nn))) {
00741           cout << "CONNECTED!!  Nodes: " << 
00742             G.number_of_nodes()+G2.number_of_nodes() << "\n";
00743           connected = true;
00744           RecoverSolution(nn2,nn);
00745         }
00746       }
00747     }
00748   }
00749 
00750   i++;
00751 
00752   if (!connected)
00753     cout << "Failure to connect after " << 
00754       G.number_of_nodes()+G2.number_of_nodes() 
00755          << " nodes\n";
00756 
00757   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
00758 
00759   //total += i;
00760   // }
00761   //cout << "Avg: " << total/100 << "\n";
00762   
00763   cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
00764 
00765   return connected;
00766 }
00767 
00768 
00769 
00770 
00771 // *********************************************************************
00772 // *********************************************************************
00773 // CLASS:     RRTGoalZoom
00774 // 
00775 // Bias sampling towards a region that contains the goal.  The 
00776 // region shrinks around the goal as the RRT comes nearer.  This
00777 // planner is based on a class project at Iowa State by Jun Qu in 1999.
00778 // *********************************************************************
00779 // *********************************************************************
00780 
00781 RRTGoalZoom::RRTGoalZoom(Problem *p):RRT(p) {
00782   GoalProb = 0.05;
00783   ZoomProb = 0.5;
00784   ZoomFactor = 2.0;
00785 }
00786 
00787 
00788 // Zoom using a square box
00789 vector RRTGoalZoom::ChooseState()
00790 {
00791   double rv,r,diff;
00792   vector zx;
00793   int i;
00794 
00795   R >> rv;
00796   diff = 0.0;
00797   zx = P->LowerState;  // Initialize to anything
00798 
00799   if (rv < GoalProb)
00800     return P->GoalState;
00801   else {
00802     if (rv < (ZoomProb+GoalProb)) {
00803       for (i = 0; i < P->GoalState.dim(); i++) {
00804         if (fabs(P->GoalState[i] - BestState[i]) > diff)
00805           diff = fabs(P->GoalState[i] - BestState[i]);
00806       }
00807       for (i = 0; i < P->GoalState.dim(); i++) {
00808         R >> r;
00809         zx[i] += P->GoalState[i] - diff + 2.0*r*ZoomFactor*diff;
00810       }
00811       return zx;
00812     }
00813     else
00814       return RandomState();
00815   }
00816   
00817 }
00818 
00819 
00820 // *********************************************************************
00821 // *********************************************************************
00822 // CLASS:     RRTPolar
00823 // 
00824 // Instead of random sampling, attempt to gradually bias samples
00825 // toward the goal.
00826 // *********************************************************************
00827 // *********************************************************************
00828 
00829 RRTPolar::RRTPolar(Problem *p):RRT(p) {
00830   // RadiusExp = 1.0/(P->InitialState.dim() - 1);
00831   RadiusExp = 1.0;
00832 }
00833 
00834 
00835 
00836 
00837 
00838 
00839 // This implementation ignores C-space topology, VERY BAD!
00840 vector RRTPolar::ChooseState()
00841 {
00842   double r,w;
00843   vector zx;
00844   int i,j;
00845   bool success;
00846   
00847   w = 0.0;
00848   zx = P->GoalState;  // Initialize to anything
00849 
00850   success = false;
00851 
00852   while (!success) {
00853     for (i = 0; i < P->GoalState.dim(); i++) {
00854       // Generate sample from N(0,1)
00855       zx[i] = 0.0;
00856       for (j = 0; j < 12; j++) {
00857         R >> r; zx[i] += r;
00858       }
00859       zx[i] -= 6.0;
00860       w += zx[i]*zx[i];
00861     }
00862     
00863     w = sqrt(w);
00864     
00865     //cout << "RadiusExp: " << RadiusExp;
00866     
00867     R >> r;  // Radius
00868     r = pow(r,RadiusExp);
00869     for (i = 0; i < P->GoalState.dim(); i++) {
00870       zx[i] = (P->UpperState[i] - P->LowerState[i])*
00871         sqrt(P->GoalState.dim())*r*zx[i]/w + 
00872         P->GoalState[i];
00873     }
00874     
00875     // Check if sample is within bounds
00876     success = true;
00877     for (i = 0; i < P->GoalState.dim(); i++) {
00878       if ((zx[i] >= P->UpperState[i])||(zx[i] <= P->LowerState[i]))
00879         success = false;
00880     }
00881   }
00882 
00883   return zx;
00884 }
00885 
00886 
00887 
00888 vector RRTPolar::SelectBestInput(const vector &x1, const vector &x2, 
00889                                  vector &nx_best, bool &success)
00890 {
00891   vector u,u_best,nx;
00892   double d,dg,dmax,d_min;
00893   success = false;
00894   d_min = INFINITY;
00895   dg  = P->Metric(x1,x2);
00896   dmax  = P->Metric(P->LowerState,P->UpperState);
00897   list<vector> il = P->GetInputs(x1);
00898   forall(u,il) {
00899     //nx = P->Integrate(x1,u,PlannerDeltaT*sqrt(dg/dmax));  // Slow down near goal
00900     nx = P->Integrate(x1,u,PlannerDeltaT);
00901     d  = P->Metric(nx,x2);
00902     if ((d < d_min)&&(P->Satisfied(nx))&&(x1 != nx)) 
00903       {
00904         d_min = d; u_best = u; nx_best = nx; success = true;
00905       }
00906   }
00907   
00908   //cout << "u_best: " << u_best << "\n");
00909 
00910   return u_best;
00911 }
00912 
00913 
00914 
00915 // *********************************************************************
00916 // *********************************************************************
00917 // CLASS:     RRTHull
00918 // 
00919 // A simple exploration of what happens to the RRT in a large disc.
00920 // Only works for 2DPoint model!!!!!
00921 // *********************************************************************
00922 // *********************************************************************
00923 
00924 RRTHull::RRTHull(Problem *p):RRT(p) {
00925   Radius = 100000000.0;
00926 }
00927 
00928 
00929 
00930 vector RRTHull::ChooseState() {
00931   double theta;
00932   vector v(2);
00933 
00934   R >> theta;  theta *= 2.0*PI;
00935 
00936   v[0] = Radius*cos(theta);
00937   v[1] = Radius*sin(theta);
00938 
00939   return v;
00940 }
00941 
00942 
00943 
00944 // *********************************************************************
00945 // *********************************************************************
00946 // CLASS:     RRTExtCon
00947 // 
00948 // The planner presented in Kuffner, LaValle, 2000.  There are two
00949 // trees, and the attempt to connect them is greedy.
00950 // *********************************************************************
00951 // *********************************************************************
00952 
00953 RRTExtCon::RRTExtCon(Problem *p):RRTDual(p) {
00954 }
00955 
00956 
00957 
00958 bool RRTExtCon::Plan()
00959 {
00960   int i;
00961   vector nx,nx_prev;
00962   node nn,nn2;
00963   bool connected;
00964   list<node> path1;
00965 
00966   connected = false;
00967 
00968   float t = used_time();
00969 
00970   //G.clear();
00971   if (G.number_of_nodes() == 0) {
00972     nn = G.new_node(P->InitialState);
00973     GEdgeTime[nn] = 0.0;
00974   }
00975   //G2.clear();
00976   if (G2.number_of_nodes() == 0) {
00977     nn = G2.new_node(P->GoalState);
00978     G2EdgeTime[nn] = 0.0;
00979   }
00980 
00981   nn = G.first_node();
00982   nn2 = G2.first_node();
00983 
00984   i = 0;
00985   connected = false;
00986   while ((i < NumNodes) && (!connected)) {
00987     if (Extend(ChooseState(),G,nn)) { 
00988       // Update the goal RRT
00989       if (Connect(G.inf(nn),G2,nn2,false)) {
00990         if (GapSatisfied(G.inf(nn),G2.inf(nn2))) {
00991           cout << "CONNECTED!!  Nodes: " << G.number_of_nodes()+G2.number_of_nodes()
00992                << "\n";
00993           connected = true;
00994           RecoverSolution(nn,nn2);
00995         }
00996       }
00997     }
00998     i++;
00999 
01000     if ((!connected)&&(Extend(ChooseState(),G2,nn,false))) { 
01001       // Update the initial RRT
01002       if (Connect(G2.inf(nn),G,nn2)) { 
01003         if (GapSatisfied(G.inf(nn),G2.inf(nn2))) {
01004           cout << "CONNECTED!!  Nodes: " << G.number_of_nodes()+G2.number_of_nodes()
01005                << "\n";
01006           connected = true;
01007           RecoverSolution(nn2,nn);
01008         }
01009       }
01010     }
01011     i++;
01012   }
01013 
01014   if (!connected)
01015     cout << "Failure to connect after " << G.number_of_nodes()+G2.number_of_nodes() 
01016          << " nodes\n";
01017 
01018   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
01019 
01020   cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
01021 
01022   return connected;
01023 }
01024 
01025 
01026 
01027 // *********************************************************************
01028 // *********************************************************************
01029 // CLASS:     RRTConCon
01030 // 
01031 // One the best dual-tree planners.  Be greedy in growing the trees
01032 // and in trying to connect them to each other.  This is usually the
01033 // fastest planner, but there is often a price for being greedy...
01034 // *********************************************************************
01035 // *********************************************************************
01036 
01037 RRTConCon::RRTConCon(Problem *p):RRTDual(p) {
01038 }
01039 
01040 
01041 
01042 bool RRTConCon::Plan()
01043 {
01044   int i;
01045   vector nx,nx_prev;
01046   node nn,nn2;
01047   bool connected;
01048 
01049   connected = false;
01050 
01051   // Code for time trials
01052   float t = used_time();
01053 
01054   //int total = 0;
01055   //for (int j=0; j < 100; j++) {
01056 
01057   //G.clear();
01058   if (G.number_of_nodes() == 0) {
01059     nn = G.new_node(P->InitialState);
01060     GEdgeTime[nn] = 0.0;
01061   }
01062   //G2.clear();
01063   if (G2.number_of_nodes() == 0) {
01064     nn = G2.new_node(P->GoalState);
01065     G2EdgeTime[nn] = 0.0;
01066   }
01067 
01068   nn = G.first_node();
01069   nn2 = G2.first_node();
01070 
01071   i = 0;
01072   connected = false;
01073   while ((i < NumNodes) && (!connected)) {
01074     if (Connect(ChooseState(),G,nn)) { 
01075       // Update the goal RRT
01076       if (Connect(G.inf(nn),G2,nn2,false)) {
01077         if (GapSatisfied(G.inf(nn),G2.inf(nn2))) {
01078           cout << "CONNECTED!!  Nodes: " << 
01079             G.number_of_nodes() + G2.number_of_nodes() 
01080                << "\n";
01081           connected = true;
01082           RecoverSolution(nn,nn2); // Defined in RRTDual
01083         }
01084       }
01085     }
01086     i++;
01087 
01088     if ((!connected)&&(Connect(ChooseState(),G2,nn,false))) { 
01089       // Update the initial RRT
01090       if (Connect(G2.inf(nn),G,nn2)) { 
01091         if (GapSatisfied(G.inf(nn),G2.inf(nn2))) {
01092           cout << "CONNECTED!!  Nodes: " << 
01093             G.number_of_nodes() + G2.number_of_nodes()  
01094                << "\n";
01095           connected = true;
01096           RecoverSolution(nn2,nn);
01097         }
01098       }
01099     }
01100     i++;
01101   }
01102 
01103   if (!connected)
01104     cout << "Failure to connect after " << 
01105       G.number_of_nodes()+G2.number_of_nodes() 
01106          << " nodes\n";
01107 
01108   cout << "Collision Detection Calls: " << SatisfiedCount << "\n";
01109 
01110   // Code for time trials
01111   //total += i;
01112   //  }
01113   //cout << "Avg: " << total/100.0 << "\n";
01114   //cout << "Avg time: " << ((double)used_time(t)/100.0) << "\n"; 
01115   
01116   cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 
01117 
01118   return connected;
01119 }
01120 
01121 
01122 // *********************************************************************
01123 // *********************************************************************
01124 // CLASS:     RRTStar
01125 // 
01126 // An attempt to blend A^* ideas with the RRT.
01127 // *********************************************************************
01128 // *********************************************************************
01129 
01130 RRTStar::RRTStar(Problem *p):RRT(p) {
01131   NodeDepth.init(G);
01132   OptimalityFactor = 0.9;
01133 }
01134 
01135 
01136 
01137 node RRTStar::NearestNeighbor(const vector &x, const GRAPH<vector,vector> &g,
01138                               bool forward = true) {
01139   double d,d_min;
01140   node n,n_best;
01141   
01142   n_best = g.choose_node(); // Keeps the warnings away
01143   d_min = INFINITY;
01144   forall_nodes(n,g) {
01145     d = P->Metric(g[n],x) +
01146       OptimalityFactor*P->Metric(P->InitialState,g[n]);
01147     //OptimalityFactor*PlannerDeltaT*NodeDepth[n]; // Here is the new part
01148     // The speed from Model used to appear in the second product above
01149     if (d < d_min) {
01150       d_min = d; n_best = n; 
01151     }
01152   } 
01153 
01154   return n_best;
01155 }
01156 
01157 
01158 
01159 bool RRTStar::Extend(const vector &x, 
01160                  GRAPH<vector,vector> &g,
01161                  node &nn, bool forward = true) {
01162   node n_best;
01163   vector nx,u_best;
01164   bool success;
01165   
01166   n_best = NearestNeighbor(x,g,forward);
01167   u_best = SelectBestInput(g.inf(n_best),x,nx,success); // nx gets next state
01168   if (success) {   // If a collision-free input was found
01169     nn = g.new_node(nx); // Make a new node with state nx
01170     // This NodeDepth relies on root initialization to zero!!!!
01171     NodeDepth[nn] = NodeDepth[n_best] + 1;
01172     //cout << "Node Depth: " << NodeDepth[nn] << "\n";
01173     g.new_edge(n_best,nn,u_best);
01174     //cout << "New node: " << g.inf(nn) << "   " << "u:" << u_best << "\n";
01175   }
01176 
01177   return success;
01178 }
01179 
01180 
01181 
01182 // *********************************************************************
01183 // *********************************************************************
01184 // CLASS:     PlannerHsu
01185 // 
01186 // The planner of Hsu, Latombe, Motwani (not an RRT).
01187 // *********************************************************************
01188 // *********************************************************************
01189 
01190 PlannerHsu::PlannerHsu(Problem *p):RRT(p) {
01191 
01192   NeighborFactor = 0.02;
01193   NeighborRadius = 2.0;
01194   InverseWeights = node_map<double>(G);
01195   WeightScale = 0.0;
01196 
01197   // Set the step size
01198   StepSize = P->Metric(P->InitialState,P->Integrate(P->InitialState,
01199              P->GetInputs(P->InitialState).head(),PlannerDeltaT));
01200 
01201 }
01202 
01203 
01204 bool PlannerHsu::Extend(const vector &x, 
01205                  GRAPH<vector,vector> &g,
01206                  node &nn, bool forward = true) {
01207 
01208   bool success;
01209   vector u_best = P->GetInputs(x).head();
01210   vector x1,x2;
01211   node n_best;
01212 
01213   // Fix the graph for the initial case
01214   if ((g.number_of_nodes() == 1) && (InverseWeights[g.first_node()] == 0.0)) {
01215     InverseWeights[g.first_node()] = 1.0;
01216     WeightScale = 1.0;
01217   }
01218 
01219   // Select node
01220   n_best = SelectNode(g);
01221   //n_best = g.choose_node();
01222   x1 = g.inf(n_best);
01223 
01224   // Make random neighbor
01225   x2 = ChooseNearbyState(x1);
01226 
01227   // Attempt connection
01228   success = Connect(x1,x2);
01229 
01230   // nx gets next state
01231   if (success) {   // If a collision-free input was found
01232     nn = g.new_node(x2); // Make a new node with state nx
01233     UpdateWeights(nn,g);
01234     g.new_edge(n_best,nn,u_best);
01235     //cout << "New node: " << g.inf(nn) << "   " << "u:" << u_best << "\n";
01236   }
01237 
01238   return success;
01239 }
01240 
01241 
01242 
01243 bool PlannerHsu::Connect(const vector &x1, const vector &x2) {
01244   double a;
01245   int k;
01246   vector x;
01247 
01248   k = (int) (P->Metric(x1,x2) / StepSize); 
01249 
01250   for (a = 1.0/(k+1); a <= 1.0; a += 1.0/(k+1) ) {
01251     x = P->InterpolateState(x1,x2,a);
01252     if (!P->Satisfied(x))
01253       return false;
01254   }
01255 
01256   return true;
01257 }
01258 
01259 
01260 
01261 node PlannerHsu::SelectNode(const GRAPH<vector,vector> &g) {
01262   node n;
01263   double r,ptr,w;
01264 
01265   //cout << "W: ";
01266   //forall_nodes(n,g) {
01267   //  cout << InverseWeights[n] << " ";
01268   //}
01269   //cout << "\n";
01270 
01271   // Select the node
01272   R >> r; r *= WeightScale;
01273   ptr = 0.0;
01274   forall_nodes(n,g) {
01275     w = InverseWeights[n];
01276     ptr += w;
01277     if (ptr > r) {
01278       return n; 
01279     }
01280   }
01281 
01282   cout << "ERROR: No node was selected\n";
01283   return n;  // Just a fake to avoid warnings
01284 }
01285 
01286 
01287 
01288 vector PlannerHsu::ChooseNearbyState(const vector &x) {
01289   int i;
01290   double r,txi;
01291   vector rx;
01292 
01293   rx = x;  // Set the proper dimension
01294   for (i = 0; i < P->StateDim; i++) {
01295       txi = INFINITY;
01296       while ((txi < P->LowerState[i])||(txi > P->UpperState[i])) {
01297         R >> r; r -= 0.5;
01298         txi = x[i] + 
01299           r * (P->UpperState[i] - P->LowerState[i]) * NeighborFactor;
01300       }
01301       rx[i] = txi;
01302   }
01303 
01304   return rx;
01305 }
01306 
01307 
01308 
01309 void PlannerHsu::UpdateWeights(const node &nn,
01310                            const GRAPH<vector,vector> &g) {
01311   vector x;
01312   node n;
01313   
01314   x = g.inf(nn);
01315 
01316   InverseWeights[nn] = 1.0;  // Count itself
01317   WeightScale += 1.0;
01318   forall_nodes(n,g) {
01319     if ((P->Metric(x,g.inf(n)) < NeighborRadius)&&
01320         (n != nn))
01321     {
01322       // Update the new node
01323       InverseWeights[nn] = 1/((1/InverseWeights[nn]) + 1);
01324       WeightScale += InverseWeights[nn] - 1/((1/InverseWeights[nn]) - 1);
01325       // Update the other nodes 
01326       InverseWeights[n] = 1/((1/InverseWeights[n]) + 1);
01327       WeightScale += InverseWeights[n] - 1/((1/InverseWeights[n]) - 1);
01328     }
01329   }
01330 }
01331 
01332 
01333 
01334 // *********************************************************************
01335 // *********************************************************************
01336 // CLASS:     RandomTree
01337 // 
01338 // Technically not an RRT.  Pick a vertex and input at random.
01339 // *********************************************************************
01340 // *********************************************************************
01341 
01342 RandomTree::RandomTree(Problem *p):RRT(p) {
01343 }
01344 
01345 
01346 node RandomTree::NearestNeighbor(const vector &x, const GRAPH<vector,vector> &g,
01347                           bool forward = true) {
01348 
01349   return g.choose_node();
01350 }
01351 
01352 
01353 vector RandomTree::SelectBestInput(const vector &x1, const vector &x2, 
01354                                  vector &nx_best, bool &success,
01355                                  bool forward = true) {
01356   double r;
01357   list<vector> il;
01358   int k;
01359   vector u_best;
01360 
01361   il = P->GetInputs(x1);
01362   R >> r;
01363 
01364   k = (int) (r * il.length());
01365 
01366   u_best = il.inf(il[k]);
01367 
01368   nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
01369   SatisfiedCount++;
01370 
01371   return il.inf(il[k]);
01372   
01373 }
01374 
01375 #include <LEDA/UNDEFINE_NAMES.h>
01376 
01377 
01378 
01379 
01380 
01381 
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.