Refresh views only when accessed with invalidated view

Author: hang-yin

omnigibson/examples/teleoperation/vr_robot_control_demo.py

@@ -80,15 +80,19 @@ def main():
         eef_tracking_mode="controller",
         align_anchor_to="camera",
         # roll, pitch, yaw
-        view_angle_limits=[180, 30, 30],
+        # view_angle_limits=[180, 30, 30],
     )
     vrsys.start()
 
-    for _ in range(3000):
+    robot = env.robots[0]
+
+    for _ in range(2000):
         # update the VR system
         vrsys.update()
         # get the action from the VR system and step the environment
-        env.step(vrsys.get_robot_teleop_action())
+        action = vrsys.get_robot_teleop_action()
+        env.step(action)
+        print("Controller action: " + str(action[robot.base_action_idx]))
 
     print("Cleaning up...")
     vrsys.stop()

omnigibson/examples/teleoperation/vr_scene_tour_demo.py

@@ -41,9 +41,12 @@ def main():
     )
     vrsys.start()
     # set headset position to be 1m above ground and facing +x
-    vrsys.vr_profile.set_physical_world_to_world_anchor_transform_to_match_xr_device(
-        vrsys.og2xr(th.tensor([0.0, 0.0, 1.0]), th.tensor([-0.5, 0.5, 0.5, -0.5])).numpy(), vrsys.hmd
-    )
+    # vrsys.vr_profile.set_physical_world_to_world_anchor_transform_to_match_xr_device(
+    #     vrsys.og2xr(th.tensor([0.0, 0.0, 1.0]), th.tensor([-0.5, 0.5, 0.5, -0.5])).numpy(), vrsys.hmd
+    # )
+
+    # anchor_pose = self.og2xr(anchor_pos, anchor_orn)
+    # self.xr_core.schedule_set_camera(anchor_pose.numpy())
 
     # main simulation loop
     for _ in range(3000):

omnigibson/object_states/attached_to.py

@@ -390,7 +390,6 @@ def _detach(self):
         if self.parent_link is not None:
             # Remove the attachment joint prim from the stage
             og.sim.stage.RemovePrim(self.attachment_joint_prim_path)
-            og.sim.update_handles()
 
             # Remove child reference from the parent object
             self.parent.states[AttachedTo].children[self.parent_link.body_name] = None

omnigibson/prims/cloth_prim.py

@@ -780,10 +780,6 @@ def report_hit(hit):
 
         return contacts
 
-    def update_handles(self):
-        # no handles to update
-        pass
-
     @property
     def volume(self):
         mesh = mesh_prim_to_trimesh_mesh(self.prim, include_normals=False, include_texcoord=False, world_frame=True)

omnigibson/prims/entity_prim.py

@@ -277,7 +277,6 @@ def update_joints(self):
         """
         # Initialize joints dictionary
         self._joints = dict()
-        self.update_handles()
 
         # Handle case separately based on whether we are actually articulated or not
         if self._articulation_view and not self.kinematic_only:
@@ -379,10 +378,10 @@ def _articulation_view(self):
         # Validate that the articulation view is initialized and that if physics is running, the
         # view is valid.
         if og.sim.is_playing() and self.initialized:
-            assert (
-                self._articulation_view_direct.is_physics_handle_valid()
-                and self._articulation_view_direct._physics_view.check()
-            ), "Articulation view must be valid if physics is running!"
+            physics_valid = self._articulation_view_direct.is_physics_handle_valid() and self._articulation_view_direct._physics_view.check()
+            if not physics_valid:
+                breakpoint()
+            assert physics_valid, "Articulation view must be valid if physics is running!"
 
         return self._articulation_view_direct
 
@@ -864,27 +863,6 @@ def _denormalize_efforts(self, efforts, indices=None):
         """
         return efforts * self.max_joint_efforts if indices is None else efforts * self.max_joint_efforts[indices]
 
