Refactors pose and velocities to link frame and COM frame APIs

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

@@ -70,6 +70,9 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str):
         self._joint_acc = TimestampedBuffer()
         self._joint_vel = TimestampedBuffer()
 
+        # Link com
+        self._com_pos_b, _ = self._root_physx_view.get_coms().to(self.device).split([3, 4], dim=-1)
+
     def update(self, dt: float):
         # update the simulation timestamp
         self._sim_timestamp += dt
@@ -256,14 +259,17 @@ def update(self, dt: float):
     def root_state_w(self):
         """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world 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 the articulation root's center of mass frame.
+        The position and quaternion are of the articulation root's actor frame relative to the world.
         """
         if self._root_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()
+            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_state_w.data = torch.cat((pose, velocity), dim=-1)
             self._root_state_w.timestamp = self._sim_timestamp
@@ -274,14 +280,17 @@ def body_state_w(self):
         """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame.
         Shape is (num_instances, num_bodies, 13).
 
-        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.
+        The position and quaternion are of all the articulation links's actor frame relative to the world.
         """
         if self._body_state_w.timestamp < self._sim_timestamp:
             # read data from simulation
             poses = self._root_physx_view.get_link_transforms().clone()
             poses[..., 3:7] = math_utils.convert_quat(poses[..., 3:7], to="wxyz")
             velocities = self._root_physx_view.get_link_velocities()
+            velocities[..., :3] += torch.linalg.cross(
+                velocities[..., 3:], math_utils.quat_rotate(poses[..., 3:7], -self._com_pos_b), dim=-1
+            )
+
             # set the buffer data and timestamp
             self._body_state_w.data = torch.cat((poses, velocities), dim=-1)
             self._body_state_w.timestamp = self._sim_timestamp
@@ -291,11 +300,18 @@ def body_state_w(self):
     def body_acc_w(self):
         """Acceleration of all bodies. Shape is (num_instances, num_bodies, 6).
 
-        This quantity is the acceleration of the articulation links' center of mass frame.
+        All values are relative to the world.
         """
         if self._body_acc_w.timestamp < self._sim_timestamp:
             # read data from simulation and set the buffer data and timestamp
             self._body_acc_w.data = self._root_physx_view.get_link_accelerations()
+            # move linear acceleration to link frame
+            ang_acc_w = self._body_acc_w.data[..., 3:]
+            ang_vel_w = self.body_state_w[..., 7:10]
+            com_pos_w = math_utils.quat_rotate(self.body_state_w[..., 3:7], -self._com_pos_b)
+            self._body_acc_w.data[..., :3] += torch.linalg.cross(ang_acc_w, com_pos_w, dim=-1) + torch.linalg.cross(
+                ang_vel_w, torch.linalg.cross(ang_vel_w, com_pos_w, dim=-1), dim=-1
+            )
             self._body_acc_w.timestamp = self._sim_timestamp
         return self._body_acc_w.data
 
@@ -353,48 +369,50 @@ def joint_acc(self):
     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.
+        This quantity is the position of the actor frame of the articulation root relative to the world.
         """
         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.
+        This quantity is the orientation of the actor frame of the articulation root relative to the world.
         """
         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.
+        This quantity contains the linear and angular velocities of the articulation root's link frame relative to the
+        world.
         """
         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.
+        This quantity is the linear velocity of the articulation root's link frame relative to the
+        world.
         """
         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.
+        This quantity is the angular velocity of the articulation root's link frame relative to the
+        world.
         """
         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
+        This quantity is the linear velocity of the articulation root's link frame relative to the world with
         respect to the articulation root's actor frame.
         """
         return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_lin_vel_w)
@@ -403,63 +421,63 @@ def root_lin_vel_b(self) -> torch.Tensor:
     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.
+        This quantity is the angular velocity of the articulation root's link frame relative to the world 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.
+        This quantity is the position of the rigid bodies' actor frame relative to the world.
         """
         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.
+        This quantity is the orientation of the rigid bodies' actor frame relative to the world.
         """
         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.
+        This quantity contains the linear and angular velocities of the rigid bodies' link frame relative to the world.
         """
         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.
+        This quantity is the linear velocity of the rigid bodies' link frame relative to the world.
         """
         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.
+        This quantity is the angular velocity of the rigid bodies' link frame relative to the world.
         """
         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.
+        This quantity is the linear acceleration of the rigid bodies' link frame relative to the world.
         """
         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.
