Temp commit

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

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'])

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
 

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.

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
+            GND_EFF_COEFF, PROP_RADIUS, DRAG_COEFF, DW_COEFF_1, DW_COEFF_2, DW_COEFF_3, \
+            PWM2RPM_SCALE, PWM2RPM_CONST, MIN_PWM, MAX_PWM