format

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

@@ -281,7 +281,10 @@ 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
-        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.")
+        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)
@@ -325,9 +328,12 @@ 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
-        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.")
+        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)
+        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.
@@ -350,7 +356,7 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence
         root_poses_xyzw = self._data.root_state_w[:, :7].clone()
         root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw")
         # set into simulation
-        self.root_physx_view.set_root_transforms(root_poses_xyzw, indices=physx_env_ids)  
+        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.
@@ -366,31 +372,36 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[
         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)
-
+        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 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). 
+            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
-        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.")
+        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)
+        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.
@@ -399,7 +410,7 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S
         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). 
+            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.
         """
 
@@ -422,7 +433,7 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
         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). 
+            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
@@ -431,13 +442,13 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
 
         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,:]
+        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] += 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)
+        self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=env_ids)
 
     def write_joint_state_to_sim(
         self,

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

@@ -3,19 +3,21 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
-import carb
 import torch
 import weakref
 
+import carb
 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):
     """Single use deprecation warning"""
     carb.log_warn("DeprecationWarning: " + dep_msg)
 
+
 class ArticulationData:
     """Data container for an articulation.
 
@@ -262,7 +264,10 @@ def root_state_w(self):
         the linear and angular velocities are of the articulation root'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.")
+        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
@@ -295,7 +300,7 @@ def root_link_state_w(self):
             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 
+        return self._root_link_state_w.data
 
     @property
     def root_com_state_w(self):
@@ -312,15 +317,14 @@ def root_com_state_w(self):
             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)
+            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 
+        return self._root_com_state_w.data
 
     @property
     def body_state_w(self):
@@ -331,8 +335,11 @@ def body_state_w(self):
         velocities are of the articulation links's 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.")
-
+        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
             poses = self._root_physx_view.get_link_transforms().clone()
@@ -363,7 +370,7 @@ def body_link_state_w(self):
             # 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
@@ -382,15 +389,14 @@ def body_com_state_w(self):
             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)
+            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 
+        return self._body_com_state_w.data
 
     @property
     def body_acc_w(self):
@@ -538,7 +544,7 @@ def root_link_quat_w(self) -> torch.Tensor:
     def root_link_vel_w(self) -> torch.Tensor:
         """Root link velocity in simulation world frame. Shape is (num_instances, 6).
 
-        This quantity contains the linear and angular velocities of the actor frame of the root 
+        This quantity contains the linear and angular velocities of the actor frame of the root
         rigid body relative to the world.
         """
         return self.root_link_state_w[:, 7:13]
@@ -576,10 +582,10 @@ def root_link_ang_vel_b(self) -> torch.Tensor:
         rigid body's actor frame.
         """
         return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)
-    
-#
-# Root Center of Mass state properties
-#  
+
+    #
+    # Root Center of Mass state properties
+    #
 
     @property
     def root_com_pos_w(self) -> torch.Tensor:
@@ -696,10 +702,9 @@ def body_ang_acc_w(self) -> torch.Tensor:
         """
         return self.body_acc_w[..., 3:6]
 
-
-#
-# Link body properties
-#
+    #
+    # Link body properties
+    #
 
     @property
     def body_link_pos_w(self) -> torch.Tensor:
@@ -721,7 +726,7 @@ def body_link_quat_w(self) -> torch.Tensor:
     def body_link_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' center of mass frame
         relative to the world.
         """
         return self.body_link_state_w[..., 7:13]
@@ -741,10 +746,10 @@ def body_link_ang_vel_w(self) -> torch.Tensor:
         This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world.
         """
         return self.body_link_state_w[..., 10:13]
-    
-#
-# Center of mass body properties
-#
+
+    #
+    # Center of mass body properties
+    #
 
     @property
     def body_com_pos_w(self) -> torch.Tensor:
@@ -756,8 +761,8 @@ def body_com_pos_w(self) -> torch.Tensor:
 
     @property
     def body_com_quat_w(self) -> torch.Tensor:
-        """Orientation (w, x, y, z) of the prinicple axies of inertia of all bodies in simulation world frame. 
-        
+        """Orientation (w, x, y, z) of the prinicple axies of inertia of all bodies in simulation world frame.
+
         Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame.
         """
         return self.body_com_state_w[..., 3:7]
@@ -789,16 +794,16 @@ def body_com_ang_vel_w(self) -> torch.Tensor:
     @property
     def com_pos_b(self) -> torch.Tensor:
         """Center of mass of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
-        
+
         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]
+
     @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, num_bodies, 4).
-        
+
         This quantity is the orientation of the principles axes of inertia relative to its body frame.
         """
-        quat = self._root_physx_view.get_coms().to(self.device)[...,3:7]
-        return math_utils.convert_quat(quat, to="wxyz")
+        quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7]
+        return math_utils.convert_quat(quat, to="wxyz")