crane_mpc3.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 
36 #include <acado_toolkit.hpp>
37 #include <acado_gnuplot.hpp>
38 
39 
40 int main( ){
41 
43 
44  // VARIABLES:
45  // ------------------------
46  DifferentialState x; // Position of the trolley
47  DifferentialState v; // Velocity of the trolley
48  DifferentialState phi; // excitation angle
49  DifferentialState dphi; // rotational velocity
50 
51  Control ax; // trolley accelaration
52  Disturbance W; // disturbance
53 
54  double L = 1.0 ; // length
55  double m = 1.0 ; // mass
56  double g = 9.81; // gravitational constant
57  double b = 0.2 ; // friction coefficient
58 
59 
60  // DIFFERENTIAL EQUATION:
61  // ------------------------
62  DifferentialEquation f, fSim; // The model equations
63 
64  f << dot(x) == v;
65  f << dot(v) == ax;
66  f << dot(phi ) == dphi;
67  f << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
68 
69  L = 1.2; // introduce model plant mismatch
70 
71  fSim << dot(x) == v;
72  fSim << dot(v) == ax + W;
73  fSim << dot(phi ) == dphi;
74  fSim << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
75 
76 
77  // DEFINE LEAST SQUARE FUNCTION:
78  // -----------------------------
79  Function h;
80 
81  h << x;
82  h << v;
83  h << phi;
84  h << dphi;
85 
86  DMatrix Q(4,4); // LSQ coefficient matrix
87  Q.setIdentity();
88 
89  DVector r(4); // Reference
90 
91 
92  // DEFINE AN OPTIMAL CONTROL PROBLEM:
93  // ----------------------------------
94  const double t_start = 0.0;
95  const double t_end = 5.0;
96 
97  OCP ocp( t_start, t_end, 25 );
98 
99  ocp.minimizeLSQ( Q, h, r );
100  ocp.subjectTo( f );
101  ocp.subjectTo( -5.0 <= ax <= 5.0 );
102 
103 
104  // SETTING UP THE (SIMULATED) PROCESS:
105  // -----------------------------------
106  OutputFcn identity;
107  DynamicSystem dynamicSystem( fSim,identity );
108 
109  Process process( dynamicSystem,INT_RK45 );
110 
111  VariablesGrid disturbance; disturbance.read( "dist.txt" );
112  if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
113  exit( EXIT_FAILURE );
114 
115  // SETTING UP THE MPC CONTROLLER:
116  // ------------------------------
117 
118  RealTimeAlgorithm alg( ocp,0.1 );
119 // alg.set( USE_REALTIME_ITERATIONS,NO );
120 // alg.set( MAX_NUM_ITERATIONS,20 );
121 
122  StaticReferenceTrajectory zeroReference;
123 
124  Controller controller( alg,zeroReference );
125 
126 
127  // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
128  // ----------------------------------------------------------
129  SimulationEnvironment sim( 0.0,20.0,process,controller );
130 
131  DVector x0(4);
132  x0.setZero();
133  x0(3) = 5.0;
134 
135  if (sim.init( x0 ) != SUCCESSFUL_RETURN)
136  exit( EXIT_FAILURE );
137  if (sim.run( ) != SUCCESSFUL_RETURN)
138  exit( EXIT_FAILURE );
139 
140  // ...AND PLOT THE RESULTS
141  // ----------------------------------------------------------
142  VariablesGrid diffStates;
143  if (sim.getProcessDifferentialStates( diffStates ) != SUCCESSFUL_RETURN)
144  exit( EXIT_FAILURE );
145 
146  VariablesGrid feedbackControl;
147  if (sim.getFeedbackControl( feedbackControl ) != SUCCESSFUL_RETURN)
148  exit( EXIT_FAILURE );
149 
150  GnuplotWindow window;
151  window.addSubplot( diffStates(0), "POSITION OF THE TROLLEY" );
152  window.addSubplot( diffStates(1), "VELOCITY OF THE TROLLEY" );
153  window.addSubplot( diffStates(2), "PHI" );
154  window.addSubplot( diffStates(3), "DPHI" );
155  window.addSubplot( feedbackControl, "Accelaration [m/s^2]" );
156  // window.addSubplot( disturbance, "Disturbance [m/s^2]" );
157  window.plot();
158 
159  diffStates.print( "diffStates.txt" );
160 
161 
162  return EXIT_SUCCESS;
163 }
164 
165 /* <<< end tutorial code <<< */
int main()
Definition: crane_mpc3.cpp:40
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
Calculates the control inputs of the Process based on the Process outputs.
Definition: controller.hpp:71
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
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
virtual returnValue plot(PlotFrequency _frequency=PLOT_IN_ANY_CASE)
returnValue getProcessDifferentialStates(VariablesGrid &_diffStates)
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...
returnValue getFeedbackControl(Curve &_feedbackControl) const
User-interface to formulate and solve model predictive control problems.
returnValue subjectTo(const DifferentialEquation &differentialEquation_)
Definition: ocp.cpp:153
returnValue addSubplot(PlotWindowSubplot &_subplot)
IntermediateState cos(const Expression &arg)
returnValue minimizeLSQ(const DMatrix &S, const Function &h, const DVector &r)
Definition: ocp.cpp:244
returnValue init(const DVector &x0_, const DVector &p_=emptyConstVector)
Derived & setZero(Index size)
Data class for defining optimal control problems.
Definition: ocp.hpp:89
Allows to define a static reference trajectory that the ControlLaw aims to track. ...
#define v
Expression dot(const Expression &arg)
#define L
returnValue read(std::istream &stream)
const double t_end
returnValue setProcessDisturbance(const Curve &_processDisturbance)
Definition: process.cpp:289
const double t_start
Allows to run closed-loop simulations of dynamic systems.
Simulates the process to be controlled based on a dynamic model.
Definition: process.hpp:71
Provides an interface to Gnuplot for plotting algorithmic outputs.
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:31