OCP solver fails after adding nonlinear constraint

eBunny · March 29, 2022, 3:56am

Hello,

I have been working on the following OCP recently and I have trouble getting a solution when control allocation is implemented into the constraints.

Here is the code repo:

GitHub - ErcBunny/learn_acados.

To reproduce, please run omnihex.py. Without the allocation part (see the referenced lines below), the code runs nicely.

github.com

ErcBunny/learn_acados/blob/d8f9346a6d3841c4a5bca7f899a263308de4916a/omnihex.py#L154

      
        
                y = B_inv @ vertcat(f, t)
                f_unit_actuator = vertcat(
                    sqrt(y[0]**2 + y[1]**2),
                    sqrt(y[2]**2 + y[3]**2),
                    sqrt(y[4]**2 + y[5]**2),
                    sqrt(y[6]**2 + y[7]**2),
                    sqrt(y[8]**2 + y[9]**2),
                    sqrt(y[10]**2 + y[11]**2)
                )
            
            
    return vertcat(v, w)#, f_unit_actuator)
            
            

            

            
def formulate_omnihex_ocp():
            
            
    # cost weighting matrices
                Q_p = SX([1, 1, 1]) * 10
                Q_q = SX([2, 4, 1]) * 15
                Q_v = SX([1, 1, 1]) * 5
                Q_w = SX([1, 1, 1]) * 5

github.com

ErcBunny/learn_acados/blob/d8f9346a6d3841c4a5bca7f899a263308de4916a/omnihex.py#L185

      
        
            Q_q_e = SX([2, 4, 1]) * 15
            Q_v_e = SX([1, 1, 1]) * 5
            Q_w_e = SX([1, 1, 1]) * 5
            Q_f_e = SX([1, 1, 1]) * 0.001
            Q_t_e = SX([1, 1, 1]) * 0.001
            Q_e = diag(vertcat(Q_p_e, Q_q_e, Q_v_e, Q_w_e, Q_f_e, Q_t_e))
            
            
# constraints, u = [f_dot, t_dot], h = [v, w, f_unit_actuator]
            u_lb = np.array([-1, -1, -1, -1, -1, -1]) * 100
            u_ub = np.array([ 1,  1,  1,  1,  1,  1]) * 100
            h_lb = np.array([-10, -10, -10, -90, -90, -90]) #, 0, 0, 0, 0, 0, 0])
            h_ub = np.array([10, 10, 10, 90, 90, 90])#, 20, 20, 20, 20, 20, 20])
            
            
# optimal control problem
            # https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcp
            ocp = AcadosOcp()
            
            
# https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcpDims
            # prediction horizon only, other fields are set automatically
            ocp.dims.N = 20

And this is what I can get when the allocation part is commented out as shown in the above code.

But once the control allocation part is involved, the solver (HPIPM) returns NaN immediately, even if the allocated thrust constraints are not violated as shown above in thrust.png.

I sincerely look forward to your suggestions. Thank you for reading and have a good day~

FreyJo · March 30, 2022, 1:33pm

Hi,

I think the term allocation is used in a confusing way in your post.
I changed the title of this thread to
“OCP solver fails after adding nonlinear constraint”

You will need to initialize the solver explicitly, for this problem formulation.
The default initialization is zeros, and an sqrt() constraint can not be linearized at 0.

Cheers!

eBunny · March 30, 2022, 3:01pm

Hi Jonathan,

Thank you for pointing out the problem here. I tried with a nonlinear constraint without sqrt() and it worked without initializing the solver explicitly.

Thanks again!