Skip to content

Commit

Permalink
make new properties lazy buffers and clean up dep warning
Browse files Browse the repository at this point in the history
  • Loading branch information
jtigue-bdai committed Oct 10, 2024
1 parent df7e8a6 commit 9f621e8
Show file tree
Hide file tree
Showing 5 changed files with 116 additions and 97 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
from collections.abc import Sequence
from prettytable import PrettyTable
from typing import TYPE_CHECKING
import warnings

import carb
import omni.isaac.core.utils.stage as stage_utils
Expand Down Expand Up @@ -282,13 +281,7 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in
env_ids: Environment indices. If None, then all indices are used.
"""
# deprecation warning
dep_msg = (
"""Articluation.write_root_state_to_sim will be removed in a future release. Please use
write_root_link_state_to_sim or write_root_com_state_to_sim instead."""
)
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
carb.log_warn("DeprecationWarning: Articluation.write_root_state_to_sim will be removed in a future release. Please use write_root_link_state_to_sim or write_root_com_state_to_sim instead.")

# set into simulation
self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids)
Expand Down Expand Up @@ -332,13 +325,7 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
env_ids: Environment indices. If None, then all indices are used.
"""
# deprecation warning
dep_msg = (
"""Articluation.write_root_pos_to_sim will be removed in a future release. Please use
write_root_link_pose_to_sim or write_root_com_pose_to_sim instead."""
)
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
carb.log_warn("DeprecationWarning: Articluation.write_root_pos_to_sim will be removed in a future release. Please use write_root_link_pose_to_sim or write_root_com_pose_to_sim instead.")

self.write_root_link_pose_to_sim(root_pose,env_ids)

Expand Down Expand Up @@ -401,13 +388,7 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque
env_ids: Environment indices. If None, then all indices are used.
"""
# deprecation warning
dep_msg = (
"""Articluation.write_root_velocity_to_sim will be removed in a future release. Please use
write_root_link_velocity_to_sim or write_root_com_velocity_to_sim instead."""
)
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
carb.log_warn("DeprecationWarning: Articluation.write_root_velocity_to_sim will be removed in a future release. Please use write_root_link_velocity_to_sim or write_root_com_velocity_to_sim instead.")

self.write_root_com_velocity_to_sim(root_velocity=root_velocity,env_ids=env_ids)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@

import carb
import torch
import warnings
import weakref

import omni.physics.tensors.impl.api as physx
Expand All @@ -14,10 +13,8 @@
from omni.isaac.lab.utils.buffers import TimestampedBuffer

def single_deprecation_warning(dep_msg: str):
"""Singlue use deprecation warning"""
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
"""Single use deprecation warning"""
carb.log_warn("DeprecationWarning: " + dep_msg)

class ArticulationData:
"""Data container for an articulation.
Expand Down Expand Up @@ -71,7 +68,11 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str):

# Initialize the lazy buffers.
self._root_state_w = TimestampedBuffer()
self._root_link_state_w = TimestampedBuffer()
self._root_com_state_w = TimestampedBuffer()
self._body_state_w = TimestampedBuffer()
self._body_link_state_w = TimestampedBuffer()
self._body_com_state_w = TimestampedBuffer()
self._body_acc_w = TimestampedBuffer()
self._joint_pos = TimestampedBuffer()
self._joint_acc = TimestampedBuffer()
Expand Down Expand Up @@ -261,8 +262,7 @@ def root_state_w(self):
the linear and angular velocities are of the articulation root's center of mass frame.
"""

single_deprecation_warning("""root_state_w it's derived properties will be deprecated in a future release.
Please use root_link_state_w or root_com_state_w.""")
carb.log_warn("DeprecationWarning: root_state_w and it's derived properties will be deprecated in a future release. Please use root_link_state_w or root_com_state_w.")

if self._root_state_w.timestamp < self._sim_timestamp:
# read data from simulation
Expand All @@ -281,13 +281,21 @@ def root_link_state_w(self):
The position, quaternion, and linear/angular velocity are of the articulation root's actor frame relative to the
world.
"""
state = self.root_state_w.clone()
quat = state[:, 3:7]
# adjust linear velocity to link
state[:, 7:10] += torch.linalg.cross(
state[:, 10:13], math_utils.quat_rotate(quat, -self.com_pos_b[:, 0, :]), dim=-1
)
return state
if self._root_link_state_w.timestamp < self._sim_timestamp:
# read data from simulation
pose = self._root_physx_view.get_root_transforms().clone()
pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz")
velocity = self._root_physx_view.get_root_velocities()

# adjust linear velocity to link from center of mass
velocity[:, :3] += torch.linalg.cross(
velocity[:, 3:], math_utils.quat_rotate(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1
)
# set the buffer data and timestamp
self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1)
self._root_link_state_w.timestamp = self._sim_timestamp

