feature + test: Introduced intermediary target state as weighted stat…

…e from current and next target position from the trajectory.

NEXT_TARGET_WEIGHTING = 0.05
target_position_next = TARGET_TRAJECTORY[next_step].coordinate
# Shouldn't taht be the other way around? It doesn't work the other way around
vector_cur_next = target_position - target_position_next
intermediary_target_position = target_position + NEXT_TARGET_WEIGHTING * vector_cur_next

It actually boosts the performance significantly, but it is less accurate

gym_pybullet_drones/control/MPCCasADiControl.py

@@ -419,6 +419,52 @@ def __init__(self,
         # AVERAGE DEVIATION:  0.09995147199727226
         # MAXIMUM DEVIATION: 0.2386860064898518
 
+        # First track performance
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 3.6875
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.08253830255256683
+        # MAXIMUM DEVIATION: 0.1627968474411629
+
+
+        ### Usage of Intermediary position
+
+        # NEXT_TARGET_WEIGHTING = 0.05
+        # First track performance
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 3.2083333333333335
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.08938545527353497
+        # MAXIMUM DEVIATION: 0.13600094796004633
+
+        # NEXT_TARGET_WEIGHTING = 0.1
+        # First track performance
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 2.7916666666666665
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.07589474097833902
+        # MAXIMUM DEVIATION: 0.1358326740671975
+        # Misses the goal significantly in the fourth track or so
+
+        # NEXT_TARGET_WEIGHTING = 0.15
+        # First track performance
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 2.6458333333333335
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.06783051249599097
+        # MAXIMUM DEVIATION: 0.14791415079374462
+
+        # NEXT_TARGET_WEIGHTING = 0.2
+        # First track performance
+        # N DISCR LEVEL: 10
+        # COMPLETION TIME MEAN: 2.4791666666666665
+        # SUCCESS RATE: 1.0
+        # AVERAGE DEVIATION:  0.07267128234973419
+        # MAXIMUM DEVIATION: 0.1564386918440997
+        # ! fails one the second track though
+
+
+
         '''# Care (or only  42.5%) 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)

runnables/test_suite_eval/pid_CasADiMPC_20tracks_testsuite.py

@@ -27,8 +27,8 @@
 from gym_pybullet_drones.utils.enums import DroneModel, Physics
 from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary
 from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl
-#from gym_pybullet_drones.control.MPCCasADiControl import MPCCasADiControl
-from gym_pybullet_drones.control.MPCCasADiControl2States import MPCCasADiControl
+from gym_pybullet_drones.control.MPCCasADiControl import MPCCasADiControl
+#from gym_pybullet_drones.control.MPCCasADiControl2States import MPCCasADiControl
 from gym_pybullet_drones.utils.Logger import Logger
 from gym_pybullet_drones.utils.utils import sync, str2bool
 from runnables.test_suite_eval.eval_tracks import load_eval_tracks
@@ -51,7 +51,7 @@
 DEFAULT_DURATION_SEC = 10
 DEFAULT_OUTPUT_FOLDER = 'results'
 DEFAULT_COLAB = False
-DEFAULT_DISCR_LEVEL = 25
+DEFAULT_DISCR_LEVEL = 10
 # Original folder
 DEFAULT_EVAL_SET = "./eval-v0_n-ctrl-points-3_n-tracks-20_2024-02-11_22:18:28_46929077-0248-4c6e-b2b1-da2afb13b2e2"
 
@@ -150,27 +150,30 @@ def run(
             #### Step the simulation ###################################
             obs, reward, terminated, truncated, info = env.step(action)
             target_position = TARGET_TRAJECTORY[current_step].coordinate
+
+
+            '''NEXT_TARGET_WEIGHTING = 0.05
             target_position_next = TARGET_TRAJECTORY[next_step].coordinate
+            # Shouldn't taht be the other way around? It doesn't work the other way around
+            vector_cur_next = target_position - target_position_next
+            intermediary_target_position = target_position + NEXT_TARGET_WEIGHTING * vector_cur_next'''
 
-            two_target_position = np.hstack([target_position, target_position_next])
 
-            '''#### Compute control for the current way point #############
+            #### Compute control for the current way point #############
             action, _, _ = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP,
                 state=obs[0],
                 target_pos=np.hstack([target_position]),
                 # target_pos=INIT_XYZS[j, :] + TARGET_POS[wp_counters[j], :],
                 target_rpy=INIT_RPYS[0]
-            )'''
-
-            #### Compute control for the current and next way point #############
-            action, _, _ = ctrl.computeControlfrom2States(control_timestep=env.CTRL_TIMESTEP,
-                                                          cur_pos=obs[0][:3],
-                                                          # Extract current position from observation
-                                                          cur_quat=obs[0][3:7],  # Extract current quaternion
-                                                          cur_vel=obs[0][7:10],  # Current velocity
-                                                          cur_ang_vel=obs[0][10:13],  # Current angular velocity
-                                                          target_pos=two_target_position,
-                                                          target_rpy=INIT_RPYS[0])
+            )
+
+            '''#### Compute control for the intermediary of the current and next way point #############
+            action, _, _ = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP,
+                                                        state=obs[0],
+                                                        target_pos=np.hstack([intermediary_target_position]),
+                                                        # target_pos=INIT_XYZS[j, :] + TARGET_POS[wp_counters[j], :],
+                                                        target_rpy=INIT_RPYS[0]
+                                                        )'''
 
             action = np.array([action])