ocp_export.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 
00032 #include <acado/code_generation/ocp_export.hpp>
00033 #include <acado/code_generation/export_nlp_solver.hpp>
00034 #include <acado/code_generation/export_simulink_interface.hpp>
00035 #include <acado/code_generation/export_auxiliary_functions.hpp>
00036 #include <acado/code_generation/export_hessian_regularization.hpp>
00037 #include <acado/code_generation/export_common_header.hpp>
00038 
00039 #include <acado/code_generation/templates/templates.hpp>
00040 
00041 #include <acado/objective/objective.hpp>
00042 #include <acado/ocp/ocp.hpp>
00043 
00044 using namespace std;
00045 
00046 BEGIN_NAMESPACE_ACADO
00047 
00048 OCPexport::OCPexport( ) : ExportModule( )
00049 {
00050         setStatus( BS_NOT_INITIALIZED );
00051 }
00052 
00053 
00054 OCPexport::OCPexport(   const OCP& _ocp
00055                                                 ) : ExportModule( )
00056 {
00057         ocp = _ocp;
00058 
00059         setStatus( BS_NOT_INITIALIZED );
00060 }
00061 
00062 returnValue OCPexport::exportCode(      const std::string& dirName,
00063                                                                         const std::string& _realString,
00064                                                                         const std::string& _intString,
00065                                                                         int _precision
00066                                                                         )
00067 {
00068         int qpSolver;
00069         get(QP_SOLVER, qpSolver);
00070         string moduleName;
00071         get(CG_MODULE_NAME, moduleName);
00072 
00073         acadoPrintCopyrightNotice( "Code Generation Tool" );
00074 
00075         //
00076         // Create the export folders
00077         //
00078 
00079         set(CG_EXPORT_FOLDER_NAME, dirName);
00080 
00081         returnValue dirStatus = acadoCreateFolder( dirName );
00082         if (dirStatus != SUCCESSFUL_RETURN)
00083                 return dirStatus;
00084 
00085         //
00086         // Setup the export structures
00087         //
00088         returnValue setupStatus = setup( );
00089         if ( setupStatus != SUCCESSFUL_RETURN )
00090                 return setupStatus;
00091 
00092         //
00093         // Export common header
00094         //
00095         if (exportAcadoHeader(dirName, commonHeaderName, _realString, _intString, _precision)
00096                         != SUCCESSFUL_RETURN )
00097                 return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00098 
00099         //
00100         // Export integrator
00101         //
00102         if (integrator != 0)
00103         {
00104                 ExportFile integratorFile(dirName + "/" + moduleName + "_integrator.c",
00105                                 commonHeaderName, _realString, _intString, _precision);
00106 
00107                 integrator->getCode( integratorFile );
00108 
00109                 if (integratorFile.exportCode( ) != SUCCESSFUL_RETURN)
00110                         return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00111         }
00112         else
00113                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00114 
00115         //
00116         // Export solver
00117         //
00118         if( solver != 0 )
00119         {
00120                 ExportFile solverFile(dirName + "/" + moduleName + "_solver.c",
00121                                 commonHeaderName, _realString, _intString, _precision);
00122 
00123                 solver->getCode( solverFile );
00124 
00125                 if ( solverFile.exportCode( ) != SUCCESSFUL_RETURN )
00126                         return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00127         }
00128         else
00129                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00130 
00131         LOG( LVL_DEBUG ) << "Export templates" << endl;
00132 
00133         //
00134         // Export auxiliary functions, always
00135         //
00136         std::string str;
00137 
00138         ExportAuxiliaryFunctions eaf(
00139                         dirName + string("/") + moduleName + "_auxiliary_functions.h",
00140                         dirName + string("/") + moduleName + "_auxiliary_functions.c",
00141                         moduleName
00142                         );
00143         eaf.configure();
00144         eaf.exportCode();
00145 
00146         //
00147         // Export Makefile
00148         //
00149         int generateMakeFile;
00150         get(GENERATE_MAKE_FILE, generateMakeFile);
00151 
00152         if ( (bool)generateMakeFile == true )
00153         {
00154                 str = dirName + "/Makefile";
00155 
00156                 switch ( (QPSolverName)qpSolver )
00157                 {
00158                         case QP_QPOASES:
00159                                 acadoCopyTemplateFile(MAKEFILE_QPOASES, str, "#", true);
00160                                 break;
00161 
00162                         case QP_FORCES:
00163                                 acadoCopyTemplateFile(MAKEFILE_FORCES, str, "#", true);
00164                                 break;
00165 
00166                         case QP_QPDUNES:
00167                                 acadoCopyTemplateFile(MAKEFILE_QPDUNES, str, "#", true);
00168                                 break;
00169 
00170                         case QP_HPMPC:
00171                                 acadoCopyTemplateFile(MAKEFILE_HPMPC, str, "#", true);
00172                                 break;
00173 
00174                         default:
00175                                 ACADOWARNINGTEXT(RET_NOT_IMPLEMENTED_YET, "Makefile is not yet available.");
00176                                 break;
00177                 }
00178         }
00179 
00180         //
00181         // Export a dummy test file
00182         //
00183         int generateTestFile;
00184         get(GENERATE_TEST_FILE, generateTestFile);
00185         string testFileName = dirName + "/test.c";
00186         if ((bool) generateTestFile == true)
00187                 acadoCopyTemplateFile(DUMMY_TEST_FILE, testFileName, "", true);
00188 
00189         //
00190         // Generate MATLAB MEX interface
00191         //
00192         int generateMexInterface;
00193         get(GENERATE_MATLAB_INTERFACE, generateMexInterface);
00194         int hessianApproximation;
00195         get( HESSIAN_APPROXIMATION, hessianApproximation );
00196         if ( (bool)generateMexInterface == true )
00197         {
00198                 str = dirName + "/" + moduleName + "_solver_mex.c";
00199 
00200                 if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN ) {
00201                         acadoCopyTemplateFile(EH_SOLVER_MEX, str, "", true);
00202                 }
00203                 else {
00204                         acadoCopyTemplateFile(SOLVER_MEX, str, "", true);
00205                 }
00206 
00207                 str = dirName + "/make_" + moduleName + "_solver.m";
00208 
00209                 switch ( (QPSolverName)qpSolver )
00210                 {
00211                 case QP_QPOASES:
00212                         if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN ) {
00213                                 acadoCopyTemplateFile(MAKE_MEX_EH_QPOASES, str, "%", true);
00214                         }
00215                         else {
00216                                 acadoCopyTemplateFile(MAKE_MEX_QPOASES, str, "%", true);
00217                         }
00218                         break;
00219 
00220                 case QP_FORCES:
00221                         acadoCopyTemplateFile(MAKE_MEX_FORCES, str, "%", true);
00222                         break;
00223 
00224                 case QP_QPDUNES:
00225                         if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN ) {
00226                                 acadoCopyTemplateFile(MAKE_MEX_EH_QPDUNES, str, "%", true);
00227                         }
00228                         else {
00229                                 acadoCopyTemplateFile(MAKE_MEX_QPDUNES, str, "%", true);
00230                         }
00231                         break;
00232                         
00233                 case QP_QPDUNES2:
00234 
00235                 default:
00236                         ACADOWARNINGTEXT(RET_NOT_IMPLEMENTED_YET, "MEX interface is not yet available.");
00237                         break;
00238                 }
00239         }
00240 
00241         //
00242         // Generate MATLAB Simulink interface
00243         //
00244         int generateSimulinkInterface;
00245         get(GENERATE_SIMULINK_INTERFACE, generateSimulinkInterface);
00246         if ((bool) generateSimulinkInterface == true)
00247         {
00248                 if (!((QPSolverName)qpSolver == QP_QPOASES || (QPSolverName)qpSolver == QP_QPDUNES))
00249                         ACADOWARNINGTEXT(RET_NOT_IMPLEMENTED_YET,
00250                                         "At the moment, Simulink interface is available only with qpOASES and qpDUNES based OCP solvers.");
00251                 else
00252                 {
00253                         string makefileName = dirName + "/make_" + moduleName + "_solver_sfunction.m";
00254                         string wrapperHeaderName = dirName + "/" + moduleName + "_solver_sfunction.h";
00255                         string wrapperSourceName = dirName + "/" + moduleName + "_solver_sfunction.c";
00256                         string qpSolverString;
00257 
00258                         if ((QPSolverName)qpSolver == QP_QPOASES)
00259                                 qpSolverString = "QPOASES";
00260                         else
00261                                 qpSolverString = "QPDUNES";
00262 
00263                         ExportSimulinkInterface esi(makefileName, wrapperHeaderName, wrapperSourceName, moduleName);
00264 
00265                         // Get options
00266                         int useSinglePrecision;
00267                         get(USE_SINGLE_PRECISION, useSinglePrecision);
00268 
00269                         int hardcodeConstraintValues;
00270                         get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00271                         if ((bool)hardcodeConstraintValues == false)
00272                                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00273 
00274                         int fixInitialState;
00275                         get(FIX_INITIAL_STATE, fixInitialState);
00276                         int useAC;
00277                         get(CG_USE_ARRIVAL_COST, useAC);
00278                         int covCalc;
00279                         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
00280 
00281                         // Configure templates
00282                         esi.configure(
00283                                         ocp.getN(), ocp.getNX(), ocp.getNDX(), ocp.getNXA(), ocp.getNU(), ocp.getNOD(),
00284                                         solver->getNY(), solver->getNYN(),
00285                                         (bool)fixInitialState,
00286                                         (unsigned)solver->weightingMatricesType(),
00287                                         (bool)hardcodeConstraintValues,
00288                                         (bool)useAC,
00289                                         (bool)covCalc,
00290                                         qpSolverString);
00291 
00292                         esi.exportCode();
00293                 }
00294         }
00295 
00296         //
00297         // Generate Symmetric EVD code
00298         //
00299         if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN ) {
00300 //              LOG( LVL_DEBUG ) << "Exporting Hessian regularization code... " << endl;
00301                 ExportHessianRegularization evd(
00302                                 dirName + string("/") + moduleName + "_hessian_regularization.c",
00303                                 moduleName
00304                 );
00305                 evd.configure( ocp.getNX()+ocp.getNU(), 1e-12 );
00306                 if ( evd.exportCode() != SUCCESSFUL_RETURN )
00307                         return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00308         }
00309 
00310     return SUCCESSFUL_RETURN;
00311 }
00312 
00313 
00314 
00315 returnValue OCPexport::printDimensionsQP( )
00316 {
00317         if (getStatus() != BS_READY)
00318                 return SUCCESSFUL_RETURN;
00319 
00320         LOG( LVL_INFO ) << "ACADO Code Generation Tool:" << endl
00321                         << "\t* Number of QP variables: " << solver->getNumQPvars( ) << endl
00322                         << "\t* Number of path and point constraints: " << solver->getNumComplexConstraints() << endl;
00323 
00324         return SUCCESSFUL_RETURN;
00325 }
00326 
00327 returnValue OCPexport::setup( )
00328 {
00329         // Nothing to do as object is up-to-date
00330         if ( getStatus() == BS_READY )
00331                 return SUCCESSFUL_RETURN;
00332 
00333         // Consistency check
00334         returnValue returnvalue = checkConsistency( );
00335         if ( returnvalue != SUCCESSFUL_RETURN )
00336                 return returnvalue;
00337 
00338         //
00339         // Set common header name
00340         //
00341         string moduleName;
00342         get(CG_MODULE_NAME, moduleName);
00343         commonHeaderName = moduleName + "_common.h";
00344 
00345         //
00346         // Prepare integrator export
00347         //
00348         int numSteps;
00349         get(NUM_INTEGRATOR_STEPS, numSteps);
00350 
00351         int integratorType;
00352         get(INTEGRATOR_TYPE, integratorType);
00353 
00354         integrator = IntegratorExportPtr(
00355                         IntegratorExportFactory::instance().createAlgorithm(this, commonHeaderName, static_cast<ExportIntegratorType>(integratorType)));
00356         if (integrator == 0)
00357                 return ACADOERROR( RET_INVALID_OPTION );
00358 
00359         ocp.setNumberIntegrationSteps( numSteps );
00360         // NOTE: This function internally calls setup() function
00361         integrator->setModelData( ocp.getModelData() );
00362 
00363         //
00364         // Prepare solver export
00365         //
00366 
00367         int qpSolver;
00368         get(QP_SOLVER, qpSolver);
00369         int qpSolution;
00370         get(SPARSE_QP_SOLUTION, qpSolution);
00371         int hessianApproximation;
00372         get( HESSIAN_APPROXIMATION, hessianApproximation );
00373 
00374         // TODO Extend ExportNLPSolver ctor to accept OCP reference.
00375 
00376         switch ( (SparseQPsolutionMethods)qpSolution )
00377         {
00378         case FULL_CONDENSING:
00379         case CONDENSING:
00380 
00381                 if ((QPSolverName)qpSolver != QP_QPOASES)
00382                         return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
00383                                         "For condensed solution only qpOASES QP solver is supported");
00384 
00385                 solver = ExportNLPSolverPtr(
00386                                 NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_CONDENSED));
00387 
00388                 break;
00389 
00390         case FULL_CONDENSING_N2:
00391         case CONDENSING_N2:
00392 
00393                 if ((QPSolverName)qpSolver != QP_QPOASES)
00394                         return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
00395                                         "For condensed solution only qpOASES QP solver is supported");
00396 
00397                 if ( (HessianApproximationMode)hessianApproximation == GAUSS_NEWTON ) {
00398                         solver = ExportNLPSolverPtr(
00399                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_CN2));
00400                 }
00401                 else if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN ) {
00402                         solver = ExportNLPSolverPtr(
00403                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, EXACT_HESSIAN_CN2));
00404                 }
00405                 else {
00406                         return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Only Gauss-Newton and Exact Hessian methods are currently supported");
00407                 }
00408 
00409                 break;
00410 
00411         case FULL_CONDENSING_N2_FACTORIZATION:
00412 
00413                         if ((QPSolverName)qpSolver != QP_QPOASES)
00414                                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
00415                                                 "For condensed solution only qpOASES QP solver is supported");
00416 
00417                         solver = ExportNLPSolverPtr(
00418                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_CN2_FACTORIZATION));
00419 
00420                         break;
00421 
00422         case SPARSE_SOLVER:
00423                 if ((QPSolverName)qpSolver != QP_FORCES && (QPSolverName)qpSolver != QP_QPDUNES && (QPSolverName)qpSolver != QP_QPDUNES2 && (QPSolverName)qpSolver != QP_HPMPC)
00424                         return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
00425                                         "For sparse solution FORCES and qpDUNES QP solvers are supported");
00426                 if ( (QPSolverName)qpSolver == QP_FORCES)
00427                         solver = ExportNLPSolverPtr(
00428                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_FORCES));
00429                 else if ((QPSolverName)qpSolver == QP_QPDUNES) {
00430                         if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN ) {
00431                                 solver = ExportNLPSolverPtr(
00432                                                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, EXACT_HESSIAN_QPDUNES));
00433                         }
00434                         else {
00435                                 solver = ExportNLPSolverPtr(
00436                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_QPDUNES));
00437                         }
00438                 }
00439                 else if ((QPSolverName)qpSolver == QP_QPDUNES2)
00440                         solver = ExportNLPSolverPtr(
00441                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_QPDUNES2));
00442                 else if ((QPSolverName)qpSolver == QP_HPMPC)
00443                         solver = ExportNLPSolverPtr(
00444                                         NLPSolverFactory::instance().createAlgorithm(this, commonHeaderName, GAUSS_NEWTON_HPMPC));
00445                 break;
00446 
00447         default:
00448                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "QP solver option is invalid");
00449         }
00450         if (solver == 0)
00451                 return ACADOERRORTEXT(RET_INVALID_OPTION, "Cannot allocate the solver object");
00452 
00453         solver->setDimensions(ocp.getNX(), ocp.getNDX(), ocp.getNXA(), ocp.getNU(), ocp.getNP(), ocp.getN(), ocp.getNOD());
00454         solver->setIntegratorExport( integrator );
00455 
00456         Objective objective;
00457         ocp.getObjective( objective );
00458 
00459         returnValue statusObjective;
00460         statusObjective = solver->setObjective( objective );
00461         if (statusObjective != SUCCESSFUL_RETURN)
00462                 return ACADOERRORTEXT(status, "Error in retrieving the objective.");
00463 
00464         solver->setConstraints( ocp );
00465 
00466         // Get LM multiplier
00467         double levenbergMarquardt;
00468         get( LEVENBERG_MARQUARDT,levenbergMarquardt );
00469 
00470         solver->setLevenbergMarquardt( levenbergMarquardt );
00471 
00472         solver->setup( );
00473 
00474         setStatus( BS_READY );
00475 
00476         return SUCCESSFUL_RETURN;
00477 }
00478 
00479 
00480 returnValue OCPexport::checkConsistency( ) const
00481 {
00482         //
00483         // Consistency checks:
00484         //
00485         Objective objective;
00486         ocp.getObjective( objective );
00487         int hessianApproximation;
00488         get( HESSIAN_APPROXIMATION, hessianApproximation );
00489 
00490         if ( ocp.hasObjective( ) == true && !((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN &&
00491                         (objective.getNumMayerTerms() == 1 || objective.getNumLagrangeTerms() == 1)) ) { // for Exact Hessian RTI
00492                 return ACADOERROR( RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT );
00493         }
00494 
00495 
00496         int sensitivityProp;
00497         get(DYNAMIC_SENSITIVITY, sensitivityProp);
00498 
00499         if( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN && (ExportSensitivityType) sensitivityProp != THREE_SWEEPS ) {
00500                 return ACADOERROR( RET_INVALID_OPTION );
00501         }
00502 
00503         DifferentialEquation f;
00504         ocp.getModel( f );
00505 
00506 //      if ( f.isDiscretized( ) == BT_TRUE )
00507 //              return ACADOERROR( RET_NO_DISCRETE_ODE_FOR_CODE_EXPORT );
00508 
00509         if ( f.getNUI( ) > 0 )
00510                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00511 
00512         if ( f.getNP( ) > 0 )
00513                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
00514                                 "Free parameters are not supported. For the old functionality use OnlineData class.");
00515 
00516         if ( (HessianApproximationMode)hessianApproximation != GAUSS_NEWTON && (HessianApproximationMode)hessianApproximation != EXACT_HESSIAN )
00517                 return ACADOERROR( RET_INVALID_OPTION );
00518 
00519         int discretizationType;
00520         get( DISCRETIZATION_TYPE,discretizationType );
00521         if ( ( (StateDiscretizationType)discretizationType != SINGLE_SHOOTING ) &&
00522                         ( (StateDiscretizationType)discretizationType != MULTIPLE_SHOOTING ) )
00523                 return ACADOERROR( RET_INVALID_OPTION );
00524 
00525         return SUCCESSFUL_RETURN;
00526 }
00527 
00528 
00529 returnValue OCPexport::collectDataDeclarations( ExportStatementBlock& declarations,
00530                                                                                                 ExportStruct dataStruct
00531                                                                                                 ) const
00532 {
00533         if (integrator->getDataDeclarations(declarations, dataStruct) != SUCCESSFUL_RETURN)
00534                 return RET_UNABLE_TO_EXPORT_CODE;
00535 
00536         if (solver->getDataDeclarations(declarations, dataStruct) != SUCCESSFUL_RETURN)
00537                 return RET_UNABLE_TO_EXPORT_CODE;
00538 
00539         return SUCCESSFUL_RETURN;
00540 }
00541 
00542 
00543 returnValue OCPexport::collectFunctionDeclarations(     ExportStatementBlock& declarations
00544                                                                                                         ) const
00545 {
00546         if (integrator->getFunctionDeclarations( declarations ) != SUCCESSFUL_RETURN)
00547                 return RET_UNABLE_TO_EXPORT_CODE;
00548 
00549         if (solver->getFunctionDeclarations( declarations ) != SUCCESSFUL_RETURN)
00550                 return RET_UNABLE_TO_EXPORT_CODE;
00551 
00552         return SUCCESSFUL_RETURN;
00553 }
00554 
00555 returnValue OCPexport::exportAcadoHeader(       const std::string& _dirName,
00556                                                                                         const std::string& _fileName,
00557                                                                                         const std::string& _realString,
00558                                                                                         const std::string& _intString,
00559                                                                                         int _precision
00560                                                                                         ) const
00561 {
00562         string moduleName;
00563         get(CG_MODULE_NAME, moduleName);
00564 
00565         int qpSolver;
00566         get(QP_SOLVER, qpSolver);
00567 
00568         int useSinglePrecision;
00569         get(USE_SINGLE_PRECISION, useSinglePrecision);
00570 
00571         int hardcodeConstraintValues;
00572         get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00573 
00574         int fixInitialState;
00575         get(FIX_INITIAL_STATE, fixInitialState);
00576         int useAC;
00577         get(CG_USE_ARRIVAL_COST, useAC);
00578         int covCalc;
00579         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
00580 
00581         string fileName;
00582         fileName = _dirName + "/" + _fileName;
00583 
00584         map<string, pair<string, string> > options;
00585 
00586         options[ "ACADO_N" ]   = make_pair(toString( ocp.getN() ),   "Number of control/estimation intervals.");
00587         options[ "ACADO_NX" ]  = make_pair(toString( ocp.getNX() ),  "Number of differential variables.");
00588         options[ "ACADO_NXD" ] = make_pair(toString( ocp.getNDX() ), "Number of differential derivative variables.");
00589         options[ "ACADO_NXA" ] = make_pair(toString( ocp.getNXA() ), "Number of algebraic variables.");
00590         options[ "ACADO_NU" ]  = make_pair(toString( ocp.getNU() ),  "Number of control variables.");
00591         options[ "ACADO_NOD" ]  = make_pair(toString( ocp.getNOD() ),  "Number of online data values.");
00592         options[ "ACADO_NY" ]  = make_pair(toString( solver->getNY() ),  "Number of references/measurements per node on the first N nodes.");
00593         options[ "ACADO_NYN" ] = make_pair(toString( solver->getNYN() ), "Number of references/measurements on the last (N + 1)st node.");
00594 
00595         options[ "ACADO_INITIAL_STATE_FIXED" ] =
00596                         make_pair(toString( fixInitialState ), "Indicator for fixed initial state.");
00597         options[ "ACADO_WEIGHTING_MATRICES_TYPE" ] =
00598                         make_pair(toString( (unsigned)solver->weightingMatricesType() ), "Indicator for type of fixed weighting matrices.");
00599         options[ "ACADO_USE_LINEAR_TERMS" ] =
00600                                 make_pair(toString( (unsigned)solver->usingLinearTerms() ), "Indicator for usage of non-hard-coded linear terms in the objective.");
00601         options[ "ACADO_HARDCODED_CONSTRAINT_VALUES" ] =
00602                         make_pair(toString( hardcodeConstraintValues ), "Flag indicating whether constraint values are hard-coded or not.");
00603         options[ "ACADO_USE_ARRIVAL_COST" ] =
00604                         make_pair(toString( useAC ), "Providing interface for arrival cost.");
00605         options[ "ACADO_COMPUTE_COVARIANCE_MATRIX" ] =
00606                         make_pair(toString( covCalc ), "Compute covariance matrix of the last state estimate.");
00607 
00608         //
00609         // ACADO variables and workspace
00610         //
00611         ExportStatementBlock variablesBlock;
00612         stringstream variables;
00613 
00614         if (collectDataDeclarations(variablesBlock, ACADO_VARIABLES) != SUCCESSFUL_RETURN)
00615                 return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00616         variablesBlock.exportCode(variables, _realString, _intString, _precision);
00617 
00618         ExportStatementBlock workspaceBlock;
00619         stringstream workspace;
00620 
00621         if (collectDataDeclarations(workspaceBlock, ACADO_WORKSPACE) != SUCCESSFUL_RETURN)
00622                 return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00623         workspaceBlock.exportCode(workspace, _realString, _intString, _precision);
00624 
00625         ExportStatementBlock functionsBlock;
00626         stringstream functions;
00627 
00628         if (collectFunctionDeclarations( functionsBlock ) != SUCCESSFUL_RETURN)
00629                 return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00630         functionsBlock.exportCode(functions, _realString);
00631 
00632         ExportCommonHeader ech(fileName, "", _realString, _intString, _precision);
00633         ech.configure( moduleName, useSinglePrecision, (QPSolverName)qpSolver,
00634                         options, variables.str(), workspace.str(), functions.str());
00635 
00636         return ech.exportCode();
00637 }
00638 
00639 CLOSE_NAMESPACE_ACADO


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:22