NMPC controller for my car model produces abnormal outputs

Hi :

I’m building an MPC controller for a simple Ackermann-steering vehicle model to perform point stabilization control,but obtained extremely bizarre output results.The controller is not outputting correct values to guide the car toward the target point; instead, it’s wandering aimlessly.

My model:

Code
 x = ca.SX.sym('x')
        y = ca.SX.sym('y')
        theta = ca.SX.sym('theta')
        states = ca.vertcat(x, y, theta)
        if self.enable_first_order:
            v = ca.SX.sym('v')
            w = ca.SX.sym('w')
            states = ca.vertcat(states, v, w)
        self.nstates = states.size1()

        v_cmd = ca.SX.sym('v_cmd')
        w_cmd = ca.SX.sym('w_cmd')
        controls = ca.vertcat(v_cmd, w_cmd)
        self.ncontrols = controls.size1()

        if not self.enable_first_order:
            rhs = ca.vertcat(
                v_cmd * ca.cos(theta),
                v_cmd * ca.sin(theta),
                w_cmd
            )
        else:
            rhs = ca.vertcat(
                v * ca.cos(theta),
                v * ca.sin(theta),
                w,
                (v_cmd - v) / self.tau_v,
                (w_cmd - w) / self.tau_w,
            )
        xdot = ca.SX.sym('xdot', rhs.size1())

        # Target point parameters
        x_target = ca.SX.sym('x_target')
        y_target = ca.SX.sym('y_target')
        theta_target = ca.SX.sym('theta_target')

        # Obstacle parameters (assume up to 3 obstacles)
        x_obs1, y_obs1, r_obs1 = ca.SX.sym('x_obs1'), ca.SX.sym('y_obs1'), ca.SX.sym('r_obs1')
        x_obs2, y_obs2, r_obs2 = ca.SX.sym('x_obs2'), ca.SX.sym('y_obs2'), ca.SX.sym('r_obs2')
        x_obs3, y_obs3, r_obs3 = ca.SX.sym('x_obs3'), ca.SX.sym('y_obs3'), ca.SX.sym('r_obs3')
        obs_num = ca.SX.sym('obs_num')  # Actual number of obstacles

        # Combine all parameters into a single vector
        parameters = ca.vertcat(
            x_target, y_target, theta_target,
            x_obs1, y_obs1, r_obs1,
            x_obs2, y_obs2, r_obs2,
            x_obs3, y_obs3, r_obs3,
            obs_num
        )
        self.nparams = parameters.size1()

        # Hard constraints
        h = [
            (x - x_obs1)**2 + (y - y_obs1)**2 - (r_obs1 + self.safe_distance)**2,
            (x - x_obs2)**2 + (y - y_obs2)**2 - (r_obs2 + self.safe_distance)**2,
            (x - x_obs3)**2 + (y - y_obs3)**2 - (r_obs3 + self.safe_distance)**2,
            w_cmd**2 - self.w_gain**2 * v_cmd**2
        ]

        # Obstacle cost
        obs_cost = 0
        if self.enable_obs:
            for i in range(3):
                dist_sqr = (states[:2] - parameters[3+3*i:5+3*i]).T @ (states[:2] - parameters[3+3*i:5+3*i])
                sigmoid_factor = 1 / (1 + ca.exp((dist_sqr - (self.safe_distance - parameters[5+3*i])**2) * 0.5))
                obs_cost += ca.if_else(i < obs_num, self.obs_weight * sigmoid_factor, 0)

        model = AcadosModel()
        model.name = 'origin_car_model'
        model.p = parameters
        model.x = states
        model.u = controls
        model.f_expl_expr = rhs
        model.f_impl_expr = xdot - rhs
        model.cost_expr_ext_cost = self.Q_x * (x - x_target)**2 + self.Q_y * (y - y_target)**2 \
            + self.Q_theta * (ca.arctan2(ca.sin(theta - theta_target), ca.cos(theta - theta_target)))**2 \
            + self.R_v * v_cmd**2 + self.R_w * w_cmd**2 + obs_cost
        model.cost_expr_ext_cost_e = 10 * (self.Q_x * (x - x_target)**2 + self.Q_y * (y - y_target)**2 \
            + self.Q_theta * (ca.arctan2(ca.sin(theta - theta_target), ca.cos(theta - theta_target)))**2)
        model.con_h_expr = ca.vertcat(*h)

