Refactors pose and velocities to link frame and COM frame APIs (#966)

# Description

Currently the root_physx_views of the rigid bodies and articulations
output linear and angular velocities of the com of bodies rather than
the link frame. This PR transforms the velocities and accelerations to
the link frame of the body.

Fixes #942 

## 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
- [x] I have added tests that prove my fix is effective or that my
feature works
- [ ] 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

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

---------

Signed-off-by: jtigue-bdai <[email protected]>
Signed-off-by: Kelly Guo <[email protected]>
Co-authored-by: David Hoeller <[email protected]>
Co-authored-by: Kelly Guo <[email protected]>
Co-authored-by: Kelly Guo <[email protected]>

docs/source/migration/migrating_from_isaacgymenvs.rst

@@ -814,9 +814,9 @@ The ``progress_buf`` variable has also been renamed to ``episode_length_buf``.
 |                                                                       |                                                                           |
 |                                                                       |     self.joint_pos[env_ids] = joint_pos                                   |
 |                                                                       |                                                                           |
-|                                                                       |     self.cartpole.write_root_pose_to_sim(                                 |
+|                                                                       |     self.cartpole.write_root_link_pose_to_sim(                            |
 |                                                                       |         default_root_state[:, :7], env_ids)                               |
-|                                                                       |     self.cartpole.write_root_velocity_to_sim(                             |
+|                                                                       |     self.cartpole.write_root_com_velocity_to_sim(                         |
 |                                                                       |         default_root_state[:, 7:], env_ids)                               |
 |                                                                       |     self.cartpole.write_joint_state_to_sim(                               |
 |                                                                       |         joint_pos, joint_vel, None, env_ids)                              |

docs/source/migration/migrating_from_omniisaacgymenvs.rst

@@ -364,8 +364,8 @@ In Isaac Lab, ``root_pose`` and ``root_velocity`` have been combined into single
 
 .. code-block::python
 
-    self.cartpole.write_root_pose_to_sim(default_root_state[:, :7], env_ids)
-    self.cartpole.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids)
+    self.cartpole.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids)
+    self.cartpole.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids)
 
 
 Creating a New Environment
@@ -738,9 +738,9 @@ reset the ``episode_length_buf`` buffer.
 |       1.0 - 2.0 * torch.rand(num_resets, device=self._device))   |     self.joint_pos[env_ids] = joint_pos                                  |
 |                                                                  |     self.joint_vel[env_ids] = joint_vel                                  |
 |   # apply resets                                                 |                                                                          |
