add rigidobject tests and deprecation warnings

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py

@@ -286,6 +286,7 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in
             """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)
 
@@ -335,6 +336,7 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
             """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)
 
@@ -403,6 +405,7 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque
             """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)
 

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py

@@ -3,14 +3,21 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
+import carb
 import torch
+import warnings
 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
 
+def single_deprecation_warning(dep_msg: str):
+    """Singlue use deprecation warning"""
+    warnings.simplefilter("once")
+    warnings.warn(dep_msg, DeprecationWarning)
+    carb.log_warn(dep_msg)
 
 class ArticulationData:
     """Data container for an articulation.
@@ -253,6 +260,10 @@ def root_state_w(self):
         The position and quaternion are of the articulation root's actor frame relative to the world. Meanwhile,
         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.""")
+
         if self._root_state_w.timestamp < self._sim_timestamp:
             # read data from simulation
             pose = self._root_physx_view.get_root_transforms().clone()
@@ -280,7 +291,7 @@ def root_link_state_w(self):
 
     @property
     def root_com_state_w(self):
-        """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13).
+        """Root center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13).
 
         The position, quaternion, and linear/angular velocity are of the articulation root link's center of mass frame
         relative to the world. Center of mass frame is assumed to be the same orientation as the link rather than the
@@ -303,6 +314,10 @@ def body_state_w(self):
         The position and quaternion are of all the articulation links's actor frame. Meanwhile, the linear and angular
         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.""")
+
         if self._body_state_w.timestamp < self._sim_timestamp:
             # read data from simulation
             poses = self._root_physx_view.get_link_transforms().clone()

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py

@@ -10,6 +10,7 @@
 from typing import TYPE_CHECKING
 
 import carb
+import warnings
 import omni.physics.tensors.impl.api as physx
 from pxr import UsdPhysics
 
@@ -157,11 +158,12 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in
             root_state: Root state in simulation frame. Shape is (len(env_ids), 13).
             env_ids: Environment indices. If None, then all indices are used.
         """
-            # deprecation warning
+        # 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)
 
@@ -208,9 +210,10 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
         """
         # deprecation warning
         dep_msg = (
-            """Articluation.write_root_pos_to_sim will be removed in a future release. Please use 
+            """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)
 
@@ -250,20 +253,53 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[
             env_ids: Environment indices. If None, then all indices are used.
         """
 
+        # resolve all indices
+        if env_ids is None:
+            local_env_ids = slice(None)
+
+        com_pos = self.data.com_pos_b[local_env_ids,0,:]
+        com_quat = self.data.com_quat_b[local_env_ids,0,:]
+
         root_link_pos, root_link_quat = math_utils.combine_frame_transforms(root_pose[...,:3],
                                                             root_pose[...,3:7],
-                                                            self.data.com_pos_b[env_ids,0,:],
-                                                            self.data.com_quat_b[env_ids,0,:])
+                                                            math_utils.quat_rotate(math_utils.quat_inv(com_quat),-com_pos),
+                                                            math_utils.quat_inv(com_quat))
+
         root_link_pose = torch.cat((root_link_pos,root_link_quat), dim=-1)
         self.write_root_link_pose_to_sim(root_pose=root_link_pose,env_ids=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.
+        """Set the root center of mass velocity over selected environment indices into the simulation.
+
+        The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order.
+        NOTE: This sets the velocity of the root's center of mass rather than the roots frame.
 
         Args:
-            root_velocity: Root velocities in simulation frame. Shape is (len(env_ids), 6).
+            root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). 
             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)
+
+        self.write_root_com_velocity_to_sim(root_velocity=root_velocity,env_ids=env_ids)
+
+    def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None):
+        """Set the root center of mass velocity over selected environment indices into the simulation.
+
+        The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order.
+        NOTE: This sets the velocity of the root's center of mass rather than the roots frame.
+
+        Args:
+            root_velocity: Root center of mass velocities in simulation world 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:
@@ -276,6 +312,31 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque
         # set into simulation
         self.root_physx_view.set_velocities(self._data.root_state_w[:, 7:], indices=physx_env_ids)
 
+    def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None):
+        """Set the root link velocity over selected environment indices into the simulation.
+
+        The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order.
+        NOTE: This sets the velocity of the root's frame rather than the roots center of mass.
+
+        Args:
+            root_velocity: Root frame velocities in simulation world frame. Shape is (len(env_ids), 6). 
+            env_ids: Environment indices. If None, then all indices are used.
+        """
+        # resolve all indices
+        if env_ids is None:
+            local_env_ids = slice(None)
+
+        root_com_velocity = root_velocity.clone()
+        quat = self.data.root_state_w[local_env_ids, 3:7]
+        com_pos_b = self.data.com_pos_b[local_env_ids,0,:]
+        # transform given velocity to center of mass
+        root_com_velocity[:,:3] += torch.linalg.cross(
+            root_com_velocity[:, 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1
+        )
+        # write center of mass velocity to sim
+        self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity,env_ids=env_ids)
+
+
     """
     Operations - Setters.
     """

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py

@@ -127,23 +127,23 @@ def root_link_state_w(self):
         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), dim=-1)
+        state[:, 7:10] += torch.linalg.cross(state[:, 10:13], math_utils.quat_rotate(quat, -self.com_pos_b.squeeze(-2)), dim=-1)
         return state
 
     @property
     def root_com_state_w(self):
-        """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13).
+        """Root center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13).
 
         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 position to center of mass
+        # 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)
+                                                        self.com_pos_b.squeeze(-2),
+                                                        self.com_quat_b.squeeze(-2))           
+        state[:, :7] = torch.cat((pos,quat),dim=-1)
         return state
 
     @property
@@ -535,12 +535,13 @@ def com_pos_b(self) -> torch.Tensor:
         
         This quantity is the center of mass location relative to its body frame.
         """
-        return self._root_physx_view.get_coms().to(self.device)[...,:3]
+        return self._root_physx_view.get_coms().to(self.device)[...,:3].view(-1, 1, 3)
 
     @property
     def com_quat_b(self) -> torch.Tensor:
         """Orientation (w,x,y,z) of the prinicple axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, 1, 4).
         
         This quantity is the orientation of the principles axes of inertia relative to its body frame.
         """
-        return self._root_physx_view.get_coms().to(self.device)[...,3:7]
+        quat = self._root_physx_view.get_coms().to(self.device)[...,3:7]
+        return math_utils.convert_quat(quat, to="wxyz").view(-1, 1, 4)