eeVector2f cUIMap::GetMouseMapPos() {
eeVector2f mp( mMap->GetMouseMapPosf() );
if ( mClampToTile ) {
eeVector2i mpc( mMap->GetTileCoords( mMap->GetMouseTilePos() + 1 ) );
mp = eeVector2f( mpc.x, mpc.y );
}
return mp;
}
void TinyMainWindow::on_action_playlist_edit_triggered()
{
EditPlaylistDialog dlg(this, mpc());
if (dlg.exec() != QDialog::Accepted) return;
QString name = dlg.name();
if (name.isEmpty()) return;
if (!MusicPlayerClient::isValidPlaylistName(name)) {
QMessageBox::warning(this, qApp->applicationName(), tr("The name is invalid."));
return;
}
loadPlaylist(name, dlg.forReplace());
}
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;
}
void TinyMainWindow::on_action_consume_triggered()
{
mpc()->do_consume(!pv->consume_enabled);
}
void TinyMainWindow::on_action_single_triggered()
{
mpc()->do_single(!pv->single_enabled);
}
void TinyMainWindow::on_action_random_triggered()
{
mpc()->do_random(!pv->random_enabled);
}
int main( ){
USING_NAMESPACE_ACADO
// DEFINE THE VARIABLES:
// ----------------------------------------------------------
DifferentialState x ; // the trolley position
DifferentialState v ; // the trolley velocity
DifferentialState phi ; // the excitation angle
DifferentialState omega; // the angular velocity
Control ax ; // the acc. of the trolley
Parameter pp;
const double g = 9.81; // the gravitational constant
const double b = 0.20; // the friction coefficient
// ----------------------------------------------------------
// DEFINE THE MODEL EQUATIONS:
// ----------------------------------------------------------
DifferentialEquation f ;
f << dot( x ) == v+pp ;
f << dot( v ) == ax ;
f << dot( phi ) == omega ;
f << dot( omega ) == -g*sin(phi) - ax*cos(phi) - b*omega;
// ----------------------------------------------------------
// DEFINE THE WEIGHTING MATRICES:
// ----------------------------------------------------------
ExportVariable Q = eye(4);
// ExportVariable Q( "Q",4,4 );
Matrix S = eye(4);
// Matrix R = eye(1);
ExportVariable R( "R",1,1 );
// ----------------------------------------------------------
// SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
// ----------------------------------------------------------
Grid grid( 0.0, 3.0, 11 );
OCP ocp( grid );
ocp.minimizeLSQ ( Q, R );
ocp.minimizeLSQEndTerm( S );
ExportVariable QS1( "QS1",4,4 );
ExportVariable QS2( "QS2",4,4 );
Matrix fixedMatrix = zeros( 4,4 );
QS2 = fixedMatrix;
ocp.minimizeLSQStartTerm( QS1,fixedMatrix );
// ocp.minimizeLSQStartTerm( QS1,QS2 );
ocp.subjectTo( f );
ocp.subjectTo( -1.0 <= ax <= 1.0 );
// ocp.subjectTo( AT_END, x == 0.0 );
// ocp.subjectTo( AT_END, v == 0.0 );
// ocp.subjectTo( AT_END, phi == 0.0 );
// ocp.subjectTo( AT_END, omega == 0.0 );
// ocp.subjectTo( -0.5 <= v <= 1.5 );
// ----------------------------------------------------------
// DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
// ----------------------------------------------------------
MPCexportBis mpc(ocp);
// mpc.set( INTEGRATOR_TYPE, INT_RK4 );
// mpc.set( NUM_INTEGRATOR_STEPS, 30 );
// mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
// mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
mpc.set( MAX_NUM_QP_ITERATIONS, 40 );
mpc.set( HOTSTART_QP, NO );
mpc.set( QP_SOLVER, QP_QPOASES3 );
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( FIX_INITIAL_STATE, NO );
mpc.set( LEVENBERG_MARQUARDT, 0.001 );
mpc.set( GENERATE_TEST_FILE, NO );
// mpc.set( GENERATE_SIMULINK_INTERFACE, YES );
// mpc.set( OPERATING_SYSTEM, OS_WINDOWS );
// mpc.set( USE_SINGLE_PRECISION, YES );
mpc.set( PRINTLEVEL, HIGH );
// mpc.printDimensionsQP( );
mpc.exportCode( "./crane_bis_export" );
// ----------------------------------------------------------
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)));
// 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;
}
void TinyMainWindow::on_action_previous_triggered()
{
mpc()->do_previous();
}
void TinyMainWindow::onSliderPressed()
{
if (pv->status.now.status == PlayingStatus::Play) {
mpc()->do_pause(true);
}
}
void TinyMainWindow::onVolumeChanged()
{
int v = pv->volume_popup.value();
mpc()->do_setvol(v);
}
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( ){
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;
}
int main( )
{
USING_NAMESPACE_ACADO
// 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
// 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;
// Reference functions and weighting matrices:
Function h, hN;
h << p << v << phi << omega << a;
hN << p << v << phi << omega;
Matrix W = eye( h.getDim() );
Matrix WN = eye( hN.getDim() );
WN *= 5;
// Or:
// ExportVariable W(h.getDim(), h.getDim());
// ExportVariable WN(hN.getDim(), hN.getDim());
//
// Optimal Control Problem
//
OCP ocp(0.0, 3.0, 10);
ocp.subjectTo( f );
ocp.minimizeLSQ(W, h);
ocp.minimizeLSQEndTerm(WN, hN);
ocp.subjectTo( -1.0 <= a <= 1.0 );
ocp.subjectTo( -0.5 <= v <= 1.5 );
// Export the code:
OCPexport 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( 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_MATLAB_INTERFACE, YES );
mpc.set( GENERATE_SIMULINK_INTERFACE, YES );
// mpc.set( USE_SINGLE_PRECISION, YES );
if (mpc.exportCode( "getting_started_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}
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;
}
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;
}
void TinyMainWindow::on_action_next_triggered()
{
mpc()->do_next();
}
void TinyMainWindow::on_action_repeat_triggered()
{
mpc()->do_repeat(!pv->repeat_enabled);
}
int main( ){
USING_NAMESPACE_ACADO
// DEFINE THE VARIABLES:
// ----------------------------------------------------------
DifferentialState x ; // the trolley position
DifferentialState v ; // the trolley velocity
DifferentialState phi ; // the excitation angle
DifferentialState omega; // the angular velocity
Control ax ; // 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( x ) == v ;
f << dot( v ) == ax ;
f << dot( phi ) == omega ;
f << dot( omega ) == -g*sin(phi) - ax*cos(phi) - b*omega;
// ----------------------------------------------------------
// DEFINE THE WEIGHTING MATRICES:
// ----------------------------------------------------------
Matrix Q = eye(4);
Matrix S = eye(4);
Matrix R = eye(1);
// ----------------------------------------------------------
// SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
// ----------------------------------------------------------
Grid grid( 0.0, 3.0, 11 );
OCP ocp( grid );
ocp.minimizeLSQ ( Q, R );
ocp.minimizeLSQEndTerm( S );
ocp.subjectTo( f );
ocp.subjectTo( -1.0 <= ax <= 1.0 );
// Vector tmp(4), zero(4);
// zero.setZero();
//
// tmp.setAll( -INFTY );
// VariablesGrid lb( tmp,grid );
// lb.setVector( 10,zero );
//
// tmp.setAll( INFTY );
// VariablesGrid ub( tmp,grid );
// ub.setVector( 10,zero );
// ocp.subjectTo( lb(0) <= x <= ub(0) );
// ocp.subjectTo( lb(1) <= v <= ub(1) );
// ocp.subjectTo( lb(2) <= phi <= ub(2) );
// ocp.subjectTo( lb(3) <= omega <= ub(3) );
// ocp.subjectTo( -0.5 <= v <= 1.5 );
// ----------------------------------------------------------
// DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
// ----------------------------------------------------------
MPCexport mpc(ocp);
// mpc.set( INTEGRATOR_TYPE , INT_RK4 );
// mpc.set( NUM_INTEGRATOR_STEPS , 30 );
// mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( GENERATE_TEST_FILE,NO );
mpc.set( HOTSTART_QP,NO );
mpc.set( USE_SINGLE_PRECISION,YES );
mpc.exportCode("crane_export");
// ----------------------------------------------------------
return 0;
}