00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00032 #include <acado/code_generation/export_exact_hessian_qpdunes.hpp>
00033 #include <acado/code_generation/export_qpdunes_interface.hpp>
00034
00035 BEGIN_NAMESPACE_ACADO
00036
00037 using namespace std;
00038
00039 ExportExactHessianQpDunes::ExportExactHessianQpDunes( UserInteraction* _userInteraction,
00040 const std::string& _commonHeaderName
00041 ) : ExportGaussNewtonQpDunes( _userInteraction,_commonHeaderName )
00042 {}
00043
00044 returnValue ExportExactHessianQpDunes::setup( )
00045 {
00046 LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00047 setupInitialization();
00048
00049
00050
00051
00052 initialize << "for( ret = 0; ret < ACADO_N*(ACADO_NX+ACADO_NU)*(ACADO_NX+ACADO_NU)+ACADO_NX*ACADO_NX; ret++ ) acadoWorkspace.qpH[ret] = 1.0;\n";
00053 ExportFunction initializeQpDunes( "initializeQpDunes" );
00054 initialize
00055 << "ret = (int)initializeQpDunes();\n"
00056 << "if ((return_t)ret != QPDUNES_OK) return ret;\n";
00057
00058 cleanup.setup( "cleanupSolver" );
00059 ExportFunction cleanupQpDunes( "cleanupQpDunes" );
00060 cleanup.addFunctionCall( cleanupQpDunes );
00061 LOG( LVL_DEBUG ) << "done!" << endl;
00062
00063 LOG( LVL_DEBUG ) << "Solver: setup setupVariables... " << endl;
00064 setupVariables();
00065 LOG( LVL_DEBUG ) << "done!" << endl;
00066
00067 LOG( LVL_DEBUG ) << "Solver: setup setupSimulation... " << endl;
00068 setupSimulation();
00069 LOG( LVL_DEBUG ) << "done!" << endl;
00070
00071 LOG( LVL_DEBUG ) << "Solver: setup setupObjectiveEvaluation... " << endl;
00072 setupObjectiveEvaluation();
00073 LOG( LVL_DEBUG ) << "done!" << endl;
00074
00075 LOG( LVL_DEBUG ) << "Solver: setup setupConstraintsEvaluation... " << endl;
00076 setupConstraintsEvaluation();
00077 LOG( LVL_DEBUG ) << "done!" << endl;
00078
00079 LOG( LVL_DEBUG ) << "Solver: setup hessian regularization... " << endl;
00080 setupHessianRegularization();
00081 LOG( LVL_DEBUG ) << "done!" << endl;
00082
00083 LOG( LVL_DEBUG ) << "Solver: setup Evaluation... " << endl;
00084 setupEvaluation();
00085 LOG( LVL_DEBUG ) << "done!" << endl;
00086
00087 LOG( LVL_DEBUG ) << "Solver: setup setupAuxiliaryFunctions... " << endl;
00088 setupAuxiliaryFunctions();
00089 LOG( LVL_DEBUG ) << "done!" << endl;
00090
00091 return SUCCESSFUL_RETURN;
00092 }
00093
00094 returnValue ExportExactHessianQpDunes::getFunctionDeclarations( ExportStatementBlock& declarations
00095 ) const
00096 {
00097 ExportGaussNewtonQpDunes::getFunctionDeclarations( declarations );
00098
00099 declarations.addDeclaration( regularization );
00100
00101 return SUCCESSFUL_RETURN;
00102 }
00103
00104 returnValue ExportExactHessianQpDunes::getCode( ExportStatementBlock& code
00105 )
00106 {
00107 setupQPInterface();
00108 code.addStatement( *qpInterface );
00109
00110 code.addLinebreak( 2 );
00111 code.addStatement( "/******************************************************************************/\n" );
00112 code.addStatement( "/* */\n" );
00113 code.addStatement( "/* ACADO code generation */\n" );
00114 code.addStatement( "/* */\n" );
00115 code.addStatement( "/******************************************************************************/\n" );
00116 code.addLinebreak( 2 );
00117
00118 int useOMP;
00119 get(CG_USE_OPENMP, useOMP);
00120 if ( useOMP )
00121 {
00122 code.addDeclaration( state );
00123 }
00124
00125 code.addFunction( modelSimulation );
00126
00127 code.addFunction( evaluateStageCost );
00128 code.addFunction( evaluateTerminalCost );
00129 code.addFunction( setObjQ1Q2 );
00130 code.addFunction( setObjR1R2 );
00131 code.addFunction( setObjQN1QN2 );
00132 code.addFunction( setStageH );
00133 code.addFunction( setStagef );
00134 code.addFunction( evaluateObjective );
00135
00136 code.addFunction( regularizeHessian );
00137
00138 code.addFunction( evaluatePathConstraints );
00139
00140 for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00141 {
00142 if (evaluatePointConstraints[ i ] == 0)
00143 continue;
00144 code.addFunction( *evaluatePointConstraints[ i ] );
00145 }
00146
00147 code.addFunction( setStagePac );
00148 code.addFunction( evaluateConstraints );
00149
00150 code.addFunction( acc );
00151
00152 code.addFunction( preparation );
00153 code.addFunction( feedback );
00154
00155 code.addFunction( initialize );
00156 code.addFunction( initializeNodes );
00157 code.addFunction( shiftStates );
00158 code.addFunction( shiftControls );
00159 code.addFunction( getKKT );
00160 code.addFunction( getObjective );
00161
00162 code.addFunction( cleanup );
00163 code.addFunction( shiftQpData );
00164
00165 return SUCCESSFUL_RETURN;
00166 }
00167
00168
00169
00170
00171
00172 returnValue ExportExactHessianQpDunes::setupObjectiveEvaluation( void )
00173 {
00174 evaluateObjective.setup("evaluateObjective");
00175
00176
00177
00178
00179 ExportIndex runObj( "runObj" );
00180 ExportForLoop loopObjective( runObj, 0, N );
00181
00182 evaluateObjective.addIndex( runObj );
00183
00184
00185 qpH.setup("qpH", N * (NX + NU) * (NX + NU) + NX * NX, 1, REAL, ACADO_WORKSPACE);
00186 qpg.setup("qpG", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
00187
00188
00189
00190 ExportVariable evLmX = zeros<double>(NX, NX);
00191 ExportVariable evLmU = zeros<double>(NU, NU);
00192
00193 if (levenbergMarquardt > 0.0)
00194 {
00195 DMatrix lmX = eye<double>( NX );
00196 lmX *= levenbergMarquardt;
00197
00198 DMatrix lmU = eye<double>( NU );
00199 lmU *= levenbergMarquardt;
00200
00201 evLmX = lmX;
00202 evLmU = lmU;
00203 }
00204
00205 ExportVariable stagef;
00206 stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
00207
00208 ExportVariable stageH;
00209 stageH.setup("stageH", NX + NU, NX + NU, REAL, ACADO_LOCAL);
00210
00211 if( evaluateStageCost.getFunctionDim() > 0 ) {
00212 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00213 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00214 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od );
00215 loopObjective.addLinebreak( );
00216
00217
00218 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00219 loopObjective.addLinebreak( );
00220
00221 ExportVariable tmpFxx, tmpFxu, tmpFuu;
00222 tmpFxx.setup("tmpFxx", NX, NX, REAL, ACADO_LOCAL);
00223 tmpFxu.setup("tmpFxu", NX, NU, REAL, ACADO_LOCAL);
00224 tmpFuu.setup("tmpFuu", NU, NU, REAL, ACADO_LOCAL);
00225
00226 setStageH.setup("addObjTerm", tmpFxx, tmpFxu, tmpFuu, stageH);
00227 setStageH.addStatement( stageH.getSubMatrix(0,NX,0,NX) += tmpFxx + evLmX );
00228 setStageH.addStatement( stageH.getSubMatrix(0,NX,NX,NX+NU) += tmpFxu );
00229 setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,0,NX) += tmpFxu.getTranspose() );
00230 setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,NX,NX+NU) += tmpFuu + evLmU );
00231
00232 loopObjective.addFunctionCall(
00233 setStageH, objValueOut.getAddress(0, 1+NX+NU), objValueOut.getAddress(0, 1+NX+NU+NX*NX),
00234 objValueOut.getAddress(0, 1+NX+NU+NX*(NX+NU)), objS.getAddress(runObj*(NX+NU), 0) );
00235
00236 ExportVariable tmpDF;
00237 tmpDF.setup("tmpDF", NX+NU, 1, REAL, ACADO_LOCAL);
00238 setStagef.setup("addObjLinearTerm", tmpDF, stagef);
00239 setStagef.addStatement( stagef == tmpDF.getRows(0,NX+NU) );
00240
00241 loopObjective.addFunctionCall(
00242 setStagef, objValueOut.getAddress(0, 1), qpg.getAddress(runObj * (NX+NU)) );
00243
00244 loopObjective.addLinebreak( );
00245 }
00246 else {
00247 if(levenbergMarquardt > 0.0) {
00248 setStageH.setup("addObjTerm", stageH);
00249 setStageH.addStatement( stageH.getSubMatrix(0,NX,0,NX) += evLmX );
00250 setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,NX,NX+NU) += evLmU );
00251
00252 loopObjective.addFunctionCall( setStageH, objS.getAddress(runObj*(NX+NU), 0) );
00253 }
00254 DMatrix D(NX+NU,1); D.setAll(0);
00255 loopObjective.addStatement( qpg.getRows(runObj*(NX+NU), runObj*(NX+NU)+NX+NU) == D );
00256 }
00257
00258 evaluateObjective.addStatement( loopObjective );
00259
00260
00261
00262
00263 if( evaluateTerminalCost.getFunctionDim() > 0 ) {
00264 evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00265 evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od );
00266
00267
00268 evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00269 evaluateObjective.addLinebreak( );
00270
00271 evaluateObjective.addStatement( objSEndTerm.makeRowVector() == objValueOut.getCols(1+NX,1+NX+NX*NX) + evLmX.makeRowVector() );
00272 evaluateObjective.addStatement( qpg.getRows(N * NX, (N + 1) * NX) == objValueOut.getCols(1,1+NX).getTranspose() );
00273
00274 evaluateObjective.addLinebreak( );
00275 }
00276 else {
00277 if(levenbergMarquardt > 0.0) {
00278 evaluateObjective.addStatement( objSEndTerm == evLmX );
00279 }
00280 else {
00281 DMatrix hess(NX,NX); hess.setAll(0);
00282 evaluateObjective.addStatement( objSEndTerm == hess );
00283 }
00284
00285 DMatrix Dx(NX,1); Dx.setAll(0);
00286 evaluateObjective.addStatement( qpg.getRows(N*NX, (N+1)*NX) == Dx );
00287 }
00288
00289 return SUCCESSFUL_RETURN;
00290 }
00291
00292 returnValue ExportExactHessianQpDunes::setupHessianRegularization( )
00293 {
00294 ExportVariable block( "hessian_block", NX+NU, NX+NU );
00295 regularization = ExportFunction( "acado_regularize", block );
00296 regularization.doc( "EVD-based regularization of a Hessian block." );
00297 regularization.addLinebreak();
00298
00299 regularizeHessian.setup( "regularizeHessian" );
00300 regularizeHessian.doc( "Regularization procedure of the computed exact Hessian." );
00301
00302 ExportIndex oInd;
00303 regularizeHessian.acquire( oInd );
00304
00305 ExportForLoop loopObjective(oInd, 0, N);
00306 loopObjective.addFunctionCall( regularization, objS.getAddress(oInd*(NX+NU),0) );
00307 for( uint row = 0; row < NX+NU; row++ ) {
00308 loopObjective.addStatement( qpH.getRows((oInd*(NX+NU)+row)*(NX+NU),(oInd*(NX+NU)+row+1)*(NX+NU)) == objS.getRow(oInd*(NX+NU)+row).getTranspose() );
00309 }
00310 regularizeHessian.addStatement( loopObjective );
00311
00312 regularizeHessian.addStatement( qpH.getRows(N*(NX+NU)*(NX+NU), N*(NX+NU)*(NX+NU)+NX*NX) == objSEndTerm.makeColVector() );
00313
00314 return SUCCESSFUL_RETURN;
00315 }
00316
00317 CLOSE_NAMESPACE_ACADO