Description
We can introduce slack variables in an OCP by introducing extra controls. This is kinda a hack, but it works out in the end. However, there are problems... In the current version of ACADO, if one introduces an extra control which is not used in the ODE. For example, if one modifies the getting_started example like:
// 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
Control s; // Slack
const double g = 9.81; // the gravitational constant
const double b = 0.20; // the friction coefficient
// Model equations:
DifferentialEquation f;
f << dot( p ) == v + 1e-8 * s;
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;
// Provide defined weighting matrices:
DMatrix W = eye<double>( h.getDim() );
DMatrix WN = eye<double>( hN.getDim() );
WN *= 5;
//
// 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 );
ocp.subjectTo( 0 <= v + 0.5 + s);
ocp.subjectTo( v - 1.5 - s <= 0 );
ocp.subjectTo( s >= 0 );... is not gonna work. Assertion gives:
[ACADO] Error: Index out of bounds
Code: (RET_INDEX_OUT_OF_BOUNDS)
File: .../include/acado/variables_grid/matrix_variables_grid.ipp
Line: 422
Abort trap: 6
This should be investigated further. My first guess is that since s is not used in the ODE, OCP formulation does not specify it's bounds in corresponding structures that hold constraints.
BTW, if we add a dummy state to the ODE:
f << dot( dummy ) == s;
it works, and this is how we have handled slacks before. Plus one can penalize dummy and this showed to be useful in some cases. This is not optimal, of course, since then the unnecessary sensitivities are calculated.
Looking a bit deeper, this may not be so simple to solve. IMO, in ACADO symbolics there are no "true" optimization variables -- everything is related solely to optimal control...
Any thoughts on how to improve this are welcome.