Adding in backup controller to SF when constraints will be violated a…

…nd using real MPSF
Federico-PizarroBejarano · Feb 29, 2024 · 1cc12c3 · 1cc12c3
1 parent b994f5b
commit 1cc12c3
Show file tree

Hide file tree

Showing 19 changed files with 64 additions and 57 deletions.
diff --git a/experiments/crazyflie/all_trajs/none_dm/cert/test0.pkl b/experiments/crazyflie/all_trajs/none_dm/cert/test0.pkl
diff --git a/experiments/crazyflie/all_trajs/none_dm/cert/test1.pkl b/experiments/crazyflie/all_trajs/none_dm/cert/test1.pkl
diff --git a/experiments/crazyflie/all_trajs/none_dm/cert/test2.pkl b/experiments/crazyflie/all_trajs/none_dm/cert/test2.pkl
diff --git a/experiments/crazyflie/all_trajs/none_dm/cert/test3.pkl b/experiments/crazyflie/all_trajs/none_dm/cert/test3.pkl
diff --git a/experiments/crazyflie/all_trajs/none_dm/cert/test4.pkl b/experiments/crazyflie/all_trajs/none_dm/cert/test4.pkl
diff --git a/experiments/crazyflie/crazyflie_experiment.py b/experiments/crazyflie/crazyflie_experiment.py
@@ -263,10 +263,15 @@ def env_reset(env, mpsf):
 def identify_system():
     A,B = linear_regression()
 
-    with open(f'./results_cf/ppo/mpsf_1.pkl', 'rb') as f:
+    print_numpy(A)
+    print_numpy(B)
+    print(A)
+    print(B)
+
+    with open(f'./all_trajs/mpsf_10/cert/test0.pkl', 'rb') as f:
         data = pickle.load(f)
-    states = data['state'][-2][:, [0,1,2,3,6,7]]
-    actions = data['certified_action'][-2]
+    states = data['states'][:, [0,1,2,3,6,7]]
+    actions = data['actions']
 
     errors = []
     next_states = [states[0, :]]
@@ -278,32 +283,32 @@ def identify_system():
         errors.append(np.squeeze(states[i+1,:]) - np.squeeze(pred_next_state))
 
     errors = np.array(errors)
-    errors[:, 0][errors[:, 0] > 0.0005] = 0.0005
-    errors[:, 0][errors[:, 0] < -0.0005] = -0.0005
-    errors[:, 2][errors[:, 2] > 0.0003] = 0.0003
-    errors[:, 2][errors[:, 2] < -0.0003] = -0.0003
+    # errors[:, 0][errors[:, 0] > 0.0005] = 0.0005
+    # errors[:, 0][errors[:, 0] < -0.0005] = -0.0005
+    # errors[:, 2][errors[:, 2] > 0.0003] = 0.0003
+    # errors[:, 2][errors[:, 2] < -0.0003] = -0.0003
     next_states = np.array(next_states)
 
-    # plt.plot(states[:, 0], label='x')
-    # plt.plot(states[:, 2], label='y')
-    # plt.plot(next_states[:, 0], label='pred_x')
-    # plt.plot(next_states[:, 2], label='pred_y')
-    # plt.legend()
-    # plt.show()
+    plt.plot(states[:, 0], label='x')
+    plt.plot(states[:, 2], label='y')
+    plt.plot(next_states[:, 0], label='pred_x')
+    plt.plot(next_states[:, 2], label='pred_y')
+    plt.legend()
+    plt.show()
 
     np.save('./models/traj_data/errors.npy', errors)
 
     print_errors(errors)
 
 
 def linear_regression():
-    with open(f'./results_cf/ppo/none.pkl', 'rb') as f:
+    with open(f'./all_trajs/mpsf_10/cert/test0.pkl', 'rb') as f:
         data = pickle.load(f)
-    states = data['state'][-2][:, [0,1,2,3,6,7]]
-    actions = data['certified_action'][-2]
+    states = data['states'][:, [0,1,2,3,6,7]]
+    actions = data['actions']
     n = states.shape[1]
 
-    X = np.hstack((states[:-1, :], actions[:, :]))
+    X = np.hstack((states[:-1, :], actions[:-1, :]))
     y = states[1:, :]
 
     lamb = 0.02
