From a7167bbca12b6c9b3154a0db54ed2996c332d4cc Mon Sep 17 00:00:00 2001 From: binbash Date: Mon, 12 Feb 2024 11:59:02 +0100 Subject: [PATCH] removed unnecessary logs --- agents/test_policy.py | 9 +-- aviaries/rewards/uzh_trajectory_reward.py | 2 +- gym_pybullet_drones/utils/Logger.py | 3 +- .../test_suite_eval/C_trajectory_following.py | 60 +++++---------- runnables/test_suite_eval/eval_pid.sh | 2 +- runnables/test_suite_eval/eval_rl.sh | 2 +- runnables/test_suite_eval/eval_tracks.py | 2 +- runnables/test_suite_eval/pid.py | 66 +++++------------ runnables/test_suite_eval/utils.py | 73 +++++++++++++++++++ 9 files changed, 118 insertions(+), 101 deletions(-) create mode 100644 runnables/test_suite_eval/utils.py diff --git a/agents/test_policy.py b/agents/test_policy.py index 3fb152a8..1d6bdccd 100644 --- a/agents/test_policy.py +++ b/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 diff --git a/aviaries/rewards/uzh_trajectory_reward.py b/aviaries/rewards/uzh_trajectory_reward.py index ec7f6ee7..97d84fc0 100644 --- a/aviaries/rewards/uzh_trajectory_reward.py +++ b/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 diff --git a/gym_pybullet_drones/utils/Logger.py b/gym_pybullet_drones/utils/Logger.py index 191557de..6af2387a 100644 --- a/gym_pybullet_drones/utils/Logger.py +++ b/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 diff --git a/runnables/test_suite_eval/C_trajectory_following.py b/runnables/test_suite_eval/C_trajectory_following.py index 4dc9a54c..58258eeb 100644 --- a/runnables/test_suite_eval/C_trajectory_following.py +++ b/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() diff --git a/runnables/test_suite_eval/eval_pid.sh b/runnables/test_suite_eval/eval_pid.sh index 4a48cc68..6c3fc062 100755 --- a/runnables/test_suite_eval/eval_pid.sh +++ b/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 \ No newline at end of file diff --git a/runnables/test_suite_eval/eval_rl.sh b/runnables/test_suite_eval/eval_rl.sh index 3b7958fb..7ee6d4ce 100644 --- a/runnables/test_suite_eval/eval_rl.sh +++ b/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 \ No newline at end of file diff --git a/runnables/test_suite_eval/eval_tracks.py b/runnables/test_suite_eval/eval_tracks.py index 7d28029c..9d631b15 100644 --- a/runnables/test_suite_eval/eval_tracks.py +++ b/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) diff --git a/runnables/test_suite_eval/pid.py b/runnables/test_suite_eval/pid.py index 70242ea9..22dcbaa2 100644 --- a/runnables/test_suite_eval/pid.py +++ b/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,7 +70,8 @@ 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 ############################# @@ -109,13 +79,14 @@ def run( 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)) diff --git a/runnables/test_suite_eval/utils.py b/runnables/test_suite_eval/utils.py new file mode 100644 index 00000000..a9289bce --- /dev/null +++ b/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 \ No newline at end of file