-    def update_handles(self):
-        """
-        Updates all internal handles for this prim, in case they change since initialization
-        """
-        assert og.sim.is_playing(), "Simulator must be playing if updating handles!"
-
-        # Reinitialize the articulation view
-        if self._articulation_view_direct is not None:
-            self._articulation_view_direct.initialize(og.sim.physics_sim_view)
-
-        # Update all links and joints as well
-        for link in self._links.values():
-            if not link.initialized:
-                link.initialize()
-            link.update_handles()
-
-        for joint in self._joints.values():
-            if not joint.initialized:
-                joint.initialize()
-            joint.update_handles()
-
     def get_joint_positions(self, normalized=False):
         """
         Grabs this entity's joint positions

omnigibson/prims/joint_prim.py

@@ -135,8 +135,17 @@ def _initialize(self):
         # Always run super first
         super()._initialize()
 
-        # Update the joint indices etc.
-        self.update_handles()
+        # It's a bit tricky to get the joint index here. We need to find the first dof at this prim path
+        # first, then get the corresponding joint index from that dof offset.
+        self._joint_dof_offset = list(self._articulation_view._dof_paths[0]).index(self.prim_path)
+        joint_dof_offsets = self._articulation_view._metadata.joint_dof_offsets
+        # Note that we are finding the last occurrence of the dof offset, since that corresponds to the joint index
+        # The first occurrence can be a fixed link that is 0-dof, meaning the offset will be repeated.
+        self._joint_idx = next(
+            i for i in reversed(range(len(joint_dof_offsets))) if joint_dof_offsets[i] == self._joint_dof_offset
+        )
+        self._joint_name = self._articulation_view._metadata.joint_names[self._joint_idx]
+        self._n_dof = self._articulation_view._metadata.joint_dof_counts[self._joint_idx]
 
         # Get control type
         if self.articulated:
@@ -157,22 +166,6 @@ def _initialize(self):
             assert len(set(control_types)) == 1, f"Got multiple control types for this single joint: {control_types}"
             self._control_type = control_types[0]
 
-    def update_handles(self):
-        """
-        Updates all internal handles for this prim, in case they change since initialization
-        """
-        # It's a bit tricky to get the joint index here. We need to find the first dof at this prim path
-        # first, then get the corresponding joint index from that dof offset.
-        self._joint_dof_offset = list(self._articulation_view._dof_paths[0]).index(self.prim_path)
-        joint_dof_offsets = self._articulation_view._metadata.joint_dof_offsets
-        # Note that we are finding the last occurrence of the dof offset, since that corresponds to the joint index
-        # The first occurrence can be a fixed link that is 0-dof, meaning the offset will be repeated.
-        self._joint_idx = next(
-            i for i in reversed(range(len(joint_dof_offsets))) if joint_dof_offsets[i] == self._joint_dof_offset
-        )
-        self._joint_name = self._articulation_view._metadata.joint_names[self._joint_idx]
-        self._n_dof = self._articulation_view._metadata.joint_dof_counts[self._joint_idx]
-
     def set_control_type(self, control_type, kp=None, kd=None):
         """
         Sets the control type for this joint. Note that ControlType.NONE is equivalent to

omnigibson/prims/material_prim.py

@@ -96,7 +96,6 @@ def _load(self):
 
         # Move prim to desired location
         lazy.omni.kit.commands.execute("MovePrim", path_from=material_path, path_to=self.prim_path)
-        og.sim.update_handles()
 
         # Return generated material
         return lazy.isaacsim.core.utils.prims.get_prim_at_path(self.prim_path)

omnigibson/prims/rigid_dynamic_prim.py

@@ -66,22 +66,6 @@ def _initialize(self):
         if self.contact_reporting_enabled:
             og.sim.contact_sensor.get_rigid_body_raw_data(self.prim_path)
 
