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 }