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