properly handle collision groups

omnigibson/objects/object_base.py

@@ -185,14 +185,6 @@ def _post_load(self):
             lazy.pxr.PhysxSchema.PhysxArticulationAPI.Apply(root_prim)
             self.self_collisions = self._load_config["self_collisions"]
 
-        # 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(
             prim=self._prim,

omnigibson/prims/entity_prim.py

@@ -900,11 +900,13 @@ 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()
-        # Delegate to RigidPrim if we are not articulated
+        # If the simulation isn't running, we should set this prim's XForm (object-level) properties directly
         if og.sim.is_stopped():
             XFormPrim.set_position_orientation(self, position=position, orientation=orientation)
+        # Delegate to RigidPrim if we are not articulated
         elif self._articulation_view is None:
             self.root_link.set_position_orientation(position=position, orientation=orientation)
+        # Sim is running and articulation view exists, so use that physx API backend
         else:
             if position is not None:
                 position = np.asarray(position)[None, :]
@@ -914,11 +916,13 @@ def set_position_orientation(self, position=None, orientation=None):
             BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
+        # If the simulation isn't running, we should read from this prim's XForm (object-level) properties directly
         if og.sim.is_stopped():
             return XFormPrim.get_position_orientation(self)
         # Delegate to RigidPrim if we are not articulated
         elif self._articulation_view is None:
             return self.root_link.get_position_orientation()
+        # Sim is running and articulation view exists, so use that physx API backend
         else:
             positions, orientations = self._articulation_view.get_world_poses()
             return positions[0], orientations[0][[1, 2, 3, 0]]
@@ -927,11 +931,13 @@ 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 the simulation isn't running, we should set this prim's XForm (object-level) properties directly
         if og.sim.is_stopped():
             return XFormPrim.set_local_pose(self, position, orientation)
         # Delegate to RigidPrim if we are not articulated
         elif self._articulation_view is None:
             self.root_link.set_local_pose(position=position, orientation=orientation)
+        # Sim is running and articulation view exists, so use that physx API backend
         else:
             if position is not None:
                 position = np.asarray(position)[None, :]
@@ -941,11 +947,13 @@ def set_local_pose(self, position=None, orientation=None):
             BoundingBoxAPI.clear()
 
     def get_local_pose(self):
+        # If the simulation isn't running, we should read from this prim's XForm (object-level) properties directly
         if og.sim.is_stopped():
             return XFormPrim.get_local_pose(self)
         # Delegate to RigidPrim if we are not articulated
         elif self._articulation_view is None:
             return self.root_link.get_local_pose()
+        # Sim is running and articulation view exists, so use that physx API backend
         else:
             positions, orientations = self._articulation_view.get_local_poses()
             return positions[0], orientations[0][[1, 2, 3, 0]]

omnigibson/scenes/scene_base.py

@@ -171,8 +171,17 @@ 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)
+        # Create collision group for fixed base objects' non root links, root links, and building structures
+        CollisionAPI.create_collision_group(col_group="fixed_base_nonroot_links", filter_self_collisions=False)
+        # Disable collision between root links of fixed base objects
+        CollisionAPI.create_collision_group(col_group="fixed_base_root_links", filter_self_collisions=True)
+        # Disable collision between building structures
+        CollisionAPI.create_collision_group(col_group="structures", filter_self_collisions=True)
+
+        # Disable collision between building structures and fixed base objects
+        CollisionAPI.add_group_filter(col_group="structures", filter_group="fixed_base_nonroot_links")
+        CollisionAPI.add_group_filter(col_group="structures", filter_group="fixed_base_root_links")
+
         # 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)
@@ -427,6 +436,21 @@ 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
+        # We do this by adding the object to our tracked collision groups
+        structure_categories = {"walls", "floors", "ceilings"}
+        if obj.fixed_base and obj.category != robot_macros.ROBOT_CATEGORY:
+            # TODO: Remove structure hotfix once asset collision meshes are fixed!!
+            if obj.category in structure_categories:
+                CollisionAPI.add_to_collision_group(col_group="structures", prim_path=obj.prim_path)
+            else:
+                for link in obj.links.values():
+                    CollisionAPI.add_to_collision_group(
+                        col_group="fixed_base_root_links" if link == obj.root_link else "fixed_base_nonroot_links",
+                        prim_path=link.prim_path,
+                    )
+
         # Add this object to our registry based on its type, if we want to register it
         if register:
             self.object_registry.add(obj)

omnigibson/simulator.py

@@ -306,9 +306,9 @@ def _set_physics_engine_settings(self):
             """
             assert self.is_stopped(), f"Cannot set simulator physics settings while simulation is playing!"
             self._physics_context.set_gravity(value=-self.gravity)
-            # Also make sure we invert the collision group filter settings so that different collision groups cannot
-            # collide with each other, and modify settings for speed optimization
-            self._physics_context.set_invert_collision_group_filter(True)
+            # Also make sure we don't invert the collision group filter settings so that different collision groups by
+            # default collide with each other, and modify settings for speed optimization
+            self._physics_context.set_invert_collision_group_filter(False)
             self._physics_context.enable_ccd(gm.ENABLE_CCD)
 
             if meets_minimum_isaac_version("2023.0.0"):

omnigibson/utils/usd_utils.py

@@ -395,6 +395,25 @@ def add_to_collision_group(cls, col_group, prim_path):
         # Add this prim to the collision group
         cls.ACTIVE_COLLISION_GROUPS[col_group].GetCollidersCollectionAPI().GetIncludesRel().AddTarget(prim_path)
 
+    @classmethod
+    def add_group_filter(cls, col_group, filter_group):
+        """
+        Adds a new group filter for group @col_group, filtering all collision with group @filter_group
+        Args:
+            col_group (str): Name of the collision group which will have a new filter group added
+            filter_group (str): Name of the group that should be filtered
+        """
+        # Make sure the group doesn't already exist
+        for group_name in (col_group, filter_group):
+            assert group_name in cls.ACTIVE_COLLISION_GROUPS, \
+                (f"Cannot add group filter {filter_group} to collision group {col_group} because at least one group "
+                 f"does not exist!")
+
+        # Grab the group, and add the filter
+        filter_group_prim_path = f"/World/collision_groups/{filter_group}"
+        group = cls.ACTIVE_COLLISION_GROUPS[col_group]
+        group.GetFilteredGroupsRel().AddTarget(filter_group_prim_path)
+
     @classmethod
     def clear(cls):
         """