test: Tested and added more information about PID controller and MPC …

…controller.

Compared both with the following trajectory and termination if statements:

if distance < 0.2 and velocity < 1.0:
    if current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0:

PID:
        # DEFAULT_DISCR_LEVEL = 10
        # COMPLETION TIME MEAN: 4.537280701754386 (+ 1 Second and minus some accuracy, because it terminates quite early)
        # SUCCESS RATE: 0.95 -> It actually fails in the ninth track
        # AVERAGE DEVIATION:  0.06635711385416021
        # MAXIMUM DEVIATION: 0.1806313058800846

MPC:
        # N DISCR LEVEL: 10
        # COMPLETION TIME MEAN: 4.672916666666667
        # SUCCESS RATE: 1.0
        # AVERAGE DEVIATION:  0.10109089137096094
        # MAXIMUM DEVIATION: 0.22336670663036812

gym_pybullet_drones/control/MPCCasADiControl.py

@@ -173,6 +173,8 @@ def __init__(self,
         ### 20 tracks test suite experiments and test results with varying trajectory discretization levels
 
         ##### PID reference performance on the 20 tracks test suite
+
+        ### Stop and Go
         #####################             if distance < 0.2 and velocity < 0.05: # Original Stop-And-GO
             # if current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0: #####################
         # N DISCR LEVEL: 10
@@ -190,7 +192,11 @@ def __init__(self,
         ##################### if distance < 0.2 and velocity < 1.0:
         #    if current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0: #####################
 
-        # DEFAULT_DISCR_LEVEL = 10 -> It actually fails in the ninth track
+        # DEFAULT_DISCR_LEVEL = 10
+        # COMPLETION TIME MEAN: 4.537280701754386 (+ 1 Second and minus some accuracy, because it terminates quite early)
+        # SUCCESS RATE: 0.95 -> It actually fails in the ninth track
+        # AVERAGE DEVIATION:  0.06635711385416021
+        # MAXIMUM DEVIATION: 0.1806313058800846
 
         ##################### distance < 0.05 #####################
 
@@ -227,6 +233,8 @@ def __init__(self,
         Q = diagcat(1, 1, 1, 0.6, 0.6, 1, 0, 0, 0, 0, 0, 0)
         R = diagcat(0.3, 0.3, 0.3, 0.8)
 
+        ##################### distance < 0.05 #####################
+
         # DEFAULT_DISCR_LEVEL = 10 ->
         # 5%|▌         | 1/20 [01:04<20:33, 64.94s/it]
         # 10%|█         | 2/20 [02:14<20:16, 67.61s/it]
@@ -274,6 +282,15 @@ def __init__(self,
             # 15%|█▌        | 3/20 [06:32<37:19, 131.74s/it]
             # 20%|██        | 4/20 [09:32<40:12, 150.75s/it]
 
+        #####################if distance < 0.2 and velocity < 1.0:
+        #    if current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0: #####################
+
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 4.672916666666667
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.10109089137096094
+        # MAXIMUM DEVIATION: 0.22336670663036812
+
 
         '''# Only care greedily about the position 
         Q = diagcat(1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0)

runnables/test_suite_eval/pid_CasADiMPC_20tracks_testsuite.py

@@ -46,7 +46,8 @@
 DEFAULT_OBSTACLES = True
 DEFAULT_SIMULATION_FREQ_HZ = 240
 DEFAULT_CONTROL_FREQ_HZ = 48
-DEFAULT_DURATION_SEC = 50
+# DEFAULT_DURATION_SEC = 50
+DEFAULT_DURATION_SEC = 30
 DEFAULT_OUTPUT_FOLDER = 'results'
 DEFAULT_COLAB = False
 DEFAULT_DISCR_LEVEL = 10
@@ -122,8 +123,8 @@ def run(
 
         #### Initialize the controller
         drone = DroneModel.CF2X
-        ctrl = DSLPIDControl(drone_model=drone)
-        #ctrl = MPCCasADiControl(drone_model=drone)
+        #ctrl = DSLPIDControl(drone_model=drone)
+        ctrl = MPCCasADiControl(drone_model=drone)
 
         #### Run the simulation
         action = np.zeros((num_drones,4))
@@ -194,9 +195,9 @@ def run(
                 current_step = (current_step + 1) % len(TARGET_TRAJECTORY)'''
 
             #PID distance if statements
-            if distance < 0.2 and velocity < 0.05: # Original Stop-And-GO
+            #if distance < 0.2 and velocity < 0.05: # Original Stop-And-GO
             # if distance < 0.2: # this creates a final position bug somehow
-            #if distance < 0.2 and velocity < 1.0:
+            if distance < 0.2 and velocity < 1.0:
                 if current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0:
                     # env.render()
                     all_pos = env.pos_logger.load_all()