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