Constraints on algebraic variables - Path following formulation

MPC Discussions
,
1

Hello,

I would like to formulate a path following constraint like the following:

-s_l < q - y(z) < + s_u

where q is the generalized position vector (subset of sym.x), z is an algebraic variable which is not part of the dynamics, sl,su are slack variables and y = q_2z + q_1(1-z).
What i am trying to achieve with this is to have the state close to a line path from q1 to q2. But i do not want to pre-specify the timing.

Question: So my general question is, how can i achieve the above? I will describe my current approach, with some additional questions along the way.

Approach 1:

% variables:
s  = SX.sym('s' ,1) ;
x1 = SX.sym('x1' ,3) ;
x2 = SX.sym('x2' ,3) ;
ocp_model.set('sym_p', [x1;x2]);
ocp_model.set('sym_z', s);

% Dynamics:
expr_f_impl = sym_xdot - expr_f_expl;
ocp_model.set('dyn_type', 'implicit');
ocp_model.set('dyn_expr_f', [model.expr_f_impl;0]);

%constraints: -> they are slacked
path_constr = x2*s +x1*(1-s);
state = model.sym_x([1,2,4]);
path_error = state - path_constr;

But when i try to solve it, i get:

iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter	alpha
0	NaN	NaN	NaN	0.000000e+00	0	0	0.000000e+00

Question1: I do not add the constraint on terminal state, as it seems terminal constraints on algebraic variables are not supported (got some error messages for constr_h_expr_e when i deleted the corresponding terminal constraints). Am i right about this?

Approach 2:

If the dynamics are not appended by a zero:

ocp_model.set('dyn_expr_f', [model.expr_f_impl]);

then i get this error:

Error using casadi.GenericMatrixCommon/jtimes
Error in SX::jtimes at .../casadi/core/generic_matrix.hpp:1191:
.../casadi/core/generic_matrix.hpp:1164: Assertion "v.size1() == ex.size1() && v.size2()
0.000000e+00x.size2() == 0" failed:
'v' has inconsistent dimensions

Error in generate_c_code_implicit_ode (line 189)
    ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, true);

Question2: I initially appended the dynamics with a zero to get arround this error, but i guess i created a singular jacobian which is why the first approach returns NaN. Am i right about this ?

  • According to this, i can add constraints on algebraic variables, to ensure z is in [0,1].
2

Hi :wave:

and welcome to the acados forum! Your analysis of the issue is quite right: In approach 1, the jacobian of f_expl is singular leading to NaNs right away. In approach 2, the number of differential and algebraic states does not match the number of equations.

I am wondering why you need to model s as an algebraic variable in the first place. If they only appear in the constraint x2*s +x1*(1-s), you might simply model them as controls with additional bounds constraining them to be in [0, 1].

Best, Katrin

3

Hello Katrin,

thanks for the answer.
Indeed, s doesn’t need to be an algebraic variable. I ended up extending the state with s and input with Δs and adding the following equation:
\dot{s} = Δs
which allows me to ensure the path travelled in a certain direction (by bounding s \in [0,1] and Δs \in [0,+ \infty). But i will also try to put it as a control to see if it improves the performance.

1 Like