test: Tested the MPC controller and it performed faster than PID!

With the following if statemets:
if distance < 0.2:
   if current_step == len(TARGET_TRAJECTORY) - 1:

and

Q = diagcat(1, 1, 1, 0.3, 0.3, 0.2, 0, 0, 0, 0, 0, 0)
R = diagcat(0.15, 0.15, 0.15, 0.4)

        # N DISCR LEVEL: 10
        # COMPLETION TIME MEAN: 4.119791666666666
        # SUCCESS RATE: 1.0
        # AVERAGE DEVIATION:  0.09995147199727226
        # MAXIMUM DEVIATION: 0.2386860064898518

gym_pybullet_drones/control/MPCCasADiControl.py

@@ -206,6 +206,8 @@ def __init__(self,
         # N DISCR LEVEL: 30
         # -> misses the second track
 
+        ################################################################################################################
+
         '''# My own designed matrices partially inspired from TinyMPC
         #Q = diagcat(100, 100, 100, 1, 1, 1, 1, 1, 1, 1, 1, 1)
         #R = diagcat(10.0, 10.0, 10.0, 10.0)
@@ -226,7 +228,7 @@ def __init__(self,
         # DEFAULT_DISCR_LEVEL = 50 -> Completely goes off rails at the beginning'''
 
 
-        # Paper: Non-Linear Model Predictive Control Using CasADi Package for Trajectory Tracking of Quadrotor
+        '''# Paper: Non-Linear Model Predictive Control Using CasADi Package for Trajectory Tracking of Quadrotor
         # The weighting matrix Q = Diag[1, 1, 1, 0.6, 0.6, 1, 0, 0, 0, 0, 0, 0],
         # while the control input weighting matrix R = Diag[0.3, 0.3, 0.3, 0.8]
         
@@ -280,16 +282,7 @@ def __init__(self,
             # 5%|▌         | 1/20 [02:16<43:10, 136.33s/it]
             # 10%|█         | 2/20 [04:10<37:04, 123.58s/it]
             # 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
+            # 20%|██        | 4/20 [09:32<40:12, 150.75s/it]'''
 
 
         '''# Only care greedily about the position 
@@ -370,6 +363,46 @@ def __init__(self,
         # 5%|▌         | 1/20 [04:23<1:23:31, 263.79s/it]
         # 10%|█         | 2/20 [07:45<1:08:12, 227.35s/it]'''
 
+        ################################################################################################################
+
+        #####################if distance < 0.2 and velocity < 1.0:
+        #    if current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0: #####################
+
+        '''# Paper: Non-Linear Model Predictive Control Using CasADi Package for Trajectory Tracking of Quadrotor
+        # The weighting matrix Q = Diag[1, 1, 1, 0.6, 0.6, 1, 0, 0, 0, 0, 0, 0],
+        # while the control input weighting matrix R = Diag[0.3, 0.3, 0.3, 0.8]
+
+        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)
+        
+        # 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)
+        R = diagcat(0, 0, 0, 0)
+        # -> Test result: Drone completely goes off the rails and crazyflie flies crazily and fails'''
+
+        # Care 2x less about and velocities and reference input of the drone
+                    # Reference Weighting matrices from the paper
+                    # 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)
+
+        Q = diagcat(1, 1, 1, 0.3, 0.3, 0.2, 0, 0, 0, 0, 0, 0)
+        R = diagcat(0.15, 0.15, 0.15, 0.4)
+
+        ###             if distance < 0.2:
+                # if current_step == len(TARGET_TRAJECTORY) - 1:
+
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 4.119791666666666
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.09995147199727226
+        # MAXIMUM DEVIATION: 0.2386860064898518
+
         x_init = P[0:Nx]
         g = X[:, 0] - P[0:Nx]  # initial condition constraints
         h = 0.15

runnables/test_suite_eval/pid_CasADiMPC_20tracks_testsuite.py

@@ -197,8 +197,8 @@ def run(
             #PID distance if statements
             #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 current_step == len(TARGET_TRAJECTORY) - 1 and velocity < 1.0:
+            if distance < 0.2:
+                if current_step == len(TARGET_TRAJECTORY) - 1:
                     # env.render()
                     all_pos = env.pos_logger.load_all()
                     t = env.step_counter * env.PYB_TIMESTEP
@@ -207,6 +207,14 @@ def run(
                     max_devs.append(max_dev)
                     times.append(t)
                     successes.append(True)
+
+                    ## Already print after each track
+                    print(f'N DISCR LEVEL: {n_discr_level}')
+                    print("COMPLETION TIME MEAN:", np.mean(times))
+                    print("SUCCESS RATE:", np.mean(successes))
+                    print("AVERAGE DEVIATION: ", np.mean(mean_devs))
+                    print("MAXIMUM DEVIATION:", np.mean(max_devs))
+
                     break
                 current_step = (current_step + 1) % len(TARGET_TRAJECTORY)