[WIP] Ensure contact happens within slicer volume before slicing

omnigibson/envs/env_base.py

@@ -525,7 +525,7 @@ def step(self, action):
             self._current_step += 1
 
             return obs, reward, done, info
-        except:
+        except KeyError:
             raise ValueError(f"Failed to execute environment step {self._current_step} in episode {self._current_episode}")
 
     def _reset_variables(self):

omnigibson/examples/object_states/slicing_demo.py

@@ -37,9 +37,8 @@ def main(random_selection=False, headless=False, short_exec=False):
     knife_cfg = dict(
         type="DatasetObject",
         name="knife",
-        category="table_knife",
-        model="lrdmpf",
-        bounding_box=[0.401, 0.044, 0.009],
+        category="carving_knife",
+        model="usqmjc",
         position=[0, 0, 10.0],
     )
 
@@ -95,6 +94,7 @@ def main(random_selection=False, headless=False, short_exec=False):
 
     # Step simulation for a bit so that apple is sliced
     for i in range(1000):
+    # while True:
         env.step(np.array([]))
 
     input("Apple has been sliced! Press [ENTER] to terminate the demo.")

omnigibson/transition_rules.py

@@ -23,6 +23,7 @@
 import omnigibson.utils.transform_utils as T
 from omnigibson.utils.ui_utils import disclaimer, create_module_logger
 from omnigibson.utils.usd_utils import RigidContactAPI
+from omnigibson.utils.geometry_utils import generate_points_in_volume_checker_function
 from omnigibson.utils.bddl_utils import translate_bddl_recipe_to_og_recipe, translate_bddl_washer_rule_to_og_washer_rule
 import bddl
 
@@ -334,13 +335,14 @@ class TouchingAnyCondition(RuleCondition):
     Rule condition that prunes object candidates from @filter_1_name, only keeping any that are touching any object
     from @filter_2_name
     """
-    def __init__(self, filter_1_name, filter_2_name):
+    def __init__(self, filter_1_name, filter_2_name, allow_optimized=True):
         """
         Args:
             filter_1_name (str): Name of the filter whose object candidates will be pruned based on whether or not
                 they are touching any object from @filter_2_name
             filter_2_name (str): Name of the filter whose object candidates will be used to prune the candidates from
                 @filter_1_name
+            allow_optimized (bool): Whether optimization using RigidContactAPI should be allowed
         """
         self._filter_1_name = filter_1_name
         self._filter_2_name = filter_2_name
@@ -357,11 +359,12 @@ def __init__(self, filter_1_name, filter_2_name):
 
         # Flag whether optimized call can be used
         self._optimized = None
+        self._allow_optimized = allow_optimized
 
     def refresh(self, object_candidates):
         # Check whether we can use optimized computation or not -- this is determined by whether or not any objects
         # in our collision set are kinematic only
-        self._optimized = not np.any([obj.kinematic_only or obj.prim_type == PrimType.CLOTH
+        self._optimized = self._allow_optimized and not np.any([obj.kinematic_only or obj.prim_type == PrimType.CLOTH
                                   for f in (self._filter_1_name, self._filter_2_name) for obj in object_candidates[f]])
 
         if self._optimized:
@@ -405,6 +408,63 @@ def modifies_filter_names(self):
         return {self._filter_1_name}
 
 
+class TouchingSlicerVolumeCondition(TouchingAnyCondition):
+    def __init__(self, filter_1_name, filter_2_name):
+        '''
+        Checks if filter_2_bodies have a contact point within the collision volume which is a slicer meta link generated by objs in filter_1_bodies.
+        '''
+        super().__init__(filter_1_name, filter_2_name, allow_optimized=False)
+
+
+    def __call__(self, object_candidates):
+        # Keep any object that has non-zero impulses between itself and any of the @filter_2_name's objects
+        objs = []
+
+        # Manually check contact
+        filter_2_bodies = {x.prim_path for obj in object_candidates[self._filter_2_name] for x in self._filter_2_bodies[obj] }
+
+        for obj in object_candidates[self._filter_1_name]:
+            # Find contact points with eligible objects
+            contact_points = []
+            for contact in obj.contact_list():
+                if contact.body1 in filter_2_bodies:
+                    contact_points.append(contact.position.tolist())
+
+            # Skip non-contacted objects
+            if not contact_points:
+                continue
+
+            # Find slicer volume
+            slicer_volume = None
+            for name, link in obj.links.items():
+                if 'slicer' in name:
+                    slicer_volume = link
+                    break
+
+            # Skip non-slicer objects
+            if slicer_volume is None:
+                continue
+
+            # TODO: Consider moving this to the SlicerActive state for performance.
+            check_in_volume, _ = generate_points_in_volume_checker_function(
+                obj=obj,
+                volume_link=slicer_volume  
+            )
+
+            # Check if the contact point is within the check_in_volume function
+            if not np.any(check_in_volume(contact_points)):
+                continue
+
+            # If we make it here, this is a legit slicer contact.
+            objs.append(obj)
+
+        # Update candidates
+        object_candidates[self._filter_1_name] = objs
+
+        # If objs is empty, return False, otherwise, True
+        return len(objs) > 0
+
+
 class StateCondition(RuleCondition):
     """
     Rule condition that checks all objects from @filter_name whether a state condition is equal to @val for
@@ -896,7 +956,7 @@ def candidate_filters(cls):
     @classmethod
     def _generate_conditions(cls):
         # sliceables should be touching any slicer
-        return [TouchingAnyCondition(filter_1_name="sliceable", filter_2_name="slicer"),
+        return [TouchingSlicerVolumeCondition(filter_1_name="slicer", filter_2_name="sliceable"),
                 StateCondition(filter_name="slicer", state=SlicerActive, val=True, op=operator.eq)]
 
     @classmethod

tests/test_transition_rules.py

@@ -1546,3 +1546,81 @@ def test_single_toggleable_machine_rule_output_object_success():
         obj = DatasetObject(**obj_cfg)
         og.sim.import_object(obj)
     og.sim.step()
+
+@og_test
+def test_slicer_volume_sharp_end():
+    """
+    Demo of slicing an apple into two apple slices
+    """
+
+    # Create the scene config to load -- empty scene with knife and apple
+
+    # Grab reference to apple and knife
+    apple = og.sim.scene.object_registry("name", "apple")
+    knife = og.sim.scene.object_registry("name", "knife")
+    place_obj_on_floor_plane(apple)
+    place_obj_on_floor_plane(knife)
+
+
+    # Let apple settle
+    for _ in range(100):
+        og.sim.step()
+
+    # knife.keep_still()
+    knife.set_position_orientation(
+        position=apple.get_position() + np.array([-0.15, 0.0, 0.2]),
+        orientation=T.euler2quat([-np.pi, 0, 0]),
+    )
+
+    # Step simulation for a bit so that apple is sliced
+    for i in range(100):
+        og.sim.step()
+
+    # Check if apple is sliced
+    is_empty = len(og.sim.scene.object_registry("category", "half_apple", default_val=[])) == 0 
+    assert not is_empty
+
+    # Remove objects
+    og.sim.remove_object(apple)
+    og.sim.remove_object(knife)
+
+@og_test
+def test_slicer_volume_blunt_end():
+    """
+    Demo of slicing an apple into two apple slices
+    """
+
+    # Create the scene config to load -- empty scene with knife and apple
+
+    # Grab reference to apple and knife
+    apple = og.sim.scene.object_registry("name", "apple")
+    knife = og.sim.scene.object_registry("name", "knife")
+    place_obj_on_floor_plane(apple)
+    place_obj_on_floor_plane(knife)
+
+    # Let apple settle
+    for _ in range(100):
+        og.sim.step()
+
+    knife.keep_still()
+    knife.set_position_orientation(
+        position=apple.get_position() + np.array([-0.15, 0.0, 0.2]),
+        orientation=T.euler2quat([np.pi, 0, 0]),
+    )
+
+    # Step simulation for a bit so that apple is sliced
+    for i in range(1000):
+        og.sim.step()
+
+
+    # Check if apple is not sliced
+    is_sliced = og.sim.scene.object_registry("name", "apple") is None
+    assert not is_sliced
+
+    # Remove objects
+    og.sim.remove_object(apple)
+    og.sim.remove_object(knife)
+
+if __name__ == "__main__":
+    test_slicer_volume_blunt_end()
+    test_slicer_volume_sharp_end()

tests/utils.py

@@ -2,6 +2,7 @@
 
 from omnigibson.macros import gm
 from omnigibson.object_states import *
+from omnigibson.simulator import launch_simulator
 from omnigibson.utils.constants import PrimType, ParticleModifyCondition, ParticleModifyMethod
 from omnigibson.systems import *
 import omnigibson.utils.transform_utils as T
@@ -20,10 +21,11 @@
 def og_test(func):
     def wrapper():
         assert_test_scene()
-        try:
-            func()
-        finally:
-            og.sim.scene.reset()
+        func()
+        # try:
+        #     func()
+        # finally:
+        #     og.sim.scene.reset()
     return wrapper
 
 num_objs = 0
@@ -88,6 +90,8 @@ def assert_test_scene():
                 get_obj_cfg("vacuum", "vacuum", "bdmsbr", visual_only=True, abilities={"toggleable": {}, "particleRemover": {"method": ParticleModifyMethod.PROJECTION, "conditions": {"water": [(ParticleModifyCondition.TOGGLEDON, True)]}}}),
                 get_obj_cfg("blender", "blender", "cwkvib", bounding_box=[0.316, 0.318, 0.649], abilities={"fillable": {}, "toggleable": {}, "heatable": {}}),
                 get_obj_cfg("oven", "oven", "cgtaer", bounding_box=[0.943, 0.837, 1.297]),
+                get_obj_cfg("knife", "carving_knife", "usqmjc"),
+                get_obj_cfg("apple", "apple", "agveuv", bounding_box=[0.098, 0.098, 0.115]),
                 get_obj_cfg("baking_sheet", "baking_sheet", "yhurut", bounding_box=[0.41607812, 0.43617093, 0.02281223]),
                 get_obj_cfg("bagel_dough", "bagel_dough", "iuembm"),
                 get_obj_cfg("raw_egg", "raw_egg", "ydgivr"),