removed unnecessary logs

agents/test_policy.py

@@ -46,7 +46,7 @@ def test_simple_follower(local: bool, filename: str, test_env_nogui: BaseRLAviar
             obs, reward, terminated, truncated, info = test_env.step(action)
             obs2 = obs.squeeze()
             act2 = action.squeeze()
-            print("Obs:", obs, "\tAction", action, "\tReward:", reward, "\tTerminated:", terminated, "\tTruncated:", truncated)
+            # print("Obs:", obs, "\tAction", action, "\tReward:", reward, "\tTerminated:", terminated, "\tTruncated:", truncated)
             logger.log(drone=0,
                         timestamp=i/test_env.CTRL_FREQ,
                         state=np.hstack([obs2[0:3],
@@ -57,8 +57,8 @@ def test_simple_follower(local: bool, filename: str, test_env_nogui: BaseRLAviar
                         control=np.zeros(12)
                         )
 
-            test_env.render()
-            print(terminated)
+            # test_env.render()
+            # print(terminated)
             if not eval_mode:
                 sync(i, start, test_env.CTRL_TIMESTEP)
             if terminated:
@@ -96,9 +96,6 @@ def run_test(config: Configuration, env_factory: BaseFactory, eval_mode=False):
         output_folder=config.output_path_location,
         eval_mode=eval_mode
     )
-    print("YY")
-    print(eval_mode)
-    print(eval_res)
     return eval_res
 
 

aviaries/rewards/uzh_trajectory_reward.py

@@ -44,7 +44,7 @@ def __init__(self,
         self.k_wp = k_wp
         self.k_s = k_s 
         self.k_w = k_w
-        print(f'k_p: {k_p}; k_wp: {k_wp}; k_s: {k_s}; k_w: {k_w}')
+        # print(f'k_p: {k_p}; k_wp: {k_wp}; k_s: {k_s}; k_w: {k_w}')
 
         self.wp_rewards = np.zeros(len(self.trajectory))
         self.max_reward_distance = max_reward_distance

gym_pybullet_drones/utils/Logger.py

@@ -101,7 +101,8 @@ def log(self,
 
         """
         if drone < 0 or drone >= self.NUM_DRONES or timestamp < 0 or len(state) != 20 or len(control) != 12:
-            print(f'[ERROR] in Logger.log(), invalid data. len(state)={len(state)}!=20 or len(control){len(control)}!=12.')
+            pass
+            # print(f'[ERROR] in Logger.log(), invalid data. len(state)={len(state)}!=20 or len(control){len(control)}!=12.')
 
         current_counter = int(self.counters[drone])
         #### Add rows to the matrices if a counter exceeds their size

runnables/test_suite_eval/C_trajectory_following.py

@@ -15,12 +15,13 @@
 from typing import List, Dict
 from tqdm import tqdm
 import json 
-
+from runnables.test_suite_eval.utils import compute_metrics_single
 ###### INFRASTRUCTURE PARAMS #######
 GUI = True
 RECORD_VIDEO = False
 OUTPUT_FOLDER = 'checkpointed_models'
 COLAB = False
+DEFAULT_EVAL_SET_FOLDER = "/shared/d/master/ws23/UAV-lab/git_repos/RL-pybullets-cf/runnables/test_suite_eval/eval-v0_n-ctrl-points-3_n-tracks-20_2024-02-11_22:18:28_46929077-0248-4c6e-b2b1-da2afb13b2e2"
 ####################################
 
 ###### USUALLY NOT CHANGED #########
@@ -57,40 +58,6 @@ def save_benchmark(benchmarks: Dict[str, float], file_path: str):
     with open(file_path, 'w') as file:
         json.dump(benchmarks, file)
 
-def compute_metrics(all_visisted_positions: np.ndarray, successes, tracks: List[DiscretizedTrajectory], n_discr_level=int(1e4)):
-
-    means = []
-    max_devs = []
-    n_fails = 0
-    for idx, success in enumerate(tqdm(successes)):
-
-        if success:
-            visited_positions = all_visisted_positions[idx - n_fails]
-            track = [wp for wp in tracks[idx]]
-            high_discr_ref_traj = TrajectoryFactory.get_pol_discretized_trajectory(
-                t_waypoints=track,
-                n_points_discretization_level=n_discr_level
-            )
-            ref_wps = np.array([wp.coordinate for wp in high_discr_ref_traj])
-
-            # metrics
-            time = len(visited_positions)
-
-            # Compute norms
-            # Reshape A and B for broadcasting, compute difference, norm, then mean across axis=1 (corresponding to M)
-            norms: np.ndarray = np.linalg.norm(visited_positions[:, np.newaxis, :] - ref_wps[np.newaxis, :, :], axis=2)
-            min_distances = norms.min(axis=1)
-            mean_dist = np.mean(min_distances)
-
-            max_dist = np.max(min_distances)
-
-            # max_dev_norms = norms.max(axis=1)
-
-            means.append(mean_dist)
-            max_devs.append(max_dist)
-        else:
-            n_fails += 1
-    return means, max_devs
 def init_targets():
     points_per_segment = 4
     z_segment = np.array([
@@ -123,10 +90,11 @@ def run(output_folder=OUTPUT_FOLDER,
         k_s: float = K_S,
         max_reward_distance: float = MAX_REWARD_DISTANCE,
         waypoint_dist_tol: float = WAYPOINT_DIST_TOL,
-        discr_level: float=DEFAULT_DISCR_LEVEL
+        discr_level: float=DEFAULT_DISCR_LEVEL,
+        eval_set: set = DEFAULT_EVAL_SET_FOLDER
     ):
 
-    eval_set_folder = "./eval-v0_n-ctrl-points-3_n-tracks-1000_2024-02-12_10:59:21_d689e67e-4179-4764-a474-e5f3237a239d"
+    eval_set_folder = eval_set# "./eval-v0_n-ctrl-points-3_n-tracks-1000_2024-02-12_10:59:21_d689e67e-4179-4764-a474-e5f3237a239d"
     tracks = load_eval_tracks(eval_set_folder, discr_level=discr_level)
 
 
@@ -136,9 +104,12 @@ def run(output_folder=OUTPUT_FOLDER,
     print(f"Output folder: {output_folder}")
 
     all_visited_positions = []
+    mean_devs = []
+    max_devs = []
     successes = []
     times = []
-    for track in tracks:
+
+    for track in tqdm(tracks):
         t_traj, init_wp = track, np.array([track[0].coordinate])
         config = Configuration(
             action_type=ACT,
@@ -168,19 +139,21 @@ def run(output_folder=OUTPUT_FOLDER,
                     env_factory=env_factory, eval_mode=True)
         successes.append(success)
         if success:
+            mean_dev, max_dev = compute_metrics_single(visited_positions, track)
+            mean_devs.append(mean_dev)
+            max_devs.append(max_dev)
             all_visited_positions.append(visited_positions)
             times.append(time)
 
-    mean_dev, max_dev = compute_metrics(all_visited_positions, successes, tracks)
     print("SUCCESS RATE: ", np.mean(np.array(successes)))
-    print("AVERAGE MEAN DEVIATION: ", np.mean(mean_dev))
-    print("AVERAGE MAX DEVIATION: ", np.mean(max_dev))
+    print("AVERAGE MEAN DEVIATION: ", np.mean(mean_devs))
+    print("AVERAGE MAX DEVIATION: ", np.mean(max_devs))
     print("AVERAGE TIME UNTIL LANDING: ", np.mean(times))
 
     save_benchmark({
         "success_rate": np.mean(successes),
-        "avg mean dev": np.mean(mean_dev),
-        "avg max dev": np.mean(max_dev),
+        "avg mean dev": np.mean(mean_devs),
+        "avg max dev": np.mean(max_devs),
         "avt time": np.mean(times)
     }, 
     f'rl_{discr_level}.json')
@@ -207,6 +180,7 @@ def run(output_folder=OUTPUT_FOLDER,
     parser.add_argument('--max_reward_distance',    default=MAX_REWARD_DISTANCE,    type=float,         help='number of parallel environments', metavar='')
     parser.add_argument('--waypoint_dist_tol',      default=WAYPOINT_DIST_TOL,      type=float,         help='number of parallel environments', metavar='')
     parser.add_argument('--discr_level',              default=DEFAULT_DISCR_LEVEL, type=int,           help='Whether example is being run by a notebook (default: "False")', metavar='')
+    parser.add_argument('--eval_set',              default=DEFAULT_EVAL_SET_FOLDER, type=str,           help='Whether example is being run by a notebook (default: "False")', metavar='')
 
     ARGS = parser.parse_args()
 

runnables/test_suite_eval/eval_pid.sh

@@ -2,5 +2,5 @@
 
 for level in {5..50}
 do
-  python pid.py --discr_level $level --output_folder "/shared/d/master/ws23/UAV-lab/git_repos/RL-pybullets-cf/checkpointed_models"
+  python pid.py --discr_level $level --eval_set "--eval_set "./test_suite_eval/eval-v0_n-ctrl-points-3_n-tracks-1000_2024-02-12_10:59:21_d689e67e-4179-4764-a474-e5f3237a239d"
 done

runnables/test_suite_eval/eval_rl.sh

@@ -2,5 +2,5 @@
 
 for level in {5..50}
 do
-  python pid.py --discr_level $level
+  python C_trajectory_following.py --output_folder "../../checkpointed_models/" --eval_set "./test_suite_eval/eval-v0_n-ctrl-points-3_n-tracks-1000_2024-02-12_10:59:21_d689e67e-4179-4764-a474-e5f3237a239d" --discr_level $level
 done

runnables/test_suite_eval/eval_tracks.py

@@ -60,7 +60,7 @@ def load_eval_tracks(folder: str, discr_level: int=10) -> List[DiscretizedTrajec
 
     if gen:
         t_folder = "./eval-v0"
-        n_tracks = 1
+        n_tracks = 1000
         gen_eval_tracks(t_folder, n_tracks=n_tracks)
     if load: 
         tracks = load_eval_tracks(load_folder)

runnables/test_suite_eval/pid.py

@@ -48,45 +48,14 @@
 DEFAULT_OUTPUT_FOLDER = 'results'
 DEFAULT_COLAB = False
 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
 
 def save_benchmark(benchmarks: Dict[str, float], file_path: str):
     with open(file_path, 'w') as file:
         json.dump(benchmarks, file)
 
-def compute_metrics(all_visisted_positions: np.ndarray, successes, tracks: List[DiscretizedTrajectory], n_discr_level=int(1e4)):
-
-    means = []
-    max_devs = []
-    n_fails = 0
-    for idx, success in enumerate(tqdm(successes)):
-
-        if success:
-            visited_positions = all_visisted_positions[idx - n_fails]
-            track = [wp for wp in tracks[idx]]
-            high_discr_ref_traj = TrajectoryFactory.get_pol_discretized_trajectory(
-                t_waypoints=track,
-                n_points_discretization_level=n_discr_level
-            )
-            ref_wps = np.array([wp.coordinate for wp in high_discr_ref_traj])
-
-            # metrics
-            time = len(visited_positions)
-
-            # Compute norms
-            # Reshape A and B for broadcasting, compute difference, norm, then mean across axis=1 (corresponding to M)
-            norms: np.ndarray = np.linalg.norm(visited_positions[:, np.newaxis, :] - ref_wps[np.newaxis, :, :], axis=2)
-            min_distances = norms.min(axis=1)
-            mean_dist = np.mean(min_distances)
-
-            max_dist = np.max(min_distances)
-
-            # max_dev_norms = norms.max(axis=1)
-
-            means.append(mean_dist)
-            max_devs.append(max_dist)
-        else:
-            n_fails += 1
-    return means, max_devs
 def run(
         drone=DEFAULT_DRONES,
         num_drones=DEFAULT_NUM_DRONES,
@@ -101,21 +70,23 @@ def run(
         duration_sec=DEFAULT_DURATION_SEC,
         output_folder=DEFAULT_OUTPUT_FOLDER,
         colab=DEFAULT_COLAB,
-        discr_level=DEFAULT_DISCR_LEVEL
+        discr_level=DEFAULT_DISCR_LEVEL,
+        eval_set = DEFAULT_EVAL_SET
         ):
     #### Initialize the simulation #############################
 
     #### Initialize a circular trajectory ######################
     use_gui = False
     n_discr_level=discr_level
 
-    eval_set_folder = "./eval-v0_n-ctrl-points-3_n-tracks-1000_2024-02-12_10:59:21_d689e67e-4179-4764-a474-e5f3237a239d"
+    eval_set_folder = eval_set
     tracks = load_eval_tracks(eval_set_folder, discr_level=n_discr_level)
 
-    all_visited_positions = []
+    mean_devs = []
+    max_devs = []
     successes = []
     times = []
-    for track in tracks:
+    for track in tqdm(tracks):
         current_step = 0
         INIT_RPYS = np.array([[0., 0., 0.]])
 
@@ -180,10 +151,12 @@ def run(
             velocity = np.linalg.norm(obs[0][10:13])
             if distance < 0.2 and velocity < 0.05:
                 if current_step == len(TARGET_TRAJECTORY) -1 and velocity < 0.05:
-                    env.render()
+                    # env.render()
                     all_pos = env.pos_logger.load_all() 
                     t = env.step_counter*env.PYB_TIMESTEP
-                    all_visited_positions.append(all_pos)
+                    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
@@ -193,7 +166,7 @@ def run(
             ##### Log the simulation ####################################
 
             #### Printout
-            env.render()
+            # env.render()
 
             #### Sync the simulation
             if gui and use_gui:
@@ -209,18 +182,16 @@ def run(
     logger.save()
 
 
-    avg_dev, max_dev = compute_metrics(all_visited_positions, successes, tracks)
-
     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(avg_dev))
-    print("MAXIMUM DEVIATION:", np.mean(max_dev))
+    print("AVERAGE DEVIATION: ", np.mean(mean_devs))
+    print("MAXIMUM DEVIATION:", np.mean(max_devs))
 
     save_benchmark({
         "success_rate": np.mean(successes),
-        "avg mean dev": np.mean(avg_dev),
-        "avg max dev": np.mean(max_dev),
+        "avg mean dev": np.mean(mean_devs),
+        "avg max dev": np.mean(max_devs),
         "avt time": np.mean(times)
     }, 
     f'pid_{discr_level}.json')
@@ -246,6 +217,7 @@ def run(
     parser.add_argument('--output_folder',     default=DEFAULT_OUTPUT_FOLDER, type=str,           help='Folder where to save logs (default: "results")', metavar='')
     parser.add_argument('--colab',              default=DEFAULT_COLAB, type=bool,           help='Whether example is being run by a notebook (default: "False")', metavar='')
     parser.add_argument('--discr_level',              default=DEFAULT_DISCR_LEVEL, type=int,           help='Whether example is being run by a notebook (default: "False")', metavar='')
+    parser.add_argument('--eval_set',              default=DEFAULT_EVAL_SET, type=str,           help='Whether example is being run by a notebook (default: "False")', metavar='')
     ARGS = parser.parse_args()
 
     run(**vars(ARGS))

runnables/test_suite_eval/utils.py

@@ -0,0 +1,73 @@
+import argparse
+import numpy as np
+from gym_pybullet_drones.utils.utils import str2bool
+from gym_pybullet_drones.utils.enums import ObservationType, ActionType
+from trajectories import TrajectoryFactory, DiscretizedTrajectory
+from agents.utils.configuration import Configuration
+from aviaries.factories.uzh_trajectory_follower_factory import TrajectoryFollowerAviaryFactory
+
+from agents.test_policy import run_test
+from agents.train_policy import run_train
+from runnables.test_suite_eval.eval_tracks import load_eval_tracks 
+from torch import nn
+from typing import List, Dict
+from tqdm import tqdm
+import json 
+
+def compute_metrics(all_visisted_positions: np.ndarray, successes, tracks: List[DiscretizedTrajectory], n_discr_level=int(1e4)):
+
+    means = []
+    max_devs = []
+    n_fails = 0
+    for idx, success in enumerate(tqdm(successes)):
+
+        if success:
+            visited_positions = all_visisted_positions[idx - n_fails]
+            track = [wp for wp in tracks[idx]]
+            high_discr_ref_traj = TrajectoryFactory.get_pol_discretized_trajectory(
+                t_waypoints=track,
+                n_points_discretization_level=n_discr_level
+            )
+            ref_wps = np.array([wp.coordinate for wp in high_discr_ref_traj])
+
+            # metrics
+            time = len(visited_positions)
+
+            # Compute norms
+            # Reshape A and B for broadcasting, compute difference, norm, then mean across axis=1 (corresponding to M)
+            norms: np.ndarray = np.linalg.norm(visited_positions[:, np.newaxis, :] - ref_wps[np.newaxis, :, :], axis=2)
+            min_distances = norms.min(axis=1)
+            mean_dist = np.mean(min_distances)
+
+            max_dist = np.max(min_distances)
+
+            # max_dev_norms = norms.max(axis=1)
+
+            means.append(mean_dist)
+            max_devs.append(max_dist)
+        else:
+            n_fails += 1
+    return means, max_devs
+
+def compute_metrics_single(visited_positions: np.ndarray, track: DiscretizedTrajectory, n_discr_level=int(1e4)):
+
+
+    track = [wp for wp in track]
+    high_discr_ref_traj = TrajectoryFactory.get_pol_discretized_trajectory(
+        t_waypoints=track,
+        n_points_discretization_level=n_discr_level
+    )
+    ref_wps = np.array([wp.coordinate for wp in high_discr_ref_traj])
+
+    # metrics
+    time = len(visited_positions)
+
+    # Compute norms
+    # Reshape A and B for broadcasting, compute difference, norm, then mean across axis=1 (corresponding to M)
+    norms: np.ndarray = np.linalg.norm(visited_positions[:, np.newaxis, :] - ref_wps[np.newaxis, :, :], axis=2)
+    min_distances = norms.min(axis=1)
+    mean_dist = np.mean(min_distances)
+
+    max_dist = np.max(min_distances)
+
+    return mean_dist, max_dist