diff --git a/source/wheeledlab/wheeledlab/envs/mdp/curriculums.py b/source/wheeledlab/wheeledlab/envs/mdp/curriculums.py index 5d0de43..2c92241 100644 --- a/source/wheeledlab/wheeledlab/envs/mdp/curriculums.py +++ b/source/wheeledlab/wheeledlab/envs/mdp/curriculums.py @@ -33,3 +33,33 @@ def increase_reward_weight_over_time( term_cfg = env.reward_manager.get_term_cfg(reward_term_name) term_cfg.weight += increase env.reward_manager.set_term_cfg(reward_term_name, term_cfg) + + +def decrease_reward_weight_over_time( + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + reward_term_name : str, + decrease : float, + episodes_per_decrease : int = 1, + max_decreases: int = torch.inf, + lower_limit: float = 0. + ) -> torch.Tensor: + """ + Decrease the weight of a reward term after some amount of given time in episodes. + Default amount of time is one episode. + Stops decreasing the weight after `stop_after_n_changes` changes (defaults to inf) OR + after lower_limit is reached (defaults to 0), whichever comes first. + """ + num_episodes = env.common_step_counter // env.max_episode_length + num_decreases = num_episodes // episodes_per_decrease + term_cfg = env.reward_manager.get_term_cfg(reward_term_name) + + if num_decreases > max_decreases or term_cfg.weight <= lower_limit: + return # do nothing + + if env.common_step_counter % env.max_episode_length != 0: + return # only process at the beginning of an episode (not per step) + + if (num_episodes + 1) % episodes_per_decrease == 0: # discount the first episode + term_cfg.weight = max(term_cfg.weight - decrease, lower_limit) + env.reward_manager.set_term_cfg(reward_term_name, term_cfg) diff --git a/source/wheeledlab_tasks/wheeledlab_tasks/elevation/mushr_elevation_env_cfg.py b/source/wheeledlab_tasks/wheeledlab_tasks/elevation/mushr_elevation_env_cfg.py index 3619434..d589640 100644 --- a/source/wheeledlab_tasks/wheeledlab_tasks/elevation/mushr_elevation_env_cfg.py +++ b/source/wheeledlab_tasks/wheeledlab_tasks/elevation/mushr_elevation_env_cfg.py @@ -1,13 +1,19 @@ import torch +import numpy as np +import noise + +from dataclasses import MISSING import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +from isaaclab.utils import configclass from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.assets import AssetBaseCfg, ArticulationCfg from isaaclab.scene import InteractiveSceneCfg -from isaaclab.terrains import TerrainImporterCfg -from isaaclab.utils import configclass +from isaaclab.terrains import TerrainImporterCfg, TerrainGeneratorCfg, FlatPatchSamplingCfg +from isaaclab.terrains.height_field import HfTerrainBaseCfg +from isaaclab.terrains.height_field.utils import height_field_to_mesh from isaaclab.sensors import RayCasterCfg, patterns import isaaclab.envs.mdp as mdp @@ -28,13 +34,13 @@ from isaaclab.envs import ManagerBasedEnv from isaaclab.assets import Articulation, RigidObject -from isaaclab.envs.mdp.commands import UniformPose2dCommandCfg -from isaaclab.envs.mdp.events import reset_root_state_uniform +from isaaclab.envs.mdp.commands import TerrainBasedPose2dCommandCfg, UniformPose2dCommandCfg +from isaaclab.envs.mdp.events import reset_root_state_from_terrain, reset_root_state_uniform from wheeledlab_assets import WHEELEDLAB_ASSETS_DATA_DIR from wheeledlab.envs.mdp.observations import root_euler_xyz from wheeledlab_assets.mushr import MUSHR_SUS_CFG -from wheeledlab.envs.mdp import increase_reward_weight_over_time +from wheeledlab.envs.mdp import decrease_reward_weight_over_time from wheeledlab_tasks.common import Mushr4WDActionCfg # ########################## @@ -65,12 +71,16 @@ class ConcatObs(ObsGroup): world_euler_xyz = ObsTerm( func=root_euler_xyz, ) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel, clip=(-10., 10.)) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, clip=(-10., 10.)) + last_action = ObsTerm( func=mdp.last_action, clip=(-1., 1.) ) + elevation_map = ObsTerm( func=world_height_map, params={ @@ -87,13 +97,260 @@ def __post_init__(self) -> None: policy: ConcatObs = ConcatObs() -########################## -### Scene Setup Config ### -########################## +######################### +###### SCENE SETUP ###### +######################### + +#==== SUBTERRAIN CFGS ==== + +@configclass +class NoiseHfCfg(HfTerrainBaseCfg): + """Config for noise-based terrain generation.""" + + generate_roads: bool = True + octaves: int = 3 # layers of noise; increase for more complex terrain + freq: float = 250.0 # higher = smoother/wider hills + + # clipping helps create some flat surfaces and plateaus + upper_clip_prop: float = 0.25 # clips hf by upper_clip_prop proportion of ht + lower_clip_prop: float = 0.25 # clips hf by lower_clip_prop proportion of ht + vary_ht: bool = False # if true, linearly increases ht as x, y increase. + ht_range: tuple[float, float] = (0.3, 1.4) # range across difficulties. Represents max height before clipping + + + flat_patch_sampling = { + "init_pos" : # reset + FlatPatchSamplingCfg( + num_patches = 256, + patch_radius = 0.16, + max_height_diff = 0.01, # 1cm height variation + x_range = (-4., 4.), + y_range = (-4., 4.), + #z_range = () + ), + "target" : # goal pose sampling + FlatPatchSamplingCfg( + num_patches = 512, + patch_radius = 0.16, + max_height_diff = 0.1, + x_range = (-4.5, 4.5), + y_range = (-4.5, 4.5), + ) + } + + +def add_roads_v2(hf, difficulty, horizontal_scale, vertical_scale): + """Returns a new hf with a road.""" + # === Setup === + rows, cols = hf.shape + num_nodes = 20 + road_width_px = (0.5 - 0.2 * difficulty) / horizontal_scale + shoulder_width_px = 10.0 + hf = hf.astype(float) + + # === Random walk === + current_row = rows / 2 # road starts from center for now + current_col = cols / 2 + heading = np.random.uniform(0, 2 * np.pi) + + step_dist = rows * 0.8 / num_nodes + key_pts = [[current_row, current_col]] + + for _ in range(num_nodes - 1): + best_heading = heading + min_effort = float("inf") + + candidates = np.linspace(-np.pi / 4, np.pi / 4, 5) + + for angle_off in candidates: + test_h = heading + angle_off + + test_row = current_row + step_dist * np.sin(test_h) + test_col = current_col + step_dist * np.cos(test_h) + + test_row = np.clip(test_row, 0, rows - 1) + test_col = np.clip(test_col, 0, cols - 1) + + target_z = hf[int(test_row), int(test_col)] + current_z = hf[int(current_row), int(current_col)] + + effort = abs(target_z - current_z) + + # try to keep road near center + dist_to_center = np.sqrt( + (test_row - rows / 2)**2 + + (test_col - cols / 2)**2 + ) + effort += dist_to_center * 0.01 + + if effort < min_effort: + min_effort = effort + best_heading = test_h + + heading = best_heading + + current_row += step_dist * np.sin(heading) + current_col += step_dist * np.cos(heading) + + if (current_col >= cols or current_row >= rows): + break + + key_pts.append([current_row, current_col]) + + # === Smooth corners === + key_pts = np.array(key_pts) + + dense_path = [] + for i in range(len(key_pts) - 1): + segment = np.linspace(key_pts[i], key_pts[i + 1], 200) + dense_path.append(segment[:-1]) + + path = np.concatenate(dense_path) + + window = int(100 * (1.0 - 0.2 * difficulty)) + window = max(window, 5) + + path_row = np.convolve(path[:, 0], np.ones(window) / window, mode = "valid") + path_col = np.convolve(path[:, 1], np.ones(window) / window, mode = "valid") + + path_row = np.clip(path_row, 0, rows - 1) + path_col = np.clip(path_col, 0, cols - 1) + + path_array = np.stack([path_row, path_col], axis = 1) + + r = np.arange(rows) + c = np.arange(cols) + grid_row, grid_col = np.meshgrid(r, c, indexing = "ij") + + # === Smooth elevation along road === + road_z_raw = hf[path_row.astype(int), path_col.astype(int)] + + z_win = 20 + kernel = np.ones(z_win) / z_win + road_z = np.convolve(road_z_raw, kernel, mode = "same") + road_z[:z_win] = road_z_raw[:z_win] + road_z[-z_win:] = road_z_raw[-z_win:] + + # === Build full distance field === + road_points = path_array[::5] + road_heights = road_z[::5] + + distance_field = np.full((rows, cols), np.inf) + height_field = np.zeros((rows, cols)) + + for (r, c), z in zip(road_points, road_heights): + dist = np.sqrt((grid_row - r)**2 + (grid_col - c)**2) + + mask = dist < distance_field + + height_field[mask] = z + distance_field[mask] = dist[mask] + + # === Smooth blending === + core = distance_field <= road_width_px + shoulder = (distance_field > road_width_px) & ( + distance_field <= road_width_px + shoulder_width_px + ) + + new_hf = hf.copy() + new_hf[core] = height_field[core] + t = (distance_field[shoulder] - road_width_px) / shoulder_width_px + smooth = 3 * t**2 - 2 * t**3 + + new_hf[shoulder] = ( + height_field[shoulder] * (1 - smooth) + + hf[shoulder] * smooth + ) + + return new_hf + +@height_field_to_mesh +def create_noise_hf(difficulty: float, cfg: NoiseHfCfg): + """Procedurally generates height field using simplex noise.""" + # === Initialize === + rows = int(cfg.size[0] / cfg.horizontal_scale) + cols = int(cfg.size[1] / cfg.horizontal_scale) + curr_ht_mult = cfg.ht_range[0] + (cfg.ht_range[1] - cfg.ht_range[0]) * difficulty + curr_max_height = curr_ht_mult * (1.0 - cfg.upper_clip_prop) + curr_floor_clip = curr_ht_mult * cfg.lower_clip_prop + max_ht_adj = curr_max_height / cfg.vertical_scale + ht_mult_adj = curr_ht_mult / cfg.vertical_scale + min_ht_adj = curr_floor_clip / cfg.vertical_scale + offset = 10000.0 * np.random.rand() + + # == Raw noise == + x = np.linspace(0.0, rows, rows) + y = np.linspace(0.0, cols, cols) + xv, yv = np.meshgrid(x, y, indexing = 'ij') + + v_snoise2 = np.vectorize(lambda x, y: noise.snoise2( + (x + offset) / cfg.freq, + (y + offset) / cfg.freq, + octaves = cfg.octaves + )) + + hf = v_snoise2(xv, yv) + hf = (hf + 1.0) / 2.0 # Raw noise is [-1, 1] + + # == Scaling & clipping == + rx = np.linspace(0.2, 1.0, rows) + ry = np.linspace(0.2, 1.0, cols) + rxv, ryv = np.meshgrid(rx, ry, indexing = "ij") + ramp = (rxv + ryv) * 0.5 + + if cfg.vary_ht: + hf = hf * ht_mult_adj * ramp + else: + hf = hf * ht_mult_adj + + hf = np.clip(hf, min_ht_adj, max_ht_adj) + + if cfg.generate_roads: + hf = add_roads_v2(hf, difficulty, cfg.horizontal_scale, cfg.vertical_scale) + + return (hf - min_ht_adj).astype(np.float32) + +#==== TERRAIN CFGS ==== +@configclass +class ElevationTerrainGeneratorCfg(TerrainGeneratorCfg): + vertical_scale = 0.005 + horizontal_scale = 0.02 + border_width = 1.0 + border_height = - 7 + slope_threshold = .5 + + size = (10.0, 10.0) + num_rows = 4 + num_cols = 4 + + curriculum = True + + sub_terrains = { + "noise_terrain" : NoiseHfCfg( + proportion = 1.0, + function = create_noise_hf, + vertical_scale = vertical_scale, + horizontal_scale = horizontal_scale + ) + } @configclass class ElevationTerrainImporterCfg(TerrainImporterCfg): + prim_path = "/World/elevation" + terrain_type = "generator" + terrain_generator = ElevationTerrainGeneratorCfg() + physics_material = sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ) + debug_vis=False + +@configclass +class ElevationUSDTerrainImporterCfg(TerrainImporterCfg): # old + height = 0.25 prim_path="/World/elevation" terrain_type = "usd" @@ -112,6 +369,30 @@ class ElevationTerrainImporterCfg(TerrainImporterCfg): class ElevationSceneCfg(InteractiveSceneCfg): terrain = ElevationTerrainImporterCfg() + + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + robot: ArticulationCfg = MUSHR_SUS_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + height_scanner = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot/mushr_nano/base_link", + offset=RayCasterCfg.OffsetCfg( + pos=(0.0, 0.0, 20.0), + # rot=(0.0, 1.0, 0.0, 0.0), + ), + attach_yaw_only=True, + pattern_cfg=patterns.GridPatternCfg(size=[2.5, 2.5], resolution=0.1), + debug_vis=False, + mesh_prim_paths=["/World/elevation/terrain"], + ) + +@configclass +class ElevationUSDSceneCfg(InteractiveSceneCfg): # old + + terrain = ElevationUSDTerrainImporterCfg() light = AssetBaseCfg( prim_path="/World/light", spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), @@ -186,21 +467,20 @@ def steep_penalty(env, thresh_pitch): return steep_ramp def elevation_continuity(env, threshold_elev): - pos = mdp.root_pos_w(env) - z_value = pos[..., 2] - 0.19 # Base elevation adjustment - # print((z_value > 0.1).sum()) - if not hasattr(elevation_continuity, "prev_elevation"): - elevation_continuity.prev_elevation = z_value.clone() - delta_z = z_value - elevation_continuity.prev_elevation - on_ramp = z_value > threshold_elev - ascending = delta_z > 0 - descending = delta_z < 0 - rew_ascend = torch.where(on_ramp & ascending, 50*delta_z, torch.zeros_like(z_value)) - rew_descend = torch.where(on_ramp & descending, -50*delta_z, torch.zeros_like(z_value)) # Note: -delta_z makes it positive - rew = rew_ascend + rew_descend - elevation_continuity.prev_elevation = z_value.clone() - return rew - + pos = mdp.root_pos_w(env) + z_value = pos[..., 2] - 0.19 # Base elevation adjustment + # print((z_value > 0.1).sum()) + if not hasattr(elevation_continuity, "prev_elevation"): + elevation_continuity.prev_elevation = z_value.clone() + delta_z = z_value - elevation_continuity.prev_elevation + on_ramp = z_value > threshold_elev + ascending = delta_z > 0 + descending = delta_z < 0 + rew_ascend = torch.where(on_ramp & ascending, 50*delta_z, torch.zeros_like(z_value)) + rew_descend = torch.where(on_ramp & descending, -50*delta_z, torch.zeros_like(z_value)) # Note: -delta_z makes it positive + rew = rew_ascend + rew_descend + elevation_continuity.prev_elevation = z_value.clone() + return rew def yaw_change_onElev(env, threshold_yaw, threshold_z): pos = mdp.root_pos_w(env) @@ -210,10 +490,6 @@ def yaw_change_onElev(env, threshold_yaw, threshold_z): rew = torch.where(condition, 2*abs(ang_vel_yaw)**2, torch.zeros_like(ang_vel_yaw)) return rew -############################# -########## REWARDS ########## -############################# - def upright_penalty(env, thresh_deg): rot_mat = math_utils.matrix_from_quat(mdp.root_quat_w(env)) up_dot = rot_mat[:, 2, 2] @@ -227,7 +503,6 @@ def roll_on_elev(env, z_start, roll_rate_thresh): z_value = pos[..., 2] - 0.19 condition = (z_value > z_start) & (abs(ang_vel_roll)>roll_rate_thresh) rew = torch.where(condition, abs(ang_vel_roll)*2.0, torch.zeros_like(ang_vel_roll)) - # elev_map_visualizer(env) return rew def is_falling_penalty(env, map_length_px=26, sensor_cfg=SceneEntityCfg("height_scanner")): @@ -246,7 +521,18 @@ def goal_progress_rate(env): goal_vector = goal_pos - pos[:, :2] proj_scal = torch.sum(vel_vector * goal_vector, dim=-1) / torch.norm(goal_vector, dim=-1) - return 5 + proj_scal + return proj_scal + +def goal_progress(env): + pos = mdp.root_pos_w(env) + goal_pos = mdp.generated_commands(env, "goal_pose")[:, :2] + dist = torch.norm(goal_pos - pos[:, :2], dim=-1) # next-step distance + vel = mdp.root_lin_vel_w(env)[:, :2] + dt = env.step_dt + next_pos = pos[:, :2] + vel * dt + next_dist = torch.norm(goal_pos - next_pos, dim=-1) + + return dist - next_dist def is_falling_penalty(env, max_body_z_vel:float = 0.10): lin_vel = mdp.base_lin_vel(env) @@ -263,47 +549,39 @@ def low_vel_penalty(env, min_vel:float = 0.1): vel = lin_vel[..., 0] penalty = torch.where(vel < min_vel, 1., 0.) return penalty - return proj_scal - -def close_to_goal(env, dist): - pos = mdp.root_pos_w(env) - goal_pos = mdp.generated_commands(env, "goal_pose") - goal_pos = goal_pos[:, :2] - curr_dist = torch.norm(goal_pos - pos[:, :2], dim=-1) - return curr_dist < dist - -def upright_bool(env, thresh_deg): - return upright_penalty(env, thresh_deg) > 0.0 - -def stuck(env, min_vel, wheel_spin_thr): - not_moving = forward_vel(env) < min_vel - spinning_wheels = forward_wheel_spin(env) > wheel_spin_thr - return torch.logical_and(not_moving, spinning_wheels) @configclass class ElevationRewardsCfg: + goal_reached = RewTerm( + func = mdp.rewards.is_terminated_term, + params = {"term_keys": ["at_goal"]}, + weight = 2000.0, + ) + vel_towards_goal = RewTerm( func = goal_progress_rate, - weight = 200.0, + weight = 20.0, ) - height_z = RewTerm( - func = higher_elevation, - weight = 5000., + stuck_penalty = RewTerm( + func = mdp.rewards.is_terminated_term, + params={"term_keys": ["stuck"]}, + weight = -1000.0 ) - falling_penalty = RewTerm( - func = is_falling_penalty, - weight = 0., - ) - - termination_penalty = RewTerm( + rollover_penalty = RewTerm( func = mdp.rewards.is_terminated_term, - params={"term_keys": "stuck"}, - weight = -200.0 + params={"term_keys": ["rollover"]}, + weight = -1000.0 ) + time_penalty = RewTerm( + func = mdp.is_alive, + weight = -1. + ) + + ######################## ###### CURRICULUM ###### ######################## @@ -312,23 +590,14 @@ class ElevationRewardsCfg: class ElevationCurriculumCfg: """Configuration for the elevation policy curriculum.""" - more_goal = CurrTerm( - func = increase_reward_weight_over_time, + less_vel_reward = CurrTerm( + func = decrease_reward_weight_over_time, params={ "reward_term_name" : "vel_towards_goal", - "increase": 5., - "episodes_per_increase": 50, - "max_increases": 5, - } - ) - - more_falling_pen = CurrTerm( - func = increase_reward_weight_over_time, - params={ - "reward_term_name" : "falling_penalty", - "increase": 1., - "episodes_per_increase": 50, - "max_increases": 10, + "decrease": 3., + "episodes_per_decrease": 50, + "max_decreases": 10, + "lower_limit": 5. } ) @@ -339,37 +608,59 @@ class ElevationCurriculumCfg: def upright_bool(env, thresh_deg): return upright_penalty(env, thresh_deg) > 0.0 -def stuck(env, min_vel, wheel_spin_thr): +def is_stuck(env, min_vel, wheel_spin_thr): not_moving = forward_vel(env) < min_vel throttle_joints_asset = SceneEntityCfg("robot", joint_names=".*throttle") joint_vels = mdp.joint_vel(env, asset_cfg=throttle_joints_asset) spinning_wheels = torch.sum(joint_vels, dim=-1) > wheel_spin_thr return torch.logical_and(not_moving, spinning_wheels) +def close_to_goal(env, dist): + pos = mdp.root_pos_w(env) + goal_pos = mdp.generated_commands(env, "goal_pose") + goal_pos = goal_pos[:, :2] + curr_dist = torch.norm(goal_pos - pos[:, :2], dim=-1) + return curr_dist < dist + @configclass class ElevationTerminationsCfg: """Termination terms for the MDP.""" - # (1) Time out + # Time out time_out = DoneTerm(func=mdp.time_out, time_out=True) - # (2) Cart out of bounds + + # Cart out of bounds cart_out_of_bounds = DoneTerm( func=mdp.root_height_below_minimum, - params={"minimum_height": 0.15}, + params={"minimum_height": -1.0} ) + + # Stuck (unmoving) stuck = DoneTerm( - func=stuck, + func=is_stuck, params={ "min_vel": 0.02, "wheel_spin_thr": 5., }, ) - # (4) Stuck (upside down) + + # Stuck (upside down) rollover = DoneTerm( func=upright_bool, params={"thresh_deg": 60.}, ) + # Reached goal + at_goal = DoneTerm( + func=close_to_goal, + params={"dist": 0.5}, + ) + + +@configclass +class ElevationPlayTerminationsCfg: + """Terminations for play cfg""" + at_goal = DoneTerm( func=close_to_goal, params={"dist": 0.5}, @@ -407,10 +698,10 @@ class ElevationSceneEventsCfg: ) set_goal = EventTerm( - func=reset_root_state_uniform, + func=reset_root_state_from_terrain, mode="reset", params={ - "pose_range": {"x": (-19., 19.), "y": (-19., 19.), "yaw": (-3.14, 3.14)}, + "pose_range": {"x": (-9., 9.), "y": (-9., 9.), "yaw": (-3.14, 3.14)}, "velocity_range": { "x": (0.1, 0.2), "y": (0.1, 0.2), @@ -422,11 +713,9 @@ class ElevationSceneEventsCfg: class ElevationCommandCfg: """Configuration for the elevation commands.""" - goal_pose = UniformPose2dCommandCfg( + goal_pose = TerrainBasedPose2dCommandCfg( asset_name="robot", - ranges=UniformPose2dCommandCfg.Ranges( - pos_x=(-19.0, 19.0), - pos_y=(-19.0, 19.0), + ranges=TerrainBasedPose2dCommandCfg.Ranges( heading=(-3.14, 3.14), ), resampling_time_range=(10.0, 10.0), @@ -438,7 +727,7 @@ class ElevationCommandCfg: class MushrElevationRLEnvCfg(ManagerBasedRLEnvCfg): seed: int = 42 - num_envs: int = 512 + num_envs: int = 2048 env_spacing: float = 0. # Basic Settings @@ -451,13 +740,12 @@ class MushrElevationRLEnvCfg(ManagerBasedRLEnvCfg): rewards: ElevationRewardsCfg = ElevationRewardsCfg() terminations: ElevationTerminationsCfg = ElevationTerminationsCfg() - commands = ElevationCommandCfg = ElevationCommandCfg() + commands: ElevationCommandCfg = ElevationCommandCfg() def __post_init__(self): super().__post_init__() self.viewer.eye = [20., -20.0, 20.0] self.viewer.lookat = [0.0, 0.0, 0.] - self.sim.dt = 0.01 # 100 Hz self.decimation = 10 # 10 Hz self.actions.throttle_steer.scale = (3.0, 0.488) @@ -471,5 +759,34 @@ def __post_init__(self): @configclass class MushrElevationPlayEnvCfg(MushrElevationRLEnvCfg): - """no terminations""" - terminations: ElevationTerminationsCfg = None \ No newline at end of file + """Play env from USD""" + def __post_init__(self): + super().__post_init__() + self.terminations: ElevationPlayTerminationsCfg = None + self.rewards = None + self.curriculum = None + self.num_envs: int = 64 + self.seed: int = 67 + self.scene = ElevationUSDSceneCfg(num_envs=self.num_envs, env_spacing=self.env_spacing) + self.events.set_goal = EventTerm( + func=reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (0., 0.), "y": (0., 0.), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.1, 0.2), + "y": (0.1, 0.2), + }, + } + ) + self.commands.goal_pose = UniformPose2dCommandCfg( + asset_name="robot", + ranges=UniformPose2dCommandCfg.Ranges( + pos_x=(-19.0, 19.0), + pos_y=(-19.0, 19.0), + heading=(-3.14, 3.14), + ), + resampling_time_range=(10.0, 10.0), + simple_heading=True, + debug_vis=True + ) \ No newline at end of file