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

planner.C

Go to the documentation of this file.
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 "planner.h"
00023 #include "defs.h"
00024 
00025 #include <LEDA/REDEFINE_NAMES.h>
00026 
00027 
00028 // *********************************************************************
00029 // *********************************************************************
00030 // CLASS:     Planner base class
00031 // 
00032 // *********************************************************************
00033 // *********************************************************************
00034 
00035 Planner::Planner(Problem *problem):Solver(problem) {
00036   Reset();
00037 }
00038 
00039 
00040 void Planner::Reset() {
00041   int i;
00042 
00043   NumNodes = 1000;
00044 
00045   if (is_file(P->FilePath+"PlannerDeltaT")) {
00046     file_istream fin(P->FilePath + "PlannerDeltaT");
00047     fin >> PlannerDeltaT;
00048     fin.close();
00049   }
00050   else {
00051     PlannerDeltaT = 1.0;
00052   }
00053 
00054   if (is_file(P->FilePath+"GapError")) {
00055     file_istream fin(P->FilePath + "GapError");
00056     fin >> GapError;
00057     fin.close();
00058   }
00059   else {
00060     GapError = vector(P->StateDim);
00061     for (i = 0; i < P->StateDim; i++)
00062       GapError[i] = 1.0;
00063   }
00064 
00065   Path.clear();
00066   Policy.clear();
00067 
00068   Holonomic = is_file(P->FilePath+"Holonomic"); // Nonholonomic by default
00069 
00070   G.clear();
00071   G2.clear();
00072 }
00073 
00074 
00075 vector Planner::RandomState() {
00076   int i;
00077   double r;
00078   vector rx;
00079 
00080   rx = P->LowerState;
00081   for (i = 0; i < P->StateDim; i++) {
00082       R >> r; 
00083       rx[i] += r * (P->UpperState[i] - P->LowerState[i]);
00084     }
00085 
00086   return rx;
00087 }
00088 
00089 
00090 
00091 vector Planner::NormalState(vector mean, double sd = 0.5) {
00092   int i,j;
00093   double r;
00094   vector rx;
00095   bool success = false;
00096 
00097   rx = mean;
00098   for (i = 0; i < P->StateDim; i++) {
00099     success = false;
00100     while (!success) {
00101       rx[i] = 0.0;
00102       for (j = 0; j < 12; j++) {  // Increase 12 here and below for more accuracy
00103         R >> r; rx[i] += r;
00104       }
00105       rx[i] = (rx[i] - 12/2)*sd*(P->UpperState[i]-P->LowerState[i])+mean[i];
00106       if ((rx[i] <= P->UpperState[i])&&(rx[i] >= P->LowerState[i]))
00107         success = true;
00108     }
00109   }
00110 
00111   return rx;
00112 }
00113 
00114 
00115 
00116 
00117 void Planner::WriteGraphs(const string &fname)
00118 {
00119   ofstream fout(fname);
00120   G.write(fout);
00121   G2.write(fout);
00122   fout.close();
00123 }
00124 
00125 
00126 
00127 void Planner::ReadGraphs(const string &fname)
00128 {
00129   ifstream fin(fname);
00130   G.read(fin);
00131   G2.read(fin);
00132   fin.close();
00133 }
00134 
00135 
00136 
00137 void Planner::RecordSolution(const list<node> &glist, 
00138                              const list<node> &g2list)
00139 {
00140   node n,nfirst,nlast;
00141 
00142   Path.clear();
00143   Policy.clear();
00144 
00145   nfirst = glist.head();
00146   forall(n,glist) {
00147       Path.push_back(G.inf(n));
00148       if (n != nfirst) {
00149         Policy.push_back(G.inf(G.first_in_edge(n)));
00150       }
00151   }
00152 
00153   // The GapState is always comes from last node in glist
00154   GapState = Path.tail();
00155   // Make a dummy input to get from GapState to the next state
00156   Policy.push_back(P->GetInputs().head());
00157 
00158   if (g2list.length() == 0) {
00159     // Push the goal state onto the end (jumps the gap)
00160     Path.push_back(P->GoalState);
00161   }
00162   else { // Using two graphs
00163     nlast = g2list.tail();
00164     forall(n,g2list) {
00165       Path.push_back(G2.inf(n));
00166       if (n != nlast) {
00167         Policy.push_back(G2.inf(G2.first_in_edge(n)));
00168       }
00169     }
00170   }
00171 
00172   //cout << "Path: " << Path << "\n";
00173   //cout << "Policy: " << Policy << "\n";
00174 }
00175 
00176 
00177 
00178 void Planner::RecordSolution(const list<node> &glist)
00179 {
00180   list<node> emptylist;
00181   emptylist.clear(); // Make sure it is clear
00182 
00183   RecordSolution(glist,emptylist);
00184 }
00185 
00186 
00187 
00188 bool Planner::GapSatisfied(const vector &x1, const vector &x2) {
00189   vector x;
00190   int i;
00191 
00192   x = P->StateDifference(x1,x2);
00193   for (i = 0; i < P->StateDim; i++) {
00194     if (fabs(x[i]) > GapError[i])
00195       return false;
00196   }
00197 
00198   return true;
00199 }
00200 
00201 
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.