Final results from physical experiments

experiments/crazyflie/config_overrides/crazyflie_track.yaml

@@ -24,7 +24,7 @@ task_config:
     init_q: 0
     init_r: 0
   randomized_init: False
-  randomized_inertial_prop: True
+  randomized_inertial_prop: False
   inertial_prop_randomization_info:
     M:
       distrib: 'uniform'
@@ -46,7 +46,7 @@ task_config:
   init_state_randomization_info:
     init_x:
       distrib: 'uniform'
-      low: -0.5
+      low: -0.95
       high: 0.95
     init_x_dot:
       distrib: 'uniform'
@@ -132,10 +132,10 @@ task_config:
   done_on_out_of_bound: True
   done_on_violation: False
 
-  disturbances:
-    observation:
-      - disturbance_func: white_noise
-        std: 0.002
-    dynamics:
-      - disturbance_func: white_noise
-        std: 0.2
+  # disturbances:
+  #   observation:
+  #     - disturbance_func: white_noise
+  #       std: 0.002
+  #   dynamics:
+  #     - disturbance_func: white_noise
+  #       std: 0.2

experiments/crazyflie/crazyflie_controller.py

@@ -114,7 +114,7 @@
     'integration_algo': 'rk4',
     'use_terminal_set': False,
     'cost_function': 'precomputed_cost',
