Adding different constraint structure to allow for constraint tightening

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

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