pendulum_C.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 
27 
35 #include <acado_integrators.hpp>
36 
37 
39 
40 
41 void ffcn_model( double *x, double *f, void *user_data ){
42 
43 // double time = x[ 0]; // the time
44  double phi = x[ 1]; // the angle phi
45  double dphi = x[ 2]; // the first derivative of phi w.r.t time
46  double F = x[ 3]; // a force acting on the pendulum
47  double l = x[ 4]; // the length of the pendulum
48 
49  const double m = 1.0 ; // the mass of the pendulum
50  const double g = 9.81 ; // the gravitational constant
51  const double alpha = 2.0 ; // frictional constant
52 
53  f[0] = dphi;
54  f[1] = -(m*g/l)*sin(phi) - alpha*dphi + F/(m*l);
55 }
56 
57 
58 int main( ){
59 
60  DifferentialState phi, dphi;
61 
62  Control u;
63  Parameter p;
64  TIME t;
65 
66  IntermediateState x(5);
67 
68  x(0) = t ;
69  x(1) = phi ;
70  x(2) = dphi;
71  x(3) = u ;
72  x(4) = p ;
73 
74  CFunction pendulumModel( 2, ffcn_model );
75 
76  // Define a Right-Hand-Side:
77  // -------------------------
78 
80 
81  f << pendulumModel(x);
82 
83 
84  // DEFINE AN INTEGRATOR:
85  // ---------------------
86 
87  IntegratorRK45 integrator( f );
88 
89  integrator.set(INTEGRATOR_PRINTLEVEL, HIGH );
90 
91 
92  // DEFINE INITIAL VALUES:
93  // ----------------------
94 
95  double x_start[2] = { 0.0, 0.0 };
96  double u_ [1] = { 1.0 };
97  double p_ [1] = { 1.0 };
98 
99  double t_start = 0.0;
100  double t_end = 1.0;
101 
102 
103  // START THE INTEGRATION:
104  // ----------------------
105 
106  integrator.freezeAll();
107  integrator.integrate( t_start, t_end, x_start, 0, p_, u_ );
108 
109 
110  // DEFINE A SEED MATRIX:
111  // ---------------------
112  DVector seed1(2);
113  DVector seed2(2);
114 
115  seed1(0) = 1.0;
116  seed1(1) = 0.0;
117 
118  seed2(0) = 1.0;
119  seed2(1) = 0.0;
120 
121  // COMPUTE FIRST ORDER DERIVATIVES:
122  // --------------------------------
123  integrator.setForwardSeed(1,seed1);
124  integrator.integrateSensitivities();
125 
126  // COMPUTE SECOND ORDER DERIVATIVES:
127  // ---------------------------------
128  integrator.setForwardSeed(2,seed2);
129  integrator.integrateSensitivities();
130 
131 
132  // GET THE RESULTS
133  // ---------------
134 
135  VariablesGrid differentialStates;
136  integrator.getX( differentialStates );
137 
138  DVector Dx( 2 ), DDx( 2 );
139  integrator.getForwardSensitivities( Dx,1 );
140  integrator.getForwardSensitivities( DDx,2 );
141 
142  differentialStates.print( "x" );
143  Dx.print( "Dx" );
144  DDx.print( "DDx" );
145 
146 
147  return 0;
148 }
149 
150 
151 
int main()
Definition: pendulum_C.cpp:58
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
returnValue print(std::ostream &stream=std::cout, const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
returnValue getX(DVector &xEnd) const
returnValue set(OptionsName name, int value)
USING_NAMESPACE_ACADO void ffcn_model(double *x, double *f, void *user_data)
Definition: pendulum_C.cpp:41
#define USING_NAMESPACE_ACADO
Provides a time grid consisting of vector-valued optimization variables at each grid point...
returnValue setForwardSeed(const int &order, const DVector &xSeed, const DVector &pSeed=emptyVector, const DVector &uSeed=emptyVector, const DVector &wSeed=emptyVector)
Definition: integrator.cpp:308
virtual returnValue print(std::ostream &stream=std::cout, const std::string &name=DEFAULT_LABEL, const std::string &startString=DEFAULT_START_STRING, const std::string &endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const std::string &colSeparator=DEFAULT_COL_SEPARATOR, const std::string &rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: vector.cpp:97
virtual returnValue freezeAll()
Implements the Runge-Kutta-45 scheme for integrating ODEs.
const double t_end
const double t_start
(no description yet)
Definition: c_function.hpp:54
returnValue integrateSensitivities()
Definition: integrator.cpp:357
returnValue getForwardSensitivities(DVector &Dx, int order) const
Definition: integrator.cpp:406
returnValue integrate(double t0, double tend, double *x0, double *xa=0, double *p=0, double *u=0, double *w=0)
Definition: integrator.cpp:207
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.


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