-    def update_handles(self):
-        """
-        Updates all internal handles for this prim, in case they change since initialization
-        """
-        # Validate that the view is valid if physics is running
-        if og.sim.is_playing() and self.initialized:
-            assert (
-                self._rigid_prim_view.is_physics_handle_valid() and self._rigid_prim_view._physics_view.check()
-            ), "Rigid prim view must be valid if physics is running!"
-
-        assert not (
-            og.sim.is_playing() and not self._rigid_prim_view.is_valid
-        ), "Rigid prim view must be valid if physics is running!"
-
-        self._rigid_prim_view.initialize(og.sim.physics_sim_view)
-
     def set_linear_velocity(self, velocity):
         """
         Sets the linear velocity of the prim in stage.

omnigibson/prims/rigid_prim.py

@@ -141,7 +141,6 @@ def _initialize(self):
                 mesh.initialize()
 
         # Grab handle to this rigid body and get name
-        self.update_handles()
         self._body_name = self.prim_path.split("/")[-1]
 
     def remove(self):
@@ -228,13 +227,6 @@ def disable_collisions(self):
         for col_mesh in self._collision_meshes.values():
             col_mesh.collision_enabled = False
 
-    def update_handles(self):
-        """
-        Updates all internal handles for this prim, in case they change since initialization.
-        To be implemented by subclasses as needed.
-        """
-        pass
-
     def enable_gravity(self):
         """
         Enables gravity for this rigid body.

omnigibson/robots/manipulation_robot.py

@@ -613,7 +613,6 @@ def _release_grasp(self, arm="default"):
 
         # Remove joint and filtered collision restraints
         og.sim.stage.RemovePrim(self._ag_obj_constraint_params[arm]["ag_joint_prim_path"])
-        og.sim.update_handles()
         self._ag_obj_constraints[arm] = None
         self._ag_obj_constraint_params[arm] = {}
         self._ag_freeze_gripper[arm] = False

omnigibson/simulator.py

@@ -765,11 +765,7 @@ def adding_objects(self, objs):
                 # The objects have been added to the USD stage but PhysX hasn't been synchronized yet.
                 # We must flush USD changes to PhysX before updating handles to avoid errors like
                 # "Provided pattern list did not match any rigid bodies".
-                # The order of operations should strictly be:
-                #   1. Flush USD changes to PhysX
-                #   2. Update handles to reinitialize physics view
                 SimulationManager._physx_sim_interface.flush_changes()
-                self.update_handles()
 
         def _post_import_object(self, obj):
             """
@@ -846,9 +842,6 @@ def removing_objects(self, objs):
 
             # Run post-processing required if we were playing
             if playing:
-                # Update all handles that are now broken because objects have changed
-                self.update_handles()
-
                 if gm.ENABLE_TRANSITION_RULES:
                     # Prune the transition rules that are currently active
                     for scene in scenes_modified:
@@ -902,9 +895,6 @@ def remove_prim(self, prim):
                 # Remove prim
                 prim.remove()
 
-            # Update all handles that are now broken because prims have changed
-            self.update_handles()
-
         def _reset_variables(self):
             """
             Reset internal variables when a new stage is loaded
@@ -916,6 +906,9 @@ def render(self):
             PoseAPI.mark_valid()
 
         def _refresh_physics_sim_view(self):
+            # TODO: Figure out if we need to refresh this
+            breakpoint()
+
             SimulationManager = lazy.isaacsim.core.simulation_manager.SimulationManager
             IsaacEvents = lazy.isaacsim.core.simulation_manager.IsaacEvents
 
@@ -926,30 +919,6 @@ def _refresh_physics_sim_view(self):
             SimulationManager._message_bus.dispatch(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={})
             SimulationManager._message_bus.dispatch(IsaacEvents.PHYSICS_READY.value, payload={})
 
