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):

- a new state T_h,
- a new input ΔT_h with ΔT_h \in [0,0]
- 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
```