From e6f3d6b8e3c5255c93e187c6ec2b3d7810f581ce Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Thu, 4 Jul 2024 14:27:51 +0200 Subject: [PATCH 01/12] simplifies external force --- .../lab/assets/articulation/articulation.py | 71 +------------------ .../lab/assets/rigid_object/rigid_object.py | 21 +----- .../test/assets/test_articulation.py | 20 ------ 3 files changed, 4 insertions(+), 108 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py index e2198f217f..cfae257459 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py @@ -9,7 +9,6 @@ from __future__ import annotations import torch -import warnings from collections.abc import Sequence from prettytable import PrettyTable from typing import TYPE_CHECKING @@ -156,25 +155,6 @@ def root_physx_view(self) -> physx.ArticulationView: """ return self._root_physx_view - @property - def body_physx_view(self) -> physx.RigidBodyView: - """Rigid body view for the asset (PhysX). - - .. deprecated:: v0.3.0 - - In previous versions, this attribute returned the rigid body view over all the links of the articulation. - However, this led to confusion with the link ordering as they were not ordered in the same way as the - articulation view. - - Therefore, this attribute will be removed in v0.4.0. Please use the :attr:`root_physx_view` attribute - instead. - - """ - dep_msg = "The attribute 'body_physx_view' will be removed in v0.4.0. Please use 'root_physx_view' instead." - warnings.warn(dep_msg, DeprecationWarning) - carb.log_error(dep_msg) - return self._body_physx_view - """ Operations. """ @@ -194,16 +174,8 @@ def write_data_to_sim(self): If any explicit actuators are present, then the actuator models are used to compute the joint commands. Otherwise, the joint commands are directly set into the simulation. """ - # write external wrench - if self.has_external_wrench: - # apply external forces and torques - self._body_physx_view.apply_forces_and_torques_at_position( - force_data=self._external_force_body_view_b.view(-1, 3), - torque_data=self._external_torque_body_view_b.view(-1, 3), - position_data=None, - indices=self._ALL_BODY_INDICES, - is_global=False, - ) + # apply external forces and torques + super().write_data_to_sim() # apply actuator models self._apply_actuator_model() @@ -260,24 +232,6 @@ def find_fixed_tendons( # find tendons return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order) - """ - Operations - Setters. - """ - - def set_external_force_and_torque( - self, - forces: torch.Tensor, - torques: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, - ): - # call parent to set the external forces and torques into buffers - super().set_external_force_and_torque(forces, torques, body_ids, env_ids) - # reordering of the external forces and torques to match the body view ordering - if self.has_external_wrench: - self._external_force_body_view_b = self._external_force_b[:, self._body_view_ordering] - self._external_torque_body_view_b = self._external_torque_b[:, self._body_view_ordering] - """ Operations - Writers. """ @@ -821,24 +775,6 @@ def _initialize_impl(self): root_prim_path_expr = self.cfg.prim_path + root_prim_path[len(template_prim_path) :] # -- articulation self._root_physx_view = self._physics_sim_view.create_articulation_view(root_prim_path_expr.replace(".*", "*")) - # -- link views - # note: we use the root view to get the body names, but we use the body view to get the - # actual data. This is mainly needed to apply external forces to the bodies. - physx_body_names = self.root_physx_view.shared_metatype.link_names - body_names_regex = r"(" + "|".join(physx_body_names) + r")" - body_names_regex = f"{self.cfg.prim_path}/{body_names_regex}" - self._body_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_regex.replace(".*", "*")) - - # create ordering from articulation view to body view for body names - # note: we need to do this since the body view is not ordered in the same way as the articulation view - # -- root view - root_view_body_names = self.body_names - # -- body view - prim_paths = self._body_physx_view.prim_paths[: self.num_bodies] - body_view_body_names = [path.split("/")[-1] for path in prim_paths] - # -- mapping from articulation view to body view - self._body_view_ordering = [root_view_body_names.index(name) for name in body_view_body_names] - self._body_view_ordering = torch.tensor(self._body_view_ordering, dtype=torch.long, device=self.device) # log information about the articulation carb.log_info(f"Articulation initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.") @@ -848,9 +784,6 @@ def _initialize_impl(self): carb.log_info(f"Number of joints: {self.num_joints}") carb.log_info(f"Joint names: {self.joint_names}") carb.log_info(f"Number of fixed tendons: {self.num_fixed_tendons}") - # -- assert that parsing was successful - if set(physx_body_names) != set(self.body_names): - raise RuntimeError("Failed to parse all bodies properly in the articulation.") # container for data access self._data = ArticulationData(self.root_physx_view, self.device) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py index 13a87117eb..a402b5468b 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py @@ -6,7 +6,6 @@ from __future__ import annotations import torch -import warnings from collections.abc import Sequence from typing import TYPE_CHECKING @@ -89,20 +88,6 @@ def root_physx_view(self) -> physx.RigidBodyView: """ return self._root_physx_view - @property - def body_physx_view(self) -> physx.RigidBodyView: - """Rigid body view for the asset (PhysX). - - .. deprecated:: v0.3.0 - - The attribute 'body_physx_view' will be removed in v0.4.0. Please use :attr:`root_physx_view` instead. - - """ - dep_msg = "The attribute 'body_physx_view' will be removed in v0.4.0. Please use 'root_physx_view' instead." - warnings.warn(dep_msg, DeprecationWarning) - carb.log_error(dep_msg) - return self.root_physx_view - """ Operations. """ @@ -128,7 +113,7 @@ def write_data_to_sim(self): force_data=self._external_force_b.view(-1, 3), torque_data=self._external_torque_b.view(-1, 3), position_data=None, - indices=self._ALL_BODY_INDICES, + indices=self._ALL_INDICES, is_global=False, ) @@ -332,9 +317,7 @@ def _create_buffers(self): """Create buffers for storing data.""" # constants self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) - self._ALL_BODY_INDICES = torch.arange( - self.root_physx_view.count * self.num_bodies, dtype=torch.long, device=self.device - ) + # external forces and torques self.has_external_wrench = False self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device) diff --git a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py index b9757bc36e..631abd84f7 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -93,10 +93,6 @@ def test_initialization_floating_base_non_root(self): prim_path_body_names = [path.split("/")[-1] for path in robot.root_physx_view.link_paths[0]] self.assertListEqual(prim_path_body_names, robot.body_names) - # Check that the body_physx_view is deprecated - with self.assertWarns(DeprecationWarning): - robot.body_physx_view - # Simulate physics for _ in range(10): # perform rendering @@ -132,10 +128,6 @@ def test_initialization_floating_base(self): prim_path_body_names = [path.split("/")[-1] for path in robot.root_physx_view.link_paths[0]] self.assertListEqual(prim_path_body_names, robot.body_names) - # Check that the body_physx_view is deprecated - with self.assertWarns(DeprecationWarning): - robot.body_physx_view - # Simulate physics for _ in range(10): # perform rendering @@ -171,10 +163,6 @@ def test_initialization_fixed_base(self): prim_path_body_names = [path.split("/")[-1] for path in robot.root_physx_view.link_paths[0]] self.assertListEqual(prim_path_body_names, robot.body_names) - # Check that the body_physx_view is deprecated - with self.assertWarns(DeprecationWarning): - robot.body_physx_view - # Simulate physics for _ in range(10): # perform rendering @@ -225,10 +213,6 @@ def test_initialization_fixed_base_single_joint(self): prim_path_body_names = [path.split("/")[-1] for path in robot.root_physx_view.link_paths[0]] self.assertListEqual(prim_path_body_names, robot.body_names) - # Check that the body_physx_view is deprecated - with self.assertWarns(DeprecationWarning): - robot.body_physx_view - # Simulate physics for _ in range(10): # perform rendering @@ -299,10 +283,6 @@ def test_initialization_floating_base_made_fixed_base(self): prim_path_body_names = [path.split("/")[-1] for path in robot.root_physx_view.link_paths[0]] self.assertListEqual(prim_path_body_names, robot.body_names) - # Check that the body_physx_view is deprecated - with self.assertWarns(DeprecationWarning): - robot.body_physx_view - # Root state should be at the default state robot.write_root_state_to_sim(robot.data.default_root_state.clone()) # Simulate physics From 087d2bdbe20c5201cc02bbd939ebbd5c8711291a Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Thu, 4 Jul 2024 14:53:28 +0200 Subject: [PATCH 02/12] decouples articulation class --- .../lab/assets/articulation/articulation.py | 176 +++++++++++++++++- .../assets/articulation/articulation_cfg.py | 26 ++- .../assets/articulation/articulation_data.py | 137 ++++++++++++-- .../lab/assets/rigid_object/rigid_object.py | 22 +-- .../assets/rigid_object/rigid_object_data.py | 16 +- .../test/assets/test_articulation.py | 2 + 6 files changed, 336 insertions(+), 43 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py index cfae257459..671de51e8c 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py @@ -24,14 +24,14 @@ import omni.isaac.lab.utils.string as string_utils from omni.isaac.lab.actuators import ActuatorBase, ActuatorBaseCfg, ImplicitActuator -from ..rigid_object import RigidObject +from ..asset_base import AssetBase from .articulation_data import ArticulationData if TYPE_CHECKING: from .articulation_cfg import ArticulationCfg -class Articulation(RigidObject): +class Articulation(AssetBase): """An articulation asset class. An articulation is a collection of rigid bodies connected by joints. The joints can be either @@ -116,6 +116,10 @@ def is_fixed_base(self) -> bool: """Whether the articulation is a fixed-base or floating-base system.""" return self.root_physx_view.shared_metatype.fixed_base + @property + def num_instances(self) -> int: + return self.root_physx_view.count + @property def num_joints(self) -> int: """Number of joints in articulation.""" @@ -160,22 +164,35 @@ def root_physx_view(self) -> physx.ArticulationView: """ 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 = slice(None) # reset actuators for actuator in self.actuators.values(): actuator.reset(env_ids) + # reset external wrench + self._external_force_b[env_ids] = 0.0 + self._external_torque_b[env_ids] = 0.0 def write_data_to_sim(self): """Write external wrenches and joint commands to the simulation. If any explicit actuators are present, then the actuator models are used to compute the joint commands. Otherwise, the joint commands are directly set into the simulation. + + Note: + We write external wrench to the simulation here since this function is called before the simulation step. + This ensures that the external wrench is applied at every simulation step. """ - # apply external forces and torques - super().write_data_to_sim() + # write external wrench + if self.has_external_wrench: + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._external_force_b.view(-1, 3), + torque_data=self._external_torque_b.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) # apply actuator models self._apply_actuator_model() @@ -186,6 +203,28 @@ def write_data_to_sim(self): self.root_physx_view.set_dof_position_targets(self._joint_pos_target_sim, self._ALL_INDICES) self.root_physx_view.set_dof_velocity_targets(self._joint_vel_target_sim, self._ALL_INDICES) + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Finders. + """ + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the articulation based on the name keys. + + Please check the :meth:`omni.isaac.lab.utils.string_utils.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return string_utils.resolve_matching_names(name_keys, self.body_names, preserve_order) + def find_joints( self, name_keys: str | Sequence[str], joint_subset: list[str] | None = None, preserve_order: bool = False ) -> tuple[list[int], list[str]]: @@ -236,7 +275,29 @@ def find_fixed_tendons( Operations - Writers. """ + def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + # set into simulation + self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ # resolve all indices physx_env_ids = env_ids if env_ids is None: @@ -252,6 +313,12 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] self.root_physx_view.set_root_transforms(root_poses_xyzw, indices=physx_env_ids) def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root velocity over selected environment indices into the simulation. + + Args: + root_velocity: Root velocities in simulation frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ # resolve all indices physx_env_ids = env_ids if env_ids is None: @@ -470,9 +537,70 @@ def write_joint_limits_to_sim( self.root_physx_view.set_dof_limits(self._data.joint_limits.cpu(), indices=physx_env_ids.cpu()) """ - Operations - State. + Operations - Setters. """ + def set_external_force_and_torque( + self, + forces: torch.Tensor, + torques: torch.Tensor, + body_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set external force and torque to apply on the asset's bodies in their local frame. + + For many applications, we want to keep the applied external force on rigid bodies constant over a period of + time (for instance, during the policy control). This function allows us to store the external force and torque + into buffers which are then applied to the simulation at every step. + + .. caution:: + If the function is called with empty forces and torques, then this function disables the application + of external wrench to the simulation. + + .. code-block:: python + + # example of disabling external wrench + asset.set_external_force_and_torque(forces=torch.zeros(0, 3), torques=torch.zeros(0, 3)) + + .. note:: + This function does not apply the external wrench to the simulation. It only fills the buffers with + the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function + right before the simulation step. + + Args: + forces: External forces in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + torques: External torques in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + body_ids: Body indices to apply external wrench to. Defaults to None (all bodies). + env_ids: Environment indices to apply external wrench to. Defaults to None (all instances). + """ + if forces.any() or torques.any(): + self.has_external_wrench = True + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + elif not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.long, device=self.device) + # -- body_ids + if body_ids is None: + body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device) + elif isinstance(body_ids, slice): + body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device)[body_ids] + elif not isinstance(body_ids, torch.Tensor): + body_ids = torch.tensor(body_ids, dtype=torch.long, device=self.device) + + # note: we need to do this complicated indexing since torch doesn't support multi-indexing + # create global body indices from env_ids and env_body_ids + # (env_id * total_bodies_per_env) + body_id + indices = body_ids.repeat(len(env_ids), 1) + env_ids.unsqueeze(1) * self.num_bodies + indices = indices.view(-1) + # set into internal buffers + # note: these are applied in the write_to_sim function + self._external_force_b.flatten(0, 1)[indices] = forces.flatten(0, 1) + self._external_torque_b.flatten(0, 1)[indices] = torques.flatten(0, 1) + else: + self.has_external_wrench = False + def set_joint_position_target( self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None ): @@ -802,12 +930,21 @@ def _initialize_impl(self): self._log_articulation_joint_info() def _create_buffers(self): - # allocate buffers - super()._create_buffers() + # constants + self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + + # external forces and torques + self.has_external_wrench = False + self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device) + self._external_torque_b = torch.zeros_like(self._external_force_b) # asset data # -- properties self._data.joint_names = self.joint_names + self._data.body_names = self.body_names + + # -- bodies + self._data.default_mass = self.root_physx_view.get_masses().clone() # -- default joint state self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device) @@ -896,7 +1033,16 @@ def _create_buffers(self): def _process_cfg(self): """Post processing of configuration parameters.""" # default state - super()._process_cfg() + # -- root state + # note: we cast to tuple to avoid torch/numpy type mismatch. + default_root_state = ( + tuple(self.cfg.init_state.pos) + + tuple(self.cfg.init_state.rot) + + tuple(self.cfg.init_state.lin_vel) + + tuple(self.cfg.init_state.ang_vel) + ) + default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device) + self._data.default_root_state = default_root_state.repeat(self.num_instances, 1) # -- joint state # joint pos indices_list, _, values_list = string_utils.resolve_matching_names_values( @@ -913,6 +1059,18 @@ def _process_cfg(self): self._data.default_joint_limits = self.root_physx_view.get_dof_limits().to(device=self.device).clone() self._data.joint_limits = self._data.default_joint_limits.clone() + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._physics_sim_view = None + self._root_physx_view = None + """ Internal helpers -- Actuators. """ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py index 57b0958159..d02cd3138b 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py @@ -8,21 +8,25 @@ from omni.isaac.lab.actuators import ActuatorBaseCfg from omni.isaac.lab.utils import configclass -from ..rigid_object import RigidObjectCfg +from ..asset_base_cfg import AssetBaseCfg from .articulation import Articulation @configclass -class ArticulationCfg(RigidObjectCfg): +class ArticulationCfg(AssetBaseCfg): """Configuration parameters for an articulation.""" - class_type: type = Articulation - @configclass - class InitialStateCfg(RigidObjectCfg.InitialStateCfg): + class InitialStateCfg(AssetBaseCfg.InitialStateCfg): """Initial state of the articulation.""" - # root position + # root velocity + lin_vel: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Linear velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + ang_vel: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Angular velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + + # joint state joint_pos: dict[str, float] = {".*": 0.0} """Joint positions of the joints. Defaults to 0.0 for all joints.""" joint_vel: dict[str, float] = {".*": 0.0} @@ -32,10 +36,18 @@ class InitialStateCfg(RigidObjectCfg.InitialStateCfg): # Initialize configurations. ## + class_type: type = Articulation + init_state: InitialStateCfg = InitialStateCfg() """Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state.""" + soft_joint_pos_limit_factor: float = 1.0 """Fraction specifying the range of DOF position limits (parsed from the asset) to use. - Defaults to 1.0.""" + Defaults to 1.0. + + The joint position limits are scaled by this factor to allow for a limited range of motion. + This is accessible in the articulation data through :attr:`ArticulationData.soft_joint_pos_limits` attribute. + """ + actuators: dict[str, ActuatorBaseCfg] = MISSING """Actuators for the robot with corresponding joint names.""" diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 209d5a5aa6..240354b57f 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -4,16 +4,15 @@ # SPDX-License-Identifier: BSD-3-Clause import torch +import weakref import omni.physics.tensors.impl.api as physx import omni.isaac.lab.utils.math as math_utils from omni.isaac.lab.utils.buffers import TimestampedBuffer -from ..rigid_object import RigidObjectData - -class ArticulationData(RigidObjectData): +class ArticulationData: """Data container for an articulation. This class extends the :class:`RigidObjectData` class to provide additional data for @@ -29,13 +28,39 @@ class ArticulationData(RigidObjectData): """ def __init__(self, root_physx_view: physx.ArticulationView, device: str): - # Initialize the parent class - super().__init__(root_physx_view, device) # type: ignore + """Initializes the articulation data. + + Args: + root_physx_view: The root articulation view. + device: The device used for processing. + """ + # Set the parameters + self.device = device + self._root_physx_view = weakref.proxy(root_physx_view) # weak reference to avoid circular references + # Set initial time stamp + self._sim_timestamp = 0.0 + + # Obtain global physics sim view + physics_sim_view = physx.create_simulation_view("torch") + physics_sim_view.set_subspace_roots("/") + gravity = physics_sim_view.get_gravity() + # Convert to direction vector + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) + gravity_dir = math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0) + + # Initialize constants + self.GRAVITY_VEC_W = gravity_dir.repeat(self._root_physx_view.count, 1) + self.FORWARD_VEC_B = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat(self._root_physx_view.count, 1) + + # Initialize buffers for finite differencing + self._previous_body_vel_w = torch.zeros((self._root_physx_view.count, 1, 6), device=self.device) # Initialize history for finite differencing self._previous_joint_vel = self._root_physx_view.get_dof_velocities().clone() # Initialize the lazy buffers. + self._root_state_w = TimestampedBuffer() + self._body_acc_w = TimestampedBuffer() self._body_state_w = TimestampedBuffer() self._joint_pos = TimestampedBuffer() self._joint_acc = TimestampedBuffer() @@ -45,12 +70,16 @@ def update(self, dt: float): self._sim_timestamp += dt # Trigger an update of the joint acceleration buffer at a higher frequency # since we do finite differencing. + self.body_acc_w self.joint_acc ## # Names. ## + body_names: list[str] = None + """Body names in the order parsed by the simulation view.""" + joint_names: list[str] = None """Joint names in the order parsed by the simulation view.""" @@ -61,6 +90,12 @@ def update(self, dt: float): # Defaults. ## + default_root_state: torch.Tensor = None + """Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, 13).""" + + default_mass: torch.Tensor = None + """ Default mass provided by simulation. Shape is (num_instances, num_bodies).""" + default_joint_pos: torch.Tensor = None """Default joint positions of all joints. Shape is (num_instances, num_joints).""" @@ -246,14 +281,20 @@ def body_acc_w(self): return self._body_acc_w.data @property - def body_lin_acc_w(self) -> torch.Tensor: - """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" - return self.body_acc_w[..., 0:3] + def projected_gravity_b(self): + """Projection of the gravity direction on base frame. Shape is (num_instances, 3).""" + return math_utils.quat_rotate_inverse(self.root_quat_w, self.GRAVITY_VEC_W) @property - def body_ang_acc_w(self) -> torch.Tensor: - """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" - return self.body_acc_w[..., 3:6] + def heading_w(self): + """Yaw heading of the base frame (in radians). Shape is (num_instances,). + + Note: + This quantity is computed by assuming that the forward-direction of the base + frame is along x-direction, i.e. :math:`(1, 0, 0)`. + """ + forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B) + return torch.atan2(forward_w[:, 1], forward_w[:, 0]) @property def joint_pos(self): @@ -285,3 +326,77 @@ def joint_acc(self): # update the previous joint velocity self._previous_joint_vel[:] = self.joint_vel return self._joint_acc.data + + ## + # Derived properties. + ## + + @property + def root_pos_w(self) -> torch.Tensor: + """Root position in simulation world frame. Shape is (num_instances, 3).""" + return self.root_state_w[:, :3] + + @property + def root_quat_w(self) -> torch.Tensor: + """Root orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4).""" + return self.root_state_w[:, 3:7] + + @property + def root_vel_w(self) -> torch.Tensor: + """Root velocity in simulation world frame. Shape is (num_instances, 6).""" + return self.root_state_w[:, 7:13] + + @property + def root_lin_vel_w(self) -> torch.Tensor: + """Root linear velocity in simulation world frame. Shape is (num_instances, 3).""" + return self.root_state_w[:, 7:10] + + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Root angular velocity in simulation world frame. Shape is (num_instances, 3).""" + return self.root_state_w[:, 10:13] + + @property + def root_lin_vel_b(self) -> torch.Tensor: + """Root linear velocity in base frame. Shape is (num_instances, 3).""" + return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_lin_vel_w) + + @property + def root_ang_vel_b(self) -> torch.Tensor: + """Root angular velocity in base world frame. Shape is (num_instances, 3).""" + return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w) + + @property + def body_pos_w(self) -> torch.Tensor: + """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_state_w[..., :3] + + @property + def body_quat_w(self) -> torch.Tensor: + """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 4).""" + return self.body_state_w[..., 3:7] + + @property + def body_vel_w(self) -> torch.Tensor: + """Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6).""" + return self.body_state_w[..., 7:13] + + @property + def body_lin_vel_w(self) -> torch.Tensor: + """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_state_w[..., 7:10] + + @property + def body_ang_vel_w(self) -> torch.Tensor: + """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_state_w[..., 10:13] + + @property + def body_lin_acc_w(self) -> torch.Tensor: + """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_acc_w[..., 0:3] + + @property + def body_ang_acc_w(self) -> torch.Tensor: + """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_acc_w[..., 3:6] diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py index a402b5468b..1296f56b9c 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py @@ -120,6 +120,10 @@ def write_data_to_sim(self): def update(self, dt: float): self._data.update(dt) + """ + Operations - Finders. + """ + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: """Find bodies in the articulation based on the name keys. @@ -241,23 +245,11 @@ def set_external_force_and_torque( env_ids = self._ALL_INDICES elif not isinstance(env_ids, torch.Tensor): env_ids = torch.tensor(env_ids, dtype=torch.long, device=self.device) - # -- body_ids - if body_ids is None: - body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device) - elif isinstance(body_ids, slice): - body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device)[body_ids] - elif not isinstance(body_ids, torch.Tensor): - body_ids = torch.tensor(body_ids, dtype=torch.long, device=self.device) - - # note: we need to do this complicated indexing since torch doesn't support multi-indexing - # create global body indices from env_ids and env_body_ids - # (env_id * total_bodies_per_env) + body_id - indices = body_ids.repeat(len(env_ids), 1) + env_ids.unsqueeze(1) * self.num_bodies - indices = indices.view(-1) + # set into internal buffers # note: these are applied in the write_to_sim function - self._external_force_b.flatten(0, 1)[indices] = forces.flatten(0, 1) - self._external_torque_b.flatten(0, 1)[indices] = torques.flatten(0, 1) + self._external_force_b[env_ids] = forces + self._external_torque_b[env_ids] = torques else: self.has_external_wrench = False diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py index 21463785b0..9969de63b6 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py @@ -36,7 +36,7 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str): """Initializes the rigid object data. Args: - root_physx_view: The root rigid body view of the object. + root_physx_view: The root rigid body view. device: The device used for processing. """ # Set the parameters @@ -143,6 +143,10 @@ def heading_w(self): forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B) return torch.atan2(forward_w[:, 1], forward_w[:, 0]) + ## + # Derived properties. + ## + @property def root_pos_w(self) -> torch.Tensor: """Root position in simulation world frame. Shape is (num_instances, 3).""" @@ -202,3 +206,13 @@ def body_lin_vel_w(self) -> torch.Tensor: def body_ang_vel_w(self) -> torch.Tensor: """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" return self.body_state_w[..., 10:13] + + @property + def body_lin_acc_w(self) -> torch.Tensor: + """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_acc_w[..., 0:3] + + @property + def body_ang_acc_w(self) -> torch.Tensor: + """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + return self.body_acc_w[..., 3:6] diff --git a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py index 631abd84f7..1758b23bd1 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -47,6 +47,8 @@ def setUp(self): sim_cfg = sim_utils.SimulationCfg(dt=self.dt, device="cuda:0") self.sim = sim_utils.SimulationContext(sim_cfg) + torch.zeros(1, device=self.sim.device) # dummy tensor to initialize CUDA context + def tearDown(self): """Stops simulator after each test.""" # stop simulation From 41d91d5ca7322582e956ccee5426f15c98736bb6 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Thu, 4 Jul 2024 14:59:01 +0200 Subject: [PATCH 03/12] updates version --- .../omni.isaac.lab/config/extension.toml | 2 +- .../omni.isaac.lab/docs/CHANGELOG.rst | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 1b42771082..58d6fd0466 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.18.6" +version = "0.19.0" # 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 767ef45496..68abf65f4d 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,25 @@ Changelog --------- +0.19.0 (2024-07-04) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed parsing of articulations with nested rigid links while using the :class:`omni.isaac.lab.assets.Articulation` + class. Earlier, the class initialization failed when the articulation had nested rigid links since the rigid + links were not being parsed correctly by the PhysX view. + +Removed +^^^^^^^ + +* Removed the attribute :attr:`body_physx_view` from the :class:`omni.isaac.lab.assets.Articulation` and + :class:`omni.isaac.lab.assets.RigidObject` classes. These were causing confusions when used with articulation + view since the body names were not following the same ordering. +* Dropped support for Isaac Sim 2023.1.1. The minimum supported version is now Isaac Sim 4.0.0. + + 0.18.6 (2024-07-01) ~~~~~~~~~~~~~~~~~~~ From e5b73f453a1e260594c5d4cdb55b5bc0f7a9cc01 Mon Sep 17 00:00:00 2001 From: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com> Date: Fri, 2 Aug 2024 13:56:16 +0200 Subject: [PATCH 04/12] Apply suggestions from code review Signed-off-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com> --- .../extensions/omni.isaac.lab/test/assets/test_articulation.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py index 8c0b56bbcd..3fda8fcc9e 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -46,9 +46,6 @@ def setUp(self): # Load kit helper sim_cfg = sim_utils.SimulationCfg(dt=self.dt, device="cuda:0") self.sim = sim_utils.SimulationContext(sim_cfg) - - torch.zeros(1, device=self.sim.device) # dummy tensor to initialize CUDA context - def tearDown(self): """Stops simulator after each test.""" # stop simulation From 5a88fb71de9020f90dc8d7a2bb803f8c19368163 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 13:58:32 +0200 Subject: [PATCH 05/12] runs formatter --- .../omni/isaac/lab/assets/articulation/articulation_data.py | 2 +- .../extensions/omni.isaac.lab/test/assets/test_articulation.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 1e1af51aae..22f5b334ea 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -426,4 +426,4 @@ def body_ang_acc_w(self) -> torch.Tensor: This quantity is the angular acceleration of the articulation links' center of mass frame. """ - return self.body_acc_w[..., 3:6] \ No newline at end of file + return self.body_acc_w[..., 3:6] diff --git a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py index 3fda8fcc9e..f96dfc4882 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -46,6 +46,7 @@ def setUp(self): # Load kit helper sim_cfg = sim_utils.SimulationCfg(dt=self.dt, device="cuda:0") self.sim = sim_utils.SimulationContext(sim_cfg) + def tearDown(self): """Stops simulator after each test.""" # stop simulation From 989f0034c14d32d5cb66d3c9ff77138c5673ae17 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 14:11:26 +0200 Subject: [PATCH 06/12] fix docstrings --- .../assets/articulation/articulation_data.py | 80 ++++++++++++++----- .../assets/rigid_object/rigid_object_data.py | 15 ++-- 2 files changed, 70 insertions(+), 25 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 22f5b334ea..8035e4a3e2 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -80,7 +80,6 @@ def update(self, dt: float): self._sim_timestamp += dt # Trigger an update of the joint acceleration buffer at a higher frequency # since we do finite differencing. - self.body_acc_w self.joint_acc ## @@ -101,10 +100,14 @@ def update(self, dt: float): ## default_root_state: torch.Tensor = None - """Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, 13).""" + """Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, 13). + + The position and quaternion are of the articulation root's actor frame. Meanwhile, the linear and angular + velocities are of its center of mass frame. + """ default_mass: torch.Tensor = None - """ Default mass provided by simulation. Shape is (num_instances, num_bodies).""" + """Default mass read from the simulation. Shape is (num_instances, num_bodies).""" default_joint_pos: torch.Tensor = None """Default joint positions of all joints. Shape is (num_instances, num_joints).""" @@ -340,9 +343,8 @@ def joint_acc(self): """Joint acceleration of all joints. Shape is (num_instances, num_joints).""" if self._joint_acc.timestamp < self._sim_timestamp: # note: we use finite differencing to compute acceleration - self._joint_acc.data = (self.joint_vel - self._previous_joint_vel) / ( - self._sim_timestamp - self._joint_acc.timestamp - ) + time_elapsed = self._sim_timestamp - self._joint_acc.timestamp + self._joint_acc.data = (self.joint_vel - self._previous_joint_vel) / time_elapsed self._joint_acc.timestamp = self._sim_timestamp # update the previous joint velocity self._previous_joint_vel[:] = self.joint_vel @@ -354,69 +356,107 @@ def joint_acc(self): @property def root_pos_w(self) -> torch.Tensor: - """Root position in simulation world frame. Shape is (num_instances, 3).""" + """Root position in simulation world frame. Shape is (num_instances, 3). + + This quantity is the position of the actor frame of the root rigid body. + """ return self.root_state_w[:, :3] @property def root_quat_w(self) -> torch.Tensor: - """Root orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4).""" + """Root orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4). + + This quantity is the orientation of the actor frame of the root rigid body. + """ return self.root_state_w[:, 3:7] @property def root_vel_w(self) -> torch.Tensor: - """Root velocity in simulation world frame. Shape is (num_instances, 6).""" + """Root velocity in simulation world frame. Shape is (num_instances, 6). + + This quantity contains the linear and angular velocities of the root rigid body's center of mass frame. + """ return self.root_state_w[:, 7:13] @property def root_lin_vel_w(self) -> torch.Tensor: - """Root linear velocity in simulation world frame. Shape is (num_instances, 3).""" + """Root linear velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's center of mass frame. + """ return self.root_state_w[:, 7:10] @property def root_ang_vel_w(self) -> torch.Tensor: - """Root angular velocity in simulation world frame. Shape is (num_instances, 3).""" + """Root angular velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the root rigid body's center of mass frame. + """ return self.root_state_w[:, 10:13] @property def root_lin_vel_b(self) -> torch.Tensor: - """Root linear velocity in base frame. Shape is (num_instances, 3).""" + """Root linear velocity in base frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the + rigid body's actor frame. + """ return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_lin_vel_w) @property def root_ang_vel_b(self) -> torch.Tensor: - """Root angular velocity in base world frame. Shape is (num_instances, 3).""" + """Root angular velocity in base world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the + rigid body's actor frame. + """ return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w) @property def body_pos_w(self) -> torch.Tensor: - """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the position of the rigid bodies' actor frame. + """ return self.body_state_w[..., :3] @property def body_quat_w(self) -> torch.Tensor: - """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 4).""" + """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 4). + + This quantity is the orientation of the rigid bodies' actor frame. + """ return self.body_state_w[..., 3:7] @property def body_vel_w(self) -> torch.Tensor: - """Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6).""" + """Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6). + + This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame. + """ return self.body_state_w[..., 7:13] @property def body_lin_vel_w(self) -> torch.Tensor: - """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the linear velocity of the rigid bodies' center of mass frame. + """ return self.body_state_w[..., 7:10] @property def body_ang_vel_w(self) -> torch.Tensor: - """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the angular velocity of the rigid bodies' center of mass frame. + """ return self.body_state_w[..., 10:13] @property def body_lin_acc_w(self) -> torch.Tensor: """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). - This quantity is the linear acceleration of the articulation links' center of mass frame. + This quantity is the linear acceleration of the rigid bodies' center of mass frame. """ return self.body_acc_w[..., 0:3] @@ -424,6 +464,6 @@ def body_lin_acc_w(self) -> torch.Tensor: def body_ang_acc_w(self) -> torch.Tensor: """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). - This quantity is the angular acceleration of the articulation links' center of mass frame. + This quantity is the angular acceleration of the rigid bodies' center of mass frame. """ return self.body_acc_w[..., 3:6] diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py index f353b0be20..446a062852 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py @@ -97,7 +97,7 @@ def update(self, dt: float): """ default_mass: torch.Tensor = None - """ Default mass provided by simulation. Shape is (num_instances, num_bodies).""" + """Default mass read from the simulation. Shape is (num_instances, num_bodies).""" ## # Properties. @@ -133,8 +133,7 @@ def body_state_w(self): def body_acc_w(self): """Acceleration of all bodies. Shape is (num_instances, 1, 6). - This quantity is the acceleration of the rigid bodies' center of mass frame. The acceleration - is computed using finite differencing of the linear and angular velocities of the bodies. + This quantity is the acceleration of the rigid bodies' center of mass frame. """ if self._body_acc_w.timestamp < self._sim_timestamp: # note: we use finite differencing to compute acceleration @@ -262,10 +261,16 @@ def body_ang_vel_w(self) -> torch.Tensor: @property def body_lin_acc_w(self) -> torch.Tensor: - """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the linear acceleration of the rigid bodies' center of mass frame. + """ return self.body_acc_w[..., 0:3] @property def body_ang_acc_w(self) -> torch.Tensor: - """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).""" + """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the angular acceleration of the rigid bodies' center of mass frame. + """ return self.body_acc_w[..., 3:6] From 59d6b9fa04dbca3c4ae88296d8f455cb42f47be7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 14:16:47 +0200 Subject: [PATCH 07/12] fix rigid body occurances in arti --- .../lab/assets/articulation/articulation.py | 5 ---- .../assets/articulation/articulation_data.py | 24 ++++++++++--------- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py index 322bf58f47..8446be320b 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py @@ -48,11 +48,6 @@ class Articulation(AssetBase): articulation root prim and creates the corresponding articulation in the physics engine. The articulation root prim can be specified using the :attr:`AssetBaseCfg.prim_path` attribute. - The articulation class is a subclass of the :class:`RigidObject` class. Therefore, it inherits - all the functionality of the rigid object class. In case of an articulation, the :attr:`root_physx_view` - attribute corresponds to the articulation root view and can be used to access the articulation - related data. - The articulation class also provides the functionality to augment the simulation of an articulated system with custom actuator models. These models can either be explicit or implicit, as detailed in the :mod:`omni.isaac.lab.actuators` module. The actuator models are specified using the diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 8035e4a3e2..35ea1beb93 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -15,8 +15,9 @@ class ArticulationData: """Data container for an articulation. - This class extends the :class:`RigidObjectData` class to provide additional data for - an articulation mainly related to the joints and tendons. + This class contains the data for an articulation in the simulation. The data includes the state of + the root rigid body, the state of all the bodies in the articulation, and the joint state. The data is + stored in the simulation world frame unless otherwise specified. An articulation is comprised of multiple rigid bodies or links. For a rigid body, there are two frames of reference that are used: @@ -358,7 +359,7 @@ def joint_acc(self): def root_pos_w(self) -> torch.Tensor: """Root position in simulation world frame. Shape is (num_instances, 3). - This quantity is the position of the actor frame of the root rigid body. + This quantity is the position of the actor frame of the articulation root. """ return self.root_state_w[:, :3] @@ -366,7 +367,7 @@ def root_pos_w(self) -> torch.Tensor: def root_quat_w(self) -> torch.Tensor: """Root orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4). - This quantity is the orientation of the actor frame of the root rigid body. + This quantity is the orientation of the actor frame of the articulation root. """ return self.root_state_w[:, 3:7] @@ -374,7 +375,8 @@ def root_quat_w(self) -> torch.Tensor: def root_vel_w(self) -> torch.Tensor: """Root velocity in simulation world frame. Shape is (num_instances, 6). - This quantity contains the linear and angular velocities of the root rigid body's center of mass frame. + This quantity contains the linear and angular velocities of the articulation root's center of + mass frame. """ return self.root_state_w[:, 7:13] @@ -382,7 +384,7 @@ def root_vel_w(self) -> torch.Tensor: def root_lin_vel_w(self) -> torch.Tensor: """Root linear velocity in simulation world frame. Shape is (num_instances, 3). - This quantity is the linear velocity of the root rigid body's center of mass frame. + This quantity is the linear velocity of the articulation root's center of mass frame. """ return self.root_state_w[:, 7:10] @@ -390,7 +392,7 @@ def root_lin_vel_w(self) -> torch.Tensor: def root_ang_vel_w(self) -> torch.Tensor: """Root angular velocity in simulation world frame. Shape is (num_instances, 3). - This quantity is the angular velocity of the root rigid body's center of mass frame. + This quantity is the angular velocity of the articulation root's center of mass frame. """ return self.root_state_w[:, 10:13] @@ -398,8 +400,8 @@ def root_ang_vel_w(self) -> torch.Tensor: def root_lin_vel_b(self) -> torch.Tensor: """Root linear velocity in base frame. Shape is (num_instances, 3). - This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the - rigid body's actor frame. + This quantity is the linear velocity of the articulation root's center of mass frame with + respect to the articulation root's actor frame. """ return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_lin_vel_w) @@ -407,8 +409,8 @@ def root_lin_vel_b(self) -> torch.Tensor: def root_ang_vel_b(self) -> torch.Tensor: """Root angular velocity in base world frame. Shape is (num_instances, 3). - This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the - rigid body's actor frame. + This quantity is the angular velocity of the articulation root's center of mass frame with respect to the + articulation root's actor frame. """ return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w) From 62b28183da22b613f75713c8ff6d5dd050fa9614 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 14:17:41 +0200 Subject: [PATCH 08/12] keep the same order --- .../omni/isaac/lab/assets/articulation/articulation.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py index 8446be320b..6405bb3c35 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py @@ -106,15 +106,15 @@ def __init__(self, cfg: ArticulationCfg): def data(self) -> ArticulationData: return self._data + @property + def num_instances(self) -> int: + return self.root_physx_view.count + @property def is_fixed_base(self) -> bool: """Whether the articulation is a fixed-base or floating-base system.""" return self.root_physx_view.shared_metatype.fixed_base - @property - def num_instances(self) -> int: - return self.root_physx_view.count - @property def num_joints(self) -> int: """Number of joints in articulation.""" From 90ecb74c831bd0693c2d4303b53b019280ce48d3 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 14:22:32 +0200 Subject: [PATCH 09/12] adds version log --- .../extensions/omni.isaac.lab/config/extension.toml | 2 +- source/extensions/omni.isaac.lab/docs/CHANGELOG.rst | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 3e2f84a628..2bc311a18a 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.20.3" +version = "0.20.4" # 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 9972105350..7de9e2c1ab 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,19 @@ Changelog --------- +0.20.4 (2024-08-02) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Removed the hierarchy from :class:`~omni.isaac.lab.assets.RigidObject` class to + :class:`~omni.isaac.lab.assets.Articulation` class. Previously, the articulation class overrode almost + all the functions of the rigid object class making the hierarchy redundant. Now, the articulation class + is a standalone class that does not inherit from the rigid object class. This does add some code + duplication but the simplicity and clarity of the code is improved. + + 0.20.3 (2024-08-02) ~~~~~~~~~~~~~~~~~~~ From e38cfcc7b0c0bfe38b6873d176e25a8cba00fda9 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 17:25:51 +0200 Subject: [PATCH 10/12] arranges in the same way --- .../omni/isaac/lab/assets/articulation/articulation_data.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 35ea1beb93..126d7c1ce1 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -71,8 +71,8 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): # Initialize the lazy buffers. self._root_state_w = TimestampedBuffer() - self._body_acc_w = TimestampedBuffer() self._body_state_w = TimestampedBuffer() + self._body_acc_w = TimestampedBuffer() self._joint_pos = TimestampedBuffer() self._joint_acc = TimestampedBuffer() self._joint_vel = TimestampedBuffer() From 385cd6e5321c855f87a4cc47244212fadf4eb4ad Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 17:36:42 +0200 Subject: [PATCH 11/12] removes unused previous body acc --- .../assets/articulation/articulation_data.py | 18 ++++++------------ .../assets/rigid_object/rigid_object_data.py | 15 ++++++--------- 2 files changed, 12 insertions(+), 21 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 126d7c1ce1..b738cd0c37 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -30,14 +30,6 @@ class ArticulationData: can be interpreted as the link frame. """ - _root_physx_view: physx.ArticulationView - """The root articulation view of the object. - - Note: - Internally, this is stored as a weak reference to avoid circular references between the asset class - and the data container. This is important to avoid memory leaks. - """ - def __init__(self, root_physx_view: physx.ArticulationView, device: str): """Initializes the articulation data. @@ -47,7 +39,11 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): """ # Set the parameters self.device = device - self._root_physx_view = weakref.proxy(root_physx_view) # weak reference to avoid circular references + # Set the root articulation view + # note: this is stored as a weak reference to avoid circular references between the asset class + # and the data container. This is important to avoid memory leaks. + self._root_physx_view: physx.ArticulationView = weakref.proxy(root_physx_view) + # Set initial time stamp self._sim_timestamp = 0.0 @@ -63,9 +59,6 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): self.GRAVITY_VEC_W = gravity_dir.repeat(self._root_physx_view.count, 1) self.FORWARD_VEC_B = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat(self._root_physx_view.count, 1) - # Initialize buffers for finite differencing - self._previous_body_vel_w = torch.zeros((self._root_physx_view.count, 1, 6), device=self.device) - # Initialize history for finite differencing self._previous_joint_vel = self._root_physx_view.get_dof_velocities().clone() @@ -78,6 +71,7 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): self._joint_vel = TimestampedBuffer() def update(self, dt: float): + # update the simulation timestamp self._sim_timestamp += dt # Trigger an update of the joint acceleration buffer at a higher frequency # since we do finite differencing. diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py index 446a062852..de358e5cfe 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py @@ -33,14 +33,6 @@ class RigidObjectData: is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. """ - _root_physx_view: physx.RigidBodyView - """The root rigid body view of the object. - - Note: - Internally, this is stored as a weak reference to avoid circular references between the asset class - and the data container. This is important to avoid memory leaks. - """ - def __init__(self, root_physx_view: physx.RigidBodyView, device: str): """Initializes the rigid object data. @@ -50,7 +42,11 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str): """ # Set the parameters self.device = device - self._root_physx_view = weakref.proxy(root_physx_view) # weak reference to avoid circular references + # Set the root rigid body view + # note: this is stored as a weak reference to avoid circular references between the asset class + # and the data container. This is important to avoid memory leaks. + self._root_physx_view: physx.RigidBodyView = weakref.proxy(root_physx_view) + # Set initial time stamp self._sim_timestamp = 0.0 @@ -76,6 +72,7 @@ def update(self, dt: float): Args: dt: The time step for the update. This must be a positive value. """ + # update the simulation timestamp self._sim_timestamp += dt ## From f714532801232172efad46e67d6759d59d7e2f3b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Aug 2024 17:38:28 +0200 Subject: [PATCH 12/12] fix doc --- .../omni/isaac/lab/assets/articulation/articulation_cfg.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py index d02cd3138b..eca98087a3 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_cfg.py @@ -42,8 +42,7 @@ class InitialStateCfg(AssetBaseCfg.InitialStateCfg): """Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state.""" soft_joint_pos_limit_factor: float = 1.0 - """Fraction specifying the range of DOF position limits (parsed from the asset) to use. - Defaults to 1.0. + """Fraction specifying the range of DOF position limits (parsed from the asset) to use. Defaults to 1.0. The joint position limits are scaled by this factor to allow for a limited range of motion. This is accessible in the articulation data through :attr:`ArticulationData.soft_joint_pos_limits` attribute.