operator.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 
00027 
00035 #include <acado/utils/acado_utils.hpp>
00036 #include <acado/symbolic_operator/symbolic_operator.hpp>
00037 #include <acado/symbolic_expression/symbolic_expression.hpp>
00038 #include <acado/symbolic_expression/constraint_component.hpp>
00039 
00040 
00041 BEGIN_NAMESPACE_ACADO
00042 
00043 
00044 //TreeProjection emptyTreeProjection;
00045 
00046 
00047 Operator::Operator(){
00048 
00049     nCount = 0;
00050     initialized = BT_FALSE;
00051 }
00052 
00053 Operator::~Operator(){ }
00054 
00055 
00056 Operator& Operator::operator=( const double &arg ){
00057 
00058     ACADOERROR( RET_UNKNOWN_BUG );
00059     ASSERT( 1 == 0 );
00060     return emptyTreeProjection;
00061 }
00062 
00063 Operator& Operator::operator=( const DVector &arg ){
00064 
00065     ACADOERROR( RET_UNKNOWN_BUG );
00066     ASSERT( 1 == 0 );
00067     return emptyTreeProjection;
00068 }
00069 
00070 Operator& Operator::operator=( const DMatrix &arg ){
00071 
00072     ACADOERROR( RET_UNKNOWN_BUG );
00073     ASSERT( 1 == 0 );
00074     return emptyTreeProjection;
00075 }
00076 
00077 Operator& Operator::operator=( const Expression &arg ){
00078 
00079     ACADOERROR( RET_UNKNOWN_BUG );
00080     ASSERT( 1 == 0 );
00081     return emptyTreeProjection;
00082 }
00083 
00084 Operator& Operator::operator=( const Operator &arg ){
00085 
00086     ACADOERROR( RET_UNKNOWN_BUG );
00087     ASSERT( 1 == 0 );
00088     return emptyTreeProjection;
00089 }
00090 
00091 
00092 TreeProjection* Operator::cloneTreeProjection() const{
00093 
00094     // if this routine is ever called something went
00095     // really wrong ....
00096 
00097     ACADOERROR( RET_UNKNOWN_BUG );
00098     ASSERT( 1 == 0 );
00099     return new TreeProjection();
00100 }
00101 
00102 
00103 int Operator::getGlobalIndex( ) const{
00104 
00105         ACADOERROR( RET_UNKNOWN_BUG );
00106     return -1;
00107 }
00108 
00109 
00110 
00111 Operator& Operator::operator+=( const double    & arg ){ return operator=( this->operator+(arg) ); }
00112 Operator& Operator::operator+=( const DVector    & arg ){ return operator=( this->operator+(arg) ); }
00113 Operator& Operator::operator+=( const DMatrix    & arg ){ return operator=( this->operator+(arg) ); }
00114 Operator& Operator::operator+=( const Expression& arg ){ return operator=( this->operator+(arg) ); }
00115 
00116 Operator& Operator::operator-=( const double      & arg ){ return operator=( this->operator-(arg) ); }
00117 Operator& Operator::operator-=( const DVector      & arg ){ return operator=( this->operator-(arg) ); }
00118 Operator& Operator::operator-=( const DMatrix      & arg ){ return operator=( this->operator-(arg) ); }
00119 Operator& Operator::operator-=( const Expression  & arg ){ return operator=( this->operator-(arg) ); }
00120 
00121 Operator& Operator::operator*=( const double      & arg ){ return operator=( this->operator*(arg) ); }
00122 Operator& Operator::operator*=( const DVector      & arg ){ return operator=( this->operator*(arg) ); }
00123 Operator& Operator::operator*=( const DMatrix      & arg ){ return operator=( this->operator*(arg) ); }
00124 Operator& Operator::operator*=( const Expression  & arg ){ return operator=( this->operator*(arg) ); }
00125 
00126 Operator& Operator::operator/=( const double      & arg ){ return operator=( this->operator/(arg) ); }
00127 Operator& Operator::operator/=( const Expression  & arg ){ return operator=( this->operator/(arg) ); }
00128 
00129 
00130 
00131 
00132 Expression Operator::operator+( const double        & arg ) const{ return Expression(*this)+arg; }
00133 Expression Operator::operator+( const DVector        & arg ) const{ return Expression(*this)+arg; }
00134 Expression Operator::operator+( const DMatrix        & arg ) const{ return Expression(*this)+arg; }
00135 Expression Operator::operator+( const Operator& arg ) const{ return Expression(*this)+arg; }
00136 Expression Operator::operator+( const Expression    & arg ) const{ return Expression(*this)+arg; }
00137 
00138 Expression operator+( const double & arg1, const Operator& arg2 ){ return arg1 + Expression(arg2); }
00139 Expression operator+( const DVector & arg1, const Operator& arg2 ){ return arg1 + Expression(arg2); }
00140 Expression operator+( const DMatrix & arg1, const Operator& arg2 ){ return arg1 + Expression(arg2); }
00141 
00142 Expression Operator::operator-( const double        & arg ) const{ return Expression(*this)-arg; }
00143 Expression Operator::operator-( const DVector        & arg ) const{ return Expression(*this)-arg; }
00144 Expression Operator::operator-( const DMatrix        & arg ) const{ return Expression(*this)-arg; }
00145 Expression Operator::operator-( const Operator& arg ) const{ return Expression(*this)-arg; }
00146 Expression Operator::operator-( const Expression    & arg ) const{ return Expression(*this)-arg; }
00147 
00148 Expression Operator::operator-( ) const{ return -Expression(*this); }
00149 
00150 Expression operator-( const double & arg1, const Operator& arg2 ){ return arg1 - Expression(arg2); }
00151 Expression operator-( const DVector & arg1, const Operator& arg2 ){ return arg1 - Expression(arg2); }
00152 Expression operator-( const DMatrix & arg1, const Operator& arg2 ){ return arg1 - Expression(arg2); }
00153 
00154 Expression Operator::operator*( const double        & arg ) const{ return Expression(*this)*arg; }
00155 Expression Operator::operator*( const DVector        & arg ) const{ return Expression(*this)*arg; }
00156 Expression Operator::operator*( const DMatrix        & arg ) const{ return Expression(*this)*arg; }
00157 
00158 
00159 Expression Operator::operator*( const Operator& arg ) const{
00160 
00161     Expression tmp2(arg);
00162 
00163 
00164     Expression tmp1(*this);
00165 
00166 
00167     return tmp1*tmp2;
00168 }
00169 
00170 
00171 Expression Operator::operator*( const Expression    & arg ) const{ return Expression(*this)*arg; }
00172 
00173 Expression operator*( const double & arg1, const Operator& arg2 ){ return arg1 * Expression(arg2); }
00174 Expression operator*( const DVector & arg1, const Operator& arg2 ){ return arg1 * Expression(arg2); }
00175 Expression operator*( const DMatrix & arg1, const Operator& arg2 ){ return arg1 * Expression(arg2); }
00176 
00177 Expression Operator::operator/( const double        & arg ) const{ return Expression(*this)/arg; }
00178 Expression Operator::operator/( const Operator& arg ) const{ return Expression(*this)/arg; }
00179 Expression Operator::operator/( const Expression    & arg ) const{ return Expression(*this)/arg; }
00180 
00181 Expression operator/( const double & arg1, const Operator& arg2 ){ return arg1 / Expression(arg2); }
00182 Expression operator/( const DVector & arg1, const Operator& arg2 ){ return arg1 / Expression(arg2); }
00183 Expression operator/( const DMatrix & arg1, const Operator& arg2 ){ return arg1 / Expression(arg2); }
00184 
00185 ConstraintComponent Operator::operator<=( const double& ub ) const{ return Expression(*this) <= ub; }
00186 ConstraintComponent Operator::operator>=( const double& lb ) const{ return Expression(*this) >= lb; }
00187 ConstraintComponent Operator::operator==( const double&  b ) const{ return Expression(*this) ==  b; }
00188 
00189 ConstraintComponent Operator::operator<=( const DVector& ub ) const{ return Expression(*this) <= ub; }
00190 ConstraintComponent Operator::operator>=( const DVector& lb ) const{ return Expression(*this) >= lb; }
00191 ConstraintComponent Operator::operator==( const DVector&  b ) const{ return Expression(*this) ==  b; }
00192 
00193 ConstraintComponent Operator::operator<=( const VariablesGrid& ub ) const{ return Expression(*this) <= ub; }
00194 ConstraintComponent Operator::operator>=( const VariablesGrid& lb ) const{ return Expression(*this) >= lb; }
00195 ConstraintComponent Operator::operator==( const VariablesGrid&  b ) const{ return Expression(*this) ==  b; }
00196 
00197 ConstraintComponent operator<=( double lb, const Operator &arg ){ return lb <= Expression(arg); }
00198 ConstraintComponent operator==( double  b, const Operator &arg ){ return  b == Expression(arg); }
00199 ConstraintComponent operator>=( double ub, const Operator &arg ){ return ub >= Expression(arg); }
00200 
00201 ConstraintComponent operator<=( DVector lb, const Operator &arg ){ return lb <= Expression(arg); }
00202 ConstraintComponent operator==( DVector  b, const Operator &arg ){ return  b == Expression(arg); }
00203 ConstraintComponent operator>=( DVector ub, const Operator &arg ){ return ub >= Expression(arg); }
00204 
00205 ConstraintComponent operator<=( VariablesGrid lb, const Operator &arg ){ return lb <= Expression(arg); }
00206 ConstraintComponent operator==( VariablesGrid  b, const Operator &arg ){ return  b == Expression(arg); }
00207 ConstraintComponent operator>=( VariablesGrid ub, const Operator &arg ){ return ub >= Expression(arg); }
00208 
00209 
00210 
00211 double Operator::getValue() const{ return INFTY; }
00212 
00213 std::ostream& operator<<(std::ostream &stream, const Operator &arg)
00214 {
00215         return arg.print( stream );
00216 }
00217 
00218 Operator* Operator::passArgument() const{
00219 
00220     return 0;
00221 }
00222 
00223 returnValue Operator::setVariableExportName(    const VariableType &_type,
00224                                                                                                 const std::vector< std::string >& _name
00225                                                                                                 )
00226 {
00227         return SUCCESSFUL_RETURN;
00228 }
00229 
00230 
00231 
00232 Operator* Operator::myProd(Operator* a,Operator* b){
00233 
00234     if( a->isOneOrZero() == NE_ZERO ) return new DoubleConstant( 0.0 , NE_ZERO );
00235     if( b->isOneOrZero() == NE_ZERO ) return new DoubleConstant( 0.0 , NE_ZERO );
00236     
00237     if( a->isOneOrZero() == NE_ONE  ) return b->clone();
00238     if( b->isOneOrZero() == NE_ONE  ) return a->clone();
00239     
00240     if( a == b ) return new Power_Int( a->clone(), 2 );
00241     
00242     return new Product(a->clone(),b->clone()); 
00243 }
00244   
00245 
00246 Operator* Operator::myAdd (Operator* a,Operator* b){
00247 
00248     if( a->isOneOrZero() == NE_ZERO && b->isOneOrZero() == NE_ZERO )
00249         return new DoubleConstant( 0.0 , NE_ZERO );
00250 
00251     if( a->isOneOrZero() == NE_ZERO ) return b->clone();
00252     if( b->isOneOrZero() == NE_ZERO ) return a->clone();
00253     
00254     if( a->isOneOrZero() == NE_ONE && b->isOneOrZero() == NE_ONE )
00255         return new DoubleConstant( 2.0 , NE_NEITHER_ONE_NOR_ZERO );
00256     
00257     if( a == b ) return new Product( a->clone(), new DoubleConstant( 2.0 , NE_NEITHER_ONE_NOR_ZERO ) );
00258     
00259     return new Addition(a->clone(),b->clone());
00260 }
00261 
00262 
00263 Operator* Operator::mySubtract (Operator* a,Operator* b){
00264 
00265     if( a->isOneOrZero() == NE_ZERO && b->isOneOrZero() == NE_ZERO )
00266         return new DoubleConstant( 0.0 , NE_ZERO );
00267 
00268     if( a->isOneOrZero() == NE_ZERO ) return new Subtraction(new DoubleConstant( 0.0 , NE_ZERO ), b->clone());
00269     if( b->isOneOrZero() == NE_ZERO ) return a->clone();
00270 
00271     if( a->isOneOrZero() == NE_ONE && b->isOneOrZero() == NE_ONE )
00272         return new DoubleConstant( 0.0 , NE_ZERO );
00273 
00274     if( a == b ) return new DoubleConstant( 0.0 , NE_ZERO );
00275 
00276     return new Subtraction(a->clone(),b->clone());
00277 }
00278 
00279 
00280 Operator* Operator::myPower (Operator* a,Operator* b){
00281 
00282     if( a->isOneOrZero() == NE_ZERO && b->isOneOrZero() == NE_ONE ) return new DoubleConstant( 0.0 , NE_ZERO );
00283 
00284     if( b->isOneOrZero() == NE_ZERO ) return new DoubleConstant( 1.0 , NE_ONE );
00285     if( a->isOneOrZero() == NE_ONE ) return new DoubleConstant( 1.0 , NE_ONE );
00286 
00287     if( b->isOneOrZero() == NE_ONE ) return a->clone();
00288 
00289     return new Power(a->clone(),b->clone());
00290 }
00291 
00292 
00293 Operator* Operator::myPowerInt (Operator* a, int b){
00294 
00295         if( a->isOneOrZero() == NE_ZERO && b>0 )        return new DoubleConstant( 0.0 , NE_ZERO );
00296         if( b == 0 )                                                            return new DoubleConstant( 1.0 , NE_ONE );
00297         if( a->isOneOrZero() == NE_ONE )                        return new DoubleConstant( 1.0 , NE_ONE );
00298         if( b == 1 )                                                            return a->clone();
00299 
00300     return new Power_Int(a->clone(),b);
00301 }
00302 
00303 
00304 Operator* Operator::myLogarithm (Operator* a){
00305 
00306     if( a->isOneOrZero() == NE_ONE ) return new DoubleConstant( 0.0 , NE_ZERO );
00307 
00308     return new Logarithm(a->clone());
00309 }
00310 
00311 
00312 TreeProjection* Operator::convert2TreeProjection( Operator* a ) const{
00313   
00314    TreeProjection b;
00315    b = *a;
00316    delete a;
00317    return b.clone();
00318 }
00319 
00320 
00321 returnValue Operator::ADsymCommon( Operator     *a  ,
00322                                         TreeProjection &da ,
00323                                         TreeProjection &dda,
00324                                         int            dim       , 
00325                                         VariableType  *varType   , 
00326                                         int           *component , 
00327                                         Operator      *l         , 
00328                                         Operator     **S         , 
00329                                         int            dimS      , 
00330                                         Operator     **dfS       , 
00331                                         Operator     **ldf       , 
00332                                         Operator     **H         , 
00333                                         int            &nNewLIS  , 
00334                                         TreeProjection ***newLIS , 
00335                                         int            &nNewSIS  , 
00336                                         TreeProjection ***newSIS , 
00337                                         int            &nNewHIS  , 
00338                                         TreeProjection ***newHIS    ){
00339 
00340   // FIRST ORDER BACKWARD SWEEP:
00341   // ---------------------------
00342   
00343     a->AD_symmetric( dim, varType, component,
00344                     convert2TreeProjection(myProd(l,&da)),
00345                     S, dimS, dfS, ldf, H, nNewLIS, newLIS, nNewSIS, newSIS, nNewHIS, newHIS 
00346               );
00347     
00348     
00349   // SECOND ORDER FORWARD SWEEP:
00350   // -------------------------------------
00351     Operator *tmp3 = convert2TreeProjection(myProd(&dda,l));
00352 
00353     int run1, run2;
00354     for( run1 = 0; run1 < dimS; run1++ ){
00355         for( run2 = 0; run2 <= run1; run2++ ){
00356                 Operator *tmp1 = H[run1*dimS+run2]->clone();
00357                 delete H[run1*dimS+run2];
00358                 Operator *tmp2 = myProd( dfS[run1], dfS[run2] );
00359                 Operator *tmp4 = myProd( tmp2     , tmp3      );
00360                 H[run1*dimS+run2] = myAdd( tmp1, tmp4 );
00361                 delete tmp1;
00362                 delete tmp2;
00363                 delete tmp4;
00364         }
00365     }
00366     delete tmp3;
00367     
00368     
00369   // FIRST ORDER FORWARD SWEEP:
00370   // -------------------------------------
00371     
00372     for( run1 = 0; run1 < dimS; run1++ ){
00373         Operator *tmp1 = dfS[run1]->clone();
00374         delete dfS[run1];
00375         dfS[run1] = convert2TreeProjection( myProd( tmp1, &da ) );
00376         delete tmp1;
00377     }
00378     
00379   // CLEAR MEMORY FROM BACKWARD SWEEP:
00380   // -------------------------------------
00381     
00382     delete l;
00383     return SUCCESSFUL_RETURN;
00384 }
00385 
00386 
00387 returnValue Operator::ADsymCommon2( Operator       *a  ,
00388                                                                            Operator       *b  ,
00389                                                                            TreeProjection &dx ,
00390                                                                            TreeProjection &dy ,
00391                                                                            TreeProjection &dxx,
00392                                                                            TreeProjection &dxy,
00393                                                                            TreeProjection &dyy,
00394                                        int            dim       , 
00395                                        VariableType  *varType   , 
00396                                        int           *component , 
00397                                        Operator      *l         , 
00398                                        Operator     **S         , 
00399                                        int            dimS      , 
00400                                        Operator     **dfS       , 
00401                                        Operator     **ldf       , 
00402                                        Operator     **H         , 
00403                                        int            &nNewLIS  , 
00404                                        TreeProjection ***newLIS , 
00405                                        int            &nNewSIS  , 
00406                                        TreeProjection ***newSIS , 
00407                                        int            &nNewHIS  , 
00408                                        TreeProjection ***newHIS    ){
00409   
00410     int run1, run2;
00411   
00412   // FIRST ORDER BACKWARD SWEEP:
00413   // ---------------------------
00414     Operator    **S1 = new Operator*[dimS];
00415     Operator    **S2 = new Operator*[dimS];
00416     Operator    **H1 = new Operator*[dimS*dimS];
00417     Operator    **H2 = new Operator*[dimS*dimS];
00418   
00419     for( run2 = 0; run2 < dimS; run2++ ){
00420          S1[run2] = new DoubleConstant(0.0,NE_ZERO);
00421          S2[run2] = new DoubleConstant(0.0,NE_ZERO);
00422     }
00423     
00424     for( run2 = 0; run2 < dimS*dimS; run2++ ){
00425          H1[run2] = new DoubleConstant(0.0,NE_ZERO);
00426          H2[run2] = new DoubleConstant(0.0,NE_ZERO);
00427     }
00428     
00429     a->AD_symmetric( dim, varType, component, convert2TreeProjection(myProd(l,&dx)),
00430                     S, dimS, S1, ldf, H1, nNewLIS, newLIS, nNewSIS, newSIS, nNewHIS, newHIS );
00431 
00432 
00433     b->AD_symmetric( dim, varType, component, convert2TreeProjection(myProd(l,&dy)),
00434                     S, dimS, S2, ldf, H2, nNewLIS, newLIS, nNewSIS, newSIS, nNewHIS, newHIS );
00435     
00436    
00437     
00438   // SECOND ORDER FORWARD SWEEP:
00439   // -------------------------------------
00440     
00441     Operator *tmpXX = convert2TreeProjection(myProd( l,&dxx ));
00442     Operator *tmpXY = convert2TreeProjection(myProd( l,&dxy ));
00443     Operator *tmpYY = convert2TreeProjection(myProd( l,&dyy ));
00444     
00445     for( run1 = 0; run1 < dimS; run1++ ){
00446         for( run2 = 0; run2 <= run1; run2++ ){
00447                 delete H[run1*dimS+run2];
00448                 Operator *tmp1 = myProd( S1[run1], S1[run2] );
00449                 Operator *tmp2 = myProd( S1[run1], S2[run2] );
00450                 Operator *tmp3 = myProd( S2[run1], S1[run2] );
00451                 Operator *tmp4 = myProd( S2[run1], S2[run2] );
00452                 Operator *tmp5;
00453                 if( run1 == run2 ) tmp5 = myAdd(tmp2,tmp2);
00454                 else               tmp5 = myAdd(tmp2,tmp3);
00455                 Operator *tmp6 = myProd( tmp1, tmpXX );
00456                 Operator *tmp7 = myProd( tmp5, tmpXY );
00457                 Operator *tmp8 = myProd( tmp4, tmpYY );
00458                 Operator *tmp9  = myAdd ( tmp6, tmp7 );
00459                 Operator *tmp10 = myAdd ( tmp8, tmp9 );
00460                 Operator *tmp12 = myAdd ( tmp10 , H1[run1*dimS+run2] );
00461                 H[run1*dimS+run2] = myAdd ( tmp12 , H2[run1*dimS+run2] );
00462                 delete tmp1;
00463                 delete tmp2;
00464                 delete tmp3;
00465                 delete tmp4;
00466                 delete tmp5;
00467                 delete tmp6;
00468                 delete tmp7;
00469                 delete tmp8;
00470                 delete tmp9;
00471                 delete tmp10;
00472                 delete tmp12;
00473         }
00474     }
00475     
00476     delete tmpXX;
00477     delete tmpXY;
00478     delete tmpYY;
00479     
00480     
00481   // FIRST ORDER FORWARD SWEEP:
00482   // -------------------------------------
00483     
00484     for( run1 = 0; run1 < dimS; run1++ ){
00485         delete dfS[run1];
00486         Operator *tmp1 = convert2TreeProjection( myProd( S1[run1], &dx ) );
00487         Operator *tmp2 = convert2TreeProjection( myProd( S2[run1], &dy ) );
00488         dfS[run1] = myAdd(tmp1,tmp2);
00489         delete tmp1;
00490         delete tmp2;
00491     }
00492     
00493   // CLEAR MEMORY FROM SWEEPS:
00494   // -------------------------------------
00495     
00496     for( run2 = 0; run2 < dimS; run2++ ){
00497         delete S1[run2];
00498         delete S2[run2];
00499     }
00500     for( run2 = 0; run2 < dimS*dimS; run2++ ){
00501         delete H1[run2];
00502         delete H2[run2];
00503     }
00504     delete[] S1;
00505     delete[] S2;
00506     delete[] H1;
00507     delete[] H2;
00508 
00509     delete l;
00510     return SUCCESSFUL_RETURN;
00511 }
00512 
00513 
00514 BooleanType Operator::isTrivial() const {
00515         return BT_FALSE;
00516 }
00517 
00518 
00519 returnValue Operator::initDerivative() {
00520 
00521         initialized = BT_TRUE;
00522         return SUCCESSFUL_RETURN;
00523 }
00524 
00525 
00526 
00527 CLOSE_NAMESPACE_ACADO
00528 
00529 
00530 // end of file.


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