Explicit external cost function QP formulation defined in ACADOS and pass it to the solver to solve it

MPC Discussions
, ,
1

Hello all,

My initial goal is to build up the incremental MPC problem, using the change of control input to replace the absolute control input. What’s more, I want to define the cost function like

\boldsymbol{J} = \underset{\mathbf{x}}{\min} \space \frac{1}{2}{\boldsymbol{x}^\top \boldsymbol{\bar H}}\boldsymbol{x} + \boldsymbol{g}^\top \boldsymbol{x}

and replace the x with the \Delta u, which turns out:

\boldsymbol{J} = \underset{\mathbf{\Delta u}}{\min} \space \frac{1}{2}{\boldsymbol{\Delta u}^\top \boldsymbol{\bar H}}\boldsymbol{\Delta u} + \boldsymbol{g}^\top \boldsymbol{\Delta u}

The way I do is to pre-define all matrices which are necessary of setting up such cost function as QP.
I set the cost function type as ‘EXTERNAL’, and define every matrix which combines together as the Hessian matrix \bar H.
But it turns out the error is about:

RuntimeError: Error in Function::Function for 'LMPC_cost_ext_cost_e_fun' [MXFunction] at .../casadi/core/function.cpp:280:
.../casadi/core/function_internal.cpp:146: Error calling MXFunction::init for 'LMPC_cost_ext_cost_e_fun':
.../casadi/core/mx_function.cpp:406: LMPC_cost_ext_cost_e_fun::init: Initialization failed since variables [delta_kappa] are free. These symbols occur in the output expressions but you forgot to declare these as inputs. Set option 'allow_free' to allow free variables.

Which means I lost the \Delta u as the control variable.
Just wanted to know if anyone has experience setting such external cost function and solve it.

Below I provided the snippet code of settings of model and the external cost function expression:

        ## CasADi model
        # State variables
        posy   = MX.sym('posy')
        yaw    = MX.sym('yaw')
        kappa  = MX.sym('kappa')

        # Parameters
        v      = vehicle_params['velocity']

        # Control inputs
        delta_kappa = MX.sym('delta_kappa')

        # Build the state vector by vertically concatenating the state variables
        # State vector
        x = vertcat(posy, yaw, kappa)

        # Build the control vector by vertically concatenating the control inputs
        # Control vector
        u = vertcat(delta_kappa)

 # Define cost function
    ocp.cost.cost_type = 'EXTERNAL' # Cost type at intermiate shooting nodes (1 to N-1)
    ocp.cost.cost_type_e = 'EXTERNAL' # Cost type at terminal shooting node (N)

    C_bar = np.zeros((nx*N, nu*N))

    # gather all needed matrices
    A_aug = pred_mb_model.A_d_prime
    B_aug = pred_mb_model.B_d_prime[:, 0]
    C_prime = np.array([[1, 0, 0], 
                        [0, 1, 0],
                        [0, 0, 1]])
    A_hat = np.vstack([np.linalg.matrix_power(A_aug, i) for i in range(1, N+1)])
    Q_bar = np.kron(np.eye(N), C_prime.T @ Q @ C_prime)
    R_bar = np.kron(np.eye(N), R)
    T_bar = np.kron(np.eye(N), Qe @ C_prime)
    for i in range(N):
        for j in range(i+1):
            c_block = np.linalg.matrix_power(A_aug, i-j) @ B_aug # represent the influence of state at i time step when control input employed at j time step
            C_bar[i * nx:(i+1) * nx, j * nu:(j+1) * nu] = c_block # thw power of the A_aug matrix
    H_bar = C_bar.T @ Q_bar @ C_bar + R_bar # H matrix

    # Cost function weights 
    ocp.cost.W = H_bar
    ocp.cost.W_e = H_bar

    # S = np.tril(np.ones((N * nu, N * nu)))S
    S = MX.ones(N * nu, 1)
    # Extract the state and control input variables from p
    x_t = ocp.model.p[:nx]  # current state (nx, 1)
    r = ocp.model.p[nx:]    # reference states (nx*N, 1)

    f = (x_t.T @ A_hat.T @ Q_bar @ C_bar - r.T @ T_bar @ C_bar).T

    # Initial references
    ocp.model.cost_expr_ext_cost = 0.5 * ((S @ pred_mb_model.u).T @ H_bar @ (S @ pred_mb_model.u)) + f.T @ (S @ pred_mb_model.u)
    ocp.model.cost_expr_ext_cost_e = 0.5 * ((S @ pred_mb_model.u).T @ H_bar @ (S @ pred_mb_model.u))

Best regards,
Brian

2

Hi Brian,

The problem is that you try to define a terminal cost which is a function of the controls u. Note that the problem formulation of acados requires the terminal cost to only depend on the state.

Best, Katrin

3

HI Katrin, Thanks for your reply, I will try without u in the terminal cost. Hope I can fix it:)