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> 00020 #include <math.h> 00021 00022 #include "model.h" 00023 #include "defs.h" 00024 00025 Model::Model(string path) { 00026 00027 if ((path.length() > 0)&&(path[path.length()-1] != '/')) 00028 path += "/"; 00029 00030 FilePath = path; 00031 00032 READ_PARAMETER_OR_DEFAULT(ModelDeltaT,1.0); 00033 00034 StateDim = 2; 00035 InputDim = 2; 00036 00037 } 00038 00039 00040 00041 MSLVector Model::EulerIntegrate(const MSLVector &x, const MSLVector &u, 00042 const double &h) 00043 { 00044 int s,i,k; 00045 double c; 00046 MSLVector nx; 00047 00048 s = (h > 0) ? 1 : -1; 00049 00050 c = s*h/ModelDeltaT; // Number of iterations (as a double) 00051 k = (int) c; 00052 00053 nx = x; 00054 for (i = 0; i < k; i++) { 00055 nx += s * ModelDeltaT * StateTransitionEquation(nx,u); 00056 } 00057 00058 // Integrate the last step for the remaining time 00059 nx += s * (c - k) * ModelDeltaT * StateTransitionEquation(nx,u); 00060 00061 return nx; 00062 } 00063 00064 00065 00066 MSLVector Model::RungeKuttaIntegrate(const MSLVector &x, const MSLVector &u, 00067 const double &h) 00068 { 00069 MSLVector k1,k2,k3,k4; 00070 int s,i,k; 00071 double c,deltat; 00072 MSLVector nx; 00073 00074 s = (h > 0) ? 1 : -1; 00075 00076 c = s*h/ModelDeltaT; // Number of iterations (as a double) 00077 k = (int) c; 00078 deltat = s * ModelDeltaT; 00079 00080 nx = x; 00081 for (i = 0; i < k; i++) { 00082 k1 = StateTransitionEquation(nx,u); 00083 k2 = StateTransitionEquation(nx + (0.5*deltat)*k1,u); 00084 k3 = StateTransitionEquation(nx + (0.5*deltat)*k2,u); 00085 k4 = StateTransitionEquation(nx + deltat*k3,u); 00086 nx += deltat / 6.0 * (k1 + 2.0*k2 + 2.0*k3 + k4); 00087 } 00088 00089 // Integrate the last step for the remaining time 00090 deltat = s * (c - k) * ModelDeltaT; 00091 k1 = StateTransitionEquation(nx,u); 00092 k2 = StateTransitionEquation(nx + (0.5*deltat)*k1,u); 00093 k3 = StateTransitionEquation(nx + (0.5*deltat)*k2,u); 00094 k4 = StateTransitionEquation(nx + deltat*k3,u); 00095 nx += deltat / 6.0 * (k1 + 2.0*k2 + 2.0*k3 + k4); 00096 00097 return nx; 00098 } 00099 00100 00101 00102 list<MSLVector> Model::GetInputs(const MSLVector &x) { 00103 return Inputs; 00104 } 00105 00106 00107 00108 // By default, don't change anything 00109 MSLVector Model::StateToConfiguration(const MSLVector &x) { 00110 return x; 00111 } 00112 00113 00114 bool Model::Satisfied(const MSLVector &x) { 00115 return true; 00116 } 00117 00118 00119 // Default is to use the standard Euclidean metric 00120 double Model::Metric(const MSLVector &x1, const MSLVector &x2) { 00121 00122 double rho; 00123 00124 rho = (x1 - x2).length(); 00125 00126 return rho; 00127 } 00128 00129 00130 // Some models will interpolate differently because of 00131 // topology (e.g., S^1, P^3) 00132 MSLVector Model::LinearInterpolate(const MSLVector &x1, const MSLVector &x2, 00133 const double &a) { 00134 return (1.0-a)*x1 + a*x2; 00135 } 00136 00137 00138 // Ignore topology in the base class 00139 MSLVector Model::StateDifference(const MSLVector &x1, const MSLVector &x2) { 00140 return (x2 - x1); 00141 } 00142 00143