Source code for isaaclab.assets.rigid_object.rigid_object_data
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.# All rights reserved.## SPDX-License-Identifier: BSD-3-Clauseimporttorchimportweakrefimportomni.physics.tensors.impl.apiasphysximportisaaclab.utils.mathasmath_utilsfromisaaclab.utils.buffersimportTimestampedBuffer
[docs]classRigidObjectData:"""Data container for a rigid object. This class contains the data for a rigid object in the simulation. The data includes the state of the root rigid body and the state of all the bodies in the object. The data is stored in the simulation world frame unless otherwise specified. For a rigid body, there are two frames of reference that are used: - Actor frame: The frame of reference of the rigid body prim. This typically corresponds to the Xform prim with the rigid body schema. - Center of mass frame: The frame of reference of the center of mass of the rigid body. Depending on the settings of the simulation, the actor frame and the center of mass frame may be the same. This needs to be taken into account when interpreting the data. The data is lazily updated, meaning that the data is only updated when it is accessed. This is useful when the data is expensive to compute or retrieve. The data is updated when the timestamp of the buffer is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. """def__init__(self,root_physx_view:physx.RigidBodyView,device:str):"""Initializes the rigid object data. Args: root_physx_view: The root rigid body view. device: The device used for processing. """# Set the parametersself.device=device# Set the root rigid body view# note: this is stored as a weak reference to avoid circular references between the asset class# and the data container. This is important to avoid memory leaks.self._root_physx_view:physx.RigidBodyView=weakref.proxy(root_physx_view)# Set initial time stampself._sim_timestamp=0.0# Obtain global physics sim viewphysics_sim_view=physx.create_simulation_view("torch")physics_sim_view.set_subspace_roots("/")gravity=physics_sim_view.get_gravity()# Convert to direction vectorgravity_dir=torch.tensor((gravity[0],gravity[1],gravity[2]),device=self.device)gravity_dir=math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0)# Initialize constantsself.GRAVITY_VEC_W=gravity_dir.repeat(self._root_physx_view.count,1)self.FORWARD_VEC_B=torch.tensor((1.0,0.0,0.0),device=self.device).repeat(self._root_physx_view.count,1)# Initialize the lazy buffers.self._root_state_w=TimestampedBuffer()self._root_link_state_w=TimestampedBuffer()self._root_com_state_w=TimestampedBuffer()self._body_acc_w=TimestampedBuffer()
[docs]defupdate(self,dt:float):"""Updates the data for the rigid object. Args: dt: The time step for the update. This must be a positive value. """# update the simulation timestampself._sim_timestamp+=dt
### Names.##body_names:list[str]=None"""Body names in the order parsed by the simulation view."""### Defaults.##default_root_state:torch.Tensor=None"""Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, 13). The position and quaternion are of the rigid body's actor frame. Meanwhile, the linear and angular velocities are of the center of mass frame. """default_mass:torch.Tensor=None"""Default mass read from the simulation. Shape is (num_instances, 1)."""default_inertia:torch.Tensor=None"""Default inertia tensor read from the simulation. Shape is (num_instances, 9). The inertia is the inertia tensor relative to the center of mass frame. The values are stored in the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. """### Properties.##@propertydefroot_state_w(self):"""Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular velocities are of the rigid body's center of mass frame. """ifself._root_state_w.timestamp<self._sim_timestamp:# read data from simulationpose=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()# set the buffer data and timestampself._root_state_w.data=torch.cat((pose,velocity),dim=-1)self._root_state_w.timestamp=self._sim_timestampreturnself._root_state_w.data@propertydefroot_link_state_w(self):"""Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). The position, quaternion, and linear/angular velocity are of the rigid body root frame relative to the world. """ifself._root_link_state_w.timestamp<self._sim_timestamp:# read data from simulationpose=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().clone()# adjust linear velocity to link from center of massvelocity[:,: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 timestampself._root_link_state_w.data=torch.cat((pose,velocity),dim=-1)self._root_link_state_w.timestamp=self._sim_timestampreturnself._root_link_state_w.data@propertydefroot_com_state_w(self):"""Root center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame relative to the world. Center of mass frame is the orientation principle axes of inertia. """ifself._root_com_state_w.timestamp<self._sim_timestamp:# read data from simulation (pose is of link)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()# adjust pose to center of masspos,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 timestampself._root_com_state_w.data=torch.cat((pos,quat,velocity),dim=-1)self._root_com_state_w.timestamp=self._sim_timestampreturnself._root_com_state_w.data@propertydefbody_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. """returnself.root_state_w.view(-1,1,13)@propertydefbody_link_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, quaternion, and linear/angular velocity are of the body's link frame relative to the world. """returnself.root_link_state_w.view(-1,1,13)@propertydefbody_com_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, quaternion, and linear/angular velocity are of the body's center of mass frame relative to the world. Center of mass frame is assumed to be the same orientation as the link rather than the orientation of the principle inertia. """returnself.root_com_state_w.view(-1,1,13)@propertydefbody_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. """ifself._body_acc_w.timestamp<self._sim_timestamp:# note: we use finite differencing to compute accelerationself._body_acc_w.data=self._root_physx_view.get_accelerations().unsqueeze(1)self._body_acc_w.timestamp=self._sim_timestampreturnself._body_acc_w.data@propertydefprojected_gravity_b(self):"""Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.GRAVITY_VEC_W)@propertydefheading_w(self):"""Yaw heading of the base frame (in radians). Shape is (num_instances,). Note: This quantity is computed by assuming that the forward-direction of the base frame is along x-direction, i.e. :math:`(1, 0, 0)`. """forward_w=math_utils.quat_apply(self.root_link_quat_w,self.FORWARD_VEC_B)returntorch.atan2(forward_w[:,1],forward_w[:,0])### Derived properties.##@propertydefroot_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 root rigid body. """returnself.root_state_w[:,:3]@propertydefroot_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 root rigid body. """returnself.root_state_w[:,3:7]@propertydefroot_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. """returnself.root_state_w[:,7:13]@propertydefroot_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 relative to the world. """returnself.root_state_w[:,7:10]@propertydefroot_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. """returnself.root_state_w[:,10:13]@propertydefroot_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. """returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.root_lin_vel_w)@propertydefroot_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. """returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.root_ang_vel_w)@propertydefroot_link_pos_w(self)->torch.Tensor:"""Root link position in simulation world frame. Shape is (num_instances, 3). This quantity is the position of the actor frame of the root rigid body relative to the world. """ifself._root_link_state_w.timestamp<self._sim_timestamp:# read data from simulationpose=self._root_physx_view.get_transforms()returnpose[:,:3]returnself.root_link_state_w[:,:3]@propertydefroot_link_quat_w(self)->torch.Tensor:"""Root link 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 root rigid body. """ifself._root_link_state_w.timestamp<self._sim_timestamp:# read data from simulationpose=self._root_physx_view.get_transforms().clone()pose[:,3:7]=math_utils.convert_quat(pose[:,3:7],to="wxyz")returnpose[:,3:7]returnself.root_link_state_w[:,3:7]@propertydefroot_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 rigid body relative to the world. """returnself.root_link_state_w[:,7:13]@propertydefroot_link_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 actor frame relative to the world. """returnself.root_link_state_w[:,7:10]@propertydefroot_link_ang_vel_w(self)->torch.Tensor:"""Root link angular velocity in simulation world frame. Shape is (num_instances, 3). This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. """returnself.root_link_state_w[:,10:13]@propertydefroot_link_lin_vel_b(self)->torch.Tensor:"""Root link linear velocity in base frame. Shape is (num_instances, 3). This quantity is the linear velocity of the actor frame of the root rigid body frame with respect to the rigid body's actor frame. """returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.root_link_lin_vel_w)@propertydefroot_link_ang_vel_b(self)->torch.Tensor:"""Root link angular velocity in base world frame. Shape is (num_instances, 3). This quantity is the angular velocity of the actor frame of the root rigid body frame with respect to the rigid body's actor frame. """returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.root_link_ang_vel_w)@propertydefroot_com_pos_w(self)->torch.Tensor:"""Root center of mass position in simulation world frame. Shape is (num_instances, 3). This quantity is the position of the actor frame of the root rigid body relative to the world. """returnself.root_com_state_w[:,:3]@propertydefroot_com_quat_w(self)->torch.Tensor:"""Root center of mass 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 root rigid body relative to the world. """returnself.root_com_state_w[:,3:7]@propertydefroot_com_vel_w(self)->torch.Tensor:"""Root center of mass 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 relative to the world. """ifself._root_com_state_w.timestamp<self._sim_timestamp:# read data from simulationvelocity=self._root_physx_view.get_velocities()returnvelocityreturnself.root_com_state_w[:,7:13]@propertydefroot_com_lin_vel_w(self)->torch.Tensor:"""Root center of mass 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 relative to the world. """ifself._root_com_state_w.timestamp<self._sim_timestamp:# read data from simulationvelocity=self._root_physx_view.get_velocities()returnvelocity[:,0:3]returnself.root_com_state_w[:,7:10]@propertydefroot_com_ang_vel_w(self)->torch.Tensor:"""Root center of mass 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 relative to the world. """ifself._root_com_state_w.timestamp<self._sim_timestamp:# read data from simulationvelocity=self._root_physx_view.get_velocities()returnvelocity[:,3:6]returnself.root_com_state_w[:,10:13]@propertydefroot_com_lin_vel_b(self)->torch.Tensor:"""Root center of mass 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. """returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.root_com_lin_vel_w)@propertydefroot_com_ang_vel_b(self)->torch.Tensor:"""Root center of mass 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. """returnmath_utils.quat_rotate_inverse(self.root_link_quat_w,self.root_com_ang_vel_w)@propertydefbody_pos_w(self)->torch.Tensor:"""Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). This quantity is the position of the rigid bodies' actor frame. """returnself.body_state_w[...,:3]@propertydefbody_quat_w(self)->torch.Tensor:"""Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame. """returnself.body_state_w[...,3:7]@propertydefbody_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. """returnself.body_state_w[...,7:13]@propertydefbody_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. """returnself.body_state_w[...,7:10]@propertydefbody_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. """returnself.body_state_w[...,10:13]@propertydefbody_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. """returnself.body_acc_w[...,0:3]@propertydefbody_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. """returnself.body_acc_w[...,3:6]## Link body properties#@propertydefbody_link_pos_w(self)->torch.Tensor:"""Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). This quantity is the position of the rigid bodies' actor frame relative to the world. """returnself.body_link_state_w[...,:3]@propertydefbody_link_quat_w(self)->torch.Tensor:"""Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame relative to the world. """returnself.body_link_state_w[...,3:7]@propertydefbody_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 relative to the world. """returnself.body_link_state_w[...,7:13]@propertydefbody_link_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 relative to the world. """returnself.body_link_state_w[...,7:10]@propertydefbody_link_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 relative to the world. """returnself.body_link_state_w[...,10:13]## Center of mass body properties#@propertydefbody_com_pos_w(self)->torch.Tensor:"""Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). This quantity is the position of the rigid bodies' actor frame. """returnself.body_com_state_w[...,:3]@propertydefbody_com_quat_w(self)->torch.Tensor:"""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. """returnself.body_com_state_w[...,3:7]@propertydefbody_com_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. """returnself.body_com_state_w[...,7:13]@propertydefbody_com_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. """returnself.body_com_state_w[...,7:10]@propertydefbody_com_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. """returnself.body_com_state_w[...,10:13]@propertydefcom_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. """returnself._root_physx_view.get_coms().to(self.device)[...,:3].view(-1,1,3)@propertydefcom_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]returnmath_utils.convert_quat(quat,to="wxyz").view(-1,1,4)