From efc64bd3ae3a39548536a067380f6fe2dfd71ec5 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 11:06:42 -0700 Subject: [PATCH 01/11] Lower contact threshold --- omnigibson/prims/rigid_prim.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 00ac105c4..aa8f523a9 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -115,11 +115,12 @@ def _post_load(self): # Only create contact report api if we're not visual only if not self._visual_only: - ( + contact_api = ( lazy.pxr.PhysxSchema.PhysxContactReportAPI(self._prim) if self._prim.HasAPI(lazy.pxr.PhysxSchema.PhysxContactReportAPI) else lazy.pxr.PhysxSchema.PhysxContactReportAPI.Apply(self._prim) ) + contact_api.GetThresholdAttr().Set(0.0) # Store references to owned visual / collision meshes # We iterate over all children of this object's prim, From c9a2a9cd4f23c62f9cc6fe390aa9eaaa44c35638 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 13:28:51 -0700 Subject: [PATCH 02/11] Always use contact sensor for TouchingAnyCondition --- omnigibson/transition_rules.py | 72 ++++------------------------------ 1 file changed, 8 insertions(+), 64 deletions(-) diff --git a/omnigibson/transition_rules.py b/omnigibson/transition_rules.py index 86c0817fd..6fc320663 100644 --- a/omnigibson/transition_rules.py +++ b/omnigibson/transition_rules.py @@ -347,78 +347,22 @@ def __init__(self, filter_1_name, filter_2_name): self._filter_1_name = filter_1_name self._filter_2_name = filter_2_name - # Will be filled in during self.initialize - # Maps object to the list of rigid body idxs in the global contact matrix corresponding to filter 1 - self._filter_1_idxs = None - - # If optimized, filter_2_idxs will be used, otherwise filter_2_bodies will be used! - # Maps object to the list of rigid body idxs in the global contact matrix corresponding to filter 2 - self._filter_2_idxs = None # Maps object to set of rigid bodies corresponding to filter 2 self._filter_2_bodies = None - # Flag whether optimized call can be used - self._optimized = None - 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 th.any( - # th.tensor( - # [ - # 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] - # ] - # ) - # ) - - # TODO: RigidContactAPI sometimes has false negatives (returning zero impulses when there are contacts), so we will stick - # with the non-optimized version for now. We will fix this in a future release. - self._optimized = False - - if self._optimized: - # Register idx mappings - self._filter_1_idxs = { - obj: [RigidContactAPI.get_body_row_idx(link.prim_path)[1] for link in obj.links.values()] - for obj in object_candidates[self._filter_1_name] - } - self._filter_2_idxs = { - obj: th.tensor( - [RigidContactAPI.get_body_col_idx(link.prim_path)[1] for link in obj.links.values()], - dtype=th.float32, - ) - for obj in object_candidates[self._filter_2_name] - } - else: - # Register body mappings - self._filter_2_bodies = {obj: set(obj.links.values()) for obj in object_candidates[self._filter_2_name]} + # Register body mappings + self._filter_2_bodies = {obj: set(obj.links.values()) for obj in object_candidates[self._filter_2_name]} def __call__(self, object_candidates): - # Keep any object that has non-zero impulses between itself and any of the @filter_2_name's objects + # Keep any of the @filter_2_name's objects objs = [] - if self._optimized: - # Batch check for each object - for obj in object_candidates[self._filter_1_name]: - # Get all impulses between @obj and any object in @filter_2_name that are in the same scene - idxs_to_check = th.cat( - [ - self._filter_2_idxs[obj2] - for obj2 in object_candidates[self._filter_2_name] - if obj2.scene == obj.scene - ] - ) - if th.any( - RigidContactAPI.get_all_impulses(obj.scene.idx)[self._filter_1_idxs[obj]][:, idxs_to_check.tolist()] - ): - objs.append(obj) - else: - # Manually check contact - filter_2_bodies = set.union(*(self._filter_2_bodies[obj] for obj in object_candidates[self._filter_2_name])) - for obj in object_candidates[self._filter_1_name]: - if len(obj.states[ContactBodies].get_value().intersection(filter_2_bodies)) > 0: - objs.append(obj) + # Manually check contact + filter_2_bodies = set.union(*(self._filter_2_bodies[obj] for obj in object_candidates[self._filter_2_name])) + for obj in object_candidates[self._filter_1_name]: + if len(obj.states[ContactBodies].get_value().intersection(filter_2_bodies)) > 0: + objs.append(obj) # Update candidates object_candidates[self._filter_1_name] = objs From 3866ca7f77de0e62ac552037beb70ec2f58baff3 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 14:19:02 -0700 Subject: [PATCH 03/11] Move contact sensor interface to simulator --- omnigibson/prims/rigid_prim.py | 13 ++++--------- omnigibson/simulator.py | 12 ++++++++++++ 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index aa8f523a9..1f7f9bfa7 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -67,7 +67,6 @@ def __init__( # Other values that will be filled in at runtime self._rigid_prim_view_direct = None self._belongs_to_articulation = None - self._cs = None # Contact sensor interface self._body_name = None self._visual_only = None @@ -145,10 +144,6 @@ def _post_load(self): else False ) - # Create contact sensor - self._cs = lazy.omni.isaac.sensor._sensor.acquire_contact_sensor_interface() - # self._create_contact_sensor() - def _initialize(self): # Run super method first super()._initialize() @@ -160,7 +155,7 @@ def _initialize(self): # Get contact info first if self.contact_reporting_enabled: - self._cs.get_rigid_body_raw_data(self.prim_path) + og.sim.contact_sensor.get_rigid_body_raw_data(self.prim_path) # Grab handle to this rigid body and get name self.update_handles() @@ -269,12 +264,12 @@ def contact_list(self): # Make sure we have the ability to grab contacts for this object contacts = [] if self.contact_reporting_enabled: - raw_data = self._cs.get_rigid_body_raw_data(self.prim_path) + raw_data = og.sim.contact_sensor.get_rigid_body_raw_data(self.prim_path) for c in raw_data: # convert handles to prim paths for comparison c = [*c] # CsRawData enforces body0 and body1 types to be ints, but we want strings - c[2] = self._cs.decode_body_name(c[2]) - c[3] = self._cs.decode_body_name(c[3]) + c[2] = og.sim.contact_sensor.decode_body_name(c[2]) + c[3] = og.sim.contact_sensor.decode_body_name(c[3]) contacts.append(CsRawData(*c)) return contacts diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 904e46939..731f0fcd4 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -403,6 +403,9 @@ def __init__( self.viewer_width = viewer_width self.viewer_height = viewer_height + # Acquire contact sensor interface + self._contact_sensor = lazy.omni.isaac.sensor._sensor.acquire_contact_sensor_interface() + def _set_viewer_camera(self, relative_prim_path="/viewer_camera", viewport_name="Viewport"): """ Creates a camera prim dedicated for this viewer at @prim_path if it doesn't exist, @@ -493,6 +496,7 @@ def _set_renderer_settings(self): lazy.carb.settings.get_settings().set_bool("/physics/fabricUpdateJointStates", False) lazy.carb.settings.get_settings().set_bool("/physics/fabricUpdateResiduals", False) lazy.carb.settings.get_settings().set_bool("/physics/fabricUseGPUInterop", True) + # lazy.carb.settings.get_settings().set_bool("/physics/disableSleeping", True) def _validate_dts(self, physics_dt, rendering_dt, sim_step_dt): """ @@ -1733,6 +1737,14 @@ def initial_rendering_dt(self): """ return self._initial_rendering_dt + @property + def contact_sensor(self): + """ + Returns: + ContactSensor: Contact sensor object + """ + return self._contact_sensor + def _dump_state(self): # Default state is from the scene return {i: scene.dump_state(serialized=False) for i, scene in enumerate(self.scenes)} From 964c09b84d754cd23a535181030d1a0f88d226ba Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 14:19:34 -0700 Subject: [PATCH 04/11] A few RigidContactAPI fixes --- omnigibson/object_states/touching.py | 7 ------- omnigibson/transition_rules.py | 1 - omnigibson/utils/usd_utils.py | 2 +- 3 files changed, 1 insertion(+), 9 deletions(-) diff --git a/omnigibson/object_states/touching.py b/omnigibson/object_states/touching.py index 104b97cee..06a97e65f 100644 --- a/omnigibson/object_states/touching.py +++ b/omnigibson/object_states/touching.py @@ -2,7 +2,6 @@ from omnigibson.object_states.kinematics_mixin import KinematicsMixin from omnigibson.object_states.object_state_base import BooleanStateMixin, RelativeObjectState from omnigibson.utils.constants import PrimType -from omnigibson.utils.usd_utils import RigidContactAPI class Touching(KinematicsMixin, RelativeObjectState, BooleanStateMixin): @@ -20,11 +19,5 @@ def _get_value(self, other): return self._check_contact(other, self.obj) elif other.prim_type == PrimType.CLOTH: return self._check_contact(self.obj, other) - # elif not self.obj.kinematic_only and not other.kinematic_only: - # # Use optimized check for rigid bodies - # return RigidContactAPI.in_contact( - # prim_paths_a=[link.prim_path for link in self.obj.links.values()], - # prim_paths_b=[link.prim_path for link in other.links.values()], - # ) else: return self._check_contact(other, self.obj) and self._check_contact(self.obj, other) diff --git a/omnigibson/transition_rules.py b/omnigibson/transition_rules.py index 6fc320663..f99c89fd1 100644 --- a/omnigibson/transition_rules.py +++ b/omnigibson/transition_rules.py @@ -25,7 +25,6 @@ from omnigibson.utils.python_utils import Registerable, classproperty, subclass_factory, torch_delete from omnigibson.utils.registry_utils import Registry from omnigibson.utils.ui_utils import create_module_logger, disclaimer -from omnigibson.utils.usd_utils import RigidContactAPI # Create module logger log = create_module_logger(module_name=__name__) diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index 6582474c4..c7194cf12 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -230,7 +230,7 @@ def get_column_filters(cls): @classmethod def get_max_contact_data_count(cls): - return 0 + return 256 def initialize_view(self): """ From a81974979a10f760ad4ff9e6307d80cb2fe3d9c8 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 14:37:46 -0700 Subject: [PATCH 05/11] Make sleep threshold lower --- omnigibson/prims/entity_prim.py | 2 +- omnigibson/simulator.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index fd0a512dc..f9e255258 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -21,7 +21,7 @@ m = create_module_macros(module_path=__file__) # Default sleep threshold for all objects -- see https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html?highlight=sleep#sleeping -m.DEFAULT_SLEEP_THRESHOLD = 0.001 +m.DEFAULT_SLEEP_THRESHOLD = 0.00005 class EntityPrim(XFormPrim): diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 731f0fcd4..822d051b1 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -496,7 +496,6 @@ def _set_renderer_settings(self): lazy.carb.settings.get_settings().set_bool("/physics/fabricUpdateJointStates", False) lazy.carb.settings.get_settings().set_bool("/physics/fabricUpdateResiduals", False) lazy.carb.settings.get_settings().set_bool("/physics/fabricUseGPUInterop", True) - # lazy.carb.settings.get_settings().set_bool("/physics/disableSleeping", True) def _validate_dts(self, physics_dt, rendering_dt, sim_step_dt): """ From 6383f08a9cd4b3741555af3d8c00da0f3c25d836 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 14:40:19 -0700 Subject: [PATCH 06/11] Check object states dependency when we enable transition rules --- omnigibson/scenes/scene_base.py | 1 + 1 file changed, 1 insertion(+) diff --git a/omnigibson/scenes/scene_base.py b/omnigibson/scenes/scene_base.py index 81e1d7636..acb76ec18 100644 --- a/omnigibson/scenes/scene_base.py +++ b/omnigibson/scenes/scene_base.py @@ -456,6 +456,7 @@ def load(self, idx, **kwargs): self._pose_inv = th.linalg.inv_ex(self._pose).inverse if gm.ENABLE_TRANSITION_RULES: + assert gm.ENABLE_OBJECT_STATES, "Transition rules require object states to be enabled!" self._transition_rule_api = TransitionRuleAPI(scene=self) # Always stop the sim if we started it internally From ac3f5c2f6edb30d607ffb54f6b3117c5986161c5 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 15:20:03 -0700 Subject: [PATCH 07/11] Add more comments --- omnigibson/object_states/contact_bodies.py | 4 ++++ omnigibson/prims/entity_prim.py | 3 +++ omnigibson/prims/rigid_prim.py | 2 ++ omnigibson/utils/usd_utils.py | 2 ++ 4 files changed, 11 insertions(+) diff --git a/omnigibson/object_states/contact_bodies.py b/omnigibson/object_states/contact_bodies.py index 5a77dd601..e9a63ea74 100644 --- a/omnigibson/object_states/contact_bodies.py +++ b/omnigibson/object_states/contact_bodies.py @@ -4,6 +4,10 @@ class ContactBodies(AbsoluteObjectState): + """ + NOTE: This is slow and uncached, but it works even for sleeping objects. + For frequent contact checks, consider using RigidContactAPI for performance. + """ def _get_value(self, ignore_objs=None): # Compute bodies in contact, minus the self-owned bodies diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index f9e255258..18e5d4d9c 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -21,6 +21,7 @@ m = create_module_macros(module_path=__file__) # Default sleep threshold for all objects -- see https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html?highlight=sleep#sleeping +# Mass-normalized kinetic energy threshold below which an actor may go to sleep m.DEFAULT_SLEEP_THRESHOLD = 0.00005 @@ -613,6 +614,8 @@ def visual_only(self, val): def contact_list(self): """ Get list of all current contacts with this object prim + NOTE: This method is slow and uncached, but it works even for sleeping objects. + For frequent contact checks, consider using RigidContactAPI for performance. Returns: list of CsRawData: raw contact info for this rigid body diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 1f7f9bfa7..6167f4ea9 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -257,6 +257,8 @@ def update_handles(self): def contact_list(self): """ Get list of all current contacts with this rigid body + NOTE: This method is slow and uncached, but it works even for sleeping objects. + For frequent contact checks, consider using RigidContactAPI for performance. Returns: list of CsRawData: raw contact info for this rigid body diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index c7194cf12..07421c0e6 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -185,6 +185,8 @@ def create_joint( class RigidContactAPIImpl: """ Class containing class methods to aggregate rigid body contacts across all rigid bodies in the simulator + + NOTE: The RigidContactAPI only works when the contacting objects are awake. If the objects could be asleep, use ContactBodies instead. """ def __init__(self): From 5411555b42c3e399cff36fea10f3909056ece085 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 15 Oct 2024 15:21:15 -0700 Subject: [PATCH 08/11] Minor fix for transform utils --- tests/test_transform_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_transform_utils.py b/tests/test_transform_utils.py index 36d1802bc..d7eaac50f 100644 --- a/tests/test_transform_utils.py +++ b/tests/test_transform_utils.py @@ -358,7 +358,7 @@ def test_quat_slerp(self): key_rots = R.from_quat(np.stack([q1.cpu().numpy(), q2.cpu().numpy()])) key_times = [0, 1] slerp = Slerp(key_times, key_rots) - scipy_q_slerp = slerp([t]).as_quat()[0].astype(NumpyTypes.FLOAT32) + scipy_q_slerp = slerp(t).as_quat()[0].astype(NumpyTypes.FLOAT32) assert quaternions_close(q_slerp, th.from_numpy(scipy_q_slerp)) assert_close(th.norm(q_slerp), th.tensor(1.0)) From 3e0e78de14dd17e1a69d639000f4db44b28e55d4 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Wed, 16 Oct 2024 14:26:33 -0700 Subject: [PATCH 09/11] Fix ObjectsInFOVOfRobot test --- tests/test_robot_states_no_flatcache.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/test_robot_states_no_flatcache.py b/tests/test_robot_states_no_flatcache.py index 3461f62e7..b02ba6158 100644 --- a/tests/test_robot_states_no_flatcache.py +++ b/tests/test_robot_states_no_flatcache.py @@ -40,5 +40,6 @@ def test_object_in_FOV_of_robot(): vision_sensor.set_position_orientation(position=[100, 150, 100]) og.sim.step() og.sim.step() - assert robot.states[ObjectsInFOVOfRobot].get_value() == [robot] + # Since the sensor is moved away from the robot, the robot should not see itself + assert robot.states[ObjectsInFOVOfRobot].get_value() == [] og.clear() From e562915418d89e677d4f988d1149e42b56416acc Mon Sep 17 00:00:00 2001 From: hang-yin Date: Mon, 21 Oct 2024 11:36:36 -0700 Subject: [PATCH 10/11] Use RigidContactAPI for TouchingAnyCondition --- omnigibson/transition_rules.py | 45 ++++++++++++++++++++++++++++------ 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/omnigibson/transition_rules.py b/omnigibson/transition_rules.py index f99c89fd1..ecdca6e87 100644 --- a/omnigibson/transition_rules.py +++ b/omnigibson/transition_rules.py @@ -25,6 +25,7 @@ from omnigibson.utils.python_utils import Registerable, classproperty, subclass_factory, torch_delete from omnigibson.utils.registry_utils import Registry from omnigibson.utils.ui_utils import create_module_logger, disclaimer +from omnigibson.utils.usd_utils import RigidContactAPI # Create module logger log = create_module_logger(module_name=__name__) @@ -333,6 +334,10 @@ 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 + + Note that this condition uses the RigidContactAPI for contact checking. This is not a persistent contact check, + meaning that if objects get in contact for some time and both fall asleep, the contact will not be detected. + To get persistent contact checking, please use contact_sensor. """ def __init__(self, filter_1_name, filter_2_name): @@ -346,21 +351,45 @@ def __init__(self, filter_1_name, filter_2_name): self._filter_1_name = filter_1_name self._filter_2_name = filter_2_name - # Maps object to set of rigid bodies corresponding to filter 2 - self._filter_2_bodies = None + # Will be filled in during self.initialize + # Maps object to the list of rigid body idxs in the global contact matrix corresponding to filter 1 + self._filter_1_idxs = None + + # If optimized, filter_2_idxs will be used, otherwise filter_2_bodies will be used! + # Maps object to the list of rigid body idxs in the global contact matrix corresponding to filter 2 + self._filter_2_idxs = None def refresh(self, object_candidates): - # Register body mappings - self._filter_2_bodies = {obj: set(obj.links.values()) for obj in object_candidates[self._filter_2_name]} + # Register idx mappings + self._filter_1_idxs = { + obj: [RigidContactAPI.get_body_row_idx(link.prim_path)[1] for link in obj.links.values()] + for obj in object_candidates[self._filter_1_name] + } + self._filter_2_idxs = { + obj: th.tensor( + [RigidContactAPI.get_body_col_idx(link.prim_path)[1] for link in obj.links.values()], + dtype=th.float32, + ) + for obj in object_candidates[self._filter_2_name] + } def __call__(self, object_candidates): - # Keep any of the @filter_2_name's objects + # 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 = set.union(*(self._filter_2_bodies[obj] for obj in object_candidates[self._filter_2_name])) + # Batch check for each object for obj in object_candidates[self._filter_1_name]: - if len(obj.states[ContactBodies].get_value().intersection(filter_2_bodies)) > 0: + # Get all impulses between @obj and any object in @filter_2_name that are in the same scene + idxs_to_check = th.cat( + [ + self._filter_2_idxs[obj2] + for obj2 in object_candidates[self._filter_2_name] + if obj2.scene == obj.scene + ] + ) + if th.any( + RigidContactAPI.get_all_impulses(obj.scene.idx)[self._filter_1_idxs[obj]][:, idxs_to_check.tolist()] + ): objs.append(obj) # Update candidates From 15886b77cbf62583b4552297944c4decedbbb144 Mon Sep 17 00:00:00 2001 From: hang-yin Date: Tue, 22 Oct 2024 11:07:03 -0700 Subject: [PATCH 11/11] More default attempts for action primitives --- .../action_primitives/starter_semantic_action_primitives.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 05fa48131..1b1aec0d0 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -491,7 +491,7 @@ def apply(self, action): action = StarterSemanticActionPrimitiveSet(action_idx) return self.apply_ref(action, target_obj) - def apply_ref(self, prim, *args, attempts=3): + def apply_ref(self, prim, *args, attempts=5): """ Yields action for robot to execute the primitive with the given arguments.