From ffd84bca516d796bacc1eeab5f5c82e73aa5940a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Wed, 13 Dec 2023 22:11:59 -0800 Subject: [PATCH] Do not initialize rigid prim view for kinematic-only objects. --- omnigibson/prims/rigid_prim.py | 61 ++++++++++------------------------ 1 file changed, 18 insertions(+), 43 deletions(-) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 3b69ab8ca..8daaa5bd5 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -78,8 +78,7 @@ def __init__( def _post_load(self): # Create the view - if not self.kinematic_only: - self._rigid_prim_view = RigidPrimView(self._prim_path) + self._rigid_prim_view = RigidPrimView(self._prim_path) # run super first super()._post_load() @@ -96,15 +95,14 @@ def _post_load(self): self.update_meshes() # Possibly set the mass / density - if not self.kinematic_only: - if not self.has_collision_meshes: - # A meta (virtual) link has no collision meshes; set a negligible mass and a zero density (ignored) - self.mass = 1e-6 - self.density = 0.0 - elif "mass" in self._load_config and self._load_config["mass"] is not None: - self.mass = self._load_config["mass"] - if "density" in self._load_config and self._load_config["density"] is not None: - self.density = self._load_config["density"] + if not self.has_collision_meshes: + # A meta (virtual) link has no collision meshes; set a negligible mass and a zero density (ignored) + self.mass = 1e-6 + self.density = 0.0 + elif "mass" in self._load_config and self._load_config["mass"] is not None: + self.mass = self._load_config["mass"] + if "density" in self._load_config and self._load_config["density"] is not None: + self.density = self._load_config["density"] # Set the visual-only attribute # This automatically handles setting collisions / gravity appropriately @@ -205,7 +203,10 @@ def update_handles(self): """ Updates all internal handles for this prim, in case they change since initialization """ - if self._rigid_prim_view is not None: + # We only do this for non-kinematic objects, because while the USD APIs for kinematic-only + # and dynamic objects are the same, physx tensor APIs do NOT exist for kinematic-only + # objects, meaning initializing the view actively breaks the view. + if not self.kinematic_only: self._rigid_prim_view.initialize(og.sim.physics_sim_view) def contact_list(self): @@ -234,7 +235,6 @@ def set_linear_velocity(self, velocity): Args: velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,). """ - assert not self.kinematic_only, "Cannot set linear velocity for a kinematic-only body!" self._rigid_prim_view.set_linear_velocities(velocity[None, :]) def get_linear_velocity(self): @@ -242,7 +242,6 @@ def get_linear_velocity(self): Returns: np.ndarray: current linear velocity of the the rigid prim. Shape (3,). """ - assert not self.kinematic_only, "Cannot get linear velocity for a kinematic-only body!" return self._rigid_prim_view.get_linear_velocities()[0] def set_angular_velocity(self, velocity): @@ -252,7 +251,6 @@ def set_angular_velocity(self, velocity): Args: velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,). """ - assert not self.kinematic_only, "Cannot set angular velocity for a kinematic-only body!" self._rigid_prim_view.set_angular_velocities(velocity[None, :]) def get_angular_velocity(self): @@ -260,13 +258,11 @@ def get_angular_velocity(self): Returns: np.ndarray: current angular velocity of the the rigid prim. Shape (3,). """ - assert not self.kinematic_only, "Cannot get angular velocity for a kinematic-only body!" return self._rigid_prim_view.get_angular_velocities()[0] def set_position_orientation(self, position=None, orientation=None): # Fallback to the xformprim method for kinematic-only objects - if self.kinematic_only: - return super().set_position_orientation(position=position, orientation=orientation) + return super().set_position_orientation(position=position, orientation=orientation) if position is not None: position = np.asarray(position)[None, :] @@ -277,10 +273,6 @@ def set_position_orientation(self, position=None, orientation=None): self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation) def get_position_orientation(self): - # Fallback to the xformprim method for kinematic-only objects - if self.kinematic_only: - return super().get_position_orientation() - pos, ori = self._rigid_prim_view.get_world_poses() assert np.isclose(np.linalg.norm(ori), 1, atol=1e-3), \ @@ -288,10 +280,6 @@ def get_position_orientation(self): return pos[0], ori[0] def set_local_pose(self, position=None, orientation=None): - # Fallback to the xformprim method for kinematic-only objects - if self.kinematic_only: - return super().set_local_pose(position=position, orientation=orientation) - if position is not None: position = np.asarray(position)[None, :] if orientation is not None: @@ -299,10 +287,6 @@ def set_local_pose(self, position=None, orientation=None): self._rigid_prim_view.set_local_poses(position, orientation) def get_local_pose(self): - # Fallback to the xformprim method for kinematic-only objects - if self.kinematic_only: - return super().get_local_pose() - positions, orientations = self._rigid_prim_view.get_local_poses() return positions[0], orientations[0] @@ -359,12 +343,10 @@ def visual_only(self, val): # Set gravity and collisions based on value if val: self.disable_collisions() - if not self.kinematic_only: - self.disable_gravity() + self.disable_gravity() else: self.enable_collisions() - if not self.kinematic_only: - self.enable_gravity() + self.enable_gravity() # Also set the internal value self._visual_only = val @@ -395,7 +377,6 @@ def mass(self): Returns: float: mass of the rigid body in kg. """ - assert not self.kinematic_only, "Cannot get mass for a kinematic-only body!" mass = self._rigid_prim_view.get_masses()[0] # Fallback to analytical computation of volume * density @@ -410,7 +391,6 @@ def mass(self, mass): Args: mass (float): mass of the rigid body in kg. """ - assert not self.kinematic_only, "Cannot set mass for a kinematic-only body!" self._rigid_prim_view.set_masses([mass]) @property @@ -419,7 +399,6 @@ def density(self): Returns: float: density of the rigid body in kg / m^3. """ - assert not self.kinematic_only, "Cannot get density for a kinematic-only body!" raw_usd_mass = self._rigid_prim_view.get_masses()[0] # We first check if the raw usd mass is specified, since mass overrides density # If it's specified, we infer density based on that value divided by volume @@ -440,7 +419,6 @@ def density(self, density): Args: density (float): density of the rigid body in kg / m^3. """ - assert not self.kinematic_only, "Cannot set density for a kinematic-only body!" self._rigid_prim_view.set_densities([density]) @property @@ -554,14 +532,12 @@ def enable_gravity(self): """ Enables gravity for this rigid body """ - assert not self.kinematic_only, "Cannot enable gravity for a kinematic-only body!" self._rigid_prim_view.enable_gravities() def disable_gravity(self): """ Disables gravity for this rigid body """ - assert not self.kinematic_only, "Cannot disable gravity for a kinematic-only body!" self._rigid_prim_view.disable_gravities() def wake(self): @@ -591,9 +567,8 @@ def _load_state(self, state): super()._load_state(state=state) # Set velocities if not kinematic - if not self.kinematic_only: - self.set_linear_velocity(np.array(state["lin_vel"])) - self.set_angular_velocity(np.array(state["ang_vel"])) + self.set_linear_velocity(np.array(state["lin_vel"])) + self.set_angular_velocity(np.array(state["ang_vel"])) def _serialize(self, state): # Run super first