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
TIME autotime;
DifferentialState x(2);
AlgebraicState z;
Control u;
DifferentialEquation f1;
IntermediateState setc_is_1(5);
setc_is_1(0) = autotime;
setc_is_1(1) = x(0);
setc_is_1(2) = x(1);
setc_is_1(3) = z;
setc_is_1(4) = u;
CFunction cLinkModel_1( 3, myAcadoDifferentialEquation1 );
f1 << cLinkModel_1(setc_is_1);
double dconstant1 = 0.0;
double dconstant2 = 5.0;
int dconstant3 = 10;
OCP ocp1(dconstant1, dconstant2, dconstant3);
ocp1.minimizeMayerTerm(x(1));
ocp1.subjectTo(f1);
ocp1.subjectTo(AT_START, x(0) == 1.0 );
ocp1.subjectTo(AT_START, x(1) == 0.0 );
GnuplotWindow window;
window.addSubplot(x(0),"DIFFERENTIAL STATE x");
window.addSubplot(z,"ALGEBRAIC STATE z" );
window.addSubplot(u,"CONTROL u" );
OptimizationAlgorithm algo1(ocp1);
algo1.set( KKT_TOLERANCE, 1.000000E-05 );
algo1.set( RELAXATION_PARAMETER, 1.500000E+00 );
// algo1.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
//
algo1 << window;
algo1.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;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// DEFINE A VARIABLES GRID:
// ------------------------
Grid dataGrid( 0.0, 5.0, 6 );
VariablesGrid data;
data.init( 2, dataGrid );
data( 0, 0 ) = 0.0; data( 0, 1 ) = 1.0 ;
data( 1, 0 ) = 0.2; data( 1, 1 ) = 0.8 ;
data( 2, 0 ) = 0.4; data( 2, 1 ) = 0.7 ;
data( 3, 0 ) = 0.6; data( 3, 1 ) = 0.65 ;
data( 4, 0 ) = 0.8; data( 4, 1 ) = 0.625;
data( 5, 0 ) = 1.0; data( 5, 1 ) = 0.613;
// CONSTRUCT A CURVE INTERPOLATING THE DATA:
// -----------------------------------------
Curve c1, c2;
c1.add( data, IM_CONSTANT );
c2.add( data, IM_LINEAR );
// PLOT CURVES ON GIVEN GRID:
// --------------------------
GnuplotWindow window;
window.addSubplot( c1, 0.0,5.0, "Constant data Interpolation" );
window.addSubplot( c2, 0.0,5.0, "Linear data Interpolation" );
window.plot();
return 0;
}
int main( )
{
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState xB;
DifferentialState xW;
DifferentialState vB;
DifferentialState vW;
Control F;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
double h = 0.05;
DiscretizedDifferentialEquation f( h );
// f << next(xB) == ( 9.523918456856767e-01*xB - 3.093442425036754e-03*xW + 4.450257887258270e-04*vW - 2.380407715716160e-07*F );
// f << next(xW) == ( -1.780103154903307e+00*xB - 1.005721624707961e+00*xW - 3.093442425036752e-03*vW - 8.900515774516536e-06*F );
// f << next(vB) == ( -5.536210379145256e+00*xB - 2.021981836435758e-01*xW + 1.0*vB + 2.474992857984263e-02*vW + 1.294618052471308e-04*F );
// f << next(vW) == ( 1.237376970014700e+01*xB + 1.183104351525840e+01*xW - 1.005721624707961e+00*vW + 6.186884850073496e-05*F );
f << next(xB) == ( 0.9335*xB + 0.0252*xW + 0.048860*vB + 0.000677*vW + 3.324e-06*F );
f << next(xW) == ( 0.1764*xB - 0.9821*xW + 0.004739*vB - 0.002591*vW - 8.822e-06*F );
f << next(vB) == ( -2.5210*xB - 0.1867*xW + 0.933500*vB + 0.025200*vW + 0.0001261*F );
f << next(vW) == ( -1.3070*xB + 11.670*xW + 0.176400*vB - 0.982100*vW + 6.536e-05*F );
OutputFcn g;
g << xB;
g << 500.0*vB + F;
DynamicSystem dynSys( f,g );
// SETUP THE PROCESS:
// ------------------
Vector mean( 1 ), amplitude( 1 );
mean.setZero( );
amplitude.setAll( 50.0 );
GaussianNoise myNoise( mean,amplitude );
Actuator myActuator( 1 );
myActuator.setControlNoise( myNoise,0.1 );
myActuator.setControlDeadTimes( 0.1 );
mean.setZero( );
amplitude.setAll( 0.001 );
UniformNoise myOutputNoise1( mean,amplitude );
mean.setAll( 20.0 );
amplitude.setAll( 10.0 );
GaussianNoise myOutputNoise2( mean,amplitude );
Sensor mySensor( 2 );
mySensor.setOutputNoise( 0,myOutputNoise1,0.1 );
mySensor.setOutputNoise( 1,myOutputNoise2,0.1 );
mySensor.setOutputDeadTimes( 0.15 );
Process myProcess;
myProcess.setDynamicSystem( dynSys );
myProcess.set( ABSOLUTE_TOLERANCE,1.0e-8 );
myProcess.setActuator( myActuator );
myProcess.setSensor( mySensor );
Vector x0( 4 );
x0.setZero( );
x0( 0 ) = 0.01;
myProcess.initializeStartValues( x0 );
myProcess.set( PLOT_RESOLUTION,HIGH );
// myProcess.set( CONTROL_PLOTTING,PLOT_NOMINAL );
// myProcess.set( PARAMETER_PLOTTING,PLOT_NOMINAL );
myProcess.set( OUTPUT_PLOTTING,PLOT_REAL );
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( g(0),"Output 1" );
window.addSubplot( g(1),"Output 2" );
myProcess << window;
// SIMULATE AND GET THE RESULTS:
// -----------------------------
VariablesGrid u( 1,0.0,1.0,6 );
u( 0,0 ) = 10.0;
u( 1,0 ) = -200.0;
u( 2,0 ) = 200.0;
u( 3,0 ) = 200.0;
u( 4,0 ) = 0.0;
u( 5,0 ) = 0.0;
myProcess.init( 0.0,x0,u.getFirstVector() );
myProcess.run( u );
VariablesGrid uNom, uSim, ySim, ySens, xSim;
// myProcess.getLast( LOG_NOMINAL_CONTROLS,uNom ); uNom.print( "uNom" );
// myProcess.getLast( LOG_SIMULATED_CONTROLS,uSim ); uSim.print( "uSim" );
// myProcess.getLast( LOG_SIMULATED_OUTPUT,ySim ); ySim.print( "ySim" );
// myProcess.getLast( LOG_PROCESS_OUTPUT,ySens ); ySens.print( "ySens" );
//
// myProcess.getLast( LOG_DIFFERENTIAL_STATES,xSim );
return 0;
}
/* >>> 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;
}
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;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// DEFINE VALRIABLES:
// ---------------------------
DifferentialStateVector q(2); // the generalized coordinates of the pendulum
DifferentialStateVector dq(2); // the associated velocities
const double L1 = 1.00; // length of the first pendulum
const double L2 = 1.00; // length of the second pendulum
const double m1 = 1.00; // mass of the first pendulum
const double m2 = 1.00; // mass of the second pendulum
const double g = 9.81; // gravitational constant
const double alpha = 0.10; // a friction constant
const double J_11 = (m1+m2)*L1*L1; // auxiliary variable (inertia comp.)
const double J_22 = m2 *L2*L2; // auxiliary variable (inertia comp.)
const double J_12 = m2 *L1*L2; // auxiliary variable (inertia comp.)
const double E1 = -(m1+m2)*g*L1; // auxiliary variable (pot energy 1)
const double E2 = - m2 *g*L2; // auxiliary variable (pot energy 2)
IntermediateState c1;
IntermediateState c2;
IntermediateState c3;
IntermediateState T;
IntermediateState V;
IntermediateStateVector Q;
// COMPUTE THE KINETIC ENERGY T AND THE POTENTIAL V:
// -------------------------------------------------
c1 = cos(q(0));
c2 = cos(q(1));
c3 = cos(q(0)+q(1));
T = 0.5*J_11*dq(0)*dq(0) + 0.5*J_22*dq(1)*dq(1) + J_12*c3*dq(0)*dq(1);
V = E1*c1 + E2*c2;
Q = (-alpha*dq);
// AUTOMATICALLY DERIVE THE EQUATIONS OF MOTION BASED ON THE LAGRANGIAN FORMALISM:
// -------------------------------------------------------------------------------
DifferentialEquation f;
LagrangianFormalism( f, T - V, Q, q, dq );
// Define an integrator:
// ---------------------
IntegratorBDF integrator( f );
// Define an initial value:
// ------------------------
double x_start[4] = { 0.0, 0.5, 0.0, 0.1 };
double t_start = 0.0;
double t_end = 3.0;
// START THE INTEGRATION
// ----------------------
integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
integrator.set( INTEGRATOR_TOLERANCE, 1e-12 );
integrator.freezeAll();
integrator.integrate( x_start, t_start, t_end );
VariablesGrid xres;
integrator.getTrajectory(&xres,NULL,NULL,NULL,NULL,NULL);
GnuplotWindow window;
window.addSubplot( xres(0), "The excitation angle of pendulum 1" );
window.addSubplot( xres(1), "The excitation angle of pendulum 2" );
window.addSubplot( xres(2), "The angular velocity of pendulum 1" );
window.addSubplot( xres(3), "The angular velocity of pendulum 2" );
window.plot();
return 0;
}
int main( ) {
USING_NAMESPACE_ACADO
// 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
// PARAMETERS :
// ------------------------
// PARAMETERS OF THE KITE :
// -----------------------------
double mk = 850.00; // mass of the kite // [ kg ]
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 :
// ===============================================================
// CROSS AREA 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 :
// ---------------------------------------------------------------
h = r*cos(theta) ;
w = log(h/hr) / log(h0/hr) *w0 ;
// 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;
// ---------------------------
// 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;
// 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( F[0], "FORCE IN CABLE [N]" );
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( ){
// Define a Right-Hand-Side:
// -------------------------
DifferentialState x("", 4, 1), P("", 4, 4);
Control u("", 2, 1);
IntermediateState rhs = cstrModel( x, u );
DMatrix Q = zeros<double>(4,4);
Q(0,0) = 0.2;
Q(1,1) = 1.0;
Q(2,2) = 0.5;
Q(3,3) = 0.2;
DMatrix R = zeros<double>(2,2);
R(0,0) = 0.5;
R(1,1) = 5e-7;
DifferentialEquation f;
f << dot(x) == rhs;
f << dot(P) == getRiccatiODE( rhs, x, u, P, Q, R );
// Define an integrator:
// ---------------------
IntegratorRK45 integrator( f );
integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
integrator.set( PRINT_INTEGRATOR_PROFILE, YES );
// Define an initial value:
// ------------------------
//double x_ss[4] = { 2.14, 1.09, 114.2, 112.9 };
double x_start[20] = { 1.0, 0.5, 100.0, 100.0, 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0 };
double u_start[2] = { 14.19, -1113.5 };
// double u_start[2] = { 10.00, -7000.0 };
Grid timeInterval( 0.0, 5000.0, 100 );
integrator.freezeAll();
integrator.integrate( timeInterval, x_start, 0 ,0, u_start );
// GET THE RESULTS
// ---------------
VariablesGrid differentialStates;
integrator.getX( differentialStates );
DVector PP = differentialStates.getLastVector();
DMatrix PPP(4,4);
for( int i=0; i<4; ++i )
for( int j=0; j<4; ++j )
PPP(i,j) = PP(4+i*4+j);
PPP.print( "P1.txt","",PS_PLAIN );
// PPP.printToFile( "P2.txt","",PS_PLAIN );
GnuplotWindow window;
window.addSubplot( differentialStates(0), "cA [mol/l]" );
window.addSubplot( differentialStates(1), "cB [mol/l]" );
window.addSubplot( differentialStates(2), "theta [C]" );
window.addSubplot( differentialStates(3), "thetaK [C]" );
window.addSubplot( differentialStates(4 ), "P11" );
window.addSubplot( differentialStates(9 ), "P22" );
window.addSubplot( differentialStates(14), "P33" );
window.addSubplot( differentialStates(19), "P44" );
window.plot();
return 0;
}
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 THE VARIABLES:
// -------------------------
const int N = 2;
DifferentialState x, y("", N, 1);
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.9*x*x + u;
int i;
for( i = 0; i < N; i++ )
f << dot( y(i) ) == -y(i) + 0.5*y(i)*y(i) + u;
// DEFINE LEAST SQUARE FUNCTION:
// -----------------------------
Function h,m;
h << x;
h << 2.0*u;
m << 10.0*x ;
m << 0.1*x*x;
DMatrix S(2,2);
DVector r(2);
S.setIdentity();
r.setAll( 0.1 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 5 );
ocp.minimizeLSQ ( S, h, r );
ocp.minimizeLSQEndTerm( S, m, r );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == 1.0 );
for( i = 0; i < N; i++ )
ocp.subjectTo( AT_START, y(i) == 1.0 );
// Additionally, flush a plotting object
GnuplotWindow window;
window.addSubplot( x,"DifferentialState x" );
window.addSubplot( u,"Control u" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm << window;
// algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
// algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY_LIFTED );
// algorithm.set( HESSIAN_APPROXIMATION, CONSTANT_HESSIAN );
// algorithm.set( HESSIAN_APPROXIMATION, FULL_BFGS_UPDATE );
// algorithm.set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
// algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON_WITH_BLOCK_BFGS );
// algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
// Necessary to use with GN Hessian approximation.
algorithm.set( LEVENBERG_MARQUARDT, 1e-10 );
algorithm.solve();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState xB;
DifferentialState xW;
DifferentialState vB;
DifferentialState vW;
Control R;
Control F;
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;
Matrix Q(4,4);
Q.setIdentity();
Q(0,0) = 10.0;
Q(1,1) = 10.0;
Vector r(4);
r.setAll( 0.0 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 1.0;
OCP ocp( t_start, t_end, 20 );
ocp.minimizeLSQ( Q, h, r );
ocp.subjectTo( f );
ocp.subjectTo( -500.0 <= F <= 500.0 );
ocp.subjectTo( R == 0.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance = readFromFile( "road.txt" );
process.setProcessDisturbance( disturbance );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp,0.05 );
alg.set( MAX_NUM_ITERATIONS, 2 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
SimulationEnvironment sim( 0.0,3.0,process,controller );
Vector x0(4);
x0(0) = 0.01;
x0(1) = 0.0;
x0(2) = 0.0;
x0(3) = 0.0;
sim.init( x0 );
sim.run( );
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "Body Position [m]" );
window.addSubplot( sampledProcessOutput(1), "Wheel Position [m]" );
window.addSubplot( sampledProcessOutput(2), "Body Velocity [m/s]" );
window.addSubplot( sampledProcessOutput(3), "Wheel Velocity [m/s]" );
window.addSubplot( feedbackControl(1), "Damping Force [N]" );
window.addSubplot( feedbackControl(0), "Road Excitation [m]" );
window.plot( );
return 0;
}
int main( ){
double clock1 = clock();
double t_start = 0.0;
double t_end = 600.0;
int intervals = 5 ;
int i;
TIME t;
DifferentialState x("", NXD, 1);
AlgebraicState z("", NXA, 1);
Control u("", NU, 1);
Parameter p("", NP, 1);
IntermediateState is(1+NXD+NXA+NU+NP);
is(0) = t;
for (i=0; i < NXD; ++i) is(1+i) = x(i);
for (i=0; i < NXA; ++i) is(1+NXD+i) = z(i);
for (i=0; i < NU; ++i) is(1+NXD+NXA+i) = u(i);
for (i=0; i < NP; ++i) is(1+NXD+NXA+NU+i) = p(i);
CFunction hydroscalModel( NXD+NXA, ffcn_model );
// Define a Right-Hand-Side:
// -------------------------
DifferentialEquation f;
f << hydroscalModel(is);
// DEFINE INITIAL VALUES:
// ----------------------
double xd[NXD] = { 2.1936116177990631E-01, // X_0
3.3363028623863722E-01, // X_1
3.7313133250625952E-01, // X_2
3.9896472354654333E-01, // X_3
4.1533719381260475E-01, // X_4
4.2548399372287182E-01, // X_5
4.3168379354213621E-01, // X_6
4.3543569751236455E-01, // X_7
4.3768918647214428E-01, // X_8
4.3903262905928286E-01, // X_9
4.3982597315656735E-01, // X_10
4.4028774979047969E-01, // X_11
4.4055002518902953E-01, // X_12
4.4069238917008052E-01, // X_13
4.4076272408112094E-01, // X_14
4.4078980543461005E-01, // X_15
4.4079091412311144E-01, // X_16
4.4077642312834125E-01, // X_17
4.4075255679998443E-01, // X_18
4.4072304911231042E-01, // X_19
4.4069013958173919E-01, // X_20
6.7041926189645151E-01, // X_21
7.3517997375758948E-01, // X_22
7.8975978943631409E-01, // X_23
8.3481725159539033E-01, // X_24
8.7125377077380739E-01, // X_25
9.0027275078767721E-01, // X_26
9.2312464536394301E-01, // X_27
9.4096954980798608E-01, // X_28
9.5481731262797742E-01, // X_29
9.6551271145368878E-01, // X_30
9.7374401773010488E-01, // X_31
9.8006186072166701E-01, // X_32
9.8490109485675337E-01, // X_33
9.8860194771099286E-01, // X_34
9.9142879342008328E-01, // X_35
9.9358602331847468E-01, // X_36
9.9523105632238640E-01, // X_37
9.9648478785701988E-01, // X_38
9.9743986301741971E-01, // X_39
9.9816716097314861E-01, // X_40
9.9872084014280071E-01, // X_41
3.8633811956730968E+00, // n_1
3.9322260498028840E+00, // n_2
3.9771965626392531E+00, // n_3
4.0063070333869728E+00, // n_4
4.0246026844143410E+00, // n_5
4.0358888958821835E+00, // n_6
4.0427690398786789E+00, // n_7
4.0469300433477020E+00, // n_8
4.0494314648020326E+00, // n_9
4.0509267560029381E+00, // n_10
4.0518145583397631E+00, // n_11
4.0523364846379799E+00, // n_12
4.0526383977460299E+00, // n_13
4.0528081437632766E+00, // n_14
4.0528985491134542E+00, // n_15
4.0529413510270169E+00, // n_16
4.0529556049324462E+00, // n_17
4.0529527471448805E+00, // n_18
4.0529396392278008E+00, // n_19
4.0529203970496912E+00, // n_20
3.6071164950918582E+00, // n_21
3.7583754503438387E+00, // n_22
3.8917148481441974E+00, // n_23
4.0094300698741563E+00, // n_24
4.1102216725798293E+00, // n_25
4.1944038520620675E+00, // n_26
4.2633275166560596E+00, // n_27
4.3188755452109175E+00, // n_28
4.3630947909857642E+00, // n_29
4.3979622247841386E+00, // n_30
4.4252580012497740E+00, // n_31
4.4465128947193868E+00, // n_32
4.4630018314791968E+00, // n_33
4.4757626150015568E+00, // n_34
4.4856260094946823E+00, // n_35
4.4932488551808500E+00, // n_36
4.4991456959629330E+00, // n_37
4.5037168116896273E+00, // n_38
4.5072719605639726E+00, // n_39
4.5100498969782414E+00 // n_40
};
double ud[ NU] = { 4.1833910982822058E+00, // L_vol
2.4899344742988991E+00 // Q
};
double pd[ NP] = { 1.5458567140000001E-01, // n^{ref}_{tray} = 0.155 l
1.7499999999999999E-01, // (not in use?)
3.4717208398678062E-01, // \alpha_{rect} = 35 %
6.1895708603484367E-01, // \alpha_{strip} = 62 %
1.6593025789999999E-01, // W_{tray} = 0.166 l^{-.5} s^{.1}
5.0695122527590109E-01, // Q_{loss} = 0.51 kW
8.5000000000000000E+00, // n^v_0 = 8.5 l
1.7000000000000001E-01, // n^v_{N+1} = 0.17 l
9.3885430857029321E+04, // P_{top} = 939 h Pa
2.5000000000000000E+02, // \Delta P_{strip}= 2.5 h Pa and \Delta P_{rect} = 1.9 h Pa
1.4026000000000000E+01, // F_{vol} = 14.0 l h^{-1}
3.2000000000000001E-01, // X_F = 0.32
7.1054000000000002E+01, // T_F = 71 oC
4.7163089489100003E+01, // T_C = 47.2 oC
4.1833910753991770E+00, // (not in use?)
2.4899344810136301E+00, // (not in use?)
1.8760537088149468E+02 // (not in use?)
};
DVector x0(NXD, xd);
DVector p0(NP, pd);
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, intervals );
// LSQ Term on temperature deviations and controls
Function h;
// for( i = 0; i < NXD; i++ )
// h << 0.001*x(i);
h << 0.1 * ( z(94) - 88.0 ); // Temperature tray 14
h << 0.1 * ( z(108) - 70.0 ); // Temperature tray 28
h << 0.01 * ( u(0) - ud[0] ); // L_vol
h << 0.01 * ( u(1) - ud[1] ); // Q
ocp.minimizeLSQ( h );
// W.r.t. differential equation
ocp.subjectTo( f );
// Fix states
ocp.subjectTo( AT_START, x == x0 );
// Fix parameters
ocp.subjectTo( p == p0 );
// Path constraint on controls
ocp.subjectTo( ud[0] - 2.0 <= u(0) <= ud[0] + 2.0 );
ocp.subjectTo( ud[1] - 2.0 <= u(1) <= ud[1] + 2.0 );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.initializeAlgebraicStates("hydroscal_algebraic_states.txt");
algorithm.set( INTEGRATOR_TYPE, INT_BDF );
algorithm.set( MAX_NUM_ITERATIONS, 5 );
algorithm.set( KKT_TOLERANCE, 1e-3 );
algorithm.set( INTEGRATOR_TOLERANCE, 1e-4 );
algorithm.set( ABSOLUTE_TOLERANCE , 1e-6 );
algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
algorithm.set( LINEAR_ALGEBRA_SOLVER, SPARSE_LU );
algorithm.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
//algorithm.set( LEVENBERG_MARQUARDT, 1e-3 );
algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY_LIFTED );
//algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY );
//algorithm.set( CONSTRAINT_SENSITIVITY, FORWARD_SENSITIVITY );
//algorithm.set( ALGEBRAIC_RELAXATION,ART_EXPONENTIAL ); //results in an extra step but steps are quicker
algorithm.solve();
double clock2 = clock();
printf("total computation time = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC );
// PLOT RESULTS:
// ---------------------------------------------------
VariablesGrid out_states;
algorithm.getDifferentialStates( out_states );
out_states.print( "OUT_states.m","STATES",PS_MATLAB );
VariablesGrid out_controls;
algorithm.getControls( out_controls );
out_controls.print( "OUT_controls.m","CONTROLS",PS_MATLAB );
VariablesGrid out_algstates;
algorithm.getAlgebraicStates( out_algstates );
out_algstates.print( "OUT_algstates.m","ALGSTATES",PS_MATLAB );
GnuplotWindow window;
window.addSubplot( out_algstates(94), "Temperature tray 14" );
window.addSubplot( out_algstates(108), "Temperature tray 28" );
window.addSubplot( out_controls(0), "L_vol" );
window.addSubplot( out_controls(1), "Q" );
window.plot( );
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4;
// x, y, z : position
// vx, vy, vz : linear velocity
// phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1))
// p, q, r : angular velocity
// u1, u2, u3, u4 : velocity of the propellers
Control vu1,vu2,vu3,vu4;
// vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4
DifferentialEquation f;
// Quad constants
const double c = 0.00001;
const double Cf = 0.00065;
const double d = 0.250;
const double Jx = 0.018;
const double Jy = 0.018;
const double Jz = 0.026;
const double Im = 0.0001;
const double m = 0.9;
const double g = 9.81;
const double Cx = 0.1;
// Minimization Weights
double coeffX = .00001;
double coeffU = coeffX*0.0001;//0.000000000000001;
double coeffX2 = coeffX * 100.;
double coeffX3 = coeffX * 0.00001;
double coeffO = -coeffX * 0.1;
// final position (used in the cost function)
double xf = 0., yf = 0., zf = 20.;
//
double T = 8.; //length (in second) of the trajectory predicted in the MPC
int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem
// 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces
// If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2)
// In ACADO, the commands are piecewise constant functions, constant between each node.
double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm
// DEFINE A OPTIMAL CONTROL PROBLEM
// -------------------------------
OCP ocp( 0.0, T, nb_nodes );
// DEFINE THE COST FUNCTION
// -------------------------------
Function h, hf;
h << x;
h << y;
h << z;
h << vu1;
h << vu2;
h << vu3;
h << vu4;
h << p;
h << q;
h << r;
hf << x;
hf << y;
hf << z;
DMatrix Q(10,10), Qf(3,3);
Q(0,0) = coeffX;
Q(1,1) = coeffX;
Q(2,2) = coeffX;
Q(3,3) = coeffU;
Q(4,4) = coeffU;
Q(5,5) = coeffU;
Q(6,6) = coeffU;
Q(7,7) = coeffX2;
Q(8,8) = coeffX2;
Q(9,9) = coeffX2;
Qf(0,0) = 1.;
Qf(1,1) = 1.;
Qf(2,2) = 1.;
DVector reff(3), ref(10);
ref(0) = xf;
ref(1) = yf;
ref(2) = zf;
ref(3) = 58.;
ref(4) = 58.;
ref(5) = 58.;
ref(6) = 58.;
ref(7) = 0.;
ref(8) = 0.;
ref(9) = 0.;
reff(0) = xf;
reff(1) = yf;
reff(2) = zf;
// The cost function is define as : integrale from 0 to T { transpose(h(x,u,t)-ref)*Q*(h(x,u,t)-ref) } + transpose(hf(x,u,T))*Qf*hf(x,u,T)
// == integrale cost (also called running cost or Lagrange Term) + final cost (Mayer Term)
ocp.minimizeLSQ ( Q, h, ref);
// ocp.minimizeLSQEndTerm(Qf, hf, reff);
// When doing MPC, you need terms in the cost function to stabilised the system => p, q, r and vu1, vu2, vu3, vu4. You can check that if you reduce their weights "coeffX2" or "coeffU" too low, the optimization will crashed.
// DEFINE THE DYNAMIC EQUATION OF THE SYSTEM:
// ----------------------------------
f << dot(x) == vx;
f << dot(y) == vy;
f << dot(z) == vz;
f << dot(vx) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
f << dot(vy) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(psi)*cos(theta)/m;
f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(psi)*cos(theta)/m - g;
f << dot(phi) == -cos(phi)*tan(theta)*p+sin(phi)*tan(theta)*q+r;
f << dot(theta) == sin(phi)*p+cos(phi)*q;
f << dot(psi) == cos(phi)/cos(theta)*p-sin(phi)/cos(theta)*q;
f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
f << dot(u1) == vu1;
f << dot(u2) == vu2;
f << dot(u3) == vu3;
f << dot(u4) == vu4;
// ---------------------------- DEFINE CONTRAINTES --------------------------------- //
//Dynamic
ocp.subjectTo( f );
//State constraints
//Constraints on the velocity of each propeller
ocp.subjectTo( 16 <= u1 <= 95 );
ocp.subjectTo( 16 <= u2 <= 95 );
ocp.subjectTo( 16 <= u3 <= 95 );
ocp.subjectTo( 16 <= u4 <= 95 );
//Command constraints
//Constraints on the acceleration of each propeller
ocp.subjectTo( -100 <= vu1 <= 100 );
ocp.subjectTo( -100 <= vu2 <= 100 );
ocp.subjectTo( -100 <= vu3 <= 100 );
ocp.subjectTo( -100 <= vu4 <= 100 );
#if AVOID_SINGULARITIES == 1
//Constraint to avoid singularity
// In this example I used Yaw-Pitch-Roll convention to describe orientation of the quadrotor
// when using Euler Angles representation, you always have a singularity, and we need to
// avoid it otherwise the algorithm will crashed.
ocp.subjectTo( -1. <= theta <= 1.);
#endif
//Example of Eliptic obstacle constraints (here, cylinders with eliptic basis)
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-5)*(z-5)) );
ocp.subjectTo( 16 <= ((x-3)*(x-3)+2*(z-9)*(z-9)) );
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-15)*(z-15)) );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
// The next line define the equation used to simulate the system :
// here "f" is used (=the same as the one used for the ocp) so the trajectory simulated
// from the commands will be exactly the same as the one calculated by the MPC.
// If for instance you want to test robusness, you can define an other dynamic equation "f2"
// with changed parameters and put it here.
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp, tmpc );
//Usually, you do only one step of the optimisation algorithm (~Gauss-Newton here)
//at each activation of the MPC, that way the delay between getting the state and
//sending a command is as quick as possible.
alg.set( MAX_NUM_ITERATIONS, 1 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP)
Grid timeGrid(0.0,T,nb_nodes+1);
VariablesGrid x_init(16, timeGrid);
// init with static
for (int i = 0 ; i<nb_nodes+1 ; i++ ) {
x_init(i,0) = 0.;
x_init(i,1) = 0.;
x_init(i,2) = 0.;
x_init(i,3) = 0.;
x_init(i,4) = 0.;
x_init(i,5) = 0.;
x_init(i,6) = 0.;
x_init(i,7) = 0.;
x_init(i,8) = 0.;
x_init(i,9) = 0.;
x_init(i,10) = 0.;
x_init(i,11) = 0.;
x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad
x_init(i,13) = 58.;
x_init(i,14) = 58.;
x_init(i,15) = 58.;
}
alg.initializeDifferentialStates(x_init);
// SET OPTION AND PLOTS WINDOW
// ---------------------------
// Linesearch is an algorithm which will try several points along the descent direction to choose a better step length.
// It looks like activating this option produice more stable trajectories.198
alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH );
alg.set(INTEGRATOR_TYPE, INT_RK45);
// You can uncomment those lines to see how the predicted trajectory involve along time
// (but be carefull because you will have 1 ploting window per MPC loop)
// GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
// window1.addSubplot( z,"DifferentialState z" );
// window1.addSubplot( x,"DifferentialState x" );
// window1.addSubplot( theta,"DifferentialState theta" );
// window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" );
// window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" );
// window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" );
// alg<<window1;
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
// The first argument is the starting time, the second the end time.
SimulationEnvironment sim( 0.0,10.,process,controller );
//Setting the state of the sytem at the beginning of the simulation.
DVector x0(16);
x0.setZero();
x0(0) = 0.;
x0(12) = 58.;
x0(13) = 58.;
x0(14) = 58.;
x0(15) = 58.;
t = clock();
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
t = clock() - t;
std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl;
// ...SAVE THE RESULTS IN FILES
// ----------------------------------------------------------
std::ofstream file;
file.open("/tmp/log_state.txt",std::ios::out);
std::ofstream file2;
file2.open("/tmp/log_control.txt",std::ios::out);
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.print(file);
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.print(file2);
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "x " );
window.addSubplot( sampledProcessOutput(1), "y " );
window.addSubplot( sampledProcessOutput(2), "z " );
window.addSubplot( sampledProcessOutput(6),"phi" );
window.addSubplot( sampledProcessOutput(7),"theta" );
window.addSubplot( sampledProcessOutput(8),"psi" );
window.plot( );
graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp();
client.createWindow("window");
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// Define a Right-Hand-Side:
// -------------------------
DifferentialState x;
DifferentialEquation f;
TIME t;
f << dot(x) == -x + sin(0.01*t);
// Define an initial value:
// ------------------------
Vector xStart( 1 );
xStart(0) = 1.0;
double tStart = 0.0;
double tEnd = 1000.0;
Grid timeHorizon( tStart,tEnd,2 );
Grid timeGrid( tStart,tEnd,20 );
// Define an integration algorithm:
// --------------------------------
IntegrationAlgorithm intAlg;
intAlg.addStage( f, timeHorizon );
intAlg.set( INTEGRATOR_TYPE, INT_BDF );
intAlg.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
intAlg.set( INTEGRATOR_TOLERANCE, 1.0e-3 );
intAlg.set( PRINT_INTEGRATOR_PROFILE, YES );
intAlg.set( PLOT_RESOLUTION, HIGH );
GnuplotWindow window;
window.addSubplot( x,"x" );
intAlg << window;
// START THE INTEGRATION
// ----------------------
intAlg.integrate( timeHorizon, xStart );
// GET THE RESULTS
// ---------------
VariablesGrid differentialStates;
intAlg.getX( differentialStates );
differentialStates.print( "x" );
Vector xEnd;
intAlg.getX( xEnd );
xEnd.print( "xEnd" );
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// ------------------------
DifferentialState x;
Control u;
Disturbance w;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
DifferentialEquation f, f2;
f << dot(x) == -2.0*x + u;
f2 << dot(x) == -2.0*x + u + 0.1*w; //- 0.000000000001*x*x
// DEFINE LEAST SQUARE FUNCTION:
// -----------------------------
Function h;
h << x;
h << u;
Matrix Q(2,2);
Q.setIdentity();
Vector r(2);
r.setAll( 0.0 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 7.0;
OCP ocp ( t_start, t_end, 14 );
ocp.minimizeLSQ( Q, h, r );
ocp.subjectTo ( f );
ocp.subjectTo( -1.0 <= u <= 2.0 );
//ocp.subjectTo( w == 0.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f2,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance = fopen( "my_disturbance.txt", "r" );
// GnuplotWindow window2;
// window2.addSubplot( disturbance, "my disturbance" );
// window2.plot();
process.setProcessDisturbance( disturbance );
// SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
// ----------------------------------------------
double samplingTime = 0.5;
RealTimeAlgorithm algorithm( ocp,samplingTime );
// // algorithm.set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
//
// // algorithm.set( ABSOLUTE_TOLERANCE , 1e-7 );
// // algorithm.set( INTEGRATOR_TOLERANCE, 1e-9 );
//
// algorithm.set( KKT_TOLERANCE, 1e-4 );
algorithm.set( MAX_NUM_ITERATIONS,1 );
algorithm.set( USE_REALTIME_SHIFTS, YES );
// algorithm.set( USE_REALTIME_ITERATIONS,NO );
// algorithm.set( TERMINATE_AT_CONVERGENCE,YES );
// algorithm.set( PRINTLEVEL,HIGH );
Vector x0(1);
x0(0) = 1.0;
// // algorithm.solve( x0 );
//
// GnuplotWindow window1;
// window1.addSubplot( x, "DIFFERENTIAL STATE: x" );
// window1.addSubplot( u, "CONTROL: u" );
// window1.plot();
//
// return 0;
// SETTING UP THE NMPC CONTROLLER:
// -------------------------------
VariablesGrid myReference = fopen( "my_reference.txt", "r" );
PeriodicReferenceTrajectory reference( myReference );
// GnuplotWindow window3;
// window3.addSubplot( myReference(1), "my reference" );
// window3.plot();
Controller controller( algorithm,reference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
double simulationStart = 0.0;
double simulationEnd = 15.0;
SimulationEnvironment sim( simulationStart, simulationEnd, process, controller );
sim.init( x0 );
sim.run( );
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: x" );
window.addSubplot( feedbackControl(0), "CONTROL: u" );
window.plot();
return 0;
}
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;
}
/* >>> 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
// 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;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// DEFINE VALRIABLES:
// ---------------------------
DifferentialStateVector x(3); // the position of the pendulum (x,y,alpha)
DifferentialStateVector v(3); // the associated velocities
AlgebraicStateVector a(3); // the associated accelerations
AlgebraicStateVector lambda(2); // the constraint forces
const double L = 1.00; // the length of the pendulum
const double m = 1.00; // the mass of the pendulum
const double g = 9.81; // the gravitational constant
const double J = m*L*L; // the inertial of the pendulum
IntermediateStateVector R(3);
IntermediateStateVector G(2);
R.setComponent( 0, m*a(0) ); // ----------------------------------------
R.setComponent( 1, m*a(1) + m*g ); // the definition of the force residuum:
R.setComponent( 2, J*a(2) ); // R := m*a - F
G.setComponent( 0, x(0)-L*sin(x(2)) ); // definition of the constraint manifold G
G.setComponent( 1, x(1)+L*cos(x(2)) ); // ---------------------------------------
// AUTOMATIC GENERATION OF AN INDEX 1 DAE SYSTEM BASES ON THE
// NEWTON EULER FORMALISM:
// -----------------------------------------------------------
DifferentialEquation f;
NewtonEulerFormalism( f, R, G, x, v, a, lambda );
// Define an integrator:
// ---------------------
IntegratorBDF integrator( f );
// Define an initial value:
// ------------------------
double x_start[6];
double z_start[5];
x_start[0] = 1.9866932270683099e-01;
x_start[1] = -9.8006654611577315e-01;
x_start[2] = 2.0000003107582773e-01;
x_start[3] = -1.4519963562050693e-04;
x_start[4] = 4.7104175041346282e-04;
x_start[5] = 4.4177521668741377e-04;
z_start[0] = -9.5504866367984165e-01;
z_start[1] = -1.9359778029074531e-01;
z_start[2] = -9.7447321693831934e-01;
z_start[3] = -9.5504866367984165e-01;
z_start[4] = 9.6164022197092560e+00;
double t_start = 0.0;
double t_end = 10.0;
// START THE INTEGRATION
// ----------------------
integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
// integrator.set( INTEGRATOR_TOLERANCE, 1e-12 );
integrator.freezeAll();
integrator.integrate( x_start, z_start, t_start, t_end );
VariablesGrid xres,zres;
integrator.getTrajectory(&xres,&zres,NULL,NULL,NULL,NULL);
GnuplotWindow window;
window.addSubplot( xres(0), "The x-position of the mass m" );
window.addSubplot( xres(1), "The y-position of the mass m" );
window.addSubplot( xres(2), "The excitation angle of the pendulum" );
window.addSubplot( xres(3), "The velocity in x-direction" );
window.addSubplot( xres(4), "The velocity in y-direction" );
window.addSubplot( xres(5), "The angular velocity" );
// window.addSubplot( zres(0), "The acceleration in x-direction" );
// window.addSubplot( zres(1), "The acceleration in y-direction" );
// window.addSubplot( zres(2), "The angular acceleration" );
window.addSubplot( zres(3), "The constraint force in x-direction" );
window.addSubplot( zres(4), "The constraint force in y-direction" );
window.plot();
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState v,s,m;
Control u ;
DifferentialEquation f ;
const double t_start = 0.0;
const double t_end = 10.0;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
f << dot(s) == v;
f << dot(v) == (u-0.02*v*v)/m;
f << dot(m) == -0.01*u*u;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, 20 );
ocp.minimizeLagrangeTerm( u*u );
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 );
ocp.subjectTo( u*u >= -1.0 );
// 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_BDF );
// algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
// algorithm.set( KKT_TOLERANCE, 1e-3 );
//algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY );
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set( MAX_NUM_ITERATIONS, 20 );
algorithm.set( KKT_TOLERANCE, 1e-10 );
// algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 4 );
algorithm << window;
algorithm.solve();
// BlockMatrix sens;
// algorithm.getSensitivitiesX( sens );
// sens.print();
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( ){
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;
}
int main( ){
USING_NAMESPACE_ACADO
// Define a Right-Hand-Side:
// -------------------------
DifferentialState x, y;
DifferentialEquation f;
f << dot(x) == y;
f << dot(y) == -x;
// Define an integrator:
// ---------------------
IntegratorRK45 integrator( f );
integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
integrator.set( PRINT_INTEGRATOR_PROFILE, YES );
// Define an initial value:
// ------------------------
double x_start[2] = { 0.0, 1.0 };
Grid timeInterval( 0.0, 2.0*M_PI, 100 );
integrator.freezeAll();
integrator.integrate( timeInterval, x_start );
// GET THE RESULTS
// ---------------
VariablesGrid differentialStates;
integrator.getX( differentialStates );
GnuplotWindow window;
window.addSubplot( differentialStates(0) );
window.addSubplot( differentialStates(1) );
window.plot();
// Vector seed(2);
//
// seed( 0 ) = 1.0;
// seed( 1 ) = 0.0;
//
// integrator.setForwardSeed( 1, seed );
// integrator.integrateSensitivities();
//
// VariablesGrid sens;
// integrator.getForwardSensitivities( sens, 1 );
//
// GnuplotWindow window2;
// window2.addSubplot( sens(0) );
// window2.addSubplot( sens(1) );
// window2.plot();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// DEFINE A RIGHT-HAND-SIDE:
// -------------------------
DifferentialState x;
AlgebraicState z;
Parameter p,q;
IntermediateState is(4);
is(0) = x;
is(1) = z;
is(2) = p;
is(3) = q;
CFunction simpledaeModel( 2, ffcn_model );
// Define a Right-Hand-Side:
// -------------------------
DifferentialEquation f;
f << simpledaeModel(is);
// DEFINE AN INTEGRATOR:
// ---------------------
IntegratorBDF integrator(f);
// DEFINE INITIAL VALUES:
// ----------------------
double x0 = 1.0;
double z0 = 1.000000;
double pp[2] = { 1.0, 1.0 };
Grid interval( 0.0, 1.0, 100 );
// START THE INTEGRATION:
// ----------------------
integrator.integrate( interval, &x0, &z0, pp );
VariablesGrid differentialStates;
VariablesGrid algebraicStates ;
VariablesGrid intermediateStates;
integrator.getX ( differentialStates );
integrator.getXA( algebraicStates );
integrator.getI ( intermediateStates );
GnuplotWindow window;
window.addSubplot( differentialStates(0) );
window.addSubplot( algebraicStates (0) );
window.plot();
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;
// ----------------------------------------------------------
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
ExportedRTIscheme rtiScheme( 4,1, 10, 0.3 );
#ifdef USE_CVXGEN
set_defaults( );
#endif
Vector xuRef(5);
xuRef.setZero( );
VariablesGrid reference;
reference.addVector( xuRef, 0.0 );
reference.addVector( xuRef, 10.0 );
StaticReferenceTrajectory referenceTrajectory( reference );
Controller controller( rtiScheme,referenceTrajectory );
controller.set( USE_REFERENCE_PREDICTION,NO );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
SimulationEnvironment sim( 0.0,10.0, process,controller );
Vector x0(4);
x0(0) = 1.0;
x0(1) = 0.0;
x0(2) = 0.0;
x0(3) = 0.0;
sim.init( x0 );
sim.run( );
// ... AND PLOT THE RESULTS
// ------------------------
VariablesGrid diffStates;
sim.getProcessDifferentialStates( diffStates );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
GnuplotWindow window;
window.addSubplot( diffStates(0), "p" );
window.addSubplot( diffStates(1), "v" );
window.addSubplot( diffStates(2), "phi" );
window.addSubplot( diffStates(3), "omega" );
window.addSubplot( feedbackControl(0), "a" );
window.plot( );
return 0;
}
int main( )
{
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState xB; //Body Position
DifferentialState xW; //Wheel Position
DifferentialState vB; //Body Velocity
DifferentialState vW; //Wheel Velocity
Disturbance R;
Control F;
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;
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance = readFromFile( "road.txt" );
if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
Function h;
h << xB;
h << xW;
h << vB;
h << vW;
h << F;
Matrix Q = zeros(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;
Vector r(5); // Reference
r.setAll( 0.0 );
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 MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp,0.05 );
alg.set( INTEGRATOR_TYPE, INT_RK78 );
alg.set( DYNAMIC_SENSITIVITY,FORWARD_SENSITIVITY );
// alg.set( MAX_NUM_ITERATIONS, 2 );
// alg.set( USE_IMMEDIATE_FEEDBACK,YES );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
SimulationEnvironment sim( 0.0,2.5,process,controller );
Vector x0(4);
x0.setZero();
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 );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
GnuplotWindow window;
window.addSubplot( diffStates(0), "Body Position [m]" );
window.addSubplot( diffStates(1), "Wheel Position [m]" );
window.addSubplot( diffStates(2), "Body Velocity [m/s]" );
window.addSubplot( diffStates(3), "Wheel Velocity [m/s]" );
window.addSubplot( feedbackControl, "Damping Force [N]" );
window.addSubplot( disturbance, "Road Excitation [m]" );
window.plot( );
return EXIT_SUCCESS;
}
