test: Tested and added more information about PID controller

danielbinschmid · Feb 16, 2024 · 1f88d8c · 1f88d8c
1 parent 71ef2f3
commit 1f88d8c
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 4 deletions.
diff --git a/gym_pybullet_drones/control/MPCCasADiControl.py b/gym_pybullet_drones/control/MPCCasADiControl.py
@@ -173,12 +173,33 @@ 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
+        #####################             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
         # COMPLETION TIME MEAN: 8.832291666666666
         # SUCCESS RATE: 1.0
         # AVERAGE DEVIATION:  0.05192337496578584
         # MAXIMUM DEVIATION: 0.14372815993134694
 
+        ##################### distance < 0.1 #####################
+        # DEFAULT_DISCR_LEVEL = 10
+        # -> misses the end goal at one track
+
+        # Generally with this distance it does get out of the loop
+
+        ##################### 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
+
+        ##################### distance < 0.05 #####################
+
+        # DEFAULT_DISCR_LEVEL = 20
+        # Hangs up itself because of small distance
+
+        # 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)

diff --git a/runnables/test_suite_eval/pid_CasADiMPC_20tracks_testsuite.py b/runnables/test_suite_eval/pid_CasADiMPC_20tracks_testsuite.py
@@ -49,7 +49,7 @@
 DEFAULT_DURATION_SEC = 50
 DEFAULT_OUTPUT_FOLDER = 'results'
 DEFAULT_COLAB = False
-DEFAULT_DISCR_LEVEL = 20
+DEFAULT_DISCR_LEVEL = 10
 DEFAULT_EVAL_SET = "./eval-v0_n-ctrl-points-3_n-tracks-20_2024-02-11_22:18:28_46929077-0248-4c6e-b2b1-da2afb13b2e2"
 
 from runnables.test_suite_eval.utils import compute_metrics_single
@@ -122,8 +122,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))
@@ -175,9 +175,12 @@ def run(
                 physicsClientId=env.CLIENT
             )
 
+
+            '''# MPC distance if statements
             #if distance < 0.05 or ((current_step == len(TARGET_TRAJECTORY) - 2 or current_step == len(TARGET_TRAJECTORY) - 3) and distance < 0.3):
             if distance < 0.05:
                 #if current_step == len(TARGET_TRAJECTORY) -1 and velocity < 0.2:
+                #if current_step == len(TARGET_TRAJECTORY) - 1 and distance < 0.1:
                 if current_step == len(TARGET_TRAJECTORY) - 1:
                     # env.render()
                     all_pos = env.pos_logger.load_all() 
@@ -188,8 +191,23 @@ def run(
                     times.append(t)
                     successes.append(True)
                     break
+                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: # 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:
+                    # env.render()
+                    all_pos = env.pos_logger.load_all()
+                    t = env.step_counter * env.PYB_TIMESTEP
+                    mean_dev, max_dev = compute_metrics_single(all_pos, track)
+                    mean_devs.append(mean_dev)
+                    max_devs.append(max_dev)
+                    times.append(t)
+                    successes.append(True)
+                    break
                 current_step = (current_step + 1) % len(TARGET_TRAJECTORY)
-
 
             ##### Log the simulation ####################################