fix kinematic only related bugs

omnigibson/objects/object_base.py

@@ -5,11 +5,7 @@
 import omnigibson as og
 import omnigibson.lazy as lazy
 from omnigibson.macros import create_module_macros, gm
-from omnigibson.utils.constants import (
-    DEFAULT_COLLISION_GROUP,
-    SPECIAL_COLLISION_GROUPS,
-    SemanticClass,
-)
+from omnigibson.utils.constants import SemanticClass
 from omnigibson.utils.usd_utils import create_joint, CollisionAPI
 from omnigibson.prims.entity_prim import EntityPrim
 from omnigibson.utils.python_utils import Registerable, classproperty, get_uuid
@@ -88,10 +84,6 @@ def __init__(
         self.category = category
         self.fixed_base = fixed_base
 
-        # This sets the collision group of the object. In omnigibson, objects are only permitted to be part of a single
-        # collision group, e.g. collisions are only enabled within a single group
-        self.collision_group = SPECIAL_COLLISION_GROUPS.get(self.category, DEFAULT_COLLISION_GROUP)
-
         # Infer class ID if not specified
         if class_id is None:
             class_id = CLASS_NAME_TO_CLASS_ID.get(category, SemanticClass.USER_ADDED_OBJS)
@@ -182,20 +174,24 @@ def _post_load(self):
             self._prim.RemoveAPI(lazy.pxr.UsdPhysics.ArticulationRootAPI)
             self._prim.RemoveAPI(lazy.pxr.PhysxSchema.PhysxArticulationAPI)
 
+        if self.root_prim.HasAPI(lazy.pxr.UsdPhysics.ArticulationRootAPI):
+            self.root_prim.RemoveAPI(lazy.pxr.UsdPhysics.ArticulationRootAPI)
+            self.root_prim.RemoveAPI(lazy.pxr.PhysxSchema.PhysxArticulationAPI)
+
         # Potentially add articulation root APIs and also set self collisions
         root_prim = None if self.articulation_root_path is None else lazy.omni.isaac.core.utils.prims.get_prim_at_path(self.articulation_root_path)
         if root_prim is not None:
             lazy.pxr.UsdPhysics.ArticulationRootAPI.Apply(root_prim)
             lazy.pxr.PhysxSchema.PhysxArticulationAPI.Apply(root_prim)
             self.self_collisions = self._load_config["self_collisions"]
 
-        # TODO: Do we need to explicitly add all links? or is adding articulation root itself sufficient?
-        # Set the collision group
-        CollisionAPI.add_to_collision_group(
-            col_group=self.collision_group,
-            prim_path=self.prim_path,
-            create_if_not_exist=True,
-        )
+        # Set the collision group if needed
+        # We always filter collision groups between structures and fixed objects
+        if self.fixed_base:
+            CollisionAPI.add_to_collision_group(
+                col_group="fixed_base",
+                prim_path=self.prim_path,
+            )
 
         # Update semantics
         lazy.omni.isaac.core.utils.semantics.add_update_semantics(

omnigibson/prims/entity_prim.py

@@ -897,6 +897,11 @@ def get_relative_angular_velocity(self):
         return T.quat2mat(self.get_orientation()).T @ self.get_angular_velocity()
 
     def set_position_orientation(self, position=None, orientation=None):
+        # If kinematic only, clear cache for the root link
+        if self.kinematic_only:
+            self.root_link.clear_kinematic_only_cache()
+        if og.sim.is_stopped():
+            return XFormPrim.set_position_orientation(self, position, orientation)
         # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
             return self.root_link.set_position_orientation(position=position, orientation=orientation)
@@ -909,6 +914,8 @@ def set_position_orientation(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
+        if og.sim.is_stopped():
+            return XFormPrim.get_position_orientation(self)
         # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
             return self.root_link.get_position_orientation()
@@ -917,6 +924,11 @@ def get_position_orientation(self):
         return positions[0], orientations[0][[1, 2, 3, 0]]
 
     def set_local_pose(self, position=None, orientation=None):
+        # If kinematic only, clear cache for the root link
+        if self.kinematic_only:
+            self.root_link.clear_kinematic_only_cache()
+        if og.sim.is_stopped():
+            return XFormPrim.set_local_pose(self, position, orientation)
         # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
             return self.root_link.set_local_pose(position=position, orientation=orientation)
@@ -929,6 +941,8 @@ def set_local_pose(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_local_pose(self):
+        if og.sim.is_stopped():
+            return XFormPrim.get_local_pose(self)
         # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
             return self.root_link.get_local_pose()

omnigibson/prims/rigid_prim.py

@@ -291,8 +291,7 @@ def get_angular_velocity(self):
     def set_position_orientation(self, position=None, orientation=None):
         # Invalidate kinematic-only object pose caches when new pose is set
         if self.kinematic_only:
-            self._kinematic_world_pose_cache = None
-            self._kinematic_local_pose_cache = None
+            self.clear_kinematic_only_cache()
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -321,8 +320,7 @@ def get_position_orientation(self):
     def set_local_pose(self, position=None, orientation=None):
         # Invalidate kinematic-only object pose caches when new pose is set
         if self.kinematic_only:
-            self._kinematic_world_pose_cache = None
-            self._kinematic_local_pose_cache = None
+            self.clear_kinematic_only_cache()
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -622,6 +620,15 @@ def sleep(self):
         prim_id = lazy.pxr.PhysicsSchemaTools.sdfPathToInt(self.prim_path)
         og.sim.psi.put_to_sleep(og.sim.stage_id, prim_id)
 
+    def clear_kinematic_only_cache(self):
+        """
+        Clears the internal kinematic only cached pose. Useful if the parent prim's pose
+        changes without explicitly calling this prim's pose setter
+        """
+        assert self.kinematic_only
+        self._kinematic_local_pose_cache = None
+        self._kinematic_world_pose_cache = None
+
     def _dump_state(self):
         # Grab pose from super class
         state = super()._dump_state()

omnigibson/scenes/scene_base.py

@@ -11,6 +11,7 @@
     create_object_from_init_info
 from omnigibson.utils.registry_utils import SerializableRegistry
 from omnigibson.utils.ui_utils import create_module_logger
+from omnigibson.utils.usd_utils import CollisionAPI
 from omnigibson.objects.object_base import BaseObject
 from omnigibson.systems.system_base import SYSTEM_REGISTRY, clear_all_systems, get_system
 from omnigibson.objects.light_object import LightObject
@@ -170,6 +171,8 @@ def _load(self):
         Load the scene into simulator
         The elements to load may include: floor, building, objects, etc.
         """
+        # Add collision group for fixed objects
+        CollisionAPI.create_collision_group(col_group="fixed_base", filter_self_collisions=True)
         # We just add a ground plane if requested
         if self._use_floor_plane:
             self.add_ground_plane(color=self._floor_plane_color, visible=self._floor_plane_visible)
@@ -424,23 +427,6 @@ def add_object(self, obj, register=True, _is_call_from_simulator=False):
         # let scene._load() load the object when called later on.
         prim = obj.load()
 
-        # If this object is fixed and is NOT an agent, disable collisions between the fixed links of the fixed objects
-        # This is to account for cases such as Tiago, which has a fixed base which is needed for its global base joints
-        if obj.fixed_base and obj.category != robot_macros.ROBOT_CATEGORY:
-            # TODO: Remove building hotfix once asset collision meshes are fixed!!
-            building_categories = {"walls", "floors", "ceilings"}
-            for fixed_obj in self.fixed_objects.values():
-                # Filter out collisions between walls / ceilings / floors and ALL links of the other object
-                if obj.category in building_categories:
-                    for link in fixed_obj.links.values():
-                        obj.root_link.add_filtered_collision_pair(link)
-                elif fixed_obj.category in building_categories:
-                    for link in obj.links.values():
-                        fixed_obj.root_link.add_filtered_collision_pair(link)
-                else:
-                    # Only filter out root links
-                    obj.root_link.add_filtered_collision_pair(fixed_obj.root_link)
-
         # Add this object to our registry based on its type, if we want to register it
         if register:
             self.object_registry.add(obj)

omnigibson/utils/constants.py

@@ -69,15 +69,6 @@ class ParticleModifyCondition(str, Enum):
 # Structure categories that need to always be loaded for stability purposes
 STRUCTURE_CATEGORIES = frozenset({"floors", "walls", "ceilings", "lawn", "driveway", "fence"})
 
-# Note that we are starting this from bit 6 since bullet seems to be giving special meaning to groups 0-5.
-# Collision groups for objects. For special logic, different categories can be assigned different collision groups.
-ALL_COLLISION_GROUPS_MASK = -1
-DEFAULT_COLLISION_GROUP = "default"
-SPECIAL_COLLISION_GROUPS = {
-    "floors": "floors",
-    "carpet": "carpet",
-}
-
 
 # Joint friction magic values to assign to objects based on their category
 DEFAULT_JOINT_FRICTION = 10.0

omnigibson/utils/usd_utils.py

@@ -353,35 +353,47 @@ class CollisionAPI:
     """
     Class containing class methods to facilitate collision handling, e.g. collision groups
     """
-    ACTIVE_COLLISION_GROUPS = {}
+    ACTIVE_COLLISION_GROUPS = dict()
 
     @classmethod
-    def add_to_collision_group(cls, col_group, prim_path, create_if_not_exist=False):
+    def create_collision_group(cls, col_group, filter_self_collisions=False):
+        """
+        Creates a new collision group with name @col_group
+
+        Args:
+            col_group (str): Name of the collision group to create
+            filter_self_collisions (bool): Whether to ignore self-collisions within the group. Default is False
+        """
+        # Can only be done when sim is stopped
+        assert og.sim.is_stopped(), "Cannot create a collision group unless og.sim is stopped!"
+
+        # Make sure the group doesn't already exist
+        assert col_group not in cls.ACTIVE_COLLISION_GROUPS, \
+            f"Cannot create collision group {col_group} because it already exists!"
+
+        # Create the group
+        col_group_prim_path = f"/World/collision_groups/{col_group}"
+        group = lazy.pxr.UsdPhysics.CollisionGroup.Define(og.sim.stage, col_group_prim_path)
+        if filter_self_collisions:
+            # Do not collide with self
+            group.GetFilteredGroupsRel().AddTarget(col_group_prim_path)
+        cls.ACTIVE_COLLISION_GROUPS[col_group] = group
+
+    @classmethod
+    def add_to_collision_group(cls, col_group, prim_path):
         """
         Adds the prim and all nested prims specified by @prim_path to the global collision group @col_group. If @col_group
         does not exist, then it will either be created if @create_if_not_exist is True, otherwise will raise an Error.
         Args:
             col_group (str): Name of the collision group to assign the prim at @prim_path to
             prim_path (str): Prim (and all nested prims) to assign to this @col_group
-            create_if_not_exist (bool): True if @col_group should be created if it does not already exist, otherwise an
-                error will be raised
-        """
-        # TODO: This slows things down and / or crashes the sim with large number of objects. Skipping this for now, look into this later
-        pass
-        # # Check if collision group exists or not
-        # if col_group not in cls.ACTIVE_COLLISION_GROUPS:
-        #     # Raise error if we don't explicitly want to create a new group
-        #     if not create_if_not_exist:
-        #         raise ValueError(f"Collision group {col_group} not found in current registry, and create_if_not_exist"
-        #                          f"was set to False!")
-        #     # Otherwise, create the new group
-        #     col_group_name = f"/World/collisionGroup_{col_group}"
-        #     group = UsdPhysics.CollisionGroup.Define(get_current_stage(), col_group_name)
-        #     group.GetFilteredGroupsRel().AddTarget(col_group_name)  # Make sure that we can collide within our own group
-        #     cls.ACTIVE_COLLISION_GROUPS[col_group] = group
-        #
-        # # Add this prim to the collision group
-        # cls.ACTIVE_COLLISION_GROUPS[col_group].GetCollidersCollectionAPI().GetIncludesRel().AddTarget(prim_path)
+        """
+        # Make sure collision group exists
+        assert col_group in cls.ACTIVE_COLLISION_GROUPS, \
+            f"Cannot add to collision group {col_group} because it does not exist!"
+
+        # Add this prim to the collision group
+        cls.ACTIVE_COLLISION_GROUPS[col_group].GetCollidersCollectionAPI().GetIncludesRel().AddTarget(prim_path)
 
     @classmethod
     def clear(cls):