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_condensed.hpp>
00033 #include <acado/code_generation/export_qpoases_interface.hpp>
00034 #include <acado/code_generation/export_module.hpp>
00035
00036 using namespace std;
00037
00038 BEGIN_NAMESPACE_ACADO
00039
00040 ExportGaussNewtonCondensed::ExportGaussNewtonCondensed( UserInteraction* _userInteraction,
00041 const std::string& _commonHeaderName
00042 ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00043 {}
00044
00045 returnValue ExportGaussNewtonCondensed::setup( )
00046 {
00047 LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00048 setupInitialization();
00049 LOG( LVL_DEBUG ) << "done!" << endl;
00050
00051 LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
00052 setupVariables();
00053 LOG( LVL_DEBUG ) << "done!" << endl;
00054
00055 LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
00056 setupMultiplicationRoutines();
00057 LOG( LVL_DEBUG ) << "done!" << endl;
00058
00059 LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
00060 setupSimulation();
00061 LOG( LVL_DEBUG ) << "done!" << endl;
00062
00063 LOG( LVL_DEBUG ) << "Solver: setup arrival cost update... " << endl;
00064 setupArrivalCostCalculation();
00065 LOG( LVL_DEBUG ) << "done!" << endl;
00066
00067 LOG( LVL_DEBUG ) << "Solver: setup objective evaluation... " << endl;
00068 setupObjectiveEvaluation();
00069 LOG( LVL_DEBUG ) << "done!" << endl;
00070
00071 LOG( LVL_DEBUG ) << "Solver: setup condensing... " << endl;
00072 setupCondensing();
00073 LOG( LVL_DEBUG ) << "done!" << endl;
00074
00075 LOG( LVL_DEBUG ) << "Solver: setup constraints... " << endl;
00076 setupConstraintsEvaluation();
00077 LOG( LVL_DEBUG ) << "done!" << endl;
00078
00079 LOG( LVL_DEBUG ) << "Solver: setup evaluation... " << endl;
00080 setupEvaluation();
00081 LOG( LVL_DEBUG ) << "done!" << endl;
00082
00083 LOG( LVL_DEBUG ) << "Solver: setup auxiliary functions... " << endl;
00084 setupAuxiliaryFunctions();
00085 LOG( LVL_DEBUG ) << "done!" << endl;
00086
00087 return SUCCESSFUL_RETURN;
00088 }
00089
00090 returnValue ExportGaussNewtonCondensed::getDataDeclarations( ExportStatementBlock& declarations,
00091 ExportStruct dataStruct
00092 ) const
00093 {
00094 returnValue status;
00095 status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00096 if (status != SUCCESSFUL_RETURN)
00097 return status;
00098
00099 declarations.addDeclaration(x0, dataStruct);
00100 declarations.addDeclaration(Dx0, dataStruct);
00101
00102 declarations.addDeclaration(sigmaN, dataStruct);
00103
00104 declarations.addDeclaration(T, dataStruct);
00105 declarations.addDeclaration(E, dataStruct);
00106 declarations.addDeclaration(QE, dataStruct);
00107 declarations.addDeclaration(QGx, dataStruct);
00108 declarations.addDeclaration(Qd, dataStruct);
00109 declarations.addDeclaration(QDy, dataStruct);
00110 declarations.addDeclaration(H10, dataStruct);
00111
00112 declarations.addDeclaration(lbValues, dataStruct);
00113 declarations.addDeclaration(ubValues, dataStruct);
00114 declarations.addDeclaration(lbAValues, dataStruct);
00115 declarations.addDeclaration(ubAValues, dataStruct);
00116
00117 if (performFullCondensing() == true)
00118 declarations.addDeclaration(A10, dataStruct);
00119 declarations.addDeclaration(A20, dataStruct);
00120
00121 declarations.addDeclaration(pacA01Dx0, dataStruct);
00122 declarations.addDeclaration(pocA02Dx0, dataStruct);
00123
00124 declarations.addDeclaration(CEN, dataStruct);
00125 declarations.addDeclaration(sigmaTmp, dataStruct);
00126 declarations.addDeclaration(sigma, dataStruct);
00127
00128 declarations.addDeclaration(H, dataStruct);
00129 declarations.addDeclaration(R, dataStruct);
00130 declarations.addDeclaration(A, dataStruct);
00131 declarations.addDeclaration(g, dataStruct);
00132 declarations.addDeclaration(lb, dataStruct);
00133 declarations.addDeclaration(ub, dataStruct);
00134 declarations.addDeclaration(lbA, dataStruct);
00135 declarations.addDeclaration(ubA, dataStruct);
00136 declarations.addDeclaration(xVars, dataStruct);
00137 declarations.addDeclaration(yVars, dataStruct);
00138
00139 return SUCCESSFUL_RETURN;
00140 }
00141
00142 returnValue ExportGaussNewtonCondensed::getFunctionDeclarations( ExportStatementBlock& declarations
00143 ) const
00144 {
00145
00146 declarations.addDeclaration( preparation );
00147 declarations.addDeclaration( feedback );
00148
00149 declarations.addDeclaration( initialize );
00150 declarations.addDeclaration( initializeNodes );
00151 declarations.addDeclaration( shiftStates );
00152 declarations.addDeclaration( shiftControls );
00153 declarations.addDeclaration( getKKT );
00154 declarations.addDeclaration( getObjective );
00155
00156 declarations.addDeclaration( updateArrivalCost );
00157
00158 declarations.addDeclaration( evaluateStageCost );
00159 declarations.addDeclaration( evaluateTerminalCost );
00160
00161 return SUCCESSFUL_RETURN;
00162 }
00163
00164 returnValue ExportGaussNewtonCondensed::getCode( ExportStatementBlock& code
00165 )
00166 {
00167 setupQPInterface();
00168
00169 code.addLinebreak( 2 );
00170 code.addStatement( "/******************************************************************************/\n" );
00171 code.addStatement( "/* */\n" );
00172 code.addStatement( "/* ACADO code generation */\n" );
00173 code.addStatement( "/* */\n" );
00174 code.addStatement( "/******************************************************************************/\n" );
00175 code.addLinebreak( 2 );
00176
00177 int useOMP;
00178 get(CG_USE_OPENMP, useOMP);
00179 if ( useOMP )
00180 {
00181 code.addDeclaration( state );
00182 }
00183
00184 code.addFunction( modelSimulation );
00185
00186 code.addFunction( evaluateStageCost );
00187 code.addFunction( evaluateTerminalCost );
00188 code.addFunction( setObjQ1Q2 );
00189 code.addFunction( setObjR1R2 );
00190 code.addFunction( setObjQN1QN2 );
00191 code.addFunction( evaluateObjective );
00192
00193 code.addFunction( multGxd );
00194 code.addFunction( moveGxT );
00195 code.addFunction( multGxGx );
00196 code.addFunction( multGxGu );
00197 code.addFunction( moveGuE );
00198 code.addFunction( setBlockH11 );
00199 code.addFunction( setBlockH11_R1 );
00200 code.addFunction( zeroBlockH11 );
00201 code.addFunction( copyHTH );
00202 code.addFunction( multQ1d );
00203 code.addFunction( multQN1d );
00204 code.addFunction( multRDy );
00205 code.addFunction( multQDy );
00206 code.addFunction( multEQDy );
00207 code.addFunction( multQETGx );
00208 code.addFunction( zeroBlockH10 );
00209 code.addFunction( multEDu );
00210 code.addFunction( multQ1Gx );
00211 code.addFunction( multQN1Gx );
00212 code.addFunction( multQ1Gu );
00213 code.addFunction( multQN1Gu );
00214 code.addFunction( zeroBlockH00 );
00215 code.addFunction( multCTQC );
00216
00217 code.addFunction( multHxC );
00218 code.addFunction( multHxE );
00219 code.addFunction( macHxd );
00220
00221 code.addFunction( evaluatePathConstraints );
00222
00223 for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00224 {
00225 if (evaluatePointConstraints[ i ] == 0)
00226 continue;
00227 code.addFunction( *evaluatePointConstraints[ i ] );
00228 }
00229
00230 code.addFunction( macCTSlx );
00231 code.addFunction( macETSlu );
00232
00233 cholSolver.getCode( code );
00234
00235 code.addFunction( condensePrep );
00236 code.addFunction( condenseFdb );
00237 code.addFunction( expand );
00238 code.addFunction( calculateCovariance );
00239
00240 code.addFunction( preparation );
00241 code.addFunction( feedback );
00242
00243 code.addFunction( initialize );
00244 code.addFunction( initializeNodes );
00245 code.addFunction( shiftStates );
00246 code.addFunction( shiftControls );
00247 code.addFunction( getKKT );
00248 code.addFunction( getObjective );
00249
00250 int useArrivalCost;
00251 get(CG_USE_ARRIVAL_COST, useArrivalCost);
00252 if (useArrivalCost == YES)
00253 {
00254 acSolver.getCode( code );
00255 cholObjS.getCode( code );
00256 cholSAC.getCode( code );
00257 code.addFunction( updateArrivalCost );
00258 }
00259
00260 return SUCCESSFUL_RETURN;
00261 }
00262
00263
00264 unsigned ExportGaussNewtonCondensed::getNumQPvars( ) const
00265 {
00266 if (performFullCondensing() == true)
00267 return (N * NU);
00268
00269 return (NX + N * NU);
00270 }
00271
00272 unsigned ExportGaussNewtonCondensed::getNumStateBounds() const
00273 {
00274 return xBoundsIdx.size();
00275 }
00276
00277
00278
00279
00280
00281 returnValue ExportGaussNewtonCondensed::setupObjectiveEvaluation( void )
00282 {
00283 evaluateObjective.setup("evaluateObjective");
00284
00285 int variableObjS;
00286 get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00287
00288 if (S1.getGivenMatrix().isZero() == false)
00289 ACADOWARNINGTEXT(RET_INVALID_ARGUMENTS,
00290 "Mixed control-state terms in the objective function are not supported at the moment.");
00291
00292
00293
00294
00295 ExportIndex runObj( "runObj" );
00296 ExportForLoop loopObjective( runObj, 0, N );
00297
00298 evaluateObjective.addIndex( runObj );
00299
00300 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00301 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00302 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00303 loopObjective.addLinebreak( );
00304
00305
00306 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00307
00308
00309 loopObjective.addStatement(
00310 Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
00311 );
00312 loopObjective.addLinebreak( );
00313
00314
00315 unsigned indexX = getNY();
00316
00317
00318 ExportVariable tmpObjS, tmpFx, tmpFu;
00319 ExportVariable tmpFxEnd, tmpObjSEndTerm;
00320 tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00321 if (objS.isGiven() == true)
00322 tmpObjS = objS;
00323 tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00324 if (objEvFx.isGiven() == true)
00325 tmpFx = objEvFx;
00326 tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00327 if (objEvFu.isGiven() == true)
00328 tmpFu = objEvFu;
00329 tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00330 if (objEvFxEnd.isGiven() == true)
00331 tmpFxEnd = objEvFxEnd;
00332 tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00333 if (objSEndTerm.isGiven() == true)
00334 tmpObjSEndTerm = objSEndTerm;
00335
00336
00337
00338
00339 if (Q1.isGiven() == false)
00340 {
00341 ExportVariable tmpQ1, tmpQ2;
00342 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00343 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00344
00345 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00346 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00347 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00348
00349 if (tmpFx.isGiven() == true)
00350 {
00351 if (variableObjS == YES)
00352 {
00353 loopObjective.addFunctionCall(
00354 setObjQ1Q2,
00355 tmpFx, objS.getAddress(runObj * NY, 0),
00356 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00357 );
00358 }
00359 else
00360 {
00361 loopObjective.addFunctionCall(
00362 setObjQ1Q2,
00363 tmpFx, objS,
00364 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00365 );
00366 }
00367 }
00368 else
00369 {
00370 if (variableObjS == YES)
00371 {
00372 loopObjective.addFunctionCall(
00373 setObjQ1Q2,
00374 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00375 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00376 );
00377 }
00378 else
00379 {
00380 loopObjective.addFunctionCall(
00381 setObjQ1Q2,
00382 objValueOut.getAddress(0, indexX), objS,
00383 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00384 );
00385 }
00386 indexX += objEvFx.getDim();
00387 }
00388
00389 loopObjective.addLinebreak( );
00390 }
00391
00392 if (R1.isGiven() == false)
00393 {
00394 ExportVariable tmpR1, tmpR2;
00395 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00396 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00397
00398 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00399 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00400 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00401
00402 if (tmpFu.isGiven() == true)
00403 {
00404 if (variableObjS == YES)
00405 {
00406 loopObjective.addFunctionCall(
00407 setObjR1R2,
00408 tmpFu, objS.getAddress(runObj * NY, 0),
00409 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00410 );
00411 }
00412 else
00413 {
00414 loopObjective.addFunctionCall(
00415 setObjR1R2,
00416 tmpFu, objS,
00417 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00418 );
00419 }
00420 }
00421 else
00422 {
00423 if (variableObjS == YES)
00424 {
00425 loopObjective.addFunctionCall(
00426 setObjR1R2,
00427 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00428 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00429 );
00430 }
00431 else
00432 {
00433 loopObjective.addFunctionCall(
00434 setObjR1R2,
00435 objValueOut.getAddress(0, indexX), objS,
00436 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00437 );
00438 }
00439 }
00440
00441 loopObjective.addLinebreak( );
00442 }
00443
00444 evaluateObjective.addStatement( loopObjective );
00445
00446
00447
00448
00449 evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00450 evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00451
00452
00453 evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00454 evaluateObjective.addLinebreak( );
00455
00456 evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00457 evaluateObjective.addLinebreak();
00458
00459 if (QN1.isGiven() == false)
00460 {
00461 indexX = getNYN();
00462
00463 ExportVariable tmpQN1, tmpQN2;
00464 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00465 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00466
00467 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00468 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00469 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00470
00471 if (tmpFxEnd.isGiven() == true)
00472 evaluateObjective.addFunctionCall(
00473 setObjQN1QN2,
00474 tmpFxEnd, objSEndTerm,
00475 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00476 );
00477 else
00478 evaluateObjective.addFunctionCall(
00479 setObjQN1QN2,
00480 objValueOut.getAddress(0, indexX), objSEndTerm,
00481 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00482 );
00483
00484 evaluateObjective.addLinebreak( );
00485 }
00486
00487 return SUCCESSFUL_RETURN;
00488 }
00489
00490 returnValue ExportGaussNewtonCondensed::setupConstraintsEvaluation( void )
00491 {
00492 ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
00493
00494 int hardcodeConstraintValues;
00495 get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00496
00498
00499
00500
00502
00503 unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
00504 unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
00505 unsigned numStateBounds = getNumStateBounds();
00506 unsigned numPathCon = N * dimPacH;
00507 unsigned numPointCon = dimPocH;
00508
00510
00511
00512
00514
00515 DVector lbBoundValues( numBounds );
00516 DVector ubBoundValues( numBounds );
00517
00518 if (initialStateFixed( ) == false)
00519 for(unsigned el = 0; el < NX; ++el)
00520 {
00521 lbBoundValues( el )= xBounds.getLowerBound(0, el);
00522 ubBoundValues( el ) = xBounds.getUpperBound(0, el);
00523 }
00524
00525 for(unsigned node = 0; node < N; ++node)
00526 for(unsigned el = 0; el < NU; ++el)
00527 {
00528 lbBoundValues(offsetBounds + node * NU + el) = uBounds.getLowerBound(node, el);
00529 ubBoundValues(offsetBounds + node * NU + el) = uBounds.getUpperBound(node, el);
00530 }
00531
00532 if (hardcodeConstraintValues == YES || !(isFinite( lbBoundValues ) || isFinite( ubBoundValues )))
00533 {
00534 lbValues.setup("lbValues", lbBoundValues, REAL, ACADO_VARIABLES);
00535 ubValues.setup("ubValues", ubBoundValues, REAL, ACADO_VARIABLES);
00536 }
00537 else if (isFinite( lbBoundValues ) || isFinite( ubBoundValues ))
00538 {
00539 lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
00540 lbValues.setDoc( "Lower bounds values." );
00541 ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
00542 ubValues.setDoc( "Upper bounds values." );
00543
00544 initialize.addStatement( lbValues == lbBoundValues );
00545 initialize.addStatement( ubValues == ubBoundValues );
00546 }
00547
00548 ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
00549
00550 if (performFullCondensing() == true)
00551 {
00552
00553 boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
00554 boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
00555 }
00556 else
00557 {
00558
00559 if (initialStateFixed() == true)
00560 {
00561
00562 condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
00563 condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
00564
00565 boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
00566 boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
00567 }
00568 else
00569 {
00570
00571 boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00572 boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00573 boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00574 boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00575 }
00576 }
00577 boundSetFcn->addLinebreak( );
00578
00580
00581
00582
00583
00585
00586 unsigned sizeA = numStateBounds + getNumComplexConstraints();
00587
00588 if ( sizeA )
00589 {
00590 DVector lbTmp, ubTmp;
00591
00592 if ( numStateBounds )
00593 {
00594 DVector lbStateBoundValues( numStateBounds );
00595 DVector ubStateBoundValues( numStateBounds );
00596 for (unsigned i = 0; i < numStateBounds; ++i)
00597 {
00598 lbStateBoundValues( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
00599 ubStateBoundValues( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
00600 }
00601
00602 lbTmp.append( lbStateBoundValues );
00603 ubTmp.append( ubStateBoundValues );
00604 }
00605
00606 lbTmp.append( lbPathConValues );
00607 ubTmp.append( ubPathConValues );
00608
00609 lbTmp.append( lbPointConValues );
00610 ubTmp.append( ubPointConValues );
00611
00612 if (hardcodeConstraintValues == true)
00613 {
00614 lbAValues.setup("lbAValues", lbTmp, REAL, ACADO_VARIABLES);
00615 ubAValues.setup("ubAValues", ubTmp, REAL, ACADO_VARIABLES);
00616 }
00617 else
00618 {
00619 lbAValues.setup("lbAValues", sizeA, 1, REAL, ACADO_VARIABLES);
00620 lbAValues.setDoc( "Lower bounds values for affine constraints." );
00621 ubAValues.setup("ubAValues", sizeA, 1, REAL, ACADO_VARIABLES);
00622 ubAValues.setDoc( "Upper bounds values for affine constraints." );
00623
00624 initialize.addStatement( lbAValues == lbTmp );
00625 initialize.addStatement( ubAValues == ubTmp );
00626 }
00627 }
00628
00630
00631
00632
00634
00635 if ( numStateBounds )
00636 {
00637 condenseFdb.addVariable( tmp );
00638
00639 unsigned offset = (performFullCondensing() == true) ? 0 : NX;
00640 unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
00641
00642 if (numOps < 1024)
00643 {
00644 for(unsigned row = 0; row < numStateBounds; ++row)
00645 {
00646 unsigned conIdx = xBoundsIdx[ row ] - NX;
00647
00648 if (performFullCondensing() == false)
00649 condensePrep.addStatement( A.getSubMatrix(row, row + 1, 0, NX) == evGx.getRow( conIdx ) );
00650
00651 unsigned blk = conIdx / NX + 1;
00652 for (unsigned col = 0; col < blk; ++col)
00653 {
00654 unsigned blkRow = (blk * (blk - 1) / 2 + col) * NX + conIdx % NX;
00655
00656 condensePrep.addStatement(
00657 A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
00658 }
00659
00660 condensePrep.addLinebreak();
00661 }
00662 }
00663 else
00664 {
00665 DMatrix vXBoundIndices(1, numStateBounds);
00666 for (unsigned i = 0; i < numStateBounds; ++i)
00667 vXBoundIndices(0, i) = xBoundsIdx[ i ];
00668 ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
00669
00670 condensePrep.addVariable( evXBounds );
00671
00672 ExportIndex row, col, conIdx, blk, blkRow;
00673
00674 condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );
00675
00676 ExportForLoop lRow(row, 0, numStateBounds);
00677
00678 lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
00679 lRow.addStatement( blk == conIdx / NX + 1);
00680
00681 if (performFullCondensing() == false)
00682 lRow.addStatement( A.getSubMatrix(row, row + 1, 0, NX) == evGx.getRow( conIdx ) );
00683
00684 ExportForLoop lCol(col, 0, blk);
00685
00686 lCol.addStatement( blkRow == (blk * (blk - 1) / 2 + col ) * NX + conIdx % NX );
00687 lCol.addStatement(
00688 A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
00689
00690 lRow.addStatement( lCol );
00691 condensePrep.addStatement( lRow );
00692
00693 condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
00694 }
00695 condensePrep.addLinebreak( );
00696
00697
00698 for(unsigned run1 = 0; run1 < numStateBounds; ++run1)
00699 {
00700 unsigned row = xBoundsIdx[ run1 ];
00701
00702 if (performFullCondensing() == true)
00703 {
00704 if (performsSingleShooting() == true)
00705 {
00706 condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + evGx.getRow(row - NX) * Dx0 );
00707 }
00708 else
00709 {
00710 condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + evGx.getRow(row - NX) * Dx0 );
00711 condenseFdb.addStatement( tmp += d.getRow(row - NX) );
00712 }
00713 condenseFdb.addStatement( lbA.getRow( run1 ) == lbAValues.getRow( run1 ) - tmp );
00714 condenseFdb.addStatement( ubA.getRow( run1 ) == ubAValues.getRow( run1 ) - tmp );
00715 }
00716 else
00717 {
00718 if (performsSingleShooting() == true)
00719 condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) );
00720 else
00721 condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + d.getRow(row - NX) );
00722
00723 condenseFdb.addStatement( lbA.getRow( run1 ) == lbAValues.getRow( run1 ) - tmp );
00724 condenseFdb.addStatement( ubA.getRow( run1 ) == ubAValues.getRow( run1 ) - tmp );
00725 }
00726 }
00727 condenseFdb.addLinebreak( );
00728 }
00729
00731
00732
00733
00735
00736 if (getNumComplexConstraints() == 0)
00737 return SUCCESSFUL_RETURN;
00738
00739 if ( numPathCon )
00740 {
00741 unsigned rowOffset = numStateBounds;
00742 unsigned colOffset = performFullCondensing() == true ? 0 : NX;
00743
00744
00745
00746
00747 ExportIndex runPac;
00748 condensePrep.acquire( runPac );
00749 ExportForLoop loopPac(runPac, 0, N);
00750
00751 loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
00752 loopPac.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( runPac ) );
00753 loopPac.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runPac ) );
00754 loopPac.addFunctionCall( evaluatePathConstraints.getName(), conValueIn, conValueOut );
00755
00756 loopPac.addStatement( pacEvH.getRows( runPac * dimPacH, (runPac + 1) * dimPacH) ==
00757 conValueOut.getTranspose().getRows(0, dimPacH) );
00758 loopPac.addLinebreak( );
00759
00760 unsigned derOffset = dimPacH;
00761
00762
00763 if ( pacEvHx.isGiven() == false )
00764 {
00765 loopPac.addStatement(
00766 pacEvHx.makeRowVector().
00767 getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX)
00768 == conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
00769 );
00770
00771 derOffset = derOffset + dimPacH * NX;
00772 }
00773 if (pacEvHu.isGiven() == false )
00774 {
00775 loopPac.addStatement(
00776 pacEvHu.makeRowVector().
00777 getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU)
00778 == conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
00779 );
00780 }
00781
00782
00783 condensePrep.addStatement( loopPac );
00784 condensePrep.addLinebreak( );
00785
00786
00787 ExportVariable tmpA01, tmpHx, tmpGx;
00788
00789 if (pacEvHx.isGiven() == true)
00790 tmpHx = pacEvHx;
00791 else
00792 tmpHx.setup("Hx", dimPacH, NX, REAL, ACADO_LOCAL);
00793
00794 tmpGx.setup("Gx", NX, NX, REAL, ACADO_LOCAL);
00795
00796 if (performFullCondensing() == true)
00797 {
00798 tmpA01.setup("A01", dimPacH, NX, REAL, ACADO_LOCAL);
00799
00800 multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
00801 multHxC.addStatement( tmpA01 == tmpHx * tmpGx );
00802
00803 A10.setup("A01", numPathCon, NX, REAL, ACADO_WORKSPACE);
00804 }
00805 else
00806 {
00807 tmpA01.setup("A01", dimPacH, getNumQPvars(), REAL, ACADO_LOCAL);
00808 multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
00809 multHxC.addStatement( tmpA01.getSubMatrix(0, dimPacH, 0, NX) == tmpHx * tmpGx );
00810
00811 A10 = A;
00812 }
00813
00814 unsigned offsetA01 = (performFullCondensing() == true) ? 0 : rowOffset;
00815
00816
00817 if (pacEvHx.isGiven() == true)
00818 {
00819 condensePrep.addStatement(
00820 A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
00821 == pacEvHx);
00822 }
00823 else
00824 {
00825 condensePrep.addStatement(
00826 A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
00827 == pacEvHx.getSubMatrix(0, dimPacH, 0, NX));
00828 }
00829 condensePrep.addLinebreak();
00830
00831
00832 for (unsigned row = 0; row < N - 1; ++row)
00833 {
00834 if (pacEvHx.isGiven() == true)
00835 {
00836 condensePrep.addFunctionCall(
00837 multHxC,
00838 pacEvHx,
00839 evGx.getAddress(row * NX, 0),
00840 A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
00841 }
00842 else
00843 {
00844 condensePrep.addFunctionCall(
00845 multHxC,
00846 pacEvHx.getAddress((row + 1) * dimPacH, 0),
00847 evGx.getAddress(row * NX, 0),
00848 A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
00849 }
00850 }
00851 condensePrep.addLinebreak();
00852
00853
00854
00855
00856 ExportVariable tmpE;
00857 tmpE.setup("E", NX, NU, REAL, ACADO_LOCAL);
00858 ExportIndex iRow( "row" ), iCol( "col" );
00859
00860 multHxE.setup("multHxE", tmpHx, tmpE, iRow, iCol);
00861 multHxE.addStatement(
00862 A.getSubMatrix( rowOffset + iRow * dimPacH,
00863 rowOffset + (iRow + 1) * dimPacH,
00864 colOffset + iCol * NU,
00865 colOffset + (iCol + 1) * NU)
00866 == tmpHx * tmpE );
00867
00868 if ( N <= 20 )
00869 {
00870 for (unsigned row = 0; row < N - 1; ++row)
00871 {
00872 for (unsigned col = 0; col <= row; ++col)
00873 {
00874 unsigned blk = (row + 1) * row / 2 + col;
00875 unsigned row2 = row + 1;
00876
00877 if (pacEvHx.isGiven() == true)
00878 condensePrep.addFunctionCall(
00879 multHxE,
00880 pacEvHx,
00881 E.getAddress(blk * NX, 0),
00882 ExportIndex( row2 ),
00883 ExportIndex( col )
00884 );
00885 else
00886 condensePrep.addFunctionCall(
00887 multHxE,
00888 pacEvHx.getAddress((row + 1) * dimPacH, 0),
00889 E.getAddress(blk * NX, 0),
00890 ExportIndex( row2 ),
00891 ExportIndex( col )
00892 );
00893 }
00894 }
00895 }
00896 else
00897 {
00898 ExportIndex row, col, blk, row2;
00899 condensePrep.acquire( row );
00900 condensePrep.acquire( col );
00901 condensePrep.acquire( blk );
00902 condensePrep.acquire( row2 );
00903
00904 ExportForLoop eLoopI(row, 0, N - 1);
00905 ExportForLoop eLoopJ(col, 0, row + 1);
00906
00907 eLoopJ.addStatement( blk == (row + 1) * row / 2 + col );
00908 eLoopJ.addStatement( row2 == row + 1 );
00909
00910 if (pacEvHx.isGiven() == true)
00911 {
00912 eLoopJ.addFunctionCall(
00913 multHxE,
00914 pacEvHx,
00915 E.getAddress(blk * NX, 0),
00916 row2,
00917 col
00918 );
00919 }
00920 else
00921 {
00922 eLoopJ.addFunctionCall(
00923 multHxE,
00924 pacEvHx.getAddress((row + 1) * dimPacH, 0),
00925 E.getAddress(blk * NX, 0),
00926 row2,
00927 col
00928 );
00929 }
00930
00931 eLoopI.addStatement( eLoopJ );
00932 condensePrep.addStatement( eLoopI );
00933
00934 condensePrep.release( row );
00935 condensePrep.release( col );
00936 condensePrep.release( blk );
00937 condensePrep.release( row2 );
00938 }
00939 condensePrep.addLinebreak();
00940
00941 if (pacEvHu.getDim() > 0)
00942 {
00943 for (unsigned i = 0; i < N; ++i)
00944 {
00945 if (pacEvHu.isGiven() == true)
00946 initialize.addStatement(
00947 A.getSubMatrix(
00948 rowOffset + i * dimPacH,
00949 rowOffset + (i + 1) * dimPacH,
00950 colOffset + i * NU,
00951 colOffset + (i + 1) * NU)
00952 == pacEvHu
00953 );
00954 else
00955 condensePrep.addStatement(
00956 A.getSubMatrix(
00957 rowOffset + i * dimPacH,
00958 rowOffset + (i + 1) * dimPacH,
00959 colOffset + i * NU,
00960 colOffset + (i + 1) * NU)
00961 == pacEvHu.getSubMatrix(
00962 i * dimPacH,(i + 1) * dimPacH, 0, NU)
00963 );
00964 }
00965 }
00966
00967
00968
00969
00970 condensePrep.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) ==
00971 lbAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
00972 condensePrep.addLinebreak();
00973 condensePrep.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) ==
00974 ubAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
00975 condensePrep.addLinebreak();
00976
00977 if (performFullCondensing() == true)
00978 {
00979 pacA01Dx0.setup("pacA01Dx0", numPathCon, 1, REAL, ACADO_WORKSPACE);
00980
00981 condenseFdb.addStatement( pacA01Dx0 == A10 * Dx0 );
00982 condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
00983 condenseFdb.addLinebreak();
00984 condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
00985 condenseFdb.addLinebreak();
00986 }
00987
00988
00989 if ( performsSingleShooting() == false )
00990 {
00991 ExportVariable tmpd("tmpd", NX, 1, REAL, ACADO_LOCAL);
00992 ExportVariable tmpLb("lbA", dimPacH, 1, REAL, ACADO_LOCAL);
00993 ExportVariable tmpUb("ubA", dimPacH, 1, REAL, ACADO_LOCAL);
00994
00995 macHxd.setup("macHxd", tmpHx, tmpd, tmpLb, tmpUb);
00996 macHxd.addStatement( pacEvHxd == tmpHx * tmpd );
00997 macHxd.addStatement( tmpLb -= pacEvHxd );
00998 macHxd.addStatement( tmpUb -= pacEvHxd );
00999
01000 for (unsigned i = 0; i < N - 1; ++i)
01001 {
01002 if (pacEvHx.isGiven() == true)
01003 {
01004 condensePrep.addFunctionCall(
01005 macHxd,
01006 pacEvHx,
01007 d.getAddress(i * NX),
01008 lbA.getAddress(rowOffset + (i + 1) * dimPacH),
01009 ubA.getAddress(rowOffset + (i + 1) * dimPacH)
01010 );
01011 }
01012 else
01013 {
01014 condensePrep.addFunctionCall(
01015 macHxd,
01016 pacEvHx.getAddress((i + 1) * dimPacH, 0),
01017 d.getAddress(i * NX),
01018 lbA.getAddress(rowOffset + (i + 1) * dimPacH),
01019 ubA.getAddress(rowOffset + (i + 1) * dimPacH)
01020 );
01021 }
01022 }
01023 condensePrep.addLinebreak();
01024 }
01025 }
01026
01028
01029
01030
01032
01033 if ( numPointCon )
01034 {
01035 unsigned rowOffset = getNumStateBounds() + N * dimPacH;
01036 unsigned colOffset = performFullCondensing() == true ? 0 : NX;
01037 unsigned dim;
01038
01039 if (performFullCondensing() == true)
01040 A20.setup("A02", dimPocH, NX, REAL, ACADO_WORKSPACE);
01041
01042
01043
01044
01045 for (unsigned i = 0, intRowOffset = 0; i < N + 1; ++i)
01046 {
01047 if (evaluatePointConstraints[ i ] == 0)
01048 continue;
01049
01050 condensePrep.addComment( string( "Evaluating constraint on node: #" ) + toString( i ) );
01051 condensePrep.addLinebreak();
01052
01053 condensePrep.addStatement(conValueIn.getCols(0, getNX()) == x.getRow( i ) );
01054 if (i < N)
01055 {
01056 condensePrep.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
01057 condensePrep.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
01058 }
01059 else
01060 condensePrep.addStatement( conValueIn.getCols(NX, NX + NOD) == od.getRow( i ) );
01061
01062 condensePrep.addFunctionCall( evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
01063 condensePrep.addLinebreak();
01064
01065 if (i < N)
01066 dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
01067 else
01068 dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
01069
01070
01071 condensePrep.addStatement(
01072 pocEvH.getRows(intRowOffset, intRowOffset + dim)
01073 == conValueOut.getTranspose().getRows(0, dim));
01074 condensePrep.addLinebreak();
01075
01076 condensePrep.addStatement(
01077 pocEvHx.makeRowVector().getCols(intRowOffset * NX,
01078 (intRowOffset + dim) * NX)
01079 == conValueOut.getCols(dim, dim + dim * NX));
01080 condensePrep.addLinebreak();
01081
01082 if (i < N)
01083 {
01084 condensePrep.addStatement(
01085 pocEvHu.makeRowVector().getCols(intRowOffset * NU,
01086 (intRowOffset + dim) * NU)
01087 == conValueOut.getCols(dim + dim * NX,
01088 dim + dim * NX + dim * NU));
01089 condensePrep.addLinebreak();
01090 }
01091
01092 intRowOffset += dim;
01093 }
01094
01095
01096
01097
01098 for (unsigned row = 0, intRowOffset = 0; row < N + 1; ++row)
01099 {
01100 if (evaluatePointConstraints[ row ] == 0)
01101 continue;
01102
01103 condensePrep.addComment(
01104 string( "Evaluating multiplications of constraint functions on node: #" ) + toString( row ) );
01105 condensePrep.addLinebreak();
01106
01107 if (row < N)
01108 dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX + NU);
01109 else
01110 dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX);
01111
01112 if (row == 0)
01113 {
01114 if (performFullCondensing() == true)
01115 condensePrep.addStatement(
01116 A20.getSubMatrix(0, dim, 0, NX)
01117 == pocEvHx.getSubMatrix(0, dim, 0, NX));
01118 else
01119 condensePrep.addStatement(
01120 A.getSubMatrix(rowOffset, rowOffset + dim, 0, NX)
01121 == pocEvHx.getSubMatrix(0, dim, 0, NX));
01122 condensePrep.addLinebreak();
01123
01124 condensePrep.addStatement(
01125 A.getSubMatrix(rowOffset, rowOffset + dim, colOffset,
01126 colOffset + NU)
01127 == pocEvHu.getSubMatrix(0, dim, 0, NU));
01128 condensePrep.addLinebreak();
01129 }
01130 else
01131 {
01132
01133 if (performFullCondensing() == true)
01134 condensePrep.addStatement(
01135 A20.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) ==
01136 pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
01137 evGx.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
01138 else
01139 condensePrep.addStatement(
01140 A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim, 0, NX) ==
01141 pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
01142 evGx.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
01143 condensePrep.addLinebreak();
01144
01145
01146 ExportIndex iCol, iBlk;
01147 condensePrep.acquire( iCol );
01148 condensePrep.acquire( iBlk );
01149 ExportForLoop eLoop(iCol, 0, row);
01150
01151
01152 eLoop.addStatement( iBlk == row * (row - 1) / 2 + iCol );
01153 eLoop.addStatement(
01154 A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
01155 colOffset + iCol * NU, colOffset + (iCol + 1) * NU) ==
01156 pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
01157 E.getSubMatrix(iBlk * NX, (iBlk + 1) * NX, 0, NU)
01158 );
01159
01160 condensePrep.addStatement( eLoop );
01161 condensePrep.release( iCol );
01162 condensePrep.release( iBlk );
01163
01164
01165 if (performsSingleShooting() == false)
01166 {
01167 condensePrep.addStatement(
01168 pocEvHxd.getRows(intRowOffset, intRowOffset + dim) ==
01169 pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
01170 d.getRows((row - 1) * NX, row * NX) );
01171 condensePrep.addLinebreak();
01172 }
01173
01174
01175 if (row < N)
01176 {
01177 condensePrep.addStatement(
01178 A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
01179 colOffset + row * NU, colOffset + (row + 1) * NU) ==
01180 pocEvHu.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NU));
01181 condensePrep.addLinebreak();
01182 }
01183 }
01184
01185 intRowOffset += dim;
01186 }
01187
01188
01189
01190
01191 condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) ==
01192 lbAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
01193 condensePrep.addLinebreak();
01194 condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) ==
01195 ubAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
01196 condensePrep.addLinebreak();
01197
01198 if (performFullCondensing() == true)
01199 {
01200 pocA02Dx0.setup("pacA02Dx0", dimPocH, 1, REAL, ACADO_WORKSPACE);
01201
01202 condenseFdb.addStatement( pocA02Dx0 == A20 * Dx0 );
01203 condenseFdb.addLinebreak();
01204 condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
01205 condenseFdb.addLinebreak();
01206 condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
01207 condenseFdb.addLinebreak();
01208 }
01209
01210 if (performsSingleShooting() == false)
01211 {
01212 condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
01213 condensePrep.addLinebreak();
01214 condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
01215 condensePrep.addLinebreak();
01216 }
01217 }
01218
01219 return SUCCESSFUL_RETURN;
01220 }
01221
01222 returnValue ExportGaussNewtonCondensed::setupCondensing( void )
01223 {
01224
01225
01226
01227 DMatrix mRegH00 = eye<double>( getNX() );
01228 mRegH00 *= levenbergMarquardt;
01229
01230 condensePrep.setup("condensePrep");
01231 condenseFdb.setup( "condenseFdb" );
01232
01234
01235
01236
01238
01239 LOG( LVL_DEBUG ) << "Setup condensing: create C & E matrices" << endl;
01240
01241
01242 condensePrep.addFunctionCall(moveGuE, evGu.getAddress(0, 0), E.getAddress(0, 0) );
01243
01244 if (N <= 20)
01245 {
01246 unsigned row, col, prev, curr;
01247 for (row = 1; row < N; ++row)
01248 {
01249 condensePrep.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T);
01250
01251 if (performsSingleShooting() == false)
01252 condensePrep.addFunctionCall(multGxd, d.getAddress((row - 1) * NX), evGx.getAddress(row * NX), d.getAddress(row * NX));
01253
01254 condensePrep.addFunctionCall(multGxGx, T, evGx.getAddress((row - 1) * NX, 0), evGx.getAddress(row * NX, 0));
01255 condensePrep.addLinebreak();
01256
01257 for(col = 0; col < row; ++col)
01258 {
01259 prev = row * (row - 1) / 2 + col;
01260 curr = (row + 1) * row / 2 + col;
01261
01262 condensePrep.addFunctionCall(multGxGu, T, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0));
01263 }
01264 condensePrep.addLinebreak();
01265
01266 curr = (row + 1) * row / 2 + col;
01267 condensePrep.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
01268
01269 condensePrep.addLinebreak();
01270 }
01271 }
01272 else
01273 {
01274 ExportIndex row, col, curr, prev;
01275
01276 condensePrep.acquire( row );
01277 condensePrep.acquire( col );
01278 condensePrep.acquire( curr );
01279 condensePrep.acquire( prev );
01280
01281 ExportForLoop eLoopI(row, 1, N);
01282 ExportForLoop eLoopJ(col, 0, row);
01283
01284 eLoopI.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T);
01285
01286 if (performsSingleShooting() == false)
01287 eLoopI.addFunctionCall(multGxd, d.getAddress((row - 1) * NX), evGx.getAddress(row * NX), d.getAddress(row * NX));
01288
01289 eLoopI.addFunctionCall(multGxGx, T, evGx.getAddress((row - 1) * NX, 0), evGx.getAddress(row * NX, 0));
01290
01291 eLoopJ.addStatement( prev == row * (row - 1) / 2 + col );
01292 eLoopJ.addStatement( curr == (row + 1) * row / 2 + col );
01293 eLoopJ.addFunctionCall( multGxGu, T, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0) );
01294
01295 eLoopI.addStatement( eLoopJ );
01296 eLoopI.addStatement( curr == (row + 1) * row / 2 + col );
01297 eLoopI.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
01298
01299 condensePrep.addStatement( eLoopI );
01300 condensePrep.addLinebreak();
01301
01302 condensePrep.release( row );
01303 condensePrep.release( col );
01304 condensePrep.release( curr );
01305 condensePrep.release( prev );
01306 }
01307
01308
01309
01310
01311 LOG( LVL_DEBUG ) << "Setup condensing: multiply with Q1" << endl;
01312
01313 if (performFullCondensing() == false)
01314 {
01315 for (unsigned i = 0; i < N - 1; ++i)
01316 if (Q1.isGiven() == true)
01317 condensePrep.addFunctionCall(multQ1Gx, evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
01318 else
01319 condensePrep.addFunctionCall(multGxGx, Q1.getAddress((i + 1) * NX, 0), evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
01320
01321 if (QN1.isGiven() == true)
01322 condensePrep.addFunctionCall(multQN1Gx, evGx.getAddress((N - 1) * NX, 0), QGx.getAddress((N - 1) * NX, 0));
01323 else
01324 condensePrep.addFunctionCall(multGxGx, QN1, evGx.getAddress((N - 1) * NX, 0), QGx.getAddress((N - 1) * NX, 0));
01325 condensePrep.addLinebreak();
01326 }
01327
01328 if (N <= 20)
01329 {
01330 for (unsigned i = 0; i < N; ++i)
01331 for (unsigned j = 0; j <= i; ++j)
01332 {
01333 unsigned k = (i + 1) * i / 2 + j;
01334
01335 if (i < N - 1)
01336 {
01337 if (Q1.isGiven() == true)
01338 condensePrep.addFunctionCall(multQ1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01339 else
01340 condensePrep.addFunctionCall(multGxGu, Q1.getAddress((i + 1) * NX, 0), E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01341 }
01342 else
01343 {
01344 if (QN1.isGiven() == true)
01345 condensePrep.addFunctionCall(multQN1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01346 else
01347 condensePrep.addFunctionCall(multGxGu, QN1, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01348 }
01349 }
01350 condensePrep.addLinebreak();
01351 }
01352 else
01353 {
01354 ExportIndex i, j, k;
01355
01356 condensePrep.acquire( i );
01357 condensePrep.acquire( j );
01358 condensePrep.acquire( k );
01359
01360 ExportForLoop eLoopI(i, 0, N - 1);
01361 ExportForLoop eLoopJ1(j, 0, i + 1);
01362 ExportForLoop eLoopJ2(j, 0, i + 1);
01363
01364 eLoopJ1.addStatement( k == (i + 1) * i / 2 + j );
01365
01366 if (Q1.isGiven() == true)
01367 eLoopJ1.addFunctionCall(multQ1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01368 else
01369 eLoopJ1.addFunctionCall(multGxGu, Q1.getAddress((i + 1) * NX, 0), E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01370
01371 eLoopI.addStatement( eLoopJ1 );
01372
01373 eLoopJ2.addStatement( k == (i + 1) * i / 2 + j );
01374
01375 if (QN1.isGiven() == true)
01376 eLoopJ2.addFunctionCall(multQN1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01377 else
01378 eLoopJ2.addFunctionCall(multGxGu, QN1, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01379
01380 condensePrep.addStatement( eLoopI );
01381 condensePrep.addLinebreak();
01382 condensePrep.addStatement( eLoopJ2 );
01383 condensePrep.addLinebreak();
01384
01385 condensePrep.release( i );
01386 condensePrep.release( j );
01387 condensePrep.release( k );
01388 }
01389
01391
01392
01393
01395
01396 LOG( LVL_DEBUG ) << "Setup condensing: create H00, H10 & H11" << endl;
01397
01398 LOG( LVL_DEBUG ) << "---> Create H00" << endl;
01399
01400
01401
01402
01403 if (performFullCondensing() == false)
01404 {
01405 condensePrep.addFunctionCall( zeroBlockH00 );
01406
01407 for (unsigned i = 0; i < N; ++i)
01408 condensePrep.addFunctionCall(multCTQC, evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
01409
01410 condensePrep.addStatement( H00 += mRegH00 );
01411 condensePrep.addLinebreak();
01412
01413
01414 if (initialStateFixed() == false)
01415 {
01416 if (Q1.isGiven() == true)
01417 {
01418 condensePrep.addStatement( H00 += Q1 );
01419 }
01420 else
01421 {
01422 condensePrep.addStatement( H00 += Q1.getSubMatrix(0, NX, 0, NX) );
01423 }
01424 }
01425
01426 if (SAC.getDim() > 0)
01427 condensePrep.addStatement( H00 += SAC );
01428 }
01429
01430 LOG( LVL_DEBUG ) << "---> Create H10" << endl;
01431
01432
01433
01434
01435 if (N <= 20)
01436 {
01437 for (unsigned i = 0; i < N; ++i)
01438 {
01439 condensePrep.addFunctionCall(zeroBlockH10, H10.getAddress(i * NU));
01440
01441 for (unsigned j = i; j < N; ++j)
01442 {
01443 unsigned k = (j + 1) * j / 2 + i;
01444
01445 condensePrep.addFunctionCall(multQETGx, QE.getAddress(k * NX), evGx.getAddress(j * NX), H10.getAddress(i * NU));
01446 }
01447 }
01448 }
01449 else
01450 {
01451 ExportIndex ii, jj, kk;
01452 condensePrep.acquire( ii );
01453 condensePrep.acquire( jj );
01454 condensePrep.acquire( kk );
01455
01456 ExportForLoop eLoopI(ii, 0, N);
01457 ExportForLoop eLoopJ(jj, ii, N);
01458
01459 eLoopI.addFunctionCall(zeroBlockH10, H10.getAddress(ii * NU));
01460
01461 eLoopJ.addStatement( kk == (jj + 1) * jj / 2 + ii );
01462 eLoopJ.addFunctionCall( multQETGx, QE.getAddress(kk * NX), evGx.getAddress(jj * NX), H10.getAddress(ii * NU) );
01463
01464 eLoopI.addStatement( eLoopJ );
01465 condensePrep.addStatement( eLoopI );
01466
01467 condensePrep.release( ii );
01468 condensePrep.release( jj );
01469 condensePrep.release( kk );
01470 }
01471 condensePrep.addLinebreak();
01472
01473 LOG( LVL_DEBUG ) << "---> Setup H01" << endl;
01474
01475
01476
01477
01478 if (performFullCondensing() == false)
01479 {
01480 condensePrep.addStatement( H.getSubMatrix(0, NX, NX, getNumQPvars()) == H10.getTranspose() );
01481 condensePrep.addLinebreak();
01482 }
01483
01484 LOG( LVL_DEBUG ) << "---> Create H11" << endl;
01485
01486
01487
01488
01489 if (N <= 20)
01490 {
01491 unsigned row, col;
01492
01493 for (row = 0; row < N; ++row)
01494 {
01495
01496 if (R1.isGiven() == true)
01497 condensePrep.addFunctionCall(setBlockH11_R1, ExportIndex(row), ExportIndex(row), R1);
01498 else
01499 condensePrep.addFunctionCall(setBlockH11_R1, ExportIndex(row), ExportIndex(row), R1.getAddress(row * NU));
01500
01501 col = row;
01502 for(unsigned blk = col; blk < N; ++blk)
01503 {
01504 unsigned indl = (blk + 1) * blk / 2 + row;
01505 unsigned indr = (blk + 1) * blk / 2 + col;
01506
01507 condensePrep.addFunctionCall(
01508 setBlockH11, ExportIndex(row), ExportIndex(col),
01509 E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01510 }
01511 condensePrep.addLinebreak();
01512
01513
01514 for(col = row + 1; col < N; ++col)
01515 {
01516 condensePrep.addFunctionCall(
01517 zeroBlockH11, ExportIndex(row), ExportIndex(col)
01518 );
01519
01520 for(unsigned blk = col; blk < N; ++blk)
01521 {
01522 unsigned indl = (blk + 1) * blk / 2 + row;
01523 unsigned indr = (blk + 1) * blk / 2 + col;
01524
01525 condensePrep.addFunctionCall(
01526 setBlockH11, ExportIndex(row), ExportIndex(col),
01527 E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01528 }
01529 condensePrep.addLinebreak();
01530 }
01531 }
01532 }
01533 else
01534 {
01535 ExportIndex row, col, blk, indl, indr;
01536 condensePrep.acquire( row ).acquire( col ).acquire( blk ).acquire( indl ).acquire( indr );
01537
01538 ExportForLoop eLoopI(row, 0, N);
01539 ExportForLoop eLoopK(blk, row, N);
01540 ExportForLoop eLoopJ(col, row + 1, N);
01541 ExportForLoop eLoopK2(blk, col, N);
01542
01543
01544
01545 if (R1.isGiven() == true)
01546 eLoopI.addFunctionCall(setBlockH11_R1, row, row, R1);
01547 else
01548 eLoopI.addFunctionCall(setBlockH11_R1, row, row, R1.getAddress(row * NU));
01549
01550 eLoopI.addStatement( col == row );
01551 eLoopK.addStatement( indl == (blk + 1) * blk / 2 + row );
01552 eLoopK.addStatement( indr == (blk + 1) * blk / 2 + col );
01553 eLoopK.addFunctionCall( setBlockH11, row, col, E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01554 eLoopI.addStatement( eLoopK );
01555
01556
01557 eLoopJ.addFunctionCall( zeroBlockH11, row, col );
01558
01559 eLoopK2.addStatement( indl == (blk + 1) * blk / 2 + row );
01560 eLoopK2.addStatement( indr == (blk + 1) * blk / 2 + col );
01561 eLoopK2.addFunctionCall( setBlockH11, row, col, E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01562 eLoopJ.addStatement( eLoopK2 );
01563
01564 eLoopI.addStatement( eLoopJ );
01565 condensePrep.addStatement( eLoopI );
01566
01567 condensePrep.release( row ).release( col ).release( blk ).release( indl ).release( indr );
01568 }
01569 condensePrep.addLinebreak();
01570
01571 LOG( LVL_DEBUG ) << "---> Copy H11 lower part" << endl;
01572
01573 unsigned offset = (performFullCondensing() == true) ? 0 : NX;
01574
01575
01576 if (N <= 20)
01577 {
01578 for (unsigned ii = 0; ii < N; ++ii)
01579 for(unsigned jj = 0; jj < ii; ++jj)
01580 condensePrep.addFunctionCall(
01581 copyHTH, ExportIndex( ii ), ExportIndex( jj )
01582 );
01583 }
01584 else
01585 {
01586 ExportIndex ii, jj;
01587
01588 condensePrep.acquire( ii );
01589 condensePrep.acquire( jj );
01590
01591 ExportForLoop eLoopI(ii, 0, N);
01592 ExportForLoop eLoopJ(jj, 0, ii);
01593
01594 eLoopJ.addFunctionCall(copyHTH, ii, jj);
01595 eLoopI.addStatement( eLoopJ );
01596 condensePrep.addStatement( eLoopI );
01597
01598 condensePrep.release( ii );
01599 condensePrep.release( jj );
01600 }
01601 condensePrep.addLinebreak();
01602
01603 LOG( LVL_DEBUG ) << "---> Copy H10" << endl;
01604
01605
01606
01607
01608 if (performFullCondensing() == false)
01609 {
01610 condensePrep.addStatement( H.getSubMatrix(NX, getNumQPvars(), 0, NX) == H10 );
01611 condensePrep.addLinebreak();
01612 }
01613
01614 int externalCholesky;
01615 get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
01616 ASSERT((CondensedHessianCholeskyDecomposition)externalCholesky == INTERNAL_N3 ||
01617 (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL)
01618
01619 if ((CondensedHessianCholeskyDecomposition)externalCholesky == INTERNAL_N3)
01620 {
01621 R.setup("R", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01622
01623 cholSolver.init(getNumQPvars(), NX, "condensing");
01624 cholSolver.setup();
01625
01626 condensePrep.addStatement( R == H );
01627 condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), R);
01628 }
01629
01631
01632
01633
01635
01636
01637
01638
01639 LOG( LVL_DEBUG ) << "Setup condensing: compute Qd" << endl;
01640
01641 if (performsSingleShooting() == false)
01642 {
01643 Qd.setup("Qd", N * NX, 1, REAL, ACADO_WORKSPACE);
01644
01645 for(unsigned i = 0; i < N - 1; ++i)
01646 {
01647 if (Q1.isGiven() == true)
01648 condensePrep.addFunctionCall(
01649 multQ1d, Q1, d.getAddress(i * NX), Qd.getAddress(i * NX) );
01650 else
01651 condensePrep.addFunctionCall(
01652 multQ1d, Q1.getAddress((i + 1) * NX, 0), d.getAddress(i * NX), Qd.getAddress(i * NX) );
01653 }
01654
01655 condensePrep.addFunctionCall(
01656 multQN1d, QN1, d.getAddress((N - 1) * NX), Qd.getAddress((N - 1) * NX) );
01657
01658 condensePrep.addLinebreak();
01659 }
01660
01661 LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
01662
01663 if (initialStateFixed() == true)
01664 {
01665 condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
01666 condenseFdb.addLinebreak();
01667 }
01668
01669 condenseFdb.addStatement( Dy -= y );
01670 condenseFdb.addStatement( DyN -= yN );
01671 condenseFdb.addLinebreak();
01672
01673
01674 for(unsigned run1 = 0; run1 < N; ++run1)
01675 {
01676 if (R2.isGiven() == true)
01677 condenseFdb.addFunctionCall(
01678 multRDy, R2,
01679 Dy.getAddress(run1 * NY, 0),
01680 g.getAddress(offset + run1 * NU, 0) );
01681 else
01682 condenseFdb.addFunctionCall(
01683 multRDy, R2.getAddress(run1 * NU, 0),
01684 Dy.getAddress(run1 * NY, 0),
01685 g.getAddress(offset + run1 * NU, 0) );
01686 }
01687 condenseFdb.addLinebreak();
01688
01689
01690
01691 for(unsigned run1 = 0; run1 < N; run1++ )
01692 {
01693 if (Q2.isGiven() == true)
01694 condenseFdb.addFunctionCall(
01695 multQDy, Q2,
01696 Dy.getAddress(run1 * NY),
01697 QDy.getAddress(run1 * NX) );
01698 else
01699 condenseFdb.addFunctionCall(
01700 multQDy, Q2.getAddress(run1 * NX),
01701 Dy.getAddress(run1 * NY),
01702 QDy.getAddress(run1 * NX) );
01703 }
01704 condenseFdb.addLinebreak();
01705 condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == QN2 * DyN );
01706 condenseFdb.addLinebreak();
01707
01708 LOG( LVL_DEBUG ) << "Setup condensing: QDy.getRows(NX, (N + 1) * NX) += Qd" << endl;
01709
01710 if (performsSingleShooting() == false)
01711 {
01712 condenseFdb.addStatement( QDy.getRows(NX, (N + 1) * NX) += Qd );
01713 condenseFdb.addLinebreak();
01714 }
01715
01716 if (performFullCondensing() == false)
01717 {
01718
01719 condenseFdb.addStatement( g0 == (evGx ^ QDy.getRows(NX, (N + 1) * NX)) );
01720 condenseFdb.addLinebreak();
01721
01722 if (initialStateFixed() == false)
01723 condenseFdb.addStatement( g0 += QDy.getRows(0, NX) );
01724
01725 if (SAC.getDim() > 0)
01726 {
01727
01728 condenseFdb.addStatement( DxAC == x.getRow( 0 ).getTranspose() - xAC );
01729 condenseFdb.addStatement( g0 += SAC * DxAC );
01730 }
01731
01732 condenseFdb.addLinebreak();
01733 }
01734
01735 if (N <= 20)
01736 {
01737 for (unsigned i = 0; i < N; ++i)
01738 for (unsigned j = i; j < N; ++j)
01739 {
01740 unsigned k = (j + 1) * j / 2 + i;
01741
01742 condenseFdb.addFunctionCall(
01743 multEQDy, E.getAddress(k * NX, 0), QDy.getAddress((j + 1) * NX), g.getAddress(offset + i * NU) );
01744 }
01745 }
01746 else
01747 {
01748 ExportIndex i, j, k;
01749 condenseFdb.acquire( i );
01750 condenseFdb.acquire( j );
01751 condenseFdb.acquire( k );
01752
01753 ExportForLoop eLoopI(i, 0, N);
01754 ExportForLoop eLoopJ(j, i, N);
01755
01756 eLoopJ.addStatement( k == (j + 1) * j / 2 + i );
01757 eLoopJ.addFunctionCall(
01758 multEQDy, E.getAddress(k * NX, 0), QDy.getAddress((j + 1) * NX), g.getAddress(offset + i * NU) );
01759
01760 eLoopI.addStatement( eLoopJ );
01761 condenseFdb.addStatement( eLoopI );
01762
01763 condenseFdb.release( i );
01764 condenseFdb.release( j );
01765 condenseFdb.acquire( k );
01766 }
01767 condenseFdb.addLinebreak();
01768
01769 if (performFullCondensing() == true)
01770 {
01771 condenseFdb.addStatement( g1 += H10 * Dx0 );
01772 condenseFdb.addLinebreak();
01773 }
01774
01775
01776
01777
01778 if (performFullCondensing() == false && objSlx.getDim() > 0)
01779 {
01780 condensePrep.addStatement( g0 += objSlx );
01781
01782 ExportVariable g00, C0, Slx0;
01783 g00.setup("g0", NX, 1, REAL, ACADO_LOCAL);
01784 C0.setup("C0", NX, NX, REAL, ACADO_LOCAL);
01785
01786 if (objSlx.isGiven() == false)
01787 Slx0.setup("Slx0", NX, 1, REAL, ACADO_LOCAL);
01788 else
01789 Slx0 = objSlx;
01790
01791 macCTSlx.setup("macCTSlx", C0, Slx0, g00);
01792 macCTSlx.addStatement( g00 += C0.getTranspose() * Slx0);
01793
01794 for (unsigned i = 0; i < N; ++i)
01795 condensePrep.addFunctionCall(macCTSlx, evGx.getAddress(i * NX, 0), objSlx, g);
01796 }
01797
01798 if (objSlx.getDim() > 0 || objSlu.getDim() > 0)
01799 {
01800 offset = performFullCondensing() == true ? 0 : NX;
01801
01802 if (objSlu.getDim() > 0 && objSlx.getDim() == 0)
01803 {
01804 for (unsigned i = 0; i < N; ++i)
01805 {
01806 condensePrep.addStatement(
01807 g.getRows(offset + i * NU, offset + (i + 1) * NU) += objSlu
01808 );
01809 }
01810 }
01811 else
01812 {
01813 ExportVariable g10, E0, Slx0, Slu0;
01814
01815 g10.setup("g1", NU, 1, REAL, ACADO_LOCAL);
01816 E0.setup("E0", NX, NU, REAL, ACADO_LOCAL);
01817
01818 if (objSlx.isGiven() == false && objSlx.getDim() > 0)
01819 Slx0.setup("Slx0", NX, 1, REAL, ACADO_LOCAL);
01820 else
01821 Slx0 = objSlx;
01822
01823 macETSlu.setup("macETSlu", E0, Slx0, g10);
01824 macETSlu.addStatement( g10 += E0.getTranspose() * Slx0 );
01825
01826 if (N <= 20)
01827 {
01828 for (unsigned i = 0; i < N; ++i)
01829 for (unsigned j = i; j < N; ++j)
01830 {
01831 unsigned k = (j + 1) * j / 2 + i;
01832
01833 condensePrep.addFunctionCall(macETSlu, QE.getAddress(k * NX), objSlx, g.getAddress(offset + i * NU));
01834 }
01835 }
01836 else
01837 {
01838 ExportIndex ii, jj, kk;
01839 condensePrep.acquire( ii );
01840 condensePrep.acquire( jj );
01841 condensePrep.acquire( kk );
01842
01843 ExportForLoop iLoop(ii, 0, N);
01844 ExportForLoop jLoop(jj, ii, N);
01845
01846 jLoop.addStatement( kk == (jj + 1) * jj / 2 + ii );
01847 jLoop.addFunctionCall(macETSlu, QE.getAddress(kk * NX), objSlx, g.getAddress(offset + ii * NU));
01848
01849 iLoop.addStatement( jLoop );
01850 condensePrep.addStatement( iLoop );
01851
01852 condensePrep.release( ii );
01853 condensePrep.release( jj );
01854 condensePrep.release( kk );
01855 }
01856
01857 if (objSlu.getDim() > 0)
01858 {
01859 for (unsigned i = 0; i < N; ++i)
01860 condensePrep.addStatement(
01861 g.getRows(offset + i * NU, offset + (i + 1) * NU) += objSlu);
01862 }
01863 }
01864 }
01865
01867
01868
01869
01871
01872 LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
01873
01874 expand.setup( "expand" );
01875
01876 if (performFullCondensing() == true)
01877 {
01878 expand.addStatement( u.makeRowVector() += xVars.getTranspose() );
01879 expand.addLinebreak();
01880 expand.addStatement( x.getRow( 0 ) += Dx0.getTranspose() );
01881 }
01882 else
01883 {
01884 expand.addStatement( x.makeColVector().getRows(0, NX) += xVars.getRows(0, NX) );
01885 expand.addLinebreak();
01886 expand.addStatement( u.makeRowVector() += xVars.getTranspose().getCols(NX, getNumQPvars() ) );
01887 }
01888 expand.addLinebreak();
01889
01890
01891
01892 if (performsSingleShooting() == false)
01893 {
01894
01895 if (performFullCondensing() == true)
01896 expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += d + evGx * Dx0);
01897 else
01898 expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += d + evGx * xVars.getRows(0, NX) );
01899 }
01900 else
01901 {
01902 if (performFullCondensing() == true)
01903 expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += evGx * Dx0);
01904 else
01905 expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += evGx * xVars.getRows(0, NX) );
01906 }
01907
01908 expand.addLinebreak();
01909
01910
01911
01912 offset = (performFullCondensing() == true) ? 0 : NX;
01913
01914 if (N <= 20)
01915 {
01916 for (unsigned i = 0; i < N; ++i)
01917 for (unsigned j = 0; j <= i; ++j)
01918 {
01919 unsigned k = (i + 1) * i / 2 + j;
01920
01921 expand.addFunctionCall(
01922 multEDu, E.getAddress(k * NX, 0), xVars.getAddress(offset + j * NU), x.getAddress(i + 1) );
01923 }
01924 }
01925 else
01926 {
01927 ExportIndex ii, jj, kk;
01928 expand.acquire( ii );
01929 expand.acquire( jj );
01930 expand.acquire( kk );
01931
01932 ExportForLoop eLoopI(ii, 0, N);
01933 ExportForLoop eLoopJ(jj, 0, ii + 1);
01934
01935 eLoopJ.addStatement( kk == (ii + 1) * ii / 2 + jj );
01936 eLoopJ.addFunctionCall(
01937 multEDu, E.getAddress(kk * NX, 0), xVars.getAddress(offset + jj * NU), x.getAddress(ii + 1) );
01938
01939 eLoopI.addStatement( eLoopJ );
01940 expand.addStatement( eLoopI );
01941
01942 expand.release( ii );
01943 expand.release( jj );
01944 expand.release( kk );
01945 }
01946
01948
01949
01950
01952
01953 int covCalc;
01954 get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
01955
01956 if (covCalc == NO)
01957 return SUCCESSFUL_RETURN;
01958
01959 if (performFullCondensing() == true)
01960 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, \
01961 "Calculation of covariance matrix works for partial condensing only atm.");
01962
01963 LOG( LVL_DEBUG ) << "Setup condensing: covariance matrix calculation" << endl;
01964
01965 CEN.setup("CEN", NX, getNumQPvars(), REAL, ACADO_WORKSPACE);
01966 sigmaTmp.setup("sigmaTemp", getNumQPvars(), NX, REAL, ACADO_WORKSPACE);
01967 sigma.setup("sigma", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01968
01969 sigmaN.setup("sigmaN", NX, NX, REAL, ACADO_VARIABLES);
01970 sigmaN.setDoc("Covariance matrix of the last state estimate.");
01971
01972 calculateCovariance.setup("calculateCovariance");
01973
01974 calculateCovariance.addStatement(
01975 CEN.getSubMatrix(0, NX, 0, NX) == evGx.getSubMatrix((N - 1) * NX, N * NX, 0, NX) );
01976 calculateCovariance.addLinebreak();
01977
01978 ExportIndex cIndex("cIndex");
01979 ExportForLoop cLoop(cIndex, 0, N);
01980 calculateCovariance.addIndex( cIndex );
01981
01982 unsigned blk = (N - 1 + 1) * (N - 1) / 2;
01983 cLoop.addStatement(
01984 CEN.getSubMatrix(0, NX, NX + cIndex * NU, NX + NU + cIndex * NU) ==
01985 E.getSubMatrix(blk * NX + cIndex * NX, blk * NX + NX + cIndex * NX, 0, NU)
01986 );
01987 calculateCovariance.addStatement( cLoop );
01988 calculateCovariance.addLinebreak();
01989
01990
01991 calculateCovariance.addStatement(
01992 sigmaTmp == sigma * CEN.getTranspose()
01993 );
01994 calculateCovariance.addLinebreak();
01995
01996 calculateCovariance.addStatement(
01997 sigmaN == CEN * sigmaTmp
01998 );
01999
02000 return SUCCESSFUL_RETURN;
02001 }
02002
02003 returnValue ExportGaussNewtonCondensed::setupVariables( )
02004 {
02006
02007
02008
02010
02011 bool boxConIsFinite = false;
02012 xBoundsIdx.clear();
02013
02014 DVector lbBox, ubBox;
02015 for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
02016 {
02017 lbBox = xBounds.getLowerBounds( i );
02018 ubBox = xBounds.getUpperBounds( i );
02019
02020 if (isFinite( lbBox ) || isFinite( ubBox ))
02021 boxConIsFinite = true;
02022
02023
02024 if (boxConIsFinite == false || i == 0)
02025 continue;
02026
02027 for (unsigned j = 0; j < lbBox.getDim(); ++j)
02028 {
02029 if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
02030 {
02031 xBoundsIdx.push_back(i * lbBox.getDim() + j);
02032 }
02033 }
02034 }
02035
02036 if (initialStateFixed() == true)
02037 {
02038 x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
02039 x0.setDoc( (std::string)"Current state feedback vector." );
02040 Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
02041 }
02042
02043 T.setup("T", NX, NX, REAL, ACADO_WORKSPACE);
02044 E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
02045 QE.setup("QE", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
02046
02047 if (performFullCondensing() == false)
02048 QGx.setup("QGx", N * NX, NX, REAL, ACADO_WORKSPACE);
02049
02050 QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
02051
02052
02053
02054 H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
02055
02056
02057 if (performFullCondensing() == true)
02058 {
02059 H11 = H;
02060 }
02061 else
02062 {
02063 H00 = H.getSubMatrix(0, NX, 0, NX);
02064 H11 = H.getSubMatrix(NX, getNumQPvars(), NX, getNumQPvars());
02065 }
02066
02067 H10.setup("H10", N * NU, NX, REAL, ACADO_WORKSPACE);
02068
02069 A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
02070
02071 g.setup("g", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02072
02073 if (performFullCondensing() == true)
02074 {
02075 g1 = g;
02076 }
02077 else
02078 {
02079 g0 = g.getRows(0, NX);
02080 g1 = g.getRows(NX, getNumQPvars());
02081 }
02082
02083 lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02084 ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02085
02086 lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
02087 ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
02088
02089 xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02090 yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
02091
02092 return SUCCESSFUL_RETURN;
02093 }
02094
02095 returnValue ExportGaussNewtonCondensed::setupMultiplicationRoutines( )
02096 {
02097 ExportIndex iCol( "iCol" );
02098 ExportIndex iRow( "iRow" );
02099
02100 ExportVariable dp, dn, Gx1, Gx2, Gx3, Gu1, Gu2;
02101 ExportVariable R22, R11, Dy1, RDy1, Q22, QDy1, E1, U1, H101;
02102 dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
02103 dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
02104 Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
02105 Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
02106 Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
02107 Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
02108 Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
02109 R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
02110 R11.setup("R11", NU, NU, REAL, ACADO_LOCAL);
02111 Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
02112 RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
02113 Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
02114 QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
02115 E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
02116 U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
02117 H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
02118
02119 if ( Q2.isGiven() )
02120 Q22 = Q2;
02121 if ( R2.isGiven() )
02122 R22 = R2;
02123 if ( R1.isGiven() )
02124 R11 = R1;
02125
02126
02127 multGxd.setup("multGxd", dp, Gx1, dn);
02128 multGxd.addStatement( dn += Gx1 * dp );
02129
02130 moveGxT.setup("moveGxT", Gx1, Gx2);
02131 moveGxT.addStatement( Gx2 == Gx1 );
02132
02133 multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
02134 multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
02135
02136 multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
02137 multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
02138
02139 moveGuE.setup("moveGuE", Gu1, Gu2);
02140 moveGuE.addStatement( Gu2 == Gu1 );
02141
02142 unsigned offset = (performFullCondensing() == true) ? 0 : NX;
02143
02144
02145 setBlockH11.setup("setBlockH11", iRow, iCol, Gu1, Gu2);
02146 setBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) += (Gu1 ^ Gu2) );
02147
02148 DMatrix mRegH11 = eye<double>( getNU() );
02149 mRegH11 *= levenbergMarquardt;
02150
02151 setBlockH11_R1.setup("setBlockH11_R1", iRow, iCol, R11);
02152 setBlockH11_R1.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == R11 + mRegH11 );
02153
02154 zeroBlockH11.setup("zeroBlockH11", iRow, iCol);
02155 zeroBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == zeros<double>(NU, NU) );
02156
02157 copyHTH.setup("copyHTH", iRow, iCol);
02158 copyHTH.addStatement(
02159 H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
02160 H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
02161 );
02162
02163 multRDy.setup("multRDy", R22, Dy1, RDy1);
02164 multRDy.addStatement( RDy1 == R22 * Dy1 );
02165
02166 multQDy.setup("multQDy", Q22, Dy1, QDy1);
02167 multQDy.addStatement( QDy1 == Q22 * Dy1 );
02168
02169 multEQDy.setup("multEQDy", E1, QDy1, U1);
02170 multEQDy.addStatement( U1 += (E1 ^ QDy1) );
02171
02172 multQETGx.setup("multQETGx", E1, Gx1, H101);
02173 multQETGx.addStatement( H101 += (E1 ^ Gx1) );
02174
02175 zeroBlockH10.setup("zeroBlockH10", H101);
02176 zeroBlockH10.addStatement( H101 == zeros<double>(NU, NX) );
02177
02178
02179
02180
02181 multEDu.setup("multEDu", E1, U1, dn);
02182 multEDu.addStatement( dn += E1 * U1 );
02183
02184
02185 if (Q1.isGiven() == true)
02186 {
02187
02188 multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
02189 multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
02190
02191
02192 multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
02193 multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
02194
02195
02196 multQ1d.setup("multQ1d", Q1, dp, dn);
02197 multQ1d.addStatement( dn == Q1 * dp );
02198 }
02199 else
02200 {
02201
02202 multQ1d.setup("multQ1d", Gx1, dp, dn);
02203 multQ1d.addStatement( dn == Gx1 * dp );
02204 }
02205
02206 if (QN1.isGiven() == BT_TRUE)
02207 {
02208
02209 multQN1Gu.setup("multQN1Gu", Gu1, Gu2);
02210 multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
02211
02212
02213 multQN1Gx.setup("multQN1Gx", Gx1, Gx2);
02214 multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
02215 }
02216
02217 if (performsSingleShooting() == BT_FALSE)
02218 {
02219
02220 multQN1d.setup("multQN1d", QN1, dp, dn);
02221 multQN1d.addStatement( dn == QN1 * dp );
02222 }
02223
02224 if (performFullCondensing() == BT_FALSE)
02225 {
02226
02227 zeroBlockH00.setup( "zeroBlockH00" );
02228 zeroBlockH00.addStatement( H00 == zeros<double>(NX, NX) );
02229
02230
02231 multCTQC.setup("multCTQC", Gx1, Gx2);
02232 multCTQC.addStatement( H00 += (Gx1 ^ Gx2) );
02233 }
02234
02235 return SUCCESSFUL_RETURN;
02236 }
02237
02238 returnValue ExportGaussNewtonCondensed::setupEvaluation( )
02239 {
02241
02242
02243
02245
02246 preparation.setup( "preparationStep" );
02247 preparation.doc( "Preparation step of the RTI scheme." );
02248 ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
02249 retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
02250 preparation.setReturnValue(retSim, false);
02251
02252 preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
02253
02254 preparation.addFunctionCall( evaluateObjective );
02255 preparation.addFunctionCall( condensePrep );
02256
02258
02259
02260
02262
02263 ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
02264 tmp.setDoc( "Status code of the qpOASES QP solver." );
02265
02266 ExportFunction solve("solve");
02267 solve.setReturnValue( tmp );
02268
02269 feedback.setup("feedbackStep");
02270 feedback.doc( "Feedback/estimation step of the RTI scheme." );
02271 feedback.setReturnValue( tmp );
02272
02273 feedback.addFunctionCall( condenseFdb );
02274 feedback.addLinebreak();
02275
02276 feedback << tmp.getName() << " = " << solve.getName() << "( );\n";
02277 feedback.addLinebreak();
02278
02279 feedback.addFunctionCall( expand );
02280
02281 int covCalc;
02282 get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
02283 if (covCalc)
02284 feedback.addFunctionCall( calculateCovariance );
02285
02287
02288
02289
02291
02292 ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
02293 ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
02294 ExportIndex index( "index" );
02295
02296 getKKT.setup( "getKKT" );
02297 getKKT.doc( "Get the KKT tolerance of the current iterate." );
02298 kkt.setDoc( "The KKT tolerance value." );
02299 getKKT.setReturnValue( kkt );
02300 getKKT.addVariable( prd );
02301 getKKT.addIndex( index );
02302
02303
02304 getKKT.addStatement( kkt == (g ^ xVars) );
02305 getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
02306
02307 ExportForLoop bLoop(index, 0, getNumQPvars());
02308
02309 bLoop.addStatement( prd == yVars.getRow( index ) );
02310 bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
02311 bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
02312 bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
02313 bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
02314 getKKT.addStatement( bLoop );
02315
02316 if ((getNumStateBounds() + getNumComplexConstraints())> 0)
02317 {
02318 ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
02319
02320 cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ) );
02321 cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
02322 cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
02323 cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
02324 cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
02325
02326 getKKT.addStatement( cLoop );
02327 }
02328
02329 return SUCCESSFUL_RETURN;
02330 }
02331
02332 returnValue ExportGaussNewtonCondensed::setupQPInterface( )
02333 {
02334 string folderName;
02335 get(CG_EXPORT_FOLDER_NAME, folderName);
02336 string moduleName;
02337 get(CG_MODULE_NAME, moduleName);
02338 std::string sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
02339 std::string headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
02340
02341 int useSinglePrecision;
02342 get(USE_SINGLE_PRECISION, useSinglePrecision);
02343
02344 int hotstartQP;
02345 get(HOTSTART_QP, hotstartQP);
02346
02347 int covCalc;
02348 get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
02349
02350 int maxNumQPiterations;
02351 get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
02352
02353 int externalCholesky;
02354 get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
02355
02356
02357
02358
02359
02360 ExportQpOasesInterface qpInterface = ExportQpOasesInterface(headerFile, sourceFile, "");
02361
02362 qpInterface.configure(
02363 "",
02364 "QPOASES_HEADER",
02365 getNumQPvars(),
02366 getNumStateBounds() + getNumComplexConstraints(),
02367 maxNumQPiterations,
02368 "PL_NONE",
02369 useSinglePrecision,
02370
02371 commonHeaderName,
02372 "",
02373 xVars.getFullName(),
02374 yVars.getFullName(),
02375 sigma.getFullName(),
02376 hotstartQP,
02377 (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL,
02378 H.getFullName(),
02379 R.getFullName(),
02380 g.getFullName(),
02381 A.getFullName(),
02382 lb.getFullName(),
02383 ub.getFullName(),
02384 lbA.getFullName(),
02385 ubA.getFullName()
02386 );
02387
02388 return qpInterface.exportCode();
02389 }
02390
02391 bool ExportGaussNewtonCondensed::performFullCondensing() const
02392 {
02393 int sparseQPsolution;
02394 get(SPARSE_QP_SOLUTION, sparseQPsolution);
02395
02396 return (SparseQPsolutionMethods)sparseQPsolution == CONDENSING ? false : true;
02397 }
02398
02399 CLOSE_NAMESPACE_ACADO