+        This quantity is the angular acceleration of the rigid bodies' link frame relative to the world.
         """
         return self.body_acc_w[..., 3:6]

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

@@ -66,6 +66,10 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str):
         self._root_state_w = TimestampedBuffer()
         self._body_acc_w = TimestampedBuffer()
 
+        # Link com
+        com_pos_b, _ = self._root_physx_view.get_coms().to(self.device).split([3, 4], dim=-1)
+        self._com_pos_b = torch.tensor(com_pos_b, device=self.device)
+
     def update(self, dt: float):
         """Updates the data for the rigid object.
 
@@ -112,6 +116,10 @@ def root_state_w(self):
             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()
+            # transform velocity to link coordinate frame
+            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._root_state_w.data = torch.cat((pose, velocity), dim=-1)
             self._root_state_w.timestamp = self._sim_timestamp
@@ -121,20 +129,26 @@ def root_state_w(self):
     def body_state_w(self):
         """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame. Shape is (num_instances, 1, 13).
 
-        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.
+        All values are relative to the world.
         """
         return self.root_state_w.view(-1, 1, 13)
 
     @property
     def body_acc_w(self):
         """Acceleration of all bodies. Shape is (num_instances, 1, 6).
 
-        This quantity is the acceleration of the rigid bodies' center of mass frame.
+        All values are relative to the world.
         """
         if self._body_acc_w.timestamp < self._sim_timestamp:
             # note: we use finite differencing to compute acceleration
             self._body_acc_w.data = self._root_physx_view.get_accelerations().unsqueeze(1)
+            # move linear acceleration to link frame
+            ang_acc_w = self._body_acc_w.data[..., 3:]
+            ang_vel_w = self.body_state_w[..., 7:10]
+            com_pos_w = math_utils.quat_rotate(self.body_state_w[..., 3:7], -self._com_pos_b.unsqueeze(1))
+            self._body_acc_w.data[..., :3] += torch.linalg.cross(ang_acc_w, com_pos_w, dim=-1) + torch.linalg.cross(
+                ang_vel_w, torch.linalg.cross(ang_vel_w, com_pos_w, dim=-1), dim=-1
+            )
             self._body_acc_w.timestamp = self._sim_timestamp
         return self._body_acc_w.data
 
@@ -178,41 +192,42 @@ def root_quat_w(self) -> torch.Tensor:
     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 root rigid body's center of mass frame.
+        This quantity contains the linear and angular velocities of the root rigid body's link frame relative to the
+        world.
         """
         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 root rigid body's center of mass frame.
+        This quantity is the linear velocity of the root rigid body's link frame relative to the world.
         """
         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 root rigid body's center of mass frame.
+        This quantity is the angular velocity of the root rigid body's link frame relative to the world.
         """
         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 root rigid body's center of mass frame with respect to the
-        rigid body's actor frame.
+        This quantity is the linear velocity of the root rigid body's link frame relative to the world,
+        expressed in the body 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 root rigid body's center of mass frame with respect to the
-        rigid body's actor frame.
+        This quantity is the angular velocity of the root rigid body's clink frame relative to the world,
+        expressed in the body frame.
         """
         return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w)
 
@@ -236,38 +251,38 @@ def body_quat_w(self) -> torch.Tensor:
     def body_vel_w(self) -> torch.Tensor:
         """Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6).
 
-        This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame.
+        This quantity contains the linear and angular velocities of the rigid bodies' link frame relative to the world.
         """
         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, 1, 3).
 
-        This quantity is the linear velocity of the rigid bodies' center of mass frame.
+        This quantity is the linear velocity of the rigid bodies' link frame relative to the world.
         """
         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, 1, 3).
 
-        This quantity is the angular velocity of the rigid bodies' center of mass frame.
+        This quantity is the angular velocity of the rigid bodies' link frame relative to the world.
         """
         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, 1, 3).
 
-        This quantity is the linear acceleration of the rigid bodies' center of mass frame.
+        This quantity is the linear acceleration of the rigid bodies' link frame relative to the world.
         """
         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, 1, 3).
 
-        This quantity is the angular acceleration of the rigid bodies' center of mass frame.
+        This quantity is the angular acceleration of the rigid bodies' link frame relative to the world.
         """
         return self.body_acc_w[..., 3:6]