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
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
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
os.chdir(os.path.dirname(os.path.realpath(__file__)))
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
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
json_file = os.path.join(self.acados_models_dir, key_model.name + '_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]
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
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:
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
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/create_ros_gp_mpc.py", 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/quad_3d_mpc.py", 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/quad_3d_optimizer.py", 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/acados_ocp_solver.py", line 1203, in set
value = value.astype(float)
File "/home/ben/virenv/gp_mpc/lib/python3.8/site-packages/casadi/casadi.py", 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/casadi.py", 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