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