Skip to content

Commit

Permalink
Do not initialize rigid prim view for kinematic-only objects.
Browse files Browse the repository at this point in the history
  • Loading branch information
@cgokmen
cgokmen committed Dec 14, 2023
1 parent 5c2fe60 commit ffd84bc
Showing 1 changed file with 18 additions and 43 deletions.
61 changes: 18 additions & 43 deletions omnigibson/prims/rigid_prim.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -234,15 +235,13 @@ 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):
"""
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):
Expand All @@ -252,21 +251,18 @@ 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):
"""
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, :]
Expand All @@ -277,32 +273,20 @@ 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), \
f"{self.prim_path} orientation {ori} is not a unit quaternion."
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:
orientation = np.asarray(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]

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit ffd84bc

Please sign in to comment.