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 <fstream.h> 00020 #include <math.h> 00021 00022 #include "model.h" 00023 #include "defs.h" 00024 00025 #include <LEDA/REDEFINE_NAMES.h> 00026 00027 00028 Model::Model(string path) { 00029 00030 if ((path.length() > 0)&&(path[path.length()-1] != '/')) 00031 path += "/"; 00032 00033 FilePath = path; 00034 00035 if (is_file(FilePath+"ModelDeltaT")) { 00036 file_istream fin(FilePath + "ModelDeltaT"); 00037 fin >> ModelDeltaT; 00038 fin.close(); 00039 } 00040 else { 00041 ModelDeltaT = 1.0; 00042 } 00043 00044 StateDim = 2; 00045 InputDim = 2; 00046 00047 } 00048 00049 00050 00051 vector Model::EulerIntegrate(const vector &x, const vector &u, 00052 const double &h) 00053 { 00054 int s,i,k; 00055 double c; 00056 vector nx; 00057 00058 s = (h > 0) ? 1 : -1; 00059 00060 c = s*h/ModelDeltaT; // Number of iterations (as a double) 00061 k = (int) c; 00062 00063 nx = x; 00064 for (i = 0; i < k; i++) { 00065 nx += s * ModelDeltaT * StateTransitionEquation(nx,u); 00066 } 00067 00068 // Integrate the last step for the remaining time 00069 nx += s * (c - k) * ModelDeltaT * StateTransitionEquation(nx,u); 00070 00071 return nx; 00072 } 00073 00074 00075 00076 vector Model::RungeKuttaIntegrate(const vector &x, const vector &u, 00077 const double &h) 00078 { 00079 vector k1,k2,k3,k4; 00080 int s,i,k; 00081 double c,deltat; 00082 vector nx; 00083 00084 s = (h > 0) ? 1 : -1; 00085 00086 c = s*h/ModelDeltaT; // Number of iterations (as a double) 00087 k = (int) c; 00088 deltat = s * ModelDeltaT; 00089 00090 nx = x; 00091 for (i = 0; i < k; i++) { 00092 k1 = StateTransitionEquation(nx,u); 00093 k2 = StateTransitionEquation(nx + (0.5*deltat)*k1,u); 00094 k3 = StateTransitionEquation(nx + (0.5*deltat)*k2,u); 00095 k4 = StateTransitionEquation(nx + deltat*k3,u); 00096 nx += deltat / 6.0 * (k1 + 2.0*k2 + 2.0*k3 + k4); 00097 } 00098 00099 // Integrate the last step for the remaining time 00100 deltat = s * (c - k) * ModelDeltaT; 00101 k1 = StateTransitionEquation(nx,u); 00102 k2 = StateTransitionEquation(nx + (0.5*deltat)*k1,u); 00103 k3 = StateTransitionEquation(nx + (0.5*deltat)*k2,u); 00104 k4 = StateTransitionEquation(nx + deltat*k3,u); 00105 nx += deltat / 6.0 * (k1 + 2.0*k2 + 2.0*k3 + k4); 00106 00107 return nx; 00108 } 00109 00110 00111 00112 list<vector> Model::GetInputs(const vector &x) { 00113 return Inputs; 00114 } 00115 00116 00117 point Model::StateToLedaPoint(const vector &x) { 00118 return point(x[0],x[1]); 00119 } 00120 00121 00122 00123 // By default, don't change anything 00124 vector Model::StateToConfiguration(const vector &x) { 00125 return x; 00126 } 00127 00128 00129 00130 // Overrides the default Inputs by reading from a file 00131 void Model::ReadInputs() { 00132 Inputs.clear(); 00133 file_istream fin(FilePath + "Inputs"); 00134 fin >> Inputs; 00135 } 00136 00137 00138 00139 void Model::ReadLowerState() { 00140 file_istream fin(FilePath + "LowerState"); 00141 fin >> LowerState; 00142 } 00143 00144 00145 00146 void Model::ReadUpperState() { 00147 file_istream fin(FilePath + "UpperState"); 00148 fin >> UpperState; 00149 } 00150 00151 00152 bool Model::Satisfied(const vector &x) { 00153 return true; 00154 } 00155 00156 00157 // Default is to use the standard Euclidean metric 00158 double Model::Metric(const vector &x1, const vector &x2) { 00159 00160 double rho; 00161 00162 rho = (x1 - x2).length(); 00163 00164 return rho; 00165 } 00166 00167 00168 // Some models will interpolate differently because of 00169 // topology (e.g., S^1, P^3) 00170 vector Model::LinearInterpolate(const vector &x1, const vector &x2, 00171 const double &a) { 00172 return (1.0-a)*x1 + a*x2; 00173 } 00174 00175 00176 // Ignore topology in the base class 00177 vector Model::StateDifference(const vector &x1, const vector &x2) { 00178 return (x2 - x1); 00179 } 00180 00181 #include <LEDA/UNDEFINE_NAMES.h> 00182 00183 00184