Skip to content

Commit

Permalink
Fix the double ellipsis issue when resolving indices (#196)
Browse files Browse the repository at this point in the history
# Description

Earlier in the code, we were using the `Ellipsis` object to index the
dimensions of the tensor. This led to situations where
we indexed multi-dimension tensors as: `x[..., ..., 0]`. This now leads
to errors with Python 3.10.

The MR replaces `Ellipsis` with the `slice(None)` object, which results
in indexing as: `x[:, :, 0]`.

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file

---------

Co-authored-by: Farbod Farshidian <ffarshidian@theaiinstitute.com>
  • Loading branch information
Mayankm96 and farbod-farshidian authored Oct 20, 2023
1 parent 0396cf0 commit c6af307
Show file tree
Hide file tree
Showing 18 changed files with 93 additions and 41 deletions.
2 changes: 1 addition & 1 deletion source/extensions/omni.isaac.orbit/config/extension.toml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
[package]

# Note: Semantic Versioning is used: https://semver.org/
version = "0.9.12"
version = "0.9.13"

# Description
title = "ORBIT framework for Robot Learning"
Expand Down
10 changes: 10 additions & 0 deletions source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,16 @@
Changelog
---------

0.9.13 (2023-10-20)
~~~~~~~~~~~~~~~~~~~

Fixed
^^^^^

* Fixed the issue with double :obj:`Ellipsis` when indexing tensors with multiple dimensions.
The fix now uses :obj:`slice(None)` instead of :obj:`Ellipsis` to index the tensors.


0.9.12 (2023-10-18)
~~~~~~~~~~~~~~~~~~~

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,15 +45,15 @@ class ActuatorBase(ABC):
"""The damping (D gain) of the PD controller. Shape is ``(num_envs, num_joints)``."""

def __init__(
self, cfg: ActuatorBaseCfg, joint_names: list[str], joint_ids: list[int] | Ellipsis, num_envs: int, device: str
self, cfg: ActuatorBaseCfg, joint_names: list[str], joint_ids: Sequence[int], num_envs: int, device: str
):
"""Initialize the actuator.
Args:
cfg: The configuration of the actuator model.
joint_names: The joint names in the articulation.
joint_ids: The joint indices in the articulation. If :obj:`Ellipsis`, then all the joints in the
articulation are part of the group.
joint_ids: The joint indices in the articulation. If :obj:`slice(None)`, then all
the joints in the articulation are part of the group.
num_envs: Number of articulations in the view.
device: Device used for processing.
"""
Expand Down Expand Up @@ -99,7 +99,7 @@ def __str__(self) -> str:
"""Returns: A string representation of the actuator group."""
# resolve joint indices for printing
joint_indices = self.joint_indices
if joint_indices is Ellipsis:
if joint_indices == slice(None):
joint_indices = list(range(self.num_joints))
return (
f"<class {self.__class__.__name__}> object:\n"
Expand All @@ -124,11 +124,12 @@ def joint_names(self) -> list[str]:
return self._joint_names

@property
def joint_indices(self) -> list[int] | Ellipsis:
def joint_indices(self) -> Sequence[int]:
"""Articulation's joint indices that are part of the group.
Note:
If :obj:`Ellipsis` is returned, then the group contains all the joints in the articulation.
If :obj:`slice(None)` is returned, then the group contains all the joints in the articulation.
We do this to avoid unnecessary indexing of the joints for performance reasons.
"""
return self._joint_indices

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ def reset(self, env_ids: Sequence[int] | None = None):
super().reset(env_ids)
# use ellipses object to skip initial indices.
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# reset actuators
for actuator in self.actuators.values():
actuator.reset(env_ids)
Expand Down Expand Up @@ -162,7 +162,7 @@ def find_joints(
def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None):
# resolve all indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# note: we need to do this here since tensors are not set into simulation until step.
# set into internal buffers
self._data.root_state_w[env_ids, :7] = root_pose.clone()
Expand All @@ -175,7 +175,7 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None):
# resolve all indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# note: we need to do this here since tensors are not set into simulation until step.
# set into internal buffers
self._data.root_state_w[env_ids, 7:] = root_velocity.clone()
Expand All @@ -199,9 +199,9 @@ def write_joint_state_to_sim(
"""
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set into internal buffers
self._data.joint_pos[env_ids, joint_ids] = position
self._data.joint_vel[env_ids, joint_ids] = velocity
Expand All @@ -227,9 +227,9 @@ def write_joint_stiffness_to_sim(
# note: This function isn't setting the values for actuator models. (#128)
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set into internal buffers
self._data.joint_stiffness[env_ids, joint_ids] = stiffness
# set into simulation
Expand All @@ -251,9 +251,9 @@ def write_joint_damping_to_sim(
# note: This function isn't setting the values for actuator models. (#128)
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set into internal buffers
self._data.joint_damping[env_ids, joint_ids] = damping
# set into simulation
Expand All @@ -276,9 +276,9 @@ def write_joint_torque_limit_to_sim(
# note: This function isn't setting the values for actuator models. (#128)
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set into internal buffers
torque_limit_all = self.root_physx_view.get_dof_max_forces()
torque_limit_all[env_ids, joint_ids] = limits
Expand Down Expand Up @@ -306,9 +306,9 @@ def set_joint_position_target(
"""
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set targets
self._data.joint_pos_target[env_ids, joint_ids] = target

Expand All @@ -328,9 +328,9 @@ def set_joint_velocity_target(
"""
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set targets
self._data.joint_vel_target[env_ids, joint_ids] = target

Expand All @@ -350,9 +350,9 @@ def set_joint_effort_target(
"""
# resolve indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
if joint_ids is None:
joint_ids = ...
joint_ids = slice(None)
# set targets
self._data.joint_effort_target[env_ids, joint_ids] = target

Expand Down Expand Up @@ -513,7 +513,7 @@ def _process_actuators_cfg(self):
)
# for efficiency avoid indexing when over all indices
if len(joint_names) == self.num_joints:
joint_ids = ...
joint_ids = slice(None)
# create actuator collection
actuator: ActuatorBase = actuator_cfg.class_type(
cfg=actuator_cfg,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ def body_physx_view(self) -> physx.RigidBodyView:
def reset(self, env_ids: Sequence[int] | None = None):
# resolve all indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# reset external wrench
self._external_force_b[env_ids] = 0.0
self._external_torque_b[env_ids] = 0.0
Expand Down Expand Up @@ -171,7 +171,7 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
"""
# resolve all indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# note: we need to do this here since tensors are not set into simulation until step.
# set into internal buffers
self._data.root_state_w[env_ids, :7] = root_pose.clone()
Expand All @@ -190,7 +190,7 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque
"""
# resolve all indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# note: we need to do this here since tensors are not set into simulation until step.
# set into internal buffers
self._data.root_state_w[env_ids, 7:] = root_velocity.clone()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]:
"""
# resolve the environment IDs
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# set the command counter to zero
self.command_counter[env_ids] = 0
# resample the command
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ def __init__(self, cfg: actions_cfg.JointActionCfg, env: BaseEnv) -> None:

# Avoid indexing across all joints for efficiency
if self._num_joints == self._asset.num_joints:
self._joint_ids = ...
self._joint_ids = slice(None)

# create tensors for raw and processed actions
self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env:
)
# Avoid indexing across all joints for efficiency
if self._num_joints == self._asset.num_joints:
self._joint_ids = ...
self._joint_ids = slice(None)

# create the differential IK controller
self._controller = DifferentialIKController(cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]
"""
# resolve environment ids
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# reset the action history
self._prev_action[env_ids] = 0.0
self._action[env_ids] = 0.0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ def compute(self, env_ids: Sequence[int] | None = None):
"""
# resolve environment indices
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# iterate over all the curriculum terms
for name, term_cfg in zip(self._term_names, self._term_cfgs):
state = term_cfg.func(self._env, env_ids, **term_cfg.params)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]
"""
# resolve environment ids
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# store information
extras = {}
for key in self._episode_sums.keys():
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]
"""
# resolve environment ids
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# add to episode dict
extras = {}
for key in self._episode_dones.keys():
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ def reset(self, env_ids: Sequence[int] | None = None):
super().reset(env_ids)
# resolve None
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# reset accumulative data buffers
self._data.current_air_time[env_ids] = 0.0
self._data.last_air_time[env_ids] = 0.0
Expand Down Expand Up @@ -223,7 +223,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
"""Fills the buffers of the sensor data."""
# default to all sensors
if len(env_ids) == self._num_envs:
env_ids = ...
env_ids = slice(None)
# obtain the poses of the sensors:
# TODO decide if we really to track poses -- This is the body's CoM. Not contact location.
pose = self.body_physx_view.get_transforms()
Expand Down Expand Up @@ -257,7 +257,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
# since this function is called every frame, we can use the difference to get the elapsed time
elapsed_time = self._timestamp[env_ids] - self._timestamp_last_update[env_ids]
# -- check contact state of bodies
is_contact = torch.norm(self._data.net_forces_w[env_ids, 0, :, :], dim=-1) > 1.0
is_contact = torch.norm(self._data.net_forces_w[env_ids, :, :], dim=-1) > 1.0
is_first_contact = (self._data.current_air_time[env_ids] > 0) * is_contact
# -- update ongoing timer for bodies air
self._data.current_air_time[env_ids] += elapsed_time.unsqueeze(-1)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ def reset(self, env_ids: Sequence[int] | None = None):
"""
# Resolve sensor ids
if env_ids is None:
env_ids = ...
env_ids = slice(None)
# Reset the timestamp for the sensors
self._timestamp[env_ids] = 0.0
self._timestamp_last_update[env_ids] = 0.0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -380,6 +380,9 @@ def render(self, mode: RenderMode | None = None):
self._render_throttle_counter = 0
# here we don't render viewport so don't need to flush flatcache
super().render()
elif self.render_mode == self.RenderMode.HEADLESS:
# we never want to render anything here -- camera rendering will also not work then?
pass
else:
# this is called even if we are in headless mode - we allow this for off-screen rendering
# manually flush the flatcache data to update Hydra textures
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@

import carb
import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.version import get_version
import omni.kit.commands
from omni.isaac.version import get_version
from pxr import Gf, Sdf, Usd

from omni.isaac.orbit.sim import loaders, schemas
Expand Down
38 changes: 38 additions & 0 deletions source/extensions/omni.isaac.orbit/test/isaacsim/test_torch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

from __future__ import annotations

import torch
import unittest


class TestTorchOperations(unittest.TestCase):
"""Tests for assuring torch related operations used in Orbit."""

def test_array_slicing(self):
"""Check that using ellipsis and slices work for torch tensors."""

size = (400, 300, 5)
my_tensor = torch.rand(size, device="cuda:0")

self.assertEqual(my_tensor[..., 0].shape, (400, 300))
self.assertEqual(my_tensor[:, :, 0].shape, (400, 300))
self.assertEqual(my_tensor[slice(None), slice(None), 0].shape, (400, 300))
with self.assertRaises(IndexError):
my_tensor[..., ..., 0]

self.assertEqual(my_tensor[0, ...].shape, (300, 5))
self.assertEqual(my_tensor[0, :, :].shape, (300, 5))
self.assertEqual(my_tensor[0, slice(None), slice(None)].shape, (300, 5))
self.assertEqual(my_tensor[0, ..., ...].shape, (300, 5))

self.assertEqual(my_tensor[..., 0, 0].shape, (400,))
self.assertEqual(my_tensor[slice(None), 0, 0].shape, (400,))
self.assertEqual(my_tensor[:, 0, 0].shape, (400,))


if __name__ == "__main__":
unittest.main()
2 changes: 1 addition & 1 deletion source/standalone/environments/state_machine/play_lift.py
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,7 @@ def __init__(self, dt: float, num_envs: int, device: torch.device | str = "cpu")
def reset_idx(self, env_ids: Sequence[int] = None):
"""Reset the state machine."""
if env_ids is None:
env_ids = ...
env_ids = slice(None)
self.sm_state[env_ids] = 0
self.sm_wait_time[env_ids] = 0.0

Expand Down

0 comments on commit c6af307

Please sign in to comment.