ACADOS non-linear cost calculation

Question

I am new to ACADOS and MPC.

I have a task where I need to define the dynamics of a mobile robot and solve the MPC problem using ACADOS.

I have defined the model using CASADI as follows:

def mobile_robot_model():
    """
    Define a simple mobile robot model.

    Returns:
        x_vec (MX): Symbolic vector representing the state [x, y, v, theta].
        u (MX): Symbolic vector representing the control input [a, w].
        continuous_dynamics (Function): CasADi function for the continuous-time dynamics.
            Takes state vector and control input vectors as input and returns
            the vector representing the rates of change of the state variables.
    """

    model_name = 'mobile_robot'

    # Define symbolic variables (states)
    x = ca.MX.sym('x')
    y = ca.MX.sym('y')
    v = ca.MX.sym('v')
    theta = ca.MX.sym('theta')

    # Control
    a = ca.MX.sym('a')  # acceleration
    w = ca.MX.sym('w')  # angular velocity

    # Define state and control vectors
    states = ca.vertcat(x, y, v, theta)
    controls = ca.vertcat(a, w)
    rhs = [v*ca.cos(theta), v*ca.sin(theta), a, w]
    x_dot = ca.MX.sym('x_dot', len(rhs))

    # Create a CasADi function for the continuous-time dynamics
    continuous_dynamics = ca.Function(
        'continuous_dynamics',
        [states, controls],
        [ca.vcat(rhs)],
        ["state", "control_input"],
        ["rhs"]
    )

    f_impl = x_dot - continuous_dynamics(states, controls)

    model = AcadosModel()

    model.f_expl_expr = continuous_dynamics(states, controls)
    model.f_impl_expr = f_impl
    model.x = states
    model.xdot = x_dot
    model.u = controls
    model.p = []
    model.name = model_name

    return model

Next I need to setup the Acados solver, including the cost function.

● The cost function of minimizing the error between real and initial trajectory has the following general formula (I can't post the image due to low rep): formula1

where q and u are the output and input vectors of the trajectory of the robot, p and r are the output and input vector of the trajectory, 1 , 2 , 3 are weights. The desired control inputs are = [ , ] = 0

● For avoiding obstacles, use the following cost function: formula2

my question is how I can implement this kind of cost function in ACADOS. I was thinking I need to use EXTERNAL cost type but later I was told I should use NONLINEAR_LS.

This is my code till now:

def create_ocp_solver():

    # Create AcadosOcp object
    ocp = AcadosOcp()

    # Set up the optimization problem
    model = mobile_robot_model()
    ocp.model = model

    # --------------------PARAMETERS--------------
    # constants
    nx = model.x.size()[0]
    nu = model.u.size()[0]
    ny = nx + nu
    ny_e = nx
    T = 30
    N = 100
    n_params = len(model.p)

    # Setting initial conditions
    ocp.dims.N = N
    ocp.dims.nx = nx
    ocp.dims.nu = nu
    # ocp.dims.ny = ny
    # ocp.dims.ny_e = ny_e
    ocp.solver_options.tf = T

    # initial state
    x_ref = np.zeros(nx)

    # Set initial condition for the robot
    ocp.constraints.x0 = x_ref

    # initialize parameters
    ocp.dims.np = n_params
    ocp.parameter_values = np.zeros(n_params)

    # ---------------------CONSTRAINTS------------------
    # Define constraints on states and control inputs
    ocp.constraints.lbu = np.array([-0.1, -0.3])  # Lower bounds on control inputs
    ocp.constraints.ubu = np.array([0.1, 0.3])    # Upper bounds on control inputs
    ocp.constraints.lu = np.array([100, 100, 1, 10])  # Upper bounds on states
    ocp.constraints.idxbu = np.array([0, 1])  # for indices 0 & 1