Go to the documentation of this file.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
00027
00034 #include <acado/objective/objective_element.hpp>
00035
00036
00037
00038 BEGIN_NAMESPACE_ACADO
00039
00040
00041
00042
00043 ObjectiveElement::ObjectiveElement( ){
00044
00045 y_index = 0;
00046 t_index = 0;
00047 nx = 0;
00048 na = 0;
00049 nu = 0;
00050 np = 0;
00051 nw = 0;
00052 ny = 0;
00053
00054 xSeed = 0 ;
00055 xaSeed = 0 ;
00056 pSeed = 0 ;
00057 uSeed = 0 ;
00058 wSeed = 0 ;
00059 bSeed = 0 ;
00060 xSeed2 = 0 ;
00061 xaSeed2 = 0 ;
00062 pSeed2 = 0 ;
00063 uSeed2 = 0 ;
00064 wSeed2 = 0 ;
00065 bSeed2 = 0 ;
00066
00067 obj = 0.0;
00068 }
00069
00070
00071 ObjectiveElement::ObjectiveElement( const Grid &grid_ ){
00072
00073 grid = grid_;
00074 y_index = 0 ;
00075 t_index = 0 ;
00076 nx = 0 ;
00077 na = 0 ;
00078 nu = 0 ;
00079 np = 0 ;
00080 nw = 0 ;
00081 ny = 0 ;
00082
00083 xSeed = 0 ;
00084 xaSeed = 0 ;
00085 pSeed = 0 ;
00086 uSeed = 0 ;
00087 wSeed = 0 ;
00088 bSeed = 0 ;
00089 xSeed2 = 0 ;
00090 xaSeed2 = 0 ;
00091 pSeed2 = 0 ;
00092 uSeed2 = 0 ;
00093 wSeed2 = 0 ;
00094 bSeed2 = 0 ;
00095
00096 obj = 0.0;
00097 }
00098
00099
00100 ObjectiveElement::ObjectiveElement( const ObjectiveElement& rhs ){
00101
00102 int run1;
00103
00104 grid = rhs.grid;
00105 fcn = rhs.fcn ;
00106
00107 if( rhs.y_index != 0 ){
00108 y_index = new int[rhs.ny];
00109 for( run1 = 0; run1 < rhs.ny; run1++ )
00110 y_index[run1] = rhs.y_index[run1];
00111 }
00112 else y_index = 0;
00113
00114 t_index = rhs.t_index;
00115
00116 nx = rhs.nx;
00117 na = rhs.na;
00118 nu = rhs.nu;
00119 np = rhs.np;
00120 nw = rhs.nw;
00121 ny = rhs.ny;
00122
00123 if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
00124 else xSeed = 0 ;
00125 if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed);
00126 else xaSeed = 0 ;
00127 if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed) ;
00128 else pSeed = 0 ;
00129 if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed) ;
00130 else uSeed = 0 ;
00131 if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed) ;
00132 else wSeed = 0 ;
00133
00134 if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
00135 else bSeed = 0 ;
00136
00137 if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2) ;
00138 else xSeed2 = 0 ;
00139 if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00140 else xaSeed2 = 0 ;
00141 if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
00142 else pSeed2 = 0 ;
00143 if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
00144 else uSeed2 = 0 ;
00145 if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
00146 else wSeed2 = 0 ;
00147
00148 if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
00149 else bSeed2 = 0 ;
00150
00151
00152 obj = rhs.obj ;
00153 dForward = rhs.dForward ;
00154 dBackward = rhs.dBackward;
00155 }
00156
00157
00158 ObjectiveElement::~ObjectiveElement( ){
00159
00160 if( y_index != 0 )
00161 delete[] y_index;
00162
00163 if( xSeed != 0 ) delete xSeed ;
00164 if( xaSeed != 0 ) delete xaSeed ;
00165 if( pSeed != 0 ) delete pSeed ;
00166 if( uSeed != 0 ) delete uSeed ;
00167 if( wSeed != 0 ) delete wSeed ;
00168
00169 if( bSeed != 0 ) delete bSeed ;
00170
00171 if( xSeed2 != 0 ) delete xSeed2 ;
00172 if( xaSeed2 != 0 ) delete xaSeed2;
00173 if( pSeed2 != 0 ) delete pSeed2 ;
00174 if( uSeed2 != 0 ) delete uSeed2 ;
00175 if( wSeed2 != 0 ) delete wSeed2 ;
00176
00177 if( bSeed2 != 0 ) delete bSeed2 ;
00178 }
00179
00180
00181 ObjectiveElement& ObjectiveElement::operator=( const ObjectiveElement& rhs ){
00182
00183 int run1;
00184
00185 if( this != &rhs ){
00186
00187 if( y_index != 0 )
00188 delete[] y_index;
00189
00190 if( xSeed != 0 ) delete xSeed ;
00191 if( xaSeed != 0 ) delete xaSeed ;
00192 if( pSeed != 0 ) delete pSeed ;
00193 if( uSeed != 0 ) delete uSeed ;
00194 if( wSeed != 0 ) delete wSeed ;
00195
00196 if( bSeed != 0 ) delete bSeed ;
00197
00198 if( xSeed2 != 0 ) delete xSeed2 ;
00199 if( xaSeed2 != 0 ) delete xaSeed2;
00200 if( pSeed2 != 0 ) delete pSeed2 ;
00201 if( uSeed2 != 0 ) delete uSeed2 ;
00202 if( wSeed2 != 0 ) delete wSeed2 ;
00203
00204 if( bSeed2 != 0 ) delete bSeed2 ;
00205
00206 grid = rhs.grid;
00207 fcn = rhs.fcn ;
00208
00209 if( rhs.y_index != 0 ){
00210 y_index = new int[ny];
00211 for( run1 = 0; run1 < ny; run1++ )
00212 y_index[run1] = rhs.y_index[run1];
00213 }
00214 else y_index = 0;
00215
00216 t_index = rhs.t_index;
00217
00218 nx = rhs.nx;
00219 na = rhs.na;
00220 nu = rhs.nu;
00221 np = rhs.np;
00222 nw = rhs.nw;
00223 ny = rhs.ny;
00224
00225 if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
00226 else xSeed = 0 ;
00227 if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed );
00228 else xaSeed = 0 ;
00229 if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed );
00230 else pSeed = 0 ;
00231 if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed );
00232 else uSeed = 0 ;
00233 if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed );
00234 else wSeed = 0 ;
00235
00236 if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
00237 else bSeed = 0 ;
00238
00239 if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2 );
00240 else xSeed2 = 0 ;
00241 if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00242 else xaSeed2 = 0 ;
00243 if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
00244 else pSeed2 = 0 ;
00245 if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
00246 else uSeed2 = 0 ;
00247 if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
00248 else wSeed2 = 0 ;
00249
00250 if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
00251 else bSeed2 = 0 ;
00252
00253 obj = rhs.obj ;
00254 dForward = rhs.dForward ;
00255 dBackward = rhs.dBackward;
00256 }
00257 return *this;
00258 }
00259
00260
00261
00262
00263 returnValue ObjectiveElement::init( const OCPiterate &x ){
00264
00265 int run1;
00266
00267 z.init( fcn, x );
00268 JJ.init( fcn, x );
00269
00270
00271 if( x.x != NULL ) nx = x.x->getNumValues();
00272 else nx = 0 ;
00273
00274 if( x.xa != NULL ) na = x.xa->getNumValues();
00275 else na = 0 ;
00276
00277 if( x.p != NULL ) np = x.p ->getNumValues();
00278 else np = 0 ;
00279
00280 if( x.u != NULL ) nu = x.u ->getNumValues();
00281 else nu = 0 ;
00282
00283 if( x.w != NULL ) nw = x.w ->getNumValues();
00284 else nw = 0 ;
00285
00286 ny = nx+na+nu+np+nw;
00287
00288 if( y_index != 0 ) delete[] y_index;
00289 y_index = new int[ny];
00290
00291 for( run1 = 0; run1 < nx; run1++ )
00292 y_index[run1] = fcn.index( VT_DIFFERENTIAL_STATE, run1 );
00293
00294 for( run1 = 0; run1 < na; run1++ )
00295 y_index[nx+run1] = fcn.index( VT_ALGEBRAIC_STATE, run1 );
00296
00297 for( run1 = 0; run1 < np; run1++ )
00298 y_index[nx+na+run1] = fcn.index( VT_PARAMETER, run1 );
00299
00300 for( run1 = 0; run1 < nu; run1++ )
00301 y_index[nx+na+np+run1] = fcn.index( VT_CONTROL, run1 );
00302
00303 for( run1 = 0; run1 < nw; run1++ )
00304 y_index[nx+na+np+nu+run1] = fcn.index( VT_DISTURBANCE, run1 );
00305
00306 t_index = fcn.index( VT_TIME, 0 );
00307
00308 return SUCCESSFUL_RETURN;
00309 }
00310
00311
00312
00313 returnValue ObjectiveElement::setForwardSeed( BlockMatrix *xSeed_,
00314 BlockMatrix *xaSeed_,
00315 BlockMatrix *pSeed_,
00316 BlockMatrix *uSeed_,
00317 BlockMatrix *wSeed_,
00318 int order ){
00319
00320 if( order == 1 ){
00321
00322 if( xSeed_ != 0 ){
00323 if( xSeed != 0 ) delete xSeed;
00324 xSeed = new BlockMatrix(*xSeed_);
00325 }
00326 else{
00327 if( xSeed != 0 ) delete xSeed;
00328 xSeed = 0;
00329 }
00330 if( xaSeed_ != 0 ){
00331 if( xaSeed != 0 ) delete xaSeed;
00332 xaSeed = new BlockMatrix(*xaSeed_);
00333 }
00334 else{
00335 if( xaSeed != 0 ) delete xaSeed;
00336 xaSeed = 0;
00337 }
00338 if( pSeed_ != 0 ){
00339 if( pSeed != 0 ) delete pSeed;
00340 pSeed = new BlockMatrix(*pSeed_);
00341 }
00342 else{
00343 if( pSeed != 0 ) delete pSeed;
00344 pSeed = 0;
00345 }
00346 if( uSeed_ != 0 ){
00347 if( uSeed != 0 ) delete uSeed;
00348 uSeed = new BlockMatrix(*uSeed_);
00349 }
00350 else{
00351 if( uSeed != 0 ) delete uSeed;
00352 uSeed = 0;
00353 }
00354 if( wSeed_ != 0 ){
00355 if( wSeed != 0 ) delete wSeed;
00356 wSeed = new BlockMatrix(*wSeed_);
00357 }
00358 else{
00359 if( wSeed != 0 ) delete wSeed;
00360 wSeed = 0;
00361 }
00362
00363 return SUCCESSFUL_RETURN;
00364 }
00365 if( order == 2 ){
00366
00367 if( xSeed_ != 0 ){
00368 if( xSeed2 != 0 ) delete xSeed2;
00369 xSeed2 = new BlockMatrix(*xSeed_);
00370 }
00371 else{
00372 if( xSeed2 != 0 ) delete xSeed2;
00373 xSeed2 = 0;
00374 }
00375 if( xaSeed_ != 0 ){
00376 if( xaSeed2 != 0 ) delete xaSeed2;
00377 xaSeed2 = new BlockMatrix(*xaSeed_);
00378 }
00379 else{
00380 if( xaSeed2 != 0 ) delete xaSeed2;
00381 xaSeed2 = 0;
00382 }
00383 if( pSeed_ != 0 ){
00384 if( pSeed2 != 0 ) delete pSeed2;
00385 pSeed2 = new BlockMatrix(*pSeed_);
00386 }
00387 else{
00388 if( pSeed2 != 0 ) delete pSeed2;
00389 pSeed2 = 0;
00390 }
00391 if( uSeed_ != 0 ){
00392 if( uSeed2 != 0 ) delete uSeed2;
00393 uSeed2 = new BlockMatrix(*uSeed_);
00394 }
00395 else{
00396 if( uSeed2 != 0 ) delete uSeed2;
00397 uSeed2 = 0;
00398 }
00399 if( wSeed_ != 0 ){
00400 if( wSeed2 != 0 ) delete wSeed2;
00401 wSeed2 = new BlockMatrix(*wSeed_);
00402 }
00403 else{
00404 if( wSeed2 != 0 ) delete wSeed2;
00405 wSeed2 = 0;
00406 }
00407 return SUCCESSFUL_RETURN;
00408 }
00409
00410 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00411 }
00412
00413
00414 returnValue ObjectiveElement::setBackwardSeed( BlockMatrix *seed, int order ){
00415
00416 if( order == 1 ){
00417
00418 if( seed != 0 ){
00419 if( bSeed != 0 ) delete bSeed;
00420 bSeed = new BlockMatrix(*seed);
00421 }
00422 else{
00423 if( bSeed != 0 ) delete bSeed;
00424 bSeed = 0;
00425 }
00426
00427 return SUCCESSFUL_RETURN;
00428 }
00429 if( order == 2 ){
00430
00431 if( seed != 0 ){
00432 if( bSeed2 != 0 ) delete bSeed2;
00433 bSeed2 = new BlockMatrix(*seed);
00434 }
00435 else{
00436 if( bSeed2 != 0 ) delete bSeed2;
00437 bSeed2 = 0;
00438 }
00439 return SUCCESSFUL_RETURN;
00440 }
00441
00442 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00443 }
00444
00445
00446 returnValue ObjectiveElement::getObjectiveValue( double &objectiveValue ){
00447
00448 objectiveValue = obj;
00449 return SUCCESSFUL_RETURN;
00450 }
00451
00452
00453 returnValue ObjectiveElement::getForwardSensitivities( BlockMatrix *D, int order ){
00454
00455 ASSERT( D != 0 );
00456
00457 if( order == 1 ){
00458
00459 D[0] = dForward;
00460 return SUCCESSFUL_RETURN;
00461 }
00462 if( order == 2 ){
00463
00464 return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00465 }
00466 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00467 }
00468
00469
00470 returnValue ObjectiveElement::getBackwardSensitivities( BlockMatrix *D, int order ){
00471
00472 ASSERT( D != 0 );
00473
00474 if( order == 1 ){
00475 D[0] = dBackward;
00476 return SUCCESSFUL_RETURN;
00477 }
00478 if( order == 2 ){
00479
00480 return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00481 }
00482 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00483 }
00484
00485
00486
00487 CLOSE_NAMESPACE_ACADO
00488
00489