My OCP configuration code is as follows:

Code
        os.chdir(os.path.dirname(os.path.realpath(__file__)))
        acados_source_path = os.environ['ACADOS_SOURCE_DIR']
        sys.path.insert(0, acados_source_path)

        ocp = AcadosOcp()
        ocp.model = model
        ocp.acados_include_path = acados_source_path + '/include'
        ocp.acados_lib_path = acados_source_path + '/lib'

        ocp.dims.N = self.N
        ocp.dims.np = self.nparams
        ocp.dims.nx = self.nstates
        ocp.dims.nu = self.ncontrols
        ocp.dims.nh = 4

        # Initial parameter values
        ocp.parameter_values = np.array([
            0.0, 0.0, 0.0,
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            0
        ])

        # Set constraints
        ocp.constraints.lh = np.array([0.0] * 3 + [-1e10])
        ocp.constraints.uh = np.array([1e10] * 3 + [0])
        ocp.constraints.x0 = np.zeros(self.nstates)
        ocp.constraints.lbx = np.array([0, 0, -np.pi])
        ocp.constraints.ubx = np.array([5, 5, np.pi])
        ocp.constraints.idxbx = np.array([0, 1, 2])
        if self.enable_first_order:
            ocp.constraints.lbx = np.append(ocp.constraints.lbx, [self.v_min, -self.v_max * self.w_gain])
            ocp.constraints.ubx = np.append(ocp.constraints.ubx, [self.v_max, self.v_max * self.w_gain])
            ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4])
        ocp.constraints.lbu = np.array([self.v_min, -self.v_max * self.w_gain])
        ocp.constraints.ubu = np.array([self.v_max, self.v_max * self.w_gain])
        ocp.constraints.idxbu = np.array([0, 1])

        ocp.cost.cost_type = 'EXTERNAL'
        ocp.cost.cost_type_e = 'EXTERNAL'

        ocp.solver_options.tf = self.N * self.dt
        ocp.solver_options.N_horizon = self.N
        ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
        ocp.solver_options.nlp_solver_type = 'SQP_RTI'
        ocp.solver_options.nlp_solver_max_iter = self.max_iter
        ocp.solver_options.tol = self.solver_tol
        ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
        ocp.solver_options.ext_cost_num_hess = True
        ocp.solver_options.integrator_type = 'ERK'
        ocp.solver_options.print_level = 0

        os.makedirs("./Origin_car_model_cfg", exist_ok=True)
        try:
            solver = AcadosOcpSolver(ocp, json_file="./car_model_cfg/_acados_ocp.json", generate=True, build=True)
        except Exception as e:
            print(f"Failed to create solver: {e}")
            return

        for i in range(ocp.dims.N + 1):
            solver.set(i, 'x', np.zeros(self.nstates))

        for i in range(ocp.dims.N):
            solver.set(i, 'u', np.zeros(self.ncontrols))

        return solver

My control function:

        start_time = time.time()

        # Set initial state constraints
        self.solver.set(0, 'lbx', current_states)
        self.solver.set(0, 'ubx', current_states)
        self.solver.set(0, 'x', current_states)

        # Fill in obstacle parameters
        obs_params = np.array([
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            0
        ])
        if self.enable_obs:
            for i in range(len(obs)):
                obs_params[0+i*3] = obs[i][0]
                obs_params[1+i*3] = obs[i][1]
                obs_params[2+i*3] = obs[i][2]
            obs_params[-1] = len(obs)

        params = np.append(target_point, obs_params)
        for i in range(self.N + 1):
            self.solver.set(i, 'p', params)
        self.solver.solve()

        v_cmd = self.solver.get(0, 'u')[0]
        w_cmd = self.solver.get(0, 'u')[1]

        print(f"Solver time: {time.time() - start_time}s")
        return v_cmd, w_cmd