@@ -314,7 +319,7 @@ def linear_regression():
 
     A = np.atleast_2d(theta.T[:n,:n])
     B = np.atleast_2d(theta.T[:,n:])
-    y_est2 = A @ states[:-1, :].T + B @ actions[:, :].T
+    y_est2 = A @ states[:-1, :].T + B @ actions[:-1, :].T
     LIN_err = np.linalg.norm(y_est2.T - y)
     print(f'LIN ERROR: {LIN_err}')
 

diff --git a/experiments/crazyflie/models/mpsc_parameters/nl_mpsc_crazyflie_track.pkl b/experiments/crazyflie/models/mpsc_parameters/nl_mpsc_crazyflie_track.pkl
diff --git a/experiments/crazyflie/models/traj_data/errors.npy b/experiments/crazyflie/models/traj_data/errors.npy
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/constraint_violations.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/constraint_violations.png
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/magnitude_of_corrections.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/magnitude_of_corrections.png
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/number_of_corrections.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/number_of_corrections.png
diff --git a/.../crazyflie/results_cf/ppo/graphs/real_cert/percent_magnitude_of_corrections.png b/.../crazyflie/results_cf/ppo/graphs/real_cert/percent_magnitude_of_corrections.png
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/percent_max_correction.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/percent_max_correction.png
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/rate_of_change.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/rate_of_change.png
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/reward.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/reward.png
diff --git a/experiments/crazyflie/results_cf/ppo/graphs/real_cert/rmse.png b/experiments/crazyflie/results_cf/ppo/graphs/real_cert/rmse.png
diff --git a/experiments/crazyflie/train_all_models.sh b/experiments/crazyflie/train_all_models.sh
@@ -3,6 +3,6 @@ for ALGO in ppo; do
     ./test_crazyflie.sh mpsf $ALGO False 0.1 #mpsf_sr_pen_0.1
     ./test_crazyflie.sh mpsf $ALGO False 1 #mpsf_sr_pen_1
     ./test_crazyflie.sh mpsf $ALGO False 10 #mpsf_sr_pen_10
-    # ./test_crazyflie.sh none $ALGO False False #none
+    ./test_crazyflie.sh none $ALGO False False #none
     ./test_crazyflie.sh none $ALGO True False #none_cpen
 done
