From e0ca26a8a6d1f5a9add0361f38c165a6c910a429 Mon Sep 17 00:00:00 2001 From: Federico-PizarroBejarano Date: Fri, 6 Dec 2024 15:48:08 -0500 Subject: [PATCH] Adding different constraint structure to allow for constraint tightening --- .../mpsc_acados_quadrotor_2D_attitude.yaml | 1 + .../safety_filters/mpsc/mpsc_acados.py | 165 +++++++++--------- 2 files changed, 82 insertions(+), 84 deletions(-) diff --git a/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml b/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml index 21146f069..a3cc8b21e 100644 --- a/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml +++ b/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml @@ -21,6 +21,7 @@ sf_config: warmstart: True integration_algo: rk4 use_terminal_set: False + max_w: 0.002 # Cost function cost_function: one_step_cost diff --git a/safe_control_gym/safety_filters/mpsc/mpsc_acados.py b/safe_control_gym/safety_filters/mpsc/mpsc_acados.py index 1846f85ca..ec32fbff5 100644 --- a/safe_control_gym/safety_filters/mpsc/mpsc_acados.py +++ b/safe_control_gym/safety_filters/mpsc/mpsc_acados.py @@ -3,12 +3,10 @@ import shutil from datetime import datetime -import casadi as cs import numpy as np import scipy from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver -from safe_control_gym.controllers.mpc.mpc_utils import set_acados_constraint_bound from safe_control_gym.safety_filters.mpsc.mpsc import MPSC from safe_control_gym.safety_filters.mpsc.mpsc_utils import Cost_Function from safe_control_gym.utils.utils import timing @@ -35,6 +33,7 @@ def __init__( cost_function: Cost_Function = Cost_Function.ONE_STEP_COST, mpsc_cost_horizon: int = 5, decay_factor: float = 0.85, + max_w: float = 0.0, **kwargs ): '''Creates task and controller. @@ -71,11 +70,71 @@ def __init__( # acados settings self.use_RTI = use_RTI + self.n = self.model.nx + self.m = self.model.nu + self.q = self.model.nx + + self.state_constraint = self.constraints.state_constraints[0] + self.input_constraint = self.constraints.input_constraints[0] + + [self.X_mid, L_x, l_x] = self.box2polytopic(self.state_constraint) + [self.U_mid, L_u, l_u] = self.box2polytopic(self.input_constraint) + + # number of constraints + p_x = l_x.shape[0] + p_u = l_u.shape[0] + self.p = p_x + p_u + + self.L_x = np.vstack((L_x, np.zeros((p_u, self.n)))) + self.L_u = np.vstack((np.zeros((p_x, self.m)), L_u)) + self.l_xu = np.concatenate([l_x, l_u]) + # Dynamics model. self.setup_acados_model() # Acados optimizer. self.setup_acados_optimizer() + def box2polytopic(self, constraint): + '''Convert constraints into an explicit polytopic form. This assumes that constraints contain the origin. + + Args: + constraint (Constraint): The constraint to be converted. + + Returns: + L (ndarray): The polytopic matrix. + l (ndarray): Whether the constraint is active. + ''' + + Limit = [] + limit_active = [] + + Z_mid = (constraint.upper_bounds + constraint.lower_bounds) / 2.0 + Z_limits = np.array([[constraint.upper_bounds[i] - Z_mid[i], constraint.lower_bounds[i] - Z_mid[i]] for i in range(constraint.upper_bounds.shape[0])]) + + dim = Z_limits.shape[0] + eye_dim = np.eye(dim) + + for constraint_id in range(0, dim): + if Z_limits[constraint_id, 0] != -float('inf'): + if Z_limits[constraint_id, 0] == 0: + limit_active += [0] + Limit += [-eye_dim[constraint_id, :]] + else: + limit_active += [1] + factor = 1 / Z_limits[constraint_id, 0] + Limit += [factor * eye_dim[constraint_id, :]] + + if Z_limits[constraint_id, 1] != float('inf'): + if Z_limits[constraint_id, 1] == 0: + limit_active += [0] + Limit += [eye_dim[constraint_id, :]] + else: + limit_active += [1] + factor = 1 / Z_limits[constraint_id, 1] + Limit += [factor * eye_dim[constraint_id, :]] + + return Z_mid, np.array(Limit), np.array(limit_active) + @timing def reset(self): '''Prepares for training or evaluation.''' @@ -130,7 +189,6 @@ def setup_acados_optimizer(self): '''Sets up nonlinear optimization problem.''' nx, nu = self.model.nx, self.model.nu ny = nx + nu - ny_e = nx # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -141,21 +199,17 @@ def setup_acados_optimizer(self): # set cost (NOTE: safe-control-gym uses quadratic cost) ocp.cost.cost_type = 'LINEAR_LS' - ocp.cost.cost_type_e = 'LINEAR_LS' Q_mat = np.zeros((nx, nx)) R_mat = np.eye(nu) - ocp.cost.W_e = np.zeros((nx, nx)) ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vu = np.zeros((ny, nu)) ocp.cost.Vu[nx:nx + nu, :] = np.eye(nu) - ocp.cost.Vx_e = np.eye(nx) - # placeholder y_ref and y_ref_e (will be set in select_action) + # placeholder y_ref ocp.cost.yref = np.zeros((ny, )) - ocp.cost.yref_e = np.zeros((ny_e, )) # set up solver options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' @@ -165,33 +219,20 @@ def setup_acados_optimizer(self): ocp.solver_options.nlp_solver_max_iter = 25 if not self.use_RTI else 1 ocp.solver_options.tf = self.horizon * self.dt # prediction horizon + ocp.constraints.constr_type = 'BGH' ocp.constraints.x0 = self.model.X_EQ - - # Constraints - # general constraint expressions - state_constraint_expr_list = [] - input_constraint_expr_list = [] - for state_constraint in self.state_constraints_sym: - state_constraint_expr_list.append(state_constraint(ocp.model.x)) - for input_constraint in self.input_constraints_sym: - input_constraint_expr_list.append(input_constraint(ocp.model.u)) - - h_expr_list = state_constraint_expr_list + input_constraint_expr_list - h_expr = cs.vertcat(*h_expr_list) - h0_expr = cs.vertcat(*h_expr_list) - he_expr = cs.vertcat(*state_constraint_expr_list) # terminal constraints are only state constraints - # pass the constraints to the ocp object - ocp = self.processing_acados_constraints_expression(ocp, h0_expr, h_expr, he_expr) + ocp.constraints.C = self.L_x + ocp.constraints.D = self.L_u + ocp.constraints.lg = -1000 * np.ones((self.p)) + ocp.constraints.ug = np.zeros((self.p)) # slack costs for nonlinear constraints if self.soften_constraints: - # slack variables for all constraints - ocp.constraints.Jsh = np.eye(2 * ny) - # slack penalty - ocp.cost.Zu = self.slack_cost * np.ones(2 * ny) - ocp.cost.Zl = self.slack_cost * np.ones(2 * ny) - ocp.cost.zl = self.slack_cost * np.ones(2 * ny) - ocp.cost.zu = self.slack_cost * np.ones(2 * ny) + ocp.constraints.Jsg = np.eye(self.p) + ocp.cost.Zu = self.slack_cost * np.ones(self.p) + ocp.cost.Zl = self.slack_cost * np.ones(self.p) + ocp.cost.zl = self.slack_cost * np.ones(self.p) + ocp.cost.zu = self.slack_cost * np.ones(self.p) solver_json = 'acados_ocp_mpsf.json' ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=True, build=True) @@ -202,58 +243,14 @@ def setup_acados_optimizer(self): for stage in range(self.mpsc_cost_horizon, self.horizon): ocp_solver.cost_set(stage, 'W', 0 * ocp.cost.W) - self.ocp_solver = ocp_solver - self.ocp = ocp + g = np.zeros((self.horizon, self.p)) - def processing_acados_constraints_expression(self, ocp: AcadosOcp, h0_expr, h_expr, he_expr) -> AcadosOcp: - '''Preprocess the constraints to be compatible with acados. - Args: - ocp (AcadosOcp): acados ocp object - h0_expr (casadi expression): initial state constraints - h_expr (casadi expression): state and input constraints - he_expr (casadi expression): terminal state constraints - Returns: - ocp (AcadosOcp): acados ocp object with constraints set. - - An alternative way to set the constraints is to use bounded constraints of acados: - # bounded input constraints - idxbu = np.where(np.sum(self.env.constraints.input_constraints[0].constraint_filter, axis=0) != 0)[0] - ocp.constraints.Jbu = np.eye(nu) - ocp.constraints.lbu = self.env.constraints.input_constraints[0].lower_bounds - ocp.constraints.ubu = self.env.constraints.input_constraints[0].upper_bounds - ocp.constraints.idxbu = idxbu # active constraints dimension - ''' + for i in range(self.horizon): + for j in range(self.p): + tighten_by = (self.max_w * i) if (j < nx * 2) else 0 + g[i, j] = (self.l_xu[j] - tighten_by) + g[i, :] += (self.L_x @ self.X_mid) + (self.L_u @ self.U_mid) + ocp_solver.constraints_set(i, 'ug', g[i, :]) - ub = {'h': set_acados_constraint_bound(h_expr, 'ub', self.constraint_tol), - 'h0': set_acados_constraint_bound(h0_expr, 'ub', self.constraint_tol), - 'he': set_acados_constraint_bound(he_expr, 'ub', self.constraint_tol), } - - lb = {'h': set_acados_constraint_bound(h_expr, 'lb'), - 'h0': set_acados_constraint_bound(h0_expr, 'lb'), - 'he': set_acados_constraint_bound(he_expr, 'lb'), } - - # make sure all the ub and lb are 1D numpy arrays - # (see: https://discourse.acados.org/t/infeasible-qps-when-using-nonlinear-casadi-constraint-expressions/1595/5?u=mxche) - for key in ub.keys(): - ub[key] = ub[key].flatten() if ub[key].ndim != 1 else ub[key] - lb[key] = lb[key].flatten() if lb[key].ndim != 1 else lb[key] - # check ub and lb dimensions - for key in ub.keys(): - assert ub[key].ndim == 1, f'ub[{key}] is not 1D numpy array' - assert lb[key].ndim == 1, f'lb[{key}] is not 1D numpy array' - assert ub['h'].shape == lb['h'].shape, 'h_ub and h_lb have different shapes' - - # pass the constraints to the ocp object - ocp.model.con_h_expr_0, ocp.model.con_h_expr, ocp.model.con_h_expr_e = \ - h0_expr, h_expr, he_expr - ocp.dims.nh_0, ocp.dims.nh, ocp.dims.nh_e = \ - h0_expr.shape[0], h_expr.shape[0], he_expr.shape[0] - # assign constraints upper and lower bounds - ocp.constraints.uh_0 = ub['h0'] - ocp.constraints.lh_0 = lb['h0'] - ocp.constraints.uh = ub['h'] - ocp.constraints.lh = lb['h'] - ocp.constraints.uh_e = ub['he'] - ocp.constraints.lh_e = lb['he'] - - return ocp + self.ocp_solver = ocp_solver + self.ocp = ocp