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 4d10f7697a..433815f4dc 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 @@ -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 @@ -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) @@ -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) @@ -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) 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 c30cf0e5f3..95ac55778c 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 @@ -5,7 +5,6 @@ import carb import torch -import warnings import weakref import omni.physics.tensors.impl.api as physx @@ -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. @@ -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() @@ -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 @@ -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): @@ -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): @@ -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 @@ -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): @@ -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): 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 5cfd987e1e..1f10d8b9b4 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 @@ -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 @@ -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) @@ -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) @@ -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) 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 1d08d88356..4410e6cd91 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 @@ -3,6 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause +import carb import torch import weakref @@ -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): @@ -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() @@ -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): @@ -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): @@ -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 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 ea70ae3454..a28f37ff18 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -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)