-|   indices = env_ids.to(dtype=torch.int32)                        |     self.cartpole.write_root_pose_to_sim(                                |
+|   indices = env_ids.to(dtype=torch.int32)                        |     self.cartpole.write_root_link_pose_to_sim(                           |
 |   self._cartpoles.set_joint_positions(dof_pos, indices=indices)  |         default_root_state[:, :7], env_ids)                              |
-|   self._cartpoles.set_joint_velocities(dof_vel, indices=indices) |     self.cartpole.write_root_velocity_to_sim(                            |
+|   self._cartpoles.set_joint_velocities(dof_vel, indices=indices) |     self.cartpole.write_root_com_velocity_to_sim(                        |
 |                                                                  |         default_root_state[:, 7:], env_ids)                              |
 |   # bookkeeping                                                  |     self.cartpole.write_joint_state_to_sim(                              |
 |   self.reset_buf[env_ids] = 0                                    |         joint_pos, joint_vel, None, env_ids)                             |

docs/source/overview/sensors/ray_caster.rst

@@ -42,6 +42,7 @@ Using a ray caster sensor requires a **pattern** and a parent xform to be attach
           update_period = 1/60,
           offset=RayCasterCfg.OffsetCfg(pos=(0,0,0.5)),
           mesh_prim_paths=["/World/Ground"],
+          attach_yaw_only=True,
           pattern_cfg=patterns.LidarPatternCfg(
             channels = 100,
             vertical_fov_range=[-90, 90],

docs/source/tutorials/01_assets/run_articulation.rst

@@ -66,8 +66,8 @@ Similar to a rigid object, an articulation also has a root state. This state cor
 articulation tree. On top of the root state, an articulation also has joint states. These states correspond to the
 joint positions and velocities.
 
-To reset the articulation, we first set the root state by calling the :meth:`Articulation.write_root_state_to_sim`
-method. Similarly, we set the joint states by calling the :meth:`Articulation.write_joint_state_to_sim` method.
+To reset the articulation, we first set the root state by calling the :meth:`Articulation.write_root_link_pose_to_sim` and :meth:`Articulation.write_root_com_velocity_to_sim`
+methods. Similarly, we set the joint states by calling the :meth:`Articulation.write_joint_state_to_sim` method.
 Finally, we call the :meth:`Articulation.reset` method to reset any internal buffers and caches.
 
 .. literalinclude:: ../../../../source/standalone/tutorials/01_assets/run_articulation.py

docs/source/tutorials/01_assets/run_deformable_object.rst

@@ -149,7 +149,7 @@ the average position of all the nodes in the mesh.
 .. literalinclude:: ../../../../source/standalone/tutorials/01_assets/run_deformable_object.py
    :language: python
    :start-at: # update buffers
-   :end-at: print(f"Root position (in world): {cube_object.data.root_pos_w[:, :3]}")
+   :end-at: print(f"Root position (in world): {cube_object.data.root_link_pos_w[:, :3]}")
 
 
 The Code Execution

docs/source/tutorials/01_assets/run_rigid_object.rst

@@ -96,7 +96,7 @@ desired state of the rigid object prim into the world frame before setting it.
 We use the :attr:`assets.RigidObject.data.default_root_state` attribute to get the default root state of the
 spawned rigid object prims. This default state can be configured from the :attr:`assets.RigidObjectCfg.init_state`
 attribute, which we left as identity in this tutorial. We then randomize the translation of the root state and
-set the desired state of the rigid object prim using the :meth:`assets.RigidObject.write_root_state_to_sim` method.
+set the desired state of the rigid object prim using the :meth:`assets.RigidObject.write_root_link_pose_to_sim` and :meth:`assets.RigidObject.write_root_com_velocity_to_sim` methods.
 As the name suggests, this method writes the root state of the rigid object prim into the simulation buffer.
 
 .. literalinclude:: ../../../../source/standalone/tutorials/01_assets/run_rigid_object.py

docs/source/tutorials/05_controllers/run_diff_ik.rst

@@ -79,7 +79,7 @@ joint positions, current end-effector pose, and the Jacobian matrix.
 
 While the attribute :attr:`assets.ArticulationData.joint_pos` provides the joint positions,
 we only want the joint positions of the robot's arm, and not the gripper. Similarly, while
-the attribute :attr:`assets.ArticulationData.body_state_w` provides the state of all the
+the attribute :attr:`assets.ArticulationData.body_link_state_w` provides the state of all the
 robot's bodies, we only want the state of the robot's end-effector. Thus, we need to
 index into these arrays to obtain the desired quantities.
 

source/extensions/omni.isaac.lab/config/extension.toml

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.29.3"
+version = "0.30.0"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

@@ -1,6 +1,15 @@
 Changelog
 ---------
 
+0.30.0 (2024-12-16)
+~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Previously, physx returns the rigid bodies and articulations velocities in the com of bodies rather than the link frame, while poses are in link frames. We now explicitly provide :attr:`body_link_state` and :attr:`body_com_state` APIs replacing the previous :attr:`body_state` API. Previous APIs are now marked as deprecated. Please update any code using the previous pose and velocity APIs to use the new ``*_link_*`` or ``*_com_*`` APIs in :attr:`omni.isaac_lab.assets.RigidBody`, :attr:`omni.isaac_lab.assets.RigidBodyCollection`, and :attr:`omni.isaac_lab.assets.Articulation`.
+
+
 0.29.3 (2024-12-16)
 ~~~~~~~~~~~~~~~~~~~
 

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

@@ -98,6 +98,10 @@ def __init__(self, cfg: ArticulationCfg):
         """
         super().__init__(cfg)
 
+        self._root_state_dep_warn = False
+        self._root_pose_dep_warn = False
+        self._root_vel_dep_warn = False
+
     """
     Properties
     """
@@ -280,15 +284,71 @@ 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
+        if not self._root_state_dep_warn:
+            omni.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."
+            )
+            self._root_state_dep_warn = True
+
         # 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_com_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None):
+        """Set the root center of mass 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_com_pose_to_sim(root_state[:, :7], env_ids=env_ids)
+        self.write_root_com_velocity_to_sim(root_state[:, 7:], env_ids=env_ids)
+
+    def write_root_link_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None):
+        """Set the root link 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_link_pose_to_sim(root_state[:, :7], env_ids=env_ids)
+        self.write_root_link_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.
+        """
+        # deprecation warning
+        if not self._root_pose_dep_warn:
+            omni.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._root_pose_dep_warn = True
+
+        self.write_root_link_pose_to_sim(root_pose, env_ids)
+
+    def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None):
+        """Set the root link 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.
@@ -300,33 +360,116 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int]
             physx_env_ids = self._ALL_INDICES
         # note: we need to do this here since tensors are not set into simulation until step.
         # set into internal buffers
-        self._data.root_state_w[env_ids, :7] = root_pose.clone()
+        self._data.root_link_state_w[env_ids, :7] = root_pose.clone()
+        self._data._ignore_dep_warn = True
+        self._data.root_state_w[env_ids, :7] = self._data.root_link_state_w[env_ids, :7]
+        self._data._ignore_dep_warn = False
         # convert root quaternion from wxyz to xyzw
-        root_poses_xyzw = self._data.root_state_w[:, :7].clone()
+        root_poses_xyzw = self._data.root_link_state_w[:, :7].clone()
         root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw")
         # Need to invalidate the buffer to trigger the update with the new root pose.
         self._data._body_state_w.timestamp = -1.0
+        self._data._body_link_state_w.timestamp = -1.0
+        self._data._body_com_state_w.timestamp = -1.0
         # set into simulation
         self.root_physx_view.set_root_transforms(root_poses_xyzw, indices=physx_env_ids)
 
+    def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None):
+        """Set the root center of mass pose over selected environment indices into the simulation.
+
+        The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z).
+        The orientation is the orientation of the principle axes of inertia.
+
+        Args:
+            root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids), 7).
+            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],
+            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 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
+        if not self._root_vel_dep_warn:
+            omni.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._root_vel_dep_warn = True
+
+        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 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.
         """
+
         # resolve all indices
         physx_env_ids = env_ids
         if env_ids is None:
             env_ids = slice(None)
             physx_env_ids = self._ALL_INDICES
         # note: we need to do this here since tensors are not set into simulation until step.
         # set into internal buffers
-        self._data.root_state_w[env_ids, 7:] = root_velocity.clone()
+        self._data.root_com_state_w[env_ids, 7:] = root_velocity.clone()
+        self._data._ignore_dep_warn = True
+        self._data.root_state_w[env_ids, 7:] = self._data.root_com_state_w[env_ids, 7:]
+        self._data._ignore_dep_warn = False
         self._data.body_acc_w[env_ids] = 0.0
         # set into simulation
-        self.root_physx_view.set_root_velocities(self._data.root_state_w[:, 7:], indices=physx_env_ids)
+        self.root_physx_view.set_root_velocities(self._data.root_com_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_link_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)
 
     def write_joint_state_to_sim(
         self,
@@ -360,6 +503,8 @@ def write_joint_state_to_sim(
         self._data.joint_acc[env_ids, joint_ids] = 0.0
         # Need to invalidate the buffer to trigger the update with the new root pose.
         self._data._body_state_w.timestamp = -1.0
+        self._data._body_link_state_w.timestamp = -1.0
+        self._data._body_com_state_w.timestamp = -1.0
         # set into simulation
         self.root_physx_view.set_dof_positions(self._data.joint_pos, indices=physx_env_ids)
         self.root_physx_view.set_dof_velocities(self._data.joint_vel, indices=physx_env_ids)