int main( ){
USING_NAMESPACE_ACADO
DifferentialState x; // definition of a differential state
AlgebraicState z; // definition of an algebraic state
Control u; // definition of a control
Parameter p; // definition of a parameter
DifferentialEquation f; // a differential equation
f << dot(x) == -0.5*x-z+u*u; // an example for a differential-
f << 0 == z+exp(z)+x-1.0+u; // algebraic equation.
OCP ocp( 0.0, 4.0 ); // define an OCP with t_0 = 0.0 and T = 4.0
ocp.minimizeMayerTerm( x*x + p*p ); // a Mayer term to be minimized
ocp.subjectTo( f ); // OCP should regard the DAE
ocp.subjectTo( AT_START, x == 1.0 ); // an initial value constraint
ocp.subjectTo( AT_END , x + p == 1.0 ); // an end (or terminal) constraint
ocp.subjectTo( -1.0 <= x*u <= 1.0 ); // a path constraint
OptimizationAlgorithm algorithm(ocp); // define an algorithm
algorithm.set( KKT_TOLERANCE, 1e-5 ); // define a termination criterion
algorithm.solve(); // to solve the OCP.
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,L;
Control u;
DifferentialEquation f,g;
const double t_start = 0.0;
const double t_end = 10.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x) == -x + 0.01*x*x + u;
f << dot(L) == -x + 0.01*x*x + u;
g << dot(x) == x - 2.00*x*x + u;
g << dot(L) == x*x + u*u;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 60 );
ocp.minimizeMayerTerm( L );
ocp.subjectTo( f, 30 );
ocp.subjectTo( g, 30 );
ocp.subjectTo( AT_START, x == 1.0 );
ocp.subjectTo( AT_START, L == 0.0 );
// Additionally, flush a plotting object
GnuplotWindow window1;
window1.addSubplot( x,"DifferentialState x" );
window1.addSubplot( u,"Control u" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm << window1;
algorithm.solve();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x;
DifferentialState l;
AlgebraicState z;
Control u;
DifferentialEquation f;
const double t_start = 0.0;
const double t_end = 10.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x) == -x + 0.5*x*x + u + 0.5*z;
f << dot(l) == x*x + 3.0*u*u ;
f << 0 == z + exp(z) - 1.0 + x ;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 10 );
ocp.minimizeMayerTerm( l );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == 1.0 );
ocp.subjectTo( AT_START, l == 0.0 );
GnuplotWindow window;
window.addSubplot(x,"DIFFERENTIAL STATE x");
window.addSubplot(z,"ALGEBRAIC STATE z" );
window.addSubplot(u,"CONTROL u" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ----------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set( ABSOLUTE_TOLERANCE , 1.0e-7 );
algorithm.set( INTEGRATOR_TOLERANCE , 1.0e-7 );
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
//algorithm.set( GLOBALIZATION_STRATEGY, GS_FULLSTEP );
algorithm << window;
algorithm.solve();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x;
Control u;
Disturbance w;
Parameter p,q;
DifferentialEquation f;
const double t_start = 0.0;
const double t_end = 1.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << -dot(x) -x*x + p + u*u + w;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 20 );
ocp.minimizeMayerTerm( x + p*p + q*q );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == 1.0 );
ocp.subjectTo( 0.1 <= u <= 2.0 );
ocp.subjectTo( -0.1 <= w <= 2.1 );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
// algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.solve();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
DifferentialState s,v,m ; // the differential states
Control u ; // the control input u
Parameter T ; // the time horizon T
DifferentialEquation f( 0.0, T ); // the differential equation
// -------------------------------------
OCP ocp( 0.0, T ); // time horizon of the OCP: [0,T]
ocp.minimizeMayerTerm( T ); // the time T should be optimized
f << dot(s) == v; // an implementation
f << dot(v) == (u-0.2*v*v)/m; // of the model equations
f << dot(m) == -0.01*u*u; // for the rocket.
ocp.subjectTo( f ); // minimize T s.t. the model,
ocp.subjectTo( AT_START, s == 0.0 ); // the initial values for s,
ocp.subjectTo( AT_START, v == 0.0 ); // v,
ocp.subjectTo( AT_START, m == 1.0 ); // and m,
ocp.subjectTo( AT_END , s == 10.0 ); // the terminal constraints for s
ocp.subjectTo( AT_END , v == 0.0 ); // and v,
ocp.subjectTo( -0.1 <= v <= 1.7 ); // as well as the bounds on v
ocp.subjectTo( -1.1 <= u <= 1.1 ); // the control input u,
ocp.subjectTo( 5.0 <= T <= 15.0 ); // and the time horizon T.
// -------------------------------------
GnuplotWindow window;
window.addSubplot( s, "THE DISTANCE s" );
window.addSubplot( v, "THE VELOCITY v" );
window.addSubplot( m, "THE MASS m" );
window.addSubplot( u, "THE CONTROL INPUT u" );
OptimizationAlgorithm algorithm(ocp); // the optimization algorithm
algorithm << window;
algorithm.solve(); // solves the problem.
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
DifferentialState phi, omega; // the states of the pendulum
Parameter l, alpha ; // its length and the friction
const double g = 9.81 ; // the gravitational constant
DifferentialEquation f ; // the model equations
Function h ; // the measurement function
VariablesGrid measurements; // read the measurements
measurements = readFromFile( "data.txt" ); // from a file.
// --------------------------------------
OCP ocp(measurements.getTimePoints()); // construct an OCP
h << phi ; // the state phi is measured
ocp.minimizeLSQ( h, measurements ) ; // fit h to the data
f << dot(phi ) == omega ; // a symbolic implementation
f << dot(omega) == -(g/l) *sin(phi ) // of the model
- alpha*omega ; // equations
ocp.subjectTo( f ); // solve OCP s.t. the model,
ocp.subjectTo( 0.0 <= alpha <= 4.0 ); // the bounds on alpha
ocp.subjectTo( 0.0 <= l <= 2.0 ); // and the bounds on l.
// --------------------------------------
GnuplotWindow window;
window.addSubplot( phi , "The angle phi", "time [s]", "angle [rad]" );
window.addSubplot( omega, "The angular velocity dphi" );
window.addData( 0, measurements(0) );
ParameterEstimationAlgorithm algorithm(ocp); // the parameter estimation
algorithm << window;
algorithm.solve(); // algorithm solves the problem.
return 0;
}
void Shell::select(const std::string& exec, size_t argc, const std::string* argv)
{
if(exec == "open")
open(argc, argv);
else if(exec == "seek")
seek(argc, argv);
else if(exec == "tell")
tell(argc, argv);
else if(exec == "write")
write(argc, argv);
else if(exec == "read")
read(argc, argv);
else if(exec == "close")
close(argc, argv);
else if(exec == "useradd")
useradd(argc, argv);
else if(exec == "chpsd")
chpsd(argc, argv);
else if(exec == "cd")
cd(argc, argv);
else if(exec == "mkdir")
mkdir(argc, argv);
else if(exec == "mkfile")
mkfile(argc, argv);
else if(exec == "rm")
rm(argc, argv);
else if(exec == "cat")
cat(argc, argv);
else if(exec == "ls")
ls(argc, argv);
else if(exec == "chmod")
chmod(argc, argv);
else if(exec == "see")
see(argc, argv);
else if(exec == "ocp")
ocp(argc, argv);
else
runshell(exec, argc, argv);
}
int main( )
{
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState xB; //Body Position
DifferentialState xW; //Wheel Position
DifferentialState vB; //Body Velocity
DifferentialState vW; //Wheel Velocity
Control F;
Disturbance R;
double mB = 350.0;
double mW = 50.0;
double kS = 20000.0;
double kT = 200000.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
DifferentialEquation f;
f << dot(xB) == vB;
f << dot(xW) == vW;
f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
f << dot(vW) == ( kS*xB - (kT+kS)*xW + kT*R - F ) / mW;
// DEFINE LEAST SQUARE FUNCTION:
// -----------------------------
Function h;
h << xB;
h << xW;
h << vB;
h << vW;
h << F;
DMatrix Q(5,5); // LSQ coefficient matrix
Q(0,0) = 10.0;
Q(1,1) = 10.0;
Q(2,2) = 1.0;
Q(3,3) = 1.0;
Q(4,4) = 1.0e-8;
DVector r(5); // Reference
r.setAll( 0.0 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double tStart = 0.0;
const double tEnd = 1.0;
OCP ocp( tStart, tEnd, 20 );
ocp.minimizeLSQ( Q, h, r );
ocp.subjectTo( f );
ocp.subjectTo( -200.0 <= F <= 200.0 );
ocp.subjectTo( R == 0.0 );
// SETTING UP THE REAL-TIME ALGORITHM:
// -----------------------------------
RealTimeAlgorithm alg( ocp,0.025 );
alg.set( MAX_NUM_ITERATIONS, 1 );
alg.set( PLOT_RESOLUTION, MEDIUM );
GnuplotWindow window;
window.addSubplot( xB, "Body Position [m]" );
window.addSubplot( xW, "Wheel Position [m]" );
window.addSubplot( vB, "Body Velocity [m/s]" );
window.addSubplot( vW, "Wheel Velocity [m/s]" );
window.addSubplot( F, "Damping Force [N]" );
window.addSubplot( R, "Road Excitation [m]" );
alg << window;
// SETUP CONTROLLER AND PERFORM A STEP:
// ------------------------------------
StaticReferenceTrajectory zeroReference( "ref.txt" );
Controller controller( alg,zeroReference );
DVector y( 4 );
y.setZero( );
y(0) = 0.01;
if (controller.init( 0.0,y ) != SUCCESSFUL_RETURN)
exit( 1 );
if (controller.step( 0.0,y ) != SUCCESSFUL_RETURN)
exit( 1 );
return EXIT_SUCCESS;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE FIXED PARAMETERS:
// ---------------------------
#define Csin 2.8
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x1,x2,x3,x4,x5;
IntermediateState mu,sigma,pif;
Control u ;
Parameter tf ;
DifferentialEquation f(0.0,tf) ;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
mu = 0.125*x2/x4;
sigma = mu/0.135;
pif = (-384.0*mu*mu + 134.0*mu);
f << dot(x1) == mu*x1;
f << dot(x2) == -sigma*x1 + u*Csin;
f << dot(x3) == pif*x1;
f << dot(x4) == u;
f << dot(x5) == 0.001*(u*u + 0.01*tf*tf);
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, tf, 50 );
ocp.minimizeMayerTerm(0, 0.01*x5 -x3/tf ); // Solve productivity optimal problem (Note: - due to maximization, small regularisation term)
ocp.minimizeMayerTerm(1, 0.01*x5 -x3/(Csin*x4-x2)); // Solve yield optimal problem (Note: Csin = x2(t=0)/x4(t=0); - due to maximization, small regularisation term)
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x1 == 0.1 );
ocp.subjectTo( AT_START, x2 == 14.0 );
ocp.subjectTo( AT_START, x3 == 0.0 );
ocp.subjectTo( AT_START, x4 == 5.0 );
ocp.subjectTo( AT_START, x5 == 0.0 );
ocp.subjectTo( 0.0 <= x1 <= 15.0 );
ocp.subjectTo( 0.0 <= x2 <= 30.0 );
ocp.subjectTo( -0.1 <= x3 <= 1000.0 );
ocp.subjectTo( 5.0 <= x4 <= 20.0 );
ocp.subjectTo( 20.0 <= tf <= 40.0 );
ocp.subjectTo( 0.0 <= u <= 2.0 );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE OCP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(ocp);
algorithm.set( PARETO_FRONT_DISCRETIZATION, 21 );
algorithm.set( PARETO_FRONT_GENERATION , PFG_NORMALIZED_NORMAL_CONSTRAINT );
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set( KKT_TOLERANCE, 1e-7 );
// Minimize individual objective function
algorithm.solveSingleObjective(0);
// Minimize individual objective function
algorithm.solveSingleObjective(1);
// Generate Pareto set
algorithm.set( KKT_TOLERANCE, 1e-6 );
algorithm.solve();
algorithm.getWeights("fed_batch_bioreactor_nnc_weights.txt");
algorithm.getAllDifferentialStates("fed_batch_bioreactor_nnc_states.txt");
algorithm.getAllControls("fed_batch_bioreactor_nnc_controls.txt");
algorithm.getAllParameters("fed_batch_bioreactor_nnc_parameters.txt");
// VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
// ------------------------------------------
VariablesGrid paretoFront;
algorithm.getParetoFront( paretoFront );
GnuplotWindow window1;
window1.addSubplot( paretoFront, "Pareto Front (yield versus productivity)", "-PRODUCTIVTY", "-YIELD", PM_POINTS );
window1.plot( );
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
// SAVE INFORMATION:
// -----------------
FILE *file = fopen("fed_batch_bioreactor_nnc_pareto.txt","w");
paretoFront.print();
file << paretoFront;
fclose(file);
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x;
DifferentialState l;
AlgebraicState z;
Control u;
DifferentialEquation f;
// Disturbance R;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x) == -x + 0.5*x*x + u + 0.5*z ;
f << dot(l) == x*x + 3.0*u*u ;
f << 0 == z + exp(z) - 1.0 + x ;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, 5.0, 10 );
ocp.minimizeMayerTerm( l );
ocp.subjectTo( f );
// ocp.subjectTo( R == 0.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_BDF );
//VariablesGrid disturbance = readFromFile( "dae_simulation_disturbance.txt" );
//process.setProcessDisturbance( disturbance );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp,0.5 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
SimulationEnvironment sim( 0.0,15.0,process,controller );
Vector x0(2);
x0(0) = 1;
x0(1) = 0;
sim.init( x0 );
sim.run( );
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
VariablesGrid diffStates;
sim.getProcessDifferentialStates( diffStates );
diffStates.printToFile( "diffStates.txt" );
diffStates.printToFile( "diffStates.m","DIFFSTATES",PS_MATLAB );
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.printToFile( "sampledOut.txt" );
sampledProcessOutput.printToFile( "sampledOut.m","OUT",PS_MATLAB );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.printToFile( "controls.txt" );
feedbackControl.printToFile( "controls.m","CONTROL",PS_MATLAB );
VariablesGrid algStates;
sim.getProcessAlgebraicStates( algStates );
algStates.printToFile( "algStates.txt" );
algStates.printToFile( "algStates.m","ALGSTATES",PS_MATLAB );
GnuplotWindow window;
window.addSubplot( diffStates(0), "DIFFERENTIAL STATE: x" );
window.addSubplot( diffStates(1), "DIFFERENTIAL STATE: l" );
window.addSubplot( algStates(0), "ALGEBRAIC STATE: z" );
window.addSubplot( feedbackControl(0), "CONTRUL: u" );
window.plot( );
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// ----------------------------
DifferentialState x(10); // a differential state vector with dimension 10. (vector)
DifferentialState y ; // another differential state y (scalar)
Control u(2 ); // a control input with dimension 2. (vector)
Parameter p ; // a parameter (here a scalar). (scalar)
DifferentialEquation f ; // the differential equation
const double t_start = 0.0;
const double t_end = 1.0;
// READ A MATRIX "A" FROM A FILE:
// ------------------------------
Matrix A = readFromFile( "matrix_vector_ocp_A.txt" );
Matrix B = readFromFile( "matrix_vector_ocp_B.txt" );
// READ A VECTOR "x0" FROM A FILE:
// -------------------------------
Vector x0 = readFromFile( "matrix_vector_ocp_x0.txt" );
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x) == -(A*x) + B*u; // matrix vector notation for a linear equation
f << dot(y) == x.transpose()*x + 2.0*u.transpose()*u; // matrix vector notation: x^x = scalar product = ||x||_2^2
// u^u = scalar product = ||u||_2^2
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 20 );
ocp.minimizeMayerTerm( y );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == x0 );
ocp.subjectTo( AT_START, y == 0.0 );
GnuplotWindow window;
window.addSubplot( x(0),"x0" );
window.addSubplot( x(6),"x6" );
window.addSubplot( u(0),"u0" );
window.addSubplot( u(1),"u1" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set( MAX_NUM_ITERATIONS, 20 );
algorithm.set( KKT_TOLERANCE, 1e-10 );
algorithm << window;
algorithm.solve();
return 0;
}
int main( )
{
USING_NAMESPACE_ACADO
// DEFINE THE VARIABLES:
// ----------------------------------------------------------
DifferentialState p ; // the trolley position
DifferentialState v ; // the trolley velocity
DifferentialState phi ; // the excitation angle
DifferentialState omega; // the angular velocity
Control a ; // the acc. of the trolley
const double g = 9.81; // the gravitational constant
const double b = 0.20; // the friction coefficient
// ----------------------------------------------------------
// DEFINE THE MODEL EQUATIONS:
// ----------------------------------------------------------
DifferentialEquation f;
f << dot( p ) == v ;
f << dot( v ) == a ;
f << dot( phi ) == omega ;
f << dot( omega ) == -g*sin(phi) - a*cos(phi) - b*omega;
// ----------------------------------------------------------
// DEFINE THE WEIGHTING MATRICES:
// ----------------------------------------------------------
Matrix Q = eye(4);
Matrix R = eye(1);
Matrix P = eye(4);
P *= 5.0;
// ----------------------------------------------------------
// SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
// ----------------------------------------------------------
OCP ocp( 0.0,3.0, 10 );
ocp.minimizeLSQ ( Q,R );
ocp.minimizeLSQEndTerm( P );
ocp.subjectTo( f );
ocp.subjectTo( -1.0 <= a <= 1.0 );
// ocp.subjectTo( -0.5 <= v <= 1.5 );
// ----------------------------------------------------------
// DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
// ----------------------------------------------------------
MPCexport mpc( ocp );
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( DISCRETIZATION_TYPE, SINGLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 30 );
mpc.set( QP_SOLVER, QP_QPOASES );
// mpc.set( MAX_NUM_QP_ITERATIONS, 20 );
// mpc.set( HOTSTART_QP, YES );
// mpc.set( LEVENBERG_MARQUARDT, 1.0e-4 );
mpc.set( GENERATE_TEST_FILE, YES );
mpc.set( GENERATE_MAKE_FILE, YES );
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
// mpc.set( OPERATING_SYSTEM, OS_WINDOWS );
// mpc.set( USE_SINGLE_PRECISION, YES );
mpc.exportCode( "getting_started_export" );
// ----------------------------------------------------------
return 0;
}
USING_NAMESPACE_ACADO
int main( ){
// INTRODUCE THE VARIABLES:
// --------------------------------------------------
DifferentialState xp, yp, zp, phi, theta, psi, vx, vy, vz, p, q, r, cost;
Control u1, u2, u3, u4;
Parameter T;
// DEFINE THE DIMENSIONS OF THE C-FUNCTIONS:
// --------------------------------------------------
CFunction F( 13, myDifferentialEquation );
//CFunction M( NJ, myObjectiveFunction );
//CFunction I( NI, myInitialValueConstraint );
//CFunction E( NE, myEndPointConstraint );
//CFunction H( NH, myInequalityPathConstraint );
DifferentialEquation f ;
// const double h = 0.01;
// DiscretizedDifferentialEquation f(h);
// F.setUserData((void*) &h);
// DEFINE THE OPTIMIZATION VARIABLES:
// --------------------------------------------------
IntermediateState x(17);
x(0) = xp; x(1) = yp; x(2) = zp; x(3) = phi; x(4) = theta; x(5) = psi;
x(6) = vx; x(7) = vy; x(8) = vz; x(9) = p; x(10) = q; x(11) = r; x(12) = cost;
x(13) = u1; x(14) = u2; x(15) = u3; x(16) = u4;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, T, 20 );
ocp.minimizeMayerTerm(T);//x(12));
ocp.subjectTo( f << F(x)*T );
ocp.subjectTo( 3 <= T <= 15);
// Start constraints
for (unsigned int k=0 ; k < 13 ; k++) {
ocp.subjectTo(AT_START, x(k) == 0.0);
}
// End constraints
ocp.subjectTo( AT_END, x(0) == 10.0 );
ocp.subjectTo( AT_END, x(1) == 0.0 );
ocp.subjectTo( AT_END, x(2) == 0.0 );
for (unsigned int k=3 ; k < 12 ; k++) {
ocp.subjectTo(AT_END, x(k) == 0.0 );
}
// Limits on commands
ocp.subjectTo(0 <= x(13) <= 60);
ocp.subjectTo(-5 <= x(14) <= 5);
ocp.subjectTo(-5 <= x(15) <= 5);
ocp.subjectTo(-5 <= x(16) <= 5);
ocp.subjectTo(x(5) == 0.);
ocp.subjectTo( -1.57 <= x(3) <= 1.57);
ocp.subjectTo( -1.57 <= x(4) <= 1.57);
// ocp.subjectTo(x(4) == 0.);
// VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
// ------------------------------------------
GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
window1.addSubplot( x(0),"x" );
window1.addSubplot( x(1) , "y");
window1.addSubplot( x(2), "z");
window1.addSubplot( x(3),"phi" );
window1.addSubplot( x(4) , "theta");
window1.addSubplot( x(5), "psi");
window1.addSubplot( x(13),"u1" );
window1.addSubplot( x(14),"u2" );
window1.addSubplot( x(15),"u3" );
window1.addSubplot( x(16),"u4" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
// algorithm.set( INTEGRATOR_TOLERANCE, 1e-3 );
// algorithm.set( DISCRETIZATION_TYPE, SINGLE_SHOOTING );
//algorithm.set( HESSIAN_APPROXIMATION, FULL_BFGS_UPDATE );
algorithm.set( INTEGRATOR_TYPE, INT_RK45 );
algorithm.set( MAX_NUM_ITERATIONS, 500 );
algorithm.set( KKT_TOLERANCE, 1e-12);
algorithm << window1;
// double testx[16];
// for (i=0 ; i<12 ; i++) {
// testx[i] = 0.;
// }
// testx[4] = 0;
// testx[3] = 1.57;
// testx[13] = 0.;
// testx[14] = 0.;
// testx[15] = 0.;
// testx[16] = 1.;
// double testf[13];
// for (i=0 ; i<12 ; i++) {
// testf[i] = 0;
// }
// myDifferentialEquation( testx, testf, testf );
// for ( unsigned int k=0 ; k<12 ; k++ ) {
// std::cout << testf[k] << std::endl;
// }
algorithm.solve();
return 0;
}
USING_NAMESPACE_ACADO
int main( )
{
// DIFFERENTIAL STATES :
// -------------------------
DifferentialState r; // the length r of the cable
DifferentialState phi; // the angle phi
DifferentialState theta; // the angle theta (grosser wert- naeher am boden/kleiner wert - weiter oben)
// ------------------------- // -------------------------------------------
DifferentialState dr; // first derivative of r0 with respect to t
DifferentialState dphi; // first derivative of phi with respect to t
DifferentialState dtheta; // first derivative of theta with respect to t
// ------------------------- // -------------------------------------------
DifferentialState n; // winding number(rauslassen)
// ------------------------- // -------------------------------------------
DifferentialState Psi; // the roll angle Psi
DifferentialState CL; // the aerodynamic lift coefficient
// ------------------------- // -------------------------------------------
DifferentialState W; // integral over the power at the generator
// ( = ENERGY )(rauslassen)
// MEASUREMENT FUNCTION :
// -------------------------
Function model_response ; // the measurement function
// CONTROL :
// -------------------------
Control ddr0; // second derivative of r0 with respect to t
Control dPsi; // first derivative of Psi with respect to t
Control dCL; // first derivative of CL with respect to t
Disturbance w_extra;
// PARAMETERS :
// ------------------------
// PARAMETERS OF THE KITE :
// -----------------------------
double mk = 2000.00; // mass of the kite // [ kg ](maybe change to 5000kg)
double A = 500.00; // effective area // [ m^2 ]
double V = 720.00; // volume // [ m^3 ]
double cd0 = 0.04; // aerodynamic drag coefficient // [ ]
// ( cd0: without downwash )
double K = 0.04; // induced drag constant // [ ]
// PHYSICAL CONSTANTS :
// -----------------------------
double g = 9.81; // gravitational constant // [ m /s^2]
double rho = 1.23; // density of the air // [ kg/m^3]
// PARAMETERS OF THE CABLE :
// -----------------------------
double rhoc = 1450.00; // density of the cable // [ kg/m^3]
double cc = 1.00; // frictional constant // [ ]
double dc = 0.05614; // diameter // [ m ]
// PARAMETERS OF THE WIND :
// -----------------------------
double w0 = 10.00; // wind velocity at altitude h0 // [ m/s ]
double h0 = 100.00; // the altitude h0 // [ m ]
double hr = 0.10; // roughness length // [ m ]
// OTHER VARIABLES :
// ------------------------
double AQ ; // cross sectional area
IntermediateState mc; // mass of the cable
IntermediateState m ; // effective inertial mass
IntermediateState m_; // effective gravitational mass
IntermediateState Cg;
IntermediateState dm; // first derivative of m with respect to t
// DEFINITION OF PI :
// ------------------------
double PI = 3.1415926535897932;
// ORIENTATION AND FORCES :
// ------------------------
IntermediateState h ; // altitude of the kite
IntermediateState w ; // the wind at altitude h
IntermediateState we [ 3]; // effective wind vector
IntermediateState nwe ; // norm of the effective wind vector
IntermediateState nwep ; // -
IntermediateState ewep [ 3]; // projection of ewe
IntermediateState eta ; // angle eta: cf. [2]
IntermediateState et [ 3]; // unit vector from the left to the right wing tip
IntermediateState Caer ;
IntermediateState Cf ; // simple constants
IntermediateState CD ; // the aerodynamic drag coefficient
IntermediateState Fg [ 3]; // the gravitational force
IntermediateState Faer [ 3]; // the aerodynamic force
IntermediateState Ff [ 3]; // the frictional force
IntermediateState F [ 3]; // the total force
// TERMS ON RIGHT-HAND-SIDE
// OF THE DIFFERENTIAL
// EQUATIONS :
// ------------------------
IntermediateState a_pseudo [ 3]; // the pseudo accelaration
IntermediateState dn ; // the time derivate of the kite's winding number
IntermediateState ddr ; // second derivative of s with respect to t
IntermediateState ddphi ; // second derivative of phi with respect to t
IntermediateState ddtheta ; // second derivative of theta with respect to t
IntermediateState power ; // the power at the generator
// ------------------------ ------ // ----------------------------------------------
IntermediateState regularisation ; // regularisation of controls
// MODEL EQUATIONS :
// ===============================================================
// SPRING CONSTANT OF THE CABLE :
// ---------------------------------------------------------------
AQ = PI*dc*dc/4.0 ;
// THE EFECTIVE MASS' :
// ---------------------------------------------------------------
mc = rhoc*AQ*r ; // mass of the cable
m = mk + mc / 3.0; // effective inertial mass
m_ = mk + mc / 2.0; // effective gravitational mass
// ----------------------------- // ----------------------------
dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass
// WIND SHEAR MODEL :
// ---------------------------------------------------------------
// for startup +60m
h = r*cos(theta)+60.0 ;
// h = r*cos(theta) ;
w = log(h/hr) / log(h0/hr) *(w0+w_extra) ;
// EFFECTIVE WIND IN THE KITE`S SYSTEM :
// ---------------------------------------------------------------
we[0] = w*sin(theta)*cos(phi) - dr ;
we[1] = -w*sin(phi) - r*sin(theta)*dphi ;
we[2] = -w*cos(theta)*cos(phi) + r *dtheta;
// CALCULATION OF THE KITE`S TRANSVERSAL AXIS :
// ---------------------------------------------------------------
nwep = pow( we[1]*we[1] + we[2]*we[2], 0.5 );
nwe = pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 );
eta = asin( we[0]*tan(Psi)/ nwep ) ;
// ---------------------------------------------------------------
// ewep[0] = 0.0 ;
ewep[1] = we[1] / nwep ;
ewep[2] = we[2] / nwep ;
// ---------------------------------------------------------------
et [0] = sin(Psi) ;
et [1] = (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2];
et [2] = (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1];
// CONSTANTS FOR SEVERAL FORCES :
// ---------------------------------------------------------------
Cg = (V*rho-m_)*g ;
Caer = (rho*A/2.0 )*nwe ;
Cf = (rho*dc/8.0)*r*nwe ;
// THE DRAG-COEFFICIENT :
// ---------------------------------------------------------------
CD = cd0 + K*CL*CL ;
// SUM OF GRAVITATIONAL AND LIFTING FORCE :
// ---------------------------------------------------------------
Fg [0] = Cg * cos(theta) ;
// Fg [1] = Cg * 0.0 ;
Fg [2] = Cg * sin(theta) ;
// SUM OF THE AERODYNAMIC FORCES :
// ---------------------------------------------------------------
Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ;
Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ;
Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ;
// THE FRICTION OF THE CABLE :
// ---------------------------------------------------------------
// Ff [0] = Cf * cc* we[0] ;
Ff [1] = Cf * cc* we[1] ;
Ff [2] = Cf * cc* we[2] ;
// THE TOTAL FORCE :
// ---------------------------------------------------------------
F [0] = Fg[0] + Faer[0] ;
F [1] = Faer[1] + Ff[1] ;
F [2] = Fg[2] + Faer[2] + Ff[2] ;
// THE PSEUDO ACCELERATION:
// ---------------------------------------------------------------
a_pseudo [0] = - ddr0
+ r*( dtheta*dtheta
+ sin(theta)*sin(theta)*dphi *dphi )
- dm/m*dr ;
a_pseudo [1] = - 2.0*cos(theta)/sin(theta)*dphi*dtheta
- 2.0*dr/r*dphi
- dm/m*dphi ;
a_pseudo [2] = cos(theta)*sin(theta)*dphi*dphi
- 2.0*dr/r*dtheta
- dm/m*dtheta ;
// THE EQUATIONS OF MOTION:
// ---------------------------------------------------------------
ddr = F[0]/m + a_pseudo[0] ;
ddphi = F[1]/(m*r*sin(theta)) + a_pseudo[1] ;
ddtheta = -F[2]/(m*r ) + a_pseudo[2] ;
// THE DERIVATIVE OF THE WINDING NUMBER :
// ---------------------------------------------------------------
dn = ( dphi*ddtheta - dtheta*ddphi ) /
(2.0*PI*(dphi*dphi + dtheta*dtheta) ) ;
// THE POWER AT THE GENERATOR :
// ---------------------------------------------------------------
power = m*ddr*dr ;
// REGULARISATION TERMS :
// ---------------------------------------------------------------
regularisation = 5.0e2 * ddr0 * ddr0
+ 1.0e8 * dPsi * dPsi
+ 1.0e5 * dCL * dCL
+ 2.5e5 * dn * dn
+ 2.5e7 * ddphi * ddphi;
+ 2.5e7 * ddtheta * ddtheta;
+ 2.5e6 * dtheta * dtheta;
// ---------------------------
// REFERENCE TRAJECTORY:
// ---------------------------------------------------------------
VariablesGrid myReference; myReference.read( "ref_w_zeros.txt" );// read the measurements
PeriodicReferenceTrajectory reference( myReference );
// THE "RIGHT-HAND-SIDE" OF THE ODE:
// ---------------------------------------------------------------
DifferentialEquation f;
f << dot(r) == dr ;
f << dot(phi) == dphi ;
f << dot(theta) == dtheta ;
f << dot(dr) == ddr0 ;
f << dot(dphi) == ddphi ;
f << dot(dtheta) == ddtheta ;
f << dot(n) == dn ;
f << dot(Psi) == dPsi ;
f << dot(CL) == dCL ;
f << dot(W) == (-power + regularisation)*1.0e-6;
model_response << r ; // the state r is measured
model_response << phi ;
model_response << theta;
model_response << dr ;
model_response << dphi ;
model_response << dtheta;
model_response << ddr0;
model_response << dPsi;
model_response << dCL ;
DVector x_scal(9);
x_scal(0) = 60.0;
x_scal(1) = 1.0e-1;
x_scal(2) = 1.0e-1;
x_scal(3) = 40.0;
x_scal(4) = 1.0e-1;
x_scal(5) = 1.0e-1;
x_scal(6) = 60.0;
x_scal(7) = 1.5e-1;
x_scal(8) = 2.5;
DMatrix Q(9,9);
Q.setIdentity();
DMatrix Q_end(9,9);
Q_end.setIdentity();
int i;
for( i = 0; i < 6; i++ ){
Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));
}
for( i = 6; i < 9; i++ ){
Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));
}
DVector measurements(9);
measurements.setAll( 0.0 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 10.0;
OCP ocp( t_start, t_end, 10 );
ocp.minimizeLSQ( Q,model_response, measurements ) ; // fit h to the data
ocp.minimizeLSQEndTerm( Q_end,model_response, measurements ) ;
ocp.subjectTo( f );
ocp.subjectTo( -0.34 <= phi <= 0.34 );
ocp.subjectTo( 0.85 <= theta <= 1.45 );
ocp.subjectTo( -40.0 <= dr <= 10.0 );
ocp.subjectTo( -0.29 <= Psi <= 0.29 );
ocp.subjectTo( 0.1 <= CL <= 1.50 );
ocp.subjectTo( -0.7 <= n <= 0.90 );
ocp.subjectTo( -25.0 <= ddr0 <= 25.0 );
ocp.subjectTo( -0.065 <= dPsi <= 0.065 );
ocp.subjectTo( -3.5 <= dCL <= 3.5 );
ocp.subjectTo( -60.0 <= cos(theta)*r );
ocp.subjectTo( w_extra == 0.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance; disturbance.read( "my_wind_disturbance_controlsfree.txt" );
if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
// ----------------------------------------------
double samplingTime = 1.0;
RealTimeAlgorithm algorithm( ocp, samplingTime );
if (algorithm.initializeDifferentialStates("p_s_ref.txt" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (algorithm.initializeControls ("p_c_ref.txt" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
algorithm.set( MAX_NUM_ITERATIONS, 2 );
algorithm.set( KKT_TOLERANCE , 1e-4 );
algorithm.set( HESSIAN_APPROXIMATION,GAUSS_NEWTON);
algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
algorithm.set( GLOBALIZATION_STRATEGY,GS_FULLSTEP );
// algorithm.set( USE_IMMEDIATE_FEEDBACK, YES );
algorithm.set( USE_REALTIME_SHIFTS, YES );
algorithm.set(LEVENBERG_MARQUARDT, 1e-5);
DVector x0(10);
x0(0) = 1.8264164528775887e+03;
x0(1) = -5.1770453309520573e-03;
x0(2) = 1.2706440287266794e+00;
x0(3) = 2.1977888424944396e+00;
x0(4) = 3.1840786108641383e-03;
x0(5) = -3.8281200674676448e-02;
x0(6) = 0.0000000000000000e+00;
x0(7) = -1.0372313936413566e-02;
x0(8) = 1.4999999999999616e+00;
x0(9) = 0.0000000000000000e+00;
// SETTING UP THE NMPC CONTROLLER:
// -------------------------------
Controller controller( algorithm, reference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
double simulationStart = 0.0;
double simulationEnd = 10.0;
SimulationEnvironment sim( simulationStart, simulationEnd, process, controller );
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
VariablesGrid diffStates;
sim.getProcessDifferentialStates( diffStates );
diffStates.print( "diffStates.txt" );
diffStates.print( "diffStates.m","DIFFSTATES",PS_MATLAB );
VariablesGrid interStates;
sim.getProcessIntermediateStates( interStates );
interStates.print( "interStates.txt" );
interStates.print( "interStates.m","INTERSTATES",PS_MATLAB );
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.print( "sampledOut.txt" );
sampledProcessOutput.print( "sampledOut.m","OUT",PS_MATLAB );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.print( "controls.txt" );
feedbackControl.print( "controls.m","CONTROL",PS_MATLAB );
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: r" );
window.addSubplot( sampledProcessOutput(1), "DIFFERENTIAL STATE: phi" );
window.addSubplot( sampledProcessOutput(2), "DIFFERENTIAL STATE: theta" );
window.addSubplot( sampledProcessOutput(3), "DIFFERENTIAL STATE: dr" );
window.addSubplot( sampledProcessOutput(4), "DIFFERENTIAL STATE: dphi" );
window.addSubplot( sampledProcessOutput(5), "DIFFERENTIAL STATE: dtheta" );
window.addSubplot( sampledProcessOutput(7), "DIFFERENTIAL STATE: Psi" );
window.addSubplot( sampledProcessOutput(8), "DIFFERENTIAL STATE: CL" );
window.addSubplot( sampledProcessOutput(9), "DIFFERENTIAL STATE: W" );
window.addSubplot( feedbackControl(0), "CONTROL 1 DDR0" );
window.addSubplot( feedbackControl(1), "CONTROL 1 DPSI" );
window.addSubplot( feedbackControl(2), "CONTROL 1 DCL" );
window.plot( );
GnuplotWindow window2;
window2.addSubplot( interStates(1) );
window2.plot();
return 0;
}
int main()
{
USING_NAMESPACE_ACADO
// Variables:
DifferentialState phi;
DifferentialState theta;
DifferentialState dphi;
DifferentialState dtheta;
Control u1;
Control u2;
IntermediateState c;// c := rho * |we|^2 / (2*m)
DifferentialEquation f;// the right-hand side of the model
// Parameters:
const double R = 1.00;// radius of the carousel arm
const double Omega = 1.00;// constant rotational velocity of the carousel arm
const double m = 0.80;// the mass of the plane
const double r = 1.00;// length of the cable between arm and plane
const double A = 0.15;// wing area of the plane
const double rho = 1.20;// density of the air
const double CL = 1.00;// nominal lift coefficient
const double CD = 0.15;// nominal drag coefficient
const double b = 15.00;// roll stabilization coefficient
const double g = 9.81;// gravitational constant
// COMPUTE THE CONSTANT c:
// -------------------------
c = ( (R*R*Omega*Omega)
+ (r*r)*( (Omega+dphi)*(Omega+dphi) + dtheta*dtheta )
+ (2.0*r*R*Omega)*( (Omega+dphi)*sin(theta)*cos(phi)
+ dtheta*cos(theta)*sin(phi) ) ) * ( A*rho/( 2.0*m ) );
f << dot( phi ) == dphi;
f << dot( theta ) == dtheta;
f << dot( dphi ) == ( 2.0*r*(Omega+dphi)*dtheta*cos(theta)
+ (R*Omega*Omega)*sin(phi)
+ c*(CD*(1.0+u2)+CL*(1.0+0.5*u2)*phi) ) / (-r*sin(theta));
f << dot( dtheta ) == ( (R*Omega*Omega)*cos(theta)*cos(phi)
+ r*(Omega+dphi)*sin(theta)*cos(theta)
+ g*sin(theta) - c*( CL*u1 + b*dtheta ) ) / r;
// Reference functions and weighting matrices:
Function h, hN;
h << phi << theta << dphi << dtheta << u1 << u2;
hN << phi << theta << dphi << dtheta;
Matrix W = eye( h.getDim() );
Matrix WN = eye( hN.getDim() );
W(0,0) = 5.000;
W(1,1) = 1.000;
W(2,2) = 10.000;
W(3,3) = 10.000;
W(4,4) = 0.1;
W(5,5) = 0.1;
WN(0,0) = 1.0584373059248833e+01;
WN(0,1) = 1.2682415889087276e-01;
WN(0,2) = 1.2922232012424681e+00;
WN(0,3) = 3.7982078044271374e-02;
WN(1,0) = 1.2682415889087265e-01;
WN(1,1) = 3.2598407653299275e+00;
WN(1,2) = -1.1779732282636639e-01;
WN(1,3) = 9.8830655348904548e-02;
WN(2,0) = 1.2922232012424684e+00;
WN(2,1) = -1.1779732282636640e-01;
WN(2,2) = 4.3662024133354898e+00;
WN(2,3) = -5.9269992411260908e-02;
WN(3,0) = 3.7982078044271367e-02;
WN(3,1) = 9.8830655348904534e-02;
WN(3,2) = -5.9269992411260901e-02;
WN(3,3) = 3.6495564367004318e-01;
//
// Optimal Control Problem
//
// OCP ocp( 0.0, 12.0, 15 );
OCP ocp( 0.0, 2.0 * M_PI, 10 );
ocp.subjectTo( f );
ocp.minimizeLSQ(W, h);
ocp.minimizeLSQEndTerm(WN, hN);
ocp.subjectTo( 18.0 <= u1 <= 22.0 );
ocp.subjectTo( -0.2 <= u2 <= 0.2 );
// Export the code:
OCPexport mpc(ocp);
mpc.set( INTEGRATOR_TYPE , INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS , 30 );
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( HOTSTART_QP, YES );
mpc.set( GENERATE_TEST_FILE, NO );
if (mpc.exportCode("kite_carousel_export") != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
// DifferentialState Gx(4,4), Gu(4,2);
//
// Expression rhs;
// f.getExpression( rhs );
//
// Function Df;
// Df << rhs;
// Df << forwardDerivative( rhs, x ) * Gx;
// Df << forwardDerivative( rhs, x ) * Gu + forwardDerivative( rhs, u );
//
//
// EvaluationPoint z(Df);
//
// Vector xx(28);
// xx.setZero();
//
// xx(0) = -4.2155955213988627e-02;
// xx(1) = 1.8015724412870739e+00;
// xx(2) = 0.0;
// xx(3) = 0.0;
//
// Vector uu(2);
// uu(0) = 20.5;
// uu(1) = -0.1;
//
// z.setX( xx );
// z.setU( uu );
//
// Vector result = Df.evaluate( z );
// result.print( "x" );
return EXIT_SUCCESS;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// ------------------------------------
DifferentialState v,s,m;
Control u ;
const double t_start = 0.0;
const double t_end = 10.0;
const double h = 0.01;
DiscretizedDifferentialEquation f(h) ;
// DEFINE A DISCRETE-TIME SYTSEM:
// -------------------------------
f << next(s) == s + h*v;
f << next(v) == v + h*(u-0.02*v*v)/m;
f << next(m) == m - h*0.01*u*u;
Function eta;
eta << u;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 50 );
//ocp.minimizeLagrangeTerm( u*u );
ocp.minimizeLSQ( eta );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, s == 0.0 );
ocp.subjectTo( AT_START, v == 0.0 );
ocp.subjectTo( AT_START, m == 1.0 );
ocp.subjectTo( AT_END , s == 10.0 );
ocp.subjectTo( AT_END , v == 0.0 );
ocp.subjectTo( -0.01 <= v <= 1.3 );
// DEFINE A PLOT WINDOW:
// ---------------------
GnuplotWindow window;
window.addSubplot( s,"DifferentialState s" );
window.addSubplot( v,"DifferentialState v" );
window.addSubplot( m,"DifferentialState m" );
window.addSubplot( u,"Control u" );
window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set( INTEGRATOR_TYPE, INT_DISCRETE );
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set( KKT_TOLERANCE, 1e-10 );
algorithm << window;
algorithm.solve();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// DEFINE SAMPLE OCP
// -----------------
DifferentialState x, y;
Control u;
DifferentialEquation f;
f << dot(x) == y;
f << dot(y) == u;
OCP ocp( 0.0, 5.0 );
ocp.minimizeLagrangeTerm( x*x + 0.01*u*u );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == 1.0 );
ocp.subjectTo( AT_START, y == 0.0 );
ocp.subjectTo( -1.0 <= u <= 1.0 );
// SETUP OPTIMIZATION ALGORITHM AND DEFINE VARIOUS PLOT WINDOWS
// ------------------------------------------------------------
OptimizationAlgorithm algorithm( ocp );
GnuplotWindow window1;
window1.addSubplot( x, "1st state" );
window1.addSubplot( y, "2nd state","x label","y label",PM_POINTS );
window1.addSubplot( u, "control input","x label","y label",PM_LINES,0,5,-2,2 );
window1.addSubplot( 2.0*sin(x)+y*u, "Why not plotting fancy stuff!?" );
// window1.addSubplot( PLOT_KKT_TOLERANCE, "","iteration","KKT tolerance" );
GnuplotWindow window2( PLOT_AT_EACH_ITERATION );
window2.addSubplot( x, "1st state evolving..." );
algorithm << window1;
algorithm << window2;
algorithm.solve( );
// For illustration, an alternative way to plot differential states
// in form of a VariablesGrid
VariablesGrid states;
algorithm.getDifferentialStates( states );
GnuplotWindow window3;
window3.addSubplot( states(0), "1st state obtained as VariablesGrid" );
window3.addSubplot( states(1), "2nd state obtained as VariablesGrid" );
window3.plot();
VariablesGrid data( 1, 0.0,10.0,11 );
data.setZero();
data( 2,0 ) = 1.0;
data( 5,0 ) = -1.0;
data( 8,0 ) = 2.0;
data( 9,0 ) = 2.0;
// data.setType( VT_CONTROL );
GnuplotWindow window4;
window4.addSubplot( data, "Any data can be plotted" );
window4.plot();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// Define a Right-Hand-Side:
// -------------------------
DifferentialState cA, cB, theta, thetaK;
Control u(2);
DifferentialEquation f;
IntermediateState k1, k2, k3;
k1 = k10*exp(E1/(273.15 +theta));
k2 = k20*exp(E2/(273.15 +theta));
k3 = k30*exp(E3/(273.15 +theta));
f << dot(cA) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(cA0-cA) - k1*cA - k3*cA*cA);
f << dot(cB) == (1/TIMEUNITS_PER_HOUR)* (- u(0)*cB + k1*cA - k2*cB);
f << dot(theta) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(theta0-theta) - (1/(rho*Cp)) *(k1*cA*H1 + k2*cB*H2 + k3*cA*cA*H3)+(kw*AR/(rho*Cp*VR))*(thetaK -theta));
f << dot(thetaK) == (1/TIMEUNITS_PER_HOUR)*((1/(mK*CPK))*(u(1) + kw*AR*(theta-thetaK)));
// DEFINE THE WEIGHTING MATRICES:
// ----------------------------------------------------------
Matrix Q = eye(4);
// Matrix S = eye(4);
Matrix R = eye(2);
// ----------------------------------------------------------
Q(0,0) = sqrt(0.2);
Q(1,1) = sqrt(1.0);
Q(2,2) = sqrt(0.5);
Q(3,3) = sqrt(0.2);
R(0,0) = 0.5000;
R(1,1) = 0.0000005;
// SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
// ----------------------------------------------------------
OCP ocp( 0.0, 1500.0, 22 );
ocp.minimizeLSQ ( Q, R );
// ocp.minimizeLSQEndTerm( S );
ocp.subjectTo( f );
ocp.subjectTo( 3.0 <= u(0) <= 35.0 );
ocp.subjectTo( -9000.0 <= u(1) <= 0.0 );
// ----------------------------------------------------------
// DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
// ----------------------------------------------------------
MPCexport mpc(ocp);
mpc.set( INTEGRATOR_TYPE , INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS , 23 );
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( GENERATE_TEST_FILE,NO );
mpc.exportCode("cstr22_export");
// ----------------------------------------------------------
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x , dx ; // the position of the mounting point and its velocity
DifferentialState L , dL ; // the length of the cable and its velocity
DifferentialState phi, dphi; // the angle phi and its velocity
DifferentialState P0,P1,P2 ; // the variance-covariance states
Control ddx, ddL ; // the accelarations
Parameter T ; // duration of the maneuver
Parameter gamma ; // the confidence level
double const g = 9.81; // the gravitational constant
double const m = 10.0; // the mass at the end of the crane
double const b = 0.1 ; // a frictional constant
DifferentialEquation f(0.0,T);
const double F2 = 50.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x ) == dx + 0.000001*gamma; // small regularization term
f << dot(dx ) == ddx ;
f << dot(L ) == dL ;
f << dot(dL ) == ddL ;
f << dot(phi ) == dphi;
f << dot(dphi) == -(g/L)*phi - ( b + 2.0*dL/L )*dphi - ddx/L;
f << dot(P0) == 2.0*P1;
f << dot(P1) == -(g/L)*P0 - ( b + 2.0*dL/L )*P1 + P2;
f << dot(P2) == -2.0*(g/L)*P1 - 2.0*( b + 2.0*dL/L )*P2 + F2/(m*m*L*L);
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, T, 20 );
ocp.minimizeMayerTerm( 0, T );
ocp.minimizeMayerTerm( 1, -gamma );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == 0.0 );
ocp.subjectTo( AT_START, dx == 0.0 );
ocp.subjectTo( AT_START, L == 70.0 );
ocp.subjectTo( AT_START, dL == 0.0 );
ocp.subjectTo( AT_START, phi == 0.0 );
ocp.subjectTo( AT_START, dphi == 0.0 );
ocp.subjectTo( AT_START, P0 == 0.0 );
ocp.subjectTo( AT_START, P1 == 0.0 );
ocp.subjectTo( AT_START, P2 == 0.0 );
ocp.subjectTo( AT_END , x == 10.0 );
ocp.subjectTo( AT_END , dx == 0.0 );
ocp.subjectTo( AT_END , L == 70.0 );
ocp.subjectTo( AT_END , dL == 0.0 );
ocp.subjectTo( AT_END , -0.075 <= phi - gamma*sqrt(P0) );
ocp.subjectTo( AT_END , phi + gamma*sqrt(P0) <= 0.075 );
ocp.subjectTo( gamma >= 0.0 );
ocp.subjectTo( 5.0 <= T <= 17.0 );
ocp.subjectTo( -0.3 <= ddx <= 0.3 );
ocp.subjectTo( -1.0 <= ddL <= 1.0 );
ocp.subjectTo( -10.0 <= x <= 50.0 );
ocp.subjectTo( -20.0 <= dx <= 20.0 );
ocp.subjectTo( 30.0 <= L <= 75.0 );
ocp.subjectTo( -20.0 <= dL <= 20.0 );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE OCP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(ocp);
algorithm.set( PARETO_FRONT_DISCRETIZATION, 31 );
algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMALIZED_NORMAL_CONSTRAINT );
//algorithm.set( DISCRETIZATION_TYPE , SINGLE_SHOOTING );
// Minimize individual objective function
algorithm.solveSingleObjective(0);
// Minimize individual objective function
algorithm.solveSingleObjective(1);
// Generate Pareto set
//algorithm.set( PARETO_FRONT_HOTSTART , BT_FALSE );
algorithm.solve();
algorithm.getWeights("crane_nnc_weights.txt");
algorithm.getAllDifferentialStates("crane_nnc_states.txt");
algorithm.getAllControls("crane_nnc_controls.txt");
algorithm.getAllParameters("crane_nnc_parameters.txt");
// GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
// ------------------------------------------------
VariablesGrid paretoFront;
algorithm.getParetoFront( paretoFront );
GnuplotWindow window1;
window1.addSubplot( paretoFront, "Pareto Front (robustness versus time)", "TIME","ROBUSTNESS", PM_POINTS );
window1.plot( );
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
// SAVE INFORMATION:
// -----------------
paretoFront.print( "crane_nnc_pareto.txt" );
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE FIXED PARAMETERS:
// ---------------------------
#define v 0.1
#define L 1.0
#define Beta 0.2
#define Delta 0.25
#define E 11250.0
#define k0 1E+06
#define R 1.986
#define K1 250000.0
#define Cin 0.02
#define Tin 340.0
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x1,x2;
Control u ;
DifferentialEquation f( 0.0, L );
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
double Alpha, Gamma;
Alpha = k0*exp(-E/(R*Tin));
Gamma = E/(R*Tin);
f << dot(x1) == Alpha /v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2));
f << dot(x2) == (Alpha*Delta)/v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2)) + Beta/v * (u-x2);
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, L, 50 );
ocp.minimizeMayerTerm( 0, Cin*(1.0-x1) ); // Solve conversion optimal problem
ocp.minimizeMayerTerm( 1, (pow((Tin*x2),2.0)/K1) + 0.005*Cin*(1.0-x1) ); // Solve energy optimal problem (perturbed by small conversion cost;
// otherwise the problem is ill-defined.)
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x1 == 0.0 );
ocp.subjectTo( AT_START, x2 == 0.0 );
ocp.subjectTo( 0.0 <= x1 <= 1.0 );
ocp.subjectTo( (280.0-Tin)/Tin <= x2 <= (400.0-Tin)/Tin );
ocp.subjectTo( (280.0-Tin)/Tin <= u <= (400.0-Tin)/Tin );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE OCP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(ocp);
algorithm.set( INTEGRATOR_TYPE, INT_BDF );
algorithm.set( KKT_TOLERANCE, 1e-8 );
algorithm.set( PARETO_FRONT_GENERATION , PFG_WEIGHTED_SUM );
algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 );
// Generate Pareto set
algorithm.solve();
algorithm.getWeights("plug_flow_reactor_ws_weights.txt");
algorithm.getAllDifferentialStates("plug_flow_reactor_ws_states.txt");
algorithm.getAllControls("plug_flow_reactor_ws_controls.txt");
// VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
// ------------------------------------------
VariablesGrid paretoFront;
algorithm.getParetoFront( paretoFront );
GnuplotWindow window1;
window1.addSubplot( paretoFront, "Pareto Front (conversion versus energy)", "OUTLET CONCENTRATION", "ENERGY",PM_POINTS );
window1.plot( );
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
// SAVE INFORMATION:
// -----------------
paretoFront.print();
return 0;
}
USING_NAMESPACE_ACADO
int main(int argc, char * const argv[ ])
{
//
// Variables
//
DifferentialState x, y, w, dx, dy, dw;
AlgebraicState mu;
Control F;
IntermediateState c, dc;
const double m = 1.0;
const double mc = 1.0;
const double L = 1.0;
const double g = 9.81;
const double p = 5.0;
c = 0.5 * ((x - w) * (x - w) + y * y - L * L);
dc = dy * y + (dw - dx) * (w - x);
//
// Differential algebraic equation
//
DifferentialEquation f;
f << 0 == dot( x ) - dx;
f << 0 == dot( y ) - dy;
f << 0 == dot( w ) - dw;
f << 0 == m * dot( dx ) + (x - w) * mu;
f << 0 == m * dot( dy ) + y * mu + m * g;
f << 0 == mc * dot( dw ) + (w - x) * mu - F;
f << 0 == (x - w) * dot( dx ) + y * dot( dy ) + (w - x) * dot( dw )
- (-p * p * c - 2 * p * dc - dy * dy - (dw - dx) * (dw - dx));
//
// Weighting matrices and reference functions
//
Function rf;
Function rfN;
rf << x << y << w << dx << dy << dw << F;
rfN << x << y << w << dx << dy << dw;
DMatrix W = eye<double>( rf.getDim() );
DMatrix WN = eye<double>( rfN.getDim() ) * 10;
//
// Optimal Control Problem
//
const int N = 10;
const int Ni = 4;
const double Ts = 0.1;
OCP ocp(0, N * Ts, N);
ocp.subjectTo( f );
ocp.minimizeLSQ(W, rf);
ocp.minimizeLSQEndTerm(WN, rfN);
ocp.subjectTo(-20 <= F <= 20);
// ocp.subjectTo( -5 <= x <= 5 );
//
// Export the code:
//
OCPexport mpc( ocp );
mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
mpc.set(DISCRETIZATION_TYPE, MULTIPLE_SHOOTING);
mpc.set(INTEGRATOR_TYPE, INT_IRK_RIIA3);
mpc.set(NUM_INTEGRATOR_STEPS, N * Ni);
mpc.set(SPARSE_QP_SOLUTION, FULL_CONDENSING);
// mpc.set(SPARSE_QP_SOLUTION, CONDENSING);
mpc.set(QP_SOLVER, QP_QPOASES);
// mpc.set(MAX_NUM_QP_ITERATIONS, 20);
mpc.set(HOTSTART_QP, YES);
// mpc.set(SPARSE_QP_SOLUTION, SPARSE_SOLVER);
// mpc.set(QP_SOLVER, QP_FORCES);
// mpc.set(LEVENBERG_MARQUARDT, 1.0e-10);
mpc.set(GENERATE_TEST_FILE, NO);
mpc.set(GENERATE_MAKE_FILE, NO);
mpc.set(GENERATE_MATLAB_INTERFACE, YES);
// mpc.set(USE_SINGLE_PRECISION, YES);
// mpc.set(CG_USE_OPENMP, YES);
if (mpc.exportCode( "pendulum_dae_nmpc_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO;
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x1,x2;
Control u;
Parameter p;
Parameter T;
DifferentialEquation f(0.0,T);
const double t_start = 0.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x1) == (1.0-x2*x2)*x1 - x2 + p*u;
f << dot(x2) == x1;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, T, 27 );
// ocp.minimizeMayerTerm( T );
ocp.minimizeLagrangeTerm(10*x1*x1 + 10*x2*x2 + u*u);
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x1 == 0.0 );
ocp.subjectTo( AT_START, x2 == 1.0 );
ocp.subjectTo( AT_END , x1 == 0.0 );
ocp.subjectTo( AT_END , x2 == 0.0 );
ocp.subjectTo( -0.5 <= u <= 1.0 );
ocp.subjectTo( p == 1.0 );
ocp.subjectTo( 0.0 <= T <= 20.0 );
// VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
// ------------------------------------------
GnuplotWindow window;
window << x1;
window << x2;
window << u;
window << T;
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.initializeControls("van_der_pol_controls.txt");
algorithm << window;
algorithm.solve();
algorithm.getControls("van_der_pol_controls2.txt");
return 0;
}
int main( ){
USING_NAMESPACE_ACADO;
// Parameters
double h_hw = 10; // water level
double A_hw = 1.0; // amplitude of the waves
double T_hw = 5.0; // duration of a wave
double h_b = 3.0; // height of the buoy
double rho = 1000; // density of water
double A = 1.0; // bottom area of the buoy
double m = 100; // mass of the buoy
double g = 9.81; // gravitational constant
// Free parameter
Control u;
// Variables
DifferentialState h; // Position of the buoy
DifferentialState v; // Velocity of the buoy
DifferentialState w; // Produced wave energy
TIME t;
// Differential equation
DifferentialEquation f;
// Height of the wave
IntermediateState hw;
hw = h_hw + A_hw*sin(2*M_PI*t/T_hw);
f << dot(h) == v;
f << dot(v) == rho*A*(hw-h)/m - g - u;
f << dot(w) == u*v;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0 ;
const double t_end = 15;
OCP ocp( t_start, t_end, 100 );
ocp.maximizeMayerTerm( w );
ocp.subjectTo( f );
// double x_start[3];
// x_start[0] = h_hw - 0*A_hw;
// x_start[1] = 0;
// x_start[2] = 0;
ocp.subjectTo( AT_START, h - (h_hw-A_hw) == 0.0 );
ocp.subjectTo( AT_START, v == 0.0 );
ocp.subjectTo( AT_START, w == 0.0 );
ocp.subjectTo( -h_b <= h-hw <= 0.0 );
ocp.subjectTo( 0.0 <= u <= 100.0 );
// DEFINE A PLOT WINDOW:
// ---------------------
GnuplotWindow window;
window.addSubplot( h,"Height of buoy" );
window.addSubplot( v,"Velocity of buoy" );
window.addSubplot( w,"Objective function " );
window.addSubplot( u,"Resistance" );
window.addSubplot( hw,"Wave height" );
// window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set( MAX_NUM_ITERATIONS, 100 );
// algorithm.set( KKT_TOLERANCE, 1e-10 );
algorithm << window;
algorithm.solve();
return 0;
}
int main(int argc, char* argv[])
{
if(argc < 2)
{
std::cerr << "You should provide a system as argument, either gazebo or flat_fish." << std::endl;
return 1;
}
//========================================================================================
// SETTING VARIABLES
//========================================================================================
std::string system = argv[1];
double weight;
double buoyancy;
std::string dataFolder;
if(system == "gazebo")
{
weight = 4600;
buoyancy = 4618;
dataFolder = "./config/gazebo/";
}
else if(system == "flatfish" || system == "flat_fish")
{
weight = 2800;
buoyancy = 2812;
dataFolder = "./config/flatfish/";
}
else
{
std::cerr << "Unknown system " << system << "." << std::endl;
return 1;
}
double uncertainty = 0;
/* true - Export the code to the specified folder
* false - Simulates the controller inside Acado's environment */
std::string rockFolder = "/home/rafaelsaback/rock/1_dev/control/uwv_model_pred_control/src";
/***********************
* CONTROLLER SETTINGS
***********************/
// Prediction and control horizon
double horizon = 2;
double sampleTime = 0.1;
int iteractions = horizon / sampleTime;
bool positionControl = false;
//========================================================================================
// DEFINING VARIABLES
//========================================================================================
DifferentialEquation f; // Variable that will carry 12 ODEs (6 for position and 6 for velocity)
DifferentialState v("Velocity", 6, 1); // Velocity States
DifferentialState n("Pose", 6, 1); // Pose States
DifferentialState tau("Efforts", 6, 1); // Effort
Control tauDot("Efforts rate", 6, 1); // Effort rate of change
DVector rg(3); // Center of gravity
DVector rb(3); // Center of buoyancy
DMatrix M(6, 6); // Inertia matrix
DMatrix Minv(6, 6); // Inverse of inertia matrix
DMatrix Dl(6, 6); // Linear damping matrix
DMatrix Dq(6, 6); // Quadratic damping matrix
// H-representation of the output domain's polyhedron
DMatrix AHRep(12,6);
DMatrix BHRep(12,1);
Expression linearDamping(6, 6); // Dl*v
Expression quadraticDamping(6, 6); // Dq*|v|*v
Expression gravityBuoyancy(6); // g(n)
Expression absV(6, 6); // |v|
Expression Jv(6); // J(n)*v
Expression vDot(6); // AUV Fossen's equation
Function J; // Cost function
Function Jn; // Terminal cost function
AHRep = readVariable("./config/", "a_h_rep.dat");
BHRep = readVariable("./config/", "b_h_rep.dat");
M = readVariable(dataFolder, "m_matrix.dat");
Dl = readVariable(dataFolder, "dl_matrix.dat");
Dq = readVariable(dataFolder, "dq_matrix.dat");
/***********************
* LEAST-SQUARES PROBLEM
***********************/
// Cost Functions
if (positionControl)
{
J << n;
Jn << n;
}
else
{
J << v;
Jn << v;
}
J << tauDot;
// Weighting Matrices for simulational controller
DMatrix Q = eye<double>(J.getDim());
DMatrix QN = eye<double>(Jn.getDim());
Q = readVariable("./config/", "q_matrix.dat");
//========================================================================================
// MOTION MODEL EQUATIONS
//========================================================================================
rg(0) = 0; rg(1) = 0; rg(2) = 0;
rb(0) = 0; rb(1) = 0; rb(2) = 0;
M *= (2 -(1 + uncertainty));
Dl *= 1 + uncertainty;
Dq *= 1 + uncertainty;
SetAbsV(absV, v);
Minv = M.inverse();
linearDamping = Dl * v;
quadraticDamping = Dq * absV * v;
SetGravityBuoyancy(gravityBuoyancy, n, rg, rb, weight, buoyancy);
SetJv(Jv, v, n);
// Dynamic Equation
vDot = Minv * (tau - linearDamping - quadraticDamping - gravityBuoyancy);
// Differential Equations
f << dot(v) == vDot;
f << dot(n) == Jv;
f << dot(tau) == tauDot;
//========================================================================================
// OPTIMAL CONTROL PROBLEM (OCP)
//========================================================================================
OCP ocp(0, horizon, iteractions);
// Weighting Matrices for exported controller
BMatrix Qexp = eye<bool>(J.getDim());
BMatrix QNexp = eye<bool>(Jn.getDim());
ocp.minimizeLSQ(Qexp, J);
ocp.minimizeLSQEndTerm(QNexp, Jn);
ocp.subjectTo(f);
// Individual degrees of freedom constraints
ocp.subjectTo(tau(3) == 0);
ocp.subjectTo(tau(4) == 0);
/* Note: If the constraint for Roll is not defined the MPC does not
* work since there's no possible actuation in that DOF.
* Always consider Roll equal to zero. */
// Polyhedron set of inequalities
ocp.subjectTo(AHRep*tau - BHRep <= 0);
//========================================================================================
// CODE GENERATION
//========================================================================================
// Export code to ROCK
OCPexport mpc(ocp);
int Ni = 1;
mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
mpc.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);
mpc.set(INTEGRATOR_TYPE, INT_RK4);
mpc.set(NUM_INTEGRATOR_STEPS, iteractions * Ni);
mpc.set(HOTSTART_QP, YES);
mpc.set(QP_SOLVER, QP_QPOASES);
mpc.set(GENERATE_TEST_FILE, NO);
mpc.set(GENERATE_MAKE_FILE, NO);
mpc.set(GENERATE_MATLAB_INTERFACE, NO);
mpc.set(GENERATE_SIMULINK_INTERFACE, NO);
mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
mpc.set(CG_HARDCODE_CONSTRAINT_VALUES, YES);
if (mpc.exportCode(rockFolder.c_str()) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE);
mpc.printDimensionsQP();
return EXIT_SUCCESS;
}
int main( ){
counter = 0;
// DIFFERENTIAL STATES :
// -------------------------
DifferentialState r; // the length r of the cable
DifferentialState phi; // the angle phi
DifferentialState theta; // the angle theta
// ------------------------- // -------------------------------------------
DifferentialState dr; // first derivative of r0 with respect to t
DifferentialState dphi; // first derivative of phi with respect to t
DifferentialState dtheta; // first derivative of theta with respect to t
// ------------------------- // -------------------------------------------
DifferentialState n; // winding number
// ------------------------- // -------------------------------------------
DifferentialState Psi; // the roll angle Psi
DifferentialState CL; // the aerodynamic lift coefficient
// ------------------------- // -------------------------------------------
DifferentialState W; // integral over the power at the generator
// ( = ENERGY )
// CONTROL :
// -------------------------
Control ddr0; // second derivative of r0 with respect to t
Control dPsi; // first derivative of Psi with respect to t
Control dCL; // first derivative of CL with respect to t
// THE "RIGHT-HAND-SIDE" OF THE ODE:
// ---------------------------------------------------------------
DifferentialEquation f;
const int NX = 10;
const int NU = 3 ;
IntermediateState x( NX + NU );
x(0) = r;
x(1) = phi;
x(2) = theta;
x(3) = dr;
x(4) = dphi;
x(5) = dtheta;
x(6) = n;
x(7) = Psi;
x(8) = CL;
x(9) = W;
x(10) = ddr0;
x(11) = dPsi;
x(12) = dCL;
CFunction kiteModel( 10, ffcn_model );
f << kiteModel( x );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, 18.0, 18 );
ocp.minimizeMayerTerm( W );
ocp.subjectTo( f );
// INITIAL VALUE CONSTRAINTS:
// ---------------------------------
ocp.subjectTo( AT_START, n == 0.0 );
ocp.subjectTo( AT_START, W == 0.0 );
// PERIODIC BOUNDARY CONSTRAINTS:
// ----------------------------------------
ocp.subjectTo( 0.0, r , -r , 0.0 );
ocp.subjectTo( 0.0, phi , -phi , 0.0 );
ocp.subjectTo( 0.0, theta , -theta , 0.0 );
ocp.subjectTo( 0.0, dr , -dr , 0.0 );
ocp.subjectTo( 0.0, dphi , -dphi , 0.0 );
ocp.subjectTo( 0.0, dtheta, -dtheta, 0.0 );
ocp.subjectTo( 0.0, Psi , -Psi , 0.0 );
ocp.subjectTo( 0.0, CL , -CL , 0.0 );
ocp.subjectTo( -0.34 <= phi <= 0.34 );
ocp.subjectTo( 0.85 <= theta <= 1.45 );
ocp.subjectTo( -40.0 <= dr <= 10.0 );
ocp.subjectTo( -0.29 <= Psi <= 0.29 );
ocp.subjectTo( 0.1 <= CL <= 1.50 );
ocp.subjectTo( -0.7 <= n <= 0.90 );
ocp.subjectTo( -25.0 <= ddr0 <= 25.0 );
ocp.subjectTo( -0.065 <= dPsi <= 0.065 );
ocp.subjectTo( -3.5 <= dCL <= 3.5 );
// CREATE A PLOT WINDOW AND VISUALIZE THE RESULT:
// ----------------------------------------------
GnuplotWindow window;
window.addSubplot( r, "CABLE LENGTH r [m]" );
window.addSubplot( phi, "POSITION ANGLE phi [rad]" );
window.addSubplot( theta,"POSITION ANGLE theta [rad]" );
window.addSubplot( Psi, "ROLL ANGLE psi [rad]" );
window.addSubplot( CL, "LIFT COEFFICIENT CL" );
window.addSubplot( W, "ENERGY W [MJ]" );
window.addSubplot( phi,theta, "Kite Orbit","theta [rad]","phi [rad]" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.initializeDifferentialStates("powerkite_states.txt" );
algorithm.initializeControls ("powerkite_controls.txt" );
algorithm.set ( MAX_NUM_ITERATIONS, 100 );
algorithm.set ( KKT_TOLERANCE , 1e-2 );
algorithm << window;
algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
algorithm.solve();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO;
// VARIABLES:
// ------------------------
DifferentialState x; // Position of the trolley
DifferentialState v; // Velocity of the trolley
DifferentialState phi; // excitation angle
DifferentialState dphi; // rotational velocity
Control ax; // trolley accelaration
Disturbance W; // disturbance
double L = 1.0 ; // length
double m = 1.0 ; // mass
double g = 9.81; // gravitational constant
double b = 0.2 ; // friction coefficient
// DIFFERENTIAL EQUATION:
// ------------------------
DifferentialEquation f, fSim; // The model equations
f << dot(x) == v;
f << dot(v) == ax;
f << dot(phi ) == dphi;
f << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
L = 1.2; // introduce model plant mismatch
fSim << dot(x) == v;
fSim << dot(v) == ax + W;
fSim << dot(phi ) == dphi;
fSim << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
// DEFINE LEAST SQUARE FUNCTION:
// -----------------------------
Function h;
h << x;
h << v;
h << phi;
h << dphi;
DMatrix Q(4,4); // LSQ coefficient matrix
Q.setIdentity();
DVector r(4); // Reference
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 5.0;
OCP ocp( t_start, t_end, 25 );
ocp.minimizeLSQ( Q, h, r );
ocp.subjectTo( f );
ocp.subjectTo( -5.0 <= ax <= 5.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( fSim,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance; disturbance.read( "dist.txt" );
if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
double samplingTime = 0.1;
RealTimeAlgorithm alg( ocp, samplingTime );
// alg.set( USE_REALTIME_ITERATIONS,NO );
// alg.set( MAX_NUM_ITERATIONS,20 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg, zeroReference );
DVector x0(4);
x0.setZero();
x0(3) = 1.0;
double startTime = 0.0;
double endTime = 20.0;
DVector uCon;
VariablesGrid ySim;
if (controller.init( startTime,x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
controller.getU( uCon );
if (process.init( startTime,x0,uCon ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
process.getY( ySim );
// hand-coding call to
// sim.run( )
int nSteps = 0;
double currentTime = startTime;
while ( currentTime <= endTime )
{
printf( "\n*** Simulation Loop No. %d (starting at time %.3f) ***\n",nSteps,currentTime );
if (controller.step( currentTime,ySim.getLastVector() ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
controller.getU( uCon );
if (process.step( currentTime,currentTime+samplingTime,uCon ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
process.getY( ySim );
++nSteps;
currentTime = (double)nSteps * samplingTime;
}
return EXIT_SUCCESS;
}
int main()
{
USING_NAMESPACE_ACADO
// Define a Right-Hand-Side:
DifferentialState cA, cB, theta, thetaK;
Control u( 2 );
DifferentialEquation f;
IntermediateState k1, k2, k3;
k1 = k10*exp(E1/(273.15 +theta));
k2 = k20*exp(E2/(273.15 +theta));
k3 = k30*exp(E3/(273.15 +theta));
f << dot(cA) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(cA0-cA) - k1*cA - k3*cA*cA);
f << dot(cB) == (1/TIMEUNITS_PER_HOUR)* (- u(0)*cB + k1*cA - k2*cB);
f << dot(theta) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(theta0-theta) - (1/(rho*Cp)) *(k1*cA*H1 + k2*cB*H2 + k3*cA*cA*H3)+(kw*AR/(rho*Cp*VR))*(thetaK -theta));
f << dot(thetaK) == (1/TIMEUNITS_PER_HOUR)*((1/(mK*CPK))*(u(1) + kw*AR*(theta-thetaK)));
// Reference functions and weighting matrices:
Function h, hN;
h << cA << cB << theta << thetaK << u;
hN << cA << cB << theta << thetaK;
Matrix W = eye( h.getDim() );
Matrix WN = eye( hN.getDim() );
W(0, 0) = WN(0, 0) = 0.2;
W(1, 1) = WN(1, 1) = 1.0;
W(2, 2) = WN(2, 2) = 0.5;
W(3, 3) = WN(3, 3) = 0.2;
W(4, 4) = 0.5000;
W(5, 5) = 0.0000005;
//
// Optimal Control Problem
//
OCP ocp(0.0, 1500.0, 10);
ocp.subjectTo( f );
ocp.minimizeLSQ(W, h);
ocp.minimizeLSQEndTerm(WN, hN);
ocp.subjectTo( 3.0 <= u(0) <= 35.0 );
ocp.subjectTo( -9000.0 <= u(1) <= 0.0 );
ocp.subjectTo( cA <= 2.5 );
// ocp.subjectTo( cB <= 1.055 );
// Export the code:
OCPexport mpc( ocp );
mpc.set( INTEGRATOR_TYPE , INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS , 20 );
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( QP_SOLVER, QP_QPOASES );
mpc.set( GENERATE_TEST_FILE,NO );
// mpc.set( USE_SINGLE_PRECISION,YES );
// mpc.set( GENERATE_SIMULINK_INTERFACE,YES );
if (mpc.exportCode( "cstr_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// ----------------------------
DifferentialState x1,x2;
Control u ;
Parameter t1 ;
DifferentialEquation f(0.0,t1);
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(x1) == x2;
f << dot(x2) == u;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp(0.0,t1,25);
ocp.minimizeMayerTerm( 0, x2 );
ocp.minimizeMayerTerm( 1, 2.0*t1/20.0);
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x1 == 0.0 );
ocp.subjectTo( AT_START, x2 == 0.0 );
ocp.subjectTo( AT_END , x1 == 200.0 );
ocp.subjectTo( 0.0 <= x1 <= 200.0001 );
ocp.subjectTo( 0.0 <= x2 <= 40.0 );
ocp.subjectTo( 0.0 <= u <= 5.0 );
ocp.subjectTo( 0.1 <= t1 <= 50.0 );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE OCP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(ocp);
algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 );
algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMAL_BOUNDARY_INTERSECTION );
algorithm.set( KKT_TOLERANCE, 1e-8 );
// Minimize individual objective function
algorithm.solveSingleObjective(0);
// Minimize individual objective function
algorithm.solveSingleObjective(1);
// Generate Pareto set
algorithm.solve();
algorithm.getWeights("car_nbi_weights.txt");
algorithm.getAllDifferentialStates("car_nbi_states.txt");
algorithm.getAllControls("car_nbi_controls.txt");
algorithm.getAllParameters("car_nbi_parameters.txt");
// GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
// ------------------------------------------------
VariablesGrid paretoFront;
algorithm.getParetoFront( paretoFront );
GnuplotWindow window1;
window1.addSubplot( paretoFront, "Pareto Front (time versus energy)", "ENERGY","TIME", PM_POINTS );
window1.plot( );
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
// SAVE INFORMATION:
// -----------------
FILE *file = fopen("car_nbi_pareto.txt","w");
file << paretoFront;
fclose(file);
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState s,v,m;
Control u ;
Parameter T ;
DifferentialEquation f( 0.0, T );
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(s) == v;
f << dot(v) == (u-0.2*v*v)/m;
f << dot(m) == -0.01*u*u;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0, T, 20 );
ocp.minimizeMayerTerm( T );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, s == 0.0 );
ocp.subjectTo( AT_START, v == 0.0 );
ocp.subjectTo( AT_START, m == 1.0 );
ocp.subjectTo( AT_END , s == 10.0 );
ocp.subjectTo( AT_END , v == 0.0 );
ocp.subjectTo( -0.1 <= v <= 1.7 );
ocp.subjectTo( -1.1 <= u <= 1.1 );
ocp.subjectTo( 5.0 <= T <= 15.0 );
// VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
// ------------------------------------------
GnuplotWindow window;
window.addSubplot( s, "THE DISTANCE s" );
window.addSubplot( v, "THE VELOCITY v" );
window.addSubplot( m, "THE MASS m" );
window.addSubplot( u, "THE CONTROL INPUT u" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set( MAX_NUM_ITERATIONS, 20 );
algorithm << window;
// algorithm.initializeDifferentialStates("tor_states.txt");
// algorithm.initializeParameters("tor_pars.txt");
// algorithm.initializeControls("tor_controls.txt");
algorithm.solve();
// algorithm.getDifferentialStates("tor_states.txt");
// algorithm.getParameters("tor_pars.txt");
// algorithm.getControls("tor_controls.txt");
return 0;
}
USING_NAMESPACE_ACADO
int main( ){
// INTRODUCE THE VARIABLES:
// --------------------------------------------------
DifferentialState s,v,m,L;
Control u ;
DifferentialEquation f ;
// DEFINE THE DIMENSIONS OF THE C-FUNCTIONS:
// --------------------------------------------------
CFunction F( NX, myDifferentialEquation );
CFunction M( NJ, myObjectiveFunction );
CFunction I( NI, myInitialValueConstraint );
CFunction E( NE, myEndPointConstraint );
CFunction H( NH, myInequalityPathConstraint );
// DEFINE THE OPTIMIZATION VARIABLES:
// --------------------------------------------------
IntermediateState x(5);
x(0) = v; x(1) = m; x(2) = u; x(3) = L; x(4) = s;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( 0.0, 10.0 );
ocp.minimizeMayerTerm( M(x) );
ocp.subjectTo( f << F(x) );
ocp.subjectTo( AT_START, I(x) == 0.0 );
ocp.subjectTo( AT_END , E(x) == 0.0 );
ocp.subjectTo( H(x) <= 1.3 );
// VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
// ------------------------------------------
GnuplotWindow window1;
window1.addSubplot( s,"DifferentialState s" );
window1.addSubplot( v,"DifferentialState v" );
window1.addSubplot( m,"DifferentialState m" );
window1.addSubplot( u,"Control u" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
algorithm.set( KKT_TOLERANCE, 1e-3 );
algorithm << window1;
algorithm.solve();
return 0;
}