diff --git a/safe_control_gym/safety_filters/mpsc/mpsc_cost_function/precomputed_cost.py b/safe_control_gym/safety_filters/mpsc/mpsc_cost_function/precomputed_cost.py
@@ -41,18 +41,19 @@ def __init__(self,
 
     def set_dynamics(self):
         '''Compute the linear dynamics. '''
-        self.Ad = np.array([[0.9885, 0.0419, -0.0005, 0.0, -0.0032, 0.0354],
-                    [-0.0862, 1.0142, -0.0037, 0.0004, -0.0236, 0.2658],
-                    [-0.0015, 0.0002, 0.9979, 0.0399, -0.0485, -0.0031],
-                    [-0.0112, 0.0012, -0.0157, 0.9994, -0.3639, -0.0232],
-                    [-0.0109, 0.0018, -0.0111, 0.0017, 0.8245, -0.0036],
-                    [0.0399, -0.0072, -0.0013, 0.0008, -0.0156, 0.8651]])
-        self.Bd = np.array([[0.0011, -0.0015],
-                    [0.0082, -0.0111],
-                    [0.0083, -0.002],
-                    [0.0624, -0.0147],
-                    [0.1944, -0.0307],
-                    [0.0344, 0.1793]])
+        self.Ad = np.array([[0.9961, 0.0399, -0.001, 0.0001, -0.0001, 0.0384],
+                            [-0.0255, 0.9991, -0.0072, 0.0008, -0.0001, 0.2937],
+                            [-0.0007, 0.0005, 0.9979, 0.0395, -0.0369, -0.0005],
+                            [-0.0051, 0.0038, -0.0145, 0.9961, -0.2781, -0.0039],
+                            [-0.0009, 0.0053, -0.0088, 0.0035, 0.8917, -0.0184],
+                            [0.008, -0.0073, 0.0028, -0.0007, 0.0002, 0.8974]])
+
+        self.Bd = np.array([[0.0033, -0.0011],
+                            [0.0252, -0.0047],
+                            [0.0055, -0.0003],
+                            [0.0404, -0.0019],
+                            [0.168, 0.013],
+                            [-0.0097, 0.1138]])
 
         delta_x = cs.MX.sym('delta_x', self.model.nx, 1)
         delta_u = cs.MX.sym('delta_u', self.model.nu, 1)

diff --git a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py
@@ -96,32 +96,33 @@ def __init__(self,
 
     def set_dynamics(self):
         '''Compute the discrete dynamics.'''
-        self.Ad = np.array([[0.9885, 0.0419, -0.0005, 0.0, -0.0032, 0.0354],
-                    [-0.0862, 1.0142, -0.0037, 0.0004, -0.0236, 0.2658],
-                    [-0.0015, 0.0002, 0.9979, 0.0399, -0.0485, -0.0031],
-                    [-0.0112, 0.0012, -0.0157, 0.9994, -0.3639, -0.0232],
-                    [-0.0109, 0.0018, -0.0111, 0.0017, 0.8245, -0.0036],
-                    [0.0399, -0.0072, -0.0013, 0.0008, -0.0156, 0.8651]])
-        self.Bd = np.array([[0.0011, -0.0015],
-                    [0.0082, -0.0111],
-                    [0.0083, -0.002],
-                    [0.0624, -0.0147],
-                    [0.1944, -0.0307],
-                    [0.0344, 0.1793]])
-
-        self.Ac = np.array([[-0.2595,    1.048,   -0.01049, -0.000269,    -0.0679,   0.8021],
-                            [-2.297 ,    0.4277,  -0.09076,  0.00929,     -0.583,    7.116],
-                            [-0.03644,   0.005847,-0.05084,  0.9998,      -1.129,   -0.07294],
-                            [-0.3232,    0.04369, -0.4508,   0.003461,    -10.02,   -0.6448],
-                            [-0.2965,    0.05503, -0.305,    0.05306,     -4.822,   -0.109],
-                            [ 1.065,    -0.2141,  -0.03759,  0.02273,     -0.4588,  -3.612]])
-
-        self.Bc = np.array([[0.01629,   -0.1022],
-                            [0.1356,   -0.9382],
-                            [0.2845,  -0.05316],
-                            [2.579,  -0.4668],
-                            [5.345,   -0.8333],
-                            [0.9703,     4.806]])
+        self.Ad = np.array([[0.9961, 0.0399, -0.001, 0.0001, -0.0001, 0.0384],
+                            [-0.0255, 0.9991, -0.0072, 0.0008, -0.0001, 0.2937],
+                            [-0.0007, 0.0005, 0.9979, 0.0395, -0.0369, -0.0005],
+                            [-0.0051, 0.0038, -0.0145, 0.9961, -0.2781, -0.0039],
+                            [-0.0009, 0.0053, -0.0088, 0.0035, 0.8917, -0.0184],
+                            [0.008, -0.0073, 0.0028, -0.0007, 0.0002, 0.8974]])
+
+        self.Bd = np.array([[0.0033, -0.0011],
+                            [0.0252, -0.0047],
+                            [0.0055, -0.0003],
+                            [0.0404, -0.0019],
+                            [0.168, 0.013],
+                            [-0.0097, 0.1138]])
+
+        self.Ac = np.array([[-0.088144,1.0026,-0.022589,0.0028295,-0.0026786,0.85621],
+                            [-0.66997,0.019928,-0.19128,0.0267,-0.0030537,7.7556],
+                            [-0.015111,0.012939,-0.048778,0.99175,-0.82836,-0.020729],
+                            [-0.12895,0.11697,-0.39583,-0.076549,-7.377,-0.19453],
+                            [-0.01963,0.13845,-0.23102,0.097167,-2.8552,-0.53572],
+                            [0.20869,-0.19672,0.073128,-0.019893,0.0039517,-2.6789]])
+
+        self.Bc = np.array([[0.074194,-0.071837],
+                            [0.6692,-0.56711],
+                            [0.18415,-3.9888e-05],
+                            [1.642,0.014572],
+                            [4.4383,0.37627],
+                            [-0.25357,2.9992]])
 
         delta_x = cs.MX.sym('delta_x', self.n, 1)
         delta_u = cs.MX.sym('delta_u', self.m, 1)