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_gauss_newton_hpmpc.hpp>
00033 #include <acado/code_generation/export_hpmpc_interface.hpp>
00034
00035 BEGIN_NAMESPACE_ACADO
00036
00037 using namespace std;
00038
00039 ExportGaussNewtonHpmpc::ExportGaussNewtonHpmpc( UserInteraction* _userInteraction,
00040 const std::string& _commonHeaderName
00041 ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00042 {}
00043
00044 returnValue ExportGaussNewtonHpmpc::setup( )
00045 {
00046 setupInitialization();
00047
00048 setupVariables();
00049
00050 setupSimulation();
00051
00052 setupObjectiveEvaluation();
00053
00054 setupConstraintsEvaluation();
00055
00056 setupEvaluation();
00057
00058 setupAuxiliaryFunctions();
00059
00060 setupQPInterface();
00061
00062 return SUCCESSFUL_RETURN;
00063 }
00064
00065 returnValue ExportGaussNewtonHpmpc::getDataDeclarations( ExportStatementBlock& declarations,
00066 ExportStruct dataStruct
00067 ) const
00068 {
00069 returnValue status;
00070 status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00071 if (status != SUCCESSFUL_RETURN)
00072 return status;
00073
00074 declarations.addDeclaration(x0, dataStruct);
00075
00076 if (Q1.isGiven() == true)
00077 declarations.addDeclaration(qpQ, dataStruct);
00078 if (QN1.isGiven() == true)
00079 declarations.addDeclaration(qpQf, dataStruct);
00080 if (S1.isGiven() == true)
00081 declarations.addDeclaration(qpS, dataStruct);
00082 if (R1.isGiven() == true)
00083 declarations.addDeclaration(qpR, dataStruct);
00084
00085 declarations.addDeclaration(qpq, dataStruct);
00086 declarations.addDeclaration(qpqf, dataStruct);
00087 declarations.addDeclaration(qpr, dataStruct);
00088
00089 declarations.addDeclaration(qpx, dataStruct);
00090 declarations.addDeclaration(qpu, dataStruct);
00091
00092 declarations.addDeclaration(qpLb, dataStruct);
00093 declarations.addDeclaration(qpUb, dataStruct);
00094
00095 declarations.addDeclaration(qpLambda, dataStruct);
00096 declarations.addDeclaration(qpMu, dataStruct);
00097 declarations.addDeclaration(qpSlacks, dataStruct);
00098
00099 declarations.addDeclaration(nIt, dataStruct);
00100
00101 return SUCCESSFUL_RETURN;
00102 }
00103
00104 returnValue ExportGaussNewtonHpmpc::getFunctionDeclarations( ExportStatementBlock& declarations
00105 ) const
00106 {
00107 declarations.addDeclaration( preparation );
00108 declarations.addDeclaration( feedback );
00109
00110 declarations.addDeclaration( initialize );
00111 declarations.addDeclaration( initializeNodes );
00112 declarations.addDeclaration( shiftStates );
00113 declarations.addDeclaration( shiftControls );
00114 declarations.addDeclaration( getKKT );
00115 declarations.addDeclaration( getObjective );
00116
00117 declarations.addDeclaration( evaluateStageCost );
00118 declarations.addDeclaration( evaluateTerminalCost );
00119
00120 return SUCCESSFUL_RETURN;
00121 }
00122
00123 returnValue ExportGaussNewtonHpmpc::getCode( ExportStatementBlock& code
00124 )
00125 {
00126 qpInterface->exportCode();
00127
00128
00129 code << "#ifdef __cplusplus\n";
00130 code << "extern \"C\"{\n";
00131 code << "#endif\n";
00132 code << "int acado_hpmpc_ip_wrapper(real_t* A, real_t* B, real_t* d, real_t* Q, real_t* Qf, real_t* S, real_t* R, real_t* q, real_t* qf, real_t* r, real_t* lb, real_t* ub, real_t* x, real_t* u, real_t* lambda, real_t* mu, real_t* slacks, int* nIt);\n";
00133 code << "#ifdef __cplusplus\n";
00134 code << "}\n";
00135 code << "#endif\n";
00136
00137 code.addLinebreak( 2 );
00138 code.addStatement( "/******************************************************************************/\n" );
00139 code.addStatement( "/* */\n" );
00140 code.addStatement( "/* ACADO code generation */\n" );
00141 code.addStatement( "/* */\n" );
00142 code.addStatement( "/******************************************************************************/\n" );
00143 code.addLinebreak( 2 );
00144
00145 int useOMP;
00146 get(CG_USE_OPENMP, useOMP);
00147 if ( useOMP )
00148 {
00149 code.addDeclaration( state );
00150 }
00151
00152 code.addFunction( modelSimulation );
00153
00154 code.addFunction( evaluateStageCost );
00155 code.addFunction( evaluateTerminalCost );
00156 code.addFunction( setObjQ1Q2 );
00157 code.addFunction( setObjR1R2 );
00158 code.addFunction( setObjS1 );
00159 code.addFunction( setObjQN1QN2 );
00160 code.addFunction( setStagef );
00161 code.addFunction( evaluateObjective );
00162
00163 code.addFunction( evaluateConstraints );
00164
00165 code.addFunction( acc );
00166
00167 code.addFunction( preparation );
00168 code.addFunction( feedback );
00169
00170 code.addFunction( initialize );
00171 code.addFunction( initializeNodes );
00172 code.addFunction( shiftStates );
00173 code.addFunction( shiftControls );
00174 code.addFunction( getKKT );
00175 code.addFunction( getObjective );
00176
00177 return SUCCESSFUL_RETURN;
00178 }
00179
00180
00181 unsigned ExportGaussNewtonHpmpc::getNumQPvars( ) const
00182 {
00183 return (N + 1) * NX + N * NU;
00184 }
00185
00186
00187
00188
00189
00190 returnValue ExportGaussNewtonHpmpc::setupObjectiveEvaluation( void )
00191 {
00192 evaluateObjective.setup("evaluateObjective");
00193
00194 int variableObjS;
00195 get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00196
00197 ExportVariable evLmX = zeros<double>(NX, NX);
00198 ExportVariable evLmU = zeros<double>(NU, NU);
00199
00200 if (levenbergMarquardt > 0.0)
00201 {
00202 DMatrix lmX = eye<double>( NX );
00203 lmX *= levenbergMarquardt;
00204
00205 DMatrix lmU = eye<double>( NU );
00206 lmU *= levenbergMarquardt;
00207
00208 evLmX = lmX;
00209 evLmU = lmU;
00210 }
00211
00212
00213
00214
00215
00216 ExportIndex runObj( "runObj" );
00217 ExportForLoop loopObjective( runObj, 0, N );
00218
00219 evaluateObjective.addIndex( runObj );
00220
00221 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00222 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00223 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00224 loopObjective.addLinebreak( );
00225
00226
00227 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00228
00229
00230 loopObjective.addStatement(
00231 Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
00232 );
00233 loopObjective.addLinebreak( );
00234
00235
00236
00237 ExportVariable tmpObjS, tmpFx, tmpFu;
00238 ExportVariable tmpFxEnd, tmpObjSEndTerm;
00239 tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00240 if (objS.isGiven() == true)
00241 tmpObjS = objS;
00242 tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00243 if (objEvFx.isGiven() == true)
00244 tmpFx = objEvFx;
00245 tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00246 if (objEvFu.isGiven() == true)
00247 tmpFu = objEvFu;
00248 tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00249 if (objEvFxEnd.isGiven() == true)
00250 tmpFxEnd = objEvFxEnd;
00251 tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00252 if (objSEndTerm.isGiven() == true)
00253 tmpObjSEndTerm = objSEndTerm;
00254
00255 unsigned indexX = getNY();
00256 ExportArgument tmpFxCall = tmpFx;
00257 if (tmpFx.isGiven() == false)
00258 {
00259 tmpFxCall = objValueOut.getAddress(0, indexX);
00260 indexX += objEvFx.getDim();
00261 }
00262
00263 ExportArgument tmpFuCall = tmpFu;
00264 if (tmpFu.isGiven() == false)
00265 {
00266 tmpFuCall = objValueOut.getAddress(0, indexX);
00267 }
00268
00269 ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
00270
00271
00272
00273
00274 if (Q1.isGiven() == false)
00275 {
00276 ExportVariable tmpQ1, tmpQ2;
00277 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00278 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00279
00280 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00281 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00282 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00283 setObjQ1Q2.addStatement( tmpQ1 += evLmX );
00284
00285 loopObjective.addFunctionCall(
00286 setObjQ1Q2,
00287 tmpFxCall, objSCall,
00288 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00289 );
00290
00291 loopObjective.addLinebreak( );
00292 }
00293 else if (levenbergMarquardt > 0.0)
00294 Q1 = Q1.getGivenMatrix() + evLmX.getGivenMatrix();
00295
00296 if (R1.isGiven() == false)
00297 {
00298 ExportVariable tmpR1, tmpR2;
00299 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00300 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00301
00302 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00303 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00304 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00305 setObjR1R2.addStatement( tmpR1 += evLmU );
00306
00307 loopObjective.addFunctionCall(
00308 setObjR1R2,
00309 tmpFuCall, objSCall,
00310 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00311 );
00312
00313 loopObjective.addLinebreak( );
00314 }
00315 else if (levenbergMarquardt > 0.0)
00316 R1 = R1.getGivenMatrix() + evLmU.getGivenMatrix();
00317
00318 if (S1.isGiven() == false)
00319 {
00320 ExportVariable tmpS1;
00321 ExportVariable tmpS2;
00322
00323 tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
00324 tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
00325
00326 setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
00327 setObjS1.addVariable( tmpS2 );
00328 setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
00329 setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
00330
00331 loopObjective.addFunctionCall(
00332 setObjS1,
00333 tmpFxCall, tmpFuCall, objSCall,
00334 S1.getAddress(runObj * NX, 0)
00335 );
00336 }
00337
00338 evaluateObjective.addStatement( loopObjective );
00339
00340
00341
00342
00343 evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00344 evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00345
00346
00347 evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00348 evaluateObjective.addLinebreak( );
00349
00350 evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00351 evaluateObjective.addLinebreak();
00352
00353 if (QN1.isGiven() == false)
00354 {
00355 ExportVariable tmpQN1, tmpQN2;
00356 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00357 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00358
00359 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00360 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00361 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00362 setObjQN1QN2.addStatement( tmpQN1 += evLmX );
00363
00364 indexX = getNYN();
00365 ExportArgument tmpFxEndCall = tmpFxEnd.isGiven() == true ? tmpFxEnd : objValueOut.getAddress(0, indexX);
00366
00367 evaluateObjective.addFunctionCall(
00368 setObjQN1QN2,
00369 tmpFxEndCall, objSEndTerm,
00370 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00371 );
00372
00373 evaluateObjective.addLinebreak( );
00374 }
00375 else if (levenbergMarquardt > 0.0)
00376 QN1 = QN1.getGivenMatrix() + evLmX.getGivenMatrix();
00377
00378
00379
00380
00381
00382 ExportIndex index( "index" );
00383
00384
00385
00386
00387 ExportVariable qq, rr;
00388 qq.setup("stageq", NX, 1, REAL, ACADO_LOCAL);
00389 rr.setup("stager", NU, 1, REAL, ACADO_LOCAL);
00390 setStagef.setup("setStagef", qq, rr, index);
00391
00392 if (Q2.isGiven() == false)
00393 setStagef.addStatement(
00394 qq == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) * Dy.getRows(index * NY, (index + 1) * NY)
00395 );
00396 else
00397 {
00398 setStagef << "(void)" << index.getFullName() << ";\n";
00399 setStagef.addStatement(
00400 qq == Q2 * Dy.getRows(index * NY, (index + 1) * NY)
00401 );
00402 }
00403 setStagef.addLinebreak();
00404
00405 if (R2.isGiven() == false)
00406 setStagef.addStatement(
00407 rr == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) * Dy.getRows(index * NY, (index + 1) * NY)
00408 );
00409 else
00410 {
00411 setStagef.addStatement(
00412 rr == R2 * Dy.getRows(index * NY, (index + 1) * NY)
00413 );
00414 }
00415
00416
00417
00418
00419
00420 if (Q1.isGiven() == true)
00421 {
00422 qpQ.setup("qpQ", N * NX, NX, REAL, ACADO_WORKSPACE);
00423 for (unsigned blk = 0; blk < N; ++blk)
00424 initialize.addStatement( qpQ.getSubMatrix(blk * NX, (blk + 1) * NX, 0, NX) == Q1);
00425 }
00426 else
00427 {
00428 qpQ = Q1;
00429 }
00430
00431 if (R1.isGiven() == true)
00432 {
00433 qpR.setup("qpR", N * NU, NU, REAL, ACADO_WORKSPACE);
00434 for (unsigned blk = 0; blk < N; ++blk)
00435 initialize.addStatement( qpR.getSubMatrix(blk * NU, (blk + 1) * NU, 0, NU) == R1);
00436 }
00437 else
00438 {
00439 qpR = R1;
00440 }
00441
00442 if (S1.isGiven() == true)
00443 {
00444 qpS.setup("qpS", N * NX, NU, REAL, ACADO_WORKSPACE);
00445 if (S1.getGivenMatrix().isZero() == true)
00446 initialize.addStatement(qpS == zeros<double>(N * NX, NU));
00447 else
00448 for (unsigned blk = 0; blk < N; ++blk)
00449 initialize.addStatement( qpS.getSubMatrix(blk * NX, (blk + 1) * NX, 0, NU) == S1);
00450 }
00451 else
00452 {
00453 qpS = S1;
00454 }
00455
00456 if (QN1.isGiven() == true)
00457 {
00458 qpQf.setup("qpQf", NX, NX, REAL, ACADO_WORKSPACE);
00459 initialize.addStatement( qpQf == QN1 );
00460 }
00461 else
00462 {
00463 qpQf = QN1;
00464 }
00465
00466 return SUCCESSFUL_RETURN;
00467 }
00468
00469 returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void )
00470 {
00472
00473
00474
00476
00477 evaluateConstraints.setup("evaluateConstraints");
00478
00479 DVector lbTmp, ubTmp;
00480
00481 DVector lbXInf( NX );
00482 lbXInf.setAll( -INFTY );
00483
00484 DVector ubXInf( NX );
00485 ubXInf.setAll( INFTY );
00486
00487 DVector lbUInf( NU );
00488 lbUInf.setAll( -INFTY );
00489
00490 DVector ubUInf( NU );
00491 ubUInf.setAll( INFTY );
00492
00493 DVector lbValues, ubValues;
00494
00495 for (unsigned node = 0; node < N; ++node)
00496 {
00497 lbTmp = uBounds.getLowerBounds( node );
00498 if ( !lbTmp.getDim() )
00499 lbValues.append( lbUInf );
00500 else
00501 lbValues.append( lbTmp );
00502
00503 ubTmp = uBounds.getUpperBounds( node );
00504 if ( !ubTmp.getDim() )
00505 ubValues.append( ubUInf );
00506 else
00507 ubValues.append( ubTmp );
00508 }
00509
00510 for (unsigned node = 1; node < N + 1; ++node)
00511 {
00512 lbTmp = xBounds.getLowerBounds( node );
00513 if ( !lbTmp.getDim() )
00514 lbValues.append( lbXInf );
00515 else
00516 lbValues.append( lbTmp );
00517
00518 ubTmp = xBounds.getUpperBounds( node );
00519 if ( !ubTmp.getDim() )
00520 ubValues.append( ubXInf );
00521 else
00522 ubValues.append( ubTmp );
00523 }
00524
00525 qpLb.setup("qpLb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
00526 qpUb.setup("qpUb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
00527
00528 evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL);
00529 evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL);
00530
00531 evaluateConstraints.addVariable( evLbValues );
00532 evaluateConstraints.addVariable( evUbValues );
00533
00534 evaluateConstraints.addStatement( qpLb.getRows(0, N * NU) == evLbValues.getRows(0, N * NU) - u.makeColVector() );
00535 evaluateConstraints.addStatement( qpUb.getRows(0, N * NU) == evUbValues.getRows(0, N * NU) - u.makeColVector() );
00536
00537 evaluateConstraints.addStatement( qpLb.getRows(N * NU, N * NU + N * NX) == evLbValues.getRows(N * NU, N * NU + N * NX) - x.makeColVector().getRows(NX, NX * (N + 1)) );
00538 evaluateConstraints.addStatement( qpUb.getRows(N * NU, N * NU + N * NX) == evUbValues.getRows(N * NU, N * NU + N * NX) - x.makeColVector().getRows(NX, NX * (N + 1)) );
00539
00540 return SUCCESSFUL_RETURN;
00541 }
00542
00543 returnValue ExportGaussNewtonHpmpc::setupVariables( )
00544 {
00545 if (initialStateFixed() == true)
00546 {
00547 x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
00548 x0.setDoc( "Current state feedback vector." );
00549 }
00550
00551 return SUCCESSFUL_RETURN;
00552 }
00553
00554 returnValue ExportGaussNewtonHpmpc::setupMultiplicationRoutines( )
00555 {
00556 return SUCCESSFUL_RETURN;
00557 }
00558
00559 returnValue ExportGaussNewtonHpmpc::setupEvaluation( )
00560 {
00562
00563
00564
00566 preparation.setup("preparationStep");
00567 preparation.doc( "Preparation step of the RTI scheme." );
00568
00569 ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
00570 retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
00571 preparation.setReturnValue(retSim, false);
00572
00573 preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
00574
00575 preparation.addFunctionCall( evaluateObjective );
00576 preparation.addFunctionCall( evaluateConstraints );
00577
00579
00580
00581
00583 ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
00584 ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
00585 returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
00586 feedback.setup("feedbackStep" );
00587 feedback.doc( "Feedback/estimation step of the RTI scheme." );
00588 feedback.setReturnValue( returnValueFeedbackPhase );
00589
00590 qpx.setup("qpx", NX * (N + 1), 1, REAL, ACADO_WORKSPACE);
00591 qpu.setup("qpu", NU * N, 1, REAL, ACADO_WORKSPACE);
00592
00593 qpq.setup("qpq", NX * N, 1, REAL, ACADO_WORKSPACE);
00594 qpqf.setup("qpqf", NX, 1, REAL, ACADO_WORKSPACE);
00595 qpr.setup("qpr", NU * N, 1, REAL, ACADO_WORKSPACE);
00596
00597 qpLambda.setup("qpLambda", N * NX, 1, REAL, ACADO_WORKSPACE);
00598 qpMu.setup("qpMu", 2 * N * (NX + NU), 1, REAL, ACADO_WORKSPACE);
00599 qpSlacks.setup("qpSlacks", 2 * N * (NX + NU), 1, REAL, ACADO_WORKSPACE);
00600
00601 nIt.setup("nIt", 1, 1, INT, ACADO_WORKSPACE);
00602
00603
00604 feedback.addStatement( qpx.getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose() );
00605
00606
00607
00608
00609 feedback.addStatement( Dy -= y );
00610 feedback.addLinebreak();
00611 feedback.addStatement( DyN -= yN );
00612 feedback.addLinebreak();
00613
00614 for (unsigned i = 0; i < N; ++i)
00615 feedback.addFunctionCall(setStagef, qpq.getAddress(i * NX), qpr.getAddress(i * NU), ExportIndex( i ));
00616 feedback.addLinebreak();
00617 feedback.addStatement( qpqf == QN2 * DyN );
00618 feedback.addLinebreak();
00619
00620
00621
00622
00623
00624
00625 feedback
00626 << returnValueFeedbackPhase.getFullName() << " = " << "acado_hpmpc_ip_wrapper("
00627
00628 << evGx.getAddressString( true ) << ", "
00629 << evGu.getAddressString( true ) << ", "
00630 << d.getAddressString( true ) << ", "
00631
00632 << qpQ.getAddressString( true ) << ", "
00633 << qpQf.getAddressString( true ) << ", "
00634 << qpS.getAddressString( true ) << ", "
00635 << qpR.getAddressString( true ) << ", "
00636
00637 << qpq.getAddressString( true ) << ", "
00638 << qpqf.getAddressString( true ) << ", "
00639 << qpr.getAddressString( true ) << ", "
00640
00641 << qpLb.getAddressString( true ) << ", "
00642 << qpUb.getAddressString( true ) << ", "
00643
00644 << qpx.getAddressString( true ) << ", "
00645 << qpu.getAddressString( true ) << ", "
00646
00647 << qpLambda.getAddressString( true ) << ", "
00648 << qpMu.getAddressString( true ) << ", "
00649 << qpSlacks.getAddressString( true ) << ", "
00650
00651 << nIt.getAddressString( true )
00652 << ");\n";
00653
00654
00655 feedback.addStatement( x.makeColVector() += qpx );
00656 feedback.addStatement( u.makeColVector() += qpu );
00657
00659
00660
00661
00663
00664 ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
00665 ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
00666 ExportIndex index( "index" );
00667
00668 getKKT.setup( "getKKT" );
00669 getKKT.doc( "Get the KKT tolerance of the current iterate." );
00670 kkt.setDoc( "The KKT tolerance value." );
00671 getKKT.setReturnValue( kkt );
00672 getKKT.addVariable( tmp );
00673 getKKT.addIndex( index );
00674
00675 getKKT.addStatement( kkt == 0.0 );
00676
00677
00678
00679 getKKT.addStatement( tmp == (qpq ^ qpx.getRows(0, N * NX)) );
00680 getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
00681 getKKT.addStatement( tmp == (qpqf ^ qpx.getRows(N * NX, (N + 1) * NX)) );
00682 getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
00683 getKKT.addStatement( tmp == (qpr ^ qpu) );
00684 getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
00685
00686 ExportForLoop lamLoop(index, 0, N * NX);
00687 lamLoop << kkt.getFullName() << "+= fabs( " << d.get(index, 0) << " * " << qpLambda.get(index, 0) << ");\n";
00688 getKKT.addStatement( lamLoop );
00689
00690 ExportForLoop lbLoop(index, 0, N * NU + N * NX);
00691 lbLoop << kkt.getFullName() << "+= fabs( " << qpLb.get(index, 0) << " * " << qpMu.get(index, 0) << ");\n";
00692 ExportForLoop ubLoop(index, 0, N * NU + N * NX);
00693 ubLoop << kkt.getFullName() << "+= fabs( " << qpUb.get(index, 0) << " * " << qpMu.get(index + N * NU + N * NX, 0) << ");\n";
00694
00695 getKKT.addStatement( lbLoop );
00696 getKKT.addStatement( ubLoop );
00697
00698 return SUCCESSFUL_RETURN;
00699 }
00700
00701 returnValue ExportGaussNewtonHpmpc::setupQPInterface( )
00702 {
00703
00704
00705
00706
00707 string folderName;
00708 get(CG_EXPORT_FOLDER_NAME, folderName);
00709 string outFile = folderName + "/acado_hpmpc_interface.c";
00710
00711 qpInterface = std::tr1::shared_ptr< ExportHpmpcInterface >(new ExportHpmpcInterface(outFile, commonHeaderName));
00712
00713 int maxNumQPiterations;
00714 get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
00715
00716
00717 if ( maxNumQPiterations <= 0 )
00718 maxNumQPiterations = getNumQPvars();
00719
00720 int printLevel;
00721 get(PRINTLEVEL, printLevel);
00722
00723 if ( (PrintLevel)printLevel >= HIGH )
00724 printLevel = 2;
00725 else
00726 printLevel = 0;
00727
00728 int useSinglePrecision;
00729 get(USE_SINGLE_PRECISION, useSinglePrecision);
00730
00731 int hotstartQP;
00732 get(HOTSTART_QP, hotstartQP);
00733
00734 qpInterface->configure(
00735 maxNumQPiterations,
00736 printLevel,
00737 useSinglePrecision,
00738 hotstartQP
00739 );
00740
00741 return SUCCESSFUL_RETURN;
00742 }
00743
00744 CLOSE_NAMESPACE_ACADO