return self._root_link_state_w.data

@property
def root_com_state_w(self):
Expand All @@ -297,14 +305,22 @@ def root_com_state_w(self):
relative to the world. Center of mass frame is assumed to be the same orientation as the link rather than the
orientation of the principle inertia.
"""
state = self.root_state_w.clone()
# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(state[:,:3],
state[:,3:7],
self.com_pos_b[:,0,:],
self.com_quat_b[:,0,:])
state[:, :7] = torch.cat((pos,quat),dim=-1)
return state
if self._root_com_state_w.timestamp < self._sim_timestamp:
# read data from simulation (pose is of link)
pose = self._root_physx_view.get_root_transforms().clone()
pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz")
velocity = self._root_physx_view.get_root_velocities()

# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(pose[:,:3],
pose[:,3:7],
self.com_pos_b[:,0,:],
self.com_quat_b[:,0,:])
pose = torch.cat((pos,quat),dim=-1)
# set the buffer data and timestamp
self._root_com_state_w.data = torch.cat((pose, velocity), dim=-1)
self._root_com_state_w.timestamp = self._sim_timestamp
return self._root_com_state_w.data

@property
def body_state_w(self):
Expand All @@ -315,8 +331,7 @@ def body_state_w(self):
velocities are of the articulation links's center of mass frame.
"""

single_deprecation_warning("""body_state_w and it's derived properties will be deprecated in a future release.
Please use body_link_state_w or bodt_com_state_w.""")
carb.log_warn("DeprecationWarning: body_state_w and it's derived properties will be deprecated in a future release. Please use body_link_state_w or bodt_com_state_w.")

if self._body_state_w.timestamp < self._sim_timestamp:
# read data from simulation
Expand All @@ -335,13 +350,21 @@ def body_link_state_w(self):
The position, quaternion, and linear/angular velocity are of the body's link frame relative to the world.
"""
state = self.body_state_w.clone()
quat = state[..., 3:7]
# adjust linear velocity to link
state[..., 7:10] += torch.linalg.cross(
state[..., 10:13], math_utils.quat_rotate(quat, -self.com_pos_b), dim=-1
)
return state
if self._body_link_state_w.timestamp < self._sim_timestamp:
# read data from simulation
pose = self._root_physx_view.get_link_transforms().clone()
pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz")
velocity = self._root_physx_view.get_link_velocities()

# adjust linear velocity to link from center of mass
velocity[..., :3] += torch.linalg.cross(
velocity[..., 3:], math_utils.quat_rotate(pose[..., 3:7], -self.com_pos_b), dim=-1
)
# set the buffer data and timestamp
self._body_link_state_w.data = torch.cat((pose, velocity), dim=-1)
self._body_link_state_w.timestamp = self._sim_timestamp

return self._body_link_state_w.data

@property
def body_com_state_w(self):
Expand All @@ -352,14 +375,22 @@ def body_com_state_w(self):
world. Center of mass frame is assumed to be the same orientation as the link rather than the orientation of the
principle inertia.
"""
state = self.body_state_w.clone()
# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(state[...,:3],
state[...,3:7],
self.com_pos_b,
self.com_quat_b)
state[..., :7] = torch.cat((pos,quat),dim=-1)
return state
if self._body_com_state_w.timestamp < self._sim_timestamp:
# read data from simulation (pose is of link)
pose = self._root_physx_view.get_link_transforms().clone()
pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz")
velocity = self._root_physx_view.get_link_velocities()

# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(pose[...,:3],
pose[...,3:7],
self.com_pos_b,
self.com_quat_b)
pose = torch.cat((pos,quat),dim=-1)
# set the buffer data and timestamp
self._body_com_state_w.data = torch.cat((pose, velocity), dim=-1)
self._body_com_state_w.timestamp = self._sim_timestamp
return self._body_com_state_w.data

@property
def body_acc_w(self):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
from typing import TYPE_CHECKING

import carb
import warnings
import omni.physics.tensors.impl.api as physx
from pxr import UsdPhysics

Expand Down Expand Up @@ -159,13 +158,7 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in
env_ids: Environment indices. If None, then all indices are used.
"""
# deprecation warning
dep_msg = (
"""RigidObject.write_root_state_to_sim will be removed in a future release. Please use
write_root_link_state_to_sim or write_root_com_state_to_sim instead."""
)
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
carb.log_warn("DeprecationWarning: RigidObject.write_root_state_to_sim will be removed in a future release. Please use write_root_link_state_to_sim or write_root_com_state_to_sim instead.")

# set into simulation
self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids)
Expand Down Expand Up @@ -209,13 +202,7 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
env_ids: Environment indices. If None, then all indices are used.
"""
# deprecation warning
dep_msg = (
"""RigidObject.write_root_pos_to_sim will be removed in a future release. Please use
write_root_link_pose_to_sim or write_root_com_pose_to_sim instead."""
)
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
carb.log_warn("DeprecationWarning: RigidObject.write_root_pose_to_sim will be removed in a future release. Please use write_root_link_pose_to_sim or write_root_com_pose_to_sim instead.")

