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