diff --git a/sim/play.py b/sim/play.py index 32052bec..744425ad 100755 --- a/sim/play.py +++ b/sim/play.py @@ -144,11 +144,7 @@ def play(args: argparse.Namespace) -> None: video = cv2.VideoWriter(dir, fourcc, 50.0, (1920, 1080)) cmd_manager = CommandManager( - num_envs=env_cfg.env.num_envs, - mode=CMD_MODE, - default_cmd=DEFAULT_COMMAND, - device=env.device, - env_cfg=env_cfg + num_envs=env_cfg.env.num_envs, mode=CMD_MODE, default_cmd=DEFAULT_COMMAND, device=env.device, env_cfg=env_cfg ) for t in tqdm(range(stop_state_log)): diff --git a/sim/utils/cmd_manager.py b/sim/utils/cmd_manager.py index d223539d..88dd4f08 100644 --- a/sim/utils/cmd_manager.py +++ b/sim/utils/cmd_manager.py @@ -41,22 +41,27 @@ def __init__( self.osc_amplitude = (self.max_x_vel - self.min_x_vel) / 2 self.osc_offset = (self.max_x_vel + self.min_x_vel) / 2 elif self.mode == CommandMode.RANDOM: - self.cmd_ranges = { - 'lin_vel_x': env_cfg.commands.ranges.lin_vel_x, - 'lin_vel_y': env_cfg.commands.ranges.lin_vel_y, - 'ang_vel_yaw': env_cfg.commands.ranges.ang_vel_yaw, - 'heading': env_cfg.commands.ranges.heading - } if env_cfg else { - 'lin_vel_x': [-0.05, 0.23], - 'lin_vel_y': [-0.05, 0.05], - 'ang_vel_yaw': [-0.5, 0.5], - 'heading': [-np.pi, np.pi] - } + self.cmd_ranges = ( + { + "lin_vel_x": env_cfg.commands.ranges.lin_vel_x, + "lin_vel_y": env_cfg.commands.ranges.lin_vel_y, + "ang_vel_yaw": env_cfg.commands.ranges.ang_vel_yaw, + "heading": env_cfg.commands.ranges.heading, + } + if env_cfg + else { + "lin_vel_x": [-0.05, 0.23], + "lin_vel_y": [-0.05, 0.05], + "ang_vel_yaw": [-0.5, 0.5], + "heading": [-np.pi, np.pi], + } + ) self.resampling_time = env_cfg.commands.resampling_time if env_cfg else 8.0 self.last_sample_time = 0.0 elif self.mode == CommandMode.KEYBOARD: try: import pygame + pygame.init() pygame.display.set_mode((100, 100)) self.x_vel_cmd = 0.0 @@ -82,17 +87,27 @@ def update(self, dt: float) -> torch.Tensor: if self.time - self.last_sample_time >= self.resampling_time: self.last_sample_time = self.time # Generate random commands within training ranges - new_commands = torch.tensor([ - np.random.uniform(*self.cmd_ranges['lin_vel_x']), - np.random.uniform(*self.cmd_ranges['lin_vel_y']), - 0.0, - np.random.uniform(*self.cmd_ranges['heading']) - ], device=self.device) if self.env_cfg and self.env_cfg.commands.heading_command else torch.tensor([ - np.random.uniform(*self.cmd_ranges['lin_vel_x']), - np.random.uniform(*self.cmd_ranges['lin_vel_y']), - np.random.uniform(*self.cmd_ranges['ang_vel_yaw']), - 0.0 - ], device=self.device) + new_commands = ( + torch.tensor( + [ + np.random.uniform(*self.cmd_ranges["lin_vel_x"]), + np.random.uniform(*self.cmd_ranges["lin_vel_y"]), + 0.0, + np.random.uniform(*self.cmd_ranges["heading"]), + ], + device=self.device, + ) + if self.env_cfg and self.env_cfg.commands.heading_command + else torch.tensor( + [ + np.random.uniform(*self.cmd_ranges["lin_vel_x"]), + np.random.uniform(*self.cmd_ranges["lin_vel_y"]), + np.random.uniform(*self.cmd_ranges["ang_vel_yaw"]), + 0.0, + ], + device=self.device, + ) + ) self.commands = new_commands.repeat(self.num_envs, 1) elif self.mode == CommandMode.KEYBOARD: self._handle_keyboard_input() diff --git a/sim/utils/helpers.py b/sim/utils/helpers.py index f1530163..991390d1 100755 --- a/sim/utils/helpers.py +++ b/sim/utils/helpers.py @@ -34,6 +34,7 @@ import copy import os import random +from datetime import datetime from typing import Any, Tuple, Union import numpy as np @@ -108,18 +109,39 @@ def parse_sim_params(args, cfg): return sim_params -def get_load_path(root, load_run=-1, checkpoint=-1): +def get_load_path(root, load_run: Union[int, str] = -1, checkpoint: Union[int, str] = -1): try: runs = os.listdir(root) - # TODO sort by date to handle change of month - runs.sort() + + def parse_run_time(run_name): + try: + return datetime.strptime(run_name[:14], "%b%d_%H-%M-%S") + except: + return datetime.min + + runs.sort(key=parse_run_time) + if "exported" in runs: runs.remove("exported") - last_run = os.path.join(root, runs[-1]) - except: - raise ValueError("No runs in this directory: " + root) - if load_run == -1: - load_run = last_run + + # Keep only runs with model files + runs = [run for run in runs if any("model" in file for file in os.listdir(os.path.join(root, run)))] + if not runs: + raise ValueError("No runs with model files in this directory: " + root) + + except Exception as e: + raise ValueError("Error accessing directory: " + root) from e + + # Handle load_run selection + if isinstance(load_run, str) and load_run.lstrip("-").isdigit(): + load_run = int(load_run) + + if isinstance(load_run, int): + try: + run_name = runs[load_run] + load_run = os.path.join(root, run_name) + except IndexError: + raise ValueError(f"Run index {load_run} out of range. Available runs: {len(runs)}") else: load_run = os.path.join(root, load_run)