00001 //---------------------------------------------------------------------- 00002 // The Motion Strategy Library (MSL) 00003 //---------------------------------------------------------------------- 00004 // 00005 // Copyright (c) 1998-2001 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 <MSL/model3d.h> 00023 00024 #include "modelnew.h" 00025 00026 #include <LEDA/REDEFINE_NAMES.h> 00027 00028 00029 // ********************************************************************* 00030 // ********************************************************************* 00031 // CLASS: Model3DRigidHelical 00032 // 00033 // A nonlinear model that generates helical motions 00034 // ********************************************************************* 00035 // ********************************************************************* 00036 00037 00038 // Constructor 00039 Model3DRigidHelical::Model3DRigidHelical(string path = ""):Model3DRigid(path) { 00040 InputDim = 3; // Override the default of 6 from Model3DRigid 00041 00042 // Make inputs 00043 // u[0] is the speed: positive is forward, negative is reverse 00044 // u[1] is the steering: 0.0 is straight, 0.1 is left, -0.1 is right 00045 // u[2] changes altitude (Z direction) 00046 Inputs.clear(); // Clear whatever inputs came from Model3DRigid!!! 00047 if (is_file(FilePath+"Inputs")) 00048 ReadInputs(); 00049 else { 00050 Inputs.push(vector(1.0,0.0,0.0)); 00051 Inputs.push(vector(1.0,0.1,0.0)); 00052 Inputs.push(vector(1.0,-0.1,0.0)); 00053 Inputs.push(vector(1.0,0.0,0.5)); 00054 Inputs.push(vector(1.0,0.1,0.5)); 00055 Inputs.push(vector(1.0,-0.1,0.5)); 00056 Inputs.push(vector(1.0,0.0,-0.5)); 00057 Inputs.push(vector(1.0,0.1,-0.5)); 00058 Inputs.push(vector(1.0,-0.1,-0.5)); 00059 } 00060 } 00061 00062 00064 vector Model3DRigidHelical::StateTransitionEquation(const vector &x, 00065 const vector &u) { 00066 00067 vector dx(6); 00068 00069 // With respect to 0,1,5, it should look like a Reeds-Shepp car model 00070 00071 dx[0] = u[0]*cos(x[5]); 00072 dx[1] = u[0]*sin(x[5]); 00073 dx[2] = u[2]; // Change elevation 00074 dx[3] = 0.0; // No roll 00075 dx[4] = 0.0; // No pitch 00076 dx[5] = u[0]*u[1]; // Some yaw 00077 00078 return dx; 00079 } 00080 00081 00082 vector Model3DRigidHelical::Integrate(const vector &x, const vector &u, 00083 const double &h) 00084 { 00085 return RungeKuttaIntegrate(x,u,h); 00086 } 00087 00088 00089 #include <LEDA/UNDEFINE_NAMES.h> 00090