From 68fd07829daa8e7ab6db56df57759a4b4e0ea89a Mon Sep 17 00:00:00 2001
From: Federico-PizarroBejarano <federico.pizarrobejarano@mail.utoronto.ca>
Date: Fri, 1 Nov 2024 13:45:32 -0400
Subject: [PATCH] Temp commit

---
 .../mpsc_acados_quadrotor_2D_attitude.yaml    |  32 ++
 .../linear_mpsc_quadrotor_2D_attitude.pkl     | Bin 0 -> 447 bytes
 .../nl_mpsc_quadrotor_2D_attitude.pkl         | Bin 0 -> 1872 bytes
 experiments/mpsc/mpsc_experiment.py           |   6 +-
 experiments/mpsc/mpsc_experiment.sh           |   4 +-
 experiments/mpsc/train_model.sh               |   4 +-
 .../envs/gym_pybullet_drones/base_aviary.py   | 128 ++++----
 .../envs/gym_pybullet_drones/quadrotor.py     |  53 ++--
 safe_control_gym/safety_filters/__init__.py   |   4 +
 safe_control_gym/safety_filters/mpsc/mpsc.py  |   2 +-
 .../safety_filters/mpsc/mpsc_acados.py        | 276 ++++++++++++++++++
 .../safety_filters/mpsc/mpsc_utils.py         |  42 +++
 .../safety_filters/mpsc/nl_mpsc.py            |  52 +---
 13 files changed, 457 insertions(+), 146 deletions(-)
 create mode 100644 experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml
 create mode 100644 experiments/mpsc/models/mpsc_parameters/linear_mpsc_quadrotor_2D_attitude.pkl
 create mode 100644 experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_2D_attitude.pkl
 create mode 100644 safe_control_gym/safety_filters/mpsc/mpsc_acados.py

