From 1bd396ea683c7971702f4b8017bf4ec88bccf363 Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Wed, 8 Apr 2026 22:37:36 -0700 Subject: [PATCH 1/7] Added the basic functionality for the diff drive controller to the _apply_action method of the env.py script --- .../direct/xrl_isaaclab/xrl_isaaclab_env.py | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index eab4ed7..4ca02f8 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -22,6 +22,7 @@ from isaaclab.terrains import TerrainImporterCfg from isaaclab.terrains.config import ROUGH_TERRAINS_CFG from isaaclab.sensors import RayCaster +from isaacsim.robot.wheeled_robots.controllers.differential_controller import DifferentialController def define_markers() -> VisualizationMarkers: """Define markers with various different shapes.""" @@ -67,6 +68,9 @@ def __init__(self, cfg: XrlIsaaclabEnvCfg, render_mode: str | None = None, **kwa self._success_count = torch.zeros((N,), dtype=torch.int32, device=device) self._turned_around = torch.zeros((N,), dtype=torch.bool, device=device) self._is_stuck = torch.zeros((N,), dtype=torch.bool, device=device) + wheel_radius = 0.03 + wheel_base = 0.1125 + self.controller = DifferentialController("test_controller", wheel_radius, wheel_base) def _setup_scene(self): self.robot = Articulation(self.cfg.robot_cfg) @@ -158,10 +162,23 @@ def _pre_physics_step(self, actions: torch.Tensor) -> None: #XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX v def _apply_action(self) -> None: - #Setting values to tie the wheel velocities on either side together. - left = self.actions[:,0:1] - right = self.actions[:,1:2] - expanded = torch.cat([left, right, left, right], dim=1) + # RL outputs [v, omega] + v_cmd = self.actions[:,0:1] + w_cmd = self.actions[:,1:2] + + # controller maps body commands -> wheel commands + left_vel, right_vel = self.controller.forward(v_cmd,w_cmd) + #Setup for simplified e2e + # left = self.actions[:,0:1] + # right = self.actions[:,1:2] + + # joint_targets = torch.zeros((self.cfg.scene.num_envs, self.num_wheel_joints), device=self.device) + # joint_targets[:, self.left_wheel_ids] = left_vel.unsqueeze(-1) + # joint_targets[:, self.right_wheel_ids] = right_vel.unsqueeze(-1) + + # self.robot.set_joint_velocity_target(joint_targets) + + expanded = torch.cat([left_vel, right_vel, left_vel, right_vel], dim=1) zero_expanded = torch.zeros_like(expanded) target_vel = torch.where( self.dist <= self.dist_0, @@ -169,7 +186,7 @@ def _apply_action(self) -> None: expanded ) self.robot.set_joint_velocity_target(target_vel, joint_ids=self.dof_idx) - #self.robot.set_joint_velocity_target(self.actions, joint_ids=self.dof_idx) + #self.robot.set_joint_velocity_target(self.actions, joint_ids=self.dof_idx) #needed for straight e2e def _get_observations(self) -> dict: self.forwards = math_utils.quat_apply(self.robot.data.root_link_quat_w, self.robot.data.FORWARD_VEC_B) From 43dae547b3da0b2d8d605d836f4175f3f313e7f1 Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Fri, 17 Apr 2026 01:46:26 -0700 Subject: [PATCH 2/7] updated reward signal to work with the diff drive controller. --- XRL_isaaclab/.codex | 0 XRL_isaaclab/scripts/skrl/train.py | 98 +++++++++++- XRL_isaaclab/scripts/skrl/train1.py | 143 +++++++++--------- .../direct/xrl_isaaclab/xrl_isaaclab_env.py | 62 +++++--- .../source/XRL_isaaclab/config/extension.toml | 3 +- 5 files changed, 208 insertions(+), 98 deletions(-) create mode 100644 XRL_isaaclab/.codex diff --git a/XRL_isaaclab/.codex b/XRL_isaaclab/.codex new file mode 100644 index 0000000..e69de29 diff --git a/XRL_isaaclab/scripts/skrl/train.py b/XRL_isaaclab/scripts/skrl/train.py index 4ec3ad8..038909d 100644 --- a/XRL_isaaclab/scripts/skrl/train.py +++ b/XRL_isaaclab/scripts/skrl/train.py @@ -13,7 +13,10 @@ """Launch Isaac Sim Simulator first.""" import argparse +import inspect import sys +import tokenize +from io import StringIO from isaaclab.app import AppLauncher @@ -49,6 +52,8 @@ AppLauncher.add_app_launcher_args(parser) # parse the arguments args_cli, hydra_args = parser.parse_known_args() +print(args_cli) +print("TASK:", args_cli.task) # always enable cameras to record video if args_cli.video: args_cli.enable_cameras = True @@ -59,7 +64,12 @@ # launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app - +#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX +print("TASK:", args_cli.task) +import omni.kit.app +ext_mgr = omni.kit.app.get_app().get_extension_manager() +print("wheeled robots enabled:", ext_mgr.is_extension_enabled("isaacsim.robot.wheeled_robots")) +#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX """Rest everything follows.""" import gymnasium as gym @@ -108,6 +118,89 @@ agent_cfg_entry_point = "skrl_cfg_entry_point" if algorithm in ["ppo"] else f"skrl_{algorithm}_cfg_entry_point" +def _read_text_if_exists(path: str) -> str: + if not os.path.exists(path): + return f"[missing] {path}" + with open(path, encoding="utf-8") as file: + return file.read().strip() + + +def _strip_python_comments(source: str) -> str: + tokens = tokenize.generate_tokens(StringIO(source).readline) + cleaned_tokens = [] + last_lineno = 1 + last_col = 0 + + for token_type, token_string, start, end, _ in tokens: + start_lineno, start_col = start + end_lineno, end_col = end + + if token_type == tokenize.COMMENT: + continue + + if start_lineno > last_lineno: + last_col = 0 + if start_col > last_col: + cleaned_tokens.append((tokenize.NL, " " * (start_col - last_col))) + + cleaned_tokens.append((token_type, token_string)) + last_lineno = end_lineno + last_col = end_col + + cleaned_source = tokenize.untokenize(cleaned_tokens) + cleaned_lines = [line.rstrip() for line in cleaned_source.splitlines() if line.strip()] + return "\n".join(cleaned_lines) + + +def _write_run_metadata_report(log_dir: str, env, env_cfg, agent_cfg: dict) -> None: + base_env = env.unwrapped + env_yaml_path = os.path.join(log_dir, "params", "env.yaml") + agent_yaml_path = os.path.join(log_dir, "params", "agent.yaml") + report_path = os.path.join(log_dir, "run_details.txt") + + report_sections = [ + "Run Details", + "===========", + "", + f"Task: {args_cli.task}", + f"Algorithm: {algorithm}", + f"ML framework: {args_cli.ml_framework}", + f"Seed: {env_cfg.seed}", + f"Log directory: {log_dir}", + "", + "Spaces", + "------", + f"Observation space (cfg): {getattr(env_cfg, 'observation_space', 'N/A')}", + f"Action space (cfg): {getattr(env_cfg, 'action_space', 'N/A')}", + f"State space (cfg): {getattr(env_cfg, 'state_space', 'N/A')}", + f"Gym observation space: {base_env.observation_space}", + f"Gym action space: {base_env.action_space}", + "", + "Observation Function", + "--------------------", + _strip_python_comments(inspect.getsource(type(base_env)._get_observations)), + "", + "Reward Function", + "---------------", + _strip_python_comments(inspect.getsource(type(base_env)._get_rewards)), + "", + "Environment Configuration", + "-------------------------", + _read_text_if_exists(env_yaml_path), + "", + "Agent Configuration", + "-------------------", + _read_text_if_exists(agent_yaml_path), + "", + "Experiment Configuration", + "------------------------", + str(agent_cfg.get("agent", {}).get("experiment", {})), + ] + + with open(report_path, "w", encoding="utf-8") as file: + file.write("\n".join(report_sections) + "\n") + + @hydra_task_config(args_cli.task, agent_cfg_entry_point) def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Train with skrl agent.""" @@ -163,6 +256,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + # save a human-readable run summary with reward, observation, and config details + _write_run_metadata_report(log_dir, env, env_cfg, agent_cfg) + # convert to single-agent instance if required by the RL algorithm if isinstance(env.unwrapped, DirectMARLEnv) and algorithm in ["ppo"]: env = multi_agent_to_single_agent(env) diff --git a/XRL_isaaclab/scripts/skrl/train1.py b/XRL_isaaclab/scripts/skrl/train1.py index 7d3676c..6c70e13 100644 --- a/XRL_isaaclab/scripts/skrl/train1.py +++ b/XRL_isaaclab/scripts/skrl/train1.py @@ -182,6 +182,8 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) + # base_env = env.unwrapped + # wrap around environment for skrl env = SkrlVecEnvWrapper(env, ml_framework=args_cli.ml_framework) # same as: `wrap_env(env, wrapper="auto")` @@ -202,7 +204,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: tracked_total = agent.tracking_data.get("Reward / Total reward (mean)") if tracked_total: - agent._last_tb_total_reward_mean = float(np.mean(tracked_total)) + agent._last_tb_total_reward_mean = float(tracked_total[-1]) else: agent._last_tb_total_reward_mean = None _orig_write_tracking_data(timestep, timesteps) @@ -215,24 +217,26 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: # run training timesteps = agent_cfg["trainer"]["timesteps"] - - # Early stopping based on plateau of Total Reward (mean) - # plateau_window = 300 - # plateau_patience = 2000 # number of iterations without improvement before stopping - plateau_rel_delta = 0.3 # relative improvement threshold (e.g., 0.1%) - - # total_reward_means = [] - # best_mean = -float("inf") - # plateau_counter = 0 - # min_episodes = timesteps / 10 - - # Previous early-stopping logic (kept for reference) - window = 300 - #tgt_avg = 450 - episode_returns = [] - avg_return = [] - delta_count = 0 - min_episodes = timesteps / 100 + checkpoint_dir = os.path.join(log_dir, "checkpoints") + os.makedirs(checkpoint_dir, exist_ok=True) + + # Original reward-based early stopping. + plateau_window = min(300, timesteps) + plateau_patience = 10000 + plateau_rel_delta = 1e-4 + total_reward_means = [] + best_mean = -float("inf") + plateau_counter = 0 + + # Early stopping based on plateau of rolling success rate over completed episodes (kept for reference). + # success_window = 300 + # success_patience = 5000 + # success_rate_delta = 0.001 + # episode_successes = [] + # best_success_rate = -float("inf") + # success_plateau_counter = 0 + checkpoint_interval = max(1, timesteps // 100) + early_stop_start = max(plateau_window, int(timesteps * 0.3)) for i in range(timesteps): @@ -241,61 +245,54 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: next_states, rewards, terminated, truncated, infos = trainer.train() - # # Track per-step reward mean (useful for inspection/debug) - # if hasattr(rewards, "mean"): - # step_reward_mean = float(rewards.mean()) - # else: - # step_reward_mean = float(sum(rewards) / len(rewards)) - # # Log per-step reward mean to TensorBoard (via skrl tracking) - # agent.track_data("Reward / Step reward (mean)", step_reward_mean) - - # # Track Total Reward (mean) exactly as TensorBoard logs it (raw, unsmoothed) - # tracked_total = getattr(agent, "_last_tb_total_reward_mean", None) - # if tracked_total is not None: - # total_reward_means.append(tracked_total) - - #Previous early-stopping logic (kept for reference) - episode_returns.append(rewards) - - if i % min_episodes == 0: - agent.save(os.path.join(log_dir, "checkpoints", f"checkpoint_{i}.pt")) - - # if len(total_reward_means) >= plateau_window: - # rolling_mean = sum(total_reward_means[-plateau_window:]) / plateau_window - # improvement_threshold = plateau_rel_delta * max(1.0, abs(best_mean)) - # if rolling_mean > best_mean + improvement_threshold: - # best_mean = rolling_mean - # plateau_counter = 0 - # else: - # plateau_counter += 1 - - # if plateau_counter >= plateau_patience: - # print( - # f"[INFO] Early Stop (plateau); rolling_mean = {rolling_mean:.4f}, " - # f"best_mean = {best_mean:.4f}, patience = {plateau_patience}" - # ) - # break - - # Previous early-stopping logic (kept for reference) - if len(episode_returns) > min_episodes: - avg = sum(episode_returns[-window:]) / window - if len(avg_return) == 0: - avg_return.append(avg) - else: - delta_avg = avg - avg_return[-1] - delta_percent = delta_avg.abs()/avg_return[-1].abs() - print(delta_percent) - if delta_percent < plateau_rel_delta: - delta_count += 1 - if delta_count == window: - print(f"[INFO] Early Stop; avg_reward = {avg}") - break + # Original reward-based early stopping. + tracked_total = getattr(agent, "_last_tb_total_reward_mean", None) + if tracked_total is not None: + total_reward_means.append(tracked_total) + + if len(total_reward_means) >= early_stop_start: + rolling_mean = sum(total_reward_means[-plateau_window:]) / plateau_window + improvement_threshold = 0.0 + if best_mean != -float("inf"): + improvement_threshold = plateau_rel_delta * max(1.0, abs(best_mean)) + + if best_mean == -float("inf") or rolling_mean > best_mean + improvement_threshold: + best_mean = rolling_mean + plateau_counter = 0 else: - delta_count = 0 - continue - # if avg >= tgt_avg: - # print(f"[INFO] Early Stop; avg_reward = {avg}") - # break + plateau_counter += 1 + + if plateau_counter >= plateau_patience: + print( + f"[INFO] Early Stop (plateau); rolling_mean = {rolling_mean:.4f}, " + f"best_mean = {best_mean:.4f}, patience = {plateau_patience}" + ) + break + + # Success-rate early stopping (kept for reference). + # completed_episodes = (terminated | truncated).reshape(-1) + # if completed_episodes.any(): + # success_flags = base_env.goal.reshape(-1)[completed_episodes] + # episode_successes.extend(success_flags.float().tolist()) + # + # if i + 1 >= early_stop_start and len(episode_successes) >= success_window: + # rolling_success_rate = sum(episode_successes[-success_window:]) / success_window + # + # if best_success_rate == -float("inf") or rolling_success_rate > best_success_rate + success_rate_delta: + # best_success_rate = rolling_success_rate + # success_plateau_counter = 0 + # else: + # success_plateau_counter += 1 + # + # if success_plateau_counter >= success_patience: + # print( + # f"[INFO] Early Stop (success plateau); rolling_success_rate = {rolling_success_rate:.4f}, " + # f"best_success_rate = {best_success_rate:.4f}, patience = {success_patience}" + # ) + # break + + if i % checkpoint_interval == 0: + agent.save(os.path.join(checkpoint_dir, f"checkpoint_{i}.pt")) #close the simulator env.close() diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index 4ca02f8..361efcf 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -7,6 +7,7 @@ import os import math +import numpy as np import torch from collections.abc import Sequence @@ -22,7 +23,7 @@ from isaaclab.terrains import TerrainImporterCfg from isaaclab.terrains.config import ROUGH_TERRAINS_CFG from isaaclab.sensors import RayCaster -from isaacsim.robot.wheeled_robots.controllers.differential_controller import DifferentialController +#from isaacsim.robot.wheeled_robots.controllers import DifferentialController def define_markers() -> VisualizationMarkers: """Define markers with various different shapes.""" @@ -59,6 +60,8 @@ class XrlIsaaclabEnv(DirectRLEnv): def __init__(self, cfg: XrlIsaaclabEnvCfg, render_mode: str | None = None, **kwargs): super().__init__(cfg, render_mode, **kwargs) + self._controller = None + self.dof_idx, _ = self.robot.find_joints(self.cfg.dof_names) N = self.cfg.scene.num_envs device = self.device @@ -68,9 +71,18 @@ def __init__(self, cfg: XrlIsaaclabEnvCfg, render_mode: str | None = None, **kwa self._success_count = torch.zeros((N,), dtype=torch.int32, device=device) self._turned_around = torch.zeros((N,), dtype=torch.bool, device=device) self._is_stuck = torch.zeros((N,), dtype=torch.bool, device=device) - wheel_radius = 0.03 - wheel_base = 0.1125 - self.controller = DifferentialController("test_controller", wheel_radius, wheel_base) + + + def _build_controller(self): + if self._controller is None: + from isaacsim.robot.wheeled_robots.controllers import DifferentialController + wheel_radius = 0.2 + wheel_base = 0.3765 + self._controller = DifferentialController( + name="diff_drive", + wheel_radius=wheel_radius, + wheel_base=wheel_base, + ) def _setup_scene(self): self.robot = Articulation(self.cfg.robot_cfg) @@ -144,7 +156,7 @@ def _visualize_markers(self): # offset markers so they are above the jetbot #forward_loc = self.forward_marker_location + self.marker_offset command_loc = self.command_marker_location + self.marker_offset - target_loc = self.target_marker_location + self.marker_offset # offset target marker to be above ground plane + target_loc = self.target_marker_location #+ self.marker_offset # offset target marker to be above ground plane loc = torch.vstack((command_loc, target_loc)) rots = torch.vstack((self.command_marker_orientations, self.target_marker_orientations)) @@ -163,11 +175,16 @@ def _pre_physics_step(self, actions: torch.Tensor) -> None: #XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX v def _apply_action(self) -> None: # RL outputs [v, omega] - v_cmd = self.actions[:,0:1] - w_cmd = self.actions[:,1:2] + self._build_controller() - # controller maps body commands -> wheel commands - left_vel, right_vel = self.controller.forward(v_cmd,w_cmd) + commands = self.actions[:, :2].detach().cpu().numpy() + wheel_targets = np.asarray( + [self._controller.forward(command).joint_velocities for command in commands], + dtype=np.float32, + ) + wheel_targets = torch.as_tensor(wheel_targets, device=self.device, dtype=self.actions.dtype) + left_vel = wheel_targets[:, 0:1] + right_vel = wheel_targets[:, 1:2] #Setup for simplified e2e # left = self.actions[:,0:1] # right = self.actions[:,1:2] @@ -193,13 +210,11 @@ def _get_observations(self) -> dict: self.forwards[:,-1] = 0.0 denom_for = torch.linalg.norm(self.forwards, dim=1, keepdim=True).clamp_min(1e-6) self.forwards_unit = self.forwards / denom_for - self.forwards_unit = self.forwards/torch.linalg.norm(self.forwards, dim=1, keepdim=True) self.pose = self.robot.data.root_com_pose_w[:,0:3] self.pose_target = torch.sub(self.pose_commands, self.pose) self.pose_target[:,-1] = 0.0 denom_targ = torch.linalg.norm(self.pose_target, dim=1, keepdim=True).clamp_min(1e-6) self.pose_target_unit = self.pose_target / denom_targ - self.pose_target_unit = self.pose_target/torch.linalg.norm(self.pose_target, dim=1, keepdim=True) self.dot = torch.sum(self.forwards * self.pose_target, dim=-1, keepdim=True) self.dot_norm = torch.sum(self.forwards_unit * self.pose_target_unit, dim=-1, keepdim=True) @@ -295,14 +310,15 @@ def _get_rewards(self) -> torch.Tensor: alignment_reward = self.dot_norm - is_aligned = alignment_reward >= 0.5 - scale = 0.0001 + is_aligned = alignment_reward >= 0.0 + scale = 0.001 dist_delta = (self._prev_dist - self.dist)/scale - distance_reward = torch.where( - is_aligned, - dist_delta, - torch.zeros_like(self.dist) - ) + distance_reward = dist_delta + # distance_reward = torch.where( + # is_aligned, + # dist_delta, + # 0.5 * dist_delta + # ) self._prev_dist = self.dist.detach() speed_reward = torch.sigmoid(self.forward_speed) @@ -316,14 +332,14 @@ def _get_rewards(self) -> torch.Tensor: ) - total_reward = (speed_reward * alignment_reward) + success_reward + total_reward = (alignment_reward) + distance_reward + success_reward print(f'A:{alignment_reward[0][0]} S:{speed_reward[0][0]} D:{distance_reward[0][0]} Tot:{total_reward[0][0]}') return total_reward - def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: time_out = self.episode_length_buf >= self.max_episode_length - 1 - N = self.num_envs # or self.cfg.scene.num_envs depending on your class + N = self.num_envs #calculate necessary data to determine if the vehicle is stuck no_change = 0.0005 @@ -340,7 +356,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Te # R_crit = R_crit.squeeze(-1) # P_crit = self.pitch_deg.abs() >= 0.7*self.pitch_crit_deg # P_crit = P_crit.squeeze(-1) - psi_target = torch.acos(self.dot) + psi_target = torch.acos(self.cos_psi.squeeze(-1)) psi_target_deg = torch.rad2deg(psi_target).abs() A_crit = psi_target_deg > 135 #self._turned_around = turned_around.squeeze(-1) @@ -352,7 +368,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Te self._turned_around = self._angle_count > steps_required #terminated = R_crit | P_crit | A_crit | self._is_stuck self._success_count = torch.where( - self.success, + self.success.squeeze(-1), self._success_count + 1, torch.zeros_like(self._angle_count) ) diff --git a/XRL_isaaclab/source/XRL_isaaclab/config/extension.toml b/XRL_isaaclab/source/XRL_isaaclab/config/extension.toml index ba2d85b..ae077f8 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/config/extension.toml +++ b/XRL_isaaclab/source/XRL_isaaclab/config/extension.toml @@ -20,6 +20,7 @@ keywords = ["extension", "template", "isaaclab"] "isaaclab_mimic" = {} "isaaclab_rl" = {} "isaaclab_tasks" = {} +"isaacsim.robot.wheeled_robots" = {} # NOTE: Add additional dependencies here [[python.module]] @@ -32,4 +33,4 @@ name = "XRL_isaaclab" # TODO: Uncomment and provide path to a ros_ws # with rosdeps to be installed. If none, # leave it commented out. -# ros_ws = "path/from/extension_root/to/ros_ws" \ No newline at end of file +# ros_ws = "path/from/extension_root/to/ros_ws" From ea53176c9d2e47c290be04c9b18dc445544545f3 Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Mon, 11 May 2026 15:20:59 -0700 Subject: [PATCH 3/7] Found solution for common environment. --- XRL_isaaclab/scripts/skrl/train.py | 103 ++++++++++++- XRL_isaaclab/scripts/skrl/train1.py | 137 +++++++++++++++++- .../direct/xrl_isaaclab/xrl_isaaclab_env.py | 44 +++--- 3 files changed, 257 insertions(+), 27 deletions(-) diff --git a/XRL_isaaclab/scripts/skrl/train.py b/XRL_isaaclab/scripts/skrl/train.py index 038909d..6411e6b 100644 --- a/XRL_isaaclab/scripts/skrl/train.py +++ b/XRL_isaaclab/scripts/skrl/train.py @@ -14,6 +14,7 @@ import argparse import inspect +import shlex import sys import tokenize from io import StringIO @@ -118,6 +119,7 @@ agent_cfg_entry_point = "skrl_cfg_entry_point" if algorithm in ["ppo"] else f"skrl_{algorithm}_cfg_entry_point" +# ADDED: Helpers for command metadata and success-count reporting. def _read_text_if_exists(path: str) -> str: if not os.path.exists(path): return f"[missing] {path}" @@ -152,6 +154,58 @@ def _strip_python_comments(source: str) -> str: return "\n".join(cleaned_lines) +def _write_training_command(log_dir: str, training_cfg: dict) -> None: + command = shlex.join(sys.orig_argv) + with open(os.path.join(log_dir, "command.txt"), "w", encoding="utf-8") as file: + file.write( + "\n".join( + [ + "Command", + "-------", + command, + "", + "Training Config", + "---------------", + str(training_cfg), + ] + ) + + "\n" + ) + + +def _append_success_summary( + log_dir: str, + success_iteration_count: int, + success_env_iteration_count: int, + completed_iterations: int, + configured_num_envs: int, +) -> None: + total_env_iterations = completed_iterations * configured_num_envs + success_percentage = ( + 100.0 * success_env_iteration_count / total_env_iterations if total_env_iterations > 0 else 0.0 + ) + + with open(os.path.join(log_dir, "command.txt"), "a", encoding="utf-8") as file: + file.write( + "\n".join( + [ + "", + "Success Count", + "-------------", + "Criterion: success_reward mask from _get_rewards", + f"Successful training iterations: {success_iteration_count}", + f"Successful env iterations: {success_env_iteration_count}", + f"Total env iterations: {total_env_iterations}", + f"Successful env iteration percentage: {success_percentage:.2f}%", + f"Configured num envs: {configured_num_envs}", + f"Completed training iterations: {completed_iterations}", + ] + ) + + "\n" + ) +# END ADDED: Helpers for command metadata and success-count reporting. + + def _write_run_metadata_report(log_dir: str, env, env_cfg, agent_cfg: dict) -> None: base_env = env.unwrapped env_yaml_path = os.path.join(log_dir, "params", "env.yaml") @@ -255,9 +309,13 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + # ADDED: Keep the unwrapped env so success counting can read the reward mask. + base_env = env.unwrapped # save a human-readable run summary with reward, observation, and config details _write_run_metadata_report(log_dir, env, env_cfg, agent_cfg) + # ADDED: Write command and training config details for this run. + _write_training_command(log_dir, agent_cfg["trainer"]) # convert to single-agent instance if required by the RL algorithm if isinstance(env.unwrapped, DirectMARLEnv) and algorithm in ["ppo"]: @@ -287,8 +345,49 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print(f"[INFO] Loading model checkpoint from: {resume_path}") runner.agent.load(resume_path) - # run training - runner.run() + # ADDED: Run training step-by-step so success counts can be collected. + agent = runner.agent + agents_scope = [(0, env.num_envs)] + trainer = StepTrainer(env, agent, agents_scope=agents_scope, cfg=agent_cfg["trainer"]) + timesteps = agent_cfg["trainer"]["timesteps"] + success_iteration_count = 0 + success_env_iteration_count = 0 + completed_iterations = 0 + + for i in range(timesteps): + if isinstance(trainer.agents, list): + trainer.agents = trainer.agents[0] + + trainer.train() + completed_iterations = i + 1 + + success_mask = getattr(base_env, "_last_success_reward_mask", None) + if success_mask is None: + success_mask = getattr(base_env, "success", None) + if success_mask is not None: + success_mask = success_mask.reshape(-1).bool() + successful_envs = int(success_mask.sum().item()) + success_env_iteration_count += successful_envs + if successful_envs > 0: + success_iteration_count += 1 + + configured_num_envs = int(env_cfg.scene.num_envs) + total_env_iterations = completed_iterations * configured_num_envs + success_percentage = ( + 100.0 * success_env_iteration_count / total_env_iterations if total_env_iterations > 0 else 0.0 + ) + _append_success_summary( + log_dir, + success_iteration_count, + success_env_iteration_count, + completed_iterations, + configured_num_envs, + ) + print( + f"[INFO] Success count: {success_iteration_count} training iterations, " + f"{success_env_iteration_count}/{total_env_iterations} env iterations " + f"({success_percentage:.2f}%)" + ) # close the simulator env.close() diff --git a/XRL_isaaclab/scripts/skrl/train1.py b/XRL_isaaclab/scripts/skrl/train1.py index 6c70e13..df47bee 100644 --- a/XRL_isaaclab/scripts/skrl/train1.py +++ b/XRL_isaaclab/scripts/skrl/train1.py @@ -13,7 +13,9 @@ """Launch Isaac Sim Simulator first.""" import argparse +import shlex import sys +import textwrap from isaaclab.app import AppLauncher @@ -68,6 +70,7 @@ import os import random from datetime import datetime +from pprint import pformat import skrl import numpy as np @@ -111,6 +114,85 @@ agent_cfg_entry_point = "skrl_cfg_entry_point" if algorithm in ["ppo"] else f"skrl_{algorithm}_cfg_entry_point" +# ADDED: Helpers for run metadata, reward-source capture, and success-count reporting. +def _get_method_source(obj, method_name: str) -> str: + method = getattr(type(obj), method_name, None) + if method is None: + return f"[missing] {type(obj).__name__}.{method_name}" + + try: + return textwrap.dedent(inspect.getsource(method)).strip() + except (OSError, TypeError): + return f"[unavailable] {type(obj).__name__}.{method_name}" + + +def _write_training_command( + log_dir: str, early_stopping_params: dict, training_cfg: dict, reward_details: str +) -> None: + command = shlex.join(sys.orig_argv) + early_stopping_args = " ".join( + f"{name}={value}" for name, value in early_stopping_params.items() + ) + command_with_early_stopping = f"{command} # early_stop: {early_stopping_args}" + + with open(os.path.join(log_dir, "command.txt"), "w", encoding="utf-8") as file: + file.write( + "\n".join( + [ + "Command", + "-------", + command_with_early_stopping, + "", + "Early Stopping", + "--------------", + *(f"{name}: {value}" for name, value in early_stopping_params.items()), + "", + "Training Config", + "---------------", + pformat(training_cfg, sort_dicts=False), + "", + "Reward Function (_get_rewards)", + "------------------------------", + reward_details, + ] + ) + + "\n" + ) + + +def _append_success_summary( + log_dir: str, + success_iteration_count: int, + success_env_iteration_count: int, + completed_iterations: int, + configured_num_envs: int, +) -> None: + total_env_iterations = completed_iterations * configured_num_envs + success_percentage = ( + 100.0 * success_env_iteration_count / total_env_iterations if total_env_iterations > 0 else 0.0 + ) + + with open(os.path.join(log_dir, "command.txt"), "a", encoding="utf-8") as file: + file.write( + "\n".join( + [ + "", + "Success Count", + "-------------", + "Criterion: success_reward mask from _get_rewards", + f"Successful training iterations: {success_iteration_count}", + f"Successful env iterations: {success_env_iteration_count}", + f"Total env iterations: {total_env_iterations}", + f"Successful env iteration percentage: {success_percentage:.2f}%", + f"Configured num envs: {configured_num_envs}", + f"Completed training iterations: {completed_iterations}", + ] + ) + + "\n" + ) +# END ADDED: Helpers for run metadata, reward-source capture, and success-count reporting. + + @hydra_task_config(args_cli.task, agent_cfg_entry_point) def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Train with skrl agent.""" @@ -165,6 +247,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + # ADDED: Keep the unwrapped env and capture the reward function source for command.txt. + base_env = env.unwrapped + reward_details = _get_method_source(env.unwrapped, "_get_rewards") # convert to single-agent instance if required by the RL algorithm if isinstance(env.unwrapped, DirectMARLEnv) and algorithm in ["ppo"]: @@ -182,8 +267,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen print_dict(video_kwargs, nesting=4) env = gym.wrappers.RecordVideo(env, **video_kwargs) - # base_env = env.unwrapped - # wrap around environment for skrl env = SkrlVecEnvWrapper(env, ml_framework=args_cli.ml_framework) # same as: `wrap_env(env, wrapper="auto")` @@ -221,9 +304,9 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: os.makedirs(checkpoint_dir, exist_ok=True) # Original reward-based early stopping. - plateau_window = min(300, timesteps) - plateau_patience = 10000 - plateau_rel_delta = 1e-4 + plateau_window = min(1000, timesteps) + plateau_patience = 50000 + plateau_rel_delta = 1e-6 total_reward_means = [] best_mean = -float("inf") plateau_counter = 0 @@ -236,7 +319,18 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: # best_success_rate = -float("inf") # success_plateau_counter = 0 checkpoint_interval = max(1, timesteps // 100) - early_stop_start = max(plateau_window, int(timesteps * 0.3)) + early_stop_start = int(timesteps * 0.4) + early_stopping_params = { + "plateau_window": plateau_window, + "plateau_patience": plateau_patience, + "plateau_rel_delta": plateau_rel_delta, + "early_stop_start": early_stop_start, + } + # ADDED: Write command metadata, training config, reward details, and initialize success counters. + _write_training_command(log_dir, early_stopping_params, agent_cfg["trainer"], reward_details) + success_iteration_count = 0 + success_env_iteration_count = 0 + completed_iterations = 0 for i in range(timesteps): @@ -244,6 +338,18 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: trainer.agents = trainer.agents[0] next_states, rewards, terminated, truncated, infos = trainer.train() + completed_iterations = i + 1 + + # ADDED: Count successes using the same mask that drives success_reward in _get_rewards. + success_mask = getattr(base_env, "_last_success_reward_mask", None) + if success_mask is None: + success_mask = getattr(base_env, "success", None) + if success_mask is not None: + success_mask = success_mask.reshape(-1).bool() + successful_envs = int(success_mask.sum().item()) + success_env_iteration_count += successful_envs + if successful_envs > 0: + success_iteration_count += 1 # Original reward-based early stopping. tracked_total = getattr(agent, "_last_tb_total_reward_mean", None) @@ -294,6 +400,25 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: if i % checkpoint_interval == 0: agent.save(os.path.join(checkpoint_dir, f"checkpoint_{i}.pt")) + # ADDED: Persist and print the final success-count summary. + configured_num_envs = int(env_cfg.scene.num_envs) + total_env_iterations = completed_iterations * configured_num_envs + success_percentage = ( + 100.0 * success_env_iteration_count / total_env_iterations if total_env_iterations > 0 else 0.0 + ) + _append_success_summary( + log_dir, + success_iteration_count, + success_env_iteration_count, + completed_iterations, + configured_num_envs, + ) + print( + f"[INFO] Success count: {success_iteration_count} training iterations, " + f"{success_env_iteration_count}/{total_env_iterations} env iterations " + f"({success_percentage:.2f}%)" + ) + #close the simulator env.close() diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index 361efcf..f802458 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -174,10 +174,11 @@ def _pre_physics_step(self, actions: torch.Tensor) -> None: #XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX v def _apply_action(self) -> None: - # RL outputs [v, omega] + # RL outputs [v, omega]. self._build_controller() - commands = self.actions[:, :2].detach().cpu().numpy() + actions = torch.nan_to_num(self.actions[:, :2], nan=0.0, posinf=0.0, neginf=0.0) + commands = actions.detach().cpu().numpy() wheel_targets = np.asarray( [self._controller.forward(command).joint_velocities for command in commands], dtype=np.float32, @@ -185,9 +186,16 @@ def _apply_action(self) -> None: wheel_targets = torch.as_tensor(wheel_targets, device=self.device, dtype=self.actions.dtype) left_vel = wheel_targets[:, 0:1] right_vel = wheel_targets[:, 1:2] - #Setup for simplified e2e - # left = self.actions[:,0:1] - # right = self.actions[:,1:2] + # print( + # f"raw0:{self.actions[0, 0].item():.3f} " + # f"raw1:{self.actions[0, 1].item():.3f} " + # f"cmd0:{actions[0, 0].item():.3f} " + # f"cmd1:{actions[0, 1].item():.3f} " + # f"L:{left_vel[0, 0].item():.3f} " + # f"R:{right_vel[0, 0].item():.3f} " + # f"cross:{self.cross[0, 0].item():.3f} " + # f"dotn:{self.dot_norm[0, 0].item():.3f}" + # ) # joint_targets = torch.zeros((self.cfg.scene.num_envs, self.num_wheel_joints), device=self.device) # joint_targets[:, self.left_wheel_ids] = left_vel.unsqueeze(-1) @@ -308,21 +316,17 @@ def _get_rewards(self) -> torch.Tensor: # -1*pitch_sig # ) - alignment_reward = self.dot_norm + #alignment_reward = self.dot_norm + alignment_reward = torch.exp(self.dot_norm) is_aligned = alignment_reward >= 0.0 - scale = 0.001 + scale = 0.005 dist_delta = (self._prev_dist - self.dist)/scale - distance_reward = dist_delta - # distance_reward = torch.where( - # is_aligned, - # dist_delta, - # 0.5 * dist_delta - # ) + distance_reward = torch.clamp(dist_delta, -5.0, 5.0) self._prev_dist = self.dist.detach() - speed_reward = torch.sigmoid(self.forward_speed) - #speed_reward = torch.tanh(self.forward_speed) + #speed_reward = torch.sigmoid(self.forward_speed) + speed_reward = torch.tanh(self.forward_speed) success_sig = torch.full((self.cfg.scene.num_envs,1), 100, device=self.device) success_reward = torch.where( @@ -330,10 +334,11 @@ def _get_rewards(self) -> torch.Tensor: success_sig, torch.zeros_like(self.dist) ) + # ADDED: Preserve the exact success_reward criterion for training-script success counting. + self._last_success_reward_mask = self.success.detach().clone() total_reward = (alignment_reward) + distance_reward + success_reward - print(f'A:{alignment_reward[0][0]} S:{speed_reward[0][0]} D:{distance_reward[0][0]} Tot:{total_reward[0][0]}') return total_reward def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: @@ -344,7 +349,8 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: #calculate necessary data to determine if the vehicle is stuck no_change = 0.0005 #no_change = 1e-2 #threshold to determine if there is no change from the previous timestep. currently set to 1mm - steps_required = 500 # X consecutive timesteps + steps_required = 1000 # X consecutive timesteps + steps_required_success = 250 delta = (self._prev_dist - self.dist).abs().squeeze(-1) not_moving = delta < no_change @@ -358,7 +364,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: # P_crit = P_crit.squeeze(-1) psi_target = torch.acos(self.cos_psi.squeeze(-1)) psi_target_deg = torch.rad2deg(psi_target).abs() - A_crit = psi_target_deg > 135 + A_crit = psi_target_deg > 90 #self._turned_around = turned_around.squeeze(-1) self._angle_count = torch.where( A_crit, @@ -372,7 +378,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: self._success_count + 1, torch.zeros_like(self._angle_count) ) - self.goal = self._success_count > steps_required + self.goal = self._success_count > steps_required_success terminated = self._turned_around | self._is_stuck | self.goal From c821d20f1c8ae7f4bc5f9dbc3aa92173da9e8a2a Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Tue, 12 May 2026 05:43:23 -0700 Subject: [PATCH 4/7] updated common training environment. --- .../tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index f802458..f954bd1 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -316,8 +316,8 @@ def _get_rewards(self) -> torch.Tensor: # -1*pitch_sig # ) - #alignment_reward = self.dot_norm - alignment_reward = torch.exp(self.dot_norm) + alignment_reward = self.dot_norm + #alignment_reward = torch.exp(self.dot_norm) is_aligned = alignment_reward >= 0.0 scale = 0.005 @@ -349,8 +349,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: #calculate necessary data to determine if the vehicle is stuck no_change = 0.0005 #no_change = 1e-2 #threshold to determine if there is no change from the previous timestep. currently set to 1mm - steps_required = 1000 # X consecutive timesteps - steps_required_success = 250 + steps_required = 500 # X consecutive timesteps delta = (self._prev_dist - self.dist).abs().squeeze(-1) not_moving = delta < no_change @@ -364,7 +363,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: # P_crit = P_crit.squeeze(-1) psi_target = torch.acos(self.cos_psi.squeeze(-1)) psi_target_deg = torch.rad2deg(psi_target).abs() - A_crit = psi_target_deg > 90 + A_crit = psi_target_deg >135 #self._turned_around = turned_around.squeeze(-1) self._angle_count = torch.where( A_crit, @@ -378,7 +377,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: self._success_count + 1, torch.zeros_like(self._angle_count) ) - self.goal = self._success_count > steps_required_success + self.goal = self._success_count > steps_required terminated = self._turned_around | self._is_stuck | self.goal From 24e255e0c37185b44063ed3d00dade19b01d42ca Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Wed, 13 May 2026 10:10:32 -0700 Subject: [PATCH 5/7] Added the terrain generation back into the environment. --- .../direct/xrl_isaaclab/xrl_isaaclab_env.py | 235 ++++++++---------- .../xrl_isaaclab/xrl_isaaclab_env_cfg.py | 66 ++--- 2 files changed, 142 insertions(+), 159 deletions(-) diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index f954bd1..acd4351 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -14,14 +14,12 @@ import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.envs import DirectRLEnv -from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane import isaaclab.utils.math as math_utils from .xrl_isaaclab_env_cfg import XrlIsaaclabEnvCfg -from isaaclab.terrains import TerrainImporterCfg -from isaaclab.terrains.config import ROUGH_TERRAINS_CFG from isaaclab.sensors import RayCaster #from isaacsim.robot.wheeled_robots.controllers import DifferentialController @@ -86,28 +84,23 @@ def _build_controller(self): def _setup_scene(self): self.robot = Articulation(self.cfg.robot_cfg) + self.scene.articulations["robot"] = self.robot + # add ground plane spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) - #add background - # # Terrain importer configuration - # terrain_importer_cfg = TerrainImporterCfg( - # prim_path="/World/Terrain", - # terrain_type="generator", - # terrain_generator=ROUGH_TERRAINS_CFG, # <-- REQUIRED; Adjustment made in rough.py script in the IsaacLab source files outside the current project - # #noise range = (-0.12, 0.12), noise step = 0.008, downsampled scale = 0.4; for jetbot - # #noise range = (-0.2, 0.2), noise step = 0.005, downsampled scale = 0.4; for jackal - # ) - # # Instantiate importer - # self.terrain_importer = terrain_importer_cfg.class_type(terrain_importer_cfg) - # # Auto-import happens inside __init__, so NO further calls needed + + # add raycaster for terrain-relative height measurements + self.ground_ray = RayCaster(self.cfg.ground_ray) + self.scene.sensors["ground_ray"] = self.ground_ray + + self.cfg.terrain.num_envs = self.scene.cfg.num_envs + self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + self.terrain_importer = self.cfg.terrain.class_type(self.cfg.terrain) + # clone and replicate self.scene.clone_environments(copy_from_source=False) - # add articulation to scene - self.scene.articulations["robot"] = self.robot - - # # add raycaster for depth measurments - # self.ground_ray = RayCaster(self.cfg.ground_ray) - # self.scene.sensors["ground_ray"] = self.ground_ray + if self.device == "cpu": + self.scene.filter_collisions(global_prim_paths=[self.cfg.terrain.prim_path]) # add lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) @@ -116,13 +109,12 @@ def _setup_scene(self): self.visualization_markers = define_markers() # setting aside useful variables for later - self.up_dir = torch.tensor([0.0, 0.0, 1.0]).cuda() - self.yaws = torch.zeros((self.cfg.scene.num_envs, 1)).cuda() - self.pose_commands = 2 * torch.randn((self.cfg.scene.num_envs, 3)).cuda() #set to 3 to account for the x,y, and z position data - #self.pose_commands = self.pose_commands/torch.linalg.norm(self.pose_commands, dim=1, keepdim=True) + self.up_dir = torch.tensor([0.0, 0.0, 1.0], device=self.device) + self.yaws = torch.zeros((self.cfg.scene.num_envs, 1), device=self.device) + self.offsets = self._sample_root_spawn_positions(self.cfg.scene.num_envs) + self.pose_commands = torch.zeros((self.cfg.scene.num_envs, 3), device=self.device) + self.pose_commands[:, :2] = self.offsets[:, :2] + self._sample_annulus_xy(self.cfg.scene.num_envs) self.pose_commands[:, -1] = 0.0 - self.offsets = self.scene.env_origins[:,:3].clone() #save the individual environment offsets - self.pose_commands += self.offsets ratio = self.pose_commands[:,1]/(self.pose_commands[:,0]+1E-8) gzero = torch.where(self.pose_commands > 0, True, False) @@ -132,14 +124,32 @@ def _setup_scene(self): offsets = torch.pi*plus - torch.pi*minus self.yaws = torch.atan(ratio).reshape(-1,1) + offsets.reshape(-1,1) - self.forward_marker_location = torch.zeros((self.cfg.scene.num_envs, 3)).cuda() - self.command_marker_location = torch.zeros((self.cfg.scene.num_envs, 3)).cuda() - self.target_marker_location = torch.zeros((self.cfg.scene.num_envs, 3)).cuda() - self.marker_offset = torch.zeros((self.cfg.scene.num_envs, 3)).cuda() + self.forward_marker_location = torch.zeros((self.cfg.scene.num_envs, 3), device=self.device) + self.command_marker_location = torch.zeros((self.cfg.scene.num_envs, 3), device=self.device) + self.target_marker_location = torch.zeros((self.cfg.scene.num_envs, 3), device=self.device) + self.marker_offset = torch.zeros((self.cfg.scene.num_envs, 3), device=self.device) self.marker_offset[:,-1] = 0.5 - self.forward_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4)).cuda() - self.command_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4)).cuda() - self.target_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4)).cuda() + self.forward_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4), device=self.device) + self.command_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4), device=self.device) + self.target_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4), device=self.device) + + def _sample_root_spawn_positions(self, count: int) -> torch.Tensor: + root_spawn = self.terrain_importer.flat_patches.get("root_spawn") + if root_spawn is None: + terrain_origins = self.terrain_importer.env_origins[:, :3] + if terrain_origins.shape[0] == count: + return terrain_origins.clone() + origin_ids = torch.randint(terrain_origins.shape[0], (count,), device=self.device) + return terrain_origins[origin_ids].clone() + + flat_patches = root_spawn.reshape(-1, 3) + patch_ids = torch.randint(flat_patches.shape[0], (count,), device=self.device) + return flat_patches[patch_ids].clone() + + def _sample_annulus_xy(self, count: int, min_r: float = 5.0, max_r: float = 10.0) -> torch.Tensor: + theta = 2 * torch.pi * torch.rand((count,), device=self.device) + rad = torch.sqrt(torch.rand((count,), device=self.device) * (max_r**2 - min_r**2) + min_r**2) + return torch.stack((rad * torch.cos(theta), rad * torch.sin(theta)), dim=1) def _visualize_markers(self): # get marker locations and orientations @@ -186,23 +196,6 @@ def _apply_action(self) -> None: wheel_targets = torch.as_tensor(wheel_targets, device=self.device, dtype=self.actions.dtype) left_vel = wheel_targets[:, 0:1] right_vel = wheel_targets[:, 1:2] - # print( - # f"raw0:{self.actions[0, 0].item():.3f} " - # f"raw1:{self.actions[0, 1].item():.3f} " - # f"cmd0:{actions[0, 0].item():.3f} " - # f"cmd1:{actions[0, 1].item():.3f} " - # f"L:{left_vel[0, 0].item():.3f} " - # f"R:{right_vel[0, 0].item():.3f} " - # f"cross:{self.cross[0, 0].item():.3f} " - # f"dotn:{self.dot_norm[0, 0].item():.3f}" - # ) - - # joint_targets = torch.zeros((self.cfg.scene.num_envs, self.num_wheel_joints), device=self.device) - # joint_targets[:, self.left_wheel_ids] = left_vel.unsqueeze(-1) - # joint_targets[:, self.right_wheel_ids] = right_vel.unsqueeze(-1) - - # self.robot.set_joint_velocity_target(joint_targets) - expanded = torch.cat([left_vel, right_vel, left_vel, right_vel], dim=1) zero_expanded = torch.zeros_like(expanded) target_vel = torch.where( @@ -211,7 +204,6 @@ def _apply_action(self) -> None: expanded ) self.robot.set_joint_velocity_target(target_vel, joint_ids=self.dof_idx) - #self.robot.set_joint_velocity_target(self.actions, joint_ids=self.dof_idx) #needed for straight e2e def _get_observations(self) -> dict: self.forwards = math_utils.quat_apply(self.robot.data.root_link_quat_w, self.robot.data.FORWARD_VEC_B) @@ -254,67 +246,52 @@ def _get_observations(self) -> dict: self.pitch = self.euler[1] self.pitch_deg = torch.rad2deg(self.pitch).abs().unsqueeze(-1) - # # Get the ray sensor (use ONE handle consistently) - # ray = self.scene.sensors["ground_ray"] - - # # Update it each step (your env doesn't have self.dt) - # ray.update(self.cfg.sim.dt) - - # # Hit positions (num_envs, num_rays, 3) - # ray_hits_w = ray.data.ray_hits_w - - # # One ray → ground z - # #ground_z = ray_hits_w[:, 0, 2] - # ground_z = ray_hits_w[..., 0, 2].reshape(self.num_envs) - - - # # Fallback if miss is encoded as NaN/inf - # ground_z = torch.where( - # torch.isfinite(ground_z), - # ground_z, - # self.robot.data.root_pos_w[:, 2] - 1.0 - # ) - - # self.ground_z = ground_z - - # com_z = self.robot.data.root_com_pos_w[:, 2] - # h = torch.clamp(com_z - self.ground_z, min=1e-3) - # track_width = 0.3765 - # track_width_t = torch.full_like(h, track_width) #create a tensor the same size as h with the trackwidth value - # wheel_base = 0.430 - # wheel_base_t = torch.full_like(h, wheel_base) - # roll_crit = torch.atan2(2.0*h, track_width_t) #tread (t) is the distance between the center point of both tires on one axle. 376.5 mm or 0.3765 m on the Jackal - # self.roll_crit_deg = torch.rad2deg(roll_crit).abs().unsqueeze(-1) - # pitch_crit = torch.atan2(2.0*h, wheel_base_t) - # self.pitch_crit_deg = torch.rad2deg(pitch_crit).abs().unsqueeze(-1) + ray = self.scene.sensors["ground_ray"] + ray_hits_w = ray.data.ray_hits_w + ground_z = ray_hits_w[..., 0, 2].reshape(self.num_envs) + ground_z = torch.where( + torch.isfinite(ground_z), + ground_z, + self.robot.data.root_pos_w[:, 2] - 1.0, + ) + self.ground_z = ground_z + + com_z = self.robot.data.root_com_pos_w[:, 2] + h = torch.clamp(com_z - self.ground_z, min=1e-3) + track_width = 0.3765 + track_width_t = torch.full_like(h, track_width) #create a tensor the same size as h with the trackwidth value + wheel_base = 0.430 + wheel_base_t = torch.full_like(h, wheel_base) + roll_crit = torch.atan2(2.0*h, track_width_t) #tread (t) is the distance between the center point of both tires on one axle. 376.5 mm or 0.3765 m on the Jackal + self.roll_crit_deg = torch.rad2deg(roll_crit).abs().unsqueeze(-1) + pitch_crit = torch.atan2(2.0*h, wheel_base_t) + self.pitch_crit_deg = torch.rad2deg(pitch_crit).abs().unsqueeze(-1) obs = torch.hstack((self.forward_speed, self.dot, self.cross, self.dist, self.roll_deg, self.pitch_deg)) observations = {"policy": obs} return observations def _get_rewards(self) -> torch.Tensor: - # roll_0 = 0.2 * self.roll_crit_deg #set the threshold angle for the reward to 80% of the critical roll angle - # pitch_0 = 0.2 * self.pitch_crit_deg - # r = 1-(self.roll_deg/roll_0) - # roll_sig = 1/(1+torch.exp(-r)) - # p = 1-(self.pitch_deg/pitch_0) - # pitch_sig = 1/(1+torch.exp(-p)) - - # not_rolling = self.roll_deg < roll_0 - # roll_reward = torch.where( - # not_rolling, - # #roll_sig, - # torch.zeros_like(roll_sig), - # -1*roll_sig - # ) - - # not_pitching = self.pitch_deg < pitch_0 - # pitch_reward = torch.where( - # not_pitching, - # #pitch_sig, - # torch.zeros_like(pitch_sig), - # -1*pitch_sig - # ) + roll_0 = torch.clamp(0.2 * self.roll_crit_deg, min=1.0) #set the threshold angle for the reward to 20% of the critical roll angle + pitch_0 = torch.clamp(0.2 * self.pitch_crit_deg, min=1.0) + r = 1-(self.roll_deg/roll_0) + roll_sig = 1/(1+torch.exp(-r)) + p = 1-(self.pitch_deg/pitch_0) + pitch_sig = 1/(1+torch.exp(-p)) + + not_rolling = self.roll_deg < roll_0 + roll_reward = torch.where( + not_rolling, + torch.zeros_like(roll_sig), + -1*roll_sig + ) + + not_pitching = self.pitch_deg < pitch_0 + pitch_reward = torch.where( + not_pitching, + torch.zeros_like(pitch_sig), + -1*pitch_sig + ) alignment_reward = self.dot_norm #alignment_reward = torch.exp(self.dot_norm) @@ -338,7 +315,7 @@ def _get_rewards(self) -> torch.Tensor: self._last_success_reward_mask = self.success.detach().clone() - total_reward = (alignment_reward) + distance_reward + success_reward + total_reward = (alignment_reward) + distance_reward + success_reward + roll_reward + pitch_reward return total_reward def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: @@ -357,10 +334,10 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: self._stuck_count = torch.where(not_moving, self._stuck_count + 1, torch.zeros_like(self._stuck_count)) self._is_stuck = self._stuck_count >= steps_required - # R_crit = self.roll_deg.abs() >= 0.7*self.roll_crit_deg - # R_crit = R_crit.squeeze(-1) - # P_crit = self.pitch_deg.abs() >= 0.7*self.pitch_crit_deg - # P_crit = P_crit.squeeze(-1) + R_crit = self.roll_deg.abs() >= 0.85*self.roll_crit_deg + R_crit = R_crit.squeeze(-1) + P_crit = self.pitch_deg.abs() >= 0.85*self.pitch_crit_deg + P_crit = P_crit.squeeze(-1) psi_target = torch.acos(self.cos_psi.squeeze(-1)) psi_target_deg = torch.rad2deg(psi_target).abs() A_crit = psi_target_deg >135 @@ -379,7 +356,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: ) self.goal = self._success_count > steps_required - terminated = self._turned_around | self._is_stuck | self.goal + terminated = self._turned_around | self._is_stuck | self.goal | R_crit | P_crit terminated = terminated.to(torch.bool).reshape(N) time_out = time_out.to(torch.bool).reshape(N) @@ -395,7 +372,17 @@ def _reset_idx(self, env_ids: Sequence[int] | None): if not torch.is_tensor(env_ids): env_ids = torch.as_tensor(env_ids, device=self.device, dtype=torch.long) env_ids = env_ids.reshape(-1) - self.pose = self.robot.data.root_com_pose_w[:,0:3] + + joint_pos = self.robot.data.default_joint_pos[env_ids] + joint_vel = self.robot.data.default_joint_vel[env_ids] + default_root_state = self.robot.data.default_root_state[env_ids].clone() + spawn_pos = self._sample_root_spawn_positions(env_ids.numel()) + default_root_state[:, 0:2] = spawn_pos[:, 0:2] + default_root_state[:, 2] += spawn_pos[:, 2] + self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + reset_pos = default_root_state[:, :3] # # Old rejection sampling (kept for reference) # min_r = torch.full((env_ids.numel(),), 3.0, device=self.device) @@ -411,25 +398,13 @@ def _reset_idx(self, env_ids: Sequence[int] | None): # self.pose_commands[env_ids, 2] = 0.0 # Annulus sampling: always respects minimum radius - min_r = 3.0 - max_r = 8.0 - theta = 2 * torch.pi * torch.rand((env_ids.numel(),), device=self.device) - rad = torch.sqrt( - torch.rand((env_ids.numel(),), device=self.device) * (max_r**2 - min_r**2) + min_r**2 - ) - xy = torch.stack((rad * torch.cos(theta), rad * torch.sin(theta)), dim=1) - self.pose_commands[env_ids, :2] = xy + self.pose[env_ids, :2] + self.pose_commands[env_ids, :2] = self._sample_annulus_xy(env_ids.numel()) + reset_pos[:, :2] self.pose_commands[env_ids, 2] = 0.0 #calculate distance to target - x_pose = self.pose[:,0] #column vector for all current x positions - x_commands = self.pose_commands[:,0] #column vector for all x commands - y_pose = self.pose[:,1] #column vector for all current y positions - y_commands = self.pose_commands[:,1] #column vector for all x commands - x_dif = torch.sub(x_commands,x_pose) - y_dif = torch.sub(y_commands,y_pose) - dist_all = torch.sqrt((torch.pow(x_dif,2) + torch.pow(y_dif,2))).reshape(-1, 1) - dist = dist_all[env_ids] + x_dif = torch.sub(self.pose_commands[env_ids, 0], reset_pos[:, 0]) + y_dif = torch.sub(self.pose_commands[env_ids, 1], reset_pos[:, 1]) + dist = torch.sqrt((torch.pow(x_dif, 2) + torch.pow(y_dif, 2))).reshape(-1, 1) #reset the environment buffers for determining if the robot is stuck self._prev_dist[env_ids] = dist.detach() diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py index d83377f..46b6be8 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py @@ -5,15 +5,27 @@ from XRL_isaaclab.robots.jackal_basic import JACKAL_BASIC_CONFIG -from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from copy import deepcopy + +from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg -from isaaclab.envs.common import ViewerCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationCfg -from isaaclab.utils import configclass -import math from isaaclab.sensors import RayCasterCfg from isaaclab.sensors.ray_caster.patterns.patterns_cfg import GridPatternCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.terrains.config import ROUGH_TERRAINS_CFG +from isaaclab.utils import configclass + + +XRL_ROLLING_TERRAINS_CFG = deepcopy(ROUGH_TERRAINS_CFG) +XRL_ROLLING_TERRAINS_CFG.size = (50.0, 50.0) +XRL_ROLLING_TERRAINS_CFG.num_rows = 1 +XRL_ROLLING_TERRAINS_CFG.num_cols = 1 +ROOT_SPAWN_PATCH_CFG = XRL_ROLLING_TERRAINS_CFG.sub_terrains["random_rough"].flat_patch_sampling["root_spawn"] +ROOT_SPAWN_PATCH_CFG.num_patches = 128 +ROOT_SPAWN_PATCH_CFG.x_range = (-17.0, 17.0) +ROOT_SPAWN_PATCH_CFG.y_range = (-17.0, 17.0) @configclass @@ -38,31 +50,27 @@ class XrlIsaaclabEnvCfg(DirectRLEnvCfg): ) # scene scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1, env_spacing=5.0, replicate_physics=True) - ################################################################## v - # # viewer - # viewer: ViewerCfg = ViewerCfg( - # origin_type="asset_root", - # asset_name="robot", - # env_index=0, - # eye=(3.0, 3.0, 2.0), - # lookat=(0.0, 0.0, 0.5), - # ) - dof_names = ['front_left_wheel', 'front_right_wheel', 'rear_left_wheel', 'rear_right_wheel'] - # # ray sensor - # ground_ray = RayCasterCfg( - # prim_path="/World/envs/env_.*/Robot", - # # THIS is the key line - # mesh_prim_paths=["/World/Terrain"], + terrain = TerrainImporterCfg( + prim_path="/World/Terrain", + terrain_type="generator", + terrain_generator=XRL_ROLLING_TERRAINS_CFG, + collision_group=-1, + debug_vis=False, + ) - # offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 0.5)), + dof_names = ['front_left_wheel', 'front_right_wheel', 'rear_left_wheel', 'rear_right_wheel'] - # pattern_cfg=GridPatternCfg( - # resolution=0.05, # bigger than size -> ~1 sample - # size=(0.01, 0.01), # tiny footprint - # direction=(0.0, 0.0, -1.0), - # ), - # max_distance=5.0, - # ray_alignment="world", - # ) - ################################################################## ^ + # ray sensor + ground_ray = RayCasterCfg( + prim_path="/World/envs/env_.*/Robot", + mesh_prim_paths=["/World/Terrain"], + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 0.5)), + pattern_cfg=GridPatternCfg( + resolution=0.05, + size=(0.01, 0.01), + direction=(0.0, 0.0, -1.0), + ), + max_distance=5.0, + ray_alignment="world", + ) From a5768b251b5e975b9fff27a13acea08cf75e85ae Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Mon, 25 May 2026 22:00:52 -0700 Subject: [PATCH 6/7] finished running necessary tests. --- XRL_isaaclab/scripts/skrl/train1.py | 45 ++++- .../direct/xrl_isaaclab/xrl_isaaclab_env.py | 159 ++++++++++-------- .../xrl_isaaclab/xrl_isaaclab_env_cfg.py | 75 +++++---- 3 files changed, 172 insertions(+), 107 deletions(-) diff --git a/XRL_isaaclab/scripts/skrl/train1.py b/XRL_isaaclab/scripts/skrl/train1.py index df47bee..f7bfff8 100644 --- a/XRL_isaaclab/scripts/skrl/train1.py +++ b/XRL_isaaclab/scripts/skrl/train1.py @@ -190,6 +190,38 @@ def _append_success_summary( ) + "\n" ) + + +def _append_training_reward_summary( + log_dir: str, + seed: int, + completed_episode_returns: list[float], +) -> None: + if completed_episode_returns: + total_reward_mean = float(np.mean(completed_episode_returns)) + total_reward_std = ( + float(np.std(completed_episode_returns, ddof=1)) if len(completed_episode_returns) > 1 else 0.0 + ) + else: + total_reward_mean = float("nan") + total_reward_std = float("nan") + + with open(os.path.join(log_dir, "command.txt"), "a", encoding="utf-8") as file: + file.write( + "\n".join( + [ + "", + "Training Reward Summary", + "-----------------------", + f"Training seed: {seed}", + "Metric: raw completed episode total rewards accumulated across the full training run", + f"Completed episodes: {len(completed_episode_returns)}", + f"Total reward mean: {total_reward_mean:.6f}", + f"Total reward std: {total_reward_std:.6f}", + ] + ) + + "\n" + ) # END ADDED: Helpers for run metadata, reward-source capture, and success-count reporting. @@ -280,8 +312,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen runner.agent.load(resume_path) agent = runner.agent + completed_episode_returns = [] - # Hook TensorBoard write to capture the exact scalar value that TB logs + # Hook TensorBoard write before skrl clears its tracking buffers. _orig_write_tracking_data = agent.write_tracking_data def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: @@ -290,6 +323,7 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: agent._last_tb_total_reward_mean = float(tracked_total[-1]) else: agent._last_tb_total_reward_mean = None + completed_episode_returns.extend(float(r) for r in getattr(agent, "_track_rewards", [])) _orig_write_tracking_data(timestep, timesteps) agent.write_tracking_data = _write_tracking_data_hook @@ -400,6 +434,9 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: if i % checkpoint_interval == 0: agent.save(os.path.join(checkpoint_dir, f"checkpoint_{i}.pt")) + # Include any completed episodes since the last TensorBoard write. + completed_episode_returns.extend(float(r) for r in getattr(agent, "_track_rewards", [])) + # ADDED: Persist and print the final success-count summary. configured_num_envs = int(env_cfg.scene.num_envs) total_env_iterations = completed_iterations * configured_num_envs @@ -413,11 +450,17 @@ def _write_tracking_data_hook(timestep: int, timesteps: int) -> None: completed_iterations, configured_num_envs, ) + _append_training_reward_summary(log_dir, int(agent_cfg["seed"]), completed_episode_returns) print( f"[INFO] Success count: {success_iteration_count} training iterations, " f"{success_env_iteration_count}/{total_env_iterations} env iterations " f"({success_percentage:.2f}%)" ) + if completed_episode_returns: + print( + f"[INFO] Total reward over completed episodes: mean = {np.mean(completed_episode_returns):.6f}, " + f"episodes = {len(completed_episode_returns)}" + ) #close the simulator env.close() diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index acd4351..33b4cdd 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -20,7 +20,9 @@ from .xrl_isaaclab_env_cfg import XrlIsaaclabEnvCfg -from isaaclab.sensors import RayCaster +# --- Rolling terrain sensor import (disabled for flat-plane environment) --- +# from isaaclab.sensors import RayCaster +# --- End rolling terrain sensor import --- #from isaacsim.robot.wheeled_robots.controllers import DifferentialController def define_markers() -> VisualizationMarkers: @@ -89,18 +91,19 @@ def _setup_scene(self): # add ground plane spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) - # add raycaster for terrain-relative height measurements - self.ground_ray = RayCaster(self.cfg.ground_ray) - self.scene.sensors["ground_ray"] = self.ground_ray - - self.cfg.terrain.num_envs = self.scene.cfg.num_envs - self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing - self.terrain_importer = self.cfg.terrain.class_type(self.cfg.terrain) + # --- Rolling terrain setup (disabled for flat-plane environment) --- + # self.ground_ray = RayCaster(self.cfg.ground_ray) + # self.scene.sensors["ground_ray"] = self.ground_ray + # + # self.cfg.terrain.num_envs = self.scene.cfg.num_envs + # self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + # self.terrain_importer = self.cfg.terrain.class_type(self.cfg.terrain) + # --- End rolling terrain setup --- # clone and replicate self.scene.clone_environments(copy_from_source=False) if self.device == "cpu": - self.scene.filter_collisions(global_prim_paths=[self.cfg.terrain.prim_path]) + self.scene.filter_collisions(global_prim_paths=[]) # add lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) @@ -111,7 +114,7 @@ def _setup_scene(self): # setting aside useful variables for later self.up_dir = torch.tensor([0.0, 0.0, 1.0], device=self.device) self.yaws = torch.zeros((self.cfg.scene.num_envs, 1), device=self.device) - self.offsets = self._sample_root_spawn_positions(self.cfg.scene.num_envs) + self.offsets = self.scene.env_origins[:, :3].clone() self.pose_commands = torch.zeros((self.cfg.scene.num_envs, 3), device=self.device) self.pose_commands[:, :2] = self.offsets[:, :2] + self._sample_annulus_xy(self.cfg.scene.num_envs) self.pose_commands[:, -1] = 0.0 @@ -133,18 +136,20 @@ def _setup_scene(self): self.command_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4), device=self.device) self.target_marker_orientations = torch.zeros((self.cfg.scene.num_envs, 4), device=self.device) - def _sample_root_spawn_positions(self, count: int) -> torch.Tensor: - root_spawn = self.terrain_importer.flat_patches.get("root_spawn") - if root_spawn is None: - terrain_origins = self.terrain_importer.env_origins[:, :3] - if terrain_origins.shape[0] == count: - return terrain_origins.clone() - origin_ids = torch.randint(terrain_origins.shape[0], (count,), device=self.device) - return terrain_origins[origin_ids].clone() - - flat_patches = root_spawn.reshape(-1, 3) - patch_ids = torch.randint(flat_patches.shape[0], (count,), device=self.device) - return flat_patches[patch_ids].clone() + # --- Rolling terrain flat-patch spawn sampler (disabled for flat-plane environment) --- + # def _sample_root_spawn_positions(self, count: int) -> torch.Tensor: + # root_spawn = self.terrain_importer.flat_patches.get("root_spawn") + # if root_spawn is None: + # terrain_origins = self.terrain_importer.env_origins[:, :3] + # if terrain_origins.shape[0] == count: + # return terrain_origins.clone() + # origin_ids = torch.randint(terrain_origins.shape[0], (count,), device=self.device) + # return terrain_origins[origin_ids].clone() + # + # flat_patches = root_spawn.reshape(-1, 3) + # patch_ids = torch.randint(flat_patches.shape[0], (count,), device=self.device) + # return flat_patches[patch_ids].clone() + # --- End rolling terrain flat-patch spawn sampler --- def _sample_annulus_xy(self, count: int, min_r: float = 5.0, max_r: float = 10.0) -> torch.Tensor: theta = 2 * torch.pi * torch.rand((count,), device=self.device) @@ -246,52 +251,56 @@ def _get_observations(self) -> dict: self.pitch = self.euler[1] self.pitch_deg = torch.rad2deg(self.pitch).abs().unsqueeze(-1) - ray = self.scene.sensors["ground_ray"] - ray_hits_w = ray.data.ray_hits_w - ground_z = ray_hits_w[..., 0, 2].reshape(self.num_envs) - ground_z = torch.where( - torch.isfinite(ground_z), - ground_z, - self.robot.data.root_pos_w[:, 2] - 1.0, - ) - self.ground_z = ground_z - - com_z = self.robot.data.root_com_pos_w[:, 2] - h = torch.clamp(com_z - self.ground_z, min=1e-3) - track_width = 0.3765 - track_width_t = torch.full_like(h, track_width) #create a tensor the same size as h with the trackwidth value - wheel_base = 0.430 - wheel_base_t = torch.full_like(h, wheel_base) - roll_crit = torch.atan2(2.0*h, track_width_t) #tread (t) is the distance between the center point of both tires on one axle. 376.5 mm or 0.3765 m on the Jackal - self.roll_crit_deg = torch.rad2deg(roll_crit).abs().unsqueeze(-1) - pitch_crit = torch.atan2(2.0*h, wheel_base_t) - self.pitch_crit_deg = torch.rad2deg(pitch_crit).abs().unsqueeze(-1) + # --- Rolling terrain ray-derived critical-angle observations (disabled for flat-plane environment) --- + # ray = self.scene.sensors["ground_ray"] + # ray_hits_w = ray.data.ray_hits_w + # ground_z = ray_hits_w[..., 0, 2].reshape(self.num_envs) + # ground_z = torch.where( + # torch.isfinite(ground_z), + # ground_z, + # self.robot.data.root_pos_w[:, 2] - 1.0, + # ) + # self.ground_z = ground_z + # + # com_z = self.robot.data.root_com_pos_w[:, 2] + # h = torch.clamp(com_z - self.ground_z, min=1e-3) + # track_width = 0.3765 + # track_width_t = torch.full_like(h, track_width) + # wheel_base = 0.430 + # wheel_base_t = torch.full_like(h, wheel_base) + # roll_crit = torch.atan2(2.0*h, track_width_t) + # self.roll_crit_deg = torch.rad2deg(roll_crit).abs().unsqueeze(-1) + # pitch_crit = torch.atan2(2.0*h, wheel_base_t) + # self.pitch_crit_deg = torch.rad2deg(pitch_crit).abs().unsqueeze(-1) + # --- End rolling terrain ray-derived critical-angle observations --- obs = torch.hstack((self.forward_speed, self.dot, self.cross, self.dist, self.roll_deg, self.pitch_deg)) observations = {"policy": obs} return observations def _get_rewards(self) -> torch.Tensor: - roll_0 = torch.clamp(0.2 * self.roll_crit_deg, min=1.0) #set the threshold angle for the reward to 20% of the critical roll angle - pitch_0 = torch.clamp(0.2 * self.pitch_crit_deg, min=1.0) - r = 1-(self.roll_deg/roll_0) - roll_sig = 1/(1+torch.exp(-r)) - p = 1-(self.pitch_deg/pitch_0) - pitch_sig = 1/(1+torch.exp(-p)) - - not_rolling = self.roll_deg < roll_0 - roll_reward = torch.where( - not_rolling, - torch.zeros_like(roll_sig), - -1*roll_sig - ) - - not_pitching = self.pitch_deg < pitch_0 - pitch_reward = torch.where( - not_pitching, - torch.zeros_like(pitch_sig), - -1*pitch_sig - ) + # --- Rolling terrain roll/pitch rewards (disabled for flat-plane environment) --- + # roll_0 = torch.clamp(0.2 * self.roll_crit_deg, min=1.0) + # pitch_0 = torch.clamp(0.2 * self.pitch_crit_deg, min=1.0) + # r = 1-(self.roll_deg/roll_0) + # roll_sig = 1/(1+torch.exp(-r)) + # p = 1-(self.pitch_deg/pitch_0) + # pitch_sig = 1/(1+torch.exp(-p)) + # + # not_rolling = self.roll_deg < roll_0 + # roll_reward = torch.where( + # not_rolling, + # torch.zeros_like(roll_sig), + # -1*roll_sig + # ) + # + # not_pitching = self.pitch_deg < pitch_0 + # pitch_reward = torch.where( + # not_pitching, + # torch.zeros_like(pitch_sig), + # -1*pitch_sig + # ) + # --- End rolling terrain roll/pitch rewards --- alignment_reward = self.dot_norm #alignment_reward = torch.exp(self.dot_norm) @@ -315,7 +324,7 @@ def _get_rewards(self) -> torch.Tensor: self._last_success_reward_mask = self.success.detach().clone() - total_reward = (alignment_reward) + distance_reward + success_reward + roll_reward + pitch_reward + total_reward = (alignment_reward) + distance_reward + success_reward return total_reward def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: @@ -334,10 +343,12 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: self._stuck_count = torch.where(not_moving, self._stuck_count + 1, torch.zeros_like(self._stuck_count)) self._is_stuck = self._stuck_count >= steps_required - R_crit = self.roll_deg.abs() >= 0.85*self.roll_crit_deg - R_crit = R_crit.squeeze(-1) - P_crit = self.pitch_deg.abs() >= 0.85*self.pitch_crit_deg - P_crit = P_crit.squeeze(-1) + # --- Rolling terrain roll/pitch terminations (disabled for flat-plane environment) --- + # R_crit = self.roll_deg.abs() >= 0.85*self.roll_crit_deg + # R_crit = R_crit.squeeze(-1) + # P_crit = self.pitch_deg.abs() >= 0.85*self.pitch_crit_deg + # P_crit = P_crit.squeeze(-1) + # --- End rolling terrain roll/pitch terminations --- psi_target = torch.acos(self.cos_psi.squeeze(-1)) psi_target_deg = torch.rad2deg(psi_target).abs() A_crit = psi_target_deg >135 @@ -356,7 +367,7 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: ) self.goal = self._success_count > steps_required - terminated = self._turned_around | self._is_stuck | self.goal | R_crit | P_crit + terminated = self._turned_around | self._is_stuck | self.goal terminated = terminated.to(torch.bool).reshape(N) time_out = time_out.to(torch.bool).reshape(N) @@ -376,9 +387,12 @@ def _reset_idx(self, env_ids: Sequence[int] | None): joint_pos = self.robot.data.default_joint_pos[env_ids] joint_vel = self.robot.data.default_joint_vel[env_ids] default_root_state = self.robot.data.default_root_state[env_ids].clone() - spawn_pos = self._sample_root_spawn_positions(env_ids.numel()) - default_root_state[:, 0:2] = spawn_pos[:, 0:2] - default_root_state[:, 2] += spawn_pos[:, 2] + default_root_state[:, :3] += self.scene.env_origins[env_ids] + # --- Rolling terrain flat-patch reset spawn (disabled for flat-plane environment) --- + # spawn_pos = self._sample_root_spawn_positions(env_ids.numel()) + # default_root_state[:, 0:2] = spawn_pos[:, 0:2] + # default_root_state[:, 2] += spawn_pos[:, 2] + # --- End rolling terrain flat-patch reset spawn --- self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) @@ -411,6 +425,7 @@ def _reset_idx(self, env_ids: Sequence[int] | None): self._stuck_count[env_ids] = 0 self._is_stuck[env_ids] = False self._angle_count[env_ids] = 0 + self._success_count[env_ids] = 0 self._turned_around[env_ids] = False # recalculate the orientations for the command markers with the new commands @@ -425,6 +440,7 @@ def _reset_idx(self, env_ids: Sequence[int] | None): #self.yaws[env_ids] = torch.atan(ratio).reshape(-1,1) + offsets.reshape(-1,1) self.yaws[env_ids, 0] = torch.atan(ratio) + offsets + # --- Rolling terrain legacy flat-patch reset path (disabled for flat-plane environment) --- # # set the root state for the reset envs # fp = self.terrain_importer.flat_patches # default_root_state = self.robot.data.default_root_state[env_ids] @@ -463,4 +479,5 @@ def _reset_idx(self, env_ids: Sequence[int] | None): # self.pose_commands[env_ids] = new_cmds # default_root_state[:, :3] += self.scene.env_origins[env_ids] # self.robot.write_root_state_to_sim(default_root_state, env_ids) + # --- End rolling terrain legacy flat-patch reset path --- self._visualize_markers() diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py index 46b6be8..471026f 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py @@ -5,27 +5,29 @@ from XRL_isaaclab.robots.jackal_basic import JACKAL_BASIC_CONFIG -from copy import deepcopy - from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationCfg -from isaaclab.sensors import RayCasterCfg -from isaaclab.sensors.ray_caster.patterns.patterns_cfg import GridPatternCfg -from isaaclab.terrains import TerrainImporterCfg -from isaaclab.terrains.config import ROUGH_TERRAINS_CFG from isaaclab.utils import configclass -XRL_ROLLING_TERRAINS_CFG = deepcopy(ROUGH_TERRAINS_CFG) -XRL_ROLLING_TERRAINS_CFG.size = (50.0, 50.0) -XRL_ROLLING_TERRAINS_CFG.num_rows = 1 -XRL_ROLLING_TERRAINS_CFG.num_cols = 1 -ROOT_SPAWN_PATCH_CFG = XRL_ROLLING_TERRAINS_CFG.sub_terrains["random_rough"].flat_patch_sampling["root_spawn"] -ROOT_SPAWN_PATCH_CFG.num_patches = 128 -ROOT_SPAWN_PATCH_CFG.x_range = (-17.0, 17.0) -ROOT_SPAWN_PATCH_CFG.y_range = (-17.0, 17.0) +# --- Rolling terrain configuration (disabled for flat-plane environment) --- +# from copy import deepcopy +# from isaaclab.sensors import RayCasterCfg +# from isaaclab.sensors.ray_caster.patterns.patterns_cfg import GridPatternCfg +# from isaaclab.terrains import TerrainImporterCfg +# from isaaclab.terrains.config import ROUGH_TERRAINS_CFG +# +# XRL_ROLLING_TERRAINS_CFG = deepcopy(ROUGH_TERRAINS_CFG) +# XRL_ROLLING_TERRAINS_CFG.size = (50.0, 50.0) +# XRL_ROLLING_TERRAINS_CFG.num_rows = 1 +# XRL_ROLLING_TERRAINS_CFG.num_cols = 1 +# ROOT_SPAWN_PATCH_CFG = XRL_ROLLING_TERRAINS_CFG.sub_terrains["random_rough"].flat_patch_sampling["root_spawn"] +# ROOT_SPAWN_PATCH_CFG.num_patches = 128 +# ROOT_SPAWN_PATCH_CFG.x_range = (-17.0, 17.0) +# ROOT_SPAWN_PATCH_CFG.y_range = (-17.0, 17.0) +# --- End rolling terrain configuration --- @configclass @@ -49,28 +51,31 @@ class XrlIsaaclabEnvCfg(DirectRLEnvCfg): ), ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1, env_spacing=5.0, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=100, env_spacing=5.0, replicate_physics=True) - terrain = TerrainImporterCfg( - prim_path="/World/Terrain", - terrain_type="generator", - terrain_generator=XRL_ROLLING_TERRAINS_CFG, - collision_group=-1, - debug_vis=False, - ) + # --- Rolling terrain importer (disabled for flat-plane environment) --- + # terrain = TerrainImporterCfg( + # prim_path="/World/Terrain", + # terrain_type="generator", + # terrain_generator=XRL_ROLLING_TERRAINS_CFG, + # collision_group=-1, + # debug_vis=False, + # ) + # --- End rolling terrain importer --- dof_names = ['front_left_wheel', 'front_right_wheel', 'rear_left_wheel', 'rear_right_wheel'] - # ray sensor - ground_ray = RayCasterCfg( - prim_path="/World/envs/env_.*/Robot", - mesh_prim_paths=["/World/Terrain"], - offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 0.5)), - pattern_cfg=GridPatternCfg( - resolution=0.05, - size=(0.01, 0.01), - direction=(0.0, 0.0, -1.0), - ), - max_distance=5.0, - ray_alignment="world", - ) + # --- Rolling terrain ground ray (disabled for flat-plane environment) --- + # ground_ray = RayCasterCfg( + # prim_path="/World/envs/env_.*/Robot", + # mesh_prim_paths=["/World/Terrain"], + # offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 0.5)), + # pattern_cfg=GridPatternCfg( + # resolution=0.05, + # size=(0.01, 0.01), + # direction=(0.0, 0.0, -1.0), + # ), + # max_distance=5.0, + # ray_alignment="world", + # ) + # --- End rolling terrain ground ray --- From 2d0184b96aba1056d9925a960822f025417d1f52 Mon Sep 17 00:00:00 2001 From: jrshs79 Date: Mon, 25 May 2026 23:53:13 -0700 Subject: [PATCH 7/7] ran playback on the previous tests. --- .../XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py | 3 ++- .../tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py index 33b4cdd..e794010 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env.py @@ -324,7 +324,8 @@ def _get_rewards(self) -> torch.Tensor: self._last_success_reward_mask = self.success.detach().clone() - total_reward = (alignment_reward) + distance_reward + success_reward + total_reward = alignment_reward + distance_reward + success_reward + print(f"A:{alignment_reward[0][0]}, D:{distance_reward[0][0]}, S:{success_reward[0][0]}") return total_reward def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: diff --git a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py index 471026f..9c69d5a 100644 --- a/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py +++ b/XRL_isaaclab/source/XRL_isaaclab/XRL_isaaclab/tasks/direct/xrl_isaaclab/xrl_isaaclab_env_cfg.py @@ -51,7 +51,7 @@ class XrlIsaaclabEnvCfg(DirectRLEnvCfg): ), ) # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=100, env_spacing=5.0, replicate_physics=True) + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1, env_spacing=5.0, replicate_physics=True) # --- Rolling terrain importer (disabled for flat-plane environment) --- # terrain = TerrainImporterCfg(