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

rrtshow.C

Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdlib.h>
00003 
00004 #include <LEDA/window.h>
00005 
00006 #include <MSL/rrt.h>
00007 #include <MSL/model2d.h>
00008 
00009 #include <LEDA/REDEFINE_NAMES.h>
00010 
00011 
00012 
00013 void DrawPath(const list<vector> &path, Planner *Pl, window *w)
00014 {
00015   vector x1,x2;
00016   double tranx,trany,scalex,scaley;
00017   double lw;
00018 
00019   lw = w->get_line_width();
00020   w->set_line_width(5.0);
00021 
00022   // The default window coords are [0,100]x[0,100]
00023   scalex = 100.0/(Pl->P->UpperState[0] - 
00024                   Pl->P->LowerState[0]); 
00025   scaley = 100.0/(Pl->P->UpperState[1] - 
00026                   Pl->P->LowerState[1]);
00027   tranx = -1.0 * scalex * Pl->P->LowerState[0];
00028   trany = -1.0 * scaley * Pl->P->LowerState[1];
00029 
00030   x1 = path.head();
00031   forall(x2,path) {
00032     w->draw_segment(x1[0]*scalex+tranx,
00033                     x1[1]*scaley+trany,
00034                     x2[0]*scalex+tranx,
00035                     x2[1]*scaley+trany,violet);
00036     x1 = x2;
00037   }
00038 
00039   w->set_line_width(lw);
00040 }
00041 
00042 
00043 
00044 
00045 void DrawRRT(Planner *Pl, window *w)
00046 {
00047   edge e;
00048   vector x1,x2;
00049   double tranx,trany,scalex,scaley;
00050   
00051   // The default window coords are [0,100]x[0,100]
00052   scalex = 100.0/(Pl->P->UpperState[0] - 
00053                   Pl->P->LowerState[0]); 
00054   scaley = 100.0/(Pl->P->UpperState[1] - 
00055                   Pl->P->LowerState[1]); 
00056   tranx = -1.0 * scalex * Pl->P->LowerState[0];
00057   trany = -1.0 * scaley * Pl->P->LowerState[1];
00058 
00059   // Show first tree
00060   if (Pl->G.number_of_nodes() > 0) {
00061     forall_edges(e,Pl->G) {
00062       x1 = Pl->G.inf(Pl->G.source(e));
00063       x2 = Pl->G.inf(Pl->G.target(e));
00064       w->draw_segment(x1[0]*scalex+tranx,
00065                       x1[1]*scaley+trany,
00066                       x2[0]*scalex+tranx,
00067                       x2[1]*scaley+trany,blue);
00068     }
00069   }
00070   // Show second tree (if it exists)
00071   if (Pl->G2.number_of_nodes() > 0) {
00072     forall_edges(e,Pl->G2) {
00073       x1 = Pl->G2.inf(Pl->G2.source(e));
00074       x2 = Pl->G2.inf(Pl->G2.target(e));
00075       w->draw_segment(x1[0]*scalex+tranx,x1[1]*scaley+trany,
00076                       x2[0]*scalex+tranx,x2[1]*scaley+trany,
00077                       red);
00078     }
00079   }
00080 }
00081 
00082 
00083 
00084 
00085 int main(int argc, char *argv[])
00086 {
00087   string path = "";
00088   window *w;
00089   Model *m = NULL;
00090   Geom *g = NULL;
00091   Problem *prob;
00092   RRT *rrt;
00093   int i;
00094   list<vector> solution;
00095 
00096   // Initialize the window
00097   w = new window(600,600,"Rapidly-Exploring Random Trees");
00098   w->set_line_width(2.0);
00099   w->display();
00100   w->draw_rectangle(0.0,0.0,100.0,100.0,red);
00101 
00102 
00103   leda_wait(2.0);
00104 
00105   cout << "Draw an RRT starting in the center of a box\n";
00106   m = new Model2DPoint(path); // Define the differential model
00107   g = new GeomLedaPolygonal(path); 
00108   prob = new Problem(g,m,path); // This is the input to a planner
00109   prob->InitialState = vector(50.0,50.0);
00110   prob->GoalState = vector(100.0,100.0);
00111   rrt = new RRT(prob);
00112   rrt->NumNodes = 50;
00113   rrt->PlannerDeltaT = 1.0;
00114 
00115   for (i = 0; i < 15; i++) {
00116     rrt->Construct();
00117     DrawRRT(rrt,w);
00118     leda_wait(0.2);
00119   }
00120   for (i = 0; i < 30; i++) {
00121     rrt->Construct();
00122     DrawRRT(rrt,w);
00123     leda_wait(0.05);
00124   }
00125 
00126   leda_wait(2.0);
00127 
00128   cout << "Draw an RRT starting in the corner\n";
00129   w->clear();
00130   w->draw_rectangle(0.0,0.0,100.0,100.0,red);
00131   rrt->Reset();
00132   rrt->NumNodes = 50;
00133   rrt->PlannerDeltaT = 1.0;
00134   prob->InitialState = vector(1.0,1.0);
00135   for (i = 0; i < 15; i++) {
00136     rrt->Construct();
00137     DrawRRT(rrt,w);
00138     leda_wait(0.1);
00139   }
00140   for (i = 0; i < 30; i++) {
00141     rrt->Construct();
00142     DrawRRT(rrt,w);
00143     leda_wait(0.05);
00144   }
00145 
00146   leda_wait(2.0);
00147 
00148   cout << "Draw an RRT for the car\n";
00149   w->clear();
00150   w->draw_rectangle(0.0,0.0,100.0,100.0,red);
00151   rrt->Reset();
00152   rrt->NumNodes = 50;
00153   rrt->PlannerDeltaT = 1.0;
00154   m = new Model2DRigidCar(path); // Define the differential model
00155   prob->SetModel(m);
00156   prob->InitialState = vector(50.0,50.0,0.0);
00157   prob->GoalState = vector(100.0,50.0,0.0);
00158   for (i = 0; i < 50; i++) {
00159     rrt->Construct();
00160     DrawRRT(rrt,w);
00161     leda_wait(0.05);
00162   }
00163 
00164   leda_wait(2.0);
00165 
00166   cout << "Draw an RRT for the forward-only car\n";
00167   w->clear();
00168   w->draw_rectangle(0.0,0.0,100.0,100.0,red);
00169   rrt->Reset();
00170   rrt->NumNodes = 50;
00171   rrt->PlannerDeltaT = 1.0;
00172   m = new Model2DRigidCarForward(path); // Define the differential model
00173   prob->SetModel(m);
00174   prob->InitialState = vector(50.0,50.0,1.732);
00175   prob->GoalState = vector(100.0,50.0,1.732);
00176   for (i = 0; i < 50; i++) {
00177     rrt->Construct();
00178     DrawRRT(rrt,w);
00179     leda_wait(0.05);
00180   }
00181 
00182   // Finished
00183   cout << "Click a mouse button to terminate\n";
00184   w->read_mouse();
00185   return 0;
00186 }
00187 
00188 
00189 #include <LEDA/UNDEFINE_NAMES.h>
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.