Complete code:

class MPCController:
    """
    Ackermann steering vehicle controller based on nonlinear Model Predictive Control (MPC)
    """
    def __init__(self, config: Dict[str, Any]):
        """
        Initialize the MPC controller

        Args:
            config: Configuration dictionary containing MPC parameters
        """
        # System model settings
        self.enable_first_order = config.get('enable_first_order', False)

        # Control parameters
        self.dt = config.get('dt', 0.1)                     # Control period (s)
        self.N = config.get('horizon', 10)                  # Prediction horizon length
        self.Q_x = config.get('Q_x', 10.0)                  # Weight for x position error
        self.Q_y = config.get('Q_y', 10.0)                  # Weight for y position error
        self.Q_theta = config.get('Q_theta', 5.0)           # Weight for heading angle error
        self.R_v = config.get('R_v', 1.0)                   # Weight for linear velocity control input
        self.R_w = config.get('R_w', 1.0)                   # Weight for angular velocity control input
        self.v_target = config.get('v_target', 1.0)         # Target velocity (m/s)
        self.Q_v = config.get('Q_v', 2)                     # Weight for velocity tracking error

        # Constraint parameters
        self.v_max = config.get('v_max', 0.8)               # Maximum linear velocity (m/s)
        self.v_min = config.get('v_min', -0.5)              # Minimum linear velocity (m/s)
        self.w_gain = config.get('w_gain', 3.0)             # Proportional gain between angular and linear velocity

        # First-order dynamic response parameters
        self.tau_v = config.get('tau_v', 0.8)               # Time constant for linear velocity response
        self.tau_w = config.get('tau_w', 0.8)               # Time constant for angular velocity response

        # Solver parameters
        self.max_iter = config.get('max_iter', 100)         # Maximum number of iterations
        self.solver_tol = config.get('solver_tol', 1e-8)    # Solver tolerance

        # Obstacle avoidance parameters
        self.enable_obs = config.get('enable_obs', False)
        self.max_obs_num = config.get('max_obs_num', 3)
        self.obs_weight = config.get('obs_weight', 5)
        self.safe_distance = config.get('safe_distance', 0.2)

        # Initialize solver
        self._setup_solver()
        print("Solver initialization complete")

        # Previous control inputs for computing control change rate
        self.u_prev = np.array([0.0, 0.0] * self.N)

    def _setup_solver(self):
        model = self.AkmCarModel()
        self.solver = self.configure_ocp(model)

    def AkmCarModel(self):
        x = ca.SX.sym('x')
        y = ca.SX.sym('y')
        theta = ca.SX.sym('theta')
        states = ca.vertcat(x, y, theta)
        if self.enable_first_order:
            v = ca.SX.sym('v')
            w = ca.SX.sym('w')
            states = ca.vertcat(states, v, w)
        self.nstates = states.size1()

        v_cmd = ca.SX.sym('v_cmd')
        w_cmd = ca.SX.sym('w_cmd')
        controls = ca.vertcat(v_cmd, w_cmd)
        self.ncontrols = controls.size1()

        if not self.enable_first_order:
            rhs = ca.vertcat(
                v_cmd * ca.cos(theta),
                v_cmd * ca.sin(theta),
                w_cmd
            )
        else:
            rhs = ca.vertcat(
                v * ca.cos(theta),
                v * ca.sin(theta),
                w,
                (v_cmd - v) / self.tau_v,
                (w_cmd - w) / self.tau_w,
            )
        xdot = ca.SX.sym('xdot', rhs.size1())

        # Target point parameters
        x_target = ca.SX.sym('x_target')
        y_target = ca.SX.sym('y_target')
        theta_target = ca.SX.sym('theta_target')

        # Obstacle parameters (assume up to 3 obstacles)
        x_obs1, y_obs1, r_obs1 = ca.SX.sym('x_obs1'), ca.SX.sym('y_obs1'), ca.SX.sym('r_obs1')
        x_obs2, y_obs2, r_obs2 = ca.SX.sym('x_obs2'), ca.SX.sym('y_obs2'), ca.SX.sym('r_obs2')
        x_obs3, y_obs3, r_obs3 = ca.SX.sym('x_obs3'), ca.SX.sym('y_obs3'), ca.SX.sym('r_obs3')
        obs_num = ca.SX.sym('obs_num')  # Actual number of obstacles

        # Combine all parameters into a single vector
        parameters = ca.vertcat(
            x_target, y_target, theta_target,
            x_obs1, y_obs1, r_obs1,
            x_obs2, y_obs2, r_obs2,
            x_obs3, y_obs3, r_obs3,
            obs_num
        )
        self.nparams = parameters.size1()

        # Hard constraints
        h = [
            (x - x_obs1)**2 + (y - y_obs1)**2 - (r_obs1 + self.safe_distance)**2,
            (x - x_obs2)**2 + (y - y_obs2)**2 - (r_obs2 + self.safe_distance)**2,
            (x - x_obs3)**2 + (y - y_obs3)**2 - (r_obs3 + self.safe_distance)**2,
            w_cmd**2 - self.w_gain**2 * v_cmd**2
        ]

        # Obstacle cost
        obs_cost = 0
        if self.enable_obs:
            for i in range(3):
                dist_sqr = (states[:2] - parameters[3+3*i:5+3*i]).T @ (states[:2] - parameters[3+3*i:5+3*i])
                sigmoid_factor = 1 / (1 + ca.exp((dist_sqr - (self.safe_distance - parameters[5+3*i])**2) * 0.5))
                obs_cost += ca.if_else(i < obs_num, self.obs_weight * sigmoid_factor, 0)

        model = AcadosModel()
        model.name = 'origin_car_model'
        model.p = parameters
        model.x = states
        model.u = controls
        model.f_expl_expr = rhs
        model.f_impl_expr = xdot - rhs
        model.cost_expr_ext_cost = self.Q_x * (x - x_target)**2 + self.Q_y * (y - y_target)**2 \
            + self.Q_theta * (ca.arctan2(ca.sin(theta - theta_target), ca.cos(theta - theta_target)))**2 \
            + self.R_v * v_cmd**2 + self.R_w * w_cmd**2 + obs_cost
        model.cost_expr_ext_cost_e = 10 * (self.Q_x * (x - x_target)**2 + self.Q_y * (y - y_target)**2 \
            + self.Q_theta * (ca.arctan2(ca.sin(theta - theta_target), ca.cos(theta - theta_target)))**2)
        model.con_h_expr = ca.vertcat(*h)

        return model

    def configure_ocp(self, model):
        os.chdir(os.path.dirname(os.path.realpath(__file__)))
        acados_source_path = os.environ['ACADOS_SOURCE_DIR']
        sys.path.insert(0, acados_source_path)

        ocp = AcadosOcp()
        ocp.model = model
        ocp.acados_include_path = acados_source_path + '/include'
        ocp.acados_lib_path = acados_source_path + '/lib'

        ocp.dims.N = self.N
        ocp.dims.np = self.nparams
        ocp.dims.nx = self.nstates
        ocp.dims.nu = self.ncontrols
        ocp.dims.nh = 4

        # Initial parameter values
        ocp.parameter_values = np.array([
            0.0, 0.0, 0.0,
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            0
        ])

        # Set constraints
        ocp.constraints.lh = np.array([0.0] * 3 + [-1e10])
        ocp.constraints.uh = np.array([1e10] * 3 + [0])
        ocp.constraints.x0 = np.zeros(self.nstates)
        ocp.constraints.lbx = np.array([0, 0, -np.pi])
        ocp.constraints.ubx = np.array([5, 5, np.pi])
        ocp.constraints.idxbx = np.array([0, 1, 2])
        if self.enable_first_order:
            ocp.constraints.lbx = np.append(ocp.constraints.lbx, [self.v_min, -self.v_max * self.w_gain])
            ocp.constraints.ubx = np.append(ocp.constraints.ubx, [self.v_max, self.v_max * self.w_gain])
            ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4])
        ocp.constraints.lbu = np.array([self.v_min, -self.v_max * self.w_gain])
        ocp.constraints.ubu = np.array([self.v_max, self.v_max * self.w_gain])
        ocp.constraints.idxbu = np.array([0, 1])

        ocp.cost.cost_type = 'EXTERNAL'
        ocp.cost.cost_type_e = 'EXTERNAL'

        ocp.solver_options.tf = self.N * self.dt
        ocp.solver_options.N_horizon = self.N
        ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
        ocp.solver_options.nlp_solver_type = 'SQP_RTI'
        ocp.solver_options.nlp_solver_max_iter = self.max_iter
        ocp.solver_options.tol = self.solver_tol
        ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
        ocp.solver_options.ext_cost_num_hess = True
        ocp.solver_options.integrator_type = 'ERK'
        ocp.solver_options.print_level = 0

        os.makedirs("./Origin_car_model_cfg", exist_ok=True)
        try:
            solver = AcadosOcpSolver(ocp, json_file="./Origin_car_model_cfg/_acados_ocp.json", generate=True, build=True)
        except Exception as e:
            print(f"Failed to create solver: {e}")
            return

        for i in range(ocp.dims.N + 1):
            solver.set(i, 'x', np.zeros(self.nstates))

        for i in range(ocp.dims.N):
            solver.set(i, 'u', np.zeros(self.ncontrols))

        return solver

    def control(self, current_states, target_point: np.ndarray, obs: List):
        start_time = time.time()

        # Set initial state constraints
        self.solver.set(0, 'lbx', current_states)
        self.solver.set(0, 'ubx', current_states)
        self.solver.set(0, 'x', current_states)

        # Fill in obstacle parameters
        obs_params = np.array([
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            1e10, 1e10, 0,
            0
        ])
        if self.enable_obs:
            for i in range(len(obs)):
                obs_params[0+i*3] = obs[i][0]
                obs_params[1+i*3] = obs[i][1]
                obs_params[2+i*3] = obs[i][2]
            obs_params[-1] = len(obs)

        params = np.append(target_point, obs_params)
        for i in range(self.N + 1):
            self.solver.set(i, 'p', params)
        self.solver.solve()

        v_cmd = self.solver.get(0, 'u')[0]
        w_cmd = self.solver.get(0, 'u')[1]

        print(f"Solver time: {time.time() - start_time}s")
        return v_cmd, w_cmd


