diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 6061385a94..26503d4e06 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -49,6 +49,7 @@ Guidelines for modifications: * Johnson Sun * Kaixi Bao * Kourosh Darvish +* Lionel Gulich * Lorenz Wellhausen * Masoud Moghani * Michael Gussert diff --git a/docs/source/how-to/save_camera_output.rst b/docs/source/how-to/save_camera_output.rst index 21a3927126..bbc7327b22 100644 --- a/docs/source/how-to/save_camera_output.rst +++ b/docs/source/how-to/save_camera_output.rst @@ -89,7 +89,7 @@ To run the accompanying script, execute the following command: .. code-block:: bash # Usage with saving and drawing - ./isaaclab.sh -p source/standalone/tutorials/04_sensors/run_usd_camera.py --save --draw + ./isaaclab.sh -p source/standalone/tutorials/04_sensors/run_usd_camera.py --save --draw --enable_cameras # Usage with saving only in headless mode ./isaaclab.sh -p source/standalone/tutorials/04_sensors/run_usd_camera.py --save --headless --enable_cameras diff --git a/docs/source/overview/teleop_imitation.rst b/docs/source/overview/teleop_imitation.rst index 519306a48d..642aada3fe 100644 --- a/docs/source/overview/teleop_imitation.rst +++ b/docs/source/overview/teleop_imitation.rst @@ -60,7 +60,7 @@ format. # install python module (for robomimic) ./isaaclab.sh -i robomimic # split data - ./isaaclab.sh -p source/standalone//workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5 --ratio 0.2 + ./isaaclab.sh -p source/standalone/workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5 --ratio 0.2 3. Train a BC agent for ``Isaac-Lift-Cube-Franka-IK-Rel-v0`` with `Robomimic `__: diff --git a/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst b/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst index 500eaa7666..3ddd39bf48 100644 --- a/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst +++ b/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst @@ -174,7 +174,7 @@ Now that we have gone through the code, let's run the script and see the result: .. code-block:: bash - ./isaaclab.sh -p source/standalone/tutorials/04_sensors/add_sensors_on_robot.py --num_envs 2 + ./isaaclab.sh -p source/standalone/tutorials/04_sensors/add_sensors_on_robot.py --num_envs 2 --enable_cameras This command should open a stage with a ground plane, lights, and two quadrupedal robots. diff --git a/pyproject.toml b/pyproject.toml index 402fcff6aa..51d4375907 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,6 +46,7 @@ known_third_party = [ "omni.kit.*", "warp", "carb", + "Semantics", ] # Imports from this repository known_first_party = "omni.isaac.lab" diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 2c780361d4..473b7a0986 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.24.13" +version = "0.24.19" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index 05d6084ef1..2e67c3708c 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,13 +1,57 @@ Changelog --------- -0.22.15 (2024-09-20) +0.24.19 (2024-10-05) ~~~~~~~~~~~~~~~~~~~~ Added ^^^^^ -* Added :meth:`grab_images` to be able to use images for an observation term in manager based environments +* Added new functionalities to the FrameTransformer to make it more general. It is now possible to track: + + * Target frames that aren't children of the source frame prim_path + * Target frames that are based upon the source frame prim_path + + +0.24.18 (2024-10-04) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixes parsing and application of ``size`` parameter for :class:`~omni.isaac.lab.sim.spawn.GroundPlaneCfg` to correctly + scale the grid-based ground plane. + + +0.24.17 (2024-10-04) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the deprecation notice for using ``pxr.Semantics``. The corresponding modules use ``Semantics`` module + directly. + + +0.24.16 (2024-10-03) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Renamed the observation function :meth:`grab_images` to :meth:`image` to follow convention of noun-based naming. +* Renamed the function :meth:`convert_perspective_depth_to_orthogonal_depth` to a shorter name + :meth:`omni.isaac.lab.utils.math.orthogonalize_perspective_depth`. + + +0.24.15 (2024-09-20) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`grab_images` to be able to use images for an observation term in manager-based environments. + 0.24.14 (2024-09-20) ~~~~~~~~~~~~~~~~~~~~ @@ -15,10 +59,10 @@ Added Added ^^^^^ -* Added :meth:`convert_perspective_depth_to_orthogonal_depth`. :meth:`unproject_depth` assumes - that the input depth image is orthogonal. The new :meth:`convert_perspective_depth_to_orthogonal_depth` - can be used to convert a perspective depth image into an orthogonal depth image, so that the point cloud - can be unprojected correctly with :meth:`unproject_depth`. +* Added the method :meth:`convert_perspective_depth_to_orthogonal_depth` to convert perspective depth + images to orthogonal depth images. This is useful for the :meth:`~omni.isaac.lab.utils.math.unproject_depth`, + since it expects orthogonal depth images as inputs. + 0.24.13 (2024-09-08) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_marl_env.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_marl_env.py index c467225d49..ab7cb5e3c0 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_marl_env.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_marl_env.py @@ -81,7 +81,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # set the seed for the environment if self.cfg.seed is not None: - self.seed(self.cfg.seed) + self.cfg.seed = self.seed(self.cfg.seed) else: carb.log_warn("Seed not set for the environment. The environment creation may not be deterministic.") diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_rl_env.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_rl_env.py index 1e51f89a76..656bf3e2dc 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_rl_env.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_rl_env.py @@ -86,7 +86,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # set the seed for the environment if self.cfg.seed is not None: - self.seed(self.cfg.seed) + self.cfg.seed = self.seed(self.cfg.seed) else: carb.log_warn("Seed not set for the environment. The environment creation may not be deterministic.") diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/manager_based_env.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/manager_based_env.py index 2712d4f10a..4cc1b86b8c 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/manager_based_env.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/manager_based_env.py @@ -76,7 +76,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # set the seed for the environment if self.cfg.seed is not None: - self.seed(self.cfg.seed) + self.cfg.seed = self.seed(self.cfg.seed) else: carb.log_warn("Seed not set for the environment. The environment creation may not be deterministic.") diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py index c5b51fc6ae..c770915337 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py @@ -182,38 +182,52 @@ def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> tor return link_incoming_forces.view(env.num_envs, -1) -def grab_images( +def image( env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"), data_type: str = "rgb", convert_perspective_to_orthogonal: bool = False, normalize: bool = True, ) -> torch.Tensor: - """Grab all of the latest images of a specific datatype produced by a specific camera. + """Images of a specific datatype from the camera sensor. + + If the flag :attr:`normalize` is True, post-processing of the images are performed based on their + data-types: + + - "rgb": Scales the image to (0, 1) and subtracts with the mean of the current image batch. + - "depth" or "distance_to_camera" or "distance_to_plane": Replaces infinity values with zero. Args: env: The environment the cameras are placed within. sensor_cfg: The desired sensor to read from. Defaults to SceneEntityCfg("tiled_camera"). data_type: The data type to pull from the desired camera. Defaults to "rgb". - convert_perspective_to_orthogonal: Whether to convert perspective - depth images to orthogonal depth images. Defaults to False. - normalize: Set to True to normalize images. Defaults to True. + convert_perspective_to_orthogonal: Whether to orthogonalize perspective depth images. + This is used only when the data type is "distance_to_camera". Defaults to False. + normalize: Whether to normalize the images. This depends on the selected data type. + Defaults to True. Returns: - The images produced at the last timestep + The images produced at the last time-step """ + # extract the used quantities (to enable type-hinting) sensor: TiledCamera | Camera | RayCasterCamera = env.scene.sensors[sensor_cfg.name] + + # obtain the input image images = sensor.data.output[data_type] + + # depth image conversion if (data_type == "distance_to_camera") and convert_perspective_to_orthogonal: - images = math_utils.convert_perspective_depth_to_orthogonal_depth(images, sensor.data.intrinsic_matrices) + images = math_utils.orthogonalize_perspective_depth(images, sensor.data.intrinsic_matrices) + # rgb/depth image normalization if normalize: if data_type == "rgb": - images = images / 255 + images = images.float() / 255.0 mean_tensor = torch.mean(images, dim=(1, 2), keepdim=True) images -= mean_tensor elif "distance_to" in data_type or "depth" in data_type: images[images == float("inf")] = 0 + return images.clone() diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer.py index 320007cabd..87205cf4cd 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer.py @@ -5,6 +5,7 @@ from __future__ import annotations +import re import torch from collections.abc import Sequence from typing import TYPE_CHECKING @@ -50,21 +51,6 @@ class FrameTransformer(SensorBase): typically a fictitious body, the user may need to specify an offset from the end-effector to the body of the manipulator. - .. note:: - - Currently, this implementation only handles frames within an articulation. This is because the frame - regex expressions are resolved based on their parent prim path. This can be extended to handle - frames outside of articulation by using the frame prim path instead. However, this would require - additional checks to ensure that the user-specified frames are valid which is not currently implemented. - - .. warning:: - - The implementation assumes that the parent body of a target frame is not the same as that - of the source frame (i.e. :attr:`FrameTransformerCfg.prim_path`). While a corner case, this can occur - if the user specifies the same prim path for both the source frame and target frame. In this case, - the target frame will be ignored and not reported. This is a limitation of the current implementation - and will be fixed in a future release. - """ cfg: FrameTransformerCfg @@ -136,9 +122,9 @@ def _initialize_impl(self): self._source_frame_offset_pos = source_frame_offset_pos.unsqueeze(0).repeat(self._num_envs, 1) self._source_frame_offset_quat = source_frame_offset_quat.unsqueeze(0).repeat(self._num_envs, 1) - # Keep track of mapping from the rigid body name to the desired frame, as there may be multiple frames + # Keep track of mapping from the rigid body name to the desired frames and prim path, as there may be multiple frames # based upon the same body name and we don't want to create unnecessary views - body_names_to_frames: dict[str, set[str]] = {} + body_names_to_frames: dict[str, dict[str, set[str] | str]] = {} # The offsets associated with each target frame target_offsets: dict[str, dict[str, torch.Tensor]] = {} # The frames whose offsets are not identity @@ -148,6 +134,9 @@ def _initialize_impl(self): # rotation offsets are not the identity quaternion for efficiency in _update_buffer_impl self._apply_target_frame_offset = False + # Need to keep track of whether the source frame is also a target frame + self._source_is_also_target_frame = False + # Collect all target frames, their associated body prim paths and their offsets so that we can extract # the prim, check that it has the appropriate rigid body API in a single loop. # First element is None because user can't specify source frame name @@ -155,7 +144,8 @@ def _initialize_impl(self): frame_prim_paths = [self.cfg.prim_path] + [target_frame.prim_path for target_frame in self.cfg.target_frames] # First element is None because source frame offset is handled separately frame_offsets = [None] + [target_frame.offset for target_frame in self.cfg.target_frames] - for frame, prim_path, offset in zip(frames, frame_prim_paths, frame_offsets): + frame_types = ["source"] + ["target"] * len(self.cfg.target_frames) + for frame, prim_path, offset, frame_type in zip(frames, frame_prim_paths, frame_offsets, frame_types): # Find correct prim matching_prims = sim_utils.find_matching_prims(prim_path) if len(matching_prims) == 0: @@ -180,9 +170,19 @@ def _initialize_impl(self): # Keep track of which frames are associated with which bodies if body_name in body_names_to_frames: - body_names_to_frames[body_name].add(frame_name) + body_names_to_frames[body_name]["frames"].add(frame_name) + + # This is a corner case where the source frame is also a target frame + if body_names_to_frames[body_name]["type"] == "source" and frame_type == "target": + self._source_is_also_target_frame = True + else: - body_names_to_frames[body_name] = {frame_name} + # Store the first matching prim path and the type of frame + body_names_to_frames[body_name] = { + "frames": {frame_name}, + "prim_path": matching_prim_path, + "type": frame_type, + } if offset is not None: offset_pos = torch.tensor(offset.pos, device=self.device) @@ -191,7 +191,6 @@ def _initialize_impl(self): if not is_identity_pose(offset_pos, offset_quat): non_identity_offset_frames.append(frame_name) self._apply_target_frame_offset = True - target_offsets[frame_name] = {"pos": offset_pos, "quat": offset_quat} if not self._apply_target_frame_offset: @@ -206,37 +205,75 @@ def _initialize_impl(self): ) # The names of bodies that RigidPrimView will be tracking to later extract transforms from - tracked_body_names = list(body_names_to_frames.keys()) - # Construct regex expression for the body names - body_names_regex = r"(" + "|".join(tracked_body_names) + r")" - body_names_regex = f"{self.cfg.prim_path.rsplit('/', 1)[0]}/{body_names_regex}" + tracked_prim_paths = [body_names_to_frames[body_name]["prim_path"] for body_name in body_names_to_frames.keys()] + tracked_body_names = [body_name for body_name in body_names_to_frames.keys()] + + body_names_regex = [tracked_prim_path.replace("env_0", "env_*") for tracked_prim_path in tracked_prim_paths] + # Create simulation view self._physics_sim_view = physx.create_simulation_view(self._backend) self._physics_sim_view.set_subspace_roots("/") # Create a prim view for all frames and initialize it # order of transforms coming out of view will be source frame followed by target frame(s) - self._frame_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_regex.replace(".*", "*")) + self._frame_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_regex) # Determine the order in which regex evaluated body names so we can later index into frame transforms # by frame name correctly all_prim_paths = self._frame_physx_view.prim_paths - # Only need first env as the names and their ordering are the same across environments - first_env_prim_paths = all_prim_paths[0 : len(tracked_body_names)] - first_env_body_names = [first_env_prim_path.split("/")[-1] for first_env_prim_path in first_env_prim_paths] + if "env_" in all_prim_paths[0]: + + def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: + """Separates the environment number and prim_path from the item. + + Args: + item: The item to extract the environment number from. Assumes item is of the form + `/World/envs/env_1/blah` or `/World/envs/env_11/blah`. + Returns: + The environment number and the prim_path. + """ + match = re.search(r"env_(\d+)(.*)", item) + return (int(match.group(1)), match.group(2)) + + # Find the indices that would reorganize output to be per environment. We want `env_1/blah` to come before `env_11/blah` + # and env_1/Robot/base to come before env_1/Robot/foot so we need to use custom key function + self._per_env_indices = [ + index + for index, _ in sorted( + list(enumerate(all_prim_paths)), key=lambda x: extract_env_num_and_prim_path(x[1]) + ) + ] + + # Only need 0th env as the names and their ordering are the same across environments + sorted_prim_paths = [ + all_prim_paths[index] for index in self._per_env_indices if "env_0" in all_prim_paths[index] + ] + + else: + # If no environment is present, then the order of the body names is the same as the order of the prim paths sorted alphabetically + self._per_env_indices = [index for index, _ in sorted(enumerate(all_prim_paths), key=lambda x: x[1])] + sorted_prim_paths = [all_prim_paths[index] for index in self._per_env_indices] + + # -- target frames + self._target_frame_body_names = [prim_path.split("/")[-1] for prim_path in sorted_prim_paths] - # Re-parse the list as it may have moved when resolving regex above # -- source frame self._source_frame_body_name = self.cfg.prim_path.split("/")[-1] - source_frame_index = first_env_body_names.index(self._source_frame_body_name) - # -- target frames - self._target_frame_body_names = first_env_body_names[:] - self._target_frame_body_names.remove(self._source_frame_body_name) + source_frame_index = self._target_frame_body_names.index(self._source_frame_body_name) + + # Only remove source frame from tracked bodies if it is not also a target frame + if not self._source_is_also_target_frame: + self._target_frame_body_names.remove(self._source_frame_body_name) # Determine indices into all tracked body frames for both source and target frames all_ids = torch.arange(self._num_envs * len(tracked_body_names)) self._source_frame_body_ids = torch.arange(self._num_envs) * len(tracked_body_names) + source_frame_index - self._target_frame_body_ids = all_ids[~torch.isin(all_ids, self._source_frame_body_ids)] + + # If source frame is also a target frame, then the target frame body ids are the same as the source frame body ids + if self._source_is_also_target_frame: + self._target_frame_body_ids = all_ids + else: + self._target_frame_body_ids = all_ids[~torch.isin(all_ids, self._source_frame_body_ids)] # The name of each of the target frame(s) - either user specified or defaulted to the body name self._target_frame_names: list[str] = [] @@ -249,26 +286,34 @@ def _initialize_impl(self): duplicate_frame_indices = [] # Go through each body name and determine the number of duplicates we need for that frame - # and extract the offsets. This is all done to handles the case where multiple frames + # and extract the offsets. This is all done to handle the case where multiple frames # reference the same body, but have different names and/or offsets for i, body_name in enumerate(self._target_frame_body_names): - for frame in body_names_to_frames[body_name]: - target_frame_offset_pos.append(target_offsets[frame]["pos"]) - target_frame_offset_quat.append(target_offsets[frame]["quat"]) - self._target_frame_names.append(frame) - duplicate_frame_indices.append(i) + for frame in body_names_to_frames[body_name]["frames"]: + # Only need to handle target frames here as source frame is handled separately + if frame in target_offsets: + target_frame_offset_pos.append(target_offsets[frame]["pos"]) + target_frame_offset_quat.append(target_offsets[frame]["quat"]) + self._target_frame_names.append(frame) + duplicate_frame_indices.append(i) # To handle multiple environments, need to expand so [0, 1, 1, 2] with 2 environments becomes # [0, 1, 1, 2, 3, 4, 4, 5]. Again, this is a optimization to make _update_buffer_impl more efficient duplicate_frame_indices = torch.tensor(duplicate_frame_indices, device=self.device) - num_target_body_frames = len(tracked_body_names) - 1 + if self._source_is_also_target_frame: + num_target_body_frames = len(tracked_body_names) + else: + num_target_body_frames = len(tracked_body_names) - 1 + self._duplicate_frame_indices = torch.cat( [duplicate_frame_indices + num_target_body_frames * env_num for env_num in range(self._num_envs)] ) - # Stack up all the frame offsets for shape (num_envs, num_frames, 3) and (num_envs, num_frames, 4) - self._target_frame_offset_pos = torch.stack(target_frame_offset_pos).repeat(self._num_envs, 1) - self._target_frame_offset_quat = torch.stack(target_frame_offset_quat).repeat(self._num_envs, 1) + # Target frame offsets are only applied if at least one of the offsets are non-identity + if self._apply_target_frame_offset: + # Stack up all the frame offsets for shape (num_envs, num_frames, 3) and (num_envs, num_frames, 4) + self._target_frame_offset_pos = torch.stack(target_frame_offset_pos).repeat(self._num_envs, 1) + self._target_frame_offset_quat = torch.stack(target_frame_offset_quat).repeat(self._num_envs, 1) # fill the data buffer self._data.target_frame_names = self._target_frame_names @@ -288,6 +333,10 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): # Extract transforms from view - shape is: # (the total number of source and target body frames being tracked * self._num_envs, 7) transforms = self._frame_physx_view.get_transforms() + + # Reorder the transforms to be per environment as is expected of SensorData + transforms = transforms[self._per_env_indices] + # Convert quaternions as PhysX uses xyzw form transforms[:, 3:] = convert_quat(transforms[:, 3:], to="wxyz") @@ -309,6 +358,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): target_frames = transforms[self._target_frame_body_ids] duplicated_target_frame_pos_w = target_frames[self._duplicate_frame_indices, :3] duplicated_target_frame_quat_w = target_frames[self._duplicate_frame_indices, 3:] + # Only apply offset if the offsets will result in a coordinate frame transform if self._apply_target_frame_offset: target_pos_w, target_quat_w = combine_frame_transforms( diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_cfg.py index ef225bb1fc..014b9ab235 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_cfg.py @@ -31,10 +31,15 @@ class FrameCfg: """Information specific to a coordinate frame.""" prim_path: str = MISSING - """The prim path corresponding to the parent rigid body. + """The prim path corresponding to a rigid body. - This prim should be part of the same articulation as :attr:`FrameTransformerCfg.prim_path`. + This can be a regex pattern to match multiple prims. For example, "/Robot/.*" will match all prims under "/Robot". + + This means that if the source :attr:`FrameTransformerCfg.prim_path` is "/Robot/base", and the target :attr:`FrameTransformerCfg.FrameCfg.prim_path` is "/Robot/.*", + then the frame transformer will track the poses of all the prims under "/Robot", + including "/Robot/base" (even though this will result in an identity pose w.r.t. the source frame). """ + name: str | None = None """User-defined name for the new coordinate frame. Defaults to None. diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_data.py index 0f4f4e0171..2523416502 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/frame_transformer/frame_transformer_data.py @@ -15,8 +15,8 @@ class FrameTransformerData: """Target frame names (this denotes the order in which that frame data is ordered). The frame names are resolved from the :attr:`FrameTransformerCfg.FrameCfg.name` field. - This usually follows the order in which the frames are defined in the config. However, in - the case of regex matching, the order may be different. + This does not necessarily follow the order in which the frames are defined in the config due to + the regex matching. """ target_pos_source: torch.Tensor = None diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/from_files/from_files.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/from_files/from_files.py index 5e3c88dc1d..cab6bf06c9 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/from_files/from_files.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/from_files/from_files.py @@ -151,16 +151,11 @@ def spawn_ground_plane( # Scale only the mesh # Warning: This is specific to the default grid plane asset. - if prim_utils.is_prim_path_valid(f"{prim_path}/Enviroment"): + if prim_utils.is_prim_path_valid(f"{prim_path}/Environment"): # compute scale from size scale = (cfg.size[0] / 100.0, cfg.size[1] / 100.0, 1.0) # apply scale to the mesh - omni.kit.commands.execute( - "ChangeProperty", - prop_path=Sdf.Path(f"{prim_path}/Enviroment.xformOp:scale"), - value=scale, - prev=None, - ) + prim_utils.set_prim_property(f"{prim_path}/Environment", "xformOp:scale", scale) # Change the color of the plane # Warning: This is specific to the default grid plane asset. diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/utils.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/utils.py index d7a06de866..0828e4887a 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/utils.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/utils.py @@ -17,7 +17,13 @@ import omni.isaac.core.utils.stage as stage_utils import omni.kit.commands from omni.isaac.cloner import Cloner -from pxr import PhysxSchema, Sdf, Semantics, Usd, UsdGeom, UsdPhysics, UsdShade +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade + +# from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated +try: + import Semantics +except ModuleNotFoundError: + from pxr import Semantics from omni.isaac.lab.utils.string import to_camel_case diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/math.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/math.py index 8be8f520da..1d3c0db81f 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/math.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/math.py @@ -987,115 +987,30 @@ def transform_points( @torch.jit.script -def unproject_depth(depth: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tensor: - r"""Unproject depth image into a pointcloud. This method assumes that depth - is provided orthogonally relative to the image plane, as opposed to absolutely relative to the camera's - principal point (perspective depth). To unproject a perspective depth image, use - :meth:`convert_perspective_depth_to_orthogonal_depth` to convert - to an orthogonal depth image prior to calling this method. Otherwise, the - created point cloud will be distorted, especially around the edges. +def orthogonalize_perspective_depth(depth: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tensor: + """Converts perspective depth image to orthogonal depth image. - This function converts depth images into points given the calibration matrix of the camera. - - .. math:: - p_{3D} = K^{-1} \times [u, v, 1]^T \times d - - where :math:`p_{3D}` is the 3D point, :math:`d` is the depth value, :math:`u` and :math:`v` are - the pixel coordinates and :math:`K` is the intrinsic matrix. - - If `depth` is a batch of depth images and `intrinsics` is a single intrinsic matrix, the same - calibration matrix is applied to all depth images in the batch. - - The function assumes that the width and height are both greater than 1. This makes the function - deal with many possible shapes of depth images and intrinsics matrices. - - Args: - depth: The depth measurement. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). - intrinsics: A tensor providing camera's calibration matrix. Shape is (3, 3) or (N, 3, 3). - - Returns: - The 3D coordinates of points. Shape is (P, 3) or (N, P, 3). - - Raises: - ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). - ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). - """ - depth_batch = depth.clone() - intrinsics_batch = intrinsics.clone() - # check if inputs are batched - is_batched = depth_batch.dim() == 4 or (depth_batch.dim() == 3 and depth_batch.shape[-1] != 1) - # make sure inputs are batched - if depth_batch.dim() == 3 and depth_batch.shape[-1] == 1: - depth_batch = depth_batch.squeeze(dim=2) # (H, W, 1) -> (H, W) - if depth_batch.dim() == 2: - depth_batch = depth_batch[None] # (H, W) -> (1, H, W) - if depth_batch.dim() == 4 and depth_batch.shape[-1] == 1: - depth_batch = depth_batch.squeeze(dim=3) # (N, H, W, 1) -> (N, H, W) - if intrinsics_batch.dim() == 2: - intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) - # check shape of inputs - if depth_batch.dim() != 3: - raise ValueError(f"Expected depth images to have dim = 2 or 3 or 4: got shape {depth.shape}") - if intrinsics_batch.dim() != 3: - raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}") - - # get image height and width - im_height, im_width = depth_batch.shape[1:] - # create image points in homogeneous coordinates (3, H x W) - indices_u = torch.arange(im_width, device=depth.device, dtype=depth.dtype) - indices_v = torch.arange(im_height, device=depth.device, dtype=depth.dtype) - img_indices = torch.stack(torch.meshgrid([indices_u, indices_v], indexing="ij"), dim=0).reshape(2, -1) - pixels = torch.nn.functional.pad(img_indices, (0, 0, 0, 1), mode="constant", value=1.0) - pixels = pixels.unsqueeze(0) # (3, H x W) -> (1, 3, H x W) - - # unproject points into 3D space - points = torch.matmul(torch.inverse(intrinsics_batch), pixels) # (N, 3, H x W) - points = points / points[:, -1, :].unsqueeze(1) # normalize by last coordinate - # flatten depth image (N, H, W) -> (N, H x W) - depth_batch = depth_batch.transpose_(1, 2).reshape(depth_batch.shape[0], -1).unsqueeze(2) - depth_batch = depth_batch.expand(-1, -1, 3) - # scale points by depth - points_xyz = points.transpose_(1, 2) * depth_batch # (N, H x W, 3) - - # return points in same shape as input - if not is_batched: - points_xyz = points_xyz.squeeze(0) - - return points_xyz - - -@torch.jit.script -def convert_perspective_depth_to_orthogonal_depth( - perspective_depth: torch.Tensor, intrinsics: torch.Tensor -) -> torch.Tensor: - r"""Provided depth image(s) where depth is provided as the distance to the principal - point of the camera (perspective depth), this function converts it so that depth - is provided as the distance to the camera's image plane (orthogonal depth). - - This is helpful because `unproject_depth` assumes that depth is expressed in - the orthogonal depth format. - - If `perspective_depth` is a batch of depth images and `intrinsics` is a single intrinsic matrix, - the same calibration matrix is applied to all depth images in the batch. + Perspective depth images contain distances measured from the camera's optical center. + Meanwhile, orthogonal depth images provide the distance from the camera's image plane. + This method uses the camera geometry to convert perspective depth to orthogonal depth image. The function assumes that the width and height are both greater than 1. Args: - perspective_depth: The depth measurement obtained with the distance_to_camera replicator. - Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). - intrinsics: A tensor providing camera's calibration matrix. Shape is (3, 3) or (N, 3, 3). + depth: The perspective depth images. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). + intrinsics: The camera's calibration matrix. If a single matrix is provided, the same + calibration matrix is used across all the depth images in the batch. + Shape is (3, 3) or (N, 3, 3). Returns: - The depth image as if obtained by the distance_to_image_plane replicator. Shape - matches the input shape of depth + The orthogonal depth images. Shape matches the input shape of depth images. Raises: ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). """ - # Clone inputs to avoid in-place modifications - perspective_depth_batch = perspective_depth.clone() + perspective_depth_batch = depth.clone() intrinsics_batch = intrinsics.clone() # Check if inputs are batched @@ -1123,7 +1038,7 @@ def convert_perspective_depth_to_orthogonal_depth( # Validate input shapes if perspective_depth_batch.dim() != 3: - raise ValueError(f"Expected perspective_depth to have 2, 3, or 4 dimensions; got {perspective_depth.shape}.") + raise ValueError(f"Expected depth images to have 2, 3, or 4 dimensions; got {depth.shape}.") if intrinsics_batch.dim() != 3: raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3); got {intrinsics.shape}.") @@ -1137,8 +1052,8 @@ def convert_perspective_depth_to_orthogonal_depth( cy = intrinsics_batch[:, 1, 2].view(-1, 1, 1) # Create meshgrid of pixel coordinates - u_grid = torch.arange(im_width, device=perspective_depth.device, dtype=perspective_depth.dtype) - v_grid = torch.arange(im_height, device=perspective_depth.device, dtype=perspective_depth.dtype) + u_grid = torch.arange(im_width, device=depth.device, dtype=depth.dtype) + v_grid = torch.arange(im_height, device=depth.device, dtype=depth.dtype) u_grid, v_grid = torch.meshgrid(u_grid, v_grid, indexing="xy") # Expand the grids for batch processing @@ -1150,17 +1065,104 @@ def convert_perspective_depth_to_orthogonal_depth( y_term = ((v_grid - cy) / fy) ** 2 # Calculate the orthogonal (normal) depth - normal_depth = perspective_depth_batch / torch.sqrt(1 + x_term + y_term) + orthogonal_depth = perspective_depth_batch / torch.sqrt(1 + x_term + y_term) # Restore the last dimension if it was present in the input if add_last_dim: - normal_depth = normal_depth.unsqueeze(-1) + orthogonal_depth = orthogonal_depth.unsqueeze(-1) # Return to original shape if input was not batched if not is_batched: - normal_depth = normal_depth.squeeze(0) + orthogonal_depth = orthogonal_depth.squeeze(0) - return normal_depth + return orthogonal_depth + + +@torch.jit.script +def unproject_depth(depth: torch.Tensor, intrinsics: torch.Tensor, is_ortho: bool = True) -> torch.Tensor: + r"""Un-project depth image into a pointcloud. + + This function converts orthogonal or perspective depth images into points given the calibration matrix + of the camera. It uses the following transformation based on camera geometry: + + .. math:: + p_{3D} = K^{-1} \times [u, v, 1]^T \times d + + where :math:`p_{3D}` is the 3D point, :math:`d` is the depth value (measured from the image plane), + :math:`u` and :math:`v` are the pixel coordinates and :math:`K` is the intrinsic matrix. + + The function assumes that the width and height are both greater than 1. This makes the function + deal with many possible shapes of depth images and intrinsics matrices. + + .. note:: + If :attr:`is_ortho` is False, the input depth images are transformed to orthogonal depth images + by using the :meth:`orthogonalize_perspective_depth` method. + + Args: + depth: The depth measurement. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). + intrinsics: The camera's calibration matrix. If a single matrix is provided, the same + calibration matrix is used across all the depth images in the batch. + Shape is (3, 3) or (N, 3, 3). + is_ortho: Whether the input depth image is orthogonal or perspective depth image. If True, the input + depth image is considered as the *orthogonal* type, where the measurements are from the camera's + image plane. If False, the depth image is considered as the *perspective* type, where the + measurements are from the camera's optical center. Defaults to True. + + Returns: + The 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + + Raises: + ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). + ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). + """ + # clone inputs to avoid in-place modifications + intrinsics_batch = intrinsics.clone() + # convert depth image to orthogonal if needed + if not is_ortho: + depth_batch = orthogonalize_perspective_depth(depth, intrinsics) + else: + depth_batch = depth.clone() + + # check if inputs are batched + is_batched = depth_batch.dim() == 4 or (depth_batch.dim() == 3 and depth_batch.shape[-1] != 1) + # make sure inputs are batched + if depth_batch.dim() == 3 and depth_batch.shape[-1] == 1: + depth_batch = depth_batch.squeeze(dim=2) # (H, W, 1) -> (H, W) + if depth_batch.dim() == 2: + depth_batch = depth_batch[None] # (H, W) -> (1, H, W) + if depth_batch.dim() == 4 and depth_batch.shape[-1] == 1: + depth_batch = depth_batch.squeeze(dim=3) # (N, H, W, 1) -> (N, H, W) + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + # check shape of inputs + if depth_batch.dim() != 3: + raise ValueError(f"Expected depth images to have dim = 2 or 3 or 4: got shape {depth.shape}") + if intrinsics_batch.dim() != 3: + raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}") + + # get image height and width + im_height, im_width = depth_batch.shape[1:] + # create image points in homogeneous coordinates (3, H x W) + indices_u = torch.arange(im_width, device=depth.device, dtype=depth.dtype) + indices_v = torch.arange(im_height, device=depth.device, dtype=depth.dtype) + img_indices = torch.stack(torch.meshgrid([indices_u, indices_v], indexing="ij"), dim=0).reshape(2, -1) + pixels = torch.nn.functional.pad(img_indices, (0, 0, 0, 1), mode="constant", value=1.0) + pixels = pixels.unsqueeze(0) # (3, H x W) -> (1, 3, H x W) + + # unproject points into 3D space + points = torch.matmul(torch.inverse(intrinsics_batch), pixels) # (N, 3, H x W) + points = points / points[:, -1, :].unsqueeze(1) # normalize by last coordinate + # flatten depth image (N, H, W) -> (N, H x W) + depth_batch = depth_batch.transpose_(1, 2).reshape(depth_batch.shape[0], -1).unsqueeze(2) + depth_batch = depth_batch.expand(-1, -1, 3) + # scale points by depth + points_xyz = points.transpose_(1, 2) * depth_batch # (N, H x W, 3) + + # return points in same shape as input + if not is_batched: + points_xyz = points_xyz.squeeze(0) + + return points_xyz @torch.jit.script @@ -1191,8 +1193,10 @@ def project_points(points: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tens Returns: Projected 3D coordinates of points. Shape is (P, 3) or (N, P, 3). """ + # clone the inputs to avoid in-place operations modifying the original data points_batch = points.clone() intrinsics_batch = intrinsics.clone() + # check if inputs are batched is_batched = points_batch.dim() == 2 # make sure inputs are batched @@ -1205,12 +1209,14 @@ def project_points(points: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tens raise ValueError(f"Expected points to have dim = 3: got shape {points.shape}.") if intrinsics_batch.dim() != 3: raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}.") + # project points into 2D image plane points_2d = torch.matmul(intrinsics_batch, points_batch.transpose(1, 2)) points_2d = points_2d / points_2d[:, -1, :].unsqueeze(1) # normalize by last coordinate points_2d = points_2d.transpose_(1, 2) # (N, 3, P) -> (N, P, 3) # replace last coordinate with depth points_2d[:, :, -1] = points_batch[:, :, -1] + # return points in same shape as input if not is_batched: points_2d = points_2d.squeeze(0) # (1, 3, P) -> (3, P) diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py b/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py index f379bc86b2..aeab3d9e72 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py @@ -3,10 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -""" -This script checks the FrameTransformer sensor by visualizing the frames that it creates. -""" - """Launch Isaac Sim Simulator first.""" from omni.isaac.lab.app import AppLauncher, run_tests @@ -26,6 +22,7 @@ import omni.isaac.lab.sim as sim_utils import omni.isaac.lab.utils.math as math_utils +from omni.isaac.lab.assets import RigidObjectCfg from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg from omni.isaac.lab.sensors import FrameTransformerCfg, OffsetCfg from omni.isaac.lab.terrains import TerrainImporterCfg @@ -62,6 +59,19 @@ class MySceneCfg(InteractiveSceneCfg): # sensors - frame transformer (filled inside unit test) frame_transformer: FrameTransformerCfg = None + # block + cube: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/cube", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(max_depenetration_velocity=1.0), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + physics_material=sim_utils.RigidBodyMaterialCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 0.0)), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(2.0, 0.0, 5)), + ) + class TestFrameTransformer(unittest.TestCase): """Test for frame transformer sensor.""" @@ -71,7 +81,7 @@ def setUp(self): # Create a new stage stage_utils.create_new_stage() # Load kit helper - self.sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.005)) + self.sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.005, device="cpu")) # Set main camera self.sim.set_camera_view(eye=[5, 5, 5], target=[0.0, 0.0, 0.0]) @@ -90,8 +100,7 @@ def tearDown(self): def test_frame_transformer_feet_wrt_base(self): """Test feet transformations w.r.t. base source frame. - In this test, the source frame is the robot base. This frame is at index 0, when - the frame bodies are sorted in the order of the regex matching in the frame transformer. + In this test, the source frame is the robot base. """ # Spawn things into stage scene_cfg = MySceneCfg(num_envs=32, env_spacing=5.0, lazy_sensor_update=False) @@ -141,9 +150,15 @@ def test_frame_transformer_feet_wrt_base(self): feet_indices, feet_names = scene.articulations["robot"].find_bodies( ["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"] ) - # Check names are parsed the same order - user_feet_names = [f"{name}_USER" for name in feet_names] - self.assertListEqual(scene.sensors["frame_transformer"].data.target_frame_names, user_feet_names) + + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + + # Reorder the feet indices to match the order of the target frames with _USER suffix removed + target_frame_names = [name.split("_USER")[0] for name in target_frame_names] + + # Find the indices of the feet in the order of the target frames + reordering_indices = [feet_names.index(name) for name in target_frame_names] + feet_indices = [feet_indices[i] for i in reordering_indices] # default joint targets default_actions = scene.articulations["robot"].data.default_joint_pos.clone() @@ -185,11 +200,12 @@ def test_frame_transformer_feet_wrt_base(self): source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf, rtol=1e-3, atol=1e-3) + torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) + torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) + torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) + torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) # check if relative transforms are same feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source @@ -200,8 +216,8 @@ def test_frame_transformer_feet_wrt_base(self): root_pose_w[:, :3], root_pose_w[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] ) # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b, rtol=1e-3, atol=1e-3) + torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) + torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) def test_frame_transformer_feet_wrt_thigh(self): """Test feet transformation w.r.t. thigh source frame. @@ -285,10 +301,10 @@ def test_frame_transformer_feet_wrt_thigh(self): feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w # check if they are same - torch.testing.assert_close(source_pose_w_gt[:, :3], source_pos_w_tf, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(source_pose_w_gt[:, 3:], source_quat_w_tf, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf, rtol=1e-3, atol=1e-3) + torch.testing.assert_close(source_pose_w_gt[:, :3], source_pos_w_tf) + torch.testing.assert_close(source_pose_w_gt[:, 3:], source_quat_w_tf) + torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) + torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) # check if relative transforms are same feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source @@ -299,8 +315,269 @@ def test_frame_transformer_feet_wrt_thigh(self): source_pose_w_gt[:, :3], source_pose_w_gt[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] ) # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b, rtol=1e-3, atol=1e-3) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b, rtol=1e-3, atol=1e-3) + torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) + torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) + + def test_frame_transformer_robot_body_to_external_cube(self): + """Test transformation from robot body to a cube in the scene. + + In this test, the source frame is the robot base. + + The target_frame is a cube in the scene, external to the robot. + """ + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="CUBE_USER", + prim_path="{ENV_REGEX_NS}/cube", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + self.sim.reset() + + # default joint targets + default_actions = scene.articulations["robot"].data.default_joint_pos.clone() + # Define simulation stepping + sim_dt = self.sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + joint_pos = scene.articulations["robot"].data.default_joint_pos + joint_vel = scene.articulations["robot"].data.default_joint_vel + # -- set root state + # -- robot + scene.articulations["robot"].write_root_state_to_sim(root_state) + scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + # reset buffers + scene.reset() + + # set joint targets + robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) + scene.articulations["robot"].set_joint_position_target(robot_actions) + # write data to sim + scene.write_data_to_sim() + # perform step + self.sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] + cube_pos_w_gt = scene.rigid_objects["cube"].data.root_state_w[:, :3] + cube_quat_w_gt = scene.rigid_objects["cube"].data.root_state_w[:, 3:7] + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + cube_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.squeeze() + cube_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.squeeze() + + # check if they are same + torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) + torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) + torch.testing.assert_close(cube_pos_w_gt, cube_pos_w_tf) + torch.testing.assert_close(cube_quat_w_gt, cube_quat_w_tf) + + # check if relative transforms are same + cube_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source + cube_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source + # ground-truth + cube_pos_b, cube_quat_b = math_utils.subtract_frame_transforms( + root_pose_w[:, :3], root_pose_w[:, 3:], cube_pos_w_tf, cube_quat_w_tf + ) + # check if they are same + torch.testing.assert_close(cube_pos_source_tf[:, 0], cube_pos_b) + torch.testing.assert_close(cube_quat_source_tf[:, 0], cube_quat_b) + + def test_frame_transformer_offset_frames(self): + """Test body transformation w.r.t. base source frame. + + In this test, the source frame is the cube frame. + """ + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/cube", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="CUBE_CENTER", + prim_path="{ENV_REGEX_NS}/cube", + ), + FrameTransformerCfg.FrameCfg( + name="CUBE_TOP", + prim_path="{ENV_REGEX_NS}/cube", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.1), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ), + FrameTransformerCfg.FrameCfg( + name="CUBE_BOTTOM", + prim_path="{ENV_REGEX_NS}/cube", + offset=OffsetCfg( + pos=(0.0, 0.0, -0.1), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + self.sim.reset() + + # Define simulation stepping + sim_dt = self.sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene["cube"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + # -- set root state + # -- cube + scene["cube"].write_root_state_to_sim(root_state) + # reset buffers + scene.reset() + + # write data to sim + scene.write_data_to_sim() + # perform step + self.sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + cube_pos_w_gt = scene["cube"].data.root_state_w[:, :3] + cube_quat_w_gt = scene["cube"].data.root_state_w[:, 3:7] + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + target_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.squeeze() + target_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.squeeze() + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + + cube_center_idx = target_frame_names.index("CUBE_CENTER") + cube_bottom_idx = target_frame_names.index("CUBE_BOTTOM") + cube_top_idx = target_frame_names.index("CUBE_TOP") + + # check if they are same + torch.testing.assert_close(cube_pos_w_gt, source_pos_w_tf) + torch.testing.assert_close(cube_quat_w_gt, source_quat_w_tf) + torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) + torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) + + # test offsets are applied correctly + # -- cube top + cube_pos_top = target_pos_w_tf[:, cube_top_idx] + cube_quat_top = target_quat_w_tf[:, cube_top_idx] + torch.testing.assert_close(cube_pos_top, cube_pos_w_gt + torch.tensor([0.0, 0.0, 0.1])) + torch.testing.assert_close(cube_quat_top, cube_quat_w_gt) + + # -- cube bottom + cube_pos_bottom = target_pos_w_tf[:, cube_bottom_idx] + cube_quat_bottom = target_quat_w_tf[:, cube_bottom_idx] + torch.testing.assert_close(cube_pos_bottom, cube_pos_w_gt + torch.tensor([0.0, 0.0, -0.1])) + torch.testing.assert_close(cube_quat_bottom, cube_quat_w_gt) + + def test_frame_transformer_all_bodies(self): + """Test transformation of all bodies w.r.t. base source frame. + + In this test, the source frame is the robot base. + + The target_frames are all bodies in the robot, implemented using .* pattern. + """ + # Spawn things into stage + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/.*", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + self.sim.reset() + + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + articulation_body_names = scene.articulations["robot"].data.body_names + + reordering_indices = [target_frame_names.index(name) for name in articulation_body_names] + + # default joint targets + default_actions = scene.articulations["robot"].data.default_joint_pos.clone() + # Define simulation stepping + sim_dt = self.sim.get_physics_dt() + # Simulate physics + for count in range(100): + # # reset + if count % 25 == 0: + # reset root state + root_state = scene.articulations["robot"].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + joint_pos = scene.articulations["robot"].data.default_joint_pos + joint_vel = scene.articulations["robot"].data.default_joint_vel + # -- set root state + # -- robot + scene.articulations["robot"].write_root_state_to_sim(root_state) + scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + # reset buffers + scene.reset() + + # set joint targets + robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) + scene.articulations["robot"].set_joint_position_target(robot_actions) + # write data to sim + scene.write_data_to_sim() + # perform step + self.sim.step() + # read data from sim + scene.update(sim_dt) + + # check absolute frame transforms in world frame + # -- ground-truth + root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] + bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w + bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w + + # -- frame transformer + source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w + source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w + bodies_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w + bodies_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w + + # check if they are same + torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) + torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) + torch.testing.assert_close(bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices]) + torch.testing.assert_close(bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices]) + + bodies_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source + bodies_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source + + # Go through each body and check if relative transforms are same + for index in range(len(articulation_body_names)): + body_pos_b, body_quat_b = math_utils.subtract_frame_transforms( + root_pose_w[:, :3], root_pose_w[:, 3:], bodies_pos_w_tf[:, index], bodies_quat_w_tf[:, index] + ) + + torch.testing.assert_close(bodies_pos_source_tf[:, index], body_pos_b) + torch.testing.assert_close(bodies_quat_source_tf[:, index], body_quat_b) if __name__ == "__main__": diff --git a/source/extensions/omni.isaac.lab/test/utils/test_math.py b/source/extensions/omni.isaac.lab/test/utils/test_math.py index 06f1f3f8dc..d6d6086290 100644 --- a/source/extensions/omni.isaac.lab/test/utils/test_math.py +++ b/source/extensions/omni.isaac.lab/test/utils/test_math.py @@ -376,23 +376,27 @@ def iter_old_quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tens iter_old_quat_rotate_inverse(q_rand, v_rand), ) - def test_depth_perspective_conversion(self): - # Create a sample perspective depth image (N, H, W) - perspective_depth = torch.tensor([[[10.0, 0.0, 100.0], [0.0, 3000.0, 0.0], [100.0, 0.0, 100.0]]]) + def test_orthogonalize_perspective_depth(self): + """Test for converting perspective depth to orthogonal depth.""" + for device in ["cpu", "cuda:0"]: + # Create a sample perspective depth image (N, H, W) + perspective_depth = torch.tensor( + [[[10.0, 0.0, 100.0], [0.0, 3000.0, 0.0], [100.0, 0.0, 100.0]]], device=device + ) - # Create sample intrinsic matrix (3, 3) - intrinsics = torch.tensor([[500.0, 0.0, 5.0], [0.0, 500.0, 5.0], [0.0, 0.0, 1.0]]) + # Create sample intrinsic matrix (3, 3) + intrinsics = torch.tensor([[500.0, 0.0, 5.0], [0.0, 500.0, 5.0], [0.0, 0.0, 1.0]], device=device) - # Convert perspective depth to orthogonal depth - orthogonal_depth = math_utils.convert_perspective_depth_to_orthogonal_depth(perspective_depth, intrinsics) + # Convert perspective depth to orthogonal depth + orthogonal_depth = math_utils.orthogonalize_perspective_depth(perspective_depth, intrinsics) - # Manually compute expected orthogonal depth based on the formula for comparison - expected_orthogonal_depth = torch.tensor( - [[[9.9990, 0.0000, 99.9932], [0.0000, 2999.8079, 0.0000], [99.9932, 0.0000, 99.9964]]] - ) + # Manually compute expected orthogonal depth based on the formula for comparison + expected_orthogonal_depth = torch.tensor( + [[[9.9990, 0.0000, 99.9932], [0.0000, 2999.8079, 0.0000], [99.9932, 0.0000, 99.9964]]], device=device + ) - # Assert that the output is close to the expected result - torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth) + # Assert that the output is close to the expected result + torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth) if __name__ == "__main__": diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand/shadow_hand_vision_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand/shadow_hand_vision_env.py index fd274a187d..b025bfb052 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand/shadow_hand_vision_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand/shadow_hand_vision_env.py @@ -9,7 +9,12 @@ import torch import omni.usd -from pxr import Semantics + +# from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated +try: + import Semantics +except ModuleNotFoundError: + from pxr import Semantics import omni.isaac.lab.sim as sim_utils from omni.isaac.lab.assets import Articulation, RigidObject diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py index 889e844843..ce5a6c90b8 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py @@ -4,14 +4,15 @@ # SPDX-License-Identifier: BSD-3-Clause import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.envs.mdp.observations import grab_images from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm from omni.isaac.lab.managers import SceneEntityCfg from omni.isaac.lab.sensors import TiledCameraCfg from omni.isaac.lab.utils import configclass -from omni.isaac.lab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import CartpoleEnvCfg, CartpoleSceneCfg +import omni.isaac.lab_tasks.manager_based.classic.cartpole.mdp as mdp + +from .cartpole_env_cfg import CartpoleEnvCfg, CartpoleSceneCfg ## # Scene definition @@ -20,6 +21,8 @@ @configclass class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): + + # add camera to the scene tiled_camera: TiledCameraCfg = TiledCameraCfg( prim_path="{ENV_REGEX_NS}/Camera", offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), @@ -34,6 +37,8 @@ class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): @configclass class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg): + + # add camera to the scene tiled_camera: TiledCameraCfg = TiledCameraCfg( prim_path="{ENV_REGEX_NS}/Camera", offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), @@ -46,17 +51,22 @@ class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg): ) +## +# MDP settings +## + + @configclass class RGBObservationsCfg: """Observation specifications for the MDP.""" @configclass class RGBCameraPolicyCfg(ObsGroup): - """Observations for policy group.""" + """Observations for policy group with RGB images.""" - image = ObsTerm(func=grab_images, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "rgb"}) + image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "rgb"}) - def __post_init__(self) -> None: + def __post_init__(self): self.enable_corruption = False self.concatenate_terms = True @@ -65,22 +75,35 @@ def __post_init__(self) -> None: @configclass class DepthObservationsCfg: + """Observation specifications for the MDP.""" + @configclass class DepthCameraPolicyCfg(RGBObservationsCfg.RGBCameraPolicyCfg): + """Observations for policy group with depth images.""" + image = ObsTerm( - func=grab_images, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "distance_to_camera"} + func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "distance_to_camera"} ) policy: ObsGroup = DepthCameraPolicyCfg() +## +# Environment configuration +## + + @configclass class CartpoleRGBCameraEnvCfg(CartpoleEnvCfg): + """Configuration for the cartpole environment with RGB camera.""" + scene: CartpoleSceneCfg = CartpoleRGBCameraSceneCfg(num_envs=1024, env_spacing=20) - observations = RGBObservationsCfg() + observations: RGBObservationsCfg = RGBObservationsCfg() @configclass class CartpoleDepthCameraEnvCfg(CartpoleEnvCfg): + """Configuration for the cartpole environment with depth camera.""" + scene: CartpoleSceneCfg = CartpoleDepthCameraSceneCfg(num_envs=1024, env_spacing=20) - observations = DepthObservationsCfg() + observations: DepthObservationsCfg = DepthObservationsCfg() diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py index e372334602..8c92d3d5ae 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py @@ -176,7 +176,7 @@ class CurriculumCfg: @configclass class CartpoleEnvCfg(ManagerBasedRLEnvCfg): - """Configuration for the locomotion velocity-tracking environment.""" + """Configuration for the cartpole environment.""" # Scene settings scene: CartpoleSceneCfg = CartpoleSceneCfg(num_envs=4096, env_spacing=4.0) diff --git a/source/standalone/benchmarks/benchmark_cameras.py b/source/standalone/benchmarks/benchmark_cameras.py index bb50128277..2ba16c357a 100644 --- a/source/standalone/benchmarks/benchmark_cameras.py +++ b/source/standalone/benchmarks/benchmark_cameras.py @@ -261,7 +261,7 @@ TiledCameraCfg, patterns, ) -from omni.isaac.lab.utils.math import convert_perspective_depth_to_orthogonal_depth, unproject_depth +from omni.isaac.lab.utils.math import orthogonalize_perspective_depth, unproject_depth from omni.isaac.lab_tasks.utils import load_cfg_from_registry @@ -677,9 +677,8 @@ def run_simulator( depth = camera.data.output[data_type] depth_images[data_label + "_raw"] = depth if perspective_depth_predicate(data_type) and convert_depth_to_camera_to_image_plane: - depth = convert_perspective_depth_to_orthogonal_depth( - perspective_depth=camera.data.output[data_type], - intrinsics=camera.data.intrinsic_matrices, + depth = orthogonalize_perspective_depth( + camera.data.output[data_type], camera.data.intrinsic_matrices ) depth_images[data_label + "_undistorted"] = depth diff --git a/source/standalone/demos/quadrupeds.py b/source/standalone/demos/quadrupeds.py index 492ee07af1..3122a4741c 100644 --- a/source/standalone/demos/quadrupeds.py +++ b/source/standalone/demos/quadrupeds.py @@ -49,7 +49,7 @@ def define_origins(num_origins: int, spacing: float) -> list[list[float]]: - """Defines the origins of the the scene.""" + """Defines the origins of the scene.""" # create tensor based on number of environments env_origins = torch.zeros(num_origins, 3) # create a grid of origins diff --git a/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py b/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py index 0250f058a0..8ba29c2921 100644 --- a/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py +++ b/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py @@ -15,7 +15,7 @@ .. code-block:: bash # Usage - ./isaaclab.sh -p source/standalone/tutorials/04_sensors/add_sensors_on_robot.py + ./isaaclab.sh -p source/standalone/tutorials/04_sensors/add_sensors_on_robot.py --enable_cameras """ diff --git a/source/standalone/tutorials/04_sensors/run_frame_transformer.py b/source/standalone/tutorials/04_sensors/run_frame_transformer.py index b0d84afb4d..e8c702e025 100644 --- a/source/standalone/tutorials/04_sensors/run_frame_transformer.py +++ b/source/standalone/tutorials/04_sensors/run_frame_transformer.py @@ -139,10 +139,10 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict): if count % 50 == 0: # get frame names frame_names = frame_transformer.data.target_frame_names - print(f"Displaying Frame ID {frame_index}: {frame_names[frame_index]}") # increment frame index frame_index += 1 frame_index = frame_index % len(frame_names) + print(f"Displaying Frame ID {frame_index}: {frame_names[frame_index]}") # visualize frame source_pos = frame_transformer.data.source_pos_w diff --git a/source/standalone/tutorials/04_sensors/run_usd_camera.py b/source/standalone/tutorials/04_sensors/run_usd_camera.py index ca4ad1d4c1..129a76f566 100644 --- a/source/standalone/tutorials/04_sensors/run_usd_camera.py +++ b/source/standalone/tutorials/04_sensors/run_usd_camera.py @@ -12,7 +12,7 @@ .. code-block:: bash # Usage with GUI - ./isaaclab.sh -p source/standalone/tutorials/04_sensors/run_usd_camera.py + ./isaaclab.sh -p source/standalone/tutorials/04_sensors/run_usd_camera.py --enable_cameras # Usage with headless ./isaaclab.sh -p source/standalone/tutorials/04_sensors/run_usd_camera.py --headless --enable_cameras @@ -53,7 +53,7 @@ AppLauncher.add_app_launcher_args(parser) # parse the arguments args_cli = parser.parse_args() -args_cli.enable_cameras = True + # launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app diff --git a/source/standalone/workflows/rl_games/train.py b/source/standalone/workflows/rl_games/train.py index 79241df823..a925be8575 100644 --- a/source/standalone/workflows/rl_games/train.py +++ b/source/standalone/workflows/rl_games/train.py @@ -47,6 +47,7 @@ import gymnasium as gym import math import os +import random from datetime import datetime from rl_games.common import env_configurations, vecenv @@ -76,6 +77,10 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + agent_cfg["params"]["seed"] = args_cli.seed if args_cli.seed is not None else agent_cfg["params"]["seed"] agent_cfg["params"]["config"]["max_epochs"] = ( args_cli.max_iterations if args_cli.max_iterations is not None else agent_cfg["params"]["config"]["max_epochs"] diff --git a/source/standalone/workflows/rsl_rl/cli_args.py b/source/standalone/workflows/rsl_rl/cli_args.py index e4b647f727..cf5d38b7bb 100644 --- a/source/standalone/workflows/rsl_rl/cli_args.py +++ b/source/standalone/workflows/rsl_rl/cli_args.py @@ -6,6 +6,7 @@ from __future__ import annotations import argparse +import random from typing import TYPE_CHECKING if TYPE_CHECKING: @@ -68,6 +69,9 @@ def update_rsl_rl_cfg(agent_cfg: RslRlOnPolicyRunnerCfg, args_cli: argparse.Name """ # override the default configuration with CLI arguments if hasattr(args_cli, "seed") and args_cli.seed is not None: + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) agent_cfg.seed = args_cli.seed if args_cli.resume is not None: agent_cfg.resume = args_cli.resume diff --git a/source/standalone/workflows/sb3/train.py b/source/standalone/workflows/sb3/train.py index 4c894a7355..1ce8062961 100644 --- a/source/standalone/workflows/sb3/train.py +++ b/source/standalone/workflows/sb3/train.py @@ -46,6 +46,7 @@ import gymnasium as gym import numpy as np import os +import random from datetime import datetime from stable_baselines3 import PPO @@ -71,6 +72,10 @@ @hydra_task_config(args_cli.task, "sb3_cfg_entry_point") def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: dict): """Train with stable-baselines agent.""" + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + # override configurations with non-hydra CLI arguments env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs agent_cfg["seed"] = args_cli.seed if args_cli.seed is not None else agent_cfg["seed"] diff --git a/source/standalone/workflows/skrl/train.py b/source/standalone/workflows/skrl/train.py index 4b7f6b9942..bbbdabf6a1 100644 --- a/source/standalone/workflows/skrl/train.py +++ b/source/standalone/workflows/skrl/train.py @@ -63,6 +63,7 @@ import gymnasium as gym import os +import random from datetime import datetime import skrl @@ -119,9 +120,14 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen if args_cli.ml_framework.startswith("jax"): skrl.config.jax.backend = "jax" if args_cli.ml_framework == "jax" else "numpy" - # set the environment seed + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + + # set the agent and environment seed from command line # note: certain randomization occur in the environment initialization so we set the seed here - env_cfg.seed = args_cli.seed if args_cli.seed is not None else agent_cfg["seed"] + agent_cfg["seed"] = args_cli.seed if args_cli.seed is not None else agent_cfg["seed"] + env_cfg.seed = agent_cfg["seed"] # specify directory for logging experiments log_root_path = os.path.join("logs", "skrl", agent_cfg["agent"]["experiment"]["directory"])