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_qpdunes2.hpp>
00033
00034 #include <sstream>
00035
00036 BEGIN_NAMESPACE_ACADO
00037
00038 using namespace std;
00039
00040 ExportGaussNewtonQpDunes2::ExportGaussNewtonQpDunes2( UserInteraction* _userInteraction,
00041 const std::string& _commonHeaderName
00042 ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00043 {}
00044
00045 returnValue ExportGaussNewtonQpDunes2::setup( )
00046 {
00047 LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00048 setupInitialization();
00049
00050
00051
00052
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
00062 LOG( LVL_DEBUG ) << "done!" << endl;
00063
00064 setupVariables();
00065
00066 setupSimulation();
00067
00068 setupObjectiveEvaluation();
00069
00070 setupConstraintsEvaluation();
00071
00072 setupEvaluation();
00073
00074 setupAuxiliaryFunctions();
00075
00076 return SUCCESSFUL_RETURN;
00077 }
00078
00079 returnValue ExportGaussNewtonQpDunes2::getDataDeclarations( ExportStatementBlock& declarations,
00080 ExportStruct dataStruct
00081 ) const
00082 {
00083 returnValue status;
00084 status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00085 if (status != SUCCESSFUL_RETURN)
00086 return status;
00087
00088 declarations.addDeclaration(x0, dataStruct);
00089
00090 declarations.addDeclaration(qpH, dataStruct);
00091 declarations.addDeclaration(qpg, dataStruct);
00092 declarations.addDeclaration(qpLb0, dataStruct);
00093 declarations.addDeclaration(qpUb0, dataStruct);
00094 declarations.addDeclaration(qpLb, dataStruct);
00095 declarations.addDeclaration(qpUb, dataStruct);
00096 declarations.addDeclaration(qpC, dataStruct);
00097 declarations.addDeclaration(qpc, dataStruct);
00098
00099 declarations.addDeclaration(qpA, dataStruct);
00100 declarations.addDeclaration(qpLbA, dataStruct);
00101 declarations.addDeclaration(qpUbA, dataStruct);
00102
00103 declarations.addDeclaration(qpPrimal, dataStruct);
00104 declarations.addDeclaration(qpLambda, dataStruct);
00105 declarations.addDeclaration(qpMu, dataStruct);
00106
00107 return SUCCESSFUL_RETURN;
00108 }
00109
00110 returnValue ExportGaussNewtonQpDunes2::getFunctionDeclarations( ExportStatementBlock& declarations
00111 ) const
00112 {
00113 declarations.addDeclaration( preparation );
00114 declarations.addDeclaration( feedback );
00115
00116 declarations.addDeclaration( initialize );
00117 declarations.addDeclaration( initializeNodes );
00118 declarations.addDeclaration( shiftStates );
00119 declarations.addDeclaration( shiftControls );
00120 declarations.addDeclaration( getKKT );
00121 declarations.addDeclaration( getObjective );
00122
00123 declarations.addDeclaration( cleanup );
00124 declarations.addDeclaration( shiftQpData );
00125
00126 declarations.addDeclaration( evaluateStageCost );
00127 declarations.addDeclaration( evaluateTerminalCost );
00128
00129 return SUCCESSFUL_RETURN;
00130 }
00131
00132 returnValue ExportGaussNewtonQpDunes2::getCode( ExportStatementBlock& code
00133 )
00134 {
00135 setupQPInterface();
00136 code.addStatement( *qpInterface );
00137
00138 code.addLinebreak( 2 );
00139 code.addStatement( "/******************************************************************************/\n" );
00140 code.addStatement( "/* */\n" );
00141 code.addStatement( "/* ACADO code generation */\n" );
00142 code.addStatement( "/* */\n" );
00143 code.addStatement( "/******************************************************************************/\n" );
00144 code.addLinebreak( 2 );
00145
00146 int useOMP;
00147 get(CG_USE_OPENMP, useOMP);
00148 if ( useOMP )
00149 {
00150 code.addDeclaration( state );
00151 }
00152
00153 code.addFunction( modelSimulation );
00154
00155 code.addFunction( evaluateStageCost );
00156 code.addFunction( evaluateTerminalCost );
00157 code.addFunction( setObjQ1Q2 );
00158 code.addFunction( setObjR1R2 );
00159 code.addFunction( setObjQN1QN2 );
00160 code.addFunction( setStageH );
00161 code.addFunction( setStagef );
00162 code.addFunction( evaluateObjective );
00163
00164 code.addFunction( evaluatePathConstraints );
00165
00166 for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00167 {
00168 if (evaluatePointConstraints[ i ] == 0)
00169 continue;
00170 code.addFunction( *evaluatePointConstraints[ i ] );
00171 }
00172
00173 code.addFunction( setStagePac );
00174 code.addFunction( evaluateConstraints );
00175
00176 code.addFunction( acc );
00177
00178 code.addFunction( preparation );
00179 code.addFunction( feedback );
00180
00181 code.addFunction( initialize );
00182 code.addFunction( initializeNodes );
00183 code.addFunction( shiftStates );
00184 code.addFunction( shiftControls );
00185 code.addFunction( getKKT );
00186 code.addFunction( getObjective );
00187
00188 code.addFunction( cleanup );
00189 code.addFunction( shiftQpData );
00190
00191 return SUCCESSFUL_RETURN;
00192 }
00193
00194
00195 unsigned ExportGaussNewtonQpDunes2::getNumQPvars( ) const
00196 {
00197 return (N + 1) * NX + N * NU;
00198 }
00199
00200
00201
00202
00203
00204 returnValue ExportGaussNewtonQpDunes2::setupObjectiveEvaluation( void )
00205 {
00206 evaluateObjective.setup("evaluateObjective");
00207
00208 int variableObjS;
00209 get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00210
00211
00212
00213
00214
00215 qpH.setup("qpH", N * (NX + NU) * (NX + NU) + NX * NX, 1, REAL, ACADO_WORKSPACE);
00216 qpg.setup("qpG", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
00217
00218
00219
00220
00221
00222 ExportVariable evLmX = zeros<double>(NX, NX);
00223 ExportVariable evLmU = zeros<double>(NU, NU);
00224
00225 if (levenbergMarquardt > 0.0)
00226 {
00227 DMatrix lmX = eye<double>( NX );
00228 lmX *= levenbergMarquardt;
00229
00230 DMatrix lmU = eye<double>( NU );
00231 lmU *= levenbergMarquardt;
00232
00233 evLmX = lmX;
00234 evLmU = lmU;
00235 }
00236
00237
00238
00239
00240
00241 ExportIndex runObj( "runObj" );
00242 ExportForLoop loopObjective( runObj, 0, N );
00243
00244 evaluateObjective.addIndex( runObj );
00245
00246 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00247 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00248 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00249 loopObjective.addLinebreak( );
00250
00251
00252 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00253
00254
00255 loopObjective.addStatement(
00256 Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
00257 );
00258 loopObjective.addLinebreak( );
00259
00260
00261 unsigned indexX = getNY();
00262
00263
00264 ExportVariable tmpObjS, tmpFx, tmpFu;
00265 ExportVariable tmpFxEnd, tmpObjSEndTerm;
00266 tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00267 if (objS.isGiven() == true)
00268 tmpObjS = objS;
00269 tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00270 if (objEvFx.isGiven() == true)
00271 tmpFx = objEvFx;
00272 tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00273 if (objEvFu.isGiven() == true)
00274 tmpFu = objEvFu;
00275 tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00276 if (objEvFxEnd.isGiven() == true)
00277 tmpFxEnd = objEvFxEnd;
00278 tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00279 if (objSEndTerm.isGiven() == true)
00280 tmpObjSEndTerm = objSEndTerm;
00281
00282
00283
00284
00285 if (Q1.isGiven() == false)
00286 {
00287 ExportVariable tmpQ1, tmpS1, tmpQ2;
00288 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00289 tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
00290 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00291
00292 setObjQ1Q2.setup("setObjQ1S1Q2", tmpFx, tmpObjS, tmpQ1, tmpS1, tmpQ2);
00293 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00294 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00295 setObjQ1Q2.addStatement( tmpS1 == tmpQ2 * tmpFu );
00296
00297 if (tmpFx.isGiven() == true)
00298 {
00299 if (variableObjS == YES)
00300 {
00301 loopObjective.addFunctionCall(
00302 setObjQ1Q2,
00303 tmpFx, objS.getAddress(runObj * NY, 0),
00304 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00305 );
00306 }
00307 else
00308 {
00309 loopObjective.addFunctionCall(
00310 setObjQ1Q2,
00311 tmpFx, objS,
00312 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00313 );
00314 }
00315 }
00316 else
00317 {
00318 if (variableObjS == YES)
00319 {
00320 if (objEvFx.isGiven() == true)
00321
00322 loopObjective.addFunctionCall(
00323 setObjQ1Q2,
00324 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00325 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00326 );
00327 }
00328 else
00329 {
00330 loopObjective.addFunctionCall(
00331 setObjQ1Q2,
00332 objValueOut.getAddress(0, indexX), objS,
00333 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00334 );
00335 }
00336 indexX += objEvFx.getDim();
00337 }
00338
00339 loopObjective.addLinebreak( );
00340 }
00341
00342 if (R1.isGiven() == false)
00343 {
00344 ExportVariable tmpR1, tmpR2;
00345 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00346 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00347
00348 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00349 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00350 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00351
00352 if (tmpFu.isGiven() == true)
00353 {
00354 if (variableObjS == YES)
00355 {
00356 loopObjective.addFunctionCall(
00357 setObjR1R2,
00358 tmpFu, objS.getAddress(runObj * NY, 0),
00359 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00360 );
00361 }
00362 else
00363 {
00364 loopObjective.addFunctionCall(
00365 setObjR1R2,
00366 tmpFu, objS,
00367 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00368 );
00369 }
00370 }
00371 else
00372 {
00373 if (variableObjS == YES)
00374 {
00375 loopObjective.addFunctionCall(
00376 setObjR1R2,
00377 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00378 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00379 );
00380 }
00381 else
00382 {
00383 loopObjective.addFunctionCall(
00384 setObjR1R2,
00385 objValueOut.getAddress(0, indexX), objS,
00386 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00387 );
00388 }
00389 }
00390
00391 loopObjective.addLinebreak( );
00392 }
00393
00394 evaluateObjective.addStatement( loopObjective );
00395
00396
00397
00398
00399 evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00400 evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00401
00402
00403 evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00404 evaluateObjective.addLinebreak( );
00405
00406 evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00407 evaluateObjective.addLinebreak();
00408
00409 if (QN1.isGiven() == false)
00410 {
00411 indexX = getNYN();
00412
00413 ExportVariable tmpQN1, tmpQN2;
00414 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00415 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00416
00417 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00418 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00419 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00420
00421 if (tmpFxEnd.isGiven() == true)
00422 evaluateObjective.addFunctionCall(
00423 setObjQN1QN2,
00424 tmpFxEnd, objSEndTerm,
00425 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00426 );
00427 else
00428 evaluateObjective.addFunctionCall(
00429 setObjQN1QN2,
00430 objValueOut.getAddress(0, indexX), objSEndTerm,
00431 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00432 );
00433
00434 evaluateObjective.addLinebreak( );
00435 }
00436
00437
00438
00439
00440
00441 ExportVariable stageH;
00442 ExportIndex index( "index" );
00443 stageH.setup("stageH", NX + NU, NX + NU, REAL, ACADO_LOCAL);
00444 setStageH.setup("setStageH", stageH, index);
00445
00446 if (Q1.isGiven() == false)
00447 setStageH.addStatement(
00448 stageH.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(index * NX, (index + 1) * NX, 0, NX) + evLmX
00449 );
00450 else
00451 {
00452 setStageH.addStatement( index == index );
00453 setStageH.addStatement(
00454 stageH.getSubMatrix(0, NX, 0, NX) == Q1 + evLmX
00455 );
00456 }
00457 setStageH.addLinebreak();
00458
00459 if (S1.isGiven() == false) {
00460 setStageH.addStatement(
00461 stageH.getSubMatrix(0, NX, NX, NX+NU) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU)
00462 );
00463 setStageH.addStatement(
00464 stageH.getSubMatrix(NX, NX+NU, 0, NX) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU).getTranspose()
00465 );
00466 }
00467 else
00468 {
00469 setStageH.addStatement( index == index );
00470 setStageH.addStatement(
00471 stageH.getSubMatrix(0, NX, NX, NX+NU) == S1
00472 );
00473 setStageH.addStatement(
00474 stageH.getSubMatrix(NX, NX+NU, 0, NX) == S1.getTranspose()
00475 );
00476 }
00477 setStageH.addLinebreak();
00478
00479 if (R1.isGiven() == false)
00480 setStageH.addStatement(
00481 stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1.getSubMatrix(index * NU, (index + 1) * NU, 0, NU) + evLmU
00482 );
00483 else
00484 setStageH.addStatement(
00485 stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1 + evLmU
00486 );
00487
00488 if (Q1.isGiven() == true && R1.isGiven() == true)
00489 {
00490 for (unsigned i = 0; i < N; ++i)
00491 {
00492 initialize.addFunctionCall(
00493 setStageH, qpH.getAddress(i * (NX + NU) * (NX + NU)), ExportIndex( i ));
00494 }
00495 initialize.addLinebreak();
00496 initialize.addStatement(
00497 qpH.getTranspose().getCols(N * (NX + NU) * (NX + NU), N * (NX + NU) * (NX + NU) + NX * NX) == QN1.makeRowVector() + evLmX.makeRowVector()
00498 );
00499 }
00500 else
00501 {
00502 ACADOWARNINGTEXT(RET_NOT_YET_IMPLEMENTED, "Under dev, has to be tested!");
00503
00504 for (unsigned i = 0; i < N; ++i)
00505 {
00506 evaluateObjective.addFunctionCall(
00507 setStageH, qpH.getAddress(i * (NX + NU) * (NX + NU)), ExportIndex( i ));
00508 }
00509 evaluateObjective.addLinebreak();
00510 evaluateObjective.addStatement(
00511 qpH.getTranspose().getCols(N * (NX + NU) * (NX + NU), N * (NX + NU) * (NX + NU) + NX * NX) == QN1.makeRowVector() + evLmX.makeRowVector()
00512 );
00513 }
00514
00515
00516
00517
00518
00519 ExportVariable stagef;
00520 stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
00521 setStagef.setup("setStagef", stagef, index);
00522
00523 if (Q2.isGiven() == false)
00524 setStagef.addStatement(
00525 stagef.getRows(0, NX) == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) *
00526 Dy.getRows(index * NY, (index + 1) * NY)
00527 );
00528 else
00529 {
00530 setStagef.addStatement( index == index );
00531 setStagef.addStatement(
00532 stagef.getRows(0, NX) == Q2 *
00533 Dy.getRows(index * NY, (index + 1) * NY)
00534 );
00535 }
00536 setStagef.addLinebreak();
00537
00538 if (R2.isGiven() == false)
00539 setStagef.addStatement(
00540 stagef.getRows(NX, NX + NU) == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) *
00541 Dy.getRows(index * NY, (index + 1) * NY)
00542 );
00543 else
00544 {
00545 setStagef.addStatement(
00546 stagef.getRows(NX, NX + NU) == R2 *
00547 Dy.getRows(index * NY, (index + 1) * NY)
00548 );
00549 }
00550
00551 return SUCCESSFUL_RETURN;
00552 }
00553
00554 returnValue ExportGaussNewtonQpDunes2::setupConstraintsEvaluation( void )
00555 {
00557
00558
00559
00561
00562
00563 qpLb0.setup("qpLb0", 1, NX + NU, REAL, ACADO_WORKSPACE);
00564 qpUb0.setup("qpUb0", 1, NX + NU, REAL, ACADO_WORKSPACE);
00565 qpLb.setup("qpLb", 1, N * (NX + NU) + NX, REAL, ACADO_WORKSPACE);
00566 qpUb.setup("qpUb", 1, N * (NX + NU) + NX, REAL, ACADO_WORKSPACE);
00567
00568 DVector lbTmp, ubTmp;
00569 DVector lbXValues, ubXValues;
00570 DVector lbUValues, ubUValues;
00571
00572 DVector lbXInf( NX );
00573 lbXInf.setAll( -INFTY );
00574
00575 DVector ubXInf( NX );
00576 ubXInf.setAll( INFTY );
00577
00578
00579
00580
00581 for (unsigned i = 0; i < N + 1; ++i)
00582 {
00583 lbTmp = xBounds.getLowerBounds( i );
00584 if ( !lbTmp.getDim() )
00585 lbXValues.append( lbXInf );
00586 else
00587 lbXValues.append( lbTmp );
00588
00589 ubTmp = xBounds.getUpperBounds( i );
00590 if ( !ubTmp.getDim() )
00591 ubXValues.append( ubXInf );
00592 else
00593 ubXValues.append( ubTmp );
00594 }
00595
00596 ExportVariable evLbXValues("lbXValues", lbXValues, STATIC_CONST_REAL, ACADO_LOCAL);
00597 ExportVariable evUbXValues("ubXValues", ubXValues, STATIC_CONST_REAL, ACADO_LOCAL);
00598
00599
00600
00601
00602 for (unsigned i = 0; i < N; ++i)
00603 {
00604 lbTmp = uBounds.getLowerBounds( i );
00605 lbUValues.append( lbTmp );
00606
00607 ubTmp = uBounds.getUpperBounds( i );
00608 ubUValues.append( ubTmp );
00609 }
00610
00611 ExportVariable evLbUValues("lbUValues", lbUValues, STATIC_CONST_REAL, ACADO_LOCAL);
00612 ExportVariable evUbUValues("ubUValues", ubUValues, STATIC_CONST_REAL, ACADO_LOCAL);
00613
00614
00615
00616
00617 evaluateConstraints.setup("evaluateConstraints");
00618 evaluateConstraints.addVariable( evLbXValues );
00619 evaluateConstraints.addVariable( evUbXValues );
00620 evaluateConstraints.addVariable( evLbUValues );
00621 evaluateConstraints.addVariable( evUbUValues );
00622
00623 evaluateConstraints.addStatement(
00624 qpLb0.getCols(NX, NX + NU) == evLbUValues.getTranspose().getCols(0, NU) - u.getRow( 0 )
00625 );
00626 evaluateConstraints.addStatement(
00627 qpUb0.getCols(NX, NX + NU) == evUbUValues.getTranspose().getCols(0, NU) - u.getRow( 0 )
00628 );
00629
00630 ExportIndex ind( "ind" );
00631 evaluateConstraints.addIndex( ind );
00632 ExportForLoop lbLoop(ind, 0, N);
00633 ExportForLoop ubLoop(ind, 0, N);
00634
00635 lbLoop.addStatement(
00636 qpLb.getCols(ind * (NX + NU), ind * (NX + NU) + NX) ==
00637 evLbXValues.getTranspose().getCols(ind * NX, (ind + 1) * NX) - x.getRow( ind )
00638 );
00639 lbLoop.addStatement(
00640 qpLb.getCols(ind * (NX + NU) + NX, (ind + 1) * (NX + NU)) ==
00641 evLbUValues.getTranspose().getCols(ind * NU, (ind + 1) * NU) - u.getRow( ind )
00642 );
00643
00644 ubLoop.addStatement(
00645 qpUb.getCols(ind * (NX + NU), ind * (NX + NU) + NX) ==
00646 evUbXValues.getTranspose().getCols(ind * NX, (ind + 1) * NX) - x.getRow( ind )
00647 );
00648 ubLoop.addStatement(
00649 qpUb.getCols(ind * (NX + NU) + NX, (ind + 1) * (NX + NU)) ==
00650 evUbUValues.getTranspose().getCols(ind * NU, (ind + 1) * NU) - u.getRow( ind )
00651 );
00652
00653 evaluateConstraints.addStatement( lbLoop );
00654 evaluateConstraints.addStatement( ubLoop );
00655 evaluateConstraints.addLinebreak();
00656
00657 evaluateConstraints.addStatement(
00658 qpLb.getCols(N * (NX + NU), N * (NX + NU) + NX) ==
00659 evLbXValues.getTranspose().getCols(N * NX, (N + 1) * NX) - x.getRow( N )
00660 );
00661 evaluateConstraints.addStatement(
00662 qpUb.getCols(N * (NX + NU), N * (NX + NU) + NX) ==
00663 evUbXValues.getTranspose().getCols(N * NX, (N + 1) * NX) - x.getRow( N )
00664 );
00665 evaluateConstraints.addLinebreak();
00666
00668
00669
00670
00671
00673
00674
00675
00676
00677
00678 qpC.setup("qpC", N * NX, NX + NU, REAL, ACADO_WORKSPACE);
00679 qpc.setup("qpc", N * NX, 1, REAL, ACADO_WORKSPACE);
00680
00681 ExportForLoop cLoop(ind, 0, N);
00682
00683 cLoop.addStatement(
00684 qpC.getSubMatrix(ind * NX, (ind + 1) * NX, 0, NX) ==
00685 evGx.getSubMatrix(ind * NX, (ind + 1) * NX, 0, NX)
00686 );
00687 cLoop.addStatement(
00688 qpC.getSubMatrix(ind * NX, (ind + 1) * NX, NX, NX + NU) ==
00689 evGu.getSubMatrix(ind * NX, (ind + 1) * NX, 0, NU)
00690 );
00691
00692 evaluateConstraints.addStatement( cLoop );
00693 evaluateConstraints.addLinebreak();
00694
00695
00696
00697
00698
00699
00700
00701 evaluateConstraints.addStatement(
00702 qpc == d
00703 );
00704 evaluateConstraints.addLinebreak();
00705
00707
00708
00709
00711
00712 if (getNumComplexConstraints() == 0)
00713 return SUCCESSFUL_RETURN;
00714
00715 unsigned dimLbA = N * dimPacH;
00716 unsigned dimConA = dimLbA * (NX + NU);
00717
00718 qpConDim.resize(N + 1, 0);
00719 for (unsigned i = 0; i < N; ++i)
00720 qpConDim[ i ] += dimPacH;
00721
00722 for (unsigned i = 0; i < N; ++i)
00723 if (evaluatePointConstraints[ i ])
00724 {
00725 unsigned dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
00726
00727 dimLbA += dim;
00728 dimConA += dim * (NX + NU);
00729
00730 qpConDim[ i ] += dim;
00731 }
00732
00733 if (evaluatePointConstraints[ N ])
00734 {
00735 unsigned dim = evaluatePointConstraints[ N ]->getFunctionDim() / (1 + NX);
00736 dimLbA += dim;
00737 dimConA += dim * NX;
00738
00739 qpConDim[ N ] += dim;
00740 }
00741
00742 qpA.setup("qpA", dimConA, 1, REAL, ACADO_WORKSPACE);
00743 qpLbA.setup("qpLbA", dimLbA, 1, REAL, ACADO_WORKSPACE);
00744 qpUbA.setup("qpUbA", dimLbA, 1, REAL, ACADO_WORKSPACE);
00745
00746
00747
00748
00749 DVector lbAValues;
00750 DVector ubAValues;
00751
00752 for (unsigned i = 0; i < N; ++i)
00753 {
00754 if ( dimPacH )
00755 {
00756 lbAValues.append( lbPathConValues.block(i * NX, 0, NX, 1) );
00757 ubAValues.append( ubPathConValues.block(i * NX, 0, NX, 1) );
00758 }
00759 lbAValues.append( pocLbStack[ i ] );
00760 ubAValues.append( pocUbStack[ i ] );
00761 }
00762 lbAValues.append( pocLbStack[ N ] );
00763 ubAValues.append( pocUbStack[ N ] );
00764
00765 ExportVariable evLbAValues("lbAValues", lbAValues, STATIC_CONST_REAL, ACADO_LOCAL);
00766 ExportVariable evUbAValues("ubAValues", ubAValues, STATIC_CONST_REAL, ACADO_LOCAL);
00767
00768 evaluateConstraints.addVariable( evLbAValues );
00769 evaluateConstraints.addVariable( evUbAValues );
00770
00771
00772
00773
00774
00775 if ( dimPacH )
00776 {
00777 ExportIndex runPac;
00778 evaluateConstraints.acquire( runPac );
00779 ExportForLoop loopPac(runPac, 0, N);
00780
00781 loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
00782 loopPac.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( runPac ) );
00783 loopPac.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runPac ) );
00784 loopPac.addFunctionCall( evaluatePathConstraints.getName(), conValueIn, conValueOut );
00785
00786 loopPac.addStatement( pacEvH.getRows( runPac * dimPacH, (runPac + 1) * dimPacH) ==
00787 conValueOut.getTranspose().getRows(0, dimPacH) );
00788 loopPac.addLinebreak( );
00789
00790 unsigned derOffset = dimPacH;
00791
00792
00793 if (pacEvHx.isGiven() == false)
00794 {
00795 loopPac.addStatement(
00796 pacEvHx.makeRowVector().getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX) ==
00797 conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
00798 );
00799
00800 derOffset = derOffset + dimPacH * NX;
00801 }
00802 if (pacEvHu.isGiven() == false )
00803 {
00804 loopPac.addStatement(
00805 pacEvHu.makeRowVector().getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU) ==
00806 conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
00807 );
00808 }
00809
00810
00811 evaluateConstraints.addStatement( loopPac );
00812 evaluateConstraints.release( runPac );
00813 evaluateConstraints.addLinebreak( );
00814 }
00815
00816
00817
00818
00819
00820 for (unsigned i = 0, intRowOffset = 0, dim = 0; i < N + 1; ++i)
00821 {
00822 if (evaluatePointConstraints[ i ] == 0)
00823 continue;
00824
00825 evaluateConstraints.addComment(
00826 string( "Evaluating constraint on node: #" ) + toString( i )
00827 );
00828
00829 evaluateConstraints.addStatement(conValueIn.getCols(0, getNX()) == x.getRow( i ) );
00830 if (i < N)
00831 {
00832 evaluateConstraints.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
00833 evaluateConstraints.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
00834 }
00835 else
00836 evaluateConstraints.addStatement( conValueIn.getCols(NX, NX + NOD) == od.getRow( i ) );
00837
00838 evaluateConstraints.addFunctionCall(
00839 evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
00840 evaluateConstraints.addLinebreak();
00841
00842 if (i < N)
00843 dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
00844 else
00845 dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
00846
00847
00848 evaluateConstraints.addStatement(
00849 pocEvH.getRows(intRowOffset, intRowOffset + dim) ==
00850 conValueOut.getTranspose().getRows(0, dim));
00851 evaluateConstraints.addLinebreak();
00852
00853 evaluateConstraints.addStatement(
00854 pocEvHx.makeRowVector().getCols(intRowOffset * NX, (intRowOffset + dim) * NX)
00855 == conValueOut.getCols(dim, dim + dim * NX));
00856 evaluateConstraints.addLinebreak();
00857
00858 if (i < N)
00859 {
00860 evaluateConstraints.addStatement(
00861 pocEvHu.makeRowVector().getCols(intRowOffset * NU, (intRowOffset + dim) * NU)
00862 == conValueOut.getCols(dim + dim * NX, dim + dim * NX + dim * NU));
00863 evaluateConstraints.addLinebreak();
00864 }
00865
00866 intRowOffset += dim;
00867 }
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877 ExportVariable tLbAValues, tUbAValues, tPacA;
00878 ExportIndex offsetPac("offset"), indPac( "ind" );
00879
00880 tLbAValues.setup("lbAValues", dimPacH, 1, REAL, ACADO_LOCAL);
00881 tUbAValues.setup("ubAValues", dimPacH, 1, REAL, ACADO_LOCAL);
00882 tPacA.setup("tPacA", dimPacH, NX + NU, REAL, ACADO_LOCAL);
00883
00884 setStagePac.setup("setStagePac", offsetPac, indPac, tPacA, tLbAValues, tUbAValues);
00885
00886 if (pacEvHx.isGiven() == true)
00887 setStagePac << (tPacA.getSubMatrix(0, dimPacH, 0, NX) == pacEvHx);
00888 else
00889 setStagePac << (tPacA.getSubMatrix(0, dimPacH, 0, NX) ==
00890 pacEvHx.getSubMatrix(indPac * dimPacH, indPac * dimPacH + dimPacH, 0 , NX));
00891
00892 if (pacEvHu.isGiven() == true)
00893 setStagePac << (tPacA.getSubMatrix(0, dimPacH, NX, NX + NU) == pacEvHu);
00894 else
00895 setStagePac << (tPacA.getSubMatrix(0, dimPacH, NX, NX + NU) ==
00896 pacEvHu.getSubMatrix(indPac * dimPacH, indPac * dimPacH + dimPacH, 0 , NU));
00897
00898 setStagePac
00899 << (qpLbA.getRows(offsetPac, offsetPac + dimPacH) == tLbAValues - pacEvH.getRows(indPac * dimPacH, indPac * dimPacH + dimPacH))
00900 << (qpUbA.getRows(offsetPac, offsetPac + dimPacH) == tUbAValues - pacEvH.getRows(indPac * dimPacH, indPac * dimPacH + dimPacH));
00901
00902 ExportVariable tPocA;
00903 tPocA.setup("tPocA", conValueOut.getDim(), NX + NU, REAL, ACADO_LOCAL);
00904 if ( dimPocH )
00905 evaluateConstraints.addVariable( tPocA );
00906
00907 unsigned offsetEval = 0;
00908 unsigned offsetPoc = 0;
00909 for (unsigned i = 0; i < N; ++i)
00910 {
00911 if ( dimPacH )
00912 {
00913 evaluateConstraints.addFunctionCall(
00914 setStagePac,
00915 ExportIndex( offsetEval ), ExportIndex( i ),
00916 qpA.getAddress(offsetEval * (NX + NU)),
00917 evLbAValues.getAddress( offsetEval ), evUbAValues.getAddress( offsetEval )
00918 );
00919
00920 offsetEval += dimPacH;
00921 }
00922
00923 if ( evaluatePointConstraints[ i ] )
00924 {
00925 unsigned dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
00926
00927 evaluateConstraints.addLinebreak();
00928
00929 evaluateConstraints
00930 << (tPocA.getSubMatrix(0, dim, 0, NX) == pocEvHx.getSubMatrix(offsetPoc, offsetPoc + dim, 0, NX))
00931 << (tPocA.getSubMatrix(0, dim, NX, NX + NU) == pocEvHu.getSubMatrix(offsetPoc, offsetPoc + dim, 0, NU))
00932 << (qpA.getRows(offsetEval * (NX + NU), (offsetEval + dim) * (NX + NU)) == tPocA.makeColVector().getRows(0, dim * (NX + NU)))
00933 << (qpLbA.getRows(offsetEval, offsetEval + dim) ==
00934 evLbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim))
00935 << (qpUbA.getRows(offsetEval, offsetEval + dim) ==
00936 evUbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim));
00937
00938 offsetEval += dim;
00939 offsetPoc += dim;
00940 }
00941 }
00942
00943 if ( evaluatePointConstraints[ N ] )
00944 {
00945 unsigned dim = evaluatePointConstraints[ N ]->getFunctionDim() / (1 + NX);
00946
00947 evaluateConstraints
00948 << (qpA.getRows(offsetEval * (NX + NU), offsetEval * (NX + NU) + dim * NX) ==
00949 pocEvHx.makeColVector().getRows(offsetPoc * (NX + NU), offsetPoc * (NX + NU) + dim * NX))
00950 << (qpLbA.getRows(offsetEval, offsetEval + dim) ==
00951 evLbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim))
00952 << (qpUbA.getRows(offsetEval, offsetEval + dim) ==
00953 evUbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim));
00954 }
00955
00956 return SUCCESSFUL_RETURN;
00957 }
00958
00959 returnValue ExportGaussNewtonQpDunes2::setupVariables( )
00960 {
00961 if (initialStateFixed() == true)
00962 {
00963 x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
00964 x0.setDoc( (std::string)"Current state feedback vector." );
00965 }
00966
00967 return SUCCESSFUL_RETURN;
00968 }
00969
00970 returnValue ExportGaussNewtonQpDunes2::setupMultiplicationRoutines( )
00971 {
00972 return SUCCESSFUL_RETURN;
00973 }
00974
00975 returnValue ExportGaussNewtonQpDunes2::setupEvaluation( )
00976 {
00977 stringstream ss;
00978
00980
00981
00982
00984 preparation.setup("preparationStep");
00985 preparation.doc( "Preparation step of the RTI scheme." );
00986
00987 ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
00988 retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
00989 preparation.setReturnValue(retSim, false);
00990
00991 preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
00992
00993 preparation.addFunctionCall( evaluateObjective );
00994 preparation.addFunctionCall( evaluateConstraints );
00995
00997
00998
00999
01001 ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
01002 ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
01003 returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
01004 feedback.setup("feedbackStep" );
01005 feedback.doc( "Feedback/estimation step of the RTI scheme." );
01006 feedback.setReturnValue( returnValueFeedbackPhase );
01007
01008
01009 feedback.addStatement(
01010 qpLb0.getTranspose().getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
01011 );
01012 feedback.addStatement(
01013 qpUb0.getCols(0, NX) == qpLb0.getCols(0, NX)
01014 );
01015 feedback.addLinebreak();
01016
01017
01018
01019
01020 feedback.addStatement( Dy -= y );
01021 feedback.addLinebreak();
01022 feedback.addStatement( DyN -= yN );
01023 feedback.addLinebreak();
01024
01025 for (unsigned i = 0; i < N; ++i)
01026 feedback.addFunctionCall(setStagef, qpg.getAddress(i * (NX + NU)), ExportIndex( i ));
01027 feedback.addStatement( qpg.getRows(N * (NX + NU), N * (NX + NU) + NX) == QN2 * DyN );
01028 feedback.addLinebreak();
01029
01030 feedback << returnValueFeedbackPhase.getFullName() << " = solveQpDunes();\n";
01031
01032
01033
01034
01035
01036 qpPrimal.setup("qpPrimal", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
01037 qpLambda.setup("qpLambda", N * NX, 1, REAL, ACADO_WORKSPACE);
01038
01039
01040 qpMu.setup("qpMu", 2 * N * (NX + NU) + 2 * NX, 1, REAL, ACADO_WORKSPACE);
01041
01042
01043
01044 ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
01045 ExportIndex index( "index" );
01046 acc.setup("accumulate", stageOut, index);
01047
01048 acc << (x.getRow( index ) += stageOut.getCols(0, NX))
01049 << (u.getRow( index ) += stageOut.getCols(NX, NX + NU));
01050
01051 for (unsigned i = 0; i < N; ++i)
01052 feedback.addFunctionCall(acc, qpPrimal.getAddress(i * (NX + NU)), ExportIndex( i ));
01053 feedback.addLinebreak();
01054 feedback.addStatement(
01055 x.getRow( N ) += qpPrimal.getTranspose().getCols(N * (NX + NU), N * (NX + NU) + NX)
01056 );
01057 feedback.addLinebreak();
01058
01060
01061
01062
01064
01065 shiftQpData.setup( "shiftQpData" );
01066 ss.str( string() );
01067 ss << "qpDUNES_shiftLambda( &qpData );" << endl
01068 << "qpDUNES_shiftIntervals( &qpData );" << endl;
01069 shiftQpData.addStatement( ss.str().c_str() );
01070
01072
01073
01074
01076 ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
01077
01078 getKKT.setup( "getKKT" );
01079 getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
01080
01081 kkt.setDoc( "0." );
01082 getKKT.setReturnValue( kkt );
01083
01084 getKKT.addStatement( kkt == 0 );
01085
01086 return SUCCESSFUL_RETURN;
01087 }
01088
01089 returnValue ExportGaussNewtonQpDunes2::setupQPInterface( )
01090 {
01091
01092
01093
01094
01095 qpInterface = std::tr1::shared_ptr< ExportQpDunesInterface >(new ExportQpDunesInterface("", commonHeaderName));
01096
01097 int maxNumQPiterations;
01098 get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
01099
01100
01101 if ( maxNumQPiterations <= 0 )
01102 maxNumQPiterations = getNumQPvars();
01103
01104 int printLevel;
01105 get(PRINTLEVEL, printLevel);
01106
01107 if ( (PrintLevel)printLevel >= HIGH )
01108 printLevel = 2;
01109 else
01110 printLevel = 0;
01111
01112 qpInterface->configure(
01113 maxNumQPiterations,
01114 printLevel,
01115 qpH.getFullName(),
01116 qpg.getFullName(),
01117 " 0 ",
01118 qpC.getFullName(),
01119 qpc.getFullName(),
01120 qpA.getFullName(),
01121 qpLb0.getFullName(),
01122 qpUb0.getFullName(),
01123 qpLb.getFullName(),
01124 qpUb.getFullName(),
01125 qpLbA.getFullName(),
01126 qpUbA.getFullName(),
01127 qpPrimal.getFullName(),
01128 qpLambda.getFullName(),
01129 qpMu.getFullName(),
01130 qpConDim,
01131 "1",
01132 "1",
01133 "1"
01134 );
01135
01136 return SUCCESSFUL_RETURN;
01137 }
01138
01139 CLOSE_NAMESPACE_ACADO