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
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
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
00143
00144 if ( initializeCondensingOperator( ) != SUCCESSFUL_RETURN )
00145 return ACADOERROR( RET_BANDED_CP_INIT_FAILED );
00146
00147
00148
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
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
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
00221
00222
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
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
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
00352
00353
00354 returnValue CondensingBasedCPsolver::projectHessian( DMatrix &H_, double dampingFactor ){
00355
00356 if( dampingFactor < 0.0 ) return SUCCESSFUL_RETURN;
00357
00358
00359
00360
00361 SelfAdjointEigenSolver< MatrixXd > es( H_ );
00362 MatrixXd V = es.eigenvectors();
00363 VectorXd D = es.eigenvalues();
00364
00365
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
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
00395 if ( denseCP.H.isSymmetric( ) == BT_FALSE )
00396 {
00397 ACADOINFO( RET_NONSYMMETRIC_HESSIAN_MATRIX );
00398 denseCP.H.symmetrize();
00399 }
00400
00401
00402
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
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
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
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
00439
00440 int maxQPiter;
00441 get( MAX_NUM_QP_ITERATIONS, maxQPiter );
00442
00443
00444 RealClock clock;
00445 clock.start( );
00446
00447
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:
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
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
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
00578 gDense = (cp.objectiveGradient*T) + (d^hT);
00579
00580 generateObjectiveGradient( );
00581
00582
00583
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
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
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
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
01080
01081
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
01196
01197
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
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
01388
01389 cp.dynGradient.getSubBlock( run1, 0, Gx );
01390
01391 if ( condensingStatus != COS_FROZEN )
01392 {
01393 T .getSubBlock( run1, 0, tmp );
01394
01395 T.setDense( run1+1, 0, Gx*tmp );
01396
01397
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
01414
01415
01416 cp.dynGradient.getSubBlock( run1, 2 , G );
01417 T .getSubBlock( run1, N+1, tmp );
01418
01419 if( tmp.getDim() != 0 ){
01420 if( G.getDim() != 0 )
01421 T.setDense( run1+1, N+1, Gx*tmp + G );
01422 }
01423 else{
01424 if( G.getDim() != 0 )
01425 T.setDense( run1+1, N+1, G );
01426 }
01427
01428
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
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
01461
01462
01463 cp.dynResiduum.getSubBlock( run1, 0, G );
01464 d .getSubBlock( run1, 0, tmp );
01465
01466 if( tmp.getDim() != 0 ){
01467 if( G.getDim() != 0 )
01468 d.setDense( run1+1, 0, Gx*tmp + G );
01469 }
01470 else{
01471 if( G.getDim() != 0 )
01472 d.setDense( run1+1, 0, G );
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
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
01518
01519
01520
01521
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
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
01575
01576
01577
01578
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
01630
01631
01632
01633
01634 return SUCCESSFUL_RETURN;
01635 }
01636
01637
01638
01639 returnValue CondensingBasedCPsolver::solveQP( uint maxIter,
01640 InfeasibleQPhandling infeasibleQPhandling
01641 )
01642 {
01643
01644
01645
01646
01647 if ( infeasibleQPhandling == IQH_UNDEFINED )
01648 return cpSolver->solve( &denseCP );
01649
01650
01651
01652 DenseCP denseCPrelaxed;
01653
01654 if ( setupRelaxedQPdata( infeasibleQPhandling,denseCPrelaxed ) != SUCCESSFUL_RETURN )
01655 return ACADOERROR( RET_COULD_NOT_RELAX_QP );
01656
01657
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
01689
01690
01691
01692
01693
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
01701 denseCP.setQPsolution( deltaDense,lambdaDense );
01702
01703 return returnvalue;
01704 }
01705
01706
01707
01708
01709 CLOSE_NAMESPACE_ACADO
01710
01711