examples/process/getting_started_discretized.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 
36 #include <acado_toolkit.hpp>
37 #include <acado_gnuplot.hpp>
38 
39 
40 int main( )
41 {
43 
44 
45  // INTRODUCE THE VARIABLES:
46  // -------------------------
51 
52  Control F;
53 
54 
55  // DEFINE A DIFFERENTIAL EQUATION:
56  // -------------------------------
57 
58  double h = 0.05;
60 
61 // f << next(xB) == ( 9.523918456856767e-01*xB - 3.093442425036754e-03*xW + 4.450257887258270e-04*vW - 2.380407715716160e-07*F );
62 // f << next(xW) == ( -1.780103154903307e+00*xB - 1.005721624707961e+00*xW - 3.093442425036752e-03*vW - 8.900515774516536e-06*F );
63 // f << next(vB) == ( -5.536210379145256e+00*xB - 2.021981836435758e-01*xW + 1.0*vB + 2.474992857984263e-02*vW + 1.294618052471308e-04*F );
64 // f << next(vW) == ( 1.237376970014700e+01*xB + 1.183104351525840e+01*xW - 1.005721624707961e+00*vW + 6.186884850073496e-05*F );
65 
66  f << next(xB) == ( 0.9335*xB + 0.0252*xW + 0.048860*vB + 0.000677*vW + 3.324e-06*F );
67  f << next(xW) == ( 0.1764*xB - 0.9821*xW + 0.004739*vB - 0.002591*vW - 8.822e-06*F );
68  f << next(vB) == ( -2.5210*xB - 0.1867*xW + 0.933500*vB + 0.025200*vW + 0.0001261*F );
69  f << next(vW) == ( -1.3070*xB + 11.670*xW + 0.176400*vB - 0.982100*vW + 6.536e-05*F );
70 
71  OutputFcn g;
72  g << xB;
73  g << 500.0*vB + F;
74 
75  DynamicSystem dynSys( f,g );
76 
77 
78  // SETUP THE PROCESS:
79  // ------------------
80  DVector mean( 1 ), amplitude( 1 );
81  mean.setZero( );
82  amplitude.setAll( 50.0 );
83 
84  GaussianNoise myNoise( mean,amplitude );
85 
86  Actuator myActuator( 1 );
87 
88  myActuator.setControlNoise( myNoise,0.1 );
89  myActuator.setControlDeadTimes( 0.1 );
90 
91 
92  mean.setZero( );
93  amplitude.setAll( 0.001 );
94  UniformNoise myOutputNoise1( mean,amplitude );
95 
96  mean.setAll( 20.0 );
97  amplitude.setAll( 10.0 );
98  GaussianNoise myOutputNoise2( mean,amplitude );
99 
100  Sensor mySensor( 2 );
101  mySensor.setOutputNoise( 0,myOutputNoise1,0.1 );
102  mySensor.setOutputNoise( 1,myOutputNoise2,0.1 );
103  mySensor.setOutputDeadTimes( 0.15 );
104 
105 
106  Process myProcess;
107 
108  myProcess.setDynamicSystem( dynSys );
109  myProcess.set( ABSOLUTE_TOLERANCE,1.0e-8 );
110 
111  myProcess.setActuator( myActuator );
112  myProcess.setSensor( mySensor );
113 
114  DVector x0( 4 );
115  x0.setZero( );
116  x0( 0 ) = 0.01;
117 
118  myProcess.initializeStartValues( x0 );
119 
120  myProcess.set( PLOT_RESOLUTION,HIGH );
121 // myProcess.set( CONTROL_PLOTTING,PLOT_NOMINAL );
122 // myProcess.set( PARAMETER_PLOTTING,PLOT_NOMINAL );
123  myProcess.set( OUTPUT_PLOTTING,PLOT_REAL );
124 
125  GnuplotWindow window;
126  window.addSubplot( xB, "Body Position [m]" );
127  window.addSubplot( xW, "Wheel Position [m]" );
128  window.addSubplot( vB, "Body Velocity [m/s]" );
129  window.addSubplot( vW, "Wheel Velocity [m/s]" );
130 
131  window.addSubplot( F,"Damping Force [N]" );
132  window.addSubplot( g(0),"Output 1" );
133  window.addSubplot( g(1),"Output 2" );
134 
135  myProcess << window;
136 
137 
138  // SIMULATE AND GET THE RESULTS:
139  // -----------------------------
140  VariablesGrid u( 1,0.0,1.0,6 );
141 
142  u( 0,0 ) = 10.0;
143  u( 1,0 ) = -200.0;
144  u( 2,0 ) = 200.0;
145  u( 3,0 ) = 200.0;
146  u( 4,0 ) = 0.0;
147  u( 5,0 ) = 0.0;
148 
149  myProcess.init( 0.0,x0,u.getFirstVector() );
150  myProcess.run( u );
151 
152 
153  VariablesGrid uNom, uSim, ySim, ySens, xSim;
154 
155 // myProcess.getLast( LOG_NOMINAL_CONTROLS,uNom ); uNom.print( "uNom" );
156 // myProcess.getLast( LOG_SIMULATED_CONTROLS,uSim ); uSim.print( "uSim" );
157 // myProcess.getLast( LOG_SIMULATED_OUTPUT,ySim ); ySim.print( "ySim" );
158 // myProcess.getLast( LOG_PROCESS_OUTPUT,ySens ); ySens.print( "ySens" );
159 //
160 // myProcess.getLast( LOG_DIFFERENTIAL_STATES,xSim );
161 
162 
163 
164  return 0;
165 }
166 
167 
168 
returnValue setActuator(const Actuator &_actuator)
Definition: process.cpp:254
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
Stores a DifferentialEquation together with an OutputFcn.
#define USING_NAMESPACE_ACADO
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Generates pseudo-random uniformly distributed noise for simulating the Process.
Allows to simulate the behaviour of sensors within the Process.
Definition: sensor.hpp:54
returnValue addSubplot(PlotWindowSubplot &_subplot)
returnValue initializeStartValues(const DVector &_xStart, const DVector &_xaStart=emptyConstVector)
Definition: process.cpp:330
Allows to setup and evaluate discretized differential equations based on SymbolicExpressions.
returnValue set(OptionsName name, int value)
Definition: options.cpp:126
DVector getFirstVector() const
returnValue setOutputNoise(const Noise &_noise, double _noiseSamplingTime)
Definition: sensor.cpp:81
Derived & setZero(Index size)
virtual returnValue run(const VariablesGrid &_u, const VariablesGrid &_p=emptyVariablesGrid)
Definition: process.cpp:632
Generates pseudo-random Gaussian noise for simulating the Process.
void setAll(const T &_value)
Definition: vector.hpp:160
Expression next(const Expression &arg)
Simulates the process to be controlled based on a dynamic model.
Definition: process.hpp:71
returnValue setControlNoise(const Noise &_noise, double _noiseSamplingTime)
Definition: actuator.cpp:89
Allows to simulate the behaviour of actuators within the Process.
Definition: actuator.hpp:54
Provides an interface to Gnuplot for plotting algorithmic outputs.
returnValue setDynamicSystem(const DynamicSystem &_dynamicSystem, IntegratorType _integratorType=INT_UNKNOWN)
Definition: process.cpp:204
returnValue setOutputDeadTimes(const DVector &_deadTimes)
Definition: sensor.cpp:125
returnValue setSensor(const Sensor &_sensor)
Definition: process.cpp:271
virtual returnValue init(double _startTime=0.0, const DVector &_xStart=emptyConstVector, const DVector &_uStart=emptyConstVector, const DVector &_pStart=emptyConstVector)
Definition: process.cpp:353
returnValue setControlDeadTimes(const DVector &_deadTimes)
Definition: actuator.cpp:176


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