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

model.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 <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 
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.