# 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,
B_x=None, gp_regressors=None, rdrv_d_mat=None,
"""
: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

# 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)

# Linearized model dynamics symbolic function

# Initialize objective function, 0 target state and integration equations
self.L = None
self.target = 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
else:
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

# Convert dynamics variables to functions of the state and input vectors
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 ### #

# 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:]))

# Ensure current working directory is current folder
os.chdir(os.path.dirname(os.path.realpath(__file__)))

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

sys.path.insert(0, '../common')

# Create OCP object to formulate the optimization
ocp.model = key_model
ocp.dims.N = self.N
ocp.solver_options.tf = t_horizon

# Initialize parameters
ocp.dims.np = 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
``````

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]
constraint_exprs.append(constraint_expr)

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

# Apply the constraints to the model
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:

``````A:
[[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]]
b:
[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/trial_mtoa2.py", line 208, in <module>
move2Goal()
File "/home/ben/data_driven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/nodes/trial_mtoa2.py", line 162, in move2Goal
File "/home/ben/data_driven_mpc_ws/src/data_driven_mpc/ros_gp_mpc/src/quad_mpc/create_ros_gp_mpc.py", line 117, in set_SFC
value = value.astype(float)
__getattr = lambda self, name: _swig_getattr(self, MX, name)
raise AttributeError("'%s' object has no attribute '%s'" % (class_type.__name, name))
AttributeError: 'MX' object has no attribute 'astype'
``````

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)`