scp_evaluation.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
35 
36 
37 
39 
40 
41 //
42 // PUBLIC MEMBER FUNCTIONS:
43 //
44 
46 {
47  setupOptions( );
48  setupLogging( );
49 
50  objective = 0;
52  constraint = 0;
53 
54  objectiveValue = 0.0;
55 
56  isCP = BT_FALSE;
58 }
59 
60 
62  const Objective* const objective_,
63  const DynamicDiscretization* const dynamic_discretization_,
64  const Constraint* const constraint_,
65  BooleanType _isCP
66  ) : AlgorithmicBase( _userInteraction )
67 {
68  // setup options and loggings for stand-alone instances
69  if ( _userInteraction == 0 )
70  {
71  setupOptions( );
72  setupLogging( );
73  }
74 
75  if( objective_ != 0 ) objective = new Objective( *objective_ );
76  else objective = 0;
77 
78  if( dynamic_discretization_ != 0 ) dynamicDiscretization = dynamic_discretization_->clone();
79  else dynamicDiscretization = 0;
80 
81  if( constraint_ != 0 ) constraint = new Constraint( *constraint_ );
82  else constraint = 0;
83 
84  objectiveValue = 0.0;
85 
86  isCP = _isCP;
88 }
89 
90 
92 {
93  if( rhs.objective != 0 ) objective = new Objective( *rhs.objective );
94  else objective = 0;
95 
97  else dynamicDiscretization = 0;
98 
99  if( rhs.constraint != 0 ) constraint = new Constraint( *rhs.constraint );
100  else constraint = 0;
101 
103 
104  isCP = rhs.isCP;
106 }
107 
108 
110 {
111  if( objective != 0 ) delete objective;
113  if( constraint != 0 ) delete constraint;
114 }
115 
116 
118 {
119  if ( this != &rhs )
120  {
121  if( objective != 0 ) delete objective;
123  if( constraint != 0 ) delete constraint;
124 
126 
127  if( rhs.objective != 0 ) objective = new Objective( *rhs.objective );
128  else objective = 0;
129 
131  else dynamicDiscretization = 0;
132 
133  if( rhs.constraint != 0 ) constraint = new Constraint( *rhs.constraint );
134  else constraint = 0;
135 
137 
138  isCP = rhs.isCP;
140  }
141 
142  return *this;
143 }
144 
145 
147 {
148  return new SCPevaluation( *this );
149 }
150 
151 
152 
154  ){
155 
156  return SUCCESSFUL_RETURN;
157 }
158 
159 
160 
162 
163  // EVALUATE THE OBJECTIVE AND CONSTRAINTS:
164  // ---------------------------------------
165  if( dynamicDiscretization != 0 )
167 
168  if( constraint != 0 )
170 
172 
173 
175 
176  if( dynamicDiscretization != 0 )
178 
179  if( constraint != 0 )
180  {
183  }
184 
185  if( dynamicDiscretization == 0 ){
186  cp.lowerBoundResiduum.setZero(0,0);
187  cp.lowerBoundResiduum.setZero(1,0);
188  cp.lowerBoundResiduum.setZero(3,0);
189  cp.lowerBoundResiduum.setZero(4,0);
190  cp.upperBoundResiduum.setZero(0,0);
191  cp.upperBoundResiduum.setZero(1,0);
192  cp.upperBoundResiduum.setZero(3,0);
193  cp.upperBoundResiduum.setZero(4,0);
194  }
195 
196  return SUCCESSFUL_RETURN;
197 }
198 
199 
200 
202  BandedCP& cp
203  )
204 {
206  return SUCCESSFUL_RETURN;
207 
208 
209  // DETERMINE THE HESSIAN APPROXIMATION MODE:
210  // -----------------------------------------
211  int hessMode;
212  get( HESSIAN_APPROXIMATION, hessMode );
213 
214  int dynHessMode;
215  get( DYNAMIC_HESSIAN_APPROXIMATION, dynHessMode );
217  dynHessMode = hessMode;
218 
219  int dynMode;
220  get( DYNAMIC_SENSITIVITY, dynMode );
221 
222  int objMode;
223  get( OBJECTIVE_SENSITIVITY, objMode );
224 
225  int conMode;
226  get( CONSTRAINT_SENSITIVITY, conMode );
227 
228 
229  // COMPUTE THE 1st ORDER DERIVATIVES:
230  // ----------------------------------
231 
235  else{
236  if( (HessianApproximationMode)hessMode == EXACT_HESSIAN ){
237  cp.hessian.setZero();
239  }
241  }
242 
243 
245 
246 
247 // printf("cp.hessian = \n");
248 // cp.hessian.print();
249 
250  if( dynamicDiscretization != 0 ){
251 
252  if( (HessianApproximationMode)dynHessMode == EXACT_HESSIAN ){
253 
257  }
258  else{
259  if( dynMode == BACKWARD_SENSITIVITY ){
260 
264  }
265  if( dynMode == FORWARD_SENSITIVITY ){
266 
270  }
271  if( dynMode == FORWARD_SENSITIVITY_LIFTED ){
272 
276  }
277  }
278  }
279 
280 // printf("cp.dynGradient = \n");
281 // (cp.dynGradient).print();
282 
283  if( constraint != 0 ){
284 
285  if( (HessianApproximationMode)hessMode == EXACT_HESSIAN ){
286 
290  }
291  else{
292  if( conMode == BACKWARD_SENSITIVITY ){
296  }
297  if( conMode == FORWARD_SENSITIVITY ){
301  }
302  }
303  }
304 
305  return SUCCESSFUL_RETURN;
306 }
307 
308 
309 
311  const OCPiterate& iter,
312  const BandedCP& cp,
313  BlockMatrix &nablaL
314  )
315 {
316  uint run1;
317  DMatrix tmp1, tmp2;
318  BlockMatrix aux( 5*N, 1 );
319 
320  for( run1 = 0; run1 < N-1; run1++ ){
321 
322  if( run1 < N-1 ) cp.lambdaDynamic.getSubBlock( run1, 0, tmp1, iter.getNX(), 1 );
323 
324  if( iter.getNX() != 0 ){
325  cp.dynGradient.getSubBlock( run1, 0, tmp2, iter.getNX(), iter.getNX() );
326  aux.addDense( run1 , 0, tmp2.transpose() * tmp1 );
327  aux.setDense( run1+1, 0, -tmp1 );
328  }
329  if( iter.getNXA() != 0 ){
330  cp.dynGradient.getSubBlock( run1, 1, tmp2, iter.getNX(), iter.getNXA() );
331  aux.setDense( N+run1, 0, tmp2.transpose() * tmp1 );
332  }
333  if( iter.getNP() != 0 ){
334  cp.dynGradient.getSubBlock( run1, 2, tmp2, iter.getNX(), iter.getNP() );
335  aux.setDense( 2*N+run1, 0, tmp2.transpose() * tmp1 );
336  }
337  if( iter.getNU() != 0 ){
338  cp.dynGradient.getSubBlock( run1, 3, tmp2, iter.getNX(), iter.getNU() );
339  aux.setDense( 3*N+run1, 0, tmp2.transpose() * tmp1 );
340  }
341  if( iter.getNW() != 0 ){
342  cp.dynGradient.getSubBlock( run1, 4, tmp2, iter.getNX(), iter.getNW() );
343  aux.setDense( 4*N+run1, 0, tmp2.transpose() * tmp1 );
344  }
345  }
346 
347 // printf("aux = \n");
348 // aux.print();
349 //
350 // cp.objectiveGradient.print();
351 // (cp.objectiveGradient.transpose()).print();
352 // lambdaBound.print();
353 // (cp.constraintGradient^cp.lambdaConstraint).print();
354 
355  if( dynamicDiscretization != 0 ){
356 
357  BlockMatrix lambdaBoundExpand(5*N,1);
358  for( run1 = 0; run1 < N; run1++ ){
359 
360  cp.lambdaBound.getSubBlock(run1,0,tmp1);
361  if( tmp1.getDim() != 0 )
362  lambdaBoundExpand.setDense(run1,0,tmp1);
363 
364  cp.lambdaBound.getSubBlock(N+run1,0,tmp1);
365  if( tmp1.getDim() != 0 )
366  lambdaBoundExpand.setDense(N+run1,0,tmp1);
367 
368  cp.lambdaBound.getSubBlock(2*N,0,tmp1);
369  if( tmp1.getDim() != 0 )
370  lambdaBoundExpand.setDense(2*N+run1,0,tmp1);
371 
372  cp.lambdaBound.getSubBlock(2*N+1+run1,0,tmp1);
373  if( tmp1.getDim() != 0 )
374  lambdaBoundExpand.setDense(3*N+run1,0,tmp1);
375 
376  cp.lambdaBound.getSubBlock(3*N+1+run1,0,tmp1);
377  if( tmp1.getDim() != 0 )
378  lambdaBoundExpand.setDense(4*N+run1,0,tmp1);
379  }
380 
381  nablaL = cp.objectiveGradient.transpose() - lambdaBoundExpand - (cp.constraintGradient^cp.lambdaConstraint) + aux;
382  }
383  else{
384  BlockMatrix lambdaBoundExpand(5,1);
385  cp.lambdaBound.getSubBlock(1,0,tmp1);
386  if( tmp1.getDim() != 0 )
387  lambdaBoundExpand.setDense(2,0,tmp1);
388  nablaL = cp.objectiveGradient.transpose() - lambdaBoundExpand - (cp.constraintGradient^cp.lambdaConstraint);
389  }
390 
391  return SUCCESSFUL_RETURN;
392 }
393 
394 
395 
397  const BandedCP& cp,
398  double KKTmultiplierRegularisation
399  )
400 {
401 
402 // printf("get KKT tolerance. \n \n");
403 
404  double KKTtol = 0.0;
405  double eps = 0.0;
406 
407  DMatrix tmp;
408 
409 // printf("obj Gradient \n");
410 // cp.objectiveGradient.print();
411 //
412 // printf("cp.deltaX \n");
413 // cp.deltaX.print();
414 
415  int hessianApproximation;
416  get( HESSIAN_APPROXIMATION,hessianApproximation );
417 
418  if ( ( isCP == BT_FALSE ) ||
419  ( ( (HessianApproximationMode)hessianApproximation != GAUSS_NEWTON ) && ( (HessianApproximationMode)hessianApproximation != GAUSS_NEWTON_WITH_BLOCK_BFGS ) )
420  )
421  {
422  eps = KKTmultiplierRegularisation;
423 
424  (cp.objectiveGradient*cp.deltaX).getSubBlock( 0, 0, tmp, 1, 1 );
425 // (cp.objectiveGradient*cp.deltaX).print();
426  KKTtol = fabs(tmp(0,0));
427  }
428 
429  // --------
430 
431 // printf("lambda Dynamic \n");
432 // cp.lambdaDynamic.print();
433 //
434 // printf("dynamic residuum \n");
435 // cp.dynResiduum.print();
436 // printf("interm. = %.16e \n", KKTtol );
437 
438 
439  if( dynamicDiscretization != 0 )
440  {
441  ( (cp.lambdaDynamic.getAbsolute()).addRegularisation(eps)^cp.dynResiduum.getAbsolute()).getSubBlock( 0, 0, tmp, 1, 1 );
442  KKTtol += tmp(0,0);
443  }
444 
445  // --------
446 
447 // printf("cp.lowerBoundResiduum \n");
448 // cp.lowerBoundResiduum.print();
449 //
450 // printf("cp.upperBoundResiduum \n");
451 // cp.upperBoundResiduum.print();
452 
453 // printf("cp.lambdaBound \n");
454 // cp.lambdaBound.print();
455 //
456 // printf("interm. = %.16e \n", KKTtol );
457 
458  ((cp.lambdaBound.getAbsolute()).addRegularisation(eps)^cp.upperBoundResiduum.getNegative()).getSubBlock( 0, 0, tmp, 1, 1 );
459  KKTtol -= tmp(0,0);
460 
461  ((cp.lambdaBound.getAbsolute()).addRegularisation(eps)^cp.lowerBoundResiduum.getPositive()).getSubBlock( 0, 0, tmp, 1, 1 );
462  KKTtol += tmp(0,0);
463 
464 //
465 // // --------
466 //
467 // printf("cp.lowerConstraintResiduum \n");
468 // cp.lowerConstraintResiduum.print();
469 //
470 // printf("cp.upperConstraintResiduum \n");
471 // cp.upperConstraintResiduum.print();
472 //
473 // printf("cp.lambdaConstraint = \n");
474 // cp.lambdaConstraint.print();
475 // printf("interm. = %.16e \n", KKTtol );
476 
477  if( ( constraint != 0 ) || ( dynamicDiscretization != 0 ) )
478  {
479  ((cp.lambdaConstraint.getAbsolute()).addRegularisation(eps)^cp.upperConstraintResiduum.getNegative()).getSubBlock( 0, 0, tmp, 1, 1 );
480  KKTtol -= tmp(0,0);
481 
482  ((cp.lambdaConstraint.getAbsolute()).addRegularisation(eps)^cp.lowerConstraintResiduum.getPositive()).getSubBlock( 0, 0, tmp, 1, 1 );
483  KKTtol += tmp(0,0);
484  }
485 
486 // printf("interm. = %.16e \n", KKTtol );
487 
488 //ASSERT( 1 == 0 );
489 
490  return KKTtol;
491 }
492 
493 
494 
495 
497 {
498  return objectiveValue;
499 }
500 
501 
502 
504 {
506  return SUCCESSFUL_RETURN;
507 }
508 
509 
511 {
513  return SUCCESSFUL_RETURN;
514 }
515 
516 
517 
519 {
520  if( objective == 0 )
522 
523 // ref.print("ref");
524 
525  if( objective->setReference( ref ) != SUCCESSFUL_RETURN )
526  return ACADOERROR( RET_UNKNOWN_BUG );
527 
528  return SUCCESSFUL_RETURN;
529 }
530 
531 
532 
534 {
535  if( dynamicDiscretization != 0 )
536  {
539  }
540 
541  return SUCCESSFUL_RETURN;
542 }
543 
544 
545 
546 
547 //
548 // PROTECTED MEMBER FUNCTIONS:
549 //
550 
552 {
553  return SUCCESSFUL_RETURN;
554 }
555 
556 
558 {
559  return SUCCESSFUL_RETURN;
560 }
561 
562 
563 
565 
566 // end of file.
returnValue setReference(const VariablesGrid &ref)
virtual returnValue getForwardSensitivities(BlockMatrix &D) const
#define N
virtual returnValue setUnitBackwardSeed()
Definition: objective.cpp:394
HessianApproximationMode
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
Implements a very rudimentary block sparse matrix class.
virtual SCPevaluation * clone() const
virtual returnValue getResiduum(BlockMatrix &residuum_) const
uint getNX() const
returnValue setReference(const VariablesGrid &ref)
virtual returnValue setUnitForwardSeed()
Definition: constraint.cpp:662
returnValue clearDynamicDiscretization()
BlockMatrix upperConstraintResiduum
Definition: banded_cp.hpp:114
BlockMatrix transpose() const
BlockMatrix deltaX
Definition: banded_cp.hpp:123
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
Stores and evaluates the constraints of optimal control problems.
Definition: constraint.hpp:60
Provides a time grid consisting of vector-valued optimization variables at each grid point...
BlockMatrix objectiveGradient
Definition: banded_cp.hpp:104
Allows to pass back messages to the calling function.
virtual returnValue setUnitBackwardSeed()
Definition: constraint.cpp:725
AlgorithmicBase & operator=(const AlgorithmicBase &rhs)
Base class for all algorithmic modules within the ACADO Toolkit providing some basic functionality...
BlockMatrix getAbsolute() const
virtual returnValue evaluate(OCPiterate &iter, BandedCP &cp)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
BlockMatrix lambdaConstraint
Definition: banded_cp.hpp:126
virtual returnValue evaluateLagrangeGradient(uint N, const OCPiterate &iter, const BandedCP &cp, BlockMatrix &nablaL)
uint getNP() const
returnValue evaluateSensitivitiesGN(BlockMatrix &hessian)
Definition: objective.cpp:319
virtual returnValue deleteAllSeeds()
virtual DynamicDiscretization * clone() const =0
uint getNU() const
#define CLOSE_NAMESPACE_ACADO
BlockMatrix hessian
Definition: banded_cp.hpp:103
Base class for discretizing a DifferentialEquation for use in optimal control algorithms.
BlockMatrix getNegative() const
virtual returnValue getBoundResiduum(BlockMatrix &lowerRes, BlockMatrix &upperRes)
Definition: constraint.cpp:910
BooleanType areSensitivitiesFrozen
virtual returnValue getObjectiveValue(double &objectiveValue)
Definition: objective.cpp:402
virtual double getObjectiveValue() const
BlockMatrix lambdaDynamic
Definition: banded_cp.hpp:125
virtual returnValue unfreeze()=0
BlockMatrix lowerBoundResiduum
Definition: banded_cp.hpp:106
virtual returnValue setUnitForwardSeed()
#define ACADO_TRY(X)
virtual double getKKTtolerance(const OCPiterate &iter, const BandedCP &cp, double KKTmultiplierRegularisation=0.0)
uint getNW() const
SCPevaluation & operator=(const SCPevaluation &rhs)
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
returnValue evaluate(const OCPiterate &iter)
Definition: constraint.cpp:399
unsigned getDim() const
Definition: matrix.hpp:181
virtual returnValue evaluateSensitivities(const OCPiterate &iter, BandedCP &cp)
virtual returnValue getForwardSensitivities(BlockMatrix &D, int order)
Definition: constraint.cpp:941
BlockMatrix lowerConstraintResiduum
Definition: banded_cp.hpp:113
BlockMatrix lambdaBound
Definition: banded_cp.hpp:124
BooleanType isCP
Objective * objective
BlockMatrix upperBoundResiduum
Definition: banded_cp.hpp:107
Encapsulates all user interaction for setting options, logging data and plotting results.
virtual returnValue evaluateSensitivities()=0
void rhs(const real_t *x, real_t *f)
virtual returnValue evaluate(OCPiterate &iter)=0
returnValue evaluate(const OCPiterate &x)
Definition: objective.cpp:254
#define BT_TRUE
Definition: acado_types.hpp:47
Constraint * constraint
returnValue evaluateSensitivities()
Definition: constraint.cpp:475
virtual returnValue init(const OCPiterate &iter)
BlockMatrix dynResiduum
Definition: banded_cp.hpp:110
virtual returnValue evaluateSensitivitiesLifted()=0
returnValue evaluateSensitivities()
Definition: objective.cpp:271
virtual returnValue freezeSensitivities()
virtual returnValue setupLogging()
virtual returnValue unfreezeSensitivities()
BlockMatrix getPositive() const
BlockMatrix dynGradient
Definition: banded_cp.hpp:109
returnValue setZero(uint rowIdx, uint colIdx)
uint getNXA() const
Base class for different ways to evaluate functions and derivatives within an SCPmethod for solving N...
#define BEGIN_NAMESPACE_ACADO
DynamicDiscretization * dynamicDiscretization
#define BT_FALSE
Definition: acado_types.hpp:49
virtual returnValue getBackwardSensitivities(BlockMatrix &D) const
virtual returnValue setUnitBackwardSeed()
BlockMatrix constraintGradient
Definition: banded_cp.hpp:112
virtual ~SCPevaluation()
virtual returnValue getBackwardSensitivities(BlockMatrix &D, int order)
Definition: objective.cpp:472
returnValue addDense(uint rowIdx, uint colIdx, const DMatrix &value)
virtual returnValue getConstraintResiduum(BlockMatrix &lowerRes, BlockMatrix &upperRes)
Definition: constraint.cpp:797
Stores and evaluates the objective function of optimal control problems.
Definition: objective.hpp:123
virtual returnValue setupOptions()
#define ACADOERROR(retval)
virtual returnValue getBackwardSensitivities(BlockMatrix &D, int order)
Data class for storing conic programs arising from optimal control.
Definition: banded_cp.hpp:56


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:04