condensing_based_cp_solver.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00033 #include <acado/conic_solver/condensing_based_cp_solver.hpp>
00034 #include <acado/bindings/acado_qpoases/qp_solver_qpoases.hpp>
00035 
00036 using namespace Eigen;
00037 using namespace std;
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 CondensingBasedCPsolver::CondensingBasedCPsolver( ) : BandedCPsolver( )
00047 {
00048         nConstraints = 0;
00049     blockDims = 0;
00050 
00051         condensingStatus = COS_NOT_INITIALIZED;
00052 
00053     cpSolver = 0;
00054     cpSolverRelaxed = 0;
00055 }
00056 
00057 
00058 CondensingBasedCPsolver::CondensingBasedCPsolver(       UserInteraction* _userInteraction,
00059                                                                                                         uint nConstraints_,
00060                                                                                                 const DVector& blockDims_
00061                                                                                                 ) : BandedCPsolver( _userInteraction )
00062 {
00063         nConstraints = nConstraints_;
00064     blockDims = blockDims_;
00065 
00066         condensingStatus = COS_NOT_INITIALIZED;
00067 
00068     cpSolver = new QPsolver_qpOASES( _userInteraction );
00069     cpSolverRelaxed = new QPsolver_qpOASES( _userInteraction );
00070 }
00071 
00072 
00073 CondensingBasedCPsolver::CondensingBasedCPsolver( const CondensingBasedCPsolver& rhs )
00074                         :BandedCPsolver( rhs )
00075 {
00076         nConstraints = rhs.nConstraints;
00077     blockDims = rhs.blockDims;
00078         
00079         condensingStatus = rhs.condensingStatus;
00080 
00081     if( rhs.cpSolver != 0 ) cpSolver = rhs.cpSolver->clone();
00082     else                    cpSolver = 0                    ;
00083 
00084     if( rhs.cpSolverRelaxed != 0 ) cpSolverRelaxed = rhs.cpSolverRelaxed->cloneDenseQPsolver();
00085     else                           cpSolverRelaxed = 0;
00086 
00087     denseCP = rhs.denseCP;
00088         
00089         deltaX = rhs.deltaX;
00090         deltaP = rhs.deltaP;
00091 }
00092 
00093 
00094 CondensingBasedCPsolver::~CondensingBasedCPsolver( ){
00095 
00096     if( cpSolver != 0 ) delete cpSolver ;
00097     if( cpSolverRelaxed != 0 ) delete cpSolverRelaxed;
00098 }
00099 
00100 
00101 CondensingBasedCPsolver& CondensingBasedCPsolver::operator=( const CondensingBasedCPsolver& rhs ){
00102 
00103     if ( this != &rhs ){
00104 
00105         if( cpSolver != 0 ) delete cpSolver ;
00106         if( cpSolverRelaxed != 0 ) delete cpSolverRelaxed;
00107 
00108         BandedCPsolver::operator=( rhs );
00109 
00110                 nConstraints = rhs.nConstraints;
00111                 blockDims = rhs.blockDims;
00112 
00113                 condensingStatus = rhs.condensingStatus;
00114 
00115         if( rhs.cpSolver != 0 ) cpSolver = rhs.cpSolver->clone();
00116         else                    cpSolver = 0                    ;
00117 
00118         if( rhs.cpSolverRelaxed != 0 ) cpSolverRelaxed = rhs.cpSolverRelaxed->cloneDenseQPsolver();
00119         else                           cpSolverRelaxed = 0;
00120 
00121         denseCP = rhs.denseCP;
00122 
00123                 deltaX = rhs.deltaX;
00124                 deltaP = rhs.deltaP;
00125     }
00126     return *this;
00127 }
00128 
00129 
00130 BandedCPsolver* CondensingBasedCPsolver::clone() const
00131 {
00132      return new CondensingBasedCPsolver(*this);
00133 }
00134 
00135 
00136 
00137 returnValue CondensingBasedCPsolver::init(      const OCPiterate &iter_
00138                                                                                         )
00139 {
00140     iter = iter_;
00141 
00142     // INITIALIZE THE CONDENSING MATRICES:
00143     // -----------------------------------
00144     if ( initializeCondensingOperator( ) != SUCCESSFUL_RETURN )
00145                 return ACADOERROR( RET_BANDED_CP_INIT_FAILED );
00146 
00147 
00148     // INITIALIZE THE CP SOLVER:
00149     // -------------------------
00150         int infeasibleQPhandling;
00151         get( INFEASIBLE_QP_HANDLING,infeasibleQPhandling );
00152         
00153         if ( initializeCPsolver( (InfeasibleQPhandling)infeasibleQPhandling ) != SUCCESSFUL_RETURN )
00154                 return ACADOERROR( RET_BANDED_CP_INIT_FAILED );
00155 
00156 
00157     return SUCCESSFUL_RETURN;
00158 }
00159 
00160 
00161 
00162 returnValue CondensingBasedCPsolver::prepareSolve(      BandedCP& cp
00163                                                                                                         )
00164 {
00165         RealClock clock;
00166 
00167     // CONDENSE THE KKT-SYSTEM:
00168     // ------------------------
00169 
00170         int printLevel;
00171         get( PRINTLEVEL,printLevel );
00172 
00173         if ( (PrintLevel)printLevel >= HIGH ) 
00174                 cout << "--> Condesing banded QP ...\n";
00175 
00176         clock.reset( );
00177         clock.start( );
00178 
00179     returnValue returnvalue = condense( cp );
00180     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00181 
00182         clock.stop( );
00183         setLast( LOG_TIME_CONDENSING,clock.getTime() );
00184 
00185         if ( (PrintLevel)printLevel >= HIGH ) 
00186                 cout << "<-- Condesing banded QP done.\n";
00187 
00188         return SUCCESSFUL_RETURN;
00189 }
00190 
00191 
00192 returnValue CondensingBasedCPsolver::solve(     BandedCP& cp
00193                                                                                         )
00194 {
00195         if ( areRealTimeParametersDefined( ) == BT_FALSE )
00196                 prepareSolve( cp );
00197 
00198 
00199     // ADD THE FEEDBACK DATA TO THE DENSE QP (IF SPECIFIED):
00200     // -----------------------------------------------------
00201 
00202         if ( deltaX.isEmpty( ) == BT_FALSE )
00203         {
00204                 for( uint run1 = 0; run1 < getNX(); run1++ )
00205                 {
00206                         denseCP.lb(run1) = deltaX(run1);
00207                         denseCP.ub(run1) = deltaX(run1);
00208                 }
00209         }
00210 
00211         if ( deltaP.isEmpty( ) == BT_FALSE )
00212         {
00213                 for( uint run1 = 0; run1 < getNP(); run1++ )
00214                 {
00215                         denseCP.lb(getNX()+getNumPoints()*getNXA()+run1) = deltaP(run1);
00216                         denseCP.ub(getNX()+getNumPoints()*getNXA()+run1) = deltaP(run1);
00217                 }
00218         }
00219         
00220 //      cp.deltaX.print();
00221 
00222     // Solve QP subproblem
00223     // ------------------------------------
00224         int printLevel;
00225         get( PRINTLEVEL,printLevel );
00226 
00227         if ( (PrintLevel)printLevel >= HIGH ) 
00228                 cout << "--> Solving condesed QP ...\n";
00229         
00230         
00231         
00232     returnValue returnvalue = solveCPsubproblem( );
00233     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR( RET_BANDED_CP_SOLUTION_FAILED );
00234 
00235         if ( (PrintLevel)printLevel >= HIGH ) 
00236                 cout << "<-- Solving condesed QP done.\n";
00237 
00238     // Expand the KKT-System if neccessary:
00239     // ------------------------------------
00240 
00241         if ( areRealTimeParametersDefined( ) == BT_FALSE )
00242                 return finalizeSolve( cp );
00243         else
00244                 return SUCCESSFUL_RETURN;
00245 }
00246 
00247 
00248 returnValue CondensingBasedCPsolver::finalizeSolve(     BandedCP& cp
00249                                                                                                         )
00250 {
00251         RealClock clock;
00252 
00253         int printLevel;
00254         get( PRINTLEVEL,printLevel );
00255 
00256         if ( (PrintLevel)printLevel >= HIGH ) 
00257                 cout << "--> Expanding condensed QP solution ...\n";
00258 
00259     // Expand the KKT-System if neccessary:
00260     // ------------------------------------
00261         clock.reset( );
00262         clock.start( );
00263 
00264     returnValue returnvalue = expand( cp );
00265     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00266 
00267         clock.stop( );
00268         setLast( LOG_TIME_EXPAND,clock.getTime() );
00269         
00270         if ( (PrintLevel)printLevel >= HIGH ) 
00271                 cout << "<-- Expanding condensed QP solution done.\n";
00272 
00273     return returnvalue;
00274 }
00275 
00276 
00277 
00278 returnValue CondensingBasedCPsolver::getParameters( DVector &p_  ) const
00279 {
00280         if ( p_.getDim( ) != getNP( ) )
00281                 return ACADOERROR( RET_INCOMPATIBLE_DIMENSIONS );
00282 
00283         uint startIdx = getNX() + getNumPoints()*getNXA();
00284         
00285         for( uint i=0; i<getNP(); ++i )
00286                 p_( i ) = (*denseCP.x)( startIdx+i );
00287         
00288         return SUCCESSFUL_RETURN;
00289 }
00290 
00291 
00292 returnValue CondensingBasedCPsolver::getFirstControl( DVector &u0_ ) const
00293 {
00294         if ( u0_.getDim( ) != getNU( ) )
00295                 return ACADOERROR( RET_INCOMPATIBLE_DIMENSIONS );
00296 
00297         uint startIdx = getNX() + getNumPoints()*getNXA() + getNP();
00298         
00299         for( uint i=0; i<getNU(); ++i )
00300                 u0_( i ) = (*denseCP.x)( startIdx+i );
00301         
00302         return SUCCESSFUL_RETURN;
00303 }
00304 
00305 
00306 
00307 returnValue CondensingBasedCPsolver::getVarianceCovariance( DMatrix &var )
00308 {
00309         if ( cpSolver == 0 )
00310                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00311                 
00312         return cpSolver->getVarianceCovariance( denseCP.H,var );
00313 }
00314 
00315 
00316 
00317 returnValue CondensingBasedCPsolver::setRealTimeParameters(     const DVector& DeltaX,
00318                                                                                                                         const DVector& DeltaP
00319                                                                                                                         )
00320 {
00321         deltaX = DeltaX;
00322         deltaP = DeltaP;
00323         
00324         return SUCCESSFUL_RETURN;
00325 }
00326 
00327 
00328 
00329 returnValue CondensingBasedCPsolver::freezeCondensing( )
00330 {
00331         if ( ( condensingStatus != COS_CONDENSED ) && ( condensingStatus != COS_FROZEN ) )
00332                 return ACADOERROR( RET_NEED_TO_CONDENSE_FIRST );
00333 
00334         condensingStatus = COS_FROZEN;
00335 
00336         return SUCCESSFUL_RETURN;
00337 }
00338 
00339 
00340 returnValue CondensingBasedCPsolver::unfreezeCondensing( )
00341 {
00342         if ( condensingStatus == COS_FROZEN )
00343                 condensingStatus = COS_CONDENSED;
00344 
00345         return SUCCESSFUL_RETURN;
00346 }
00347 
00348 
00349 
00350 //
00351 // PROTECTED MEMBER FUNCTIONS:
00352 //
00353 
00354 returnValue CondensingBasedCPsolver::projectHessian( DMatrix &H_, double dampingFactor ){
00355 
00356     if( dampingFactor < 0.0 ) return SUCCESSFUL_RETURN;
00357 
00358     // COMPUTE THE EIGENVALUES OF THE HESSIAN:
00359     // ---------------------------------------
00360 
00361     SelfAdjointEigenSolver< MatrixXd > es( H_ );
00362     MatrixXd V = es.eigenvectors();
00363     VectorXd D = es.eigenvalues();
00364 
00365     // OVER-PROJECT THE EIGENVALUES BASED ON THE DAMPING TECHNIQUE:
00366     // ------------------------------------------------------------
00367 
00368         for (unsigned el = 0; el < D.size(); el++)
00369                 if (D( el ) <= 0.1 * dampingFactor)
00370                 {
00371                         if (fabs(D( el )) >= dampingFactor)
00372                                 D( el ) = fabs(D( el ));
00373                         else
00374                                 D( el ) = dampingFactor;
00375                 }
00376 
00377     // RECONSTRUCT THE PROJECTED HESSIAN MATRIX:
00378     // -----------------------------------------
00379 
00380     H_ = V * D.asDiagonal() * V.inverse();
00381 
00382     return SUCCESSFUL_RETURN;
00383 }
00384 
00385 
00386 
00387 returnValue CondensingBasedCPsolver::solveCPsubproblem( )
00388 {
00389         if( denseCP.isQP() == BT_FALSE )
00390                 return ACADOERROR( RET_QP_SOLVER_CAN_ONLY_SOLVE_QP );
00391 
00392     returnValue returnvalue;
00393 
00394         // ensure that Hessian matrix is symmetric
00395         if ( denseCP.H.isSymmetric( ) == BT_FALSE )
00396         {
00397                 ACADOINFO( RET_NONSYMMETRIC_HESSIAN_MATRIX );
00398                 denseCP.H.symmetrize();
00399         }
00400 
00401 
00402     // PROJECT HESSIAN TO POSITIVE DEFINITE CONE IF NECESSARY:
00403     // -------------------------------------------------------
00404         int hessianMode;
00405         get( HESSIAN_APPROXIMATION,hessianMode );
00406 
00407         if ( (HessianApproximationMode)hessianMode == EXACT_HESSIAN )
00408         {
00409                 double hessianProjectionFactor;
00410                 get( HESSIAN_PROJECTION_FACTOR, hessianProjectionFactor );
00411                 projectHessian( denseCP.H, hessianProjectionFactor );
00412         }
00413 
00414     // APPLY LEVENBERG-MARQUARD REGULARISATION IF DESIRED:
00415     // -------------------------------------------------------
00416     double levenbergMarquard;
00417     get(LEVENBERG_MARQUARDT, levenbergMarquard );
00418 
00419     if( levenbergMarquard > EPS )
00420         denseCP.H += eye<double>( denseCP.H.rows() ) * levenbergMarquard;
00421 
00422         // Check condition number of the condensed Hessian.
00423     double denseHConditionNumber = denseCP.H.getConditionNumber();
00424     if (denseHConditionNumber > 1.0e16)
00425     {
00426         LOG( LVL_WARNING )
00427                         << "Condition number of the condensed Hessian is quite high: log_10(kappa( H )) = "
00428                         << log10( denseHConditionNumber ) << endl;
00429         ACADOWARNING( RET_ILLFORMED_HESSIAN_MATRIX );
00430     }
00431     // Check for max and min entry in the condensed Hessian:
00432     if (denseCP.H.getMin() < -1.0e16 || denseCP.H.getMax() > 1.0e16)
00433     {
00434         LOG( LVL_WARNING ) << "Ill formed condensed Hessian: min(.) < -1e16 or max(.) > 1e16" << endl;
00435         ACADOWARNING( RET_ILLFORMED_HESSIAN_MATRIX );
00436     }
00437 
00438     // SOLVE QP ALLOWING THE GIVEN NUMBER OF ITERATIONS:
00439     // -------------------------------------------------------
00440     int maxQPiter;
00441     get( MAX_NUM_QP_ITERATIONS, maxQPiter );
00442 
00443 
00444         RealClock clock;
00445         clock.start( );
00446 
00447 //      denseCP.print( "",PS_MATLAB );
00448         returnvalue = solveQP( maxQPiter );
00449 
00450         clock.stop( );
00451         setLast( LOG_TIME_QP,clock.getTime() );
00452 
00453         switch( returnvalue )
00454         {
00455                 case SUCCESSFUL_RETURN:
00456                         setLast( LOG_TIME_RELAXED_QP,0.0 );
00457                         setLast( LOG_IS_QP_RELAXED, BT_FALSE );
00458                         break;
00459 
00460                 case RET_QP_SOLUTION_REACHED_LIMIT:
00461                         setLast( LOG_TIME_RELAXED_QP,0.0 );
00462                         setLast( LOG_IS_QP_RELAXED, BT_FALSE );
00463                         ACADOWARNING( RET_QP_SOLUTION_REACHED_LIMIT );
00464                         break;
00465 
00466                 case RET_QP_UNBOUNDED:
00467                         setLast( LOG_TIME_RELAXED_QP,0.0 );
00468                         setLast( LOG_IS_QP_RELAXED, BT_FALSE );
00469                         return ACADOERROR( RET_QP_UNBOUNDED );
00470                         break;
00471 
00472                 default: //case: RET_QP_INFEASIBLE:
00473                         int infeasibleQPhandling;
00474                         get( INFEASIBLE_QP_HANDLING,infeasibleQPhandling );
00475 
00476                         switch( (InfeasibleQPhandling) infeasibleQPhandling )
00477                         {
00478 
00479                                 case IQH_STOP:
00480                                         return ACADOERROR( RET_QP_INFEASIBLE );
00481 
00482                                 case IQH_IGNORE:
00483                                         setLast( LOG_TIME_RELAXED_QP,0.0 );
00484                                         setLast( LOG_IS_QP_RELAXED, BT_FALSE );
00485                                         break;
00486 
00487                                 default:
00488                                         clock.reset( );
00489                                         clock.start( );
00490 
00491                                         ACADOWARNING( RET_RELAXING_QP );
00492                                         
00493                     if ( solveQP(       maxQPiter - cpSolver->getNumberOfIterations( ),
00494                                                                         (InfeasibleQPhandling) infeasibleQPhandling
00495                                                                         ) != SUCCESSFUL_RETURN )
00496                                                 return ACADOERROR( RET_QP_SOLUTION_FAILED );
00497 
00498                                         clock.stop( );
00499                                         setLast( LOG_TIME_RELAXED_QP,clock.getTime() );
00500                                         setLast( LOG_IS_QP_RELAXED, BT_TRUE );
00501                         }
00502                         break;
00503         }
00504 
00505     return SUCCESSFUL_RETURN;
00506 }
00507 
00508 
00509 
00510 returnValue CondensingBasedCPsolver::condense(  BandedCP& cp
00511                                                                                                 )
00512 {
00513         if ( ( condensingStatus != COS_INITIALIZED ) && ( condensingStatus != COS_FROZEN ) )
00514                 return ACADOERROR( RET_UNABLE_TO_CONDENSE );
00515 
00516         uint run1, run2, run3;
00517         
00518     uint nF = getNF();
00519     uint nA = getNA();
00520     uint N = getNumPoints();
00521 
00522 
00523         if( getNX() + getNXA() > 0 )
00524         {
00525                 if ( computeCondensingOperator( cp ) != SUCCESSFUL_RETURN )
00526                         return ACADOERROR( RET_UNABLE_TO_CONDENSE );
00527 
00528                 int rowOffset  = 0;
00529                 uint rowOffset1 = 0;
00530 
00531                 if ( condensingStatus != COS_FROZEN )
00532                 {
00533                         // generate H
00534                         hT       = cp.hessian*T;
00535                         HDense   = T^hT;
00536 
00537                         if( getNX() != 0 ) generateHessianBlockLine( getNX(), rowOffset, rowOffset1 );
00538                         rowOffset++;
00539 
00540                         for( run3 = 0; run3 < N; run3++ ){
00541                                 if( getNXA() != 0 ) generateHessianBlockLine( getNXA(), rowOffset, rowOffset1 );
00542                                 rowOffset++;
00543                         }
00544 
00545                         if( getNP() != 0 ) generateHessianBlockLine( getNP(), rowOffset, rowOffset1 );
00546                         rowOffset++;
00547 
00548                         for( run3 = 0; run3 < N-1; run3++ ){
00549                                 if( getNU() != 0 ) generateHessianBlockLine( getNU(), rowOffset, rowOffset1 );
00550                                 rowOffset++;
00551                         }
00552 
00553                         for( run3 = 0; run3 < N-1; run3++ ){
00554                                 if( getNW() != 0 ) generateHessianBlockLine( getNW(), rowOffset, rowOffset1 );
00555                                 rowOffset++;
00556                         }
00557 
00558 
00559                         // generate A
00560                         ADense   = cp.constraintGradient*T;
00561 
00562                         denseCP.A.setZero();
00563 
00564                         rowOffset1 = 0;
00565                         for( run3 = 0; run3 < ADense.getNumRows(); run3++ ){
00566                                 ASSERT( getNC() != 0 );
00567                                 generateConstraintBlockLine( (int) blockDims(run3), run3, rowOffset1 );
00568                                 rowOffset++;
00569                         }
00570 
00571                         for( run3 = 1; run3 < N; run3++ ){
00572                                 generateStateBoundBlockLine( getNX(), run3, rowOffset1 );
00573                         }
00574                 }
00575 
00576 
00577                 // generate g
00578                 gDense   = (cp.objectiveGradient*T) + (d^hT);
00579                 
00580         generateObjectiveGradient( );
00581 
00582 
00583                 // generate lb, ub
00584         BlockMatrix dCut(4*N+1,1);
00585         DMatrix tmp;
00586         for( run1 = 0; run1 < N; run1++ ){
00587             d.getSubBlock(run1,0,tmp);
00588             if( tmp.getDim() != 0 )
00589                 dCut.setDense(run1,0,tmp);
00590         }
00591 
00592         lbDense  = cp.lowerBoundResiduum - dCut;
00593         ubDense  = cp.upperBoundResiduum - dCut;
00594 
00595         generateBoundVectors( );
00596 
00597 
00598                 // generate lbA, ubA
00599                 lbADense = cp.lowerConstraintResiduum - cp.constraintGradient*d;
00600                 ubADense = cp.upperConstraintResiduum - cp.constraintGradient*d;
00601 
00602         rowOffset1 = 0;
00603         for( run3 = 0; run3 < ADense.getNumRows(); run3++ ){
00604             ASSERT( getNC() != 0 );
00605             generateConstraintVectors( (int) blockDims(run3), run3, rowOffset1 );
00606             rowOffset++;
00607         }
00608 
00609         for( run3 = 1; run3 < N; run3++ ){
00610             generateStateBoundVectors( getNX(), run3, rowOffset1 );
00611         }
00612 
00613 
00614 //         acadoPrintf("denseCP.H = \n");
00615 //         denseCP.H.print();
00616 //
00617 //         acadoPrintf("denseCP.A = \n");
00618 //         denseCP.A.print();
00619 //
00620 //         acadoPrintf("denseCP.g = \n");
00621 //         denseCP.g.print();
00622 //
00623 //         acadoPrintf("denseCP.lbA = \n");
00624 //         denseCP.lbA.print();
00625 //
00626 //         acadoPrintf("denseCP.ubA = \n");
00627 //         denseCP.ubA.print();
00628 //
00629 //         acadoPrintf("denseCP.lb = \n");
00630 //         denseCP.lb.print();
00631 //
00632 //         acadoPrintf("denseCP.ub = \n");
00633 //         denseCP.ub.print();
00634     }
00635     else{
00636 
00637         ASSERT( N  == 1 );
00638         ASSERT( getNU() == 0 );
00639         ASSERT( getNW() == 0 );
00640 
00641         denseCP.H.init( nF, nF );
00642         denseCP.g.init( nF );
00643 
00644         denseCP.A.init( nA, nF );
00645         denseCP.lbA.init( nA );
00646         denseCP.ubA.init( nA );
00647 
00648         denseCP.lb.init( nF );
00649         denseCP.ub.init( nF );
00650 
00651         DMatrix tmp;
00652 
00653         cp.hessian           .getSubBlock( 2, 2, denseCP.H  , getNP(), getNP() );
00654         cp.objectiveGradient .getSubBlock( 0, 2, tmp, 1 , getNP() );
00655         for( run1 = 0; run1 < getNP(); run1++ )
00656             denseCP.g(run1) = tmp(0,run1);
00657 
00658         uint offset = 0;
00659         for( run2 = 0; run2 < cp.constraintGradient.getNumRows(); run2++ ){
00660 
00661             cp.constraintGradient.getSubBlock( run2, 2, tmp );
00662 
00663             for( run1 = 0; run1 < tmp.getNumRows(); run1++ )
00664                 for( run3 = 0; run3 < tmp.getNumCols(); run3++ )
00665                     denseCP.A(offset + run1,run3) = tmp(run1,run3);
00666 
00667             cp.lowerConstraintResiduum.getSubBlock( run2, 0, tmp );
00668             for( run1 = 0; run1 < tmp.getNumRows(); run1++ )
00669                 denseCP.lbA(offset + run1) = tmp(run1,0);
00670 
00671             cp.upperConstraintResiduum.getSubBlock( run2, 0, tmp );
00672             for( run1 = 0; run1 < tmp.getNumRows(); run1++ )
00673                 denseCP.ubA(offset + run1) = tmp(run1,0);
00674 
00675             offset += tmp.getNumRows();
00676         }
00677 
00678         cp.lowerBoundResiduum.getSubBlock( 2, 0, tmp, getNP(), 1 );
00679         for( run1 = 0; run1 < getNP(); run1++ )
00680             denseCP.lb(run1) = tmp(run1,0);
00681         cp.upperBoundResiduum.getSubBlock( 2, 0, tmp, getNP(), 1 );
00682         for( run1 = 0; run1 < getNP(); run1++ )
00683             denseCP.ub(run1) = tmp(run1,0);
00684     }
00685 
00686         if ( condensingStatus != COS_FROZEN )
00687                 condensingStatus = COS_CONDENSED;
00688 
00689     return SUCCESSFUL_RETURN;
00690 }
00691 
00692 
00693 
00694 returnValue CondensingBasedCPsolver::generateHessianBlockLine( uint nn, uint rowOffset, uint& rowOffset1 ){
00695 
00696     uint run1, run2, run3;
00697 
00698     uint  colOffset  = 0;
00699     uint  colOffset1 = 0;
00700 
00701     uint N = getNumPoints();
00702 
00703     DMatrix tmp;
00704 
00705     if( getNX() != 0 ){
00706         HDense.getSubBlock( rowOffset, 0, tmp, nn, getNX() );
00707 
00708         for( run1 = 0; run1 < nn; run1++ )
00709             for( run2 = 0; run2 < getNX(); run2++ )
00710                 denseCP.H(rowOffset1+run1,run2) = tmp(run1,run2);
00711         colOffset1 += getNX();
00712     }
00713     colOffset++;
00714 
00715     for( run3 = 0; run3 < N; run3++ ){
00716          if( getNXA() != 0 ){
00717             HDense.getSubBlock( rowOffset, colOffset, tmp, nn, getNXA() );
00718             for( run1 = 0; run1 < nn; run1++ )
00719                 for( run2 = 0; run2 < getNXA(); run2++ )
00720                     denseCP.H(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00721             colOffset1 += getNXA();
00722         }
00723         colOffset  ++;
00724     }
00725 
00726     if( getNP() != 0 ){
00727         HDense.getSubBlock( rowOffset, colOffset, tmp, nn, getNP() );
00728         for( run1 = 0; run1 < nn; run1++ )
00729             for( run2 = 0; run2 < getNP(); run2++ )
00730                 denseCP.H(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00731          colOffset1 += getNP();
00732      }
00733      colOffset  ++;
00734 
00735      for( run3 = 0; run3 < N-1; run3++ ){
00736           if( getNU() != 0 ){
00737              HDense.getSubBlock( rowOffset, colOffset, tmp, nn, getNU() );
00738              for( run1 = 0; run1 < nn; run1++ )
00739                  for( run2 = 0; run2 < getNU(); run2++ )
00740                      denseCP.H(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00741              colOffset1 += getNU();
00742          }
00743          colOffset  ++;
00744      }
00745 
00746      for( run3 = 0; run3 < N-1; run3++ ){
00747          if( getNW() != 0 ){
00748               HDense.getSubBlock( rowOffset, colOffset, tmp, nn, getNW() );
00749               for( run1 = 0; run1 < nn; run1++ )
00750                   for( run2 = 0; run2 < getNW(); run2++ )
00751                       denseCP.H(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00752               colOffset1 += getNW();
00753          }
00754          colOffset  ++;
00755      }
00756      rowOffset1 += nn;
00757 
00758      return SUCCESSFUL_RETURN;
00759 }
00760 
00761 
00762 returnValue CondensingBasedCPsolver::generateConstraintBlockLine( uint nn, uint rowOffset, uint& rowOffset1 ){
00763 
00764 
00765     uint run1, run2, run3;
00766 
00767     uint  colOffset  = 0;
00768     uint  colOffset1 = 0;
00769 
00770     uint N = getNumPoints();
00771 
00772     DMatrix tmp;
00773 
00774     if( getNX() != 0 ){
00775         ADense.getSubBlock( rowOffset, 0, tmp, nn, getNX() );
00776         for( run1 = 0; run1 < nn; run1++ )
00777             for( run2 = 0; run2 < getNX(); run2++ )
00778                 denseCP.A(rowOffset1+run1,run2) = tmp(run1,run2);
00779         colOffset1 += getNX();
00780     }
00781     colOffset++;
00782 
00783     for( run3 = 0; run3 < N; run3++ ){
00784          if( getNXA() != 0 ){
00785             ADense.getSubBlock( rowOffset, colOffset, tmp, nn, getNXA() );
00786             for( run1 = 0; run1 < nn; run1++ )
00787                 for( run2 = 0; run2 < getNXA(); run2++ )
00788                     denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00789             colOffset1 += getNXA();
00790         }
00791         colOffset  ++;
00792     }
00793 
00794     if( getNP() != 0 ){
00795         ADense.getSubBlock( rowOffset, colOffset, tmp, nn, getNP() );
00796         for( run1 = 0; run1 < nn; run1++ )
00797             for( run2 = 0; run2 < getNP(); run2++ )
00798                 denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00799          colOffset1 += getNP();
00800      }
00801      colOffset  ++;
00802 
00803      for( run3 = 0; run3 < N-1; run3++ ){
00804           if( getNU() != 0 ){
00805              ADense.getSubBlock( rowOffset, colOffset, tmp, nn, getNU() );
00806              for( run1 = 0; run1 < nn; run1++ )
00807                  for( run2 = 0; run2 < getNU(); run2++ )
00808                      denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00809              colOffset1 += getNU();
00810          }
00811          colOffset  ++;
00812      }
00813 
00814      for( run3 = 0; run3 < N-1; run3++ ){
00815          if( getNW() != 0 ){
00816               ADense.getSubBlock( rowOffset, colOffset, tmp, nn, getNW() );
00817               for( run1 = 0; run1 < nn; run1++ )
00818                   for( run2 = 0; run2 < getNW(); run2++ )
00819                       denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00820               colOffset1 += getNW();
00821          }
00822          colOffset  ++;
00823      }
00824      rowOffset1 += nn;
00825 
00826      return SUCCESSFUL_RETURN;
00827 }
00828 
00829 
00830 returnValue CondensingBasedCPsolver::generateStateBoundBlockLine( uint nn, uint rowOffset, uint& rowOffset1 ){
00831 
00832 
00833     uint run1, run2, run3;
00834 
00835     uint  colOffset  = 0;
00836     uint  colOffset1 = 0;
00837 
00838     uint N = getNumPoints();
00839 
00840     DMatrix tmp;
00841 
00842     if( getNX() != 0 ){
00843         T.getSubBlock( rowOffset, 0, tmp, nn, getNX() );
00844         for( run1 = 0; run1 < nn; run1++ )
00845             for( run2 = 0; run2 < getNX(); run2++ )
00846                 denseCP.A(rowOffset1+run1,run2) = tmp(run1,run2);
00847         colOffset1 += getNX();
00848     }
00849     colOffset++;
00850 
00851     for( run3 = 0; run3 < N; run3++ ){
00852          if( getNXA() != 0 ){
00853             T.getSubBlock( rowOffset, colOffset, tmp, nn, getNXA() );
00854             for( run1 = 0; run1 < nn; run1++ )
00855                 for( run2 = 0; run2 < getNXA(); run2++ )
00856                     denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00857             colOffset1 += getNXA();
00858         }
00859         colOffset  ++;
00860     }
00861 
00862     if( getNP() != 0 ){
00863         T.getSubBlock( rowOffset, colOffset, tmp, nn, getNP() );
00864         for( run1 = 0; run1 < nn; run1++ )
00865             for( run2 = 0; run2 < getNP(); run2++ )
00866                 denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00867          colOffset1 += getNP();
00868      }
00869      colOffset  ++;
00870 
00871      for( run3 = 0; run3 < N-1; run3++ ){
00872           if( getNU() != 0 ){
00873              T.getSubBlock( rowOffset, colOffset, tmp, nn, getNU() );
00874              for( run1 = 0; run1 < nn; run1++ )
00875                  for( run2 = 0; run2 < getNU(); run2++ )
00876                      denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00877              colOffset1 += getNU();
00878          }
00879          colOffset  ++;
00880      }
00881 
00882      for( run3 = 0; run3 < N-1; run3++ ){
00883          if( getNW() != 0 ){
00884               T.getSubBlock( rowOffset, colOffset, tmp, nn, getNW() );
00885               for( run1 = 0; run1 < nn; run1++ )
00886                   for( run2 = 0; run2 < getNW(); run2++ )
00887                       denseCP.A(rowOffset1+run1,colOffset1+run2) = tmp(run1,run2);
00888               colOffset1 += getNW();
00889          }
00890          colOffset  ++;
00891      }
00892      rowOffset1 += nn;
00893 
00894      return SUCCESSFUL_RETURN;
00895 }
00896 
00897 
00898 returnValue CondensingBasedCPsolver::generateConstraintVectors( uint nn, uint rowOffset, uint& rowOffset1 ){
00899 
00900     uint run1;
00901 
00902     DMatrix tmp;
00903 
00904     lbADense.getSubBlock( rowOffset, 0, tmp, nn, 1 );
00905     for( run1 = 0; run1 < nn; run1++ )
00906         denseCP.lbA(rowOffset1+run1) = tmp(run1,0);
00907 
00908     ubADense.getSubBlock( rowOffset, 0, tmp, nn, 1 );
00909     for( run1 = 0; run1 < nn; run1++ )
00910         denseCP.ubA(rowOffset1+run1) = tmp(run1,0);
00911 
00912     rowOffset1 += nn;
00913 
00914     return SUCCESSFUL_RETURN;
00915 }
00916 
00917 
00918 returnValue CondensingBasedCPsolver::generateStateBoundVectors( uint nn, uint rowOffset, uint& rowOffset1 ){
00919 
00920     uint run1;
00921 
00922     DMatrix tmp;
00923 
00924     lbDense.getSubBlock( rowOffset, 0, tmp, nn, 1 );
00925     for( run1 = 0; run1 < nn; run1++ )
00926         denseCP.lbA(rowOffset1+run1) = tmp(run1,0);
00927 
00928     ubDense.getSubBlock( rowOffset, 0, tmp, nn, 1 );
00929     for( run1 = 0; run1 < nn; run1++ )
00930         denseCP.ubA(rowOffset1+run1) = tmp(run1,0);
00931 
00932     rowOffset1 += nn;
00933 
00934     return SUCCESSFUL_RETURN;
00935 }
00936 
00937 
00938 returnValue CondensingBasedCPsolver::generateBoundVectors( ){
00939 
00940     uint run1, run3;
00941 
00942     uint N = getNumPoints();
00943 
00944     uint  rowOffset  = N;
00945     uint  rowOffset1 = 0;
00946 
00947     DMatrix tmp;
00948 
00949     if( getNX() != 0 ){
00950         lbDense.getSubBlock( 0, 0, tmp, getNX(), 1 );
00951         for( run1 = 0; run1 < getNX(); run1++ )
00952             denseCP.lb(rowOffset1+run1) = tmp(run1,0);
00953         ubDense.getSubBlock( 0, 0, tmp, getNX(), 1 );
00954         for( run1 = 0; run1 < getNX(); run1++ )
00955             denseCP.ub(rowOffset1+run1) = tmp(run1,0);
00956         rowOffset1 += getNX();
00957     }
00958 
00959     for( run3 = 0; run3 < N; run3++ ){
00960          if( getNXA() != 0 ){
00961             lbDense.getSubBlock( rowOffset, 0, tmp, getNXA(), 1 );
00962             for( run1 = 0; run1 < getNXA(); run1++ )
00963                 denseCP.lb(rowOffset1+run1) = tmp(run1,0);
00964             ubDense.getSubBlock( rowOffset, 0, tmp, getNXA(), 1 );
00965             for( run1 = 0; run1 < getNXA(); run1++ )
00966                 denseCP.ub(rowOffset1+run1) = tmp(run1,0);
00967             rowOffset1 += getNXA();
00968         }
00969         rowOffset++;
00970     }
00971 
00972     if( getNP() != 0 ){
00973         lbDense.getSubBlock( rowOffset, 0, tmp, getNP(), 1 );
00974         for( run1 = 0; run1 < getNP(); run1++ )
00975             denseCP.lb(rowOffset1+run1) = tmp(run1,0);
00976         ubDense.getSubBlock( rowOffset, 0, tmp, getNP(), 1 );
00977         for( run1 = 0; run1 < getNP(); run1++ )
00978             denseCP.ub(rowOffset1+run1) = tmp(run1,0);
00979          rowOffset1 += getNP();
00980      }
00981      rowOffset++;
00982 
00983      for( run3 = 0; run3 < N-1; run3++ ){
00984           if( getNU() != 0 ){
00985              lbDense.getSubBlock( rowOffset, 0, tmp, getNU(), 1 );
00986              for( run1 = 0; run1 < getNU(); run1++ )
00987                  denseCP.lb(rowOffset1+run1) = tmp(run1,0);
00988              ubDense.getSubBlock( rowOffset, 0, tmp, getNU(), 1 );
00989              for( run1 = 0; run1 < getNU(); run1++ )
00990                  denseCP.ub(rowOffset1+run1) = tmp(run1,0);
00991              rowOffset1 += getNU();
00992          }
00993          rowOffset++;
00994      }
00995      rowOffset++;
00996 
00997      for( run3 = 0; run3 < N-1; run3++ ){
00998          if( getNW() != 0 ){
00999               lbDense.getSubBlock( rowOffset, 0, tmp, getNW(), 1 );
01000               for( run1 = 0; run1 < getNW(); run1++ )
01001                   denseCP.lb(rowOffset1+run1) = tmp(run1,0);
01002               ubDense.getSubBlock( rowOffset, 0, tmp, getNW(), 1 );
01003               for( run1 = 0; run1 < getNW(); run1++ )
01004                   denseCP.ub(rowOffset1+run1) = tmp(run1,0);
01005               rowOffset1 += getNW();
01006          }
01007          rowOffset  ++;
01008      }
01009 
01010      return SUCCESSFUL_RETURN;
01011 }
01012 
01013 
01014 returnValue CondensingBasedCPsolver::generateObjectiveGradient( ){
01015 
01016     uint run1, run3;
01017 
01018     uint  rowOffset  = 0;
01019     uint  rowOffset1 = 0;
01020 
01021     uint N = getNumPoints();
01022 
01023     DMatrix tmp;
01024 
01025     if( getNX() != 0 ){
01026         gDense.getSubBlock( 0, 0, tmp, 1, getNX() );
01027         for( run1 = 0; run1 < getNX(); run1++ )
01028             denseCP.g(rowOffset1+run1) = tmp(0,run1);
01029         rowOffset1 += getNX();
01030     }
01031     rowOffset++;
01032 
01033     for( run3 = 0; run3 < N; run3++ ){
01034          if( getNXA() != 0 ){
01035             gDense.getSubBlock( 0, rowOffset, tmp, 1, getNXA() );
01036             for( run1 = 0; run1 < getNXA(); run1++ )
01037                 denseCP.g(rowOffset1+run1) = tmp(0,run1);
01038             rowOffset1 += getNXA();
01039         }
01040         rowOffset++;
01041     }
01042 
01043     if( getNP() != 0 ){
01044         gDense.getSubBlock( 0, rowOffset, tmp, 1, getNP() );
01045         for( run1 = 0; run1 < getNP(); run1++ )
01046             denseCP.g(rowOffset1+run1) = tmp(0,run1);
01047          rowOffset1 += getNP();
01048      }
01049      rowOffset++;
01050 
01051      for( run3 = 0; run3 < N-1; run3++ ){
01052           if( getNU() != 0 ){
01053              gDense.getSubBlock( 0, rowOffset, tmp, 1, getNU() );
01054              for( run1 = 0; run1 < getNU(); run1++ )
01055                  denseCP.g(rowOffset1+run1) = tmp(0,run1);
01056              rowOffset1 += getNU();
01057          }
01058          rowOffset++;
01059      }
01060 
01061      for( run3 = 0; run3 < N-1; run3++ ){
01062          if( getNW() != 0 ){
01063               gDense.getSubBlock( 0, rowOffset, tmp, 1, getNW() );
01064               for( run1 = 0; run1 < getNW(); run1++ )
01065                   denseCP.g(rowOffset1+run1) = tmp(0,run1);
01066               rowOffset1 += getNW();
01067          }
01068          rowOffset  ++;
01069      }
01070 
01071      return SUCCESSFUL_RETURN;
01072 }
01073 
01074 
01075 
01076 returnValue CondensingBasedCPsolver::expand(    BandedCP& cp
01077                                                                                                 )
01078 {
01079         // do not expand if condensing is frozen
01080 //      if ( condensingStatus == COS_FROZEN )
01081 //              return SUCCESSFUL_RETURN;
01082 
01083         if ( ( condensingStatus != COS_CONDENSED ) && ( condensingStatus != COS_FROZEN ) )
01084                 return ACADOERROR( RET_UNABLE_TO_EXPAND );
01085 
01086 
01087     uint run1,run2;
01088 
01089     int rowCount  = 0;
01090     int rowCount1 = 0;
01091 
01092     uint nF = getNF();
01093     uint N = getNumPoints();
01094 
01095 
01096         DVector denseDualSolution( denseCP.getMergedDualSolution( ) );
01097 
01098     if( getNX() != 0 ){
01099 
01100         BlockMatrix primalDense;
01101         primalDense.init( 3*N, 1 );
01102 
01103         DMatrix tmp;
01104 
01105         rowCount  = 0;
01106         rowCount1 = 0;
01107 
01108         tmp.init( getNX(), 1 );
01109         for( run1 = 0; run1 < getNX(); run1++ )
01110             tmp(run1,0) = (*denseCP.x)(run1);
01111         primalDense.setDense( 0, 0, tmp );
01112         rowCount1 += getNX();
01113         rowCount++;
01114 
01115         for( run2 = 0; run2 < N; run2++ ){
01116             if( getNXA() != 0 ){
01117                 tmp.init( getNXA(), 1 );
01118                 for( run1 = 0; run1 < getNXA(); run1++ )
01119                     tmp(run1,0) = (*denseCP.x)(rowCount1+run1);
01120                 primalDense.setDense( rowCount, 0, tmp );
01121                 rowCount1 += getNXA();
01122             }
01123             rowCount++;
01124         }
01125 
01126         if( getNP() != 0 ){
01127             tmp.init( getNP(), 1 );
01128             for( run1 = 0; run1 < getNP(); run1++ )
01129                 tmp(run1,0) = (*denseCP.x)(rowCount1+run1);
01130             primalDense.setDense( rowCount, 0, tmp );
01131             rowCount1 += getNP();
01132         }
01133         rowCount++;
01134 
01135         for( run2 = 0; run2 < N-1; run2++ ){
01136             if( getNU() != 0 ){
01137                 tmp.init( getNU(), 1 );
01138                 for( run1 = 0; run1 < getNU(); run1++ )
01139                     tmp(run1,0) = (*denseCP.x)(rowCount1+run1);
01140                 primalDense.setDense( rowCount, 0, tmp );
01141                 rowCount1 += getNU();
01142             }
01143             rowCount++;
01144         }
01145 
01146         for( run2 = 0; run2 < N-1; run2++ ){
01147             if( getNW() != 0 ){
01148                 tmp.init( getNW(), 1 );
01149                 for( run1 = 0; run1 < getNW(); run1++ )
01150                     tmp(run1,0) = (*denseCP.x)(rowCount1+run1);
01151                 primalDense.setDense( rowCount, 0, tmp );
01152                 rowCount1 += getNW();
01153             }
01154             rowCount++;
01155         }
01156 
01157         cp.deltaX = T*primalDense + d;
01158 
01159         BlockMatrix aux;
01160 
01161         aux = (cp.deltaX^cp.hessian) + cp.objectiveGradient;
01162 
01163         DVector aux2(N*getNX());
01164         aux2.setZero();
01165 
01166         int run = 0;
01167         uint run3, run4;
01168 
01169         for( run1 = 0; run1 < (uint)cp.constraintGradient.getNumRows(); run1++ ){
01170             for( run2 = 0; run2 < N; run2++ ){
01171                 cp.constraintGradient.getSubBlock( run1, run2, tmp );
01172                 for( run3 = 0; run3 < (uint)tmp.getNumRows(); run3++ ){
01173                     for( run4 = 0; run4 < getNX(); run4++ ){
01174                         aux2(run2*getNX()+run4) += denseDualSolution(nF+run+run3)*tmp(run3,run4);
01175                     }
01176                 }
01177                 run += tmp.getNumRows();
01178             }
01179         }
01180 
01181         DVector aux3(N*getNX());
01182 
01183         for( run2 = 0; run2 < getNX(); run2++ )
01184             aux3(run2) = denseDualSolution(run2);
01185 
01186         run = nF;
01187         run += getNC();
01188 
01189         for( run1 = 0; run1 < N-1; run1++ )
01190             for( run2 = 0; run2 < getNX(); run2++ )
01191                 aux3((run1+1)*getNX()+run2) = denseDualSolution(run+run1*getNX()+run2);
01192 
01193         DVector *aux4 = new DVector[N-1];
01194 
01195         // aux  = x^T denseCP.H + denseCP.g      (BlockMatrix)
01196         // aux2 = lambda^T denseCP.A     (DVector     )
01197         // aux3 = lambda_bound   (DVector     )
01198 
01199         for( run1 = 0; run1 < N-1; run1++ ){
01200             aux4[run1].init(getNX());
01201             aux.getSubBlock( 0, run1+1, tmp );
01202 
01203             for( run2 = 0; run2 < getNX(); run2++ ){
01204                 aux4[run1](run2) = tmp(0,run2) - aux2( (run1+1)*getNX()+run2 ) - aux3( (run1+1)*getNX()+run2 );
01205             }
01206         }
01207 
01208         // aux4[...] = x^T denseCP.H + denseCP.g - lambda^T denseCP.A - lambda_bound
01209 
01210         DMatrix Gx;
01211         DVector *lambdaDyn;
01212         lambdaDyn = new DVector[N-1];
01213         lambdaDyn[N-2] = aux4[N-2];
01214 
01215         for( run1 = N-2; run1 >= 1; run1-- ){
01216             cp.dynGradient.getSubBlock( run1, 0,  Gx );
01217             lambdaDyn[run1-1] = (Gx.transpose() * lambdaDyn[run1]) + aux4[run1-1];
01218         }
01219 
01220         cp.lambdaDynamic.init( N-1, 1 );
01221 
01222         for( run1 = 0; run1 < N-1; run1++ ){
01223             tmp.init( lambdaDyn[run1].getDim(), 1 );
01224             for( run2 = 0; run2 < lambdaDyn[run1].getDim(); run2++ )
01225                 tmp(run2,0) = lambdaDyn[run1].operator()(run2);
01226             cp.lambdaDynamic.setDense( run1, 0, tmp );
01227         }
01228 
01229 
01230         int dynMode;
01231         get( DYNAMIC_SENSITIVITY, dynMode );
01232 
01233         if( dynMode == FORWARD_SENSITIVITY_LIFTED ){
01234             for( run1 = 0; run1 < N-1; run1++ ){
01235                  tmp.init( lambdaDyn[run1].getDim(), 1 );
01236                  tmp.setAll(1.0/((double) lambdaDyn[run1].getDim()));
01237                  cp.lambdaDynamic.setDense( run1, 0, tmp );
01238             }
01239         }
01240 
01241         delete[] lambdaDyn;
01242         delete[] aux4;
01243     }
01244     else{
01245         DMatrix tmp ( getNP(),1 );
01246         cp.deltaX.init( 5, 1 );
01247 
01248         for( run1 = 0; run1 < getNP(); run1++ )
01249             tmp( run1, 0 ) = (*denseCP.x)(run1);
01250 
01251         cp.deltaX.setDense( 2, 0, tmp );
01252     }
01253 
01254 
01255     DMatrix tmp;
01256 
01257     cp.lambdaConstraint.init( blockDims.getDim(), 1 );
01258 
01259     int run = 0;
01260     for( run1 = 0; run1 < blockDims.getDim(); run1++ ){
01261         tmp.init( (int) blockDims(run1), 1 );
01262 
01263         for( run2 = 0; run2 < blockDims(run1); run2++ ){
01264             tmp(run2,0) = denseDualSolution(nF+run+run2);
01265         }
01266         cp.lambdaConstraint.setDense( run1, 0, tmp );
01267         run += (int) blockDims(run1);
01268     }
01269 
01270     cp.lambdaBound.init(4*N+1,1);
01271     rowCount = 0;
01272 
01273     if( getNX() != 0 ){
01274         tmp.init(getNX(),1);
01275         for( run1 = 0; run1 < getNX(); run1++ )
01276             tmp(run1,0) = denseDualSolution(run1);
01277         cp.lambdaBound.setDense( 0, 0, tmp );
01278         rowCount++;
01279     }
01280     rowCount1 = 0;
01281 
01282     for( run2 = 1; run2 < N; run2++ ){
01283         if( getNX() != 0 ){
01284             tmp.init( getNX(), 1 );
01285             for( run1 = 0; run1 < getNX(); run1++ )
01286                 tmp(run1,0) = denseDualSolution(nF+getNC()+rowCount1+run1);
01287             cp.lambdaBound.setDense( rowCount, 0, tmp );
01288             rowCount1 += getNX();
01289         }
01290         rowCount++;
01291     }
01292     rowCount1 = getNX();
01293 
01294     for( run2 = 0; run2 < N; run2++ ){
01295         if( getNXA() != 0 ){
01296             tmp.init( getNXA(), 1 );
01297             for( run1 = 0; run1 < getNXA(); run1++ )
01298                 tmp(run1,0) = denseDualSolution(rowCount1+run1);
01299             cp.lambdaBound.setDense( rowCount, 0, tmp );
01300             rowCount1 += getNXA();
01301         }
01302         rowCount++;
01303     }
01304 
01305     if( getNP() != 0 ){
01306         tmp.init( getNP(), 1 );
01307         for( run1 = 0; run1 < getNP(); run1++ )
01308             tmp(run1,0) = denseDualSolution(rowCount1+run1);
01309         cp.lambdaBound.setDense( rowCount, 0, tmp );
01310         rowCount1 += getNP();
01311     }
01312     rowCount++;
01313 
01314     for( run2 = 0; run2 < N-1; run2++ ){
01315         if( getNU() != 0 ){
01316             tmp.init( getNU(), 1 );
01317             for( run1 = 0; run1 < getNU(); run1++ )
01318                 tmp(run1,0) = denseDualSolution(rowCount1+run1);
01319             cp.lambdaBound.setDense( rowCount, 0, tmp );
01320             rowCount1 += getNU();
01321         }
01322         rowCount++;
01323     }
01324     rowCount++;
01325 
01326     for( run2 = 0; run2 < N-1; run2++ ){
01327         if( getNW() != 0 ){
01328             tmp.init( getNW(), 1 );
01329             for( run1 = 0; run1 < getNW(); run1++ )
01330                 tmp(run1,0) = denseDualSolution(rowCount1+run1);
01331             cp.lambdaBound.setDense( rowCount, 0, tmp );
01332             rowCount1 += getNW();
01333         }
01334         rowCount++;
01335     }
01336 
01337         if ( condensingStatus != COS_FROZEN )
01338                 condensingStatus = COS_INITIALIZED;
01339 
01340     return SUCCESSFUL_RETURN;
01341 }
01342 
01343 
01344 
01345 returnValue CondensingBasedCPsolver::initializeCondensingOperator( )
01346 {
01347     uint N = getNumPoints();
01348 
01349         T.init( 5*N, 3*N );
01350     d.init( 5*N, 1   );
01351 
01352     T.setZero();
01353     d.setZero();
01354 
01355     if( getNX() != 0 ) T.setIdentity( 0, 0, getNX() );
01356     for( uint run1 = 0; run1 < N; run1++ ){
01357         if( getNXA() != 0 )        T.setIdentity(   N+run1,   1+run1, getNXA() );
01358         if( getNP()  != 0 )        T.setIdentity( 2*N+run1, N+1     , getNP() );
01359         if( getNU()  != 0 ){
01360             if( run1 != N-1 ) T.setIdentity( 3*N+run1, N+2+run1, getNU() );
01361             else              T.setIdentity( 3*N+run1, N+1+run1, getNU() );
01362         }
01363         if( getNW()  != 0 ){
01364             if( run1 != N-1 ) T.setIdentity( 4*N+run1, 2*N+1+run1, getNW() );
01365             else              T.setIdentity( 4*N+run1, 2*N  +run1, getNW() );
01366         }
01367     }
01368 
01369         condensingStatus = COS_INITIALIZED;
01370 
01371     return SUCCESSFUL_RETURN;
01372 }
01373 
01374 
01375 returnValue CondensingBasedCPsolver::computeCondensingOperator( BandedCP& cp
01376                                                                                                                                 )
01377 {
01378         uint run1, run2;
01379         uint N = getNumPoints();
01380 
01381         DMatrix  Gx;
01382         DMatrix   G;
01383         DMatrix tmp;
01384 
01385         for( run1 = 0; run1 < N-1; run1++ )
01386         {
01387                         // DIFFERENTIAL STATES:
01388                         // --------------------
01389                         cp.dynGradient.getSubBlock( run1, 0,  Gx );   // Get the sensitivity G_x^i with respect to x
01390                         
01391                 if ( condensingStatus != COS_FROZEN )
01392                 {
01393                         T             .getSubBlock( run1, 0, tmp );   // get the corresponding  C_i .
01394 
01395                         T.setDense( run1+1, 0, Gx*tmp );                   // compute C_{i+1} := G_x^i * C_i
01396 
01397                         // ALGEBRAIC STATES:
01398                         // --------------------
01399 
01400                         cp.dynGradient.getSubBlock( run1, 1, G );
01401 
01402                         for( run2 = 0; run2 <= run1; run2++ ){
01403 
01404                                 T.getSubBlock( run1, run2+1, tmp );
01405 
01406                                 if( G.getDim() != 0 ){
01407 
01408                                         if( run1 == run2 ) T.setDense( run1+1, run2+1, G      );
01409                                         else               T.setDense( run1+1, run2+1, Gx*tmp );
01410                                 }
01411                         }
01412 
01413                         // PARAMETERS:
01414                         // --------------------
01415 
01416                         cp.dynGradient.getSubBlock( run1, 2  ,  G  ); // Get the sensitivity G_p^i with respect to p
01417                         T             .getSubBlock( run1, N+1, tmp ); // get the corresponding  D_p^i.
01418 
01419                         if( tmp.getDim() != 0 ){
01420                                 if( G.getDim() != 0 )
01421                                         T.setDense( run1+1, N+1, Gx*tmp + G );   // compute  D_p^{i+1} := G_x^i D_p^i + G_p^i
01422                         }
01423                         else{
01424                                 if( G.getDim() != 0 )
01425                                         T.setDense( run1+1, N+1,          G );   // compute  D_p^{i+1} := G_x^i D_p^i + G_p^i
01426                         }
01427 
01428                         // CONTROLS:
01429                         // --------------------
01430 
01431                         cp.dynGradient.getSubBlock( run1, 3, G );
01432                         for( run2 = 0; run2 <= run1; run2++ ){
01433 
01434                                 T.getSubBlock( run1, run2+2+N, tmp );
01435 
01436                                 if( G.getDim() != 0 ){
01437 
01438                                         if( run1 == run2 ) T.setDense( run1+1, run2+2+N, G      );
01439                                         else               T.setDense( run1+1, run2+2+N, Gx*tmp );
01440                                 }
01441                         }
01442 
01443                         // DISTURBANCES:
01444                         // --------------------
01445 
01446                         cp.dynGradient.getSubBlock( run1, 4, G );
01447 
01448                         for( run2 = 0; run2 <= run1; run2++ ){
01449 
01450                                 T.getSubBlock( run1, run2+1+2*N, tmp );
01451 
01452                                 if( G.getDim() != 0 ){
01453 
01454                                         if( run1 == run2 ) T.setDense( run1+1, run2+1+2*N, G      );
01455                                         else               T.setDense( run1+1, run2+1+2*N, Gx*tmp );
01456                                 }
01457                         }
01458                 }
01459 
01460                 // RESIDUUM:
01461                 // --------------------
01462 
01463                 cp.dynResiduum.getSubBlock( run1, 0,  G  );   // Get the residuum  b^i
01464                 d             .getSubBlock( run1, 0, tmp );   // get the corresponding  d^i.
01465 
01466                 if( tmp.getDim() != 0 ){
01467                         if( G.getDim() != 0 )
01468                                 d.setDense( run1+1, 0, Gx*tmp + G );   // compute  d^{i+1} := G_x^i d^i + b^i
01469                 }
01470                 else{
01471                         if( G.getDim() != 0 )
01472                                 d.setDense( run1+1, 0, G );   // compute  d^{i+1} := b^i
01473                 }
01474         }
01475 
01476         return SUCCESSFUL_RETURN;
01477 }
01478 
01479 
01480 
01481 returnValue CondensingBasedCPsolver::setupRelaxedQPdata(        InfeasibleQPhandling infeasibleQPhandling,
01482                                                                                                                         DenseCP& _denseCPrelaxed
01483                                                                                                                         ) const
01484 {
01485         switch( infeasibleQPhandling )
01486         {
01487                 case IQH_RELAX_L1:
01488                         return setupRelaxedQPdataL2( _denseCPrelaxed );
01489 
01490                 case IQH_RELAX_L2:
01491                         return setupRelaxedQPdataL2( _denseCPrelaxed );
01492 
01493                 default:
01494                         return ACADOERROR( RET_INVALID_ARGUMENTS );
01495         }
01496 }
01497 
01498 
01499 returnValue CondensingBasedCPsolver::setupRelaxedQPdataL1(      DenseCP& _denseCPrelaxed
01500                                                                                                                         ) const
01501 {
01502         return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
01503         
01504         uint nV = denseCP.getNV();
01505         uint nC = denseCP.getNC();
01506 
01507 
01508         /* allocate memory for relaxed QP data */
01509         _denseCPrelaxed.H.init( nV+nC,nV+nC );
01510         _denseCPrelaxed.A.init( nC,nV+nC );
01511         _denseCPrelaxed.g.init( nV+nC );
01512         _denseCPrelaxed.lb.init( nV+nC );
01513         _denseCPrelaxed.ub.init( nV+nC );
01514         _denseCPrelaxed.lbA = denseCP.lbA;
01515         _denseCPrelaxed.ubA = denseCP.ubA;
01516 
01517 //      acadoPrintf( "frob:  %e\n", H.getNorm( MN_FROBENIUS ) );
01518 //      acadoPrintf( "trace: %e\n", H.getTrace() );
01519 //      acadoPrintf( "min diag: %e\n", (H.getDiag()).getMin() );
01520 //      acadoPrintf( "max diag: %e\n", (H.getDiag()).getMax() );
01521 //      acadoPrintf( "max(abs(g)): %e\n", (g.getAbsolute()).getMax() );
01522 
01523         _denseCPrelaxed.H.setZero( );
01524         for( uint i=0; i<nV; ++i )
01525                 for( uint j=0; j<nV; ++j )
01526                         _denseCPrelaxed.H(i,j) = denseCP.H(i,j);
01527 
01528         _denseCPrelaxed.A.setZero( );
01529         for( uint i=0; i<nC; ++i )
01530                 for( uint j=0; j<nV; ++j )
01531                         _denseCPrelaxed.A(i,j) = denseCP.A(i,j);
01532         for( uint i=0; i<nC; ++i )
01533                 _denseCPrelaxed.A(i,nV+i) = 1.0;
01534 
01535         _denseCPrelaxed.g.setZero( );
01536         for( uint i=0; i<nV; ++i )
01537                 _denseCPrelaxed.g(i) = denseCP.g(i);
01538         double l1entry = 1.0e5 * ((denseCP.g).getAbsolute()).getMax( );
01539         for( uint i=nV; i<nV+nC; ++i )
01540                 _denseCPrelaxed.g(i) = l1entry;
01541 
01542         _denseCPrelaxed.lb.setZero( );
01543         for( uint i=0; i<nV; ++i )
01544                 _denseCPrelaxed.lb(i) = denseCP.lb(i);
01545         for( uint i=nV; i<nV+nC; ++i )
01546                 _denseCPrelaxed.lb(i) = 0.0;
01547 
01548         _denseCPrelaxed.ub.setZero( );
01549         for( uint i=0; i<nV; ++i )
01550                 _denseCPrelaxed.ub(i) = denseCP.ub(i);
01551         for( uint i=nV; i<nV+nC; ++i )
01552                 _denseCPrelaxed.ub(i) =  INFTY;
01553 
01554         return SUCCESSFUL_RETURN;
01555 }
01556 
01557 
01558 returnValue CondensingBasedCPsolver::setupRelaxedQPdataL2(      DenseCP& _denseCPrelaxed
01559                                                                                                                         ) const
01560 {
01561         uint nV = denseCP.getNV();
01562         uint nC = denseCP.getNC();
01563 
01564 
01565         /* allocate memory for relaxed QP data */
01566         _denseCPrelaxed.H.init( nV+nC,nV+nC );
01567         _denseCPrelaxed.A.init( nC,nV+nC );
01568         _denseCPrelaxed.g.init( nV+nC );
01569         _denseCPrelaxed.lb.init( nV+nC );
01570         _denseCPrelaxed.ub.init( nV+nC );
01571         _denseCPrelaxed.lbA = denseCP.lbA;
01572         _denseCPrelaxed.ubA = denseCP.ubA;
01573 
01574 //      acadoPrintf( "frob:  %e\n", H.getNorm( MN_FROBENIUS ) );
01575 //      acadoPrintf( "trace: %e\n", H.getTrace() );
01576 //      acadoPrintf( "min diag: %e\n", (H.getDiag()).getMin() );
01577 //      acadoPrintf( "max diag: %e\n", (H.getDiag()).getMax() );
01578 //      acadoPrintf( "max(abs(g)): %e\n", (g.getAbsolute()).getMax() );
01579 
01580         _denseCPrelaxed.H.setZero( );
01581         for( uint i=0; i<nV; ++i )
01582                 for( uint j=0; j<nV; ++j )
01583                         _denseCPrelaxed.H(i,j) = denseCP.H(i,j);
01584         double diagonalEntry = 1.0e5 * acadoMax( (denseCP.H).getTrace( ),((denseCP.g).getAbsolute()).getMax( ) );
01585         for( uint i=nV; i<nV+nC; ++i )
01586                 _denseCPrelaxed.H(i,i) = diagonalEntry;
01587 
01588         _denseCPrelaxed.A.setZero( );
01589         for( uint i=0; i<nC; ++i )
01590                 for( uint j=0; j<nV; ++j )
01591                         _denseCPrelaxed.A(i,j) = denseCP.A(i,j);
01592         for( uint i=0; i<nC; ++i )
01593                 _denseCPrelaxed.A(i,nV+i) = 1.0;
01594 
01595         _denseCPrelaxed.g.setZero( );
01596         for( uint i=0; i<nV; ++i )
01597                 _denseCPrelaxed.g(i) = denseCP.g(i);
01598 
01599         _denseCPrelaxed.lb.setZero( );
01600         for( uint i=0; i<nV; ++i )
01601                 _denseCPrelaxed.lb(i) = denseCP.lb(i);
01602         for( uint i=nV; i<nV+nC; ++i )
01603                 _denseCPrelaxed.lb(i) = -INFTY;
01604 
01605         _denseCPrelaxed.ub.setZero( );
01606         for( uint i=0; i<nV; ++i )
01607                 _denseCPrelaxed.ub(i) = denseCP.ub(i);
01608         for( uint i=nV; i<nV+nC; ++i )
01609                 _denseCPrelaxed.ub(i) = INFTY;
01610 
01611         return SUCCESSFUL_RETURN;
01612 }
01613 
01614 
01615 
01616 returnValue CondensingBasedCPsolver::initializeCPsolver(        InfeasibleQPhandling infeasibleQPhandling
01617                                                                                                                         )
01618 {
01619     denseCP.init( getNF(),getNA() );
01620         
01621         denseCP.g.init( getNF() );
01622         denseCP.lb.init( getNF() );
01623         denseCP.ub.init( getNF() );
01624         denseCP.lbA.init( getNA() );
01625         denseCP.ubA.init( getNA() );
01626 
01627         cpSolver->init( &denseCP );
01628 
01629         // setup relaxed QP
01630 //      uint nV_relaxed, nC_relaxed;
01631 //      getRelaxedQPdimensions( infeasibleQPhandling,nV_relaxed,nC_relaxed );
01632 //      cpSolverRelaxed->init( nV_relaxed,nC_relaxed );
01633 
01634         return SUCCESSFUL_RETURN;
01635 }
01636 
01637 
01638 
01639 returnValue CondensingBasedCPsolver::solveQP(   uint maxIter,
01640                                                                                                 InfeasibleQPhandling infeasibleQPhandling
01641                                                                                                 )
01642 {
01643 //      denseCP.lb.print( "lb" );
01644 // //   denseCP.ub.print( "ub" );
01645 //      (denseCP.ub - denseCP.lb).print( "ub-lb" );
01646         
01647         if ( infeasibleQPhandling == IQH_UNDEFINED )
01648                 return cpSolver->solve( &denseCP );
01649         
01650 
01651         /* relax QP... */
01652         DenseCP denseCPrelaxed;
01653 
01654         if ( setupRelaxedQPdata( infeasibleQPhandling,denseCPrelaxed ) != SUCCESSFUL_RETURN )
01655                 return ACADOERROR( RET_COULD_NOT_RELAX_QP );
01656 
01657         /* ... and solve relaxed QP */
01658         if ( cpSolverRelaxed == 0 )
01659                 cpSolverRelaxed = new QPsolver_qpOASES( userInteraction );
01660 
01661         if ( ( cpSolverRelaxed->getNumberOfVariables( ) != denseCPrelaxed.getNV() ) ||
01662                  ( cpSolverRelaxed->getNumberOfConstraints( ) != denseCPrelaxed.getNC() ) )
01663         {
01664                 cpSolverRelaxed->init( denseCPrelaxed.getNV(),denseCPrelaxed.getNC() );
01665         }
01666 
01667     returnValue returnvalue;
01668     returnvalue = cpSolverRelaxed->solve( &denseCPrelaxed );
01669 
01670         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_QP_SOLUTION_REACHED_LIMIT ) )
01671                 return returnvalue;
01672 
01673         const uint nV = denseCP.getNV();
01674         const uint nC = denseCP.getNC();
01675 
01676         DVector deltaDenseTmp;
01677         DVector lambdaDenseTmp;
01678 
01679         DVector deltaDense (nV);
01680         DVector lambdaDense(nV+nC);
01681 
01682         cpSolverRelaxed->getPrimalSolution( deltaDenseTmp  );
01683         cpSolverRelaxed->getDualSolution  ( lambdaDenseTmp );
01684 
01685         for( uint i=0; i < nV; ++i )
01686                 deltaDense(i) = deltaDenseTmp(i);
01687 
01688 //      double maxRelaxed = 0.0;
01689 // 
01690 //      for( uint i=nV; i < nV+nC; ++i )
01691 //              if ( fabs(deltaDenseTmp(i)) > maxRelaxed )
01692 //                      maxRelaxed = fabs(deltaDenseTmp(i));
01693 //      printf ("maxRelaxed: %e\n",maxRelaxed);
01694         
01695         for( uint i=0; i < nV; ++i )
01696                 lambdaDense(i) = lambdaDenseTmp(i);
01697         for( uint i=0; i < nC; ++i )
01698                 lambdaDense(nV+i) = lambdaDenseTmp(denseCPrelaxed.getNV()+i);
01699 
01700         // deltaDense.print( "xOpt" );
01701         denseCP.setQPsolution( deltaDense,lambdaDense );
01702 
01703     return returnvalue;
01704 }
01705 
01706 
01707 
01708 
01709 CLOSE_NAMESPACE_ACADO
01710 
01711 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:54