acado_syntax.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 
34 
36 
37 
38 // ------------------------------------------------------------------------------------
39 // STANDARD OPERATORS:
40 // ------------------------------------------------------------------------------------
41 
42 
43 IntermediateState sin ( const Expression &arg ){ return arg.getSin(); }
44 IntermediateState cos ( const Expression &arg ){ return arg.getCos (); }
45 IntermediateState tan ( const Expression &arg ){ return arg.getTan (); }
46 IntermediateState asin( const Expression &arg ){ return arg.getAsin(); }
47 IntermediateState acos( const Expression &arg ){ return arg.getAcos(); }
48 IntermediateState atan( const Expression &arg ){ return arg.getAtan(); }
49 IntermediateState exp ( const Expression &arg ){ return arg.getExp (); }
50 IntermediateState sqrt( const Expression &arg ){ return arg.getSqrt(); }
51 IntermediateState ln ( const Expression &arg ){ return arg.getLn (); }
52 IntermediateState log ( const Expression &arg ){ return arg.getLn (); }
53 
54 IntermediateState pow( const Expression &arg1, const Expression &arg2 ){
55 
56  return arg1.getPow(arg2);
57 }
58 
59 IntermediateState pow( const double &arg1, const Expression &arg2 ){
60 
61  return Expression( arg1 ).getPow( arg2 );
62 }
63 
64 IntermediateState pow( const Expression &arg1, const double &arg2 ){
65 
66  if( fabs( arg2 - floor(arg2) ) <= 10.0*EPS ){
67  int intarg = (int) floor(arg2);
68  return arg1.getPowInt( intarg );
69  }
70  if( fabs( arg2 - ceil(arg2) ) <= 10.0*EPS ){
71  int intarg = (int) ceil(arg2);
72  return arg1.getPowInt( intarg );
73  }
74  return arg1.getPow( arg2 );
75 }
76 
77 
78 
79 // ---------------------------------------------------------------------------------------------
80 // SPECIAL CONVEX DICIPLINED PROGRAMMING FUNCTIONS:
81 // ---------------------------------------------------------------------------------------------
82 
83 
84 IntermediateState square ( const Expression &arg ){ return arg.getSumSquare (); }
85 IntermediateState sum_square ( const Expression &arg ){ return arg.getSumSquare (); }
86 IntermediateState log_sum_exp ( const Expression &arg ){ return arg.getLogSumExp (); }
88 IntermediateState entropy ( const Expression &arg ){ return arg.getEntropy (); }
89 
90 
91 
92 // ---------------------------------------------------------------------------------------------
93 // SPECIAL ROUTINES FOR THE SET UP OF DYNAMIC SYSTEMS:
94 // ---------------------------------------------------------------------------------------------
95 
96 
97 
98 Expression dot ( const Expression &arg ){ return arg.getDot (); }
99 Expression next( const Expression &arg ){ return arg.getNext(); }
100 
101 // ---------------------------------------------------------------------------------------------
102 // SYMBOLIC DERIVATIVE OPERATORS:
103 // ---------------------------------------------------------------------------------------------
104 
105 
107  const Expression &arg2 ){
108 
109  return arg1.ADforward(arg2);
110 }
111 
112 
114  const Expression &arg2 ){
115 
116  return arg1.ADbackward(arg2);
117 }
118 
119 
121  const Expression &arg2,
122  const Expression &seed ){
123 
124  return arg1.ADforward(arg2,seed);
125 }
126 
127 
129  const Expression &arg2,
130  const Expression &seed ){
131 
132  Expression tmp;
133  for( uint i = 0; i < seed.getNumCols(); i++ ) {
134  tmp.appendCols( forwardDerivative( arg1, arg2, seed.getCol(i) ) );
135  }
136  return tmp;
137 }
138 
139 
141  const Expression &arg2,
142  const Expression &seed ){
143 
144  return arg1.ADbackward(arg2,seed);
145 }
146 
147 
149  const Expression &arg2,
150  const Expression &seed ){
151 
152  Expression tmp;
153  for( uint i = 0; i < seed.getNumCols(); i++ ) {
154  tmp.appendCols( backwardDerivative( arg1, arg2, seed.getCol(i) ) );
155  }
156  return tmp;
157 }
158 
160  const Expression &arg2,
161  const Expression &forward_seed,
162  const Expression &backward_seed,
163  Expression *forward_result,
164  Expression *backward_result ) {
165  return arg1.ADsymmetric( arg2, forward_seed, backward_seed, forward_result, backward_result );
166 }
167 
168 
170  const Expression &arg2 ){
171 
172  return backwardDerivative( arg1, arg2 );
173 }
174 
175 
177  const Expression &arg2 ){
178 
179  return forwardDerivative( forwardDerivative(arg1,arg2), arg2 );
180 }
181 
183  const DifferentialState &x ,
184  const Control &u ,
185  const DifferentialState &P ,
186  const DMatrix &Q ,
187  const DMatrix &R ){
188 
189  IntermediateState RHS("", x.getDim(), x.getDim());
190 
192  IntermediateState B = forwardDerivative( rhs, u );
193 
194  return A.transpose()*P + P*A + Q - P*B*(DMatrix(R.inverse()))*B.transpose()*P;
195 }
196 
197 
198 Expression chol( const Expression &arg ){
199 
200  // --------------------------------------------------
201  // COMPUTES THE CHOLESKY FACTOR L OF THE ARGUMENT
202  // SUCH THAT
203  //
204  // ARG = L * L.transpose
205  //
206  // WHERE THE MATRIX L IS RETURNED.
207  // THIS IS A SYMBOLIC CHOLESKY ROUTINE AS ACADO
208  // SIMPLIFIES PRODUCTS WITH ZERO AUTOMATICALLY.
209  //
210  // CAUTION: THE ROUTINE WORKS ON A SYMBOLIC LEVEL
211  // ALWAYS WITHOUT DETECTING PSD-MATRICES.
212  // HOWEVER, THE EVEALUATION WILL ONLY WORK
213  // FOR SYMMETRIC AND POSITIVE SEMI-DEFINITE
214  // MATRICES.
215  // THE SAFE-GUARD CONSTANT IS 100*EPS.
216  // --------------------------------------------------
217 
218 
219  ASSERT( arg.getNumRows() == arg.getNumCols() );
220 
221  int dim = arg.getNumRows();
222  IntermediateState L("", dim,dim);
223 
224  // COMPUTE THE LOWER TRIANGLE RECURSIVELY:
225  // ---------------------------------------
226 
227  int i,j,k;
228 
229  for( j = 0; j < dim; j++ ){
230  L(j,j) = arg(j,j) ;
231  for( k = 0; k < j; k++ ) L(j,j) -= L(j,k)*L(j,k) ;
232  L(j,j) = sqrt( L(j,j) + 100.0*EPS );
233  for( i = j+1; i < dim; i++ ){
234  L(i,j) = arg(i,j) ;
235  for( k = 0; k < j; k++ ) L(i,j) -= L(i,k)*L(j,k) ;
236  L(i,j) = L(i,j)/(L(j,j) + 100.0*EPS );
237  }
238  }
239 
240  for( j = 0; j < dim; j++ )
241  for( k = j+1; k < dim; k++ )
242  L(j,k) = 0.0;
243 
244  return L;
245 }
246 
247 
249 {
250  AlgebraicState dummy1;
251  Control dummy2;
252  DifferentialState dummy3;
254  Disturbance dummy5;
255  IntegerControl dummy6;
256  IntegerParameter dummy7;
257  IntermediateState dummy8;
258  Parameter dummy9;
259  OnlineData dummy10;
260 
261  dummy1.clearStaticCounters();
262  dummy2.clearStaticCounters();
263  dummy3.clearStaticCounters();
264  dummy4.clearStaticCounters();
265  dummy5.clearStaticCounters();
266  dummy6.clearStaticCounters();
267  dummy7.clearStaticCounters();
268  dummy8.clearStaticCounters();
269  dummy9.clearStaticCounters();
270  dummy10.clearStaticCounters();
271 
272  return SUCCESSFUL_RETURN;
273 }
274 
275 
276 // end of file.
277 
278 
Expression getAtan() const
Definition: expression.cpp:733
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
Expression symmetricDerivative(const Expression &arg1, const Expression &arg2, const Expression &forward_seed, const Expression &backward_seed, Expression *forward_result, Expression *backward_result)
Expression getEntropy() const
Definition: expression.cpp:945
IntermediateState sqrt(const Expression &arg)
Expression getPow(const Expression &arg) const
Definition: expression.cpp:782
IntermediateState atan(const Expression &arg)
Expression backwardDerivative(const Expression &arg1, const Expression &arg2)
Expression getRiccatiODE(const Expression &rhs, const DifferentialState &x, const Control &u, const DifferentialState &P, const DMatrix &Q, const DMatrix &R)
Expression ADsymmetric(const Expression &arg, const Expression &S, const Expression &l, Expression *dfS=0, Expression *ldf=0) const
Expression getCol(const uint &colIdx) const
Definition: expression.cpp:567
BEGIN_NAMESPACE_ACADO const double EPS
#define USING_NAMESPACE_ACADO
Allows to pass back messages to the calling function.
Expression chol(const Expression &arg)
Expression forwardDerivative(const Expression &arg1, const Expression &arg2)
IntermediateState log_sum_exp(const Expression &arg)
uint getNumCols() const
uint getNumRows() const
IntermediateState asin(const Expression &arg)
IntermediateState pow(const Expression &arg1, const Expression &arg2)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
IntermediateState tan(const Expression &arg)
Expression getLogSumExp() const
Definition: expression.cpp:860
Expression getDot() const
Definition: expression.cpp:952
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
Expression getSqrt() const
Definition: expression.cpp:757
Expression laplace(const Expression &arg1, const Expression &arg2)
IntermediateState cos(const Expression &arg)
IntermediateState sum_square(const Expression &arg)
IntermediateState euclidean_norm(const Expression &arg)
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
Expression getPowInt(const int &arg) const
Definition: expression.cpp:797
IntermediateState entropy(const Expression &arg)
Expression getExp() const
Definition: expression.cpp:745
Expression getTan() const
Definition: expression.cpp:697
Expression getSumSquare() const
Definition: expression.cpp:828
Expression jacobian(const Expression &arg1, const Expression &arg2)
Expression ADforward(const Expression &arg) const
Definition: expression.cpp:971
Expression getAsin() const
Definition: expression.cpp:709
IntermediateState acos(const Expression &arg)
Expression multipleForwardDerivative(const Expression &arg1, const Expression &arg2, const Expression &seed)
IntermediateState square(const Expression &arg)
void rhs(const real_t *x, real_t *f)
Expression dot(const Expression &arg)
#define L
Expression transpose() const
Definition: expression.cpp:811
Expression getNext() const
Definition: expression.cpp:965
returnValue clearAllStaticCounters()
#define ASSERT(x)
Expression getAcos() const
Definition: expression.cpp:721
IntermediateState ln(const Expression &arg)
Expression getSin() const
Definition: expression.cpp:673
Expression next(const Expression &arg)
Expression getLn() const
Definition: expression.cpp:769
returnValue clearStaticCounters()
Definition: expression.hpp:398
Expression getEuclideanNorm() const
Definition: expression.cpp:912
IntermediateState exp(const Expression &arg)
Expression ADbackward(const Expression &arg) const
Expression & appendCols(const Expression &arg)
Definition: expression.cpp:162
Expression multipleBackwardDerivative(const Expression &arg1, const Expression &arg2, const Expression &seed)
Expression getCos() const
Definition: expression.cpp:685
#define R
uint getDim() const
IntermediateState log(const Expression &arg)


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