Acados issues on setting inequality constraint

Hi, I am currently trying to set up a non-linear constraint for my quadrotor to simultaneously and continuously avoid obstacles during its movement. However, I found that I failed to set up the inequality constraint Ap - b ≤ 0, where p is the position (x, y, z) as part of the state x. A is an n3 matrix and b is an n1 matrix.

The model is set up as follows:

def __init__(self, quad, t_horizon=1, n_nodes=20,
                 q_cost=None, r_cost=None, q_mask=None,
                 B_x=None, gp_regressors=None, rdrv_d_mat=None,
                 model_name="quad_3d_acados_mpc", solver_options=None):
        :param quad: quadrotor object
        :type quad: Quadrotor3D
        :param t_horizon: time horizon for MPC optimization
        :param n_nodes: number of optimization nodes until time horizon
        :param q_cost: diagonal of Q matrix for LQR cost of MPC cost function. Must be a numpy array of length 12.
        :param r_cost: diagonal of R matrix for LQR cost of MPC cost function. Must be a numpy array of length 4.
        :param B_x: dictionary of matrices that maps the outputs of the gp regressors to the state space.
        :param gp_regressors: Gaussian Process ensemble for correcting the nominal model
        :type gp_regressors: GPEnsemble
        :param q_mask: Optional boolean mask that determines which variables from the state compute towards the cost
        function. In case no argument is passed, all variables are weighted.
        :param solver_options: Optional set of extra options dictionary for solvers.
        :param rdrv_d_mat: 3x3 matrix that corrects the drag with a linear model according to Faessler et al. 2018. None
        if not used

        # Weighted squared error loss function q = (p_xyz, a_xyz, v_xyz, r_xyz), r = (u1, u2, u3, u4)
        if q_cost is None:
            q_cost = np.array([10, 10, 10, 0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05])
        if r_cost is None:
            r_cost = np.array([0.1, 0.1, 0.1, 0.1])

        self.T = t_horizon  # Time horizon
        self.N = n_nodes  # number of control nodes within horizon

        self.quad = quad

        self.max_u = quad.max_input_value
        self.min_u = quad.min_input_value
        # print(quad.max_input_value)
        # print(quad.min_input_value)

        # Declare model variables
        self.p = cs.MX.sym('p', 3)  # position
        self.q = cs.MX.sym('a', 4)  # angle quaternion (wxyz)
        self.v = cs.MX.sym('v', 3)  # velocity
        self.r = cs.MX.sym('r', 3)  # angle rate

        # Full state vector (13-dimensional)
        self.x = cs.vertcat(self.p, self.q, self.v, self.r)
        self.state_dim = 13

        # Control input vector
        u1 = cs.MX.sym('u1')
        u2 = cs.MX.sym('u2')
        u3 = cs.MX.sym('u3')
        u4 = cs.MX.sym('u4')
        self.u = cs.vertcat(u1, u2, u3, u4)

        # Nominal model equations symbolic function (no GP)
        self.quad_xdot_nominal = self.quad_dynamics(rdrv_d_mat)

        # Linearized model dynamics symbolic function
        self.quad_xdot_jac = self.linearized_quad_dynamics()

        # Initialize objective function, 0 target state and integration equations
        self.L = None = None

        # Additional variable for SFC constraints
        self.constraint_expr = None

        # Check if GP ensemble has an homogeneous feature space (if actual Ensemble)
        if gp_regressors is not None and gp_regressors.homogeneous:
            self.gp_reg_ensemble = gp_regressors
            self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx]
            self.B_x = np.squeeze(np.stack(self.B_x, axis=1))
            self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x
            self.B_z = gp_regressors.B_z
        elif gp_regressors and gp_regressors.no_ensemble:
            # If not homogeneous, we have to treat each z feature vector independently
            self.gp_reg_ensemble = gp_regressors
            self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx]
            self.B_x = np.squeeze(np.stack(self.B_x, axis=1))
            self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x
            self.B_z = gp_regressors.B_z
            self.gp_reg_ensemble = None

        # Declare model variables for GP prediction (only used in real quadrotor flight with EKF estimator).
        # Will be used as initial state for GP prediction as Acados parameters.
        self.gp_p = cs.MX.sym('gp_p', 3)
        self.gp_q = cs.MX.sym('gp_a', 4)
        self.gp_v = cs.MX.sym('gp_v', 3)
        self.gp_r = cs.MX.sym('gp_r', 3)
        self.gp_x = cs.vertcat(self.gp_p, self.gp_q, self.gp_v, self.gp_r)

        # The trigger variable is used to tell ACADOS to use the additional GP state estimate in the first optimization
        # node and the regular integrated state in the rest
        self.trigger_var = cs.MX.sym('trigger', 1)

        # Build full model. Will have 13 variables. self.dyn_x contains the symbolic variable that
        # should be used to evaluate the dynamics function. It corresponds to self.x if there are no GP's, or
        # self.x_with_gp otherwise
        acados_models, nominal_with_gp = self.acados_setup_model(
            self.quad_xdot_nominal(x=self.x, u=self.u)['x_dot'], model_name)

        # Convert dynamics variables to functions of the state and input vectors
        self.quad_xdot = {}
        for dyn_model_idx in nominal_with_gp.keys():
            dyn = nominal_with_gp[dyn_model_idx]
            self.quad_xdot[dyn_model_idx] = cs.Function('x_dot', [self.x, self.u], [dyn], ['x', 'u'], ['x_dot'])

        # ### Setup and compile Acados OCP solvers ### #
        self.acados_ocp_solver = {}

        # Check if GP's have been loaded
        self.with_gp = self.gp_reg_ensemble is not None

        # Add one more weight to the rotation (use quaternion norm weighting in acados)
        q_diagonal = np.concatenate((q_cost[:3], np.mean(q_cost[3:6])[np.newaxis], q_cost[3:]))
        if q_mask is not None:
            q_mask = np.concatenate((q_mask[:3], np.zeros(1), q_mask[3:]))
            q_diagonal *= q_mask

        # Ensure current working directory is current folder
        self.acados_models_dir = '../../acados_models'
        safe_mkdir_recursive(os.path.join(os.getcwd(), self.acados_models_dir))

        for key, key_model in zip(acados_models.keys(), acados_models.values()):
            nx = key_model.x.size()[0]
            nu = key_model.u.size()[0]
            ny = nx + nu
            n_param = key_model.p.size()[0] if isinstance(key_model.p, cs.MX) else 0

            acados_source_path = os.environ['ACADOS_SOURCE_DIR']
            sys.path.insert(0, '../common')

            # Create OCP object to formulate the optimization
            ocp = AcadosOcp()
            ocp.acados_include_path = acados_source_path + '/include'
            ocp.acados_lib_path = acados_source_path + '/lib'
            ocp.model = key_model
            ocp.dims.N = self.N
   = t_horizon

            # Initialize parameters
   = n_param
            ocp.parameter_values = np.zeros(n_param)

            ocp.cost.cost_type = 'LINEAR_LS'
            ocp.cost.cost_type_e = 'LINEAR_LS'

            ocp.cost.W = np.diag(np.concatenate((q_diagonal, r_cost)))
            ocp.cost.W_e = np.diag(q_diagonal)
            terminal_cost = 0 if solver_options is None or not solver_options["terminal_cost"] else 1
            ocp.cost.W_e *= terminal_cost

            ocp.cost.Vx = np.zeros((ny, nx))
            ocp.cost.Vx[:nx, :nx] = np.eye(nx)
            ocp.cost.Vu = np.zeros((ny, nu))
            ocp.cost.Vu[-4:, -4:] = np.eye(nu)

            ocp.cost.Vx_e = np.eye(nx)

            # Initial reference trajectory (will be overwritten)
            x_ref = np.zeros(nx)
            ocp.cost.yref = np.concatenate((x_ref, np.array([0.0, 0.0, 0.0, 0.0])))
            ocp.cost.yref_e = x_ref

            # Initial state (will be overwritten)
            ocp.constraints.x0 = x_ref

            # Set constraints
            ocp.constraints.lbu = np.array([self.min_u] * 4)
            ocp.constraints.ubu = np.array([self.max_u] * 4)
            ocp.constraints.idxbu = np.array([0, 1, 2, 3])

            # Solver options
            ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
            ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
            ocp.solver_options.integrator_type = 'ERK'
            ocp.solver_options.print_level = 0
            ocp.solver_options.nlp_solver_type = 'SQP_RTI' if solver_options is None else solver_options["solver_type"]

            # Compile acados OCP solver if necessary
            json_file = os.path.join(self.acados_models_dir, + '_acados_ocp.json')
            self.acados_ocp_solver[key] = AcadosOcpSolver(ocp, json_file=json_file)

