scp_merit_function.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 
00034 #include <acado/nlp_solver/scp_merit_function.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 SCPmeritFunction::SCPmeritFunction( ) : AlgorithmicBase( )
00046 {
00047 }
00048 
00049 
00050 SCPmeritFunction::SCPmeritFunction( UserInteraction* _userInteraction ) : AlgorithmicBase( _userInteraction )
00051 {
00052 }
00053 
00054 
00055 SCPmeritFunction::SCPmeritFunction( const SCPmeritFunction& rhs ) : AlgorithmicBase( rhs )
00056 {
00057 }
00058 
00059 
00060 SCPmeritFunction::~SCPmeritFunction( )
00061 {
00062 }
00063 
00064 
00065 SCPmeritFunction& SCPmeritFunction::operator=( const SCPmeritFunction& rhs )
00066 {
00067     if ( this != &rhs )
00068     {
00069                 AlgorithmicBase::operator=( rhs );
00070     }
00071 
00072     return *this;
00073 }
00074 
00075 
00076 SCPmeritFunction* SCPmeritFunction::clone( ) const
00077 {
00078         return new SCPmeritFunction( *this );
00079 }
00080 
00081 
00082 returnValue SCPmeritFunction::evaluate( double alpha,
00083                                                                                 const OCPiterate& iter,
00084                                                                                 BandedCP& cp,
00085                                                                                 SCPevaluation& eval,
00086                                                                                 double& result
00087                                                                                 )
00088 {
00089     if( fabs( alpha ) >= EPS )
00090         {
00091                 result = INFTY;
00092 
00093         eval.clearDynamicDiscretization( );
00094 
00095                 OCPiterate iterTest = iter;
00096                 iterTest.applyStep( cp.deltaX,alpha );
00097 
00098                 if ( eval.evaluate( iterTest,cp ) != SUCCESSFUL_RETURN )
00099                         return SUCCESSFUL_RETURN;
00100     }
00101 
00102     result = eval.getObjectiveValue( );
00103 
00104 
00105     const double kappa = 1.2;
00106     DMatrix tmp;
00107 
00108 //     acadoPrintf("result1 = %.16e \n", result );
00109 
00110 //     acadoPrintf("cp.dynResiduum = \n");
00111 //     cp.dynResiduum.print();
00112 
00113 //     acadoPrintf("cp.lambdaDynamic = \n");
00114 //     cp.lambdaDynamic.print();
00115 
00116     if( eval.isDynamicNLP( ) == BT_TRUE ){
00117 
00118         (cp.lambdaDynamic.getAbsolute()^cp.dynResiduum.getAbsolute()).getSubBlock( 0, 0, tmp, 1, 1 );
00119         result += kappa*tmp(0,0);
00120     }
00121 
00122 //     acadoPrintf("result2 = %.16e \n", result );
00123 //
00124 //    --------
00125 //
00126 //
00127 //      acadoPrintf("cp.lambdaBound = \n");
00128 //     (cp.lambdaBound.absolute()       ).print();
00129 //      acadoPrintf("upperBoundRes = \n");
00130 //     (cp.upperBoundResiduum.negative()).print();
00131 //      acadoPrintf("lowerBoundRes = \n");
00132 //     (cp.lowerBoundResiduum.negative()).print();
00133 
00134 
00135     (cp.lambdaBound.getAbsolute()^cp.upperBoundResiduum.getNegative()).getSubBlock( 0, 0, tmp, 1, 1 );
00136     result -= kappa*tmp(0,0);
00137 
00138 //     acadoPrintf("result3 = %.16e \n", result );
00139 
00140     (cp.lambdaBound.getAbsolute()^cp.lowerBoundResiduum.getPositive()).getSubBlock( 0, 0, tmp, 1, 1 );
00141     result += kappa*tmp(0,0);
00142 
00143 //     acadoPrintf("result4 = %.16e \n", result );
00144 
00145     // --------
00146 
00147     (cp.lambdaConstraint.getAbsolute()^cp.upperConstraintResiduum.getNegative()).getSubBlock( 0, 0, tmp, 1, 1 );
00148     result -= kappa*tmp(0,0);
00149 
00150 //      acadoPrintf("result5 = %.16e \n", result );
00151 //
00152 //      acadoPrintf("cp.lambdaConstraint = \n");
00153 //      cp.lambdaConstraint.print();
00154 //
00155 //      acadoPrintf("cp.lambdaConstraint = \n");
00156 //      cp.lowerConstraintResiduum.print();
00157 
00158     (cp.lambdaConstraint.getAbsolute()^cp.lowerConstraintResiduum.getPositive()).getSubBlock( 0, 0, tmp, 1, 1 );
00159     result += kappa*tmp(0,0);
00160 
00161 //     acadoPrintf("result6 = %.16e \n", result );
00162 
00163     return SUCCESSFUL_RETURN;
00164 }
00165 
00166 
00167 
00168 
00169 CLOSE_NAMESPACE_ACADO
00170 
00171 // end of file.


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