YANE-Framework Tutorial 1.1.0

examples/mpc/src/main.cpp

00001 /***************************************************************************
00002  *                                                                         *
00003  * Copyright (C) 2011 by www.nonlinearmpc.com                              *
00004  *                                                                         *
00005  * Authors:                                                                *
00006  *  Thomas Gebelein <thomas.gebelein@uni-bayreuth.de>                      *
00007  *  Juergen Pannek <juergen.pannek@googlemail.com>                         *
00008  *                                                                         *
00009  ***************************************************************************/
00010 
00011 #include <iostream>
00012 #include <cmath>
00013 #include <iomanip>
00014 #include <cstdio>
00015 #include <unistd.h>
00016 #include <cstdlib>
00017 
00018 #include <examplemodels2.h>
00019 #include <yane.h>
00020 #include <sqpfortran.h>
00021 //#include <yaneipopt.h>
00022 //#include <sqpnagc3.h>
00023 
00024 #define HORIZON 50
00025 #define H_NEW 0.1
00026 #define INF 1.0E19
00027 #define HORIZONSHIFT 1
00028 #define PRECEDESTEPS 1
00029 
00030 using namespace yane::MPC;
00031 using namespace yane::SqpFortran;
00032 //using namespace yane::SqpNagC3;
00033 //using namespace yane::YaneIpopt;
00034 using namespace yane::Utils;
00035 using namespace std;
00036 
00037 int main ( )
00038 {
00039         double * x, *u, *t;
00040 
00041         yane::Model::Model * model = new yane::ExampleModels2::InvertedPendulum ( );
00042         yane::MPC::ModelShootingData * shooting =
00043                 new yane::ExampleModels2::InvertedPendulumShootingData ( model );
00044         yane::Model::Simulator * simulator = new yane::Model::Simulator ( model );
00045 
00046         //      OdeManager * odeman = new SimpleOdeManager();
00047         OdeManager * odeman = new CacheOdeManager ( );
00048         //      OdeManager * odeman = new SyncOdeManager();
00049         //      OdeManager * odeman = new MultiThreadCacheOdeManager(2);
00050 
00051         //      yane::MinProg::NLP * sqp = new YaneIpopt();
00052         yane::MinProg::NLP * sqp = new SqpFortran ( );
00053         //      yane::MinProg::NLP * sqp = new SqpNagC();
00054 
00055         SqpFortran * sqpSpecify = ( ( SqpFortran * ) sqp );
00056         sqpSpecify->setAccuracy ( 1E-4 );
00057         sqpSpecify->setMaxFun ( 20 );
00058         sqpSpecify->setMaxIterations ( 1000 );
00059         sqpSpecify->setLineSearchTol ( 0.1 );
00060 
00061         MPC * mpc = new MPC ( INF );
00062         mpc->reset ( odeman, sqp, model, HORIZON, PRECEDESTEPS, shooting );
00063 
00064         mpc->allocateMemory ( t, u );
00065         mpc->generateTimeGrid ( t, 0.0, H_NEW );
00066         for ( int i = 0; i < HORIZON; i++ )
00067         {
00068                 model->getDefaultControl ( & u [ i * model->dimensionControl ( ) ] );
00069         }
00070         mpc->initCalc ( t, u );
00071         mpc->resizeHorizon ( 17, H_NEW );
00072         mpc->setConfigShiftHorizon ( STANDARD, CURRENT_DATA );
00073 
00074         x = new double [ model->dimensionState ( ) ];
00075         model->getDefaultState ( x );
00076 
00077         OptimalDiff::calc ( );
00078         Exception::enableDebugMessage ( true );
00079 
00080         double * nextControls = new double [ model->dimensionControl ( )
00081                 * HORIZONSHIFT ];
00082         double * nextTimesteps = new double [ HORIZONSHIFT + 1 ];
00083 
00084         cout
00085                 << "  Time          u        phi      omega          x          v      ms"
00086                 << endl;
00087         cout
00088                 << "====================================================================="
00089                 << endl;
00090         for ( int i = 0; i < 1000; i++ )
00091         {
00092                 RTClock timer;
00093                 try
00094                 {
00095                         mpc->calc ( x );
00096                 }
00097                 catch ( yane::MinProg::SolverWarning e )
00098                 {
00099                         //                      cout << e.what() << endl;
00100                 }
00101 
00102                 try
00103                 {
00104                         mpc->shiftHorizon ( nextControls, nextTimesteps, HORIZONSHIFT, x,
00105                                 H_NEW );
00106                 }
00107                 catch ( yane::MinProg::SolverWarning e )
00108                 {
00109                         //                      cout << e.what() << endl;
00110                 }
00111 
00112                 double duration = timer.elapsedSeconds ( ) * 1000.0;
00113 
00114                 for ( int j = 0; j < HORIZONSHIFT; j++ )
00115                 {
00116                         cout << fixed << setprecision ( 3 ) << setw ( 6 )
00117                                 << nextTimesteps [ j ] << " " << setprecision ( 5 )
00118                                 << setw ( 10 ) << nextControls [ j ] << " ";
00119                         for ( int i = 0; i < 4; i++ )
00120                         {
00121                                 cout << setprecision ( 5 ) << setw ( 10 ) << x [ i ] << " ";
00122                         }
00123                         if ( j == 0 )
00124                         {
00125                                 cout << setw ( 7 ) << setprecision ( 2 ) << duration;
00126                         }
00127                         else
00128                         {
00129                                 cout << "     --";
00130                         }
00131                         cout << endl;
00132 
00133                         simulator->initialize ( nextTimesteps [ j ], x, & nextControls [ j ] );
00134                         simulator->run ( nextTimesteps [ j + 1 ] );
00135 
00136                 }
00137         }
00138 
00139         delete mpc;
00140         delete odeman;
00141         delete sqp;
00142         delete simulator;
00143         delete shooting;
00144         delete model;
00145         delete [ ] t;
00146         delete [ ] u;
00147 
00148         return 0;
00149 }