-        def update_handles(self):
-            # Handles are only relevant when physx is running
-            if not self.is_playing():
-                return
-
-            # Refresh the sim view
-            self._refresh_physics_sim_view()
-
-            # Then update the handles for all objects
-            for scene in self.scenes:
-                if scene is not None:
-                    for obj in scene.objects:
-                        # Only need to update if object is already initialized as well
-                        if obj.initialized:
-                            obj.update_handles()
-                    for system in scene.active_systems.values():
-                        if isinstance(system, MacroPhysicalParticleSystem):
-                            system.refresh_particles_view()
-
-            # Finally update any unified views
-            RigidContactAPI.initialize_view()
-            GripperRigidContactAPI.initialize_view()
-            ControllableObjectViewAPI.initialize_view()
-
         def _non_physics_step(self):
             """
             Complete any non-physics steps such as state updates.
@@ -981,9 +950,6 @@ def _non_physics_step(self):
 
                     self._objects_to_initialize = self._objects_to_initialize[n_objects_to_initialize:]
 
-                    # Re-initialize the physics view because the number of objects has changed
-                    self.update_handles()
-
                     if gm.ENABLE_TRANSITION_RULES:
                         # Refresh the transition rules
                         for scene in scenes_modified:
@@ -1046,10 +1012,6 @@ def play(self):
                 # correctly.
                 self.render()
 
-                # Update all object handles, unless this is a play during initialization
-                if og.sim is not None:
-                    self.update_handles()
-
                 if was_stopped:
                     # We need to update controller mode because kp and kd were set to the original (incorrect) values when
                     # sim was stopped. We need to reset them to default_kp and default_kd defined in ControllableObject.
@@ -1461,6 +1423,7 @@ def psi(self):
             Returns:
                 IPhysxSimulation: Physx Simulation Interface (psi) for controlling low-level physx simulation
             """
+            self._refresh_physics_sim_view()
             return self._physx_simulation_interface
 
         @property

omnigibson/systems/macro_particle_system.py

@@ -285,8 +285,6 @@ def generate_particles(
         for scale in scales:
             self.add_particle(relative_prim_path=f"{self.relative_prim_path}/particles", scale=scale)
 
-        og.sim.update_handles()
-
         # Set the tfs
         self.set_particles_position_orientation(positions=positions, orientations=orientations)
 
@@ -1178,7 +1176,7 @@ def __init__(self, name, create_particle_template, particle_density, scale=None,
         self._particle_density = particle_density
 
         # Physics rigid body view for keeping track of all particles' state
-        self.particles_view = None
+        self._particles_view = None
 
         # Approximate radius of the macro particle, and distance from particle frame to approximate center
         self._particle_radius = None
@@ -1198,6 +1196,11 @@ def initialize(self, scene):
         if og.sim.is_playing():
             self.refresh_particles_view()
 
+    @property
+    def particles_view(self):
+        self.refresh_particles_view()
+        return self._particles_view
+
     def _load_new_particle(self, relative_prim_path, name):
         # We copy the template prim and generate the new object if the prim doesn't already exist, otherwise we
         # reference the pre-existing one
@@ -1250,9 +1253,12 @@ def refresh_particles_view(self):
 
         Should be called every time sim.play() is called
         """
+        # TODO: Figure out if we need to update?
+        breakpoint()
+
         og.sim.pi.update_simulation(elapsedStep=0, currentTime=og.sim.current_time)
         with suppress_omni_log(channels=["omni.physx.tensors.plugin"]):
-            self.particles_view = og.sim.physics_sim_view.create_rigid_body_view(
+            self._particles_view = og.sim.physics_sim_view.create_rigid_body_view(
                 pattern=f"{self.prim_path}/particles/*"
             )
 
@@ -1261,7 +1267,7 @@ def _clear(self):
         super()._clear()
 
         # Clear internal variables
-        self.particles_view = None
+        self._particles_view = None
         self._particle_radius = None
         self._particle_offset = None
 

omnigibson/systems/system_base.py

@@ -204,7 +204,6 @@ def _clear(self):
 
         self.reset()
         lazy.isaacsim.core.utils.prims.delete_prim(self.prim_path)
-        og.sim.update_handles()
 
         if og.sim.is_playing() and gm.ENABLE_TRANSITION_RULES:
             self.scene.transition_rule_api.prune_active_rules()