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 00027 // ********************************************************************* 00028 // ********************************************************************* 00029 // CLASS: Model3DRigidHelical 00030 // 00031 // A nonlinear model that generates helical motions 00032 // ********************************************************************* 00033 // ********************************************************************* 00034 00035 00036 // Constructor 00037 Model3DRigidHelical::Model3DRigidHelical(string path = ""):Model3DRigid(path) { 00038 InputDim = 3; // Override the default of 6 from Model3DRigid 00039 00040 // Make inputs 00041 // u[0] is the speed: positive is forward, negative is reverse 00042 // u[1] is the steering: 0.0 is straight, 0.1 is left, -0.1 is right 00043 // u[2] changes altitude (Z direction) 00044 Inputs.clear(); // Clear whatever inputs came from Model3DRigid!!! 00045 Inputs.push_front(MSLVector(1.0,0.0,0.0)); 00046 Inputs.push_front(MSLVector(1.0,0.1,0.0)); 00047 Inputs.push_front(MSLVector(1.0,-0.1,0.0)); 00048 Inputs.push_front(MSLVector(1.0,0.0,0.5)); 00049 Inputs.push_front(MSLVector(1.0,0.1,0.5)); 00050 Inputs.push_front(MSLVector(1.0,-0.1,0.5)); 00051 Inputs.push_front(MSLVector(1.0,0.0,-0.5)); 00052 Inputs.push_front(MSLVector(1.0,0.1,-0.5)); 00053 Inputs.push_front(MSLVector(1.0,-0.1,-0.5)); 00054 00055 READ_OPTIONAL_PARAMETER(Inputs); 00056 00057 } 00058 00059 00061 MSLVector Model3DRigidHelical::StateTransitionEquation(const MSLVector &x, 00062 const MSLVector &u) { 00063 00064 MSLVector dx(6); 00065 00066 // With respect to 0,1,5, it should look like a Reeds-Shepp car model 00067 00068 dx[0] = u[0]*cos(x[5]); 00069 dx[1] = u[0]*sin(x[5]); 00070 dx[2] = u[2]; // Change elevation 00071 dx[3] = 0.0; // No roll 00072 dx[4] = 0.0; // No pitch 00073 dx[5] = u[0]*u[1]; // Some yaw 00074 00075 return dx; 00076 } 00077 00078 00079 MSLVector Model3DRigidHelical::Integrate(const MSLVector &x, const MSLVector &u, 00080 const double &h) 00081 { 00082 return RungeKuttaIntegrate(x,u,h); 00083 } 00084 00085