diff --git a/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml b/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml
new file mode 100644
index 000000000..1152caa0c
--- /dev/null
+++ b/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml
@@ -0,0 +1,32 @@
+safety_filter: nl_mpsc
+sf_config:
+  # LQR controller parameters
+  q_mpc: [18, 0.1, 18, 0.5, 0.5, 0.0001]
+  r_mpc: [3., 3.]
+
+  prior_info:
+    prior_prop:
+      beta_1: 18.11298
+      beta_2: 3.6800
+      beta_3: 0
+      alpha_1: -140.8
+      alpha_2: -13.4
+      alpha_3: 124.8
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+
+  # MPC Parameters
+  use_acados: True
+  horizon: 25
+  warmstart: True
+  integration_algo: rk4
+  use_terminal_set: False
+
+  # Cost function
+  cost_function: one_step_cost
+  mpsc_cost_horizon: 5
+  decay_factor: 0.85
+
+  # Softening
+  soften_constraints: True
+  slack_cost: 0.1
diff --git a/experiments/mpsc/models/mpsc_parameters/linear_mpsc_quadrotor_2D_attitude.pkl b/experiments/mpsc/models/mpsc_parameters/linear_mpsc_quadrotor_2D_attitude.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..cb616ada5af767f92ed16874e7e4648107d9625a
GIT binary patch
literal 447
zcmZo*nYx9M0Ss!V^e_fY=@HE<%`K?ZOU^G!)ypl-DalMMDoU)J(!&#9l$xBMS6ot5
znp`qv@{}G{u)--l?0G2=C6lLkGqg@|W=xvWJ|$?1hBu=(n>SnAlnmA$)|8UUf>e+u
zrZkHwogEN)<{n0yDSm!_UjKmrOn5VtOiAi=QUJPWYTDW83g4Fw&m1!y&sBbMQ2lYl
zLtw%N2i+1sW`@+Y4nd;qZ42}T9q;Aq?@dZUsIxv}a%xigZwG~h!@YG+wm7so*DUIn
z+3KLTs@nMRT1iL#z}o>E{v*tTsattvhT)a&9}WWAA+-Yj-yII_XWPcT^rFMz9ek&z
zY*+=i7iJzz-PNL~d-GR)cQ~Um&+pv|p!&ZCGd}sPhPw}DFU&lcy7W1E(~}NcIA*?>
i{FKX12<{J<`(XCM%!8>D@0a@9`sj_r6ktdd>j3~B?#PS)

literal 0
HcmV?d00001

diff --git a/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_2D_attitude.pkl b/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_2D_attitude.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..0c5896aa8894ab242cf875e27343ce92643ab441
GIT binary patch
literal 1872
zcmZuxYfuwc7$t@f2(bp0K|nOYR?(>pjzw_XunQ5<1i@B8oFZumk$}WtNCXrrDu!23
zM-~S~#l{v8%V;6iv1DB|g7{z@<RJ=EK+!4)K^|73lrG6$I!@Ui`^~-Ix#yhw-S6HE
z>%u^~mHA6YJv20hR4x>wL2yHXQXl`LNkiKvl_*8%P9o>HM<q*4^%Tn$5>J^bK^Z4f
zC`75K#wJNDN)Rc~0#rjYLx>ooOih$b&CJ^6i{@l!q2Hk%0;+}rp&=n5{7K?7{{&V_
zG@55?(eK7lHGvaYNo}Oe=}Fi;DbY|OP^q<sDO5<r@;ymPg-Wa>_@#6WeNT+VNCC0*
zD9WQmqZT=ez(!!BMy1T005-+S%9v8V=xF~NxUG>>mVvh5`A_Fwpfc!~m-p|%*IJiy
z*_7%V&hEAN$H+5`_QOrnxXc41gk1{q!uP<{JJnv??Dm8*aeg3Hr+7PgpZ6Hod9h2I
zTF0mP04`VEK<M3r*F>(%AF*%3xl)IvQd<_b;abnr;jJ_-&;@(|R~b8Z$^QM5a9nBg
z`?QuN-0KFb`9IltPx}M9fDhohK9mwZAHnixMfKv7?bG`J{(vsv12_XZPORe~C~*`T
ziU5^5Nu8$(DRl*9oJE$A_rt)d#F*M32kyZVhG8trq<34GUo58niFYdteZp$$@I+~t
zx?bnV<)%97IJ9EDecbL(H5aRJr|s_;lE5l_`}qi=slyTLtC-WJhP6;v<33%V;1zsU
zyLZJ`!fM#!Vap62K0C^|PXAG1tO>r9#B&_Y>xR{VclWkOeT1~#;Vh=B8}`9;>_$V%
zIJ~-u)l|jT!>37{x8~cPrpx{C!K96MpjQYMuFZQ?RW$--TT8ev>Em#~VE@<g;`{L7
zD-n(BAH0WhD-6CnM-c3Yv3xMfb^+paY3&vzy$>E!87f*HEaRSaP544x=#J%`ydPv-
z=7zndIz+$zdjxVOadvNJ4@}&90ngQzO{_L}VIC7{kAxd0^#i28Tv8X>cixAw$b%cx
z+q*F8nHLr{d&9o;KirUSvXXP1&>q;6+{b#--?>!=zW-TwEM{-eHrwP$y*G)gbREB2
zsjjn>(Jn@kpcDxc<YLrvp&qvRUTpmsX}M4uDsj`vMAx>#w&s^cGC@xgMvD}}U1XAX
zXjs#k{R^3-B~K;ER1G$y!?l2rCKIIylgU(DRk5}6Ks33^S}crDn&D7cM3*shcp0~R
z-V-fogh4Fo8a0nWOthjAPs9)m;>zmVp8U51Fo?>uW-}f{k-%gggP7FaXC8w{UE{lY
zYjQ6PqPgv8u_BpFMEYkwS?Z%G8w9o5GF6WM-HXq9@(d>!7jFtj(hG1_2j-8Id*?rI
z!~L<^x))={)J~|<mc3Y68Hy}_Twgxa{1OJZ1tmM@Ech}AiRY?*Exhf7U3GaA{?EGS
z`t_S~RP}4OBL#v@tj8`xFyI5Y-NOyDulIx^!M$}JgAW;q@km(K=H>IT(v;TQ_Qy^`
zKo{@<TxQX!P$q|k3~8(Toxg9@%d$Fz>G@Zu{Q+IT2XNGk*h4o$R7lJ5+O)-d#`Hdb
zKcEZv08W0-T2kCMZ0Sj=V8%lJ_rS2BiCdSCoYKGhv|H{TZ-7iO3mw8j`|*x*YaO1^
z25>n>V{``BL4Iueii(hpdfm^3HjA%x;D=bZ0)|iY;u(CG5|_MOXoqa`hnm#mdVi-i
zQ7w&Q_)4hKj=$^)Za!&-OitVlVRW2mrlz#9G^JO-Uul!eJxgz<=`Pr<$mHN#`*zq<
lo}Txcm-*YBdE!C8QBGt)Nmb)b_T?)PVM~mDQ`9QYe*sPzZR`L5

literal 0
HcmV?d00001

diff --git a/experiments/mpsc/mpsc_experiment.py b/experiments/mpsc/mpsc_experiment.py
index ff9d884ae..6edddf462 100644
--- a/experiments/mpsc/mpsc_experiment.py
+++ b/experiments/mpsc/mpsc_experiment.py
@@ -141,7 +141,7 @@ def run_multiple_models(plot, all_models):
 
     for model in all_models:
         print(model)
-        for i in range(25):
+        for i in range(1):
             X_GOAL, uncert_results, _, cert_results, _ = run(plot=plot, training=False, model=model)
             if i == 0:
                 all_uncert_results, all_cert_results = uncert_results, cert_results
@@ -168,5 +168,5 @@ def run_multiple_models(plot, all_models):
 
 
 if __name__ == '__main__':
-    # run(training=False)
-    run_multiple_models(plot=False, all_models=['True', 'False'])
+    # run(plot=True, training=False, model='ppo')
+    run_multiple_models(plot=True, all_models=['none', 'mpsf'])
diff --git a/experiments/mpsc/mpsc_experiment.sh b/experiments/mpsc/mpsc_experiment.sh
index eacad2068..678584020 100755
--- a/experiments/mpsc/mpsc_experiment.sh
+++ b/experiments/mpsc/mpsc_experiment.sh
@@ -5,7 +5,9 @@ TASK='tracking'
 ALGO='ppo'
 
 SAFETY_FILTER='nl_mpsc'
-MPSC_COST='precomputed_cost'
+# SAFETY_FILTER='mpsc_acados'
+# MPSC_COST='precomputed_cost'
+MPSC_COST='one_step_cost'
 MPSC_COST_HORIZON=25
 DECAY_FACTOR=0.9
 
diff --git a/experiments/mpsc/train_model.sh b/experiments/mpsc/train_model.sh
index 9febae2ef..7ea77edfe 100755
--- a/experiments/mpsc/train_model.sh
+++ b/experiments/mpsc/train_model.sh
@@ -10,9 +10,9 @@ FILTER=False
 SF_PEN=1
 
 if [ "$FILTER" == 'True' ]; then
-    TAG=mpsf
+    TAG=mpsf4
 else
-    TAG=none
+    TAG=none4
 fi
 
 # Train the unsafe controller/agent.
diff --git a/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py b/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py
index 1d6af8781..802e8b4b0 100644
--- a/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py
+++ b/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py
@@ -14,11 +14,10 @@
 from datetime import datetime
 from enum import Enum
 
+import casadi as cs
 import numpy as np
 import pybullet as p
 import pybullet_data
-import casadi as cs
-from termcolor import colored
 
 from safe_control_gym.envs.benchmark_env import BenchmarkEnv
 from safe_control_gym.math_and_models.transformations import csRotXYZ, get_angularvelocity_rpy
@@ -94,26 +93,26 @@ def __init__(self,
         self.RECORD = record
         # Load the drone properties from the .urdf file.
         self.MASS, \
-        self.L, \
-        self.THRUST2WEIGHT_RATIO, \
-        self.J, \
-        self.J_INV, \
-        self.KF, \
-        self.KM, \
-        self.COLLISION_H, \
-        self.COLLISION_R, \
-        self.COLLISION_Z_OFFSET, \
-        self.MAX_SPEED_KMH, \
-        self.GND_EFF_COEFF, \
-        self.PROP_RADIUS, \
-        self.DRAG_COEFF, \
-        self.DW_COEFF_1, \
-        self.DW_COEFF_2, \
-        self.DW_COEFF_3, \
-        self.PWM2RPM_SCALE, \
-        self.PWM2RPM_CONST, \
-        self.MIN_PWM, \
-        self.MAX_PWM = self._parse_urdf_parameters(self.URDF_PATH)
+            self.L, \
+            self.THRUST2WEIGHT_RATIO, \
+            self.J, \
+            self.J_INV, \
+            self.KF, \
+            self.KM, \
+            self.COLLISION_H, \
+            self.COLLISION_R, \
+            self.COLLISION_Z_OFFSET, \
+            self.MAX_SPEED_KMH, \
+            self.GND_EFF_COEFF, \
+            self.PROP_RADIUS, \
+            self.DRAG_COEFF, \
+            self.DW_COEFF_1, \
+            self.DW_COEFF_2, \
+            self.DW_COEFF_3, \
+            self.PWM2RPM_SCALE, \
+            self.PWM2RPM_CONST, \
+            self.MIN_PWM, \
+            self.MAX_PWM = self._parse_urdf_parameters(self.URDF_PATH)
         self.GROUND_PLANE_Z = -0.05
         if verbose:
             print(
@@ -270,8 +269,6 @@ def _advance_simulation(self, clipped_action, disturbance_force=None):
                                          `_preprocess_action()` in each subclass.
             disturbance_force (ndarray, optional): Disturbance force, applied to all drones.
         '''
-        time_before_stepping = time.time()
-        # clipped_action = np.reshape(clipped_action, (self.NUM_DRONES, 4))
         clipped_action = np.expand_dims(clipped_action, axis=0)
 
         # Repeat for as many as the aggregate physics steps.
@@ -280,20 +277,20 @@ def _advance_simulation(self, clipped_action, disturbance_force=None):
             # Between aggregate steps for certain types of update.
             if self.PYB_STEPS_PER_CTRL > 1 and self.PHYSICS in [
                 Physics.DYN, Physics.PYB_GND, Physics.PYB_DRAG,
-                Physics.PYB_DW, Physics.PYB_GND_DRAG_DW, Physics.RK4 #, Physics.DYN_2D
+                Physics.PYB_DW, Physics.PYB_GND_DRAG_DW, Physics.RK4  # , Physics.DYN_2D
             ]:
                 self._update_and_store_kinematic_information()
             # Step the simulation using the desired physics update.
             for i in range(self.NUM_DRONES):
-                rpm = self._preprocess_control(clipped_action[i, :])
+                executable_action = self._preprocess_control(clipped_action[i, :])
                 if self.PHYSICS == Physics.PYB:
-                    self._physics(rpm, i)
+                    self._physics(executable_action, i)
                 elif self.PHYSICS == Physics.DYN:
                     self._dynamics(clipped_action[i, :], i)
                 elif self.PHYSICS == Physics.DYN_2D:
-                    self._dynamics_2d(rpm, i)
+                    self._dynamics_2d(executable_action, i)
                 elif self.PHYSICS == Physics.DYN_SI:
-                    self._dynamics_si(clipped_action[i, :], i, disturbance_force)
+                    self._dynamics_si(executable_action, i, disturbance_force)
                 elif self.PHYSICS == Physics.RK4:
                     self._dynamics_rk4(clipped_action[i, :], i)
                 elif self.PHYSICS == Physics.DYN_SI_3D:
@@ -316,9 +313,9 @@ def _advance_simulation(self, clipped_action, disturbance_force=None):
                 if disturbance_force is not None:
                     pos = self._get_drone_state_vector(i)[:3]
                     '''
-                    NOTE: applyExternalForce only works when explicitly 
+                    NOTE: applyExternalForce only works when explicitly
                     stepping the simulation with p.stepSimulation().
-                    Therefore, 
+                    Therefore,
                     '''
                     p.applyExternalForce(
                         self.DRONE_IDS[i],
@@ -484,7 +481,7 @@ def _ground_effect(self, rpm, nth_drone):
         ])
         prop_heights = np.clip(prop_heights, self.GND_EFF_H_CLIP, np.inf)
         gnd_effects = np.array(rpm ** 2) * self.KF * self.GND_EFF_COEFF \
-                      * (self.PROP_RADIUS / (4 * prop_heights)) ** 2
+            * (self.PROP_RADIUS / (4 * prop_heights)) ** 2
         if np.abs(self.rpy[nth_drone, 0]) < np.pi / 2 and np.abs(
                 self.rpy[nth_drone, 1]) < np.pi / 2:
             for i in range(4):
@@ -642,7 +639,6 @@ def _dynamics_rk4(self, rpm, nth_drone):
         self.rpy_rates[nth_drone, :] = rpy_rates
 
     def setup_rk4_dynamics_expression(self):
-        nx, nu = 12, 4
         gamma = self.KM / self.KF
         z = cs.MX.sym('z')
         z_dot = cs.MX.sym('z_dot')
@@ -659,7 +655,7 @@ def setup_rk4_dynamics_expression(self):
         # PyBullet Euler angles use the SDFormat for rotation matrices.
         Rob = csRotXYZ(phi, theta, psi)  # rotation matrix transforming a vector in the body frame to the world frame.
 
-        # Define state variables.   
+        # Define state variables.
         X = cs.vertcat(x, x_dot, y, y_dot, z, z_dot, phi, theta, psi, p_body, q_body, r_body)
 
         # Define inputs.
@@ -682,14 +678,14 @@ def setup_rk4_dynamics_expression(self):
                         self.L / cs.sqrt(2.0) * (-f1 + f2 + f3 - f4),
                         gamma * (-f1 + f2 - f3 + f4))
         rate_dot = self.J_INV @ (
-                    Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body)))
+            Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body)))
         ang_dot = cs.blockcat([[1, cs.sin(phi) * cs.tan(theta), cs.cos(phi) * cs.tan(theta)],
                                [0, cs.cos(phi), -cs.sin(phi)],
                                [0, cs.sin(phi) / cs.cos(theta), cs.cos(phi) / cs.cos(theta)]]) @ cs.vertcat(p_body,
                                                                                                             q_body,
                                                                                                             r_body)
         X_dot = cs.vertcat(pos_dot[0], pos_ddot[0], pos_dot[1], pos_ddot[1], pos_dot[2], pos_ddot[2], ang_dot, rate_dot)
-        self.X_dot_fun = cs.Function("X_dot", [X, U], [X_dot])
+        self.X_dot_fun = cs.Function('X_dot', [X, U], [X_dot])
         self.fd_func = cs.integrator('fd', 'rk', {'x': X,
                                                   'p': U,
                                                   'ode': X_dot}, {'tf': self.PYB_TIMESTEP})
@@ -708,7 +704,6 @@ def _dynamics_2d(self, rpm, nth_drone):
         rpy = self.rpy[nth_drone, :]
         vel = self.vel[nth_drone, :]
         ang_v = self.ang_v[nth_drone, :]
-        rpy_rates = self.rpy_rates[nth_drone, :]
         # rotation = np.array(p.getMatrixFromQuaternion(quat)).reshape(3, 3)
 
         # Compute forces and torques.
@@ -721,7 +716,7 @@ def _dynamics_2d(self, rpm, nth_drone):
         # update state with RK4
         # next_state = self.fd_func(x0=state, p=input)['xf'].full()[:, 0]
         X_dot = self.X_dot_fun(state, action).full()[:, 0]
-        next_state = state + X_dot*self.PYB_TIMESTEP
+        next_state = state + X_dot * self.PYB_TIMESTEP
 
         # Updated information
         pos = np.array([next_state[0], 0, next_state[4]])
@@ -787,13 +782,13 @@ def setup_dynamics_2d_expression(self):
                         self.L / cs.sqrt(2.0) * (-f1 + f2 + f3 - f4),
                         gamma * (-f1 + f2 - f3 + f4))
         rate_dot = self.J_INV @ (
-                Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body)))
+            Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body)))
         ang_dot = (cs.blockcat([[1, cs.sin(phi) * cs.tan(theta), cs.cos(phi) * cs.tan(theta)],
                                [0, cs.cos(phi), -cs.sin(phi)],
                                [0, cs.sin(phi) / cs.cos(theta), cs.cos(phi) / cs.cos(theta)]]) @
                    cs.vertcat(p_body, q_body, r_body))
         X_dot = cs.vertcat(pos_dot[0], pos_ddot[0], pos_dot[1], pos_ddot[1], pos_dot[2], pos_ddot[2], ang_dot, rate_dot)
-        self.X_dot_fun = cs.Function("X_dot", [X, U], [X_dot])
+        self.X_dot_fun = cs.Function('X_dot', [X, U], [X_dot])
 
     def _dynamics_si(self, action, nth_drone, disturbance_force=None):
         '''Explicit dynamics implementation from the identified model.
@@ -810,7 +805,6 @@ def _dynamics_si(self, action, nth_drone, disturbance_force=None):
         # quat = self.quat[nth_drone, :]
         rpy = self.rpy[nth_drone, :]
         vel = self.vel[nth_drone, :]
-        ang_v = self.ang_v[nth_drone, :]
         rpy_rates = self.rpy_rates[nth_drone, :]
 
         # Compute forces and torques.
@@ -820,7 +814,7 @@ def _dynamics_si(self, action, nth_drone, disturbance_force=None):
         # update state
         if disturbance_force is not None:
             d = np.array([disturbance_force[0], disturbance_force[2]])
-        else: 
+        else:
             d = np.array([0, 0])
         # perform euler integration
         # next_state = state + self.PYB_TIMESTEP * self.X_dot_fun(state, action, d).full()[:, 0]
@@ -829,7 +823,7 @@ def _dynamics_si(self, action, nth_drone, disturbance_force=None):
         k2 = self.X_dot_fun(state + 0.5 * self.PYB_TIMESTEP * k1, action, d).full()[:, 0]
         k3 = self.X_dot_fun(state + 0.5 * self.PYB_TIMESTEP * k2, action, d).full()[:, 0]
         k4 = self.X_dot_fun(state + self.PYB_TIMESTEP * k3, action, d).full()[:, 0]
-        next_state = state + (self.PYB_TIMESTEP / 6) * (k1 + 2*k2 + 2*k3 + k4)
+        next_state = state + (self.PYB_TIMESTEP / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
 
         # Updated information
         pos = np.array([next_state[0], 0, next_state[2]])
@@ -853,19 +847,19 @@ def setup_dynamics_si_expression(self):
         theta_dot = cs.MX.sym('theta_dot')  # Pitch
         X = cs.vertcat(x, x_dot, z, z_dot, theta, theta_dot)
         g = self.GRAVITY_ACC
-        d = cs.MX.sym('d', 2, 1) # disturbance force
+        d = cs.MX.sym('d', 2, 1)  # disturbance force
 
         # Define inputs.
-        T = cs.MX.sym('T') # normlized thrust [N]
+        T = cs.MX.sym('T')  # normlized thrust [N]
         P = cs.MX.sym('P')  # desired pitch angle [rad]
         U = cs.vertcat(T, P)
         X_dot = cs.vertcat(x_dot,
-                            (18.112984649321753 * T+ 3.6800) * cs.sin(theta) + -0.008 + d[0] / self.MASS,
-                            z_dot,
-                            (18.112984649321753 * T + 3.6800) * cs.cos(theta) - g + d[1] / self.MASS,
-                            theta_dot,
-                            -140.8 * theta - 13.4 * theta_dot + 124.8 * P)
-        self.X_dot_fun = cs.Function("X_dot", [X, U, d], [X_dot])
+                           (18.112984649321753 * T + 3.6800) * cs.sin(theta) + -0.008 + d[0] / self.MASS,
+                           z_dot,
+                           (18.112984649321753 * T + 3.6800) * cs.cos(theta) - g + d[1] / self.MASS,
+                           theta_dot,
+                           -140.8 * theta - 13.4 * theta_dot + 124.8 * P)
+        self.X_dot_fun = cs.Function('X_dot', [X, U, d], [X_dot])
 
     def _dynamics_si_3d(self, action, nth_drone, disturbance_force=None):
         '''Explicit dynamics implementation from the identified model.
@@ -882,7 +876,6 @@ def _dynamics_si_3d(self, action, nth_drone, disturbance_force=None):
         rpy = self.rpy[nth_drone, :]
         vel = self.vel[nth_drone, :]
         ang_v = self.ang_v[nth_drone, :]
-        rpy_rates = self.rpy_rates[nth_drone, :]
 
         # Compute forces and torques.
         # Update state with discrete time dynamics.
@@ -961,21 +954,20 @@ def setup_dynamics_si_3d_expression(self):
         #                    - 61.62863740616216 * theta - 7.205874472066235 * theta_dot + 51.90335491067372 * P,
         #                    (- 12.544174350349687 * psi - 0.012945379372787613 * psi_dot + 43.839961280232046 * Y)*0)
 
-
         X_dot = cs.vertcat(x_dot,
-                            (18.112984649321753 * T + 3.6800) * (cs.cos(phi) * cs.sin(theta) * cs.cos(psi) + cs.sin(phi) * cs.sin(psi)) + d[0] / self.MASS,
-                            y_dot,
-                            0.0,
-                            z_dot,
-                            (18.112984649321753 * T + 3.6800) * cs.cos(phi) * cs.cos(theta) - g + d[1] / self.MASS,
-                            phi_dot,
-                            theta_dot,
-                            psi_dot,
-                            -140.8 * phi - 13.4 * phi_dot + 124.8 * R,
-                            -140.8 * theta - 13.4 * theta_dot + 124.8 * P,
-                            0.0)
-
-        self.X_dot_fun = cs.Function("X_dot", [X, U, d], [X_dot])
+                           (18.112984649321753 * T + 3.6800) * (cs.cos(phi) * cs.sin(theta) * cs.cos(psi) + cs.sin(phi) * cs.sin(psi)) + d[0] / self.MASS,
+                           y_dot,
+                           0.0,
+                           z_dot,
+                           (18.112984649321753 * T + 3.6800) * cs.cos(phi) * cs.cos(theta) - g + d[1] / self.MASS,
+                           phi_dot,
+                           theta_dot,
+                           psi_dot,
+                           -140.8 * phi - 13.4 * phi_dot + 124.8 * R,
+                           -140.8 * theta - 13.4 * theta_dot + 124.8 * P,
+                           0.0)
+
+        self.X_dot_fun = cs.Function('X_dot', [X, U, d], [X_dot])
 
     def _show_drone_local_axes(self, nth_drone):
         '''Draws the local frame of the n-th drone in PyBullet's GUI.
@@ -1047,5 +1039,5 @@ def _parse_urdf_parameters(self, file_name):
         MIN_PWM = float(URDF_TREE[0].attrib['pwm_min'])
         MAX_PWM = float(URDF_TREE[0].attrib['pwm_max'])
         return M, L, THRUST2WEIGHT_RATIO, J, J_INV, KF, KM, COLLISION_H, COLLISION_R, COLLISION_Z_OFFSET, MAX_SPEED_KMH, \
-               GND_EFF_COEFF, PROP_RADIUS, DRAG_COEFF, DW_COEFF_1, DW_COEFF_2, DW_COEFF_3, \
-               PWM2RPM_SCALE, PWM2RPM_CONST, MIN_PWM, MAX_PWM
\ No newline at end of file
+            GND_EFF_COEFF, PROP_RADIUS, DRAG_COEFF, DW_COEFF_1, DW_COEFF_2, DW_COEFF_3, \
+            PWM2RPM_SCALE, PWM2RPM_CONST, MIN_PWM, MAX_PWM
diff --git a/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py b/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py
index 7c2ba4283..f7cfcb0a9 100644
--- a/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py
+++ b/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py
@@ -869,6 +869,8 @@ def _set_action_space(self):
                 self.hover_thrust = self.GRAVITY_ACC * self.MASS
             else:
                 self.hover_thrust = self.GRAVITY_ACC * self.MASS / action_dim
+            self.action_scale = (self.physical_action_bounds[1] - self.physical_action_bounds[0]) / 2
+            self.action_bias = (self.physical_action_bounds[1] + self.physical_action_bounds[0]) / 2
             self.action_space = spaces.Box(low=-np.ones(action_dim),
                                            high=np.ones(action_dim),
                                            dtype=np.float32)
@@ -998,10 +1000,10 @@ def _preprocess_control(self, action):
         # Identified dynamics model works with collective thrust and pitch directly
         # No need to compute RPMs, (save compute)
         self.current_clipped_action = np.clip(self.current_noisy_physical_action,
-                                              self.physical_action_bounds[0],
-                                              self.physical_action_bounds[1])
+                                              self.action_space.low,
+                                              self.action_space.high)
         if self.PHYSICS == Physics.DYN_SI or self.PHYSICS == Physics.DYN_SI_3D:
-            return None
+            return self.current_clipped_action
 
         if self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE or self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S:
             collective_thrust, pitch = self.current_clipped_action
@@ -1036,10 +1038,11 @@ def normalize_action(self, action):
             normalized_action (ndarray): The action in the correct action space.
         """
         if self.NORMALIZED_RL_ACTION_SPACE:
-            if self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE or self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S:
-                action = np.array([(action[0] / self.hover_thrust - 1) / self.norm_act_scale, action[1]])
-            else:
-                action = (action / self.hover_thrust - 1) / self.norm_act_scale
+            # if self.QUAD_TYPE in [QuadType.TWO_D_ATTITUDE, QuadType.TWO_D_ATTITUDE_5S, QuadType.TWO_D_ATTITUDE_BODY]:
+            #     action = np.array([(action[0] / self.hover_thrust - 1) / self.norm_act_scale, action[1]])
+            # else:
+            #     action = (action / self.hover_thrust - 1) / self.norm_act_scale
+            action = (action - self.action_bias) / self.action_scale
 
         return action
 
@@ -1054,20 +1057,22 @@ def denormalize_action(self, action):
         """
 
         if self.NORMALIZED_RL_ACTION_SPACE:
-            if self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE or self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S:
-                # # divided by 4 as action[0] is a collective thrust
-                # thrust = action[0] / 4
-                # hover_pwm = (self.HOVER_RPM - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE
-                # thrust = np.where(thrust <= 0, self.MIN_PWM + (thrust + 1) * (hover_pwm - self.MIN_PWM),
-                #                    hover_pwm + (self.MAX_PWM - hover_pwm) * thrust)
-
-                thrust = (1 + self.norm_act_scale * action[0]) * self.hover_thrust
-                # thrust = self.attitude_control.thrust2pwm(thrust)
-                # thrust = self.HOVER_RPM * (1+0.05*action[0])
-
-                action = np.array([thrust, action[1]])
-            else:
-                action = (1 + self.norm_act_scale * action) * self.hover_thrust
+            # if self.QUAD_TYPE in [QuadType.TWO_D_ATTITUDE, QuadType.TWO_D_ATTITUDE_5S, QuadType.TWO_D_ATTITUDE_BODY]:
+            #     # # divided by 4 as action[0] is a collective thrust
+            #     # thrust = action[0] / 4
+            #     # hover_pwm = (self.HOVER_RPM - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE
+            #     # thrust = np.where(thrust <= 0, self.MIN_PWM + (thrust + 1) * (hover_pwm - self.MIN_PWM),
+            #     #                    hover_pwm + (self.MAX_PWM - hover_pwm) * thrust)
+            #
+            #     thrust = (1 + self.norm_act_scale * action[0]) * self.hover_thrust
+            #     # thrust = self.attitude_control.thrust2pwm(thrust)
+            #     # thrust = self.HOVER_RPM * (1+0.05*action[0])
+            #
+            #     action = np.array([thrust, action[1]])
+            # else:
+            #     action = (1 + self.norm_act_scale * action) * self.hover_thrust
+            action = action * self.action_scale + self.action_bias
+            # action = np.clip(action, self.action_space.low, self.action_space.high)
 
         return action
 
@@ -1245,9 +1250,9 @@ def _get_info(self):
         # Filter only relevant dimensions.
         state_error = state_error * self.info_mse_metric_state_weight
         info['mse'] = np.sum(state_error ** 2)
-        # if self.constraints is not None:
-        #     info['constraint_values'] = self.constraints.get_values(self)
-        #     info['constraint_violations'] = self.constraints.get_violations(self)
+        if self.constraints is not None:
+            info['constraint_values'] = self.constraints.get_values(self)
+            info['constraint_violations'] = self.constraints.get_violations(self)
         return info
 
     def _get_reset_info(self):
diff --git a/safe_control_gym/safety_filters/__init__.py b/safe_control_gym/safety_filters/__init__.py
index a283ed008..b82a4b201 100644
--- a/safe_control_gym/safety_filters/__init__.py
+++ b/safe_control_gym/safety_filters/__init__.py
@@ -10,6 +10,10 @@
          entry_point='safe_control_gym.safety_filters.mpsc.nl_mpsc:NL_MPSC',
          config_entry_point='safe_control_gym.safety_filters.mpsc:mpsc.yaml')
 
+register(idx='mpsc_acados',
+         entry_point='safe_control_gym.safety_filters.mpsc.mpsc_acados:MPSC_ACADOS',
+         config_entry_point='safe_control_gym.safety_filters.mpsc:mpsc.yaml')
+
 register(idx='cbf',
          entry_point='safe_control_gym.safety_filters.cbf.cbf:CBF',
          config_entry_point='safe_control_gym.safety_filters.cbf:cbf.yaml')
diff --git a/safe_control_gym/safety_filters/mpsc/mpsc.py b/safe_control_gym/safety_filters/mpsc/mpsc.py
index e9a8dd258..251c58985 100644
--- a/safe_control_gym/safety_filters/mpsc/mpsc.py
+++ b/safe_control_gym/safety_filters/mpsc/mpsc.py
@@ -248,7 +248,7 @@ def solve_acados_optimization(self,
             x_val = np.zeros((self.horizon + 1, self.model.nx))
             u_val = np.zeros((self.horizon, self.model.nu))
             for i in range(self.horizon):
-                self.slack_prev[i, :] = ocp_solver.get(i, 'su')
+                # self.slack_prev[i, :] = ocp_solver.get(i, 'su')
                 x_val[i, :] = ocp_solver.get(i, 'x')
                 u_val[i, :] = ocp_solver.get(i, 'u')
             x_val[self.horizon, :] = ocp_solver.get(self.horizon, 'x')
diff --git a/safe_control_gym/safety_filters/mpsc/mpsc_acados.py b/safe_control_gym/safety_filters/mpsc/mpsc_acados.py
new file mode 100644
index 000000000..44692a32e
--- /dev/null
+++ b/safe_control_gym/safety_filters/mpsc/mpsc_acados.py
@@ -0,0 +1,276 @@
+'''Model Predictive Safety Certification using Acados.'''
+import os
+import shutil
+from datetime import datetime
+
+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, box2polytopic
+from safe_control_gym.utils.utils import timing
+
+
+class MPSC_ACADOS(MPSC):
+    '''MPSC with full nonlinear model.'''
+
+    def __init__(
+            self,
+            env_func,
+            horizon: int = 5,
+            q_mpc: list = [1],
+            r_mpc: list = [1],
+            integration_algo: str = 'rk4',
+            warmstart: bool = True,
+            additional_constraints: list = None,
+            use_terminal_set: bool = True,
+            soften_constraints: bool = False,
+            slack_cost: float = 1,
+            constraint_tol: float = 1e-6,
+            seed: int = 0,
+            use_RTI: bool = False,
+            cost_function: Cost_Function = Cost_Function.ONE_STEP_COST,
+            mpsc_cost_horizon: int = 5,
+            decay_factor: float = 0.85,
+            **kwargs
+    ):
+        '''Creates task and controller.
+
+        Args:
+            env_func (Callable): function to instantiate task/environment.
+            horizon (int): mpc planning horizon.
+            q_mpc (list): diagonals of state cost weight.
+            r_mpc (list): diagonals of input/action cost weight.
+            warmstart (bool): if to initialize from previous iteration.
+            soften_constraints (bool): Formulate the constraints as soft constraints.
+            constraint_tol (float): Tolerance to add the the constraint as sometimes solvers are not exact.
+            seed (int): random seed.
+            use_RTI (bool): Real-time iteration for acados.
+        '''
+        for k, v in locals().items():
+            if k != 'self' and k != 'kwargs' and '__' not in k:
+                self.__dict__.update({k: v})
+
+        super().__init__(
+            env_func,
+            horizon,
+            q_mpc,
+            r_mpc,
+            integration_algo,
+            warmstart,
+            additional_constraints,
+            use_terminal_set,
+            cost_function,
+            mpsc_cost_horizon,
+            decay_factor,
+            **kwargs)
+
+        # 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] = box2polytopic(self.state_constraint)
+        [self.U_mid, L_u, l_u] = 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()
+
+    @timing
+    def reset(self):
+        '''Prepares for training or evaluation.'''
+        print('Resetting MPC')
+        super().reset()
+        if hasattr(self, 'acados_model'):
+            del self.acados_model
+        if hasattr(self, 'ocp'):
+            del self.ocp
+        if hasattr(self, 'acados_ocp_solver'):
+            del self.acados_ocp_solver
+
+        # delete the generated c code directory
+        if os.path.exists('./c_generated_code'):
+            print('deleting the generated MPC c code directory')
+            shutil.rmtree('./c_generated_code')
+            assert not os.path.exists('./c_generated_code'), 'Failed to delete the generated c code directory'
+
+    def setup_acados_model(self) -> AcadosModel:
+        '''Sets up symbolic model for acados.'''
+        acados_model = AcadosModel()
+        acados_model.x = self.model.x_sym
+        acados_model.u = self.model.u_sym
+        acados_model.name = self.env.NAME
+
+        # set up rk4 (acados need symbolic expression of dynamics, not function)
+        k1 = self.model.fc_func(acados_model.x, acados_model.u)
+        k2 = self.model.fc_func(acados_model.x + self.dt / 2 * k1, acados_model.u)
+        k3 = self.model.fc_func(acados_model.x + self.dt / 2 * k2, acados_model.u)
+        k4 = self.model.fc_func(acados_model.x + self.dt * k3, acados_model.u)
+        f_disc = acados_model.x + self.dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
+
+        acados_model.disc_dyn_expr = f_disc
+
+        # store meta information # NOTE: unit is missing
+        acados_model.x_labels = self.env.STATE_LABELS
+        acados_model.u_labels = self.env.ACTION_LABELS
+        acados_model.t_label = 'time'
+        # get current time stamp in $ymd_HMS format
+        current_time = datetime.now().strftime('%Y%m%d_%H%M%S')
+        acados_model.name = self.env.NAME + '_' + current_time
+
+        self.acados_model = acados_model
+
+    def set_dynamics(self):
+        pass
+
+    def setup_casadi_optimizer(self):
+        pass
+
+    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()
+        ocp.model = self.acados_model
+
+        # set dimensions
+        ocp.dims.N = self.horizon  # prediction horizon
+
+        # 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)
+        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'
+        ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+        ocp.solver_options.integrator_type = 'DISCRETE'
+        ocp.solver_options.nlp_solver_type = 'SQP' if not self.use_RTI else 'SQP_RTI'
+        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
+
+        # set constraints
+        ocp.constraints.constr_type = 'BGH'
+        ocp.constraints.constr_type_e = 'BGH'
+
+        ocp.constraints.x0 = self.model.X_EQ
+        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.Jsg = np.eye(self.p)
+            # slack penalty
+            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)
+
+        for stage in range(self.mpsc_cost_horizon):
+            ocp_solver.cost_set(stage, 'W', (self.cost_function.decay_factor**stage) * ocp.cost.W)
+
+        for stage in range(self.mpsc_cost_horizon, self.horizon):
+            ocp_solver.cost_set(stage, 'W', 0 * ocp.cost.W)
+
+        g = np.zeros((self.horizon, self.p))
+        for i in range(self.horizon):
+            for j in range(self.p):
+                g[i, j] = self.l_xu[j]
+            g[i, :] += (self.L_x @ self.X_mid) + (self.L_u @ self.U_mid)
+            ocp_solver.constraints_set(i, 'ug', g[i, :])
+
+        self.ocp_solver = ocp_solver
+        self.ocp = ocp
+
+    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
+        '''
+
+        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
diff --git a/safe_control_gym/safety_filters/mpsc/mpsc_utils.py b/safe_control_gym/safety_filters/mpsc/mpsc_utils.py
index 0771e0f75..0a66435a4 100644
--- a/safe_control_gym/safety_filters/mpsc/mpsc_utils.py
+++ b/safe_control_gym/safety_filters/mpsc/mpsc_utils.py
@@ -157,3 +157,45 @@ def get_discrete_derivative(signal, ctrl_freq):
     '''
     discrete_derivative = (signal[1:, :] - signal[:-1, :]) * ctrl_freq
     return discrete_derivative
+
+
+def box2polytopic(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)
diff --git a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py
index c9f9e199a..de1f40677 100644
--- a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py
+++ b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py
@@ -25,8 +25,7 @@
 from safe_control_gym.envs.benchmark_env import Environment, Task
 from safe_control_gym.safety_filters.cbf.cbf_utils import cartesian_product
 from safe_control_gym.safety_filters.mpsc.mpsc import MPSC
-from safe_control_gym.safety_filters.mpsc.mpsc_utils import \
-    Cost_Function  # , get_error_parameters, error_function
+from safe_control_gym.safety_filters.mpsc.mpsc_utils import Cost_Function, box2polytopic
 
 
 class NL_MPSC(MPSC):
@@ -79,8 +78,8 @@ def __init__(self,
         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)
+        [self.X_mid, L_x, l_x] = box2polytopic(self.state_constraint)
+        [self.U_mid, L_u, l_u] = box2polytopic(self.input_constraint)
 
         # number of constraints
         p_x = l_x.shape[0]
@@ -368,47 +367,6 @@ def get_terminal_ingredients(self):
         if self.integration_algo == 'LTI' and self.use_terminal_set:
             self.get_terminal_constraint()
 
-    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)
-
     def setup_tube_optimization(self, lamb):
         '''Sets up the optimization to find the lyapunov function.
 
@@ -1022,8 +980,8 @@ def setup_acados_optimizer(self):
             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.zu = 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)
 
         # Options
         ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
@@ -1036,7 +994,7 @@ def setup_acados_optimizer(self):
         ocp.solver_options.tf = self.dt * self.horizon
 
         solver_json = 'acados_ocp_mpsf.json'
-        ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=True, build=True)
+        ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=False, build=False)
 
         for stage in range(self.mpsc_cost_horizon):
             ocp_solver.cost_set(stage, 'W', (self.cost_function.decay_factor**stage) * ocp.cost.W)