-    'mpsc_cost_horizon': 10,
+    'mpsc_cost_horizon': 2,
     'decay_factor': 0.85,
     'prior_info': {
         'prior_prop': None,
@@ -171,6 +171,9 @@ def cmdFirmware(self,
 
         iteration = int(ep_time * self.CTRL_FREQ)
         z_height = 1
+        x_offset = 0.0
+        y_offset = 0.0
+        flip_traj = False
 
         self.low_pass_state(obs)
 
@@ -182,7 +185,7 @@ def cmdFirmware(self,
             args = [height, duration]
         elif iteration >= 2 * self.CTRL_FREQ and iteration < 3 * self.CTRL_FREQ:
             print(f'Iter: {iteration} - Re-centering at (0, 0, z_height).')
-            target_pos = np.array([0, 0, z_height])
+            target_pos = np.array([x_offset, y_offset, z_height])
             target_vel = np.zeros(3)
             target_acc = np.zeros(3)
             target_yaw = 0.0
@@ -194,6 +197,18 @@ def cmdFirmware(self,
             step = iteration - 3 * self.CTRL_FREQ
             info = {'current_step': step*20}
 
+            obs[0] -= x_offset
+            obs[2] -= y_offset
+
+            if flip_traj:
+                old_obs = obs.copy()
+                obs[0] = old_obs[2]
+                obs[1] = old_obs[3]
+                obs[2] = -old_obs[0]
+                obs[3] = -old_obs[1]
+                obs[6] = old_obs[7]
+                obs[7] = -old_obs[6]
+
             curr_obs = obs[[0,1,2,3,6,7]].reshape((6, 1))
 
             new_act = np.squeeze(self.ctrl.select_action(curr_obs, info))
@@ -207,13 +222,18 @@ def cmdFirmware(self,
                 else:
                     self.corrections.append([0,0])
 
+            if flip_traj:
+                new_act = np.array([-new_act[1], new_act[0]])
+            else:
+                new_act = np.array([new_act[0], new_act[1]])
+
             command_type = Command(7)  # cmdVel
             args = [new_act[0]*180.0/np.pi, new_act[1]*180.0/np.pi, 0, 0]
 
             self.recorded_obs.append(obs)
             self.actions.append(new_act)
         elif iteration == 18 * self.CTRL_FREQ:
-            self.return_path_pos = np.linspace(obs[[0, 2, 4]], [0, 0, z_height], 4 * self.CTRL_FREQ)
+            self.return_path_pos = np.linspace(obs[[0, 2, 4]], [x_offset, y_offset, z_height], 4 * self.CTRL_FREQ)
             self.return_path_vel = np.linspace(obs[[1, 3, 5]], [0, 0, 0], 4 * self.CTRL_FREQ)
             command_type = Command(1)  # cmdFullState.
             args = [self.return_path_pos[0], self.return_path_vel[0], np.zeros(3), 0.0, np.zeros(3)]
@@ -283,7 +303,7 @@ def setup_controllers(self):
                          **algo_config)
         if algo in ['ppo', 'sac', 'safe_explorer_ppo', 'cpo']:
             # Load state_dict from trained.
-            self.ctrl.load(path=f'/home/federico/GitHub/safe-control-gym/experiments/crazyflie/models/rl_models/{algo}/{MODEL}/model_best.pt')
+            self.ctrl.load(path=f'/home/federico/GitHub/safe-control-gym/experiments/crazyflie/models/rl_models/{algo}/{MODEL}/model_latest.pt')
 
             # Remove temporary files and directories
             # self.shutil.rmtree(f'{curr_path}/temp', ignore_errors=True)

experiments/crazyflie/crazyflie_utils.py

@@ -203,3 +203,4 @@ def gen_traj(CTRL_FREQ, EPISODE_LEN_SEC, plot=False):
             print(f'MODEL: {MODEL}, TEST: {test}')
             plot_traj(CTRL_FREQ=25, TEST=test, CERTIFIED=CERTIFIED, MODEL=MODEL)
             calc_error(CTRL_FREQ=25, TEST=test, CERTIFIED=CERTIFIED, MODEL=MODEL)
+        print('--------------------')

experiments/crazyflie/models/rl_models/ppo/mpsf_0.1_dm/config.yaml

@@ -9,13 +9,13 @@ algo_config:
   deque_size: 10
   entropy_coef: 0.01
   eval_batch_size: 10
-  eval_interval: 5000
+  eval_interval: 10000
   eval_save_best: true
   filter_train_actions: true
   gae_lambda: 0.95
   gamma: 0.99
   hidden_dim: 128
-  log_interval: 5000
+  log_interval: 10000
   max_env_steps: 250000
   max_grad_norm: 0.5
   mini_batch_size: 256
@@ -63,9 +63,9 @@ sf_config:
     prior_prop_rand_info: null
     randomize_prior_prop: false
   q_lin:
-  - 0.008
+  - 0.8
   - 1.85
-  - 0.008
+  - 0.8
   - 1.85
   - 10
   - 10

experiments/crazyflie/models/rl_models/ppo/mpsf_0.1_dm/logs/loss/approx_kl.log

@@ -1,51 +1,26 @@
 step,loss/approx_kl
-5000,0.017382785235531627
-10000,0.022589971769290663
-15000,0.02030100664899995
-20000,0.006828490175151576
-25000,0.0076807835061723975
-30000,0.016380948030079407
-35000,0.0125215829970936
-40000,0.009967301134020091
-45000,0.007885956488704928
-50000,0.01109645285177976
-55000,0.00792091090309744
-60000,0.020191092238140605
-65000,0.021421856192561487
-70000,0.009052827620568376
-75000,0.011448173869090777
-80000,0.013139393545376757
-85000,0.01579810444964096
-90000,0.004829494065294664
-95000,0.02899255441346516
-100000,0.01947939985742172
-105000,0.02110287871522208
-110000,0.03189673851011321
-115000,0.03193722238065675
-120000,0.006573856230049084
-125000,0.015430213169505203
-130000,0.027915337126857294
-135000,0.007019270518018553
-140000,0.011309790165008356
-145000,0.026494619712078327
-150000,0.02006649870503073
-155000,0.0199088517188405
-160000,0.019885726928866157
-165000,0.03360261083968605
-170000,0.015627667183677356
-175000,0.01978173813549802
-180000,0.017880892351968214
-185000,0.009480813091310363
-190000,0.007478245873547472
-195000,0.020076866896124555
-200000,0.016203512311525024
-205000,0.014536169182974845
-210000,0.03138880368981821
-215000,0.02211007551134874
-220000,0.014886595665787658
-225000,0.005795973601440589
-230000,0.017234807409113272
-235000,0.006469479740674919
-240000,0.028982087329495698
-245000,0.03264161537323768
-250000,0.026402933934393035
+10000,0.013607210634897149
+20000,0.013761914855179682
+30000,0.025598444991434616
+40000,0.01888002099391694
+50000,0.004441294248681516
+60000,0.014219652888520312
+70000,0.014928670569012564
+80000,0.015802517238383492
+90000,0.04180019126506522
+100000,0.013963359489571305
+110000,0.02210270546687146
+120000,0.015204811303798731
+130000,0.014395444159163162
+140000,0.014147912359718856
+150000,0.018358417790538324
+160000,0.021968793659470975
+170000,0.022407488165966542
+180000,0.01445692424895242
+190000,0.021841876379524665
+200000,0.021717270855636645
+210000,0.02077241976124545
+220000,0.026675788119124872
+230000,0.0023752693280888106
+240000,0.019338119709088157
+250000,0.008689712717508274

experiments/crazyflie/models/rl_models/ppo/mpsf_0.1_dm/logs/loss/entropy_loss.log

@@ -1,51 +1,26 @@
 step,loss/entropy_loss
-5000,-1.8361285428206124
-10000,-1.82688273191452
-15000,-1.913368135690689
-20000,-1.9855973601341248
-25000,-2.011720661322276
-30000,-1.9439181884129841
-35000,-1.8821250061194106
-40000,-1.8513302425543467
-45000,-1.83209662437439
-50000,-1.758334481716156
-55000,-1.7150042215983075
-60000,-1.6776649971803028
-65000,-1.6673952917257941
-70000,-1.5787680447101593
-75000,-1.5650407592455546
-80000,-1.5491252422332766
-85000,-1.505615246295929
-90000,-1.4219185292720795
-95000,-1.3889550387859344
-100000,-1.3552403291066488
-105000,-1.3418766697247826
-110000,-1.3675598899523416
-115000,-1.3077599783738454
-120000,-1.298532881339391
-125000,-1.3397121985753377
-130000,-1.3651795804500577
-135000,-1.3902423977851865
-140000,-1.355457752943039
-145000,-1.3550011773904165
-150000,-1.3701329986254374
-155000,-1.3242215394973753
-160000,-1.2985392471154533
-165000,-1.2860853711764018
-170000,-1.2010333816210428
-175000,-1.2163227558135987
-180000,-1.2099015633265178
-185000,-1.2442416071891784
-190000,-1.261824017763138
-195000,-1.2877426167329153
-200000,-1.3410847385724387
-205000,-1.4147231936454772
-210000,-1.3652738293011983
-215000,-1.3583995322386424
-220000,-1.3398982922236125
-225000,-1.3226667205492653
-230000,-1.274121061960856
-235000,-1.3047802408536278
-240000,-1.3364199578762055
-245000,-1.4134876867135364
-250000,-1.4060516814390818
+10000,-1.8222271064917248
+20000,-1.8108969410260518
+30000,-1.7184275488058727
+40000,-1.678019466002782
+50000,-1.622452578941981
+60000,-1.699874718983968
+70000,-1.6396838843822479
+80000,-1.5770497779051464
+90000,-1.4773497839768726
+100000,-1.3652685125668844
+110000,-1.2523182610670727
+120000,-1.1309317906697591
+130000,-1.0892229437828065
+140000,-1.1528362850348155
+150000,-1.2121699154376981
+160000,-1.2210738619168597
+170000,-1.2937161246935527
+180000,-1.265173155069351
+190000,-1.2762505888938904
+200000,-1.269532529513041
+210000,-1.3366137802600861
+220000,-1.336885952949524
+230000,-1.3322320560614267
+240000,-1.3727359831333161
+250000,-1.375965197881063

experiments/crazyflie/models/rl_models/ppo/mpsf_0.1_dm/logs/loss/policy_loss.log

@@ -1,51 +1,26 @@
 step,loss/policy_loss
-5000,-0.012522293106811789
-10000,-0.0028639159457480704
-15000,-5.32773759835751e-05
-20000,0.0010914902690196745
-25000,0.00347031129606526
-30000,-0.004767225751231375
-35000,-0.0039614881575274295
-40000,-0.01352363157549773
-45000,-0.006562991765826856
-50000,-0.005746712052132893
-55000,-0.01152805065505425
-60000,-0.0060847108972918815
-65000,-0.009534457117109556
-70000,-0.009505007624820516
-75000,-0.01352899745792801
-80000,-0.018766159857950583
-85000,-0.00534436807159421
-90000,-0.013342466192617133
-95000,-0.0024944706762069007
-100000,-0.004071236526101746
-105000,-0.0018255306541087126
-110000,-0.0006416579724792436
-115000,-0.013627894936237946
-120000,-0.020385944539960225
-125000,-0.011547697844560677
-130000,-0.0035214511768439038
-135000,-0.008306094358908849
-140000,-0.008195982065765566
-145000,-0.007619447698982798
-150000,-0.01835940148071558
-155000,-0.012947128595246526
-160000,-0.015062114466865912
-165000,0.0009759008840402453
-170000,-0.00031560759477246145
-175000,-0.01334679708111521
-180000,-0.0075879055870092275
-185000,-0.007718350967767526
-190000,-0.005011718342062056
-195000,-0.00720414434473392
-200000,-0.014478761856062361
-205000,-0.004751333425089356
-210000,-0.006541795057693897
-215000,-0.010741405941133821
-220000,-0.011413150534142444
-225000,-0.007842611009038096
-230000,-0.004305258476983395
-235000,-0.014982258770081702
-240000,-0.013144356059479737
-245000,-0.003563010794739103
-250000,-0.002930501059427549
+10000,-0.010007669983208692
+20000,-0.008122551010522231
+30000,-0.0067770435390835425
+40000,-0.0031646714114225948
+50000,-0.011301381334934886
+60000,-0.009295303222030161
+70000,-0.004106881245705277
+80000,-0.002182963554981883
+90000,-0.00534663743345262
+100000,0.0024393753451593626
+110000,0.003172114451514424
+120000,-0.008490380094660006
+130000,-0.013068873321659045
+140000,-0.01619728706391781
+150000,0.0026705906213050487
+160000,-0.010143434947884092
+170000,-0.0031631258030429743
+180000,-0.008483099821006456
+190000,-0.0060088410008760345
+200000,-0.0016735860018852636
+210000,-0.0034843393437012345
+220000,-0.002280271509115036
+230000,-0.01069025629104516
+240000,-0.009390805367583006
+250000,-0.015535020140737685