MPC path following problem works on casadi but not acados

Hi,

I am working on a similar problem as the racecar example with some changes to the model. I have got successful results in the past with the same problem formulation on casadi with qpoases (in python) as the solver. However, I wanted to switch to acados (python interface) since it has more functionalities. I have used different solver options such as ‘FULL_CONDENSING_QPOASES’ and ‘PARTIAL_CONDENSING_HPIPM’ and both SQP and SQP_RTI.
The issue is that the problem doesn’t converge or at best it oscillates over the path to converge after 10 seconds (it’s a car that is supposed to follow a straightline starting from an initial laterally offset point). I start 5 meters lateral to the straight line to see if the mpc can take the car to the straight line and keep it straight.
Sometimes, HPIPM gives me status 3 which seems to be related to NaN solution. This specifically happens when the lateral error is almost near zero and there are slight heading errors.
However, I was not able to see anywhere in my code that results in NaN solution. What can cause this type of error usually? I have tried to get rid of most of my inequalities to check whether that solves the issue but I still end up in status 3 and status 37 (for qpoases) sometimes in the middle of the simulation.

Any help/tips are greatly appreciated as I am new to acados.

Below is a snippet of my mpc problem. acados_settings function defines the OCP and solve_mpc() runs in the loop for each iteration of mpc:

    def acados_settings(self, Tf, N):
        # create render arguments
        ocp = AcadosOcp()

        # export model
        model, constraint = self.bicycle_model(self.initial_conditions, self.params)

        # define acados ODE
        model_ac = AcadosModel()
        model_ac.f_impl_expr = model.f_impl_expr
        model_ac.f_expl_expr = model.f_expl_expr
        model_ac.x = model.x
        model_ac.xdot = model.xdot
        model_ac.u = model.u
        model_ac.name = model.name
        ocp.model = model_ac

        # define constraint
        # model_ac.con_h_expr = constraint.expr

        # dimensions
        nx = model.x.size()[0]
        nu = model.u.size()[0]
        ny = nx + nu
        ny_e = nx

        nsbx = 1
        # nh = constraint.expr.shape[0]
        nh = 0
        nsh = nh
        ns = nsh + nsbx

        # discretization
        ocp.dims.N = N

        # set cost
        Q = np.diag([0, 2000.0, 6000.0, 0, 0, 0])

        R = np.eye(nu)
        R[0, 0] = 1e-3
        R[1, 1] = 5e-3

        Qe = np.diag([0, 20000.0, 6000.0, 0, 0, 0])

        ocp.cost.cost_type = "LINEAR_LS"
        ocp.cost.cost_type_e = "LINEAR_LS"
        unscale = N / Tf

        ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R)
        ocp.cost.W_e = Qe / unscale

        Vx = np.zeros((ny, nx))
        Vx[1, 1] = 1  # y
        Vx[2, 2] = 1  # yaw
        Vx[3, 3] = 0  # forward velocity - set to zero for now
        ocp.cost.Vx = Vx

        Vu = np.zeros((ny, nu))
        Vu[6, 0] = 1.0  # tau
        Vu[7, 1] = 1.0  # delta
        ocp.cost.Vu = Vu

        Vx_e = np.zeros((ny_e, nx))
        Vx_e[1, 1] = 1  # y
        Vx_e[2, 2] = 1  # yaw
        Vx_e[3, 3] = 0  # forward velocity
        ocp.cost.Vx_e = Vx_e

        # set intial references
        ocp.cost.yref = np.array([0, 10, 0, 10, 0, 0, 0, 0])
        ocp.cost.yref_e = np.array([0, 10, 0, 10, 0, 0])

        # input constraints
        ocp.constraints.lbu = np.array([model.tau_min, model.delta_min])
        ocp.constraints.ubu = np.array([model.tau_max, model.delta_max])
        ocp.constraints.Jbu = np.array([[1, 0],
                                        [0, 1]])

        # terminal constraints - commented up
        # ocp.constraints.Jbx_e = np.array([[0, 1, 0, 0, 0, 0],
        #                                   [0, 0, 1, 0, 0, 0]])
        # ocp.constraints.lbx_e = np.array([0, -80 * np.pi/180])
        # ocp.constraints.ubx_e = np.array([20, +80 * np.pi/180])

        # box constraints
        # ocp.constraints.lh = np.array(
        #     [
        #         constraint.along_min,
        #         constraint.alat_min,
        #     ]
        # )
        # ocp.constraints.uh = np.array(
        #     [
        #         constraint.along_max,
        #         constraint.alat_max,
        #     ]
        # )

        # set initial condition
        ocp.constraints.x0 = model.x0

        # set QP solver and integration
        ocp.solver_options.tf = Tf  # prediction horizon
        # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
        ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
        ocp.solver_options.nlp_solver_type = "SQP_RTI"
        # ocp.solver_options.levenberg_marquardt = 0.001
        ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
        ocp.solver_options.integrator_type = "ERK"
        ocp.solver_options.sim_method_num_stages = 4
        ocp.solver_options.sim_method_num_steps = 3
        ocp.solver_options.nlp_solver_max_iter = 1000
        ocp.solver_options.tol = 1e-4
        # ocp.solver_options.nlp_solver_tol_comp = 1e-1

        # create solver
        acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json")
        acados_solver.options_set('warm_start_first_qp', 0)

        return constraint, model, acados_solver

def solve_mpc(self, current_state, uk_prev_step):
        # initialize x0 with current measurment
        x_r, y_r, yaw_r, vx_r, vy_r, omega_r = current_state
        self.acados_solver.set(0, "lbx", current_state)
        self.acados_solver.set(0, "ubx", current_state)

        # cost parameters
        qy = 200 * 1 / 0.001 ** 2  #cost for lateral error
        qyaw = 10 * 1 / (0.01 * np.pi / 180) ** 2  #cost for yaw error

        Q = np.diag([0, qy, qyaw, 0, 0, 0])
        R = np.eye(2)
        R[0, 0] = 0 * 1e-3 # R_tau (torque request - set to zero for now as only delta is being commanded)
        R[1, 1] = 5000 * 1 / (0.1 * np.pi / 180) ** 2  # R_delta (cost for delta)

        W = self.N / self.T * scipy.linalg.block_diag(Q, R) 
        Qe = np.diag([0, 10 * qy, 10 * qyaw, 0, 0, 0]) / (self.N / self.T)
        W_e = Qe

        # Update the cost
        for i in range(self.N):
            self.acados_solver.cost_set(i, 'W', W)
            yref = np.array([0, 10, 0, 10.0, 0, 0, uk_prev_step[0], uk_prev_step[1]])
            self.acados_solver.set(i, "yref", yref)
        self.acados_solver.cost_set(self.N, 'W', W_e)
        yref_N = np.array([0, 10, 0, 0.0, 0, 0])
        self.acados_solver.set(self.N, "yref", yref_N)

        # set options
        self.acados_solver.options_set('print_level', 0)
        status = self.acados_solver.solve()

        # get solution
        x0 = self.acados_solver.get(0, "x")
        u0 = self.acados_solver.get(0, "u")
        xN = self.acados_solver.get(self.N, "x")

        if status != 0:
            print("acados returned status {}.".format(status))

        self.acados_solver.print_statistics()

        return [u0, crosstrack, x0, xN, status]

From a quick look over, in the solve_mpc routine the R being set is no longer positive definite.
Generally MPC formulations the Q,R are real and symmetric with: Q needing to be positive semidefinite and R positive definite.

1 Like