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

modelnew.C

Go to the documentation of this file.
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 
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.