High Inequality residuals in a Time Optimal formulation (which works for example problem)

MPC Discussions
, ,
1

Hello everyone!

I am trying to create a torque tracking MPC, and thus, i want to avoid having a fixed time. So, as a first step, i am trying to create a time optimal controller for position tracking of my system.

Firstly, i modified the example mentioned here. (In my case, for position control of my system, varying Δt is not good, as it results in jerky motions, even with penalizing u and \dot{q}. I believe it is because:

  • Torque can vary greatly with a corresponding small dt, which makes the stage cost small
  • State can flaxuate, but due to dt being small, the equality constraints are satisfied ).

Time optimal formulation

So, I added the following (I can make a PR of this, so it can be an example):

  1. a new state T_h,
  2. a new input ΔT_h with ΔT_h \in [0,0]
  3. the dynamics of the new state: \dot{T}_h = ΔT_h .

By constraining ΔT_h to zero, and having T_{h,0} \in [T_{h,l},T_{h,u}] i can optimize the final time. Indeed, this works very well for the simple crane example, and in my case it is solved in 6 iterations, giving a solution close to the varying dt.

Problem

When i use the same technique in my problem, it fails to converge, having inequality residuals that stay at T_{h,u}-T_{h,l} early from the starting iterations up to 500. Also, to converge at all, it needs the line:

ocp_model.set('constr_x0', [x0;Tmid]) even if this constrainted is overwritten.

At the moment, i have deactivated all the constraints apart from the input and states and only have cost at i=0.

So, when i set the initial bounds like that (last state is T_h):

ocp.set('constr_lbx', [x0;0.1], 0)
ocp.set('constr_ubx', [x0;0.35], 0)

I cannot get it to converge, and inequality residuals are at 0.35-0.1 = 2.5e-1.

But when i set the initial bounds like that (effectively specifying T_h):

ocp.set('constr_lbx', [x0;0.3], 0)
ocp.set('constr_ubx', [x0;0.3], 0)

It converges (atm in 1 iteration to a worthless solution due to the absence of terminal constraint in x, and of constraints in general).

Question: Is there any indication to why this is happening?

Extras

Below i have the code for setting constraints and cost as well as the optimization options i use (non-default ones)

ocp_model.set('constr_x0', [x0;Tmid]); %Even if `constr_x0` is overwritten, 
% if this line is deleted -> QP fails. 

ocp_model.set('constr_lbu',[model.input_constraints(:,1);0])
ocp_model.set('constr_ubu',[model.input_constraints(:,2);0])
ocp_model.set('constr_Jbu',eye(nu))

ocp_model.set('constr_lbx',[model.state_constraints(:,1)])
ocp_model.set('constr_ubx',[model.state_constraints(:,2)])
ocp_model.set('constr_Jbx',eye(nst,nx)) %nst = nx-1

ocp_model.set('constr_lbx_0',[model.state_constraints(:,1);Tmid])
ocp_model.set('constr_ubx_0',[model.state_constraints(:,2);Tmid])
ocp_model.set('constr_Jbx_0',eye(nx))

ocp_model.set('cost_expr_ext_cost'    ,0 );
ocp_model.set('cost_expr_ext_cost_0'  , 1e5*model.Th );
ocp_model.set('cost_expr_ext_cost_e'  ,0);

The non-default options (which apart from N, and sim_steps are the same as in the crane example):

compile_interface: 'auto'
                        param_scheme_N: 50
                            nlp_solver: 'sqp'
              nlp_solver_exact_hessian: 1
                   nlp_solver_tol_stat: 1.0000e-06
                     nlp_solver_tol_eq: 1.0000e-06
                   nlp_solver_tol_ineq: 1.0000e-06
                   nlp_solver_tol_comp: 1.0000e-06
                             qp_solver: 'full_condensing_hpipm'
                            sim_method: 'erk'
                     regularize_method: 'convexify'
                   levenberg_marquardt: 0
                        exact_hess_dyn: 0
                       exact_hess_cost: 1
                     exact_hess_constr: 0
                      qp_solver_cond_N: 50
                  sim_method_num_steps: 1
2

Hi :wave:

not quite sure if I understand your problem formulation

a new input ΔTh with ΔTh∈[0,0]

This seems somewhat weird to me, why not simply use \dot{T}_h = 0 and no extra controls?

3

This is a workaround to avoid having a singular jacobian.

4

Including \dot{T}_h = 0 in your dynamics is totally fine. The problem with singular jacobians discussed here is due to the jacobian with respect to the algebraic variables z being indefinite.

5

@kaethe Indeed, there was no need for ΔT_h. I removed it from the example problem and it worked.

However this did not solve my problem. In the same time though, I observed the following:

  1. Initially i had this in the constraints:
ocp_model.set('constr_x0', [x0;Tmid]); %Even if `constr_x0` is overwritten, 
% if this line is deleted -> QP fails. 
ocp_model.set('constr_lbx_0',[x0;0.1])
ocp_model.set('constr_ubx_0',[x0;0.35])
ocp_model.set('constr_Jbx_0',eye(nx))

If i remove ocp_model.set('constr_x0', [x0;Tmid]);, Then the mpc fails with qp_stat = 3 no matter what.

  1. The inequality residual is the same as the difference T_{h,u} - T_{h,l}.

So setting:

ocp.set('constr_lbx', [x0;0.01], 0)
ocp.set('constr_ubx', [x0;0.35], 0)

results in res_ineq = 3.4e-1.

  • I added these observations in the original post
6

How do you initialize the problem? And what is the solution that you get in the end, i.e. what value for T_h?

7

I initialize it by interpolating [x0;Tmid] and [xref;Tmid] where T_{mid} = 0.5(T_{h,l}+T_{h,u}) (with the function lin_interpolate_traj which returns [x0;Tmid;x1;Tmid;x2;Tmid,...].

ocp.set('init_x',lin_interpolate_traj([x0;Tmid],[xref;Tmid],N+1));
ocp.set('init_u',repmat([zeros(3,1)],   N, 1));

If i do set ocp_model.set('constr_x0', [x0;Tmid]) then T_h is always T_{h,l}.
If i do not set the above, even if i set lower and upper bounds with constr_lbx_0 and constr_ubx_0, then the qp fails, with return status 3.

8

I uploaded a simplified version of the code that reproduces these problems.

The code is here.