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

rrtslide.C

Go to the documentation of this file.
00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2001 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 
00020 #include "rrtslide.h"
00021 
00022 
00023 // *********************************************************************
00024 // *********************************************************************
00025 // CLASS:     RRTSlide
00026 //
00027 // *********************************************************************
00028 // *********************************************************************
00029 
00030 
00031 RRTSlide::RRTSlide(Problem *p):RRTCon(p) {
00032   int i;
00033   
00034   NumNodes = 100;  // Each node is costly
00035   RandomTrials = 50;
00036   // Initialize the random directions
00037   NumDirections = 1000;
00038   RandomDirections = vector<MSLVector>(NumDirections);
00039   for (i = 0; i < NumDirections; i++) {
00040     RandomDirections[i] = RandomDirection();
00041   }
00042 }
00043 
00044 
00045 MSLVector RRTSlide::SelectInput(const MSLVector &x1, const MSLVector &x2, 
00046                                 MSLVector &nx_best, bool &success,
00047                                 bool forward = true)
00048 {
00049   MSLVector u_best,nx,tu;
00050   list<MSLVector>::iterator u;
00051   double d,d_min;
00052   success = false;
00053   int i;
00054   d_min = (forward) ? P->Metric(x1,x2) : P->Metric(x2,x1);
00055   list<MSLVector> il = P->GetInputs(x1);
00056 
00057   if (Holonomic) { // Just do interpolation
00058 
00059     // First try the best direction
00060     u_best = P->InterpolateState(x1,x2,0.1) - x1;
00061     u_best = u_best.norm(); // Normalize the direction
00062     nx_best = P->Integrate(x1,u_best,PlannerDeltaT);
00063     SatisfiedCount++;
00064     if (P->Satisfied(nx_best)) {
00065       //if (forward)
00066       //        cout << "H" << flush;
00067       success = true;
00068       return u_best;
00069     }
00070 
00071     // Now try some random inputs
00072     for (i = 0; i < RandomTrials; i++) {
00073       tu = RandomDirections[i];
00074       nx = P->Integrate(x1,tu,PlannerDeltaT);
00075       d = P->Metric(nx,x2);
00076       SatisfiedCount++;
00077       if ((d < d_min)&&(x1 != nx)) { 
00078         if (P->Satisfied(nx)) {
00079           d_min = d; u_best = tu; nx_best = nx; success = true;
00080           if (forward) {
00081             //cout << "R"; 
00082             //cout << "  i: " << i << "  d: " << 
00083             //  P->DistanceComp(P->StateToConfiguration(nx)) << "\n";
00084           }
00085           return u_best;
00086         }
00087       }      
00088     }
00089   }
00090   else {
00091   
00092     // This will be called whenever the best input fails for holonomic case!
00093     // Nonholonomic (the more general case -- look at Inputs)
00094     forall(u,il) {
00095       if (forward)
00096         nx = P->Integrate(x1,*u,PlannerDeltaT);
00097       else
00098         nx = P->Integrate(x1,*u,-PlannerDeltaT);
00099       
00100       d  = (forward) ? P->Metric(nx,x2): P->Metric(x2,nx);
00101       
00102       SatisfiedCount++;
00103       
00104       if ((d < d_min)&&(x1 != nx)) { 
00105         if (P->Satisfied(nx)) {
00106           d_min = d; u_best = *u; nx_best = nx; success = true;
00107         }
00108       }
00109     }
00110   }
00111 
00112   //cout << "u_best: " << u_best << "\n";
00113   if (forward) {
00114     if (!success)
00115       cout << "  F";
00116   }
00117 
00118   return u_best;
00119 }
00120 
00121 
00122 
00123 bool RRTSlide::Connect(const MSLVector &x, 
00124                        MSLTree *t,
00125                        MSLNode *&nn, bool forward = true) {
00126   MSLNode *nn_prev,*n_best;
00127   MSLVector nx,nx_prev,u_best;
00128   bool success;
00129   double d,d_prev,clock,tstep;
00130   int steps;
00131 
00132   tstep = PlannerDeltaT;
00133   if ((!Holonomic)&&(!forward))
00134     tstep *= -1.0;
00135 
00136   n_best = SelectNode(x,t,forward);
00137 
00138   if (forward) 
00139     cout <<
00140       "CONNECT  d: " << 
00141       P->DistanceComp(P->StateToConfiguration(n_best->State())); 
00142   //     << "\n";
00143 
00144   u_best = SelectInput(n_best->State(),x,nx,success,forward); 
00145   steps = 0;
00146            // nx gets next state
00147   if (success) {   // If a collision-free input was found
00148     d = P->Metric(nx,x); d_prev = d;
00149     nx_prev = nx; // Initialize
00150     nn = n_best;
00151     clock = PlannerDeltaT;
00152     while ((P->Satisfied(nx))&&
00153            (clock <= ConnectTimeLimit)&&
00154            (d <= d_prev))
00155       {
00156         SatisfiedCount++;
00157         steps++; // Number of steps made in connecting
00158         nx_prev = nx;
00159         d_prev = d; nn_prev = nn;
00160 
00161         // First try the previous input
00162         nx = P->Integrate(nx_prev,u_best,tstep);
00163         d = P->Metric(nx,x);
00164 
00165         // If failure, then try a new input
00166         if (!(P->Satisfied(nx))||(d > d_prev)) { 
00167           u_best = SelectInput(nx_prev,x,nx,success,forward);
00168           d = P->Metric(nx,x);
00169         }
00170 
00171         clock += PlannerDeltaT;
00172 
00173       }
00174 
00175     nn = t->Extend(n_best, nx, u_best, PlannerDeltaT);
00176   }
00177 
00178   cout << " Steps: " << steps << "\n";
00179   return success;
00180 }
00181 
00182 
00183 
00184 // Generate a MSLVector in a random direction with unit magnitude
00185 MSLVector RRTSlide::RandomDirection()
00186 {
00187   MSLVector delta;
00188   int i,j,dim;
00189   double r,w;
00190 
00191   dim = P->StateDim;
00192 
00193   delta = MSLVector(dim);
00194 
00195   // Pick a random direction
00196   w = 0.0;
00197   for (i = 0; i < dim; i++) {
00198     // Generate sample from N(0,1)
00199     delta[i] = 0.0;
00200     for (j = 0; j < 12; j++) {
00201       R >> r; delta[i] += r;
00202     }
00203     delta[i] -= 6.0;
00204     w += delta[i]*delta[i];
00205   }
00206   w = sqrt(w);  
00207   for (i = 0; i < dim; i++) {
00208     delta[i] = delta[i]/w;
00209   }
00210 
00211   //cout << "delta: " << delta << "\n";
00212   return delta;
00213 }
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.