mpc = MPCController({})
print('v_cmd,w_cmd:',mpc.control(np.array([0.0,0.0,0.0,0.0,0.0]),np.array([0.001,0.001,0]),[])[0:2],)

When I input a target point extremely close to the starting point (e.g., current_states = [0,0,0,0,0] , target point [0.0001, 0.0001] or [0.00001, 0.0001] ), the controller outputs unreasonable values like a linear velocity exceeding 0.6 , causing erratic behavior.

output
Solver time:0.0013821125030517578s
v_cmd,w_cmd: (np.float64(0.6795298852283124), np.float64(0.0))

The trajectory in my simulation program is extremely bizarre.
When first order enable:


When first order disable:
The car is spinning in place (instead of converging to the target).
with:

SQP_RTI: QP solver returned error status 3 QP iteration 23.

Is the issue in my model design or solver settings ?

Hi :waving_hand:

Please change from SQP_RTI to SQP and provide the solver status as Well as the output of print_statistics for the very first OCP.

Best, Katrin

Here are statistics printed by print_statistics()

iter    res_stat        res_eq          res_ineq        res_comp        qp_stat qp_iter alpha
0       2.000000e-02    0.000000e+00    0.000000e+00    0.000000e+00    0       0       0.000000e+00
1       4.766181e+16    5.551115e-17    0.000000e+00    1.875915e+16    2       50      1.000000e+00
2       4.399013e+01    5.768797e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
3       7.242058e+02    3.298908e-03    1.511882e+00    2.000000e+04    2       50      1.000000e+00
4       8.596677e+01    1.224789e-02    2.053648e-01    2.000000e+04    2       50      1.000000e+00
5       6.011502e+01    4.892291e-03    6.375722e-02    2.000000e+04    2       50      1.000000e+00
6       4.266681e+01    2.263097e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
7       1.736256e+01    1.980970e-04    2.021981e-02    2.000000e+04    2       50      1.000000e+00
8       2.716259e+01    5.652674e-05    0.000000e+00    2.000000e+04    2       50      1.000000e+00
9       6.001400e+00    5.050050e-05    2.231747e+00    2.000000e+04    2       50      1.000000e+00
10      1.226358e+01    4.433487e-04    0.000000e+00    2.000000e+04    2       50      1.000000e+00
11      5.250236e+01    4.302397e-03    1.571269e+00    2.000000e+04    2       50      1.000000e+00
12      7.327761e+01    1.329293e-02    9.257807e-01    2.000000e+04    2       50      1.000000e+00
13      9.806769e+01    5.348120e-03    1.117907e+00    2.000000e+04    2       50      1.000000e+00
14      4.304131e+01    2.735460e-03    2.882050e-01    2.000000e+04    2       50      1.000000e+00
15      2.014410e+01    6.205748e-05    4.338715e+00    2.000000e+04    2       50      1.000000e+00
16      2.117562e+02    5.331493e-03    8.348205e-01    2.000000e+04    2       50      1.000000e+00
17      5.071366e+01    3.534687e-03    6.009224e-01    2.000000e+04    2       50      1.000000e+00
18      5.316432e+01    3.535430e-03    8.190166e-01    2.000000e+04    2       50      1.000000e+00
19      5.759929e+01    5.703898e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
20      5.182338e+01    6.258958e-03    1.527597e+00    2.000000e+04    2       50      1.000000e+00
21      5.174761e+01    3.041937e-03    1.170946e+00    2.000000e+04    2       50      1.000000e+00
22      4.775709e+01    2.766853e-03    6.447523e-01    2.000000e+04    2       50      1.000000e+00
23      4.680870e+01    2.858686e-03    1.022741e+00    2.000000e+04    2       50      1.000000e+00
24      7.036854e+01    7.278909e-03    2.197681e+00    2.000000e+04    2       50      1.000000e+00
25      7.459606e+01    6.638957e-03    8.957359e-01    2.000000e+04    2       50      1.000000e+00
26      3.114378e+01    1.197778e-03    1.044671e+00    2.000000e+04    2       50      1.000000e+00
27      1.650711e+01    1.343784e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
28      2.706635e+01    1.490366e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
29      1.562501e+01    3.654193e-04    9.667476e-01    2.000000e+04    2       50      1.000000e+00
30      1.877226e+01    3.762582e-04    1.557765e+00    2.000000e+04    2       50      1.000000e+00
31      2.348403e+01    6.443386e-04    3.705739e-01    2.000000e+04    2       50      1.000000e+00
32      1.859180e+02    8.022191e-03    1.997469e+00    2.000000e+04    2       50      1.000000e+00
33      1.007048e+02    1.408255e-02    1.859779e+00    2.000000e+04    2       50      1.000000e+00
34      7.917103e+01    7.875428e-03    2.878713e-02    2.000000e+04    2       50      1.000000e+00
35      6.333526e+01    1.017720e-02    4.721097e+00    2.000000e+04    2       50      1.000000e+00
36      6.709126e+01    7.312126e-03    1.369459e+00    2.000000e+04    2       50      1.000000e+00
37      7.326488e+01    5.935143e-03    1.732711e+00    2.000000e+04    2       50      1.000000e+00
38      1.375502e+02    7.283065e-03    8.893934e-01    2.000000e+04    2       50      1.000000e+00
39      9.125129e+01    1.002226e-02    1.705887e+00    2.000000e+04    2       50      1.000000e+00
40      9.477177e+01    8.464762e-03    1.172977e+00    2.000000e+04    2       50      1.000000e+00
41      8.015496e+01    8.475183e-03    2.319669e-01    2.000000e+04    2       50      1.000000e+00
42      9.043220e+01    9.583864e-03    8.913263e-01    2.000000e+04    2       50      1.000000e+00
43      8.825825e+01    1.244865e-02    0.000000e+00    2.000000e+04    2       50      1.000000e+00
44      1.726166e+01    5.759094e-04    4.651240e-02    2.000000e+04    2       50      1.000000e+00
45      2.992248e+01    1.837835e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
46      2.869788e+01    1.208769e-03    1.471199e+00    2.000000e+04    2       50      1.000000e+00
47      7.426034e+01    7.750078e-03    2.250111e+00    2.000000e+04    2       50      1.000000e+00
48      4.934008e+01    3.610668e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
49      6.691600e+01    2.403982e-03    1.405162e+00    2.000000e+04    2       50      1.000000e+00
50      5.949674e+01    4.709533e-03    5.479338e-01    2.000000e+04    2       50      1.000000e+00
51      7.234555e+01    9.575124e-03    1.711632e+00    2.000000e+04    2       50      1.000000e+00
52      8.343060e+01    9.286888e-03    9.125318e-01    2.000000e+04    2       50      1.000000e+00
53      8.542989e+01    9.871649e-03    4.266905e-01    2.000000e+04    2       50      1.000000e+00
54      6.733941e+01    4.880871e-03    3.039631e-01    2.000000e+04    2       50      1.000000e+00
55      6.595532e+01    5.336478e-03    1.252715e+00    2.000000e+04    2       50      1.000000e+00
56      4.527385e+01    2.416508e-03    1.231674e-01    2.000000e+04    2       50      1.000000e+00
57      4.902991e+01    4.364972e-04    1.772495e+00    2.000000e+04    2       50      1.000000e+00
58      4.340542e+01    1.057513e-03    4.558663e-01    2.000000e+04    2       50      1.000000e+00
59      4.925451e+01    3.296180e-03    4.553073e-01    2.000000e+04    2       50      1.000000e+00
60      3.760992e+01    1.637589e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
61      2.322926e+01    6.108530e-04    2.525750e+00    2.000000e+04    2       50      1.000000e+00
62      2.231497e+01    5.821167e-04    1.274632e-01    2.000000e+04    2       50      1.000000e+00
63      4.495106e+01    2.529151e-03    1.404187e+00    2.000000e+04    2       50      1.000000e+00
64      6.359147e+01    4.782581e-03    4.062270e-01    2.000000e+04    2       50      1.000000e+00
65      6.912123e+01    6.010061e-03    1.421223e+00    2.000000e+04    2       50      1.000000e+00
66      6.473667e+01    4.527319e-03    8.506808e-01    2.000000e+04    2       50      1.000000e+00
67      5.947820e+01    3.694527e-03    2.009602e+00    2.000000e+04    2       50      1.000000e+00
68      7.075272e+01    4.571067e-03    1.094345e+00    2.000000e+04    2       50      1.000000e+00
69      7.059737e+01    5.887728e-03    1.074816e+00    2.000000e+04    2       50      1.000000e+00
70      8.150690e+01    7.753619e-03    1.461367e+00    2.000000e+04    2       50      1.000000e+00
71      1.001982e+02    1.189191e-02    2.064967e+00    2.000000e+04    2       50      1.000000e+00
72      1.211639e+02    8.340713e-03    8.907554e-01    2.000000e+04    2       50      1.000000e+00
73      6.713846e+01    5.198767e-03    5.851567e-01    2.000000e+04    2       50      1.000000e+00
74      6.950699e+01    4.818485e-03    2.121037e+00    2.000000e+04    2       50      1.000000e+00
75      8.968584e+01    9.409324e-03    2.569036e+00    2.000000e+04    2       50      1.000000e+00
76      5.530068e+01    3.206740e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
77      1.948044e+01    5.508766e-04    2.203978e+00    2.000000e+04    2       50      1.000000e+00
78      6.952381e+01    1.275337e-03    1.623946e+00    2.000000e+04    2       50      1.000000e+00
79      3.332449e+01    1.497002e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
80      5.680461e+01    2.118402e-03    1.401801e-04    2.000000e+04    2       50      1.000000e+00
81      4.463969e+01    3.263347e-03    1.214634e+00    2.000000e+04    2       50      1.000000e+00
82      4.356485e+01    6.962140e-04    1.992347e+00    2.000000e+04    2       50      1.000000e+00
83      3.271749e+01    1.197585e-03    2.020683e+00    2.000000e+04    2       50      1.000000e+00
84      6.214354e+01    6.894197e-03    1.885108e+00    2.000000e+04    2       50      1.000000e+00
85      6.222415e+01    3.967523e-03    2.062100e+00    2.000000e+04    2       50      1.000000e+00
86      5.405871e+01    1.682502e-03    1.164125e+00    2.000000e+04    2       50      1.000000e+00
87      7.014465e+00    5.551642e-04    1.566313e+00    2.000000e+04    2       50      1.000000e+00
88      9.987646e+00    2.997549e-04    6.113828e-01    2.000000e+04    2       50      1.000000e+00
89      2.782642e+00    2.533327e-04    0.000000e+00    2.000000e+04    2       50      1.000000e+00
90      6.679269e+00    2.812317e-04    2.768659e-01    2.000000e+04    2       50      1.000000e+00
91      2.144465e+01    1.279824e-03    1.901472e+00    2.000000e+04    2       50      1.000000e+00
92      3.257145e+01    1.866209e-03    1.323509e+00    2.000000e+04    2       50      1.000000e+00
93      3.257063e+02    7.703641e-04    2.481529e-01    2.000000e+04    2       50      1.000000e+00
94      2.353986e+01    9.855668e-04    1.680657e+00    2.000000e+04    2       50      1.000000e+00
95      7.206853e+01    1.060502e-03    0.000000e+00    2.000000e+04    2       50      1.000000e+00
96      1.885504e+01    1.611496e-04    2.490267e+00    2.000000e+04    2       50      1.000000e+00
97      4.148197e+01    1.990607e-04    0.000000e+00    2.000000e+04    2       50      1.000000e+00
98      2.235752e+00    6.505420e-05    8.087813e-01    2.000000e+04    2       50      1.000000e+00
99      6.213210e+01    9.324859e-04    1.681562e+00    2.000000e+04    2       50      1.000000e+00
100     6.213210e+01    9.324859e-04    1.681562e+00    2.000000e+04    2       50      1.000000e+00

get_status() returns 2
I use the model with first_order enable.

mpc.control(current_states=np.array([0.0,0.0,0.0,0.0,0.0]),target_point=np.array([0.001,0.001,0]),obs=[])[0:2]

It seems to reach maximum number of iterations,but always reach it no matter how high the max is.

Thanks for answering.

Could it be an issue with the initial guess?I first used IPOPT, but that solver doesn’t seem to be sensitive to initial guesses.

Sorry,I didn’t know that it’essential to set slacks to avoid nonlinear constraints making it infeasible,so it’s quite a stupid question.However,how to set the zl,zu,Zl,Zu?I tried several values(from 10 to 1e8),but none has a nice effect.Low values can’t lead the car to avoid obstacles.Too high values make the outputs fluctuate significantly and make the solver quite slow(SQP).Moreover,how to use SQP_RTI?Should I assign it a designated path?

Hi :waving_hand:

the output of print_statistics suggest that the QP solver is not converging. Please try to increase the maximum number of QP iterations.

Introducing slacks for the obstacle constraints is a good idea! I would add a small quadratic weight and then tune mostly the linear weight.

In addition you might want to check the integrator. Did you validate it outside of an OCP formulation, i.e. using an acados simulator?

I don’t quite get your question regarding RTI. If your formulation is working well with SQP and you have a good initial guess for the very first OCP, SQP_RTI should yield reasonable results.

Best, Katrin

Thanks for reply :smiling_face_with_three_hearts:!I successfully implemented control of my model using the LINEARLS cost type, and it works well with both SQP and SQP_RTI solvers. However, the EXTERN cost type still fails. Could this be due to the introduction of non-differentiable functions like arctan2? Additionally, when using the SQP/SQP_RTI solvers, I observed significant output oscillations, and the robot’s trajectory exhibits minor wobbling (especially in point-to-point control over longer distances), unlike the IPOPT solver which moves straight to the target after turning. Would specifying a reference path resolve this issue?