self.write_root_link_pose_to_sim(root_pose,env_ids)

Expand Down Expand Up @@ -279,13 +266,7 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque
env_ids: Environment indices. If None, then all indices are used.
"""
# deprecation warning
dep_msg = (
"""RigidObject.write_root_velocity_to_sim will be removed in a future release. Please use
write_root_link_velocity_to_sim or write_root_com_velocity_to_sim instead."""
)
warnings.simplefilter("once")
warnings.warn(dep_msg, DeprecationWarning)
carb.log_warn(dep_msg)
carb.log_warn("DeprecationWarning: RigidObject.write_root_velocity_to_sim will be removed in a future release. Please use write_root_link_velocity_to_sim or write_root_com_velocity_to_sim instead.")

self.write_root_com_velocity_to_sim(root_velocity=root_velocity,env_ids=env_ids)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#
# SPDX-License-Identifier: BSD-3-Clause

import carb
import torch
import weakref

Expand Down Expand Up @@ -64,6 +65,8 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str):

# Initialize the lazy buffers.
self._root_state_w = TimestampedBuffer()
self._root_link_state_w = TimestampedBuffer()
self._root_com_state_w = TimestampedBuffer()
self._body_acc_w = TimestampedBuffer()

def update(self, dt: float):
Expand Down Expand Up @@ -107,6 +110,9 @@ def root_state_w(self):
The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular
velocities are of the rigid body's center of mass frame.
"""

carb.log_warn("DeprecationWarning: root_state_w and it's derived properties will be deprecated in a future release. Please use root_link_state_w or root_com_state_w.")

if self._root_state_w.timestamp < self._sim_timestamp:
# read data from simulation
pose = self._root_physx_view.get_transforms().clone()
Expand All @@ -124,11 +130,21 @@ def root_link_state_w(self):
The position, quaternion, and linear/angular velocity are of the rigid body root frame relative to the
world.
"""
state = self.root_state_w.clone()
quat = state[:, 3:7]
# adjust linear velocity to link
state[:, 7:10] += torch.linalg.cross(state[:, 10:13], math_utils.quat_rotate(quat, -self.com_pos_b.squeeze(-2)), dim=-1)
return state
if self._root_link_state_w.timestamp < self._sim_timestamp:
# read data from simulation
pose = self._root_physx_view.get_transforms().clone()
pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz")
velocity = self._root_physx_view.get_velocities()

# adjust linear velocity to link from center of mass
velocity[:, :3] += torch.linalg.cross(
velocity[:, 3:], math_utils.quat_rotate(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1
)
# set the buffer data and timestamp
self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1)
self._root_link_state_w.timestamp = self._sim_timestamp

return self._root_link_state_w.data

@property
def root_com_state_w(self):
Expand All @@ -137,14 +153,22 @@ def root_com_state_w(self):
The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame
relative to the world. Center of mass frame is the orientation principle axes of inertia.
"""
state = self.root_state_w.clone()
# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(state[:,:3],
state[:,3:7],
self.com_pos_b.squeeze(-2),
self.com_quat_b.squeeze(-2))
state[:, :7] = torch.cat((pos,quat),dim=-1)
return state
if self._root_com_state_w.timestamp < self._sim_timestamp:
# read data from simulation (pose is of link)
pose = self._root_physx_view.get_transforms().clone()
pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz")
velocity = self._root_physx_view.get_velocities()

# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(pose[:,:3],
pose[:,3:7],
self.com_pos_b[:,0,:],
self.com_quat_b[:,0,:])
pose = torch.cat((pos,quat),dim=-1)
# set the buffer data and timestamp
self._root_com_state_w.data = torch.cat((pose, velocity), dim=-1)
self._root_com_state_w.timestamp = self._sim_timestamp
return self._root_com_state_w.data

@property
def body_state_w(self):
Expand All @@ -153,6 +177,8 @@ def body_state_w(self):
The position and orientation are of the rigid bodies' actor frame. Meanwhile, the linear and angular
velocities are of the rigid bodies' center of mass frame.
"""
carb.log_warn("DeprecationWarning: body_state_w and it's derived properties will be deprecated in a future release. Please use body_link_state_w or bodt_com_state_w.")

return self.root_state_w.view(-1, 1, 13)

@property
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@

from omni.isaac.lab.app import AppLauncher, run_tests

HEADLESS = False
HEADLESS = True

# launch omniverse app
app_launcher = AppLauncher(headless=HEADLESS)
Expand Down

0 comments on commit 9f621e8

Please sign in to comment.