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 433815f4dc..24104ab8b8 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 @@ -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,17 +372,19 @@ 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. @@ -384,13 +392,16 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque 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, 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 95ac55778c..860f3bfeb9 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 @@ -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") \ No newline at end of file + quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7] + return math_utils.convert_quat(quat, to="wxyz") 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 1f10d8b9b4..245f89e095 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 @@ -146,7 +146,6 @@ def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = Fal Operations - Write to simulation. """ - def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): """Set the root state over selected environment indices into the simulation. @@ -158,7 +157,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: 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.") + 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) @@ -202,9 +204,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: 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.") + 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) + 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. @@ -239,21 +244,23 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[ 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,:] + 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) + 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. @@ -262,13 +269,16 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque 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: 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.") + 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) + 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. @@ -277,7 +287,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. """ @@ -300,7 +310,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 @@ -309,14 +319,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) """ Operations - Setters. 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 4410e6cd91..65988d332c 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,10 +3,10 @@ # # 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 @@ -111,7 +111,10 @@ def root_state_w(self): 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.") + 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 @@ -144,7 +147,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): @@ -160,15 +163,14 @@ def root_com_state_w(self): 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) + 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): @@ -177,8 +179,11 @@ 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.") - + 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 @@ -311,7 +316,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] @@ -350,7 +355,6 @@ def root_link_ang_vel_b(self) -> torch.Tensor: """ return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) - @property def root_com_pos_w(self) -> torch.Tensor: """Root center of mass position in simulation world frame. Shape is (num_instances, 3). @@ -409,7 +413,6 @@ def root_com_ang_vel_b(self) -> torch.Tensor: """ return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_ang_vel_w) - @property def body_pos_w(self) -> torch.Tensor: """Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). @@ -465,10 +468,10 @@ def body_ang_acc_w(self) -> torch.Tensor: This quantity is the angular acceleration of the rigid bodies' center of mass frame. """ return self.body_acc_w[..., 3:6] - -# -# Link body properties -# + + # + # Link body properties + # @property def body_link_pos_w(self) -> torch.Tensor: @@ -490,7 +493,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] @@ -510,10 +513,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: @@ -525,8 +528,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] @@ -558,16 +561,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, 1, 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].view(-1, 1, 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. """ - quat = self._root_physx_view.get_coms().to(self.device)[...,3:7] - return math_utils.convert_quat(quat, to="wxyz").view(-1, 1, 4) \ No newline at end of file + quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7] + return math_utils.convert_quat(quat, to="wxyz").view(-1, 1, 4) 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 a28f37ff18..4f3a347a50 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -26,8 +26,8 @@ import omni.isaac.core.utils.prims as prim_utils import omni.isaac.lab.sim as sim_utils -import omni.isaac.lab.utils.string as string_utils import omni.isaac.lab.utils.math as math_utils +import omni.isaac.lab.utils.string as string_utils from omni.isaac.lab.actuators import ImplicitActuatorCfg from omni.isaac.lab.assets import Articulation, ArticulationCfg from omni.isaac.lab.sim import build_simulation_context @@ -797,13 +797,15 @@ def test_apply_joint_command(self): def test_body_root_link_state(self): """Test for the root_link_state_w property""" for num_articulations in (1, 2): - # for num_articulations in ( 2,): + # for num_articulations in ( 2,): for device in ("cuda:0", "cpu"): - # for device in ("cuda:0",): + # for device in ("cuda:0",): for with_offset in [True, False]: - # for with_offset in [True,]: + # for with_offset in [True,]: with self.subTest(num_articulations=num_articulations, device=device, with_offset=with_offset): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: + with build_simulation_context( + device=device, add_ground_plane=False, auto_add_lighting=True + ) as sim: articulation_cfg = generate_articulation_cfg(articulation_type="single_joint") articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) env_idx = torch.tensor([x for x in range(num_articulations)]) @@ -868,8 +870,10 @@ def test_body_root_link_state(self): torch.testing.assert_close( lin_vel_gt[:, 0, :], root_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1 ) - # linear velocity of pendulum link should be - torch.testing.assert_close(lin_vel_gt, body_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1) + # linear velocity of pendulum link should be + torch.testing.assert_close( + lin_vel_gt, body_link_state_w[..., 7:10], atol=1e-3, rtol=1e-1 + ) # ang_vel torch.testing.assert_close(root_state_w[..., 10:], root_link_state_w[..., 10:]) @@ -880,18 +884,20 @@ def test_body_root_link_state(self): pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) py = torch.zeros(num_articulations, 1, 1, device=device) - pz = (link_offset[0] + offset[0])* torch.sin(joint_pos) + pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) pos_gt[:, 1, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) - torch.testing.assert_close(pos_gt[:, 0, :], root_com_state_w[..., :3], atol=1e-3, rtol=1e-1) + torch.testing.assert_close( + pos_gt[:, 0, :], root_com_state_w[..., :3], atol=1e-3, rtol=1e-1 + ) torch.testing.assert_close(pos_gt, body_com_state_w[..., :3], atol=1e-3, rtol=1e-1) # orientation com_quat_b = articulation.data.com_quat_b - com_quat_w = math_utils.quat_mul(body_link_state_w[...,3:7],com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_state_w[...,3:7]) - torch.testing.assert_close(com_quat_w[:,0,:], root_com_state_w[...,3:7]) - + com_quat_w = math_utils.quat_mul(body_link_state_w[..., 3:7], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_state_w[..., 3:7]) + torch.testing.assert_close(com_quat_w[:, 0, :], root_com_state_w[..., 3:7]) + # linear vel, and angular vel torch.testing.assert_close(root_state_w[..., 7:], root_com_state_w[..., 7:]) torch.testing.assert_close(body_state_w[..., 7:], body_com_state_w[..., 7:]) @@ -903,15 +909,24 @@ def test_body_root_link_state(self): torch.testing.assert_close(body_state_w, body_com_state_w) def test_write_root_state(self): - """Test the setters for root_state using both the link frame and center of mass as referece frame.""" + """Test the setters for root_state using both the link frame and center of mass as reference frame.""" for num_articulations in (1, 2): for device in ("cuda:0", "cpu"): for with_offset in [True, False]: - for state_location in ("com","link"): - with self.subTest(num_articulations=num_articulations, device=device, with_offset=with_offset, state_location=state_location): - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False) as sim: + for state_location in ("com", "link"): + with self.subTest( + num_articulations=num_articulations, + device=device, + with_offset=with_offset, + state_location=state_location, + ): + with build_simulation_context( + device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False + ) as sim: articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) + articulation, env_pos = generate_articulation( + articulation_cfg, num_articulations, device + ) env_idx = torch.tensor([x for x in range(num_articulations)]) # Play sim @@ -928,16 +943,16 @@ def test_write_root_state(self): new_com = offset com[:, 0, :3] = new_com.squeeze(-2) articulation.root_physx_view.set_coms(com, env_idx) - + # check they are set torch.testing.assert_close(articulation.root_physx_view.get_coms(), com) rand_state = torch.zeros_like(articulation.data.root_link_state_w) - rand_state[...,:7]= articulation.data.default_root_state[...,:7] - rand_state[...,:3] += env_pos + rand_state[..., :7] = articulation.data.default_root_state[..., :7] + rand_state[..., :3] += env_pos # make quaternion a unit vector - rand_state[...,3:7] = torch.nn.functional.normalize(rand_state[...,3:7],dim=-1) - + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + for i in range(10): # perform step @@ -945,43 +960,39 @@ def test_write_root_state(self): # update buffers articulation.update(sim.cfg.dt) - if state_location is "com": + if state_location == "com": articulation.write_root_com_state_to_sim(rand_state) - elif state_location is "link": + elif state_location == "link": articulation.write_root_link_state_to_sim(rand_state) - - if state_location is "com": - torch.testing.assert_close(rand_state,articulation.data.root_com_state_w) - elif state_location is "link": - torch.testing.assert_close(rand_state,articulation.data.root_link_state_w) + if state_location == "com": + torch.testing.assert_close(rand_state, articulation.data.root_com_state_w) + elif state_location == "link": + torch.testing.assert_close(rand_state, articulation.data.root_link_state_w) def test_transform_inverses(self): """Test if math utilities are true inverses of each other.""" - - pose01 = torch.rand(1,7) - pose01[:,3:7] = torch.nn.functional.normalize(pose01[...,3:7],dim=-1) - - pose12 = torch.rand(1,7) - pose12[:,3:7] = torch.nn.functional.normalize(pose12[...,3:7],dim=-1) - - - pos02, quat02 = math_utils.combine_frame_transforms(pose01[...,:3], - pose01[...,3:7], - pose12[:,:3], - pose12[:,3:7]) - - pos01, quat01 = math_utils.combine_frame_transforms(pos02, - quat02, - math_utils.quat_rotate(math_utils.quat_inv(pose12[:,3:7]),-pose12[:,:3]), - math_utils.quat_inv(pose12[:,3:7])) - print("") - print(pose01) - print(pos01,quat01) - torch.testing.assert_close(pose01,torch.cat((pos01,quat01),dim=-1)) + pose01 = torch.rand(1, 7) + pose01[:, 3:7] = torch.nn.functional.normalize(pose01[..., 3:7], dim=-1) + + pose12 = torch.rand(1, 7) + pose12[:, 3:7] = torch.nn.functional.normalize(pose12[..., 3:7], dim=-1) + pos02, quat02 = math_utils.combine_frame_transforms( + pose01[..., :3], pose01[..., 3:7], pose12[:, :3], pose12[:, 3:7] + ) + pos01, quat01 = math_utils.combine_frame_transforms( + pos02, + quat02, + math_utils.quat_rotate(math_utils.quat_inv(pose12[:, 3:7]), -pose12[:, :3]), + math_utils.quat_inv(pose12[:, 3:7]), + ) + print("") + print(pose01) + print(pos01, quat01) + torch.testing.assert_close(pose01, torch.cat((pos01, quat01), dim=-1)) if __name__ == "__main__": diff --git a/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py b/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py index a898d81540..efaaf700a2 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py @@ -32,7 +32,7 @@ from omni.isaac.lab.sim import build_simulation_context from omni.isaac.lab.sim.spawners import materials from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR -from omni.isaac.lab.utils.math import default_orientation, quat_rotate_inverse, random_orientation, quat_mul +from omni.isaac.lab.utils.math import default_orientation, quat_mul, quat_rotate_inverse, random_orientation def generate_cubes_scene( @@ -751,10 +751,10 @@ def test_body_root_state_properties(self): # orientation of com will be a constant rotation from link orientation com_quat_b = cube_object.data.com_quat_b - com_quat_w = quat_mul(body_link_state_w[...,3:7],com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_state_w[...,3:7]) - torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_state_w[...,3:7]) - + com_quat_w = quat_mul(body_link_state_w[..., 3:7], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_state_w[..., 3:7]) + torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_state_w[..., 3:7]) + # orientation of link will match root state will always match torch.testing.assert_close(root_state_w[..., 3:7], root_link_state_w[..., 3:7]) torch.testing.assert_close(body_state_w[..., 3:7], body_link_state_w[..., 3:7]) @@ -795,13 +795,15 @@ def test_write_root_state(self): for num_cubes in (1, 2): for device in ("cuda:0", "cpu"): for with_offset in [True, False]: - for state_location in ("com","link"): + for state_location in ("com", "link"): with self.subTest(num_cubes=num_cubes, device=device, with_offset=with_offset): with build_simulation_context( device=device, gravity_enabled=False, auto_add_lighting=True ) as sim: # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + cube_object, env_pos = generate_cubes_scene( + num_cubes=num_cubes, height=0.0, device=device + ) env_idx = torch.tensor([x for x in range(num_cubes)]) # Play sim @@ -824,11 +826,11 @@ def test_write_root_state(self): torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) rand_state = torch.zeros_like(cube_object.data.root_link_state_w) - rand_state[...,:7]= cube_object.data.default_root_state[...,:7] - rand_state[...,:3] += env_pos + rand_state[..., :7] = cube_object.data.default_root_state[..., :7] + rand_state[..., :3] += env_pos # make quaternion a unit vector - rand_state[...,3:7] = torch.nn.functional.normalize(rand_state[...,3:7],dim=-1) - + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + for i in range(10): # perform step @@ -836,15 +838,15 @@ def test_write_root_state(self): # update buffers cube_object.update(sim.cfg.dt) - if state_location is "com": + if state_location == "com": cube_object.write_root_com_state_to_sim(rand_state) - elif state_location is "link": + elif state_location == "link": cube_object.write_root_link_state_to_sim(rand_state) - - if state_location is "com": - torch.testing.assert_close(rand_state,cube_object.data.root_com_state_w) - elif state_location is "link": - torch.testing.assert_close(rand_state,cube_object.data.root_link_state_w) + + if state_location == "com": + torch.testing.assert_close(rand_state, cube_object.data.root_com_state_w) + elif state_location == "link": + torch.testing.assert_close(rand_state, cube_object.data.root_link_state_w) if __name__ == "__main__":