The constraint is fed by:

def set_SFC(self, A, b, model_index=0):
        Sets state feasibility constraints (SFC) for the model.
        :param x: Position (x,y,z) of the quadrotor
        :param A: Matrix A in Ax <= b constraint. Shape must be (n_constraints, 3)
        :param b: Matrix b in Ax <= b constraint. Shape must be (n_constraints, 1)
        :param model_index: Index of the model to apply constraints
        :return: CasADi MX expression representing the constraints
        # Ensure A and b are numpy arrays
        A = np.array(A).reshape(-1, 3)
        b = np.array(b).flatten()

        # Extract the position vector p from the state vector x and ensure it is a column vector
        p = self.x[0:3].reshape((3, 1))

        # Create constraint expressions
        constraint_exprs = []
        for i in range(A.shape[0]):
            A_i = A[i, :].reshape(1, -1)  # Ensure A_i is a row vector (1x3)
            constraint_expr = cs.mtimes(A_i, p) - b[i]  # Matrix multiplication A_i * p - b[i]

        # Combine constraints into a single CasADi expression
        self.constraint_expr = cs.vertcat(*constraint_exprs)

        # Apply the constraints to the model
        self.acados_ocp_solver[model_index].constraints_set(0, 'constr_expr_h', self.constraint_expr)
        self.acados_ocp_solver[model_index].constraints_set(self.N, 'lh', np.zeros(len(constraint_exprs)))
        print("passed sucessfully 1")
        return constraint_expr

And finally, it will have a function to run the optimization.
Currently, I am stuck with an error that appears as follows:

 [[0.709389, -0.704817, 0.0], [-0.709389, 0.704817, -0.0], [0.519953, 0.523326, 0.675113], [-0.519953, -0.523326, -0.675113], [0.475831, 0.478918, -0.737714], [-0.475831, -0.478918, 0.737714]]
 [2.0, 2.0, 4.9141, 1.95949, 0.955737, 1.04426]
Traceback (most recent call last):
  File "/home/ben/datadriven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/nodes/", line 208, in <module>
  File "/home/ben/data_driven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/nodes/", line 162, in move2Goal
    SFC = quad_ros_mpc.set_SFC(A=A_np, b=b_np, model_index=model_data)
  File "/home/ben/data_driven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/src/quad_mpc/", line 117, in set_SFC
    return self.quad_mpc.set_SFC(A, b, model_index)
  File "/home/ben/data_driven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/src/quad_mpc/", line 99, in set_SFC
    return self.quad_opt.set_SFC(A, b, model_index)
  File "/home/ben/data_driven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/src/quad_mpc/", line 461, in set_SFC
    self.acados_ocp_solver[model_index].set(0, 'constr_expr_h', self.constraint_expr)
  File "/home/ben/tools/acados/interfaces/acados_template/acados_template/", line 1203, in set
    value = value.astype(float)
  File "/home/ben/virenv/gp_mpc/lib/python3.8/site-packages/casadi/", line 9485, in <lambda>
    __getattr = lambda self, name: _swig_getattr(self, MX, name)
  File "/home/ben/vir_env/gp_mpc/lib/python3.8/site-packages/casadi/", line 83, in swiggetattr
    raise AttributeError("'%s' object has no attribute '%s'" % (class_type.__name, name))
AttributeError: 'MX' object has no attribute 'astype'

Thanks for your help


you cannot change the nonlinear expressions used by the AcadosOcpSolver after generating it.
All nonlinear functions are code generated from the AcadosOcp object when generating the solver.
self.acados_ocp_solver[model_index].constraints_set(0, 'constr_expr_h', self.constraint_expr)
is not supposed to work.

I see, thanks for your response. I have a question: does this mean I cannot add the constraint after generating the solver? Are all nonlinear functions code-generated and not available for modification, such as elimination or addition? So, I am not able to continuously add and remove the inequality constraint in the optimization problem. The constraint can only be set before the line self.acados_ocp_solver[key] = AcadosOcpSolver(ocp, json_file=json_file). Once the code is generated, only the state and reference can be set?

You can update the bounds for the constraints and the parameter values stage wise.