From d3713730bbd98aa3c04300e200e60544a83d75c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 13:20:07 -0800 Subject: [PATCH 01/28] Remove ENABLE_FLATCACHE flag --- omnigibson/examples/action_primitives/solve_task.py | 3 +-- omnigibson/examples/learning/navigation_policy_demo.py | 3 +-- omnigibson/examples/robots/grasping_mode_example.py | 3 +-- omnigibson/examples/robots/robot_control_example.py | 3 +-- omnigibson/examples/scenes/scene_selector.py | 1 - omnigibson/macros.py | 3 --- omnigibson/utils/usd_utils.py | 3 +-- tests/test_envs.py | 1 - tests/test_primitives.py | 1 - tests/utils.py | 1 - 10 files changed, 5 insertions(+), 17 deletions(-) diff --git a/omnigibson/examples/action_primitives/solve_task.py b/omnigibson/examples/action_primitives/solve_task.py index a9bed9daa..09fe9bbfe 100644 --- a/omnigibson/examples/action_primitives/solve_task.py +++ b/omnigibson/examples/action_primitives/solve_task.py @@ -13,9 +13,8 @@ from omnigibson.utils.ui_utils import KeyboardRobotController -# Don't use GPU dynamics and use flatcache for performance boost +# Don't use GPU dynamics gm.USE_GPU_DYNAMICS = True -gm.ENABLE_FLATCACHE = True def pause(time): for _ in range(int(time*100)): diff --git a/omnigibson/examples/learning/navigation_policy_demo.py b/omnigibson/examples/learning/navigation_policy_demo.py index 31e9821c6..bef11b72e 100644 --- a/omnigibson/examples/learning/navigation_policy_demo.py +++ b/omnigibson/examples/learning/navigation_policy_demo.py @@ -36,10 +36,9 @@ assert meets_minimum_version(gym.__version__, "0.26.1"), "Please install/update gym to version >= 0.26.1" -# We don't need object states nor transitions rules, so we disable them now, and also enable flatcache for maximum speed +# We don't need object states nor transitions rules, so we disable them now gm.ENABLE_OBJECT_STATES = False gm.ENABLE_TRANSITION_RULES = False -gm.ENABLE_FLATCACHE = True class CustomCombinedExtractor(BaseFeaturesExtractor): diff --git a/omnigibson/examples/robots/grasping_mode_example.py b/omnigibson/examples/robots/grasping_mode_example.py index d11e57c58..1218377bc 100644 --- a/omnigibson/examples/robots/grasping_mode_example.py +++ b/omnigibson/examples/robots/grasping_mode_example.py @@ -13,9 +13,8 @@ physical="Physical Grasping - No additional grasping assistance applied", ) -# Don't use GPU dynamics and Use flatcache for performance boost +# Don't use GPU dynamics gm.USE_GPU_DYNAMICS = False -gm.ENABLE_FLATCACHE = True def main(random_selection=False, headless=False, short_exec=False): diff --git a/omnigibson/examples/robots/robot_control_example.py b/omnigibson/examples/robots/robot_control_example.py index 360b7adec..1b61dfd2c 100644 --- a/omnigibson/examples/robots/robot_control_example.py +++ b/omnigibson/examples/robots/robot_control_example.py @@ -21,9 +21,8 @@ empty="Empty environment with no objects", ) -# Don't use GPU dynamics and use flatcache for performance boost +# Don't use GPU dynamics gm.USE_GPU_DYNAMICS = False -gm.ENABLE_FLATCACHE = True def choose_controllers(robot, random_selection=False): diff --git a/omnigibson/examples/scenes/scene_selector.py b/omnigibson/examples/scenes/scene_selector.py index feb676c22..a20fc8f0b 100644 --- a/omnigibson/examples/scenes/scene_selector.py +++ b/omnigibson/examples/scenes/scene_selector.py @@ -5,7 +5,6 @@ # Configure macros for maximum performance gm.USE_GPU_DYNAMICS = True -gm.ENABLE_FLATCACHE = True gm.ENABLE_OBJECT_STATES = False gm.ENABLE_TRANSITION_RULES = False diff --git a/omnigibson/macros.py b/omnigibson/macros.py index 045563ecc..00c90ccff 100644 --- a/omnigibson/macros.py +++ b/omnigibson/macros.py @@ -45,9 +45,6 @@ # Whether to use high-fidelity rendering (this includes, e.g., isosurfaces) gm.ENABLE_HQ_RENDERING = False -# Whether to use omni's flatcache feature or not (can speed up simulation) -gm.ENABLE_FLATCACHE = False - # Whether to use continuous collision detection or not (slower simulation, but can prevent # objects from tunneling through each other) gm.ENABLE_CCD = False diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index d20512c14..d06dd6541 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -431,8 +431,7 @@ def compute_aabb(cls, prim): - 3-array: end (x,y,z) corner of world-coordinate frame aligned bounding box """ # Use the correct API to calculate AABB based on whether flatcache is enabled or not - return cls._compute_flatcache_aabb(prim=prim) if gm.ENABLE_FLATCACHE else \ - cls._compute_non_flatcache_aabb(prim_path=prim.prim_path) + return cls._compute_flatcache_aabb(prim=prim) @classmethod def _compute_flatcache_aabb(cls, prim): diff --git a/tests/test_envs.py b/tests/test_envs.py index 10a8d8b5b..f0b5cb1f4 100644 --- a/tests/test_envs.py +++ b/tests/test_envs.py @@ -31,7 +31,6 @@ def task_tester(task_type): # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True - gm.ENABLE_FLATCACHE = False # Create the environment env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) diff --git a/tests/test_primitives.py b/tests/test_primitives.py index 03a3e6a48..7d1f775b0 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -92,7 +92,6 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = False - gm.ENABLE_FLATCACHE = False # Create the environment env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) diff --git a/tests/utils.py b/tests/utils.py index eecc55611..f495221df 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -85,7 +85,6 @@ def assert_test_scene(): # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True - gm.ENABLE_FLATCACHE = False # Create the environment env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) From cdd00d35d250be07ae7a27ac3ccce4475ac83a76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 13:21:06 -0800 Subject: [PATCH 02/28] Implement new cloth particle getter --- .../folded_unfolded_state_demo.py | 4 +- omnigibson/object_states/particle_modifier.py | 7 +- omnigibson/objects/dataset_object.py | 2 +- omnigibson/prims/cloth_prim.py | 68 +++++++++---------- omnigibson/prims/entity_prim.py | 2 +- omnigibson/systems/macro_particle_system.py | 9 +-- omnigibson/systems/micro_particle_system.py | 5 -- tests/test_object_states.py | 4 +- 8 files changed, 41 insertions(+), 60 deletions(-) diff --git a/omnigibson/examples/object_states/folded_unfolded_state_demo.py b/omnigibson/examples/object_states/folded_unfolded_state_demo.py index 72d8d7b5a..a8b0dfea5 100644 --- a/omnigibson/examples/object_states/folded_unfolded_state_demo.py +++ b/omnigibson/examples/object_states/folded_unfolded_state_demo.py @@ -95,7 +95,7 @@ def print_state(): # Fold all three cloths along the x-axis for i in range(3): obj = objs[i] - pos = obj.root_link.compute_particle_positions() + pos = obj.root_link.get_particle_positions() x_min, x_max = np.min(pos, axis=0)[0], np.max(pos, axis=0)[0] x_extent = x_max - x_min # Get indices for the bottom 10 percent vertices in the x-axis @@ -119,7 +119,7 @@ def print_state(): # Fold the t-shirt twice again along the y-axis for direction in [-1, 1]: obj = shirt - pos = obj.root_link.compute_particle_positions() + pos = obj.root_link.get_particle_positions() y_min, y_max = np.min(pos, axis=0)[1], np.max(pos, axis=0)[1] y_extent = y_max - y_min if direction == 1: diff --git a/omnigibson/object_states/particle_modifier.py b/omnigibson/object_states/particle_modifier.py index c6676fba9..b67d3efe6 100644 --- a/omnigibson/object_states/particle_modifier.py +++ b/omnigibson/object_states/particle_modifier.py @@ -21,7 +21,7 @@ get_particle_positions_in_frame, get_particle_positions_from_frame from omnigibson.utils.python_utils import classproperty from omnigibson.utils.ui_utils import suppress_omni_log -from omnigibson.utils.usd_utils import create_primitive_mesh, FlatcacheAPI +from omnigibson.utils.usd_utils import create_primitive_mesh import omnigibson.utils.transform_utils as T from omnigibson.utils.sampling_utils import sample_cuboid_on_object from omni.isaac.core.utils.prims import get_prim_at_path, delete_prim, is_prim_path_valid @@ -541,11 +541,6 @@ def check_conditions_for_system(self, system_name): return all(condition(self.obj) for condition in self.conditions[system_name]) def _update(self): - # If we're using projection method and flatcache, we need to manually update this object's transforms on the USD - # so the corresponding visualization and overlap meshes are updated properly - if self.method == ParticleModifyMethod.PROJECTION and gm.ENABLE_FLATCACHE: - FlatcacheAPI.sync_raw_object_transforms_in_usd(prim=self.obj) - # Check if there's any overlap and if we're at the correct step if self._current_step == 0 and (not self.requires_overlap or self._check_overlap()): # Iterate over all owned systems for this particle modifier diff --git a/omnigibson/objects/dataset_object.py b/omnigibson/objects/dataset_object.py index 37e582e70..1b7862d2b 100644 --- a/omnigibson/objects/dataset_object.py +++ b/omnigibson/objects/dataset_object.py @@ -498,7 +498,7 @@ def get_base_aligned_bbox(self, link_name=None, visual=False, xy_aligned=False, if self.prim_type == PrimType.CLOTH: particle_contact_offset = self.root_link.cloth_system.particle_contact_offset - particle_positions = self.root_link.compute_particle_positions() + particle_positions = self.root_link.get_particle_positions() particles_in_world_frame = np.concatenate([ particle_positions - particle_contact_offset, particle_positions + particle_contact_offset diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 547879ef3..639ca36c3 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -6,10 +6,11 @@ # distribution of this software and related documentation without an express # license agreement from NVIDIA CORPORATION is strictly prohibited. # +from omnigibson.utils.geometry_utils import get_particle_positions_from_frame, get_particle_positions_in_frame from pxr import UsdPhysics, Gf, Vt, PhysxSchema from pxr.Sdf import ValueTypeNames as VT -from omni.physx.scripts import particleUtils +from omni.isaac.core.prims.soft import ClothPrimView from omnigibson.macros import create_module_macros, gm from omnigibson.prims.geom_prim import GeomPrim @@ -65,6 +66,7 @@ def __init__( # Internal vars stored self._keypoint_idx = None self._keyface_idx = None + self._cloth_prim_view = None # Run super init super().__init__( @@ -77,10 +79,7 @@ def _post_load(self): # run super first super()._post_load() - # Make sure flatcache is not being used -- if so, raise an error, since we lose most of our needed functionality - # (such as R/W to specific particle states) when flatcache is enabled - assert not gm.ENABLE_FLATCACHE, "Cannot use flatcache with ClothPrim!" - + # TODO: Do this using the cloth API too! self._mass_api = UsdPhysics.MassAPI(self._prim) if self._prim.HasAPI(UsdPhysics.MassAPI) else \ UsdPhysics.MassAPI.Apply(self._prim) @@ -91,9 +90,13 @@ def _post_load(self): # Clothify this prim, which is assumed to be a mesh ClothPrim.cloth_system.clothify_mesh_prim(mesh_prim=self._prim) - # Track generated particle count - positions = self.compute_particle_positions() - self._n_particles = len(positions) + # Track generated particle count. This is the only time we use the USD API. + self._n_particles = len(self._prim.GetAttribute("points").Get()) + + # Load the cloth prim view + self._cloth_prim_view = ClothPrimView(self._prim_path) + + positions = self.get_particle_positions() # Sample mesh keypoints / keyvalues and sanity check the AABB of these subsampled points vs. the actual points success = False @@ -119,12 +122,16 @@ def _post_load(self): def _initialize(self): super()._initialize() + + self._cloth_prim_view.initialize(og.sim._physics_sim_view) + # TODO (eric): hacky way to get cloth rendering to work (otherwise, there exist some rendering artifacts). self._prim.CreateAttribute("primvars:isVolume", VT.Bool, False).Set(True) self._prim.GetAttribute("primvars:isVolume").Set(False) # Store the default position of the points in the local frame - self._default_positions = np.array(self.get_attribute(attr="points")) + self._default_positions = get_particle_positions_in_frame( + *self.get_position_orientation(), self.scale, self.get_particle_positions()) @classproperty def cloth_system(cls): @@ -146,7 +153,7 @@ def kinematic_only(self): """ return False - def compute_particle_positions(self, idxs=None): + def get_particle_positions(self, idxs=None): """ Compute individual particle positions for this cloth prim @@ -157,16 +164,8 @@ def compute_particle_positions(self, idxs=None): np.array: (N, 3) numpy array, where each of the N particles' positions are expressed in (x,y,z) cartesian coordinates relative to the world frame """ - t, r = self.get_position_orientation() - r = T.quat2mat(r) - s = self.scale - - # Don't copy to save compute, since we won't be returning a reference to the underlying object anyways - p_local = np.array(self.get_attribute(attr="points"), copy=False) - p_local = p_local[idxs] if idxs is not None else p_local - p_world = (r @ (p_local * s).T).T + t - - return p_world + all_particle_positions = self._cloth_prim_view.get_world_positions()[0, :, :] + return all_particle_positions[:self._n_particles] if idxs is None else all_particle_positions[idxs] def set_particle_positions(self, positions, idxs=None): """ @@ -180,19 +179,15 @@ def set_particle_positions(self, positions, idxs=None): n_expected = self._n_particles if idxs is None else len(idxs) assert len(positions) == n_expected, \ f"Got mismatch in particle setting size: {len(positions)}, vs. number of expected particles {n_expected}!" + + # First, get the particle positions. + cur_pos = self._cloth_prim_view.get_world_positions() - r = T.quat2mat(self.get_orientation()) - t = self.get_position() - s = self.scale - p_local = (r.T @ (positions - t).T).T / s - - # Fill the idxs if requested - if idxs is not None: - p_local_old = np.array(self.get_attribute(attr="points")) - p_local_old[idxs] = p_local - p_local = p_local_old + # Then apply the new positions at the appropriate indices + cur_pos[0, idxs] = positions - self.set_attribute(attr="points", val=Vt.Vec3fArray.FromNumpy(p_local)) + # Then set to that position + self._cloth_prim_view.set_world_positions(cur_pos) @property def keypoint_idx(self): @@ -243,7 +238,7 @@ def keypoint_particle_positions(self): np.array: (N, 3) numpy array, where each of the N keypoint particles' positions are expressed in (x,y,z) cartesian coordinates relative to the world frame """ - return self.compute_particle_positions(idxs=self._keypoint_idx) + return self.get_particle_positions(idxs=self._keypoint_idx) @property def particle_velocities(self): @@ -285,7 +280,7 @@ def compute_face_normals(self, face_ids=None): cartesian coordinates with respect to the world frame. """ faces = self.faces if face_ids is None else self.faces[face_ids] - points = self.compute_particle_positions(idxs=faces.flatten()).reshape(-1, 3, 3) + points = self.get_particle_positions(idxs=faces.flatten()).reshape(-1, 3, 3) return self.compute_face_normals_from_particle_positions(positions=points) def compute_face_normals_from_particle_positions(self, positions): @@ -328,7 +323,7 @@ def report_hit(hit): )) return True - positions = self.keypoint_particle_positions if keypoints_only else self.compute_particle_positions() + positions = self.keypoint_particle_positions if keypoints_only else self.get_particle_positions() for pos in positions: og.sim.psqi.overlap_sphere(ClothPrim.cloth_system.particle_contact_offset, pos, report_hit, False) @@ -523,7 +518,7 @@ def _dump_state(self): state = super()._dump_state() state["particle_group"] = self.particle_group state["n_particles"] = self.n_particles - state["particle_positions"] = self.compute_particle_positions() + state["particle_positions"] = self.get_particle_positions() state["particle_velocities"] = self.particle_velocities return state @@ -586,5 +581,6 @@ def reset(self): Reset the points to their default positions in the local frame, and also zeroes out velocities """ if self.initialized: - self.set_attribute(attr="points", val=Vt.Vec3fArray.FromNumpy(self._default_positions)) + self.set_particle_positions(get_particle_positions_from_frame( + *self.get_position_orientation(), self.scale, self._default_positions)) self.particle_velocities = np.zeros((self._n_particles, 3)) diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index 14793c37d..914debc95 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -1269,7 +1269,7 @@ def aabb(self): # normal method for computing bounding box if self._prim_type == PrimType.CLOTH: particle_contact_offset = self.root_link.cloth_system.particle_contact_offset - particle_positions = self.root_link.compute_particle_positions() + particle_positions = self.root_link.get_particle_positions() aabb_lo, aabb_hi = np.min(particle_positions, axis=0) - particle_contact_offset, \ np.max(particle_positions, axis=0) + particle_contact_offset else: diff --git a/omnigibson/systems/macro_particle_system.py b/omnigibson/systems/macro_particle_system.py index 13dc767c6..2876dbcdf 100644 --- a/omnigibson/systems/macro_particle_system.py +++ b/omnigibson/systems/macro_particle_system.py @@ -13,7 +13,6 @@ from omnigibson.utils.python_utils import classproperty, subclass_factory, snake_case_to_camel_case from omnigibson.utils.sampling_utils import sample_cuboid_on_object_symmetric_bimodal_distribution import omnigibson.utils.transform_utils as T -from omnigibson.utils.usd_utils import FlatcacheAPI from omnigibson.prims.geom_prim import VisualGeomPrim, CollisionVisualGeomPrim import numpy as np from scipy.spatial.transform import Rotation as R @@ -381,7 +380,7 @@ def update(cls): cloth = obj.root_link face_ids = cls._cloth_face_ids[group] idxs = cloth.faces[face_ids].flatten() - positions = cloth.compute_particle_positions(idxs=idxs).reshape(-1, 3, 3) + positions = cloth.get_particle_positions(idxs=idxs).reshape(-1, 3, 3) normals = cloth.compute_face_normals_from_particle_positions(positions=positions) # The actual positions we want are the face centroids, or the mean of all the positions @@ -495,10 +494,6 @@ def generate_group_particles( scales = cls.sample_scales_by_group(group=group, n=n_particles) if scales is None else scales bbox_extents_local = [(cls._particle_object.aabb_extent * scale).tolist() for scale in scales] - # If we're using flatcache, we need to update the object's pose on the USD manually - if gm.ENABLE_FLATCACHE: - FlatcacheAPI.sync_raw_object_transforms_in_usd(prim=obj) - # Generate particles z_up = np.zeros((3, 1)) z_up[-1] = 1.0 @@ -557,7 +552,7 @@ def generate_group_particles_on_object(cls, group, max_samples, min_samples_for_ face_ids = np.random.choice(n_faces, min(max_samples, n_faces), replace=False) # Positions are the midpoints of each requested face normals = cloth.compute_face_normals(face_ids=face_ids) - positions = cloth.compute_particle_positions(idxs=cloth.faces[face_ids].flatten()).reshape(-1, 3, 3).mean(axis=1) + positions = cloth.get_particle_positions(idxs=cloth.faces[face_ids].flatten()).reshape(-1, 3, 3).mean(axis=1) # Orientations are the normals z_up = np.zeros_like(normals) z_up[:, 2] = 1.0 diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index 0c0361f29..54c83f190 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -423,11 +423,6 @@ def initialize(cls): if not gm.USE_GPU_DYNAMICS: raise ValueError(f"Failed to initialize {cls.name} system. Please set gm.USE_GPU_DYNAMICS to be True.") - # Make sure flatcache is not being used OR isosurface is enabled -- otherwise, raise an error, since - # non-isosurface particles don't get rendered properly when flatcache is enabled - assert cls.use_isosurface or not gm.ENABLE_FLATCACHE, \ - f"Cannot use flatcache with MicroParticleSystem {cls.name} when no isosurface is used!" - cls.system_prim = cls._create_particle_system() # Create material cls._material = cls._create_particle_material_template() diff --git a/tests/test_object_states.py b/tests/test_object_states.py index 0772b9e61..e599b5594 100644 --- a/tests/test_object_states.py +++ b/tests/test_object_states.py @@ -235,7 +235,7 @@ def test_aabb(): assert np.allclose(breakfast_table.states[AABB].get_value(), BoundingBoxAPI.compute_aabb(breakfast_table)) assert np.all((breakfast_table.states[AABB].get_value()[0] < pos1) & (pos1 < breakfast_table.states[AABB].get_value()[1])) - pp = dishtowel.root_link.compute_particle_positions() + pp = dishtowel.root_link.get_particle_positions() offset = dishtowel.root_link.cloth_system.particle_contact_offset assert np.allclose(dishtowel.states[AABB].get_value(), (pp.min(axis=0) - offset, pp.max(axis=0) + offset)) assert np.all((dishtowel.states[AABB].get_value()[0] < pos2) & (pos2 < dishtowel.states[AABB].get_value()[1])) @@ -993,7 +993,7 @@ def test_folded_unfolded(): assert not carpet.states[Folded].get_value() assert carpet.states[Unfolded].get_value() - pos = carpet.root_link.compute_particle_positions() + pos = carpet.root_link.get_particle_positions() x_min, x_max = np.min(pos, axis=0)[0], np.max(pos, axis=0)[0] x_extent = x_max - x_min # Get indices for the bottom 10 percent vertices in the x-axis From 82aac5dd677ccb34ab573b79ca1a69fa01660aa8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 13:21:37 -0800 Subject: [PATCH 03/28] Always enable flatcache --- omnigibson/simulator.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 9d6f9e39f..3dd33243e 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -205,9 +205,9 @@ def _set_physics_engine_settings(self): self._physics_context.enable_ccd(gm.ENABLE_CCD) if meets_minimum_isaac_version("2023.0.0"): - self._physics_context.enable_fabric(gm.ENABLE_FLATCACHE) + self._physics_context.enable_fabric(True) else: - self._physics_context.enable_flatcache(gm.ENABLE_FLATCACHE) + self._physics_context.enable_flatcache(True) # Enable GPU dynamics based on whether we need omni particles feature if gm.USE_GPU_DYNAMICS: @@ -490,8 +490,8 @@ def play(self): # We also need to suppress the following error when flat cache is used: # [omni.physx.plugin] Transformation change on non-root links is not supported. channels = ["omni.usd", "omni.physicsschema.plugin"] - if gm.ENABLE_FLATCACHE: - channels.append("omni.physx.plugin") + # TODO: Decide what to do with this. Suppressing all physx errors is way too aggressive. + # channels.append("omni.physx.plugin") with suppress_omni_log(channels=channels): super().play() From cdedf2985d15e528a80055b0c50bb292dc027d28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 13:24:22 -0800 Subject: [PATCH 04/28] Make flatcache non-optional --- docs/getting_started/building_blocks.md | 3 ++- omnigibson/simulator.py | 5 ++--- tests/test_envs.py | 2 +- tests/test_primitives.py | 2 +- tests/test_symbolic_primitives.py | 1 - tests/utils.py | 2 +- 6 files changed, 7 insertions(+), 8 deletions(-) diff --git a/docs/getting_started/building_blocks.md b/docs/getting_started/building_blocks.md index d64c57bd8..d12b7338e 100644 --- a/docs/getting_started/building_blocks.md +++ b/docs/getting_started/building_blocks.md @@ -13,7 +13,8 @@ icon: octicons/package-16 Macros enforce global behavior that is consistent within an individual python process but can differ between processes. This is useful because globally enabling all of **`OmniGibson`**'s features can cause unnecessary slowdowns, and so configuring the macros for your specific use case can optimize performance. - For example, Omniverse provides a so-called `flatcache` feature which provides significant performance boosts, but cannot be used when fluids or soft bodies are present. So, we ideally should always have `gm.USE_FLATCACHE=True` unless we have fluids or soft bodies in our environment. + For example, Omniverse provides the option to compute physics on the GPU which provides significant performance boosts and is required when fluids or soft bodies are present. + So, we ideally should always have `gm.USE_GPU_DYNAMICS=True`, but if we don't have enough GPU memory, we can set it to False unless we have fluids or soft bodies in our environment. `macros` define a globally available set of magic numbers or flags set throughout **`OmniGibson`**. These can either be directly set in `omnigibson.macros.py`, or can be programmatically modified at runtime via: diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 3dd33243e..854ef1263 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -538,9 +538,8 @@ def stop(self): if not self.is_stopped(): super().stop() - # If we're using flatcache, we also need to reset its API - if gm.ENABLE_FLATCACHE: - FlatcacheAPI.reset() + # Reset the FlatCache sync API + FlatcacheAPI.reset() # Run all callbacks for callback in self._callbacks_on_stop.values(): diff --git a/tests/test_envs.py b/tests/test_envs.py index f0b5cb1f4..ed98c97d7 100644 --- a/tests/test_envs.py +++ b/tests/test_envs.py @@ -28,7 +28,7 @@ def task_tester(task_type): # Make sure sim is stopped og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache + # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True diff --git a/tests/test_primitives.py b/tests/test_primitives.py index 7d1f775b0..5ae3ad1d9 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -89,7 +89,7 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg # Make sure sim is stopped og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache + # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = False diff --git a/tests/test_symbolic_primitives.py b/tests/test_symbolic_primitives.py index 77d845ded..2ff0e2268 100644 --- a/tests/test_symbolic_primitives.py +++ b/tests/test_symbolic_primitives.py @@ -4,7 +4,6 @@ from omnigibson.macros import gm gm.USE_GPU_DYNAMICS = True -gm.USE_FLATCACHE = True import omnigibson as og from omnigibson import object_states diff --git a/tests/utils.py b/tests/utils.py index f495221df..c157f3ecc 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -82,7 +82,7 @@ def assert_test_scene(): # Make sure sim is stopped og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache + # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True From b19ca49aee32b2d210cff297cddd73dbe7417d7e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 14:35:51 -0800 Subject: [PATCH 05/28] Remove unused stuff --- omnigibson/prims/cloth_prim.py | 2 +- omnigibson/simulator.py | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 639ca36c3..878212a5f 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -123,7 +123,7 @@ def _post_load(self): def _initialize(self): super()._initialize() - self._cloth_prim_view.initialize(og.sim._physics_sim_view) + self._cloth_prim_view.initialize(og.sim.physics_sim_view) # TODO (eric): hacky way to get cloth rendering to work (otherwise, there exist some rendering artifacts). self._prim.CreateAttribute("primvars:isVolume", VT.Bool, False).Set(True) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 854ef1263..d89947992 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -519,10 +519,6 @@ def play(self): self.step_physics() - # Initialize physics view and RigidContactAPI - self._physics_sim_view = omni.physics.tensors.create_simulation_view(self.backend) - self._physics_sim_view.set_subspace_roots("/") - # Additionally run non physics things self._non_physics_step() From 49c2ddfb6d3d3e5c4cfa2f246a50893d49f2592f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 15:29:46 -0800 Subject: [PATCH 06/28] Fix bad import --- omnigibson/prims/cloth_prim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 878212a5f..042982e52 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -10,7 +10,7 @@ from pxr import UsdPhysics, Gf, Vt, PhysxSchema from pxr.Sdf import ValueTypeNames as VT -from omni.isaac.core.prims.soft import ClothPrimView +from omni.isaac.core.prims import ClothPrimView from omnigibson.macros import create_module_macros, gm from omnigibson.prims.geom_prim import GeomPrim From be50d5251d5c1e2222398df733248804d9ef914f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 9 Nov 2023 16:52:30 -0800 Subject: [PATCH 07/28] Fix issue with premature render breaking things --- omnigibson/simulator.py | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index d89947992..7c21fc7f2 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -9,10 +9,10 @@ import omni import carb import omni.physics +from omni.isaac.core.physics_context import PhysicsContext from omni.isaac.core.simulation_context import SimulationContext from omni.isaac.core.utils.prims import get_prim_at_path -from omni.isaac.core.utils.stage import open_stage, create_new_stage -from omni.isaac.dynamic_control import _dynamic_control +from omni.isaac.core.utils.stage import open_stage, create_new_stage, get_current_stage, set_stage_units, set_stage_up_axis from omni.physx.bindings._physx import ContactEventType, SimulationEvent import omni.kit.loop._loop as omni_loop from pxr import Usd, PhysicsSchemaTools, UsdUtils @@ -401,11 +401,6 @@ def remove_object(self, obj): # Refresh all current rules TransitionRuleAPI.prune_active_rules() - def _reset_variables(self): - """ - Reset internal variables when a new stage is loaded - """ - def _non_physics_step(self): """ Complete any non-physics steps such as state updates. @@ -519,6 +514,10 @@ def play(self): self.step_physics() + # Initialize physics view and RigidContactAPI + self._physics_sim_view = omni.physics.tensors.create_simulation_view(self.backend) + self._physics_sim_view.set_subspace_roots("/") + # Additionally run non physics things self._non_physics_step() @@ -1066,17 +1065,26 @@ def _init_stage( backend="numpy", device=None, ): - # Run super first - super()._init_stage( + # This below code is copied verbatim from the super class except for the removal of a render + # call from the original + if get_current_stage() is None: + create_new_stage() + self.render() + set_stage_up_axis("z") + if stage_units_in_meters is not None: + set_stage_units(stage_units_in_meters=stage_units_in_meters) + # self.render() # This line causes crashes in Isaac Sim 2023.1.0. We don't need to render here. + self._physics_context = PhysicsContext( physics_dt=physics_dt, - rendering_dt=rendering_dt, - stage_units_in_meters=stage_units_in_meters, - physics_prim_path=physics_prim_path, + prim_path=physics_prim_path, sim_params=sim_params, set_defaults=set_defaults, backend=backend, device=device, ) + self.set_simulation_dt(physics_dt=physics_dt, rendering_dt=rendering_dt) + self.render() + # End of super class code # Update internal vars self._physx_interface = get_physx_interface() From b954fddcf0a235c877e8744588e6b7e788174f86 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:12:25 -0800 Subject: [PATCH 08/28] Default GPU dynamics to True for now --- omnigibson/macros.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/macros.py b/omnigibson/macros.py index 00c90ccff..166d9c25b 100644 --- a/omnigibson/macros.py +++ b/omnigibson/macros.py @@ -40,7 +40,7 @@ # Whether to use omni's GPU dynamics # This is necessary for certain features; e.g. particles (fluids / cloth) -gm.USE_GPU_DYNAMICS = False +gm.USE_GPU_DYNAMICS = True # Whether to use high-fidelity rendering (this includes, e.g., isosurfaces) gm.ENABLE_HQ_RENDERING = False From 722ec9d02a12ee6a4ef18e64e56dbe7a8624b368 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:12:42 -0800 Subject: [PATCH 09/28] Prepare cloth prim with new GPU approach --- omnigibson/prims/cloth_prim.py | 56 ++++++++++++++++++---------------- 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 042982e52..390da2624 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -23,6 +23,7 @@ import omnigibson as og import numpy as np +import torch # Create settings for this module @@ -96,34 +97,37 @@ def _post_load(self): # Load the cloth prim view self._cloth_prim_view = ClothPrimView(self._prim_path) - positions = self.get_particle_positions() - - # Sample mesh keypoints / keyvalues and sanity check the AABB of these subsampled points vs. the actual points - success = False - for i in range(10): - self._keypoint_idx, self._keyface_idx = sample_mesh_keypoints( - mesh_prim=self._prim, - n_keypoints=m.N_CLOTH_KEYPOINTS, - n_keyfaces=m.N_CLOTH_KEYFACES, - seed=i, - ) - - keypoint_positions = positions[self._keypoint_idx] - keypoint_aabb = keypoint_positions.min(axis=0), keypoint_positions.max(axis=0) - true_aabb = positions.min(axis=0), positions.max(axis=0) - overlap_vol = max(min(true_aabb[1][0], keypoint_aabb[1][0]) - max(true_aabb[0][0], keypoint_aabb[0][0]), 0) * \ - max(min(true_aabb[1][1], keypoint_aabb[1][1]) - max(true_aabb[0][1], keypoint_aabb[0][1]), 0) * \ - max(min(true_aabb[1][2], keypoint_aabb[1][2]) - max(true_aabb[0][2], keypoint_aabb[0][2]), 0) - true_vol = np.product(true_aabb[1] - true_aabb[0]) - if overlap_vol / true_vol > m.KEYPOINT_COVERAGE_THRESHOLD: - success = True - break - assert success, f"Did not adequately subsample keypoints for cloth {self.name}!" + # positions = self.get_particle_positions() + + # # Sample mesh keypoints / keyvalues and sanity check the AABB of these subsampled points vs. the actual points + # success = False + # for i in range(10): + # self._keypoint_idx, self._keyface_idx = sample_mesh_keypoints( + # mesh_prim=self._prim, + # n_keypoints=m.N_CLOTH_KEYPOINTS, + # n_keyfaces=m.N_CLOTH_KEYFACES, + # seed=i, + # ) + + # keypoint_positions = positions[self._keypoint_idx] + # keypoint_aabb = keypoint_positions.min(axis=0), keypoint_positions.max(axis=0) + # true_aabb = positions.min(axis=0), positions.max(axis=0) + # overlap_vol = max(min(true_aabb[1][0], keypoint_aabb[1][0]) - max(true_aabb[0][0], keypoint_aabb[0][0]), 0) * \ + # max(min(true_aabb[1][1], keypoint_aabb[1][1]) - max(true_aabb[0][1], keypoint_aabb[0][1]), 0) * \ + # max(min(true_aabb[1][2], keypoint_aabb[1][2]) - max(true_aabb[0][2], keypoint_aabb[0][2]), 0) + # true_vol = np.product(true_aabb[1] - true_aabb[0]) + # if overlap_vol / true_vol > m.KEYPOINT_COVERAGE_THRESHOLD: + # success = True + # break + # assert success, f"Did not adequately subsample keypoints for cloth {self.name}!" def _initialize(self): super()._initialize() - + assert og.sim._physics_sim_view._backend is not None, "Physics sim backend not initialized!" self._cloth_prim_view.initialize(og.sim.physics_sim_view) + assert self._n_particles <= self._cloth_prim_view.max_particles_per_cloth, \ + f"Got more particles than the maximum allowed for this cloth! Got {self._n_particles}, max is " \ + f"{self._cloth_prim_view.max_particles_per_cloth}!" # TODO (eric): hacky way to get cloth rendering to work (otherwise, there exist some rendering artifacts). self._prim.CreateAttribute("primvars:isVolume", VT.Bool, False).Set(True) @@ -164,7 +168,7 @@ def get_particle_positions(self, idxs=None): np.array: (N, 3) numpy array, where each of the N particles' positions are expressed in (x,y,z) cartesian coordinates relative to the world frame """ - all_particle_positions = self._cloth_prim_view.get_world_positions()[0, :, :] + all_particle_positions = self._cloth_prim_view.get_world_positions()[0, :, :].cpu().numpy() return all_particle_positions[:self._n_particles] if idxs is None else all_particle_positions[idxs] def set_particle_positions(self, positions, idxs=None): @@ -184,7 +188,7 @@ def set_particle_positions(self, positions, idxs=None): cur_pos = self._cloth_prim_view.get_world_positions() # Then apply the new positions at the appropriate indices - cur_pos[0, idxs] = positions + cur_pos[0, idxs] = torch.Tensor(positions, device=cur_pos.device) # Then set to that position self._cloth_prim_view.set_world_positions(cur_pos) From 25f6727f96b0c324a24929fb2df8c57a56018c48 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:12:57 -0800 Subject: [PATCH 10/28] Use GPU pipeline, dodge bug in stage callback --- omnigibson/simulator.py | 43 ++++++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 7c21fc7f2..071798924 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -75,7 +75,6 @@ def __init__( stage_units_in_meters=1.0, viewer_width=gm.DEFAULT_VIEWER_WIDTH, viewer_height=gm.DEFAULT_VIEWER_HEIGHT, - device=None, ): # Store vars needed for initialization self.gravity = gravity @@ -87,7 +86,8 @@ def __init__( physics_dt=physics_dt, rendering_dt=rendering_dt, stage_units_in_meters=stage_units_in_meters, - device=device, + device="cuda" if gm.USE_GPU_DYNAMICS else "cpu", + backend="torch" if gm.USE_GPU_DYNAMICS else "numpy" ) if self._world_initialized: @@ -143,22 +143,9 @@ def __init__( # and crashes self._set_physics_engine_settings() - def __new__( - cls, - gravity=9.81, - physics_dt=1.0 / 60.0, - rendering_dt=1.0 / 60.0, - stage_units_in_meters=1.0, - viewer_width=gm.DEFAULT_VIEWER_WIDTH, - viewer_height=gm.DEFAULT_VIEWER_HEIGHT, - device_idx=0, - ): - # Overwrite since we have different kwargs - if Simulator._instance is None: - Simulator._instance = object.__new__(cls) - else: - carb.log_info("Simulator is defined already, returning the previously defined one") - return Simulator._instance + def __new__(cls, *args, **kwargs): + print("Outer") + return SimulationContext.__new__(cls, *args, **kwargs) def _set_viewer_camera(self, prim_path="/World/viewer_camera", viewport_name="Viewport"): """ @@ -1079,8 +1066,8 @@ def _init_stage( prim_path=physics_prim_path, sim_params=sim_params, set_defaults=set_defaults, - backend=backend, - device=device, + backend=self.backend, + device=self.device ) self.set_simulation_dt(physics_dt=physics_dt, rendering_dt=rendering_dt) self.render() @@ -1185,3 +1172,19 @@ def _serialize(self, state): def _deserialize(self, state): # Default state is from the scene return self._scene.deserialize(state=state), self._scene.state_size + + def _stage_open_callback_fn(self, event): + # This below is copied from omni.isaac.core.simulation_context.SimulationContext + self._physics_callback_functions = dict() + self._physics_functions = dict() + self._stage_callback_functions = dict() + self._timeline_callback_functions = dict() + self._render_callback_functions = dict() + # This below part causes bugs and as such is removed. + # if SimulationContext._instance is not None: + # SimulationContext._instance.clear_instance() + # carb.log_warn( + # "A new stage was opened, World or Simulation Object are invalidated and you would need to initialize them again before using them." + # ) + self._stage_open_callback = None + return \ No newline at end of file From c40e949a72a25e8a1d38067ded90d54df16bfab2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:14:17 -0800 Subject: [PATCH 11/28] Fix some bugs in cloth remeshing pipeline --- omnigibson/systems/micro_particle_system.py | 12 ++++++------ omnigibson/utils/usd_utils.py | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index 54c83f190..f1086b4a3 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -1,3 +1,4 @@ +import uuid import omnigibson as og from omnigibson.macros import gm, create_module_macros from omnigibson.prims.prim_base import BasePrim @@ -1535,7 +1536,7 @@ def clothify_mesh_prim(cls, mesh_prim, remesh=True, particle_distance=None): # we convert our mesh into a trimesh mesh, then export it to a temp file, then load it into pymeshlab tm = mesh_prim_to_trimesh_mesh(mesh_prim=mesh_prim, include_normals=True, include_texcoord=True) # Tmp file written to: {tmp_dir}/{tmp_fname}/{tmp_fname}.obj - tmp_name = f"{mesh_prim.GetName()}_{datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')}" + tmp_name = str(uuid.uuid4()) tmp_dir = os.path.join(tempfile.gettempdir(), tmp_name) tmp_fpath = os.path.join(tmp_dir, f"{tmp_name}.obj") Path(tmp_dir).mkdir(parents=True, exist_ok=True) @@ -1564,15 +1565,14 @@ def clothify_mesh_prim(cls, mesh_prim, remesh=True, particle_distance=None): # Re-write data to @mesh_prim cm = ms.current_mesh() new_face_vertex_ids = cm.face_matrix().flatten() - new_texcoord = cm.wedge_tex_coord_matrix() - new_vertices = cm.vertex_matrix()[new_face_vertex_ids] - new_normals = cm.vertex_normal_matrix()[new_face_vertex_ids] - n_vertices = len(new_vertices) + new_texcoord = cm.vertex_tex_coord_matrix() + new_vertices = cm.vertex_matrix() + new_normals = cm.vertex_normal_matrix() n_faces = len(cm.face_matrix()) mesh_prim.GetAttribute("faceVertexCounts").Set(np.ones(n_faces, dtype=int) * 3) mesh_prim.GetAttribute("points").Set(Vt.Vec3fArray.FromNumpy(new_vertices)) - mesh_prim.GetAttribute("faceVertexIndices").Set(np.arange(n_vertices)) + mesh_prim.GetAttribute("faceVertexIndices").Set(new_face_vertex_ids) mesh_prim.GetAttribute("normals").Set(Vt.Vec3fArray.FromNumpy(new_normals)) mesh_prim.GetAttribute("primvars:st").Set(Vt.Vec2fArray.FromNumpy(new_texcoord)) diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index d06dd6541..b01a9e6ac 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -767,7 +767,7 @@ def sample_mesh_keypoints(mesh_prim, n_keypoints, n_keyfaces, seed=None): n_vertices = len(faces_flat) # Sample vertices - unique_vertices = np.unique(faces_flat, return_index=True)[1] + unique_vertices = np.unique(faces_flat) assert len(unique_vertices) == n_unique_vertices keypoint_idx = np.random.choice(unique_vertices, size=n_keypoints, replace=False) if \ n_unique_vertices > n_keypoints else unique_vertices From b1ef448236bd921de95e7a287019c5960b700040 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:49:21 -0800 Subject: [PATCH 12/28] More fixes to remeshing system --- omnigibson/prims/cloth_prim.py | 2 +- omnigibson/systems/micro_particle_system.py | 48 ++++++++++++++------- 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 390da2624..3b2043ae5 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -168,7 +168,7 @@ def get_particle_positions(self, idxs=None): np.array: (N, 3) numpy array, where each of the N particles' positions are expressed in (x,y,z) cartesian coordinates relative to the world frame """ - all_particle_positions = self._cloth_prim_view.get_world_positions()[0, :, :].cpu().numpy() + all_particle_positions = self._cloth_prim_view.get_world_positions(clone=False)[0, :, :].cpu().numpy() return all_particle_positions[:self._n_particles] if idxs is None else all_particle_positions[idxs] def set_particle_positions(self, positions, idxs=None): diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index f1086b4a3..9b5902146 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -44,6 +44,7 @@ # TODO: Tune these default values! # TODO (eric): figure out whether one offset can fit all +m.MAX_CLOTH_PARTICLES = 20000 m.CLOTH_PARTICLE_CONTACT_OFFSET = 0.0075 m.CLOTH_REMESHING_ERROR_THRESHOLD = 0.05 m.CLOTH_STRETCH_STIFFNESS = 10000.0 @@ -1541,31 +1542,46 @@ def clothify_mesh_prim(cls, mesh_prim, remesh=True, particle_distance=None): tmp_fpath = os.path.join(tmp_dir, f"{tmp_name}.obj") Path(tmp_dir).mkdir(parents=True, exist_ok=True) tm.export(tmp_fpath) - ms = pymeshlab.MeshSet() - ms.load_new_mesh(tmp_fpath) - # Re-mesh based on @particle_distance - distance chosen such that at rest particles should be just touching - # each other. The 1.5 magic number comes from the particle cloth demo from omni - # Note that this means that the particles will overlap with each other, since at dist = 2 * contact_offset - # the particles are just touching each other at rest + # Start with the default particle distance particle_distance = cls.particle_contact_offset * 2 / (1.5 * np.mean(mesh_prim.GetAttribute("xformOp:scale").Get())) \ if particle_distance is None else particle_distance - avg_edge_percentage_mismatch = 1.0 - iters = 0 - # Loop re-meshing until average edge percentage is within error threshold or we reach the max number of tries - while avg_edge_percentage_mismatch > m.CLOTH_REMESHING_ERROR_THRESHOLD: - ms.meshing_isotropic_explicit_remeshing(iterations=5, targetlen=pymeshlab.AbsoluteValue(particle_distance)) - avg_edge_percentage_mismatch = abs(1.0 - particle_distance / ms.get_geometric_measures()["avg_edge_length"]) - iters += 1 - if iters > 5: + + # Repetitively re-mesh at lower resolution until we have a mesh that has less than MAX_CLOTH_PARTICLES vertices + for _ in range(3): + ms = pymeshlab.MeshSet() + ms.load_new_mesh(tmp_fpath) + + # Re-mesh based on @particle_distance - distance chosen such that at rest particles should be just touching + # each other. The 1.5 magic number comes from the particle cloth demo from omni + # Note that this means that the particles will overlap with each other, since at dist = 2 * contact_offset + # the particles are just touching each other at rest + + avg_edge_percentage_mismatch = 1.0 + # Loop re-meshing until average edge percentage is within error threshold or we reach the max number of tries + for _ in range(5): + if avg_edge_percentage_mismatch <= m.CLOTH_REMESHING_ERROR_THRESHOLD: + break + + ms.meshing_isotropic_explicit_remeshing(iterations=5, targetlen=pymeshlab.AbsoluteValue(particle_distance)) + avg_edge_percentage_mismatch = abs(1.0 - particle_distance / ms.get_geometric_measures()["avg_edge_length"]) + else: # Terminate anyways, but don't fail log.warn("The generated cloth may not have evenly distributed particles.") + + # Check if we have too many vertices + cm = ms.current_mesh() + if cm.vertex_number() > m.MAX_CLOTH_PARTICLES: + # We have too many vertices, so we will re-mesh again + particle_distance *= 1.47 # halve the number of vertices + else: break + else: + raise ValueError("Could not remesh with less than MAX_CLOTH_PARTICLES vertices!") # Re-write data to @mesh_prim - cm = ms.current_mesh() new_face_vertex_ids = cm.face_matrix().flatten() - new_texcoord = cm.vertex_tex_coord_matrix() + new_texcoord = cm.wedge_tex_coord_matrix() new_vertices = cm.vertex_matrix() new_normals = cm.vertex_normal_matrix() n_faces = len(cm.face_matrix()) From e8b3d65c60f0d8529e86ff9306e516e752331a22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:49:45 -0800 Subject: [PATCH 13/28] Some debugging code --- omnigibson/examples/object_states/overlaid_demo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/examples/object_states/overlaid_demo.py b/omnigibson/examples/object_states/overlaid_demo.py index 28ec99e53..5dbe38aaa 100644 --- a/omnigibson/examples/object_states/overlaid_demo.py +++ b/omnigibson/examples/object_states/overlaid_demo.py @@ -67,7 +67,7 @@ def main(random_selection=False, headless=False, short_exec=False): print("\nTry dragging cloth around with CTRL + Left-Click to see the Overlaid state change:\n") while steps != max_steps: - print(f"Overlaid {carpet.states[Overlaid].get_value(breakfast_table)} ", end="\r") + print(f"Overlaid {carpet.states[Overlaid].get_value(breakfast_table)}. Avg keypoint pos {np.mean(carpet.root_link.keypoint_particle_positions, axis=0)} ", end="\r") env.step(np.array([])) # Shut down env at the end From 641111acd9f37d55ca6efea234c0b9fc32ef2a5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 13 Nov 2023 11:09:46 -0800 Subject: [PATCH 14/28] Update cloth_prim.py --- omnigibson/prims/cloth_prim.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 3b2043ae5..43fc858d6 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -123,11 +123,9 @@ def _post_load(self): def _initialize(self): super()._initialize() - assert og.sim._physics_sim_view._backend is not None, "Physics sim backend not initialized!" - self._cloth_prim_view.initialize(og.sim.physics_sim_view) - assert self._n_particles <= self._cloth_prim_view.max_particles_per_cloth, \ - f"Got more particles than the maximum allowed for this cloth! Got {self._n_particles}, max is " \ - f"{self._cloth_prim_view.max_particles_per_cloth}!" + + # Update the handles so that we can access particles + self.update_handles() # TODO (eric): hacky way to get cloth rendering to work (otherwise, there exist some rendering artifacts). self._prim.CreateAttribute("primvars:isVolume", VT.Bool, False).Set(True) @@ -334,8 +332,11 @@ def report_hit(hit): return contacts def update_handles(self): - # no handles to update - pass + assert og.sim._physics_sim_view._backend is not None, "Physics sim backend not initialized!" + self._cloth_prim_view.initialize(og.sim.physics_sim_view) + assert self._n_particles <= self._cloth_prim_view.max_particles_per_cloth, \ + f"Got more particles than the maximum allowed for this cloth! Got {self._n_particles}, max is " \ + f"{self._cloth_prim_view.max_particles_per_cloth}!" @property def volume(self): From 23385825abedb1c382d966fbb55cf491830f7e9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 13 Nov 2023 14:26:52 -0800 Subject: [PATCH 15/28] No more enable flatcache flag --- omnigibson/utils/usd_utils.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index b01a9e6ac..a19567ac0 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -573,9 +573,6 @@ def sync_raw_object_transforms_in_usd(cls, prim): prim (EntityPrim): prim whose owned links and joints should have their raw local states updated to match the "true" values found from the dynamic control interface """ - # Make sure flatcache is enabled -- this should NEVER be called otherwise!! - assert gm.ENABLE_FLATCACHE, "Syncing raw object transforms should only occur if flatcache is being used!" - # We're somewhat abusing low-level dynamic control - physx - usd integration, but we (supposedly) know # what we're doing so we suppress logging so we don't see any error messages :D with suppress_omni_log(["omni.physx.plugin"]): @@ -617,9 +614,6 @@ def reset_raw_object_transforms_in_usd(cls, prim): Args: prim (EntityPrim): prim whose owned links and joints should have their local values reset to be zero """ - # Make sure flatcache is enabled -- this should NEVER be called otherwise!! - assert gm.ENABLE_FLATCACHE, "Resetting raw object transforms should only occur if flatcache is being used!" - # We're somewhat abusing low-level dynamic control - physx - usd integration, but we (supposedly) know # what we're doing so we suppress logging so we don't see any error messages :D with suppress_omni_log(["omni.physx.plugin"]): From d44e554c9affa7a926cc9ce70875ac453481498c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 22 Dec 2023 12:33:38 -0800 Subject: [PATCH 16/28] Make all the tensors into numpy arrays --- omnigibson/prims/cloth_prim.py | 2 +- omnigibson/prims/entity_prim.py | 30 +++++++++++++-------- omnigibson/prims/joint_prim.py | 26 +++++++++--------- omnigibson/prims/rigid_prim.py | 21 +++++++++------ omnigibson/systems/macro_particle_system.py | 4 +-- 5 files changed, 48 insertions(+), 35 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 43fc858d6..3455344b2 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -183,7 +183,7 @@ def set_particle_positions(self, positions, idxs=None): f"Got mismatch in particle setting size: {len(positions)}, vs. number of expected particles {n_expected}!" # First, get the particle positions. - cur_pos = self._cloth_prim_view.get_world_positions() + cur_pos = self._cloth_prim_view.get_world_positions().cpu().numpy() # Then apply the new positions at the appropriate indices cur_pos[0, idxs] = torch.Tensor(positions, device=cur_pos.device) diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index c3f5bac45..18954002f 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -796,7 +796,7 @@ def get_joint_positions(self, normalized=False): # Run sanity checks -- make sure we are articulated assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!" - joint_positions = self._articulation_view.get_joint_positions().reshape(self.n_dof) + joint_positions = self._articulation_view.get_joint_positions().cpu().numpy().reshape(self.n_dof) # Possibly normalize values when returning return self._normalize_positions(positions=joint_positions) if normalized else joint_positions @@ -814,7 +814,7 @@ def get_joint_velocities(self, normalized=False): # Run sanity checks -- make sure we are articulated assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!" - joint_velocities = self._articulation_view.get_joint_velocities().reshape(self.n_dof) + joint_velocities = self._articulation_view.get_joint_velocities().cpu().numpy().reshape(self.n_dof) # Possibly normalize values when returning return self._normalize_velocities(velocities=joint_velocities) if normalized else joint_velocities @@ -832,7 +832,7 @@ def get_joint_efforts(self, normalized=False): # Run sanity checks -- make sure we are articulated assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!" - joint_efforts = self._articulation_view.get_applied_joint_efforts().reshape(self.n_dof) + joint_efforts = self._articulation_view.get_applied_joint_efforts().cpu().numpy().reshape(self.n_dof) # Possibly normalize values when returning return self._normalize_efforts(efforts=joint_efforts) if normalized else joint_efforts @@ -889,8 +889,12 @@ def get_position_orientation(self): if self._articulation_view is None: return self.root_link.get_position_orientation() - positions, orientations = self._articulation_view.get_world_poses() - return positions[0], orientations[0][[1, 2, 3, 0]] + pos, ori = self._articulation_view.get_world_poses() + pos = pos.cpu().numpy() + ori = ori.cpu().numpy() + 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][[1, 2, 3, 0]] def set_local_pose(self, position=None, orientation=None): # Delegate to RigidPrim if we are not articulated @@ -909,8 +913,12 @@ def get_local_pose(self): if self._articulation_view is None: return self.root_link.get_local_pose() - positions, orientations = self._articulation_view.get_local_poses() - return positions[0], orientations[0][[1, 2, 3, 0]] + pos, ori = self._articulation_view.get_local_poses() + pos = pos.cpu().numpy() + ori = ori.cpu().numpy() + 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][[1, 2, 3, 0]] # TODO: Is the omni joint damping (used for driving motors) same as dissipative joint damping (what we had in pb)? @property @@ -1184,7 +1192,7 @@ def get_coriolis_and_centrifugal_forces(self): n-array: (N,) shaped per-DOF coriolis and centrifugal forces experienced by the entity, if articulated """ assert self.articulated, "Cannot get coriolis and centrifugal forces for non-articulated entity!" - return self._articulation_view.get_coriolis_and_centrifugal_forces().reshape(self.n_dof) + return self._articulation_view.get_coriolis_and_centrifugal_forces().cpu().numpy().reshape(self.n_dof) def get_generalized_gravity_forces(self): """ @@ -1192,7 +1200,7 @@ def get_generalized_gravity_forces(self): n-array: (N, N) shaped per-DOF gravity forces, if articulated """ assert self.articulated, "Cannot get generalized gravity forces for non-articulated entity!" - return self._articulation_view.get_generalized_gravity_forces().reshape(self.n_dof) + return self._articulation_view.get_generalized_gravity_forces().cpu().numpy().reshape(self.n_dof) def get_mass_matrix(self): """ @@ -1200,7 +1208,7 @@ def get_mass_matrix(self): n-array: (N, N) shaped per-DOF mass matrix, if articulated """ assert self.articulated, "Cannot get mass matrix for non-articulated entity!" - return self._articulation_view.get_mass_matrices().reshape(self.n_dof, self.n_dof) + return self._articulation_view.get_mass_matrices().cpu().numpy().reshape(self.n_dof, self.n_dof) def get_jacobian(self): """ @@ -1210,7 +1218,7 @@ def get_jacobian(self): (i.e.: there is an additional "floating" joint tying the robot to the world frame) """ assert self.articulated, "Cannot get jacobian for non-articulated entity!" - return self._articulation_view.get_jacobians().squeeze(axis=0) + return self._articulation_view.get_jacobians().cpu().numpy().squeeze(axis=0) def get_relative_jacobian(self): """ diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py index 348597b3a..f6b93ef82 100644 --- a/omnigibson/prims/joint_prim.py +++ b/omnigibson/prims/joint_prim.py @@ -148,7 +148,7 @@ def _initialize(self): if self.articulated: control_types = [] stiffnesses, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices) - for i, (kp, kd) in enumerate(zip(stiffnesses[0], dampings[0])): + for i, (kp, kd) in enumerate(zip(stiffnesses[0].cpu().numpy(), dampings[0].cpu().numpy())): # Infer control type based on whether kp and kd are 0 or not, as well as whether this joint is driven or not # TODO: Maybe assert mutual exclusiveness here? if not self._driven: @@ -335,7 +335,7 @@ def max_velocity(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_vel = self._articulation_view.get_max_velocities(joint_indices=self.dof_indices)[0][0] + raw_vel = self._articulation_view.get_max_velocities(joint_indices=self.dof_indices).cpu().numpy()[0][0] default_max_vel = m.DEFAULT_MAX_REVOLUTE_VEL if self.joint_type == JointType.JOINT_REVOLUTE else m.DEFAULT_MAX_PRISMATIC_VEL return default_max_vel if raw_vel is None or np.abs(raw_vel) > m.INF_VEL_THRESHOLD else raw_vel @@ -362,7 +362,7 @@ def max_effort(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_effort = self._articulation_view.get_max_efforts(joint_indices=self.dof_indices)[0][0] + raw_effort = self._articulation_view.get_max_efforts(joint_indices=self.dof_indices).cpu().numpy()[0][0] return m.DEFAULT_MAX_EFFORT if raw_effort is None or np.abs(raw_effort) > m.INF_EFFORT_THRESHOLD else raw_effort @max_effort.setter @@ -388,7 +388,7 @@ def stiffness(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" stiffnesses, _ = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0] - return stiffnesses[0][0] + return stiffnesses.cpu().numpy()[0][0] @stiffness.setter def stiffness(self, stiffness): @@ -413,7 +413,7 @@ def damping(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" _, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0] - return dampings[0][0] + return dampings.cpu().numpy()[0][0] @damping.setter def damping(self, damping): @@ -435,7 +435,7 @@ def friction(self): Returns: float: friction for this joint """ - return self._articulation_view.get_friction_coefficients(joint_indices=self.dof_indices)[0][0] + return self._articulation_view.get_friction_coefficients(joint_indices=self.dof_indices).cpu().numpy()[0][0] @friction.setter def friction(self, friction): @@ -459,7 +459,7 @@ def lower_limit(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).flatten() + raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).cpu().numpy().flatten() return -m.DEFAULT_MAX_POS \ if raw_pos_lower is None or raw_pos_lower == raw_pos_upper or np.abs(raw_pos_lower) > m.INF_POS_THRESHOLD \ else raw_pos_lower @@ -487,7 +487,7 @@ def upper_limit(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).flatten() + raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).cpu().numpy().flatten() return m.DEFAULT_MAX_POS \ if raw_pos_upper is None or raw_pos_lower == raw_pos_upper or np.abs(raw_pos_upper) > m.INF_POS_THRESHOLD \ else raw_pos_upper @@ -512,7 +512,7 @@ def has_limit(self): """ # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" - return np.all(np.abs(self._articulation_view.get_joint_limits(joint_indices=self.dof_indices)) < m.INF_POS_THRESHOLD) + return np.all(np.abs(self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).cpu().numpy()) < m.INF_POS_THRESHOLD) @property def axis(self): @@ -598,9 +598,9 @@ def get_state(self, normalized=False): assert self.articulated, "Can only get state for articulated joints!" # Grab raw states - pos = self._articulation_view.get_joint_positions(joint_indices=self.dof_indices)[0] - vel = self._articulation_view.get_joint_velocities(joint_indices=self.dof_indices)[0] - effort = self._articulation_view.get_applied_joint_efforts(joint_indices=self.dof_indices)[0] + pos = self._articulation_view.get_joint_positions(joint_indices=self.dof_indices).cpu().numpy()[0] + vel = self._articulation_view.get_joint_velocities(joint_indices=self.dof_indices).cpu().numpy()[0] + effort = self._articulation_view.get_applied_joint_efforts(joint_indices=self.dof_indices).cpu().numpy()[0] # Potentially normalize if requested if normalized: @@ -624,7 +624,7 @@ def get_target(self, normalized=False): assert self.articulated, "Can only get targets for articulated joints!" # Grab raw states - targets = self._articulation_view.get_applied_actions() + targets = self._articulation_view.get_applied_actions().cpu().numpy() pos = targets.joint_positions[0][self.dof_indices] vel = targets.joint_velocities[0][self.dof_indices] diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 0c8d3f0b5..948e8cd09 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -249,7 +249,7 @@ def get_linear_velocity(self): Returns: np.ndarray: current linear velocity of the the rigid prim. Shape (3,). """ - return self._rigid_prim_view.get_linear_velocities()[0] + return self._rigid_prim_view.get_linear_velocities().cpu().numpy()[0] def set_angular_velocity(self, velocity): """ @@ -265,7 +265,7 @@ def get_angular_velocity(self): Returns: np.ndarray: current angular velocity of the the rigid prim. Shape (3,). """ - return self._rigid_prim_view.get_angular_velocities()[0] + return self._rigid_prim_view.get_angular_velocities().cpu().numpy()[0] def set_position_orientation(self, position=None, orientation=None): if position is not None: @@ -279,7 +279,8 @@ def set_position_orientation(self, position=None, orientation=None): def get_position_orientation(self): pos, ori = self._rigid_prim_view.get_world_poses() - + pos = pos.cpu().numpy() + ori = ori.cpu().numpy() 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][[1, 2, 3, 0]] @@ -293,8 +294,12 @@ def set_local_pose(self, position=None, orientation=None): BoundingBoxAPI.clear() def get_local_pose(self): - positions, orientations = self._rigid_prim_view.get_local_poses() - return positions[0], orientations[0][[1, 2, 3, 0]] + pos, ori = self._rigid_prim_view.get_local_poses() + pos = pos.cpu().numpy() + ori = ori.cpu().numpy() + 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][[1, 2, 3, 0]] @property def _rigid_prim_view(self): @@ -399,7 +404,7 @@ def mass(self): Returns: float: mass of the rigid body in kg. """ - mass = self._rigid_prim_view.get_masses()[0] + mass = self._rigid_prim_view.get_masses().cpu().numpy()[0] # Fallback to analytical computation of volume * density if mass == 0: @@ -421,7 +426,7 @@ def density(self): Returns: float: density of the rigid body in kg / m^3. """ - raw_usd_mass = self._rigid_prim_view.get_masses()[0] + raw_usd_mass = self._rigid_prim_view.get_masses().cpu().numpy()[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 # Otherwise, we try to directly grab the raw usd density value, and if that value @@ -429,7 +434,7 @@ def density(self): if raw_usd_mass != 0: density = raw_usd_mass / self.volume else: - density = self._rigid_prim_view.get_densities()[0] + density = self._rigid_prim_view.get_densities().cpu().numpy()[0] if density == 0: density = 1000.0 diff --git a/omnigibson/systems/macro_particle_system.py b/omnigibson/systems/macro_particle_system.py index 85f6c6105..dda987318 100644 --- a/omnigibson/systems/macro_particle_system.py +++ b/omnigibson/systems/macro_particle_system.py @@ -1179,7 +1179,7 @@ def add_particle(cls, prim_path, scale, idn=None): def get_particles_position_orientation(cls): # Note: This gets the center of the sphere approximation of the particles, NOT the actual particle frames! if cls.n_particles > 0: - tfs = cls.particles_view.get_transforms() + tfs = cls.particles_view.get_transforms().cpu().numpy() pos, ori = tfs[:, :3], tfs[:, 3:] pos = pos + T.quat2mat(ori) @ cls._particle_offset else: @@ -1242,7 +1242,7 @@ def get_particles_velocities(cls): - (n, 3)-array: per-particle (ax, ay, az) angular velocities in the world frame """ if cls.n_particles > 0: - vels = cls.particles_view.get_velocities() + vels = cls.particles_view.get_velocities().cpu().numpy() lin_vel, ang_vel = vels[:, :3], vels[:, 3:] else: lin_vel, ang_vel = np.array([]).reshape(0, 3), np.array([]).reshape(0, 3) From c208bc4bc0955de91fae55b35e8abd58f6ee552a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 22 Dec 2023 12:33:46 -0800 Subject: [PATCH 17/28] Revert "Make all the tensors into numpy arrays" This reverts commit d44e554c9affa7a926cc9ce70875ac453481498c. --- omnigibson/prims/cloth_prim.py | 2 +- omnigibson/prims/entity_prim.py | 30 ++++++++------------- omnigibson/prims/joint_prim.py | 26 +++++++++--------- omnigibson/prims/rigid_prim.py | 21 ++++++--------- omnigibson/systems/macro_particle_system.py | 4 +-- 5 files changed, 35 insertions(+), 48 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 3455344b2..43fc858d6 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -183,7 +183,7 @@ def set_particle_positions(self, positions, idxs=None): f"Got mismatch in particle setting size: {len(positions)}, vs. number of expected particles {n_expected}!" # First, get the particle positions. - cur_pos = self._cloth_prim_view.get_world_positions().cpu().numpy() + cur_pos = self._cloth_prim_view.get_world_positions() # Then apply the new positions at the appropriate indices cur_pos[0, idxs] = torch.Tensor(positions, device=cur_pos.device) diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index 18954002f..c3f5bac45 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -796,7 +796,7 @@ def get_joint_positions(self, normalized=False): # Run sanity checks -- make sure we are articulated assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!" - joint_positions = self._articulation_view.get_joint_positions().cpu().numpy().reshape(self.n_dof) + joint_positions = self._articulation_view.get_joint_positions().reshape(self.n_dof) # Possibly normalize values when returning return self._normalize_positions(positions=joint_positions) if normalized else joint_positions @@ -814,7 +814,7 @@ def get_joint_velocities(self, normalized=False): # Run sanity checks -- make sure we are articulated assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!" - joint_velocities = self._articulation_view.get_joint_velocities().cpu().numpy().reshape(self.n_dof) + joint_velocities = self._articulation_view.get_joint_velocities().reshape(self.n_dof) # Possibly normalize values when returning return self._normalize_velocities(velocities=joint_velocities) if normalized else joint_velocities @@ -832,7 +832,7 @@ def get_joint_efforts(self, normalized=False): # Run sanity checks -- make sure we are articulated assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!" - joint_efforts = self._articulation_view.get_applied_joint_efforts().cpu().numpy().reshape(self.n_dof) + joint_efforts = self._articulation_view.get_applied_joint_efforts().reshape(self.n_dof) # Possibly normalize values when returning return self._normalize_efforts(efforts=joint_efforts) if normalized else joint_efforts @@ -889,12 +889,8 @@ def get_position_orientation(self): if self._articulation_view is None: return self.root_link.get_position_orientation() - pos, ori = self._articulation_view.get_world_poses() - pos = pos.cpu().numpy() - ori = ori.cpu().numpy() - 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][[1, 2, 3, 0]] + positions, orientations = self._articulation_view.get_world_poses() + return positions[0], orientations[0][[1, 2, 3, 0]] def set_local_pose(self, position=None, orientation=None): # Delegate to RigidPrim if we are not articulated @@ -913,12 +909,8 @@ def get_local_pose(self): if self._articulation_view is None: return self.root_link.get_local_pose() - pos, ori = self._articulation_view.get_local_poses() - pos = pos.cpu().numpy() - ori = ori.cpu().numpy() - 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][[1, 2, 3, 0]] + positions, orientations = self._articulation_view.get_local_poses() + return positions[0], orientations[0][[1, 2, 3, 0]] # TODO: Is the omni joint damping (used for driving motors) same as dissipative joint damping (what we had in pb)? @property @@ -1192,7 +1184,7 @@ def get_coriolis_and_centrifugal_forces(self): n-array: (N,) shaped per-DOF coriolis and centrifugal forces experienced by the entity, if articulated """ assert self.articulated, "Cannot get coriolis and centrifugal forces for non-articulated entity!" - return self._articulation_view.get_coriolis_and_centrifugal_forces().cpu().numpy().reshape(self.n_dof) + return self._articulation_view.get_coriolis_and_centrifugal_forces().reshape(self.n_dof) def get_generalized_gravity_forces(self): """ @@ -1200,7 +1192,7 @@ def get_generalized_gravity_forces(self): n-array: (N, N) shaped per-DOF gravity forces, if articulated """ assert self.articulated, "Cannot get generalized gravity forces for non-articulated entity!" - return self._articulation_view.get_generalized_gravity_forces().cpu().numpy().reshape(self.n_dof) + return self._articulation_view.get_generalized_gravity_forces().reshape(self.n_dof) def get_mass_matrix(self): """ @@ -1208,7 +1200,7 @@ def get_mass_matrix(self): n-array: (N, N) shaped per-DOF mass matrix, if articulated """ assert self.articulated, "Cannot get mass matrix for non-articulated entity!" - return self._articulation_view.get_mass_matrices().cpu().numpy().reshape(self.n_dof, self.n_dof) + return self._articulation_view.get_mass_matrices().reshape(self.n_dof, self.n_dof) def get_jacobian(self): """ @@ -1218,7 +1210,7 @@ def get_jacobian(self): (i.e.: there is an additional "floating" joint tying the robot to the world frame) """ assert self.articulated, "Cannot get jacobian for non-articulated entity!" - return self._articulation_view.get_jacobians().cpu().numpy().squeeze(axis=0) + return self._articulation_view.get_jacobians().squeeze(axis=0) def get_relative_jacobian(self): """ diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py index f6b93ef82..348597b3a 100644 --- a/omnigibson/prims/joint_prim.py +++ b/omnigibson/prims/joint_prim.py @@ -148,7 +148,7 @@ def _initialize(self): if self.articulated: control_types = [] stiffnesses, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices) - for i, (kp, kd) in enumerate(zip(stiffnesses[0].cpu().numpy(), dampings[0].cpu().numpy())): + for i, (kp, kd) in enumerate(zip(stiffnesses[0], dampings[0])): # Infer control type based on whether kp and kd are 0 or not, as well as whether this joint is driven or not # TODO: Maybe assert mutual exclusiveness here? if not self._driven: @@ -335,7 +335,7 @@ def max_velocity(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_vel = self._articulation_view.get_max_velocities(joint_indices=self.dof_indices).cpu().numpy()[0][0] + raw_vel = self._articulation_view.get_max_velocities(joint_indices=self.dof_indices)[0][0] default_max_vel = m.DEFAULT_MAX_REVOLUTE_VEL if self.joint_type == JointType.JOINT_REVOLUTE else m.DEFAULT_MAX_PRISMATIC_VEL return default_max_vel if raw_vel is None or np.abs(raw_vel) > m.INF_VEL_THRESHOLD else raw_vel @@ -362,7 +362,7 @@ def max_effort(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_effort = self._articulation_view.get_max_efforts(joint_indices=self.dof_indices).cpu().numpy()[0][0] + raw_effort = self._articulation_view.get_max_efforts(joint_indices=self.dof_indices)[0][0] return m.DEFAULT_MAX_EFFORT if raw_effort is None or np.abs(raw_effort) > m.INF_EFFORT_THRESHOLD else raw_effort @max_effort.setter @@ -388,7 +388,7 @@ def stiffness(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" stiffnesses, _ = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0] - return stiffnesses.cpu().numpy()[0][0] + return stiffnesses[0][0] @stiffness.setter def stiffness(self, stiffness): @@ -413,7 +413,7 @@ def damping(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" _, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0] - return dampings.cpu().numpy()[0][0] + return dampings[0][0] @damping.setter def damping(self, damping): @@ -435,7 +435,7 @@ def friction(self): Returns: float: friction for this joint """ - return self._articulation_view.get_friction_coefficients(joint_indices=self.dof_indices).cpu().numpy()[0][0] + return self._articulation_view.get_friction_coefficients(joint_indices=self.dof_indices)[0][0] @friction.setter def friction(self, friction): @@ -459,7 +459,7 @@ def lower_limit(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).cpu().numpy().flatten() + raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).flatten() return -m.DEFAULT_MAX_POS \ if raw_pos_lower is None or raw_pos_lower == raw_pos_upper or np.abs(raw_pos_lower) > m.INF_POS_THRESHOLD \ else raw_pos_lower @@ -487,7 +487,7 @@ def upper_limit(self): # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" # We either return the raw value or a default value if there is no max specified - raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).cpu().numpy().flatten() + raw_pos_lower, raw_pos_upper = self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).flatten() return m.DEFAULT_MAX_POS \ if raw_pos_upper is None or raw_pos_lower == raw_pos_upper or np.abs(raw_pos_upper) > m.INF_POS_THRESHOLD \ else raw_pos_upper @@ -512,7 +512,7 @@ def has_limit(self): """ # Only support revolute and prismatic joints for now assert self.is_single_dof, "Joint properties only supported for a single DOF currently!" - return np.all(np.abs(self._articulation_view.get_joint_limits(joint_indices=self.dof_indices).cpu().numpy()) < m.INF_POS_THRESHOLD) + return np.all(np.abs(self._articulation_view.get_joint_limits(joint_indices=self.dof_indices)) < m.INF_POS_THRESHOLD) @property def axis(self): @@ -598,9 +598,9 @@ def get_state(self, normalized=False): assert self.articulated, "Can only get state for articulated joints!" # Grab raw states - pos = self._articulation_view.get_joint_positions(joint_indices=self.dof_indices).cpu().numpy()[0] - vel = self._articulation_view.get_joint_velocities(joint_indices=self.dof_indices).cpu().numpy()[0] - effort = self._articulation_view.get_applied_joint_efforts(joint_indices=self.dof_indices).cpu().numpy()[0] + pos = self._articulation_view.get_joint_positions(joint_indices=self.dof_indices)[0] + vel = self._articulation_view.get_joint_velocities(joint_indices=self.dof_indices)[0] + effort = self._articulation_view.get_applied_joint_efforts(joint_indices=self.dof_indices)[0] # Potentially normalize if requested if normalized: @@ -624,7 +624,7 @@ def get_target(self, normalized=False): assert self.articulated, "Can only get targets for articulated joints!" # Grab raw states - targets = self._articulation_view.get_applied_actions().cpu().numpy() + targets = self._articulation_view.get_applied_actions() pos = targets.joint_positions[0][self.dof_indices] vel = targets.joint_velocities[0][self.dof_indices] diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 948e8cd09..0c8d3f0b5 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -249,7 +249,7 @@ def get_linear_velocity(self): Returns: np.ndarray: current linear velocity of the the rigid prim. Shape (3,). """ - return self._rigid_prim_view.get_linear_velocities().cpu().numpy()[0] + return self._rigid_prim_view.get_linear_velocities()[0] def set_angular_velocity(self, velocity): """ @@ -265,7 +265,7 @@ def get_angular_velocity(self): Returns: np.ndarray: current angular velocity of the the rigid prim. Shape (3,). """ - return self._rigid_prim_view.get_angular_velocities().cpu().numpy()[0] + return self._rigid_prim_view.get_angular_velocities()[0] def set_position_orientation(self, position=None, orientation=None): if position is not None: @@ -279,8 +279,7 @@ def set_position_orientation(self, position=None, orientation=None): def get_position_orientation(self): pos, ori = self._rigid_prim_view.get_world_poses() - pos = pos.cpu().numpy() - ori = ori.cpu().numpy() + 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][[1, 2, 3, 0]] @@ -294,12 +293,8 @@ def set_local_pose(self, position=None, orientation=None): BoundingBoxAPI.clear() def get_local_pose(self): - pos, ori = self._rigid_prim_view.get_local_poses() - pos = pos.cpu().numpy() - ori = ori.cpu().numpy() - 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][[1, 2, 3, 0]] + positions, orientations = self._rigid_prim_view.get_local_poses() + return positions[0], orientations[0][[1, 2, 3, 0]] @property def _rigid_prim_view(self): @@ -404,7 +399,7 @@ def mass(self): Returns: float: mass of the rigid body in kg. """ - mass = self._rigid_prim_view.get_masses().cpu().numpy()[0] + mass = self._rigid_prim_view.get_masses()[0] # Fallback to analytical computation of volume * density if mass == 0: @@ -426,7 +421,7 @@ def density(self): Returns: float: density of the rigid body in kg / m^3. """ - raw_usd_mass = self._rigid_prim_view.get_masses().cpu().numpy()[0] + 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 # Otherwise, we try to directly grab the raw usd density value, and if that value @@ -434,7 +429,7 @@ def density(self): if raw_usd_mass != 0: density = raw_usd_mass / self.volume else: - density = self._rigid_prim_view.get_densities().cpu().numpy()[0] + density = self._rigid_prim_view.get_densities()[0] if density == 0: density = 1000.0 diff --git a/omnigibson/systems/macro_particle_system.py b/omnigibson/systems/macro_particle_system.py index dda987318..85f6c6105 100644 --- a/omnigibson/systems/macro_particle_system.py +++ b/omnigibson/systems/macro_particle_system.py @@ -1179,7 +1179,7 @@ def add_particle(cls, prim_path, scale, idn=None): def get_particles_position_orientation(cls): # Note: This gets the center of the sphere approximation of the particles, NOT the actual particle frames! if cls.n_particles > 0: - tfs = cls.particles_view.get_transforms().cpu().numpy() + tfs = cls.particles_view.get_transforms() pos, ori = tfs[:, :3], tfs[:, 3:] pos = pos + T.quat2mat(ori) @ cls._particle_offset else: @@ -1242,7 +1242,7 @@ def get_particles_velocities(cls): - (n, 3)-array: per-particle (ax, ay, az) angular velocities in the world frame """ if cls.n_particles > 0: - vels = cls.particles_view.get_velocities().cpu().numpy() + vels = cls.particles_view.get_velocities() lin_vel, ang_vel = vels[:, :3], vels[:, 3:] else: lin_vel, ang_vel = np.array([]).reshape(0, 3), np.array([]).reshape(0, 3) From c087a3c829040e82380d9168c6fb50590ae4b20a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 22 Dec 2023 20:52:58 -0800 Subject: [PATCH 18/28] Get GPU pipeline working again --- omnigibson/envs/env_base.py | 2 - omnigibson/prims/cloth_prim.py | 7 ++-- omnigibson/prims/entity_prim.py | 4 +- omnigibson/prims/joint_prim.py | 4 +- omnigibson/prims/rigid_prim.py | 4 +- omnigibson/robots/fetch.py | 2 +- omnigibson/robots/tiago.py | 2 +- omnigibson/simulator.py | 15 +------- omnigibson/utils/deprecated_utils.py | 56 ++++++++++++++++++++++++++++ 9 files changed, 68 insertions(+), 28 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 9c7ab2159..a005790f8 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -60,7 +60,6 @@ def __init__(self, configs): self._flatten_obs_space = self.env_config["flatten_obs_space"] self.physics_timestep = self.env_config["physics_timestep"] self.action_timestep = self.env_config["action_timestep"] - self.device = self.env_config["device"] self._initial_pos_z_offset = self.env_config["initial_pos_z_offset"] # how high to offset object placement to account for one action step of dropping # Create the scene graph builder @@ -200,7 +199,6 @@ def _load_scene(self): # Set the rendering settings og.sim.viewer_width = self.render_config["viewer_width"] og.sim.viewer_height = self.render_config["viewer_height"] - og.sim.device = self.device assert og.sim.is_stopped(), "Simulator must be stopped after loading scene!" diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 43fc858d6..85bc18eaa 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -10,11 +10,10 @@ from pxr import UsdPhysics, Gf, Vt, PhysxSchema from pxr.Sdf import ValueTypeNames as VT -from omni.isaac.core.prims import ClothPrimView - from omnigibson.macros import create_module_macros, gm from omnigibson.prims.geom_prim import GeomPrim from omnigibson.systems import get_system +from omnigibson.utils.deprecated_utils import RetensorClothPrimView import omnigibson.utils.transform_utils as T from omnigibson.utils.sim_utils import CsRawData from omnigibson.utils.usd_utils import array_to_vtarray, mesh_prim_to_trimesh_mesh, sample_mesh_keypoints @@ -95,7 +94,7 @@ def _post_load(self): self._n_particles = len(self._prim.GetAttribute("points").Get()) # Load the cloth prim view - self._cloth_prim_view = ClothPrimView(self._prim_path) + self._cloth_prim_view = RetensorClothPrimView(self._prim_path) # positions = self.get_particle_positions() @@ -166,7 +165,7 @@ def get_particle_positions(self, idxs=None): np.array: (N, 3) numpy array, where each of the N particles' positions are expressed in (x,y,z) cartesian coordinates relative to the world frame """ - all_particle_positions = self._cloth_prim_view.get_world_positions(clone=False)[0, :, :].cpu().numpy() + all_particle_positions = self._cloth_prim_view.get_world_positions(clone=False)[0, :, :] return all_particle_positions[:self._n_particles] if idxs is None else all_particle_positions[idxs] def set_particle_positions(self, positions, idxs=None): diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index c3f5bac45..bcf5a09fd 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -15,7 +15,7 @@ from omnigibson.prims.joint_prim import JointPrim from omnigibson.prims.rigid_prim import RigidPrim from omnigibson.prims.xform_prim import XFormPrim -from omnigibson.utils.deprecated_utils import ArticulationView +from omnigibson.utils.deprecated_utils import RetensorArticulationView from omnigibson.utils.constants import PrimType, GEOM_TYPES, JointType, JointAxis from omnigibson.utils.ui_utils import suppress_omni_log from omnigibson.utils.usd_utils import BoundingBoxAPI @@ -121,7 +121,7 @@ def _post_load(self): # Prepare the articulation view. if self.n_joints > 0: - self._articulation_view_direct = ArticulationView(f"{self._prim_path}/{self.root_link_name}") + self._articulation_view_direct = RetensorArticulationView(f"{self._prim_path}/{self.root_link_name}") # Set visual only flag # This automatically handles setting collisions / gravity appropriately per-link diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py index 348597b3a..6a779e12e 100644 --- a/omnigibson/prims/joint_prim.py +++ b/omnigibson/prims/joint_prim.py @@ -625,8 +625,8 @@ def get_target(self, normalized=False): # Grab raw states targets = self._articulation_view.get_applied_actions() - pos = targets.joint_positions[0][self.dof_indices] - vel = targets.joint_velocities[0][self.dof_indices] + pos = targets.joint_positions[0][self.dof_indices].cpu().numpy() + vel = targets.joint_velocities[0][self.dof_indices].cpu().numpy() # Potentially normalize if requested if normalized: diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 0c8d3f0b5..3d38ac423 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -10,7 +10,7 @@ from omnigibson.prims.xform_prim import XFormPrim from omnigibson.prims.geom_prim import CollisionGeomPrim, VisualGeomPrim from omnigibson.utils.constants import GEOM_TYPES -from omnigibson.utils.deprecated_utils import RigidPrimView +from omnigibson.utils.deprecated_utils import RetensorRigidPrimView from omnigibson.utils.sim_utils import CsRawData from omnigibson.utils.usd_utils import BoundingBoxAPI, get_mesh_volume_and_com import omnigibson.utils.transform_utils as T @@ -79,7 +79,7 @@ def __init__( def _post_load(self): # Create the view - self._rigid_prim_view_direct = RigidPrimView(self._prim_path) + self._rigid_prim_view_direct = RetensorRigidPrimView(self._prim_path) # Set it to be kinematic if necessary kinematic_only = "kinematic_only" in self._load_config and self._load_config["kinematic_only"] diff --git a/omnigibson/robots/fetch.py b/omnigibson/robots/fetch.py index 5e41af749..c3386f11b 100644 --- a/omnigibson/robots/fetch.py +++ b/omnigibson/robots/fetch.py @@ -270,7 +270,7 @@ def _initialize(self): for arm in self.arm_names: for joint in self.finger_joints[arm]: if joint.joint_type != JointType.JOINT_FIXED: - joint.friction = 500 + joint.friction = 500. def _actions_to_control(self, action): # Run super method first diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py index 2021806af..97561abe2 100644 --- a/omnigibson/robots/tiago.py +++ b/omnigibson/robots/tiago.py @@ -309,7 +309,7 @@ def _initialize(self): for arm in self.arm_names: for joint in self.finger_joints[arm]: if joint.joint_type != JointType.JOINT_FIXED: - joint.friction = 500 + joint.friction = 500. # Name of the actual root link that we are interested in. Note that this is different from self.root_link_name, # which is "base_footprint_x", corresponding to the first of the 6 1DoF joints to control the base. diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 2517d2f00..fc28f581c 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -86,7 +86,7 @@ def __init__( physics_dt=physics_dt, rendering_dt=rendering_dt, stage_units_in_meters=stage_units_in_meters, - device="cuda" if gm.USE_GPU_DYNAMICS else "cpu", + device="cuda:0" if gm.USE_GPU_DYNAMICS else "cpu", backend="torch" if gm.USE_GPU_DYNAMICS else "numpy" ) @@ -1137,19 +1137,6 @@ def device(self): """ return self._device - @device.setter - def device(self, device): - """ - Sets the device used for sim backend - - Args: - device (None or str): Device to set for the simulation backend - """ - self._device = device - if self._device is not None and "cuda" in self._device: - device_id = self._settings.get_as_int("/physics/cudaDevice") - self._device = f"cuda:{device_id}" - @property def state_size(self): # Total state size is the state size of our scene diff --git a/omnigibson/utils/deprecated_utils.py b/omnigibson/utils/deprecated_utils.py index 755d9a4ae..d7b2a3f34 100644 --- a/omnigibson/utils/deprecated_utils.py +++ b/omnigibson/utils/deprecated_utils.py @@ -18,7 +18,9 @@ import warp as wp import math from omni.isaac.core.articulations import ArticulationView as _ArticulationView +from omni.isaac.core.simulation_context import SimulationContext from omni.isaac.core.prims import RigidPrimView as _RigidPrimView +from omni.isaac.core.prims import ClothPrimView DEG2RAD = math.pi / 180.0 @@ -476,3 +478,57 @@ def disable_gravities(self, indices: Optional[Union[np.ndarray, list, torch.Tens self._physx_rigid_body_apis[i] = rigid_api self._physx_rigid_body_apis[i].GetDisableGravityAttr().Set(True) return + +def to_tensor(a, device): + if isinstance(a, np.ndarray): + a = torch.from_numpy(a) + # For floats, make them all float64s + if a.dtype == torch.float64: + a = a.type(torch.FloatTensor) + if device is not None: + a = a.to(device) + return a + return a + +def retensor(fn, device): + def wrapper(*args, **kwargs): + # Convert any arrays in the input into tensors + args = list(args) + for i in range(len(args)): + args[i] = to_tensor(args[i], device) + for k in list(kwargs.keys()): + kwargs[k] = to_tensor(kwargs[k], device) + results = fn(*args, **kwargs) + if isinstance(results, torch.Tensor): + results = results.cpu().numpy() + elif isinstance(results, tuple): + results = tuple([r.cpu().numpy() if isinstance(r, torch.Tensor) else r for r in results]) + return results + return wrapper + + +class Retensored: + def __init__(self, sub) -> None: + self.sub = sub + + def __getattribute__(self, item: str): + sub = super().__getattribute__("sub") + + val = sub.__getattribute__(item) + + if callable(val): + return retensor(val, "cuda:0") + return val + + +def retensored_constructor(cls): + def wrapper(*args, **kwargs): + assert SimulationContext.instance() is not None, "SimulationContext must be initialized before using this function" + assert "cuda" in SimulationContext.instance().device, "This function only works with CUDA devices" + return Retensored(cls(*args, **kwargs)) + return wrapper + + +RetensorArticulationView = retensored_constructor(ArticulationView) +RetensorRigidPrimView = retensored_constructor(RigidPrimView) +RetensorClothPrimView = retensored_constructor(ClothPrimView) From ee3e2f89105b16c70a522891be27d1b7d4c46501 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 21:18:43 -0800 Subject: [PATCH 19/28] Remove GPU dynamics flag, unnecessary flag changes, FlatcacheAPI --- docs/getting_started/building_blocks.md | 3 - .../action_primitives/solve_simple_task.py | 3 - .../wip_solve_behavior_task.py | 3 - .../environments/behavior_env_demo.py | 4 - .../examples/object_states/attachment_demo.py | 3 - .../examples/object_states/dicing_demo.py | 6 -- .../folded_unfolded_state_demo.py | 4 - .../object_states/heat_source_or_sink_demo.py | 4 - .../object_states/heated_state_demo.py | 4 - .../object_state_texture_demo.py | 7 +- .../examples/object_states/onfire_demo.py | 4 - .../examples/object_states/overlaid_demo.py | 6 -- .../particle_applier_remover_demo.py | 4 +- .../particle_source_sink_demo.py | 4 +- .../object_states/sample_kinematics_demo.py | 6 -- .../examples/object_states/slicing_demo.py | 4 - .../object_states/temperature_demo.py | 4 - .../examples/robots/grasping_mode_example.py | 3 - .../examples/robots/robot_control_example.py | 3 - omnigibson/examples/scenes/scene_selector.py | 1 - omnigibson/macros.py | 4 - omnigibson/object_states/cloth_mixin.py | 4 - omnigibson/object_states/particle_modifier.py | 4 - omnigibson/prims/geom_prim.py | 4 +- omnigibson/simulator.py | 23 ++-- omnigibson/systems/micro_particle_system.py | 4 - omnigibson/utils/usd_utils.py | 102 ------------------ tests/benchmark/profiling.py | 4 - tests/test_envs.py | 9 -- tests/test_primitives.py | 4 - tests/test_robot_teleoperation.py | 5 - tests/test_symbolic_primitives.py | 1 - tests/utils.py | 4 - 33 files changed, 13 insertions(+), 239 deletions(-) diff --git a/docs/getting_started/building_blocks.md b/docs/getting_started/building_blocks.md index d12b7338e..9ae8e71c0 100644 --- a/docs/getting_started/building_blocks.md +++ b/docs/getting_started/building_blocks.md @@ -13,9 +13,6 @@ icon: octicons/package-16 Macros enforce global behavior that is consistent within an individual python process but can differ between processes. This is useful because globally enabling all of **`OmniGibson`**'s features can cause unnecessary slowdowns, and so configuring the macros for your specific use case can optimize performance. - For example, Omniverse provides the option to compute physics on the GPU which provides significant performance boosts and is required when fluids or soft bodies are present. - So, we ideally should always have `gm.USE_GPU_DYNAMICS=True`, but if we don't have enough GPU memory, we can set it to False unless we have fluids or soft bodies in our environment. - `macros` define a globally available set of magic numbers or flags set throughout **`OmniGibson`**. These can either be directly set in `omnigibson.macros.py`, or can be programmatically modified at runtime via: ```{.python .annotate} diff --git a/omnigibson/examples/action_primitives/solve_simple_task.py b/omnigibson/examples/action_primitives/solve_simple_task.py index 4c3df9284..d11378d75 100644 --- a/omnigibson/examples/action_primitives/solve_simple_task.py +++ b/omnigibson/examples/action_primitives/solve_simple_task.py @@ -9,9 +9,6 @@ from omnigibson.macros import gm from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet -# Don't use GPU dynamics and use flatcache for performance boost -# gm.USE_GPU_DYNAMICS = True -# gm.ENABLE_FLATCACHE = True def execute_controller(ctrl_gen, env): for action in ctrl_gen: diff --git a/omnigibson/examples/action_primitives/wip_solve_behavior_task.py b/omnigibson/examples/action_primitives/wip_solve_behavior_task.py index 1767e3198..737d419b9 100644 --- a/omnigibson/examples/action_primitives/wip_solve_behavior_task.py +++ b/omnigibson/examples/action_primitives/wip_solve_behavior_task.py @@ -9,9 +9,6 @@ from omnigibson.macros import gm from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet -# Don't use GPU dynamics and use flatcache for performance boost -# gm.USE_GPU_DYNAMICS = True -# gm.ENABLE_FLATCACHE = True def execute_controller(ctrl_gen, env): for action in ctrl_gen: diff --git a/omnigibson/examples/environments/behavior_env_demo.py b/omnigibson/examples/environments/behavior_env_demo.py index efa591ee3..1a7d1042a 100644 --- a/omnigibson/examples/environments/behavior_env_demo.py +++ b/omnigibson/examples/environments/behavior_env_demo.py @@ -6,10 +6,6 @@ from omnigibson.macros import gm from omnigibson.utils.ui_utils import choose_from_options -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True - def main(random_selection=False, headless=False, short_exec=False): """ diff --git a/omnigibson/examples/object_states/attachment_demo.py b/omnigibson/examples/object_states/attachment_demo.py index 3718ab4e0..526befdb8 100644 --- a/omnigibson/examples/object_states/attachment_demo.py +++ b/omnigibson/examples/object_states/attachment_demo.py @@ -2,10 +2,7 @@ import numpy as np import omnigibson as og -from omnigibson.macros import gm -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True def main(random_selection=False, headless=False, short_exec=False): """ diff --git a/omnigibson/examples/object_states/dicing_demo.py b/omnigibson/examples/object_states/dicing_demo.py index 6e92d601a..2f21ad874 100644 --- a/omnigibson/examples/object_states/dicing_demo.py +++ b/omnigibson/examples/object_states/dicing_demo.py @@ -1,14 +1,8 @@ import numpy as np import omnigibson as og -from omnigibson.macros import gm import omnigibson.utils.transform_utils as T -# Make sure object states, GPU dynamics, and transition rules are enabled -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True -gm.ENABLE_TRANSITION_RULES = True - def main(random_selection=False, headless=False, short_exec=False): """ diff --git a/omnigibson/examples/object_states/folded_unfolded_state_demo.py b/omnigibson/examples/object_states/folded_unfolded_state_demo.py index 564dec8d6..cd88620ae 100644 --- a/omnigibson/examples/object_states/folded_unfolded_state_demo.py +++ b/omnigibson/examples/object_states/folded_unfolded_state_demo.py @@ -5,10 +5,6 @@ import omnigibson as og -# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True - def main(random_selection=False, headless=False, short_exec=False): """ diff --git a/omnigibson/examples/object_states/heat_source_or_sink_demo.py b/omnigibson/examples/object_states/heat_source_or_sink_demo.py index 804d96e5c..852b17e40 100644 --- a/omnigibson/examples/object_states/heat_source_or_sink_demo.py +++ b/omnigibson/examples/object_states/heat_source_or_sink_demo.py @@ -1,10 +1,6 @@ import numpy as np import omnigibson as og from omnigibson import object_states -from omnigibson.macros import gm - -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True def main(): diff --git a/omnigibson/examples/object_states/heated_state_demo.py b/omnigibson/examples/object_states/heated_state_demo.py index 7ec010af0..6321df880 100644 --- a/omnigibson/examples/object_states/heated_state_demo.py +++ b/omnigibson/examples/object_states/heated_state_demo.py @@ -1,10 +1,6 @@ import numpy as np import omnigibson as og from omnigibson import object_states -from omnigibson.macros import gm - -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True def main(): diff --git a/omnigibson/examples/object_states/object_state_texture_demo.py b/omnigibson/examples/object_states/object_state_texture_demo.py index dd28e988b..f76c317a3 100644 --- a/omnigibson/examples/object_states/object_state_texture_demo.py +++ b/omnigibson/examples/object_states/object_state_texture_demo.py @@ -5,9 +5,7 @@ from omnigibson.systems import get_system from omnigibson.utils.constants import ParticleModifyMethod -# Make sure object states are enabled, we're using GPU dynamics, and HQ rendering is enabled -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True +# Make sure HQ rendering is enabled gm.ENABLE_HQ_RENDERING = True @@ -48,8 +46,7 @@ def main(): # In this case, we only allow our cabinet to absorb water, with no conditions needed. # This is needed for the Saturated ("saturable") state so that we can modify the texture # according to the water. - # NOTE: This will only change color if gm.ENABLE_HQ_RENDERING and gm.USE_GPU_DYNAMICS is - # enabled! + # NOTE: This will only change color if gm.ENABLE_HQ_RENDERING is enabled! "water": [], }, }, diff --git a/omnigibson/examples/object_states/onfire_demo.py b/omnigibson/examples/object_states/onfire_demo.py index 3517e8130..5f9d3ad91 100644 --- a/omnigibson/examples/object_states/onfire_demo.py +++ b/omnigibson/examples/object_states/onfire_demo.py @@ -2,10 +2,6 @@ import omnigibson as og from omnigibson import object_states -from omnigibson.macros import gm - -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True def main(random_selection=False, headless=False, short_exec=False): diff --git a/omnigibson/examples/object_states/overlaid_demo.py b/omnigibson/examples/object_states/overlaid_demo.py index c9864b72c..62fc21d23 100644 --- a/omnigibson/examples/object_states/overlaid_demo.py +++ b/omnigibson/examples/object_states/overlaid_demo.py @@ -1,16 +1,10 @@ import numpy as np import omnigibson as og -from omnigibson.macros import gm from omnigibson.utils.constants import PrimType from omnigibson.object_states import Overlaid -# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True - - def main(random_selection=False, headless=False, short_exec=False): """ Demo of cloth objects that can be overlaid on rigid objects. diff --git a/omnigibson/examples/object_states/particle_applier_remover_demo.py b/omnigibson/examples/object_states/particle_applier_remover_demo.py index 6433ff323..68276824a 100644 --- a/omnigibson/examples/object_states/particle_applier_remover_demo.py +++ b/omnigibson/examples/object_states/particle_applier_remover_demo.py @@ -15,9 +15,7 @@ macros.object_states.particle_modifier.MAX_PHYSICAL_PARTICLES_APPLIED_PER_STEP = 40 macros.object_states.covered.MAX_VISUAL_PARTICLES = 300 -# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for fluids) -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True +# Enable HQ rendering for better visual fidelity gm.ENABLE_HQ_RENDERING = True diff --git a/omnigibson/examples/object_states/particle_source_sink_demo.py b/omnigibson/examples/object_states/particle_source_sink_demo.py index d5174baac..ef0e3c935 100644 --- a/omnigibson/examples/object_states/particle_source_sink_demo.py +++ b/omnigibson/examples/object_states/particle_source_sink_demo.py @@ -5,9 +5,7 @@ from omnigibson.macros import gm from omnigibson.utils.constants import ParticleModifyCondition -# Make sure object states are enabled and GPU dynamics are used -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True +# Enable HQ rendering for better visual fidelity gm.ENABLE_HQ_RENDERING = True diff --git a/omnigibson/examples/object_states/sample_kinematics_demo.py b/omnigibson/examples/object_states/sample_kinematics_demo.py index 4d8f87a25..c9259eccc 100644 --- a/omnigibson/examples/object_states/sample_kinematics_demo.py +++ b/omnigibson/examples/object_states/sample_kinematics_demo.py @@ -4,12 +4,6 @@ import omnigibson as og from omnigibson import object_states -from omnigibson.macros import gm -from omnigibson.objects import DatasetObject - - -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True def main(random_selection=False, headless=False, short_exec=False): diff --git a/omnigibson/examples/object_states/slicing_demo.py b/omnigibson/examples/object_states/slicing_demo.py index 500db990b..beab71401 100644 --- a/omnigibson/examples/object_states/slicing_demo.py +++ b/omnigibson/examples/object_states/slicing_demo.py @@ -4,10 +4,6 @@ from omnigibson.macros import gm import omnigibson.utils.transform_utils as T -# Make sure object states and transition rules are enabled -gm.ENABLE_OBJECT_STATES = True -gm.ENABLE_TRANSITION_RULES = True - def main(random_selection=False, headless=False, short_exec=False): """ diff --git a/omnigibson/examples/object_states/temperature_demo.py b/omnigibson/examples/object_states/temperature_demo.py index d8eb4bbdb..ca1278eee 100644 --- a/omnigibson/examples/object_states/temperature_demo.py +++ b/omnigibson/examples/object_states/temperature_demo.py @@ -2,10 +2,6 @@ import omnigibson as og from omnigibson import object_states -from omnigibson.macros import gm - -# Make sure object states are enabled -gm.ENABLE_OBJECT_STATES = True def main(random_selection=False, headless=False, short_exec=False): diff --git a/omnigibson/examples/robots/grasping_mode_example.py b/omnigibson/examples/robots/grasping_mode_example.py index fc3f5ed33..4e25fdaab 100644 --- a/omnigibson/examples/robots/grasping_mode_example.py +++ b/omnigibson/examples/robots/grasping_mode_example.py @@ -14,9 +14,6 @@ physical="Physical Grasping - No additional grasping assistance applied", ) -# Don't use GPU dynamics -gm.USE_GPU_DYNAMICS = False - def main(random_selection=False, headless=False, short_exec=False): """ diff --git a/omnigibson/examples/robots/robot_control_example.py b/omnigibson/examples/robots/robot_control_example.py index e751f4410..9e9500cd1 100644 --- a/omnigibson/examples/robots/robot_control_example.py +++ b/omnigibson/examples/robots/robot_control_example.py @@ -22,9 +22,6 @@ empty="Empty environment with no objects", ) -# Don't use GPU dynamics -gm.USE_GPU_DYNAMICS = False - def choose_controllers(robot, random_selection=False): """ diff --git a/omnigibson/examples/scenes/scene_selector.py b/omnigibson/examples/scenes/scene_selector.py index a20fc8f0b..fceeb5a12 100644 --- a/omnigibson/examples/scenes/scene_selector.py +++ b/omnigibson/examples/scenes/scene_selector.py @@ -4,7 +4,6 @@ from omnigibson.utils.ui_utils import choose_from_options # Configure macros for maximum performance -gm.USE_GPU_DYNAMICS = True gm.ENABLE_OBJECT_STATES = False gm.ENABLE_TRANSITION_RULES = False diff --git a/omnigibson/macros.py b/omnigibson/macros.py index f0ee61690..0ffc2ba2d 100644 --- a/omnigibson/macros.py +++ b/omnigibson/macros.py @@ -66,10 +66,6 @@ # Whether to print out disclaimers (i.e.: known failure cases resulting from Omniverse's current bugs / limitations) gm.SHOW_DISCLAIMERS = False -# Whether to use omni's GPU dynamics -# This is necessary for certain features; e.g. particles (fluids / cloth) -gm.USE_GPU_DYNAMICS = True - # Whether to use high-fidelity rendering (this includes, e.g., isosurfaces) gm.ENABLE_HQ_RENDERING = False diff --git a/omnigibson/object_states/cloth_mixin.py b/omnigibson/object_states/cloth_mixin.py index e5ef8012d..fc7b093f1 100644 --- a/omnigibson/object_states/cloth_mixin.py +++ b/omnigibson/object_states/cloth_mixin.py @@ -21,10 +21,6 @@ def is_compatible(cls, obj, **kwargs): return False, f"Cannot use ClothStateMixin {cls.__name__} with rigid object, make sure object is created " \ f"with prim_type=PrimType.CLOTH!" - # Check for GPU dynamics - if not gm.USE_GPU_DYNAMICS: - return False, f"gm.USE_GPU_DYNAMICS must be True in order to use object state {cls.__name__}." - return True, None @classproperty diff --git a/omnigibson/object_states/particle_modifier.py b/omnigibson/object_states/particle_modifier.py index a7a040ecb..42acef4c2 100644 --- a/omnigibson/object_states/particle_modifier.py +++ b/omnigibson/object_states/particle_modifier.py @@ -1374,10 +1374,6 @@ def is_compatible(cls, obj, **kwargs): if not compatible: return compatible, reason - # Check whether GPU dynamics are enabled (necessary for this object state) - if not gm.USE_GPU_DYNAMICS: - return False, f"gm.USE_GPU_DYNAMICS must be True in order to use object state {cls.__name__}." - return True, None @property diff --git a/omnigibson/prims/geom_prim.py b/omnigibson/prims/geom_prim.py index 849ea86fd..f3d149245 100644 --- a/omnigibson/prims/geom_prim.py +++ b/omnigibson/prims/geom_prim.py @@ -238,8 +238,8 @@ def collision_enabled(self, enabled): """ # Currently, trying to toggle while simulator is playing while using GPU dynamics results in a crash, so we # assert that the sim is stopped here - if self._initialized and gm.USE_GPU_DYNAMICS: - assert og.sim.is_stopped(), "Cannot toggle collisions while using GPU dynamics unless simulator is stopped!" + if self._initialized: + assert og.sim.is_stopped(), "Cannot toggle collisions unless simulator is stopped!" self.set_attribute("physics:collisionEnabled", enabled) # TODO: Maybe this should all be added to RigidPrim instead? diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 9130b771a..527f5baa6 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -19,7 +19,7 @@ from omnigibson.utils.config_utils import NumpyEncoder from omnigibson.utils.python_utils import clear as clear_pu, create_object_from_init_info, Serializable from omnigibson.utils.sim_utils import meets_minimum_isaac_version -from omnigibson.utils.usd_utils import clear as clear_uu, FlatcacheAPI, RigidContactAPI +from omnigibson.utils.usd_utils import clear as clear_uu, RigidContactAPI from omnigibson.utils.ui_utils import (CameraMover, disclaimer, create_module_logger, suppress_omni_log, print_icon, print_logo, logo_small) from omnigibson.scenes import Scene @@ -195,8 +195,8 @@ def __init__( physics_dt=physics_dt, rendering_dt=rendering_dt, stage_units_in_meters=stage_units_in_meters, - device="cuda:0" if gm.USE_GPU_DYNAMICS else "cpu", - backend="torch" if gm.USE_GPU_DYNAMICS else "numpy" + device="cuda:0", + backend="torch" ) if self._world_initialized: @@ -318,13 +318,9 @@ def _set_physics_engine_settings(self): else: self._physics_context.enable_flatcache(True) - # Enable GPU dynamics based on whether we need omni particles feature - if gm.USE_GPU_DYNAMICS: - self._physics_context.enable_gpu_dynamics(True) - self._physics_context.set_broadphase_type("GPU") - else: - self._physics_context.enable_gpu_dynamics(False) - self._physics_context.set_broadphase_type("MBP") + # Enable GPU dynamics + self._physics_context.enable_gpu_dynamics(True) + self._physics_context.set_broadphase_type("GPU") # Set GPU Pairs capacity and other GPU settings self._physics_context.set_gpu_found_lost_pairs_capacity(gm.GPU_PAIRS_CAPACITY) @@ -671,9 +667,7 @@ def play(self): # ignore this if the scale is close to uniform. # We also need to suppress the following error when flat cache is used: # [omni.physx.plugin] Transformation change on non-root links is not supported. - channels = ["omni.usd", "omni.physicsschema.plugin"] - if gm.ENABLE_FLATCACHE: - channels.append("omni.physx.plugin") + channels = ["omni.usd", "omni.physicsschema.plugin", "omni.physx.plugin"] with suppress_omni_log(channels=channels): super().play() @@ -718,9 +712,6 @@ def stop(self): if not self.is_stopped(): super().stop() - # Reset the FlatCache sync API - FlatcacheAPI.reset() - # Run all callbacks for callback in self._callbacks_on_stop.values(): callback() diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index a19818c24..b54bbb5cc 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -414,10 +414,6 @@ def initialize(cls): # Run super first super().initialize() - # Run sanity checks - if not gm.USE_GPU_DYNAMICS: - raise ValueError(f"Failed to initialize {cls.name} system. Please set gm.USE_GPU_DYNAMICS to be True.") - cls.system_prim = cls._create_particle_system() # Get material material = cls._get_particle_material_template() diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index 03f54ece1..5f922373b 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -447,108 +447,6 @@ def clear(cls): cls.ACTIVE_COLLISION_GROUPS = {} -class FlatcacheAPI: - """ - Monolithic class for leveraging functionality meant to be used EXCLUSIVELY with flatcache. - """ - # Modified prims since transition from sim being stopped to sim being played occurred - # This should get cleared every time og.sim.stop() gets called - MODIFIED_PRIMS = set() - - @classmethod - def sync_raw_object_transforms_in_usd(cls, prim): - """ - Manually synchronizes the per-link local raw transforms per-joint raw states from entity prim @prim using - dynamic control interface as the ground truth. - - NOTE: This slightly abuses the dynamic control - usd integration, and should ONLY be used if flatcache - is active, since the USD is not R/W at runtime and so we can write directly to child link poses on the USD - without breaking the simulation! - - Args: - prim (EntityPrim): prim whose owned links and joints should have their raw local states updated to match the - "true" values found from the dynamic control interface - """ - # Make sure flatcache is enabled -- this should NEVER be called otherwise!! - assert gm.ENABLE_FLATCACHE, "Syncing raw object transforms should only occur if flatcache is being used!" - - # We're somewhat abusing low-level dynamic control - physx - usd integration, but we (supposedly) know - # what we're doing so we suppress logging so we don't see any error messages :D - with suppress_omni_log(["omni.physx.plugin"]): - # Import here to avoid circular imports - from omnigibson.prims.xform_prim import XFormPrim - - # 1. For every link, update its xformOp properties based on the delta_tf between object frame and link frame - obj_pos, obj_quat = XFormPrim.get_local_pose(prim) - for link in prim.links.values(): - rel_pos, rel_quat = T.relative_pose_transform(*link.get_position_orientation(), obj_pos, obj_quat) - XFormPrim.set_local_pose(link, rel_pos, rel_quat) - # 2. For every joint, update its linear / angular joint state - if prim.n_joints > 0: - joints_pos = prim.get_joint_positions() - for joint, joint_pos in zip(prim.joints.values(), joints_pos): - state_name = "linear" if joint.joint_type == JointType.JOINT_PRISMATIC else "angular" - joint_pos = joint_pos if joint.joint_type == JointType.JOINT_PRISMATIC else joint_pos * 180.0 / np.pi - joint.set_attribute(f"state:{state_name}:physics:position", float(joint_pos)) - - # Update the simulation without taking any time - # This is needed because physx complains that we're manually writing to child links' poses, and will - # subsequently not respect any additional writes to the object pose before an additional step is taken. - # So we take a "zero" length step so that any additional writes to the object's pose at the current - # timestep are respected - og.sim.pi.update_simulation(elapsedStep=0, currentTime=og.sim.current_time) - - # Add this prim to the set of modified prims - cls.MODIFIED_PRIMS.add(prim) - - @classmethod - def reset_raw_object_transforms_in_usd(cls, prim): - """ - Manually resets the per-link local raw transforms and per-joint raw states from entity prim @prim to be zero. - - NOTE: This slightly abuses the dynamic control - usd integration, and should ONLY be used if flatcache - is active, since the USD is not R/W at runtime and so we can write directly to child link poses on the USD - without breaking the simulation! - - Args: - prim (EntityPrim): prim whose owned links and joints should have their local values reset to be zero - """ - # Make sure flatcache is enabled -- this should NEVER be called otherwise!! - assert gm.ENABLE_FLATCACHE, "Resetting raw object transforms should only occur if flatcache is being used!" - - # We're somewhat abusing low-level dynamic control - physx - usd integration, but we (supposedly) know - # what we're doing so we suppress logging so we don't see any error messages :D - with suppress_omni_log(["omni.physx.plugin"]): - # Import here to avoid circular imports - from omnigibson.prims.xform_prim import XFormPrim - - # 1. For every link, update its xformOp properties to be 0 - for link in prim.links.values(): - XFormPrim.set_local_pose(link, np.zeros(3), np.array([0, 0, 0, 1.0])) - # 2. For every joint, update its linear / angular joint state to be 0 - if prim.n_joints > 0: - for joint in prim.joints.values(): - state_name = "linear" if joint.joint_type == JointType.JOINT_PRISMATIC else "angular" - joint.set_attribute(f"state:{state_name}:physics:position", 0.0) - - # Update the simulation without taking any time - # This is needed because physx complains that we're manually writing to child links' poses, and will - # subsequently not respect any additional writes to the object pose before an additional step is taken. - # So we take a "zero" length step so that any additional writes to the object's pose at the current - # timestep are respected - og.sim.pi.update_simulation(elapsedStep=0, currentTime=og.sim.current_time) - - @classmethod - def reset(cls): - """ - Resets the internal state of this FlatcacheAPI.This should only occur when the simulator is stopped - """ - # For any prim transforms that were manually updated, we need to restore their original transforms - for prim in cls.MODIFIED_PRIMS: - cls.reset_raw_object_transforms_in_usd(prim) - cls.MODIFIED_PRIMS = set() - - def clear(): """ Clear state tied to singleton classes diff --git a/tests/benchmark/profiling.py b/tests/benchmark/profiling.py index ea671f160..9eae9028e 100644 --- a/tests/benchmark/profiling.py +++ b/tests/benchmark/profiling.py @@ -39,10 +39,6 @@ def main(): args = parser.parse_args() # Modify macros settings gm.ENABLE_HQ_RENDERING = args.fluids - gm.ENABLE_OBJECT_STATES = True - gm.ENABLE_TRANSITION_RULES = True - gm.ENABLE_FLATCACHE = not args.cloth - gm.USE_GPU_DYNAMICS = args.gpu_denamics cfg = { "env": { diff --git a/tests/test_envs.py b/tests/test_envs.py index 3ceb796af..ed098a8c0 100644 --- a/tests/test_envs.py +++ b/tests/test_envs.py @@ -29,10 +29,6 @@ def task_tester(task_type): if og.sim is not None: og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) - gm.ENABLE_OBJECT_STATES = True - gm.USE_GPU_DYNAMICS = True - # Create the environment env = og.Environment(configs=cfg) @@ -81,11 +77,6 @@ def test_rs_int_full_load(): # Make sure sim is stopped og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) - gm.ENABLE_OBJECT_STATES = True - gm.USE_GPU_DYNAMICS = True - gm.ENABLE_FLATCACHE = True - # Create the environment env = og.Environment(configs=cfg) diff --git a/tests/test_primitives.py b/tests/test_primitives.py index 2d0284fa2..e589567d0 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -90,10 +90,6 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg # Make sure sim is stopped og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) - gm.ENABLE_OBJECT_STATES = True - gm.USE_GPU_DYNAMICS = False - # Create the environment env = og.Environment(configs=cfg) robot = env.robots[0] diff --git a/tests/test_robot_teleoperation.py b/tests/test_robot_teleoperation.py index f7028dde2..eec133181 100644 --- a/tests/test_robot_teleoperation.py +++ b/tests/test_robot_teleoperation.py @@ -28,11 +28,6 @@ def test_teleop(): # Make sure sim is stopped og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache - gm.ENABLE_OBJECT_STATES = True - gm.USE_GPU_DYNAMICS = True - gm.ENABLE_FLATCACHE = False - # Create the environment env = og.Environment(configs=cfg) robot = env.robots[0] diff --git a/tests/test_symbolic_primitives.py b/tests/test_symbolic_primitives.py index e65e413b4..0d68a25d8 100644 --- a/tests/test_symbolic_primitives.py +++ b/tests/test_symbolic_primitives.py @@ -3,7 +3,6 @@ import yaml from omnigibson.macros import gm -gm.USE_GPU_DYNAMICS = True import omnigibson as og from omnigibson import object_states diff --git a/tests/utils.py b/tests/utils.py index 2a76f6aed..92c37e309 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -126,10 +126,6 @@ def assert_test_scene(): if og.sim is not None: og.sim.stop() - # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) - gm.ENABLE_OBJECT_STATES = True - gm.USE_GPU_DYNAMICS = True - # Create the environment env = og.Environment(configs=cfg) From a4b5bf998d3178b2b173351128d6c82a86532faa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 21:21:01 -0800 Subject: [PATCH 20/28] Fix bad lazy omni calls --- omnigibson/utils/teleop_utils.py | 3 +-- omnigibson/utils/ui_utils.py | 6 ++---- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/omnigibson/utils/teleop_utils.py b/omnigibson/utils/teleop_utils.py index c70e15df7..f0c07a44c 100644 --- a/omnigibson/utils/teleop_utils.py +++ b/omnigibson/utils/teleop_utils.py @@ -1,5 +1,4 @@ import numpy as np -import omni import time from dataclasses import dataclass, field from importlib import import_module @@ -679,7 +678,7 @@ def __init__(self, robot: BaseRobot, show_control_marker: bool = True, *args, ** def start(self) -> None: # start the keyboard subscriber - appwindow = omni.appwindow.get_default_app_window() + appwindow = lazy.omni.appwindow.get_default_app_window() input_interface = lazy.carb.input.acquire_input_interface() keyboard = appwindow.get_keyboard() input_interface.subscribe_to_keyboard_events(keyboard, self._update_internal_data) diff --git a/omnigibson/utils/ui_utils.py b/omnigibson/utils/ui_utils.py index e0fcd04f4..60bd7ed12 100644 --- a/omnigibson/utils/ui_utils.py +++ b/omnigibson/utils/ui_utils.py @@ -956,8 +956,7 @@ def draw_line(start, end, color=(1., 0., 0., 1.), size=1.): """ Draws a single line between two points. """ - from omni.isaac.debug_draw import _debug_draw - draw = _debug_draw.acquire_debug_draw_interface() + draw = lazy.omni.isaac.debug_draw._debug_draw.acquire_debug_draw_interface() draw.draw_lines([start], [end], [color], [size]) def draw_box(center, extents, color=(1., 0., 0., 1.), size=1.): @@ -980,6 +979,5 @@ def clear_debug_drawing(): """ Clears all debug drawings. """ - from omni.isaac.debug_draw import _debug_draw - draw = _debug_draw.acquire_debug_draw_interface() + draw = lazy.omni.isaac.debug_draw._debug_draw.acquire_debug_draw_interface() draw.clear_lines() From a58d70c3ffe26cd4b3d8dd593542a5cf28e23550 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 21:21:40 -0800 Subject: [PATCH 21/28] Remove eager omni import --- omnigibson/prims/cloth_prim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 468824706..f88b90137 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -12,7 +12,6 @@ from omnigibson.macros import create_module_macros, gm from omnigibson.prims.geom_prim import GeomPrim from omnigibson.systems import get_system -from omnigibson.utils.deprecated_utils import RetensorClothPrimView import omnigibson.utils.transform_utils as T from omnigibson.utils.geometry_utils import get_particle_positions_from_frame, get_particle_positions_in_frame from omnigibson.utils.sim_utils import CsRawData @@ -94,6 +93,7 @@ def _post_load(self): self._n_particles = len(self._prim.GetAttribute("points").Get()) # Load the cloth prim view + from omnigibson.utils.deprecated_utils import RetensorClothPrimView self._cloth_prim_view = RetensorClothPrimView(self._prim_path) # positions = self.get_particle_positions() From 965707b0eb6230efb39a14990fb1d271d1a2101d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 21:24:34 -0800 Subject: [PATCH 22/28] More lazy imports --- omnigibson/simulator.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 527f5baa6..3592da356 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -1248,14 +1248,14 @@ def _init_stage( ): # This below code is copied verbatim from the super class except for the removal of a render # call from the original - if get_current_stage() is None: - create_new_stage() + if lazy.omni.isaac.core.utils.stage.get_current_stage() is None: + lazy.omni.isaac.core.utils.stage.create_new_stage() self.render() - set_stage_up_axis("z") + lazy.omni.isaac.core.utils.stage.set_stage_up_axis("z") if stage_units_in_meters is not None: - set_stage_units(stage_units_in_meters=stage_units_in_meters) + lazy.omni.isaac.core.utils.stage.set_stage_units(stage_units_in_meters=stage_units_in_meters) # self.render() # This line causes crashes in Isaac Sim 2023.1.0. We don't need to render here. - self._physics_context = PhysicsContext( + self._physics_context = lazy.omni.isaac.core.physics_context.PhysicsContext( physics_dt=physics_dt, prim_path=physics_prim_path, sim_params=sim_params, From 3a24e3d34fa3545a2977d668018fea4b79cda061 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 21:37:45 -0800 Subject: [PATCH 23/28] Fix merge error --- omnigibson/simulator.py | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 3592da356..0e91849b9 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -252,22 +252,8 @@ def __init__( # and crashes self._set_physics_engine_settings() - def __new__( - cls, - gravity=9.81, - physics_dt=1.0 / 60.0, - rendering_dt=1.0 / 60.0, - stage_units_in_meters=1.0, - viewer_width=gm.DEFAULT_VIEWER_WIDTH, - viewer_height=gm.DEFAULT_VIEWER_HEIGHT, - device_idx=0, - ): - # Overwrite since we have different kwargs - if Simulator._instance is None: - Simulator._instance = object.__new__(cls) - else: - lazy.carb.log_info("Simulator is defined already, returning the previously defined one") - return Simulator._instance + def __new__(cls, *args, **kwargs): + lazy.omni.isaac.core.simulation_context.SimulationContext.__new__(*args, **kwargs) def _set_viewer_camera(self, prim_path="/World/viewer_camera", viewport_name="Viewport"): """ From 6cd67d12d00b348cf4f09a30c4686e0cc5fe5638 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 21:57:54 -0800 Subject: [PATCH 24/28] More fixes --- omnigibson/simulator.py | 2 +- omnigibson/utils/usd_utils.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 0e91849b9..d896cfe5d 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -253,7 +253,7 @@ def __init__( self._set_physics_engine_settings() def __new__(cls, *args, **kwargs): - lazy.omni.isaac.core.simulation_context.SimulationContext.__new__(*args, **kwargs) + return lazy.omni.isaac.core.simulation_context.SimulationContext.__new__(cls, *args, **kwargs) def _set_viewer_camera(self, prim_path="/World/viewer_camera", viewport_name="Viewport"): """ diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index 5f922373b..844a72113 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -319,7 +319,7 @@ def get_all_impulses(cls): """ # Generate the contact matrix if it doesn't already exist if cls._CONTACT_MATRIX is None: - cls._CONTACT_MATRIX = cls._CONTACT_VIEW.get_contact_force_matrix(dt=1.0) + cls._CONTACT_MATRIX = cls._CONTACT_VIEW.get_contact_force_matrix(dt=1.0).cpu().numpy() return cls._CONTACT_MATRIX From 83f3e3008ef26c1a903ef63a0be5df102a0abbde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 22 Feb 2024 22:08:17 -0800 Subject: [PATCH 25/28] Use GPU-compatible interface for velocity --- omnigibson/prims/entity_prim.py | 4 ++-- omnigibson/prims/rigid_prim.py | 26 ++++++++++++++++++++------ 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index 8ee5ae2e8..98ec39387 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -1367,8 +1367,8 @@ def keep_still(self): """ Zero out all velocities for this prim """ - self.set_linear_velocity(velocity=np.zeros(3)) - self.set_angular_velocity(velocity=np.zeros(3)) + self.set_linear_velocity(np.zeros(3)) + self.set_angular_velocity(np.zeros(3)) for joint in self._joints.values(): joint.keep_still() # Make sure object is awake diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index f112f586e..047fbad44 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -259,14 +259,26 @@ def contact_list(self): contacts.append(CsRawData(*c)) return contacts - def set_linear_velocity(self, velocity): + def set_velocity(self, velocity): + """ + Sets the linear and angular velocity of the prim in stage. + + Args: + velocity (np.ndarray): linear and angular velocity to set the rigid prim to. Shape (6,). + """ + assert velocity.shape == (6,), f"Velocity must be a 6-array, got {velocity.shape}" + self._rigid_prim_view.set_velocities(velocity[None, :]) + + def set_linear_velocity(self, linear_velocity): """ Sets the linear velocity of the prim in stage. Args: - velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,). + linear_velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,). """ - self._rigid_prim_view.set_linear_velocities(velocity[None, :]) + ang_vel = self.get_angular_velocity() + vel = np.concatenate([linear_velocity, ang_vel]) + self.set_velocity(vel) def get_linear_velocity(self): """ @@ -275,14 +287,16 @@ def get_linear_velocity(self): """ return self._rigid_prim_view.get_linear_velocities()[0] - def set_angular_velocity(self, velocity): + def set_angular_velocity(self, angular_velocity): """ Sets the angular velocity of the prim in stage. Args: - velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,). + angular_velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,). """ - self._rigid_prim_view.set_angular_velocities(velocity[None, :]) + lin_vel = self.get_linear_velocity() + vel = np.concatenate([lin_vel, angular_velocity]) + self.set_velocity(vel) def get_angular_velocity(self): """ From 47f72b0a99a6e39ac1845d9d96fabbac4f028183 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Sat, 24 Feb 2024 01:09:50 -0800 Subject: [PATCH 26/28] No kinematic objects for now --- omnigibson/objects/object_base.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py index cf458583d..c18fb0d50 100644 --- a/omnigibson/objects/object_base.py +++ b/omnigibson/objects/object_base.py @@ -133,19 +133,19 @@ def remove(self): def _post_load(self): # Add fixed joint or make object kinematic only if we're fixing the base kinematic_only = False - if self.fixed_base: - # For optimization purposes, if we only have a single rigid body that has either - # (no custom scaling OR no fixed joints), we assume this is not an articulated object so we - # merely set this to be a static collider, i.e.: kinematic-only - # The custom scaling / fixed joints requirement is needed because omniverse complains about scaling that - # occurs with respect to fixed joints, as omni will "snap" bodies together otherwise - scale = np.ones(3) if self._load_config["scale"] is None else np.array(self._load_config["scale"]) - if self.n_joints == 0 and (np.all(np.isclose(scale, 1.0, atol=1e-3)) or self.n_fixed_joints == 0) and (self._load_config["kinematic_only"] != False): - kinematic_only = True + # if self.fixed_base: + # # For optimization purposes, if we only have a single rigid body that has either + # # (no custom scaling OR no fixed joints), we assume this is not an articulated object so we + # # merely set this to be a static collider, i.e.: kinematic-only + # # The custom scaling / fixed joints requirement is needed because omniverse complains about scaling that + # # occurs with respect to fixed joints, as omni will "snap" bodies together otherwise + # scale = np.ones(3) if self._load_config["scale"] is None else np.array(self._load_config["scale"]) + # if self.n_joints == 0 and (np.all(np.isclose(scale, 1.0, atol=1e-3)) or self.n_fixed_joints == 0) and (self._load_config["kinematic_only"] != False): + # kinematic_only = True - # Validate that we didn't make a kinematic-only decision that does not match - assert self._load_config["kinematic_only"] is None or kinematic_only == self._load_config["kinematic_only"], \ - f"Kinematic only decision does not match! Got: {kinematic_only}, expected: {self._load_config['kinematic_only']}" + # # Validate that we didn't make a kinematic-only decision that does not match + # assert self._load_config["kinematic_only"] is None or kinematic_only == self._load_config["kinematic_only"], \ + # f"Kinematic only decision does not match! Got: {kinematic_only}, expected: {self._load_config['kinematic_only']}" # Actually apply the kinematic-only decision self._load_config["kinematic_only"] = kinematic_only From a213c6d41cd1be7bf2334f4fb38784fd86ea68c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Sat, 24 Feb 2024 01:10:01 -0800 Subject: [PATCH 27/28] Temporary mods to scene selector --- omnigibson/examples/scenes/scene_selector.py | 21 +++----------------- 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/omnigibson/examples/scenes/scene_selector.py b/omnigibson/examples/scenes/scene_selector.py index fceeb5a12..b3bc77902 100644 --- a/omnigibson/examples/scenes/scene_selector.py +++ b/omnigibson/examples/scenes/scene_selector.py @@ -32,14 +32,6 @@ def main(random_selection=False, headless=False, short_exec=False): "type": scene_type, "scene_model": scene_model, }, - "robots": [ - { - "type": "Turtlebot", - "obs_modalities": ["scan", "rgb", "depth"], - "action_type": "continuous", - "action_normalize": True, - }, - ], } # If the scene type is interactive, also check if we want to quick load or full load the scene @@ -60,16 +52,9 @@ def main(random_selection=False, headless=False, short_exec=False): og.sim.enable_viewer_camera_teleoperation() # Run a simple loop and reset periodically - max_iterations = 10 if not short_exec else 1 - for j in range(max_iterations): - og.log.info("Resetting environment") - env.reset() - for i in range(100): - action = env.action_space.sample() - state, reward, done, info = env.step(action) - if done: - og.log.info("Episode finished after {} timesteps".format(i + 1)) - break + while True: + action = env.action_space.sample() + state, reward, done, info = env.step(action) # Always close the environment at the end env.close() From 767a9464333c95c4e6bf27d628625c8b635ed8ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Wed, 28 Feb 2024 17:58:53 -0800 Subject: [PATCH 28/28] Revert "No more enable flatcache flag" This reverts commit 23385825abedb1c382d966fbb55cf491830f7e9c. --- omnigibson/utils/usd_utils.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index a19567ac0..b01a9e6ac 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -573,6 +573,9 @@ def sync_raw_object_transforms_in_usd(cls, prim): prim (EntityPrim): prim whose owned links and joints should have their raw local states updated to match the "true" values found from the dynamic control interface """ + # Make sure flatcache is enabled -- this should NEVER be called otherwise!! + assert gm.ENABLE_FLATCACHE, "Syncing raw object transforms should only occur if flatcache is being used!" + # We're somewhat abusing low-level dynamic control - physx - usd integration, but we (supposedly) know # what we're doing so we suppress logging so we don't see any error messages :D with suppress_omni_log(["omni.physx.plugin"]): @@ -614,6 +617,9 @@ def reset_raw_object_transforms_in_usd(cls, prim): Args: prim (EntityPrim): prim whose owned links and joints should have their local values reset to be zero """ + # Make sure flatcache is enabled -- this should NEVER be called otherwise!! + assert gm.ENABLE_FLATCACHE, "Resetting raw object transforms should only occur if flatcache is being used!" + # We're somewhat abusing low-level dynamic control - physx - usd integration, but we (supposedly) know # what we're doing so we suppress logging so we don't see any error messages :D with suppress_omni_log(["omni.physx.plugin"]):