getting_started.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00034 #include <acado_toolkit.hpp>
00035 #include <acado_gnuplot.hpp>
00036 
00037 
00038 int main( )
00039 {
00040     USING_NAMESPACE_ACADO
00041 
00042 
00043     // INTRODUCE THE VARIABLES:
00044     // -------------------------
00045         DifferentialState xB; //Body Position
00046         DifferentialState xW; //Wheel Position
00047         DifferentialState vB; //Body Velocity
00048         DifferentialState vW; //Wheel Velocity
00049 
00050         Disturbance R;
00051         Control F;
00052 
00053         double mB = 350.0;
00054         double mW = 50.0;
00055         double kS = 20000.0;
00056         double kT = 200000.0;
00057 
00058 
00059     // DEFINE A DIFFERENTIAL EQUATION:
00060     // -------------------------------
00061     DifferentialEquation f;
00062 
00063         f << dot(xB) == vB;
00064         f << dot(xW) == vW;
00065         f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
00066         f << dot(vW) == (  kS*xB - (kT+kS)*xW + kT*R - F ) / mW;
00067 
00068 
00069     // SETTING UP THE (SIMULATED) PROCESS:
00070     // -----------------------------------
00071         OutputFcn identity;
00072         DynamicSystem dynamicSystem( f,identity );
00073 
00074         Process process( dynamicSystem,INT_RK45 );
00075 
00076         VariablesGrid disturbance; disturbance.read( "road.txt" );
00077         if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
00078                 exit( EXIT_FAILURE );
00079 
00080     // DEFINE AN OPTIMAL CONTROL PROBLEM:
00081     // ----------------------------------
00082     Function h;
00083 
00084     h << xB;
00085     h << xW;
00086         h << vB;
00087     h << vW;
00088         h << F;
00089 
00090     DMatrix Q = zeros<double>(5,5); // LSQ coefficient matrix
00091         Q(0,0) = 10.0;
00092         Q(1,1) = 10.0;
00093         Q(2,2) = 1.0;
00094         Q(3,3) = 1.0;
00095         Q(4,4) = 1.0e-8;
00096 
00097     DVector r(5); // Reference
00098     r.setAll( 0.0 );
00099 
00100 
00101     const double tStart = 0.0;
00102     const double tEnd   = 1.0;
00103 
00104     OCP ocp( tStart, tEnd, 20 );
00105 
00106     ocp.minimizeLSQ( Q, h, r );
00107 
00108         ocp.subjectTo( f );
00109 
00110         ocp.subjectTo( -200.0 <= F <= 200.0 );
00111         ocp.subjectTo( R == 0.0 );
00112 
00113 
00114     // SETTING UP THE MPC CONTROLLER:
00115     // ------------------------------
00116         RealTimeAlgorithm alg( ocp,0.05 );
00117         alg.set( INTEGRATOR_TYPE, INT_RK78 );
00118         alg.set( DYNAMIC_SENSITIVITY,FORWARD_SENSITIVITY );
00119 //      alg.set( MAX_NUM_ITERATIONS, 2 );
00120 //      alg.set( USE_IMMEDIATE_FEEDBACK,YES );
00121 
00122         StaticReferenceTrajectory zeroReference;
00123 
00124         Controller controller( alg,zeroReference );
00125 
00126 
00127     // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
00128     // ----------------------------------------------------------
00129         SimulationEnvironment sim( 0.0,2.5,process,controller );
00130 
00131         DVector x0(4);
00132         x0.setZero();
00133 
00134         if (sim.init( x0 ) != SUCCESSFUL_RETURN)
00135                 exit( EXIT_FAILURE );
00136         if (sim.run( ) != SUCCESSFUL_RETURN)
00137                 exit( EXIT_FAILURE );
00138 
00139 
00140     // ... AND PLOT THE RESULTS
00141     // ------------------------
00142         VariablesGrid diffStates;
00143         sim.getProcessDifferentialStates( diffStates );
00144 
00145         VariablesGrid feedbackControl;
00146         sim.getFeedbackControl( feedbackControl );
00147 
00148         GnuplotWindow window;
00149         window.addSubplot( diffStates(0),   "Body Position [m]" );
00150         window.addSubplot( diffStates(1),   "Wheel Position [m]" );
00151         window.addSubplot( diffStates(2),   "Body Velocity [m/s]" );
00152         window.addSubplot( diffStates(3),   "Wheel Velocity [m/s]" );
00153         window.addSubplot( feedbackControl, "Damping Force [N]" );
00154         window.addSubplot( disturbance,     "Road Excitation [m]" );
00155         window.plot( );
00156 
00157     return EXIT_SUCCESS;
00158 }
00159 
00160 
00161 


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:14