Unverified Commit 7b92c575 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Decouples rigid object and articulation asset classes (#644)

# Description

Since we override a lot of the functions from RigidObject inside the
Articulation class, we don't need to rely on inheritance anymore.
Duplicacy in the code makes it easier to understand the two classes'
functionalities without severely added overhead from the maintenance
side. Moreover, conceptually, it can be motivated that the two are
independent concepts.

This MR decouples the rigid object and articulation concepts in the
framework.

## Type of change

- Breaking change (fix or feature that would cause existing
functionality to not work as expected)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] 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
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------
Co-authored-by: 's avatarDavid Hoeller <dhoeller@nvidia.com>
parent 809d090d
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.20.5" version = "0.20.6"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.20.6 (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.5 (2024-08-02) 0.20.5 (2024-08-02)
~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~
......
...@@ -24,14 +24,14 @@ import omni.isaac.lab.utils.math as math_utils ...@@ -24,14 +24,14 @@ import omni.isaac.lab.utils.math as math_utils
import omni.isaac.lab.utils.string as string_utils import omni.isaac.lab.utils.string as string_utils
from omni.isaac.lab.actuators import ActuatorBase, ActuatorBaseCfg, ImplicitActuator from omni.isaac.lab.actuators import ActuatorBase, ActuatorBaseCfg, ImplicitActuator
from ..rigid_object import RigidObject from ..asset_base import AssetBase
from .articulation_data import ArticulationData from .articulation_data import ArticulationData
if TYPE_CHECKING: if TYPE_CHECKING:
from .articulation_cfg import ArticulationCfg from .articulation_cfg import ArticulationCfg
class Articulation(RigidObject): class Articulation(AssetBase):
"""An articulation asset class. """An articulation asset class.
An articulation is a collection of rigid bodies connected by joints. The joints can be either An articulation is a collection of rigid bodies connected by joints. The joints can be either
...@@ -48,11 +48,6 @@ class Articulation(RigidObject): ...@@ -48,11 +48,6 @@ class Articulation(RigidObject):
articulation root prim and creates the corresponding articulation in the physics engine. The 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. 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 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 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 the :mod:`omni.isaac.lab.actuators` module. The actuator models are specified using the
...@@ -111,6 +106,10 @@ class Articulation(RigidObject): ...@@ -111,6 +106,10 @@ class Articulation(RigidObject):
def data(self) -> ArticulationData: def data(self) -> ArticulationData:
return self._data return self._data
@property
def num_instances(self) -> int:
return self.root_physx_view.count
@property @property
def is_fixed_base(self) -> bool: def is_fixed_base(self) -> bool:
"""Whether the articulation is a fixed-base or floating-base system.""" """Whether the articulation is a fixed-base or floating-base system."""
...@@ -160,22 +159,35 @@ class Articulation(RigidObject): ...@@ -160,22 +159,35 @@ class Articulation(RigidObject):
""" """
def reset(self, env_ids: Sequence[int] | None = None): def reset(self, env_ids: Sequence[int] | None = None):
super().reset(env_ids)
# use ellipses object to skip initial indices. # use ellipses object to skip initial indices.
if env_ids is None: if env_ids is None:
env_ids = slice(None) env_ids = slice(None)
# reset actuators # reset actuators
for actuator in self.actuators.values(): for actuator in self.actuators.values():
actuator.reset(env_ids) 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): def write_data_to_sim(self):
"""Write external wrenches and joint commands to the simulation. """Write external wrenches and joint commands to the simulation.
If any explicit actuators are present, then the actuator models are used to compute the 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. 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 # write external wrench
super().write_data_to_sim() 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 # apply actuator models
self._apply_actuator_model() self._apply_actuator_model()
...@@ -186,6 +198,28 @@ class Articulation(RigidObject): ...@@ -186,6 +198,28 @@ class Articulation(RigidObject):
self.root_physx_view.set_dof_position_targets(self._joint_pos_target_sim, self._ALL_INDICES) 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) 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( def find_joints(
self, name_keys: str | Sequence[str], joint_subset: list[str] | None = None, preserve_order: bool = False self, name_keys: str | Sequence[str], joint_subset: list[str] | None = None, preserve_order: bool = False
) -> tuple[list[int], list[str]]: ) -> tuple[list[int], list[str]]:
...@@ -236,7 +270,29 @@ class Articulation(RigidObject): ...@@ -236,7 +270,29 @@ class Articulation(RigidObject):
Operations - Writers. 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): 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 # resolve all indices
physx_env_ids = env_ids physx_env_ids = env_ids
if env_ids is None: if env_ids is None:
...@@ -252,6 +308,12 @@ class Articulation(RigidObject): ...@@ -252,6 +308,12 @@ class Articulation(RigidObject):
self.root_physx_view.set_root_transforms(root_poses_xyzw, indices=physx_env_ids) 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): 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 # resolve all indices
physx_env_ids = env_ids physx_env_ids = env_ids
if env_ids is None: if env_ids is None:
...@@ -477,8 +539,69 @@ class Articulation(RigidObject): ...@@ -477,8 +539,69 @@ class Articulation(RigidObject):
self.root_physx_view.set_dof_limits(self._data.joint_limits.cpu(), indices=physx_env_ids.cpu()) 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( def set_joint_position_target(
self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None
...@@ -812,12 +935,21 @@ class Articulation(RigidObject): ...@@ -812,12 +935,21 @@ class Articulation(RigidObject):
self._log_articulation_joint_info() self._log_articulation_joint_info()
def _create_buffers(self): def _create_buffers(self):
# allocate buffers # constants
super()._create_buffers() 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 # asset data
# -- properties # -- properties
self._data.joint_names = self.joint_names 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 # -- default joint state
self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device) self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device)
...@@ -906,7 +1038,16 @@ class Articulation(RigidObject): ...@@ -906,7 +1038,16 @@ class Articulation(RigidObject):
def _process_cfg(self): def _process_cfg(self):
"""Post processing of configuration parameters.""" """Post processing of configuration parameters."""
# default state # 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 state
# joint pos # joint pos
indices_list, _, values_list = string_utils.resolve_matching_names_values( indices_list, _, values_list = string_utils.resolve_matching_names_values(
...@@ -923,6 +1064,18 @@ class Articulation(RigidObject): ...@@ -923,6 +1064,18 @@ class Articulation(RigidObject):
self._data.default_joint_limits = self.root_physx_view.get_dof_limits().to(device=self.device).clone() 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() 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. Internal helpers -- Actuators.
""" """
......
...@@ -8,21 +8,25 @@ from dataclasses import MISSING ...@@ -8,21 +8,25 @@ from dataclasses import MISSING
from omni.isaac.lab.actuators import ActuatorBaseCfg from omni.isaac.lab.actuators import ActuatorBaseCfg
from omni.isaac.lab.utils import configclass from omni.isaac.lab.utils import configclass
from ..rigid_object import RigidObjectCfg from ..asset_base_cfg import AssetBaseCfg
from .articulation import Articulation from .articulation import Articulation
@configclass @configclass
class ArticulationCfg(RigidObjectCfg): class ArticulationCfg(AssetBaseCfg):
"""Configuration parameters for an articulation.""" """Configuration parameters for an articulation."""
class_type: type = Articulation
@configclass @configclass
class InitialStateCfg(RigidObjectCfg.InitialStateCfg): class InitialStateCfg(AssetBaseCfg.InitialStateCfg):
"""Initial state of the articulation.""" """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_pos: dict[str, float] = {".*": 0.0}
"""Joint positions of the joints. Defaults to 0.0 for all joints.""" """Joint positions of the joints. Defaults to 0.0 for all joints."""
joint_vel: dict[str, float] = {".*": 0.0} joint_vel: dict[str, float] = {".*": 0.0}
...@@ -32,10 +36,17 @@ class ArticulationCfg(RigidObjectCfg): ...@@ -32,10 +36,17 @@ class ArticulationCfg(RigidObjectCfg):
# Initialize configurations. # Initialize configurations.
## ##
class_type: type = Articulation
init_state: InitialStateCfg = InitialStateCfg() init_state: InitialStateCfg = InitialStateCfg()
"""Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state.""" """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 soft_joint_pos_limit_factor: float = 1.0
"""Fraction specifying the range of DOF position limits (parsed from the asset) to use. """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: dict[str, ActuatorBaseCfg] = MISSING
"""Actuators for the robot with corresponding joint names.""" """Actuators for the robot with corresponding joint names."""
...@@ -4,20 +4,20 @@ ...@@ -4,20 +4,20 @@
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
import torch import torch
import weakref
import omni.physics.tensors.impl.api as physx import omni.physics.tensors.impl.api as physx
import omni.isaac.lab.utils.math as math_utils import omni.isaac.lab.utils.math as math_utils
from omni.isaac.lab.utils.buffers import TimestampedBuffer from omni.isaac.lab.utils.buffers import TimestampedBuffer
from ..rigid_object import RigidObjectData
class ArticulationData:
class ArticulationData(RigidObjectData):
"""Data container for an articulation. """Data container for an articulation.
This class extends the :class:`RigidObjectData` class to provide additional data for This class contains the data for an articulation in the simulation. The data includes the state of
an articulation mainly related to the joints and tendons. 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 An articulation is comprised of multiple rigid bodies or links. For a rigid body, there are two frames
of reference that are used: of reference that are used:
...@@ -30,28 +30,48 @@ class ArticulationData(RigidObjectData): ...@@ -30,28 +30,48 @@ class ArticulationData(RigidObjectData):
can be interpreted as the link frame. can be interpreted as the link frame.
""" """
_root_physx_view: physx.ArticulationView def __init__(self, root_physx_view: physx.ArticulationView, device: str):
"""The root articulation view of the object. """Initializes the articulation data.
Note: Args:
Internally, this is stored as a weak reference to avoid circular references between the asset class root_physx_view: The root articulation view.
and the data container. This is important to avoid memory leaks. device: The device used for processing.
""" """
# Set the parameters
def __init__(self, root_physx_view: physx.ArticulationView, device: str): self.device = device
# Initialize the parent class # Set the root articulation view
super().__init__(root_physx_view, device) # type: ignore # 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
# 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 history for finite differencing # Initialize history for finite differencing
self._previous_joint_vel = self._root_physx_view.get_dof_velocities().clone() self._previous_joint_vel = self._root_physx_view.get_dof_velocities().clone()
# Initialize the lazy buffers. # Initialize the lazy buffers.
self._root_state_w = TimestampedBuffer()
self._body_state_w = TimestampedBuffer() self._body_state_w = TimestampedBuffer()
self._body_acc_w = TimestampedBuffer()
self._joint_pos = TimestampedBuffer() self._joint_pos = TimestampedBuffer()
self._joint_acc = TimestampedBuffer() self._joint_acc = TimestampedBuffer()
self._joint_vel = TimestampedBuffer() self._joint_vel = TimestampedBuffer()
def update(self, dt: float): def update(self, dt: float):
# update the simulation timestamp
self._sim_timestamp += dt self._sim_timestamp += dt
# Trigger an update of the joint acceleration buffer at a higher frequency # Trigger an update of the joint acceleration buffer at a higher frequency
# since we do finite differencing. # since we do finite differencing.
...@@ -61,6 +81,9 @@ class ArticulationData(RigidObjectData): ...@@ -61,6 +81,9 @@ class ArticulationData(RigidObjectData):
# Names. # Names.
## ##
body_names: list[str] = None
"""Body names in the order parsed by the simulation view."""
joint_names: list[str] = None joint_names: list[str] = None
"""Joint names in the order parsed by the simulation view.""" """Joint names in the order parsed by the simulation view."""
...@@ -71,6 +94,16 @@ class ArticulationData(RigidObjectData): ...@@ -71,6 +94,16 @@ class ArticulationData(RigidObjectData):
# Defaults. # 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).
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 read from the simulation. Shape is (num_instances, num_bodies)."""
default_joint_pos: torch.Tensor = None default_joint_pos: torch.Tensor = None
"""Default joint positions of all joints. Shape is (num_instances, num_joints).""" """Default joint positions of all joints. Shape is (num_instances, num_joints)."""
...@@ -267,20 +300,20 @@ class ArticulationData(RigidObjectData): ...@@ -267,20 +300,20 @@ class ArticulationData(RigidObjectData):
return self._body_acc_w.data return self._body_acc_w.data
@property @property
def body_lin_acc_w(self) -> torch.Tensor: def projected_gravity_b(self):
"""Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). """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)
This quantity is the linear acceleration of the articulation links' center of mass frame.
"""
return self.body_acc_w[..., 0:3]
@property @property
def body_ang_acc_w(self) -> torch.Tensor: def heading_w(self):
"""Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). """Yaw heading of the base frame (in radians). Shape is (num_instances,).
This quantity is the angular acceleration of the articulation links' center of mass frame. 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)`.
""" """
return self.body_acc_w[..., 3:6] forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B)
return torch.atan2(forward_w[:, 1], forward_w[:, 0])
@property @property
def joint_pos(self): def joint_pos(self):
...@@ -305,10 +338,128 @@ class ArticulationData(RigidObjectData): ...@@ -305,10 +338,128 @@ class ArticulationData(RigidObjectData):
"""Joint acceleration of all joints. Shape is (num_instances, num_joints).""" """Joint acceleration of all joints. Shape is (num_instances, num_joints)."""
if self._joint_acc.timestamp < self._sim_timestamp: if self._joint_acc.timestamp < self._sim_timestamp:
# note: we use finite differencing to compute acceleration # note: we use finite differencing to compute acceleration
self._joint_acc.data = (self.joint_vel - self._previous_joint_vel) / ( time_elapsed = self._sim_timestamp - self._joint_acc.timestamp
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 self._joint_acc.timestamp = self._sim_timestamp
# update the previous joint velocity # update the previous joint velocity
self._previous_joint_vel[:] = self.joint_vel self._previous_joint_vel[:] = self.joint_vel
return self._joint_acc.data 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).
This quantity is the position of the actor frame of the articulation root.
"""
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).
This quantity is the orientation of the actor frame of the articulation root.
"""
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).
This quantity contains the linear and angular velocities of the articulation root'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).
This quantity is the linear velocity of the articulation root'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).
This quantity is the angular velocity of the articulation root'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).
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)
@property
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 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)
@property
def body_pos_w(self) -> torch.Tensor:
"""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).
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).
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).
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).
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 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).
This quantity is the angular acceleration of the rigid bodies' center of mass frame.
"""
return self.body_acc_w[..., 3:6]
...@@ -120,6 +120,10 @@ class RigidObject(AssetBase): ...@@ -120,6 +120,10 @@ class RigidObject(AssetBase):
def update(self, dt: float): def update(self, dt: float):
self._data.update(dt) self._data.update(dt)
"""
Operations - Finders.
"""
def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: 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. """Find bodies in the articulation based on the name keys.
......
...@@ -33,24 +33,20 @@ class RigidObjectData: ...@@ -33,24 +33,20 @@ class RigidObjectData:
is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. 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): def __init__(self, root_physx_view: physx.RigidBodyView, device: str):
"""Initializes the rigid object data. """Initializes the rigid object data.
Args: 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. device: The device used for processing.
""" """
# Set the parameters # Set the parameters
self.device = device 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 # Set initial time stamp
self._sim_timestamp = 0.0 self._sim_timestamp = 0.0
...@@ -76,6 +72,7 @@ class RigidObjectData: ...@@ -76,6 +72,7 @@ class RigidObjectData:
Args: Args:
dt: The time step for the update. This must be a positive value. dt: The time step for the update. This must be a positive value.
""" """
# update the simulation timestamp
self._sim_timestamp += dt self._sim_timestamp += dt
## ##
...@@ -97,7 +94,7 @@ class RigidObjectData: ...@@ -97,7 +94,7 @@ class RigidObjectData:
""" """
default_mass: torch.Tensor = None 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. # Properties.
...@@ -133,8 +130,7 @@ class RigidObjectData: ...@@ -133,8 +130,7 @@ class RigidObjectData:
def body_acc_w(self): def body_acc_w(self):
"""Acceleration of all bodies. Shape is (num_instances, 1, 6). """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 This quantity is the acceleration of the rigid bodies' center of mass frame.
is computed using finite differencing of the linear and angular velocities of the bodies.
""" """
if self._body_acc_w.timestamp < self._sim_timestamp: if self._body_acc_w.timestamp < self._sim_timestamp:
# note: we use finite differencing to compute acceleration # note: we use finite differencing to compute acceleration
...@@ -158,6 +154,10 @@ class RigidObjectData: ...@@ -158,6 +154,10 @@ class RigidObjectData:
forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B) forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B)
return torch.atan2(forward_w[:, 1], forward_w[:, 0]) return torch.atan2(forward_w[:, 1], forward_w[:, 0])
##
# Derived properties.
##
@property @property
def root_pos_w(self) -> torch.Tensor: 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).
...@@ -255,3 +255,19 @@ class RigidObjectData: ...@@ -255,3 +255,19 @@ class RigidObjectData:
This quantity is the angular velocity of the rigid bodies' center of mass frame. This quantity is the angular velocity of the rigid bodies' center of mass frame.
""" """
return self.body_state_w[..., 10:13] 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 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).
This quantity is the angular acceleration of the rigid bodies' center of mass frame.
"""
return self.body_acc_w[..., 3:6]
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment