From 9f9c64dca7c370515dca5384ce1ec41d6a620d86 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 24 Oct 2023 12:21:42 -0700
Subject: [PATCH 01/76] Start articulationview work

---
 omnigibson/prims/entity_prim.py | 22 +++++++++++++++++++---
 1 file changed, 19 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index ea33efd2b..11c426544 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -59,6 +59,7 @@ def __init__(
         self._joints = None
         self._materials = None
         self._visual_only = None
+        self._articulation_view = None
 
         # This needs to be initialized to be used for _load() of PrimitiveObject
         self._prim_type = load_config["prim_type"] if load_config is not None and "prim_type" in load_config else PrimType.RIGID
@@ -94,6 +95,10 @@ def _initialize(self):
         # Update joint information
         self.update_joints()
 
+        # Construct physics view
+        if self.articulated:
+            self._articulation_view = og.sim.physics_sim_view.create_articulation_view(self.articulation_root_path)
+
     def _load(self):
         # By default, this prim cannot be instantiated from scratch!
         raise NotImplementedError("By default, an entity prim cannot be created from scratch.")
@@ -763,6 +768,14 @@ 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 _verify_articulation_view_is_valid(self):
+        """
+        Helper function to make sure that the internal physics view is valid -- if not, will automatically refresh the
+        internal pointer
+        """
+        if not self._articulation_view.check():
+            self._articulation_view = og.sim.physics_sim_view.create_articulation_view(self.articulation_root_path)
+
     def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
@@ -807,7 +820,8 @@ def get_joint_positions(self, normalized=False):
         assert self._handle is not None, "handles are not initialized yet!"
         assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!"
 
-        joint_positions = self._dc.get_articulation_dof_states(self._handle, _dynamic_control.STATE_POS)["pos"]
+        self._verify_articulation_view_is_valid()
+        joint_positions = self._articulation_view.get_dof_positions().reshape(self.n_dof)
 
         # Possibly normalize values when returning
         return self._normalize_positions(positions=joint_positions) if normalized else joint_positions
@@ -826,7 +840,8 @@ def get_joint_velocities(self, normalized=False):
         assert self._handle is not None, "handles are not initialized yet!"
         assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!"
 
-        joint_velocities = self._dc.get_articulation_dof_states(self._handle, _dynamic_control.STATE_VEL)["vel"]
+        self._verify_articulation_view_is_valid()
+        joint_velocities = self._articulation_view.get_dof_velocities().reshape(self.n_dof)
 
         # Possibly normalize values when returning
         return self._normalize_velocities(velocities=joint_velocities) if normalized else joint_velocities
@@ -845,7 +860,8 @@ def get_joint_efforts(self, normalized=False):
         assert self._handle is not None, "handles are not initialized yet!"
         assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!"
 
-        joint_efforts = self._dc.get_articulation_dof_states(self._handle, _dynamic_control.STATE_EFFORT)["effort"]
+        self._verify_articulation_view_is_valid()
+        joint_efforts = self._articulation_view.get_dof_actuation_forces().reshape(self.n_dof)
 
         # Possibly normalize values when returning
         return self._normalize_efforts(efforts=joint_efforts) if normalized else joint_efforts

From 4af5a0fcfedb6d3eedd76205bac9c4f21300d7a6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 14:23:37 -0800
Subject: [PATCH 02/76] Update entityprim to use articulationview instead of dc

---
 omnigibson/prims/entity_prim.py | 274 +++++---------------------------
 1 file changed, 38 insertions(+), 236 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index b5947ed93..f9ddc3285 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -1,5 +1,6 @@
 import numpy as np
 
+from omni.isaac.core.articulations import ArticulationView
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
 from omni.isaac.core.utils.transformations import tf_matrix_from_pose
 from omni.isaac.core.utils.types import DOFInfo
@@ -49,11 +50,7 @@ def __init__(
         load_config=None,
     ):
         # Other values that will be filled in at runtime
-        self._dc = None                         # Dynamics control interface
-        self._handle = None                     # Handle to this articulation
-        self._root_handle = None                # Handle to the root rigid body of this articulation
         self._root_link_name = None             # Name of the root link
-        self._dofs_infos = None
         self._n_dof = None                      # dof with dynamic control
         self._links = None
         self._joints = None
@@ -84,26 +81,20 @@ def _initialize(self):
                 material.shader_force_populate(render=False)
 
         # Initialize all the links
-        # This must happen BEFORE the handle is generated for this prim, because things changing in the RigidPrims may
-        # cause the handle to change!
         for link in self._links.values():
             link.initialize()
 
-        # Get dynamic control info
-        self._dc = _dynamic_control.acquire_dynamic_control_interface()
-
         # Update joint information
         self.update_joints()
 
-        # Construct physics view
-        if self.articulated:
-            self._articulation_view = og.sim.physics_sim_view.create_articulation_view(self.articulation_root_path)
-
     def _load(self):
         # By default, this prim cannot be instantiated from scratch!
         raise NotImplementedError("By default, an entity prim cannot be created from scratch.")
 
     def _post_load(self):
+        # Prepare the articulation view (at this point only working via the USD interface)
+        self._articulation_view = ArticulationView(self._prim_path)
+
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
             # Verify only a single link and a single mesh exists
@@ -218,9 +209,8 @@ def update_links(self, load_config=None):
         # in the children joints
         valid_root_links = list(set(self._links.keys()) - joint_children)
 
-        # TODO: Uncomment safety check here after we figure out how to handle legacy multi-bodied assets like bed with pillow
-        # assert len(valid_root_links) == 1, f"Only a single root link should have been found for this entity prim, " \
-        #                                    f"but found multiple instead: {valid_root_links}"
+        assert len(valid_root_links) == 1, f"Only a single root link should have been found for this entity prim, " \
+                                           f"but found multiple instead: {valid_root_links}"
         self._root_link_name = valid_root_links[0] if len(valid_root_links) == 1 else "base_link"
 
     def update_joints(self):
@@ -237,51 +227,31 @@ def update_joints(self):
         self._joints = dict()
         self.update_handles()
 
-        # Handle case separately based on whether the handle is valid (i.e.: whether we are actually articulated or not)
-        if (not self.kinematic_only) and self._handle is not None:
-            root_prim = get_prim_at_path(self._dc.get_rigid_body_path(self._root_handle))
-            n_dof = self._dc.get_articulation_dof_count(self._handle)
+        # Handle case separately based on whether we are actually articulated or not
+        if not self.kinematic_only:
+            self._n_dof = self._articulation_view.num_dof
 
+            # TODO: This still includes fixed joints for objects that also have non-fixed?
             # Additionally grab DOF info if we have non-fixed joints
-            if n_dof > 0:
-                self._dofs_infos = dict()
-                # Grab DOF info
-                for index in range(n_dof):
-                    dof_handle = self._dc.get_articulation_dof(self._handle, index)
-                    dof_name = self._dc.get_dof_name(dof_handle)
-                    # add dof to list
-                    prim_path = self._dc.get_dof_path(dof_handle)
-                    self._dofs_infos[dof_name] = DOFInfo(prim_path=prim_path, handle=dof_handle, prim=self.prim,
-                                                         index=index)
-
-                for i in range(self._dc.get_articulation_joint_count(self._handle)):
-                    joint_handle = self._dc.get_articulation_joint(self._handle, i)
-                    joint_name = self._dc.get_joint_name(joint_handle)
-                    joint_path = self._dc.get_joint_path(joint_handle)
-                    joint_prim = get_prim_at_path(joint_path)
+            if self._n_dof > 0:
+                for i in range(self._articulation_view._metadata.joint_count):
+                    joint_name = self._articulation_view._metadata.joint_names[i]
+                    joint_dof_offset = self._articulation_view._metadata.joint_dof_offsets[i]
+                    joint_path = self._articulation_view._metadata.dof_paths[0][joint_dof_offset]
                     # Only add the joint if it's not fixed (i.e.: it has DOFs > 0)
-                    if self._dc.get_joint_dof_count(joint_handle) > 0:
+                    if self._articulation_view._metadata.joint_dof_counts[i] > 0:
                         joint = JointPrim(
                             prim_path=joint_path,
                             name=f"{self._name}:joint_{joint_name}",
-                            articulation=self._handle,
+                            articulation=self._articulation_view,
                         )
                         joint.initialize()
                         self._joints[joint_name] = joint
         else:
             # TODO: May need to extend to clusters of rigid bodies, that aren't exactly joined
             # We assume this object contains a single rigid body
-            body_path = f"{self._prim_path}/{self.root_link_name}"
-            root_prim = get_prim_at_path(body_path)
-            n_dof = 0
+            self._n_dof = 0
 
-        # Make sure root prim stored is the same as the one found during initialization
-        assert self.root_prim == root_prim, \
-            f"Mismatch in root prims! Original was {self.root_prim.GetPrimPath()}, " \
-            f"initialized is {root_prim.GetPrimPath()}!"
-
-        # Store values internally
-        self._n_dof = n_dof
         self._update_joint_limits()
 
     def _update_joint_limits(self):
@@ -349,7 +319,7 @@ def articulated(self):
              bool: Whether this prim is articulated or not
         """
         # An invalid handle implies that there is no articulation available for this object
-        return self._handle is not None or self.articulation_root_path is not None
+        return self.articulation_root_path is not None
 
     @property
     def articulation_root_path(self):
@@ -482,14 +452,6 @@ def materials(self):
         """
         return self._materials
 
-    @property
-    def dof_properties(self):
-        """
-        Returns:
-            n-array: Array of DOF properties assigned to this articulation's DoFs.
-        """
-        return self._dc.get_articulation_dof_properties(self._handle)
-
     @property
     def visual_only(self):
         """
@@ -564,26 +526,13 @@ def set_joint_positions(self, positions, indices=None, normalized=False, drive=F
         if normalized:
             positions = self._denormalize_positions(positions=positions, indices=indices)
 
-        # Grab current DOF states
-        dof_states = self._dc.get_articulation_dof_states(self._handle, _dynamic_control.STATE_POS)
-
-        # Possibly set specific values in the array if indies are specified
-        if indices is None:
-            assert len(positions) == self._n_dof, \
-                "set_joint_positions called without specifying indices, but the desired positions do not match n_dof."
-            new_positions = positions
-        else:
-            new_positions = dof_states["pos"]
-            new_positions[indices] = positions
-
         # Set the DOF states
-        dof_states["pos"] = new_positions
         if not drive:
-            self._dc.set_articulation_dof_states(self._handle, dof_states, _dynamic_control.STATE_POS)
+            self._articulation_view.set_joint_positions(positions=positions, joint_indices=indices)
             BoundingBoxAPI.clear()
 
         # Also set the target
-        self._dc.set_articulation_dof_position_targets(self._handle, new_positions.astype(np.float32))
+        self._articulation_view.set_joint_position_targets(positions=positions, joint_indices=indices)
 
     def set_joint_velocities(self, velocities, indices=None, normalized=False, drive=False):
         """
@@ -610,23 +559,12 @@ def set_joint_velocities(self, velocities, indices=None, normalized=False, drive
         if normalized:
             velocities = self._denormalize_velocities(velocities=velocities, indices=indices)
 
-        # Grab current DOF states
-        dof_states = self._dc.get_articulation_dof_states(self._handle, _dynamic_control.STATE_VEL)
-
-        # Possibly set specific values in the array if indies are specified
-        if indices is None:
-            new_velocities = velocities
-        else:
-            new_velocities = dof_states["vel"]
-            new_velocities[indices] = velocities
-
         # Set the DOF states
-        dof_states["vel"] = new_velocities
         if not drive:
-            self._dc.set_articulation_dof_states(self._handle, dof_states, _dynamic_control.STATE_VEL)
+            self._articulation_view.set_joint_velocities(velocities=velocities, joint_indices=indices)
 
         # Also set the target
-        self._dc.set_articulation_dof_velocity_targets(self._handle, new_velocities.astype(np.float32))
+        self._articulation_view.set_joint_velocity_targets(velocities=velocities, joint_indices=indices)
 
     def set_joint_efforts(self, efforts, indices=None, normalized=False):
         """
@@ -650,19 +588,8 @@ def set_joint_efforts(self, efforts, indices=None, normalized=False):
         if normalized:
             efforts = self._denormalize_efforts(efforts=efforts, indices=indices)
 
-        # Grab current DOF states
-        dof_states = self._dc.get_articulation_dof_states(self._handle, _dynamic_control.STATE_EFFORT)
-
-        # Possibly set specific values in the array if indies are specified
-        if indices is None:
-            new_efforts = efforts
-        else:
-            new_efforts = dof_states["effort"]
-            new_efforts[indices] = efforts
-
         # Set the DOF states
-        dof_states["effort"] = new_efforts
-        self._dc.set_articulation_dof_states(self._handle, dof_states, _dynamic_control.STATE_EFFORT)
+        self._articulation_view.set_joint_efforts(velocities=efforts, joint_indices=indices)
 
     def _normalize_positions(self, positions, indices=None):
         """
@@ -768,32 +695,14 @@ 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 _verify_articulation_view_is_valid(self):
-        """
-        Helper function to make sure that the internal physics view is valid -- if not, will automatically refresh the
-        internal pointer
-        """
-        if not self._articulation_view.check():
-            self._articulation_view = og.sim.physics_sim_view.create_articulation_view(self.articulation_root_path)
-
     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!"
 
-        # Grab the handle -- we know it might not return a valid value, so we suppress omni's warning here
-        self._handle = None if self.articulation_root_path is None else \
-            self._dc.get_articulation(self.articulation_root_path)
-
-        # Sanity check -- make sure handle is not invalid handle -- it should only ever be None or a valid integer
-        assert self._handle != _dynamic_control.INVALID_HANDLE, \
-            f"Got invalid articulation handle for entity at {self.articulation_root_path}"
-
-        # We only have a root handle if we're not a cloth prim
-        if self._prim_type != PrimType.CLOTH:
-            self._root_handle = self._dc.get_articulation_root_body(self._handle) if \
-                self._handle is not None else self.root_link.handle
+        # Reinitialize the articulation view
+        self._articulation_view.initialize(og.sim.physics_sim_view)
 
         # Update all links and joints as well
         for link in self._links.values():
@@ -806,10 +715,6 @@ def update_handles(self):
                 joint.initialize()
             joint.update_handles()
 
-        # Update the physics view if we're articulated
-        if self.articulated:
-            self._physics_view = og.sim.physics_sim_view.create_articulation_view(self.articulation_root_path)
-
     def get_joint_positions(self, normalized=False):
         """
         Grabs this entity's joint positions
@@ -820,12 +725,10 @@ def get_joint_positions(self, normalized=False):
         Returns:
             n-array: n-DOF length array of positions
         """
-        # Run sanity checks -- make sure our handle is initialized and that we are articulated
-        assert self._handle is not None, "handles are not initialized yet!"
+        # 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!"
 
-        self._verify_articulation_view_is_valid()
-        joint_positions = self._articulation_view.get_dof_positions().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
@@ -840,12 +743,10 @@ def get_joint_velocities(self, normalized=False):
         Returns:
             n-array: n-DOF length array of velocities
         """
-        # Run sanity checks -- make sure our handle is initialized and that we are articulated
-        assert self._handle is not None, "handles are not initialized yet!"
+        # 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!"
 
-        self._verify_articulation_view_is_valid()
-        joint_velocities = self._articulation_view.get_dof_velocities().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
@@ -860,12 +761,10 @@ def get_joint_efforts(self, normalized=False):
         Returns:
             n-array: n-DOF length array of efforts
         """
-        # Run sanity checks -- make sure our handle is initialized and that we are articulated
-        assert self._handle is not None, "handles are not initialized yet!"
+        # 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!"
 
-        self._verify_articulation_view_is_valid()
-        joint_efforts = self._articulation_view.get_dof_actuation_forces().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
@@ -906,117 +805,20 @@ def get_angular_velocity(self):
         return self.root_link.get_angular_velocity()
 
     def set_position_orientation(self, position=None, orientation=None):
-        current_position, current_orientation = self.get_position_orientation()
-        if position is None:
-            position = current_position
-        if orientation is None:
-            orientation = current_orientation
-
-        if self._prim_type == PrimType.CLOTH:
-            if self._dc is not None and self._dc.is_simulating():
-                self.root_link.set_position_orientation(position, orientation)
-            else:
-                super().set_position_orientation(position, orientation)
-        else:
-            if self._root_handle is not None and self._root_handle != _dynamic_control.INVALID_HANDLE and \
-                    self._dc is not None and self._dc.is_simulating():
-                self.root_link.set_position_orientation(position, orientation)
-            else:
-                super().set_position_orientation(position=position, orientation=orientation)
-
+        self._articulation_view.set_world_poses(position[None, :], orientation[None, :])
         BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
-        if self._prim_type == PrimType.CLOTH:
-            if self._dc is not None and self._dc.is_simulating():
-                return self.root_link.get_position_orientation()
-            else:
-                return super().get_position_orientation()
-        else:
-            if self._root_handle is not None and self._root_handle != _dynamic_control.INVALID_HANDLE and \
-                    self._dc is not None and self._dc.is_simulating():
-                return self.root_link.get_position_orientation()
-            else:
-                return super().get_position_orientation()
-
-    def _set_local_pose_when_simulating(self, translation=None, orientation=None):
-        """
-        Sets prim's pose with respect to the local frame (the prim's parent frame) when simulation is running.
-
-        Args:
-            translation (None or 3-array): if specified, (x,y,z) translation in the local frame of the prim
-                (with respect to its parent prim). Default is None, which means left unchanged.
-            orientation (None or 4-array): if specified, (x,y,z,w) quaternion orientation in the local frame of the prim
-                (with respect to its parent prim). Default is None, which means left unchanged.
-        """
-        current_translation, current_orientation = self.get_local_pose()
-        if translation is None:
-            translation = current_translation
-        if orientation is None:
-            orientation = current_orientation
-        orientation = orientation[[3, 0, 1, 2]]
-        local_transform = tf_matrix_from_pose(translation=translation, orientation=orientation)
-        parent_world_tf = UsdGeom.Xformable(get_prim_parent(self._prim)).ComputeLocalToWorldTransform(
-            Usd.TimeCode.Default()
-        )
-        my_world_transform = np.matmul(parent_world_tf, local_transform)
-        transform = Gf.Transform()
-        transform.SetMatrix(Gf.Matrix4d(np.transpose(my_world_transform)))
-        calculated_position = transform.GetTranslation()
-        calculated_orientation = transform.GetRotation().GetQuat()
-        self.set_position_orientation(
-            position=np.array(calculated_position),
-            orientation=gf_quat_to_np_array(calculated_orientation)[[1, 2, 3, 0]],
-        )
+        positions, orientations = self._articulation_view.get_world_poses()
+        return positions[0], orientations[0]
 
     def set_local_pose(self, translation=None, orientation=None):
-        if self._prim_type == PrimType.CLOTH:
-            if self._dc is not None and self._dc.is_simulating():
-                self._set_local_pose_when_simulating(translation=translation, orientation=orientation)
-            else:
-                super().set_local_pose(translation=translation, orientation=orientation)
-        else:
-            if self._root_handle is not None and self._root_handle != _dynamic_control.INVALID_HANDLE and \
-                    self._dc is not None and self._dc.is_simulating():
-                self._set_local_pose_when_simulating(translation=translation, orientation=orientation)
-            else:
-                super().set_local_pose(translation=translation, orientation=orientation)
+        self._articulation_view.set_local_poses(translation[None, :], orientation[None, :])
         BoundingBoxAPI.clear()
 
-    def _get_local_pose_when_simulating(self):
-        """
-        Gets prim's pose with respect to the prim's local frame (it's parent frame) when simulation is running
-
-        Returns:
-            2-tuple:
-                - 3-array: (x,y,z) position in the local frame
-                - 4-array: (x,y,z,w) quaternion orientation in the local frame
-        """
-        parent_world_tf = UsdGeom.Xformable(get_prim_parent(self._prim)).ComputeLocalToWorldTransform(
-            Usd.TimeCode.Default()
-        )
-        world_position, world_orientation = self.get_position_orientation()
-        my_world_transform = tf_matrix_from_pose(translation=world_position,
-                                                 orientation=world_orientation[[3, 0, 1, 2]])
-        local_transform = np.matmul(np.linalg.inv(np.transpose(parent_world_tf)), my_world_transform)
-        transform = Gf.Transform()
-        transform.SetMatrix(Gf.Matrix4d(np.transpose(local_transform)))
-        calculated_translation = transform.GetTranslation()
-        calculated_orientation = transform.GetRotation().GetQuat()
-        return np.array(calculated_translation), gf_quat_to_np_array(calculated_orientation)[[1, 2, 3, 0]]
-
     def get_local_pose(self):
-        if self._prim_type == PrimType.CLOTH:
-            if self._dc is not None and self._dc.is_simulating():
-                return self._get_local_pose_when_simulating()
-            else:
-                return super().get_local_pose()
-        else:
-            if self._root_handle is not None and self._root_handle != _dynamic_control.INVALID_HANDLE and \
-                    self._dc is not None and self._dc.is_simulating():
-                return self._get_local_pose_when_simulating()
-            else:
-                return super().get_local_pose()
+        positions, orientations = self._articulation_view.get_local_poses()
+        return positions[0], orientations[0]
 
     # TODO: Is the omni joint damping (used for driving motors) same as dissipative joint damping (what we had in pb)?
     @property

From 323a4c1f17828c57156865b3566302cb2d057ba3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 14:32:58 -0800
Subject: [PATCH 03/76] Finish changes to entity prim

---
 omnigibson/prims/entity_prim.py         | 45 ++++++-------------------
 omnigibson/prims/rigid_prim.py          | 12 +++----
 omnigibson/robots/manipulation_robot.py |  1 +
 3 files changed, 17 insertions(+), 41 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index f9ddc3285..7d22787ac 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -2,14 +2,11 @@
 
 from omni.isaac.core.articulations import ArticulationView
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
-from omni.isaac.core.utils.transformations import tf_matrix_from_pose
-from omni.isaac.core.utils.types import DOFInfo
-from omni.isaac.dynamic_control import _dynamic_control
 from omni.isaac.core.utils.stage import get_current_stage
-from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema
+from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, PhysicsSchemaTools
 import omni
 
-from omni.isaac.core.utils.prims import get_prim_property, set_prim_property, get_prim_parent, get_prim_at_path
+from omni.isaac.core.utils.prims import get_prim_property, set_prim_property
 
 import omnigibson as og
 import omnigibson.utils.transform_utils as T
@@ -51,7 +48,7 @@ def __init__(
     ):
         # Other values that will be filled in at runtime
         self._root_link_name = None             # Name of the root link
-        self._n_dof = None                      # dof with dynamic control
+        self._n_dof = None
         self._links = None
         self._joints = None
         self._materials = None
@@ -355,27 +352,6 @@ def root_prim(self):
         # The root prim belongs to the link with name root_link_name
         return self._links[self.root_link_name].prim
 
-    @property
-    def handle(self):
-        """
-        Returns:
-            None or int: ID (articulation) handle assigned to this prim from dynamic_control interface. Note that
-                if this prim is not an articulation, it is None
-        """
-        return self._handle
-
-    @property
-    def root_handle(self):
-        """
-        Handle used by Isaac Sim's dynamic control module to reference the root body in this object.
-        Note: while self.handle may be 0 (i.e.: invalid articulation, i.e.: object with no joints), root_handle should
-            always be non-zero (i.e.: valid) if this object is initialized!
-
-        Returns:
-            int: ID handle assigned to this prim's root prim from dynamic_control interface
-        """
-        return self._root_handle
-
     @property
     def n_dof(self):
         """
@@ -518,8 +494,7 @@ def set_joint_positions(self, positions, indices=None, normalized=False, drive=F
                 motors or manual values to immediately set. Default is False, corresponding to an instantaneous
                 setting of the positions
         """
-        # Run sanity checks -- make sure our handle is initialized and that we are articulated
-        assert self._handle is not None, "handles are not initialized yet!"
+        # Run sanity checks -- make sure that we are articulated
         assert self.n_joints > 0, "Tried to call method not intended for entity prim with no joints!"
 
         # Possibly de-normalize the inputs
@@ -551,8 +526,7 @@ def set_joint_velocities(self, velocities, indices=None, normalized=False, drive
                 motors or manual values to immediately set. Default is False, corresponding to an instantaneous
                 setting of the velocities
         """
-        # Run sanity checks -- make sure our handle is initialized and that we are articulated
-        assert self._handle is not None, "handles are not initialized yet!"
+        # 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!"
 
         # Possibly de-normalize the inputs
@@ -580,8 +554,7 @@ def set_joint_efforts(self, efforts, indices=None, normalized=False):
             normalized (bool): Whether the inputted joint efforts should be interpreted as normalized values. Default
                 is False
         """
-        # Run sanity checks -- make sure our handle is initialized and that we are articulated
-        assert self._handle is not None, "handles are not initialized yet!"
+        # 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!"
 
         # Possibly de-normalize the inputs
@@ -1155,7 +1128,8 @@ def wake(self):
         Enable physics for this articulation
         """
         if self.articulated:
-            self._dc.wake_up_articulation(self._handle)
+            prim_id = PhysicsSchemaTools.sdfPathToInt(self.prim_path)
+            og.sim.psi.wake_up(og.sim.stage_id, prim_id)
         else:
             for link in self._links.values():
                 link.wake()
@@ -1165,7 +1139,8 @@ def sleep(self):
         Disable physics for this articulation
         """
         if self.articulated:
-            self._dc.sleep_articulation(self._handle)
+            prim_id = PhysicsSchemaTools.sdfPathToInt(self.prim_path)
+            og.sim.psi.put_to_sleep(og.sim.stage_id, prim_id)
         else:
             for link in self._links.values():
                 link.sleep()
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index c3bb50217..546b6ce81 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -1,11 +1,11 @@
-import trimesh.triangles
 from omni.isaac.core.utils.prims import get_prim_at_path, get_prim_parent
 from omni.isaac.core.utils.transformations import tf_matrix_from_pose
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
-from pxr import Gf, UsdPhysics, Usd, UsdGeom, PhysxSchema
+from pxr import Gf, UsdPhysics, Usd, UsdGeom, PhysxSchema, PhysicsSchemaTools
 import numpy as np
 from omni.isaac.dynamic_control import _dynamic_control
 
+import omnigibson as og
 from omnigibson.macros import gm, create_module_macros
 from omnigibson.prims.xform_prim import XFormPrim
 from omnigibson.prims.geom_prim import CollisionGeomPrim, VisualGeomPrim
@@ -658,15 +658,15 @@ def wake(self):
         """
         Enable physics for this rigid body
         """
-        if self.dc_is_accessible:
-            self._dc.wake_up_rigid_body(self._handle)
+        prim_id = PhysicsSchemaTools.sdfPathToInt(self.prim_path)
+        og.sim.psi.wake_up(og.sim.stage_id, prim_id)
 
     def sleep(self):
         """
         Disable physics for this rigid body
         """
-        if self.dc_is_accessible:
-            self._dc.sleep_rigid_body(self._handle)
+        prim_id = PhysicsSchemaTools.sdfPathToInt(self.prim_path)
+        og.sim.psi.put_to_sleep(og.sim.stage_id, prim_id)
 
     def _dump_state(self):
         # Grab pose from super class
diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index 05b569558..c99cc6553 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -1055,6 +1055,7 @@ def _get_assisted_grasp_joint_type(self, ag_obj, ag_link):
             return None
         
         # Otherwise, compute the joint type. We use a fixed joint unless the link is a non-fixed link.
+        # TODO: Make this not use the handle!
         joint_type = "FixedJoint"
         if ag_obj.root_link != ag_link:
             # We search up the tree path from the ag_link until we encounter the root (joint == 0) or a non fixed

From c1dab0a9fe748df3132ec45dd13f40dab8a3c49e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 15:00:53 -0800
Subject: [PATCH 04/76] Finish removing dc stuff from rigid prim

---
 omnigibson/prims/rigid_prim.py | 177 +++++++--------------------------
 1 file changed, 34 insertions(+), 143 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 546b6ce81..6d451a0cf 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -1,3 +1,4 @@
+from omni.isaac.core.prims import RigidPrimView
 from omni.isaac.core.utils.prims import get_prim_at_path, get_prim_parent
 from omni.isaac.core.utils.transformations import tf_matrix_from_pose
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
@@ -60,15 +61,9 @@ def __init__(
         load_config=None,
     ):
         # Other values that will be filled in at runtime
-        self._dc = None                     # Dynamic control interface
+        self._rigid_prim_view = None
         self._cs = None                     # Contact sensor interface
-        self._handle = None
-        self._contact_handle = None
         self._body_name = None
-        self._rigid_api = None
-        self._physx_rigid_api = None
-        self._physx_contact_report_api = None
-        self._mass_api = None
 
         self._visual_only = None
         self._collision_meshes = None
@@ -85,17 +80,12 @@ def _post_load(self):
         # run super first
         super()._post_load()
 
-        # Apply rigid body and mass APIs
-        self._rigid_api = UsdPhysics.RigidBodyAPI(self._prim) if self._prim.HasAPI(UsdPhysics.RigidBodyAPI) else \
-            UsdPhysics.RigidBodyAPI.Apply(self._prim)
-        self._physx_rigid_api = PhysxSchema.PhysxRigidBodyAPI(self._prim) if \
-            self._prim.HasAPI(PhysxSchema.PhysxRigidBodyAPI) else PhysxSchema.PhysxRigidBodyAPI.Apply(self._prim)
-        self._mass_api = UsdPhysics.MassAPI(self._prim) if self._prim.HasAPI(UsdPhysics.MassAPI) else \
-            UsdPhysics.MassAPI.Apply(self._prim)
+        # Create the view
+        self._rigid_prim_view = RigidPrimView(self._prim_path)
 
         # Only create contact report api if we're not visual only
         if not self._visual_only:
-            self._physx_contact_report_api_api = PhysxSchema.PhysxContactReportAPI(self._prim) if \
+            PhysxSchema.PhysxContactReportAPI(self._prim) if \
                 self._prim.HasAPI(PhysxSchema.PhysxContactReportAPI) else \
                 PhysxSchema.PhysxContactReportAPI.Apply(self._prim)
 
@@ -127,15 +117,12 @@ def _initialize(self):
         # Run super method first
         super()._initialize()
 
-        # Get dynamic control and contact sensing interfaces
-        self._dc = _dynamic_control.acquire_dynamic_control_interface()
-
         # Initialize all owned meshes
         for mesh_group in (self._collision_meshes, self._visual_meshes):
             for mesh in mesh_group.values():
                 mesh.initialize()
 
-        # We grab contact info for the first time before setting our internal handle, because this changes the dc handle
+        # Get contact info first
         if self.contact_reporting_enabled:
             self._cs.get_rigid_body_raw_data(self._prim_path)
 
@@ -216,7 +203,7 @@ def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
         """
-        self._handle = None if self.kinematic_only else self._dc.get_rigid_body(self._prim_path)
+        self._rigid_prim_view.initialize(og.sim.physics_sim_view)
 
     def contact_list(self):
         """
@@ -225,17 +212,12 @@ def contact_list(self):
         Returns:
             list of CsRawData: raw contact info for this rigid body
         """
-        # # Make sure we have the ability to grab contacts for this object
-        # assert self._physx_contact_report_api is not None, \
-        #     "Cannot grab contacts for this rigid prim without Physx's contact report API being added!"
+        # Make sure we have the ability to grab contacts for this object
         contacts = []
         if self.contact_reporting_enabled:
             raw_data = self._cs.get_rigid_body_raw_data(self._prim_path)
             for c in raw_data:
-                # contact sensor handles and dynamic articulation handles are not comparable
-                # every prim has a cs to convert (cs) handle to prim path (decode_body_name)
-                # but not every prim (e.g. groundPlane) has a dc to convert prim path to (dc) handle (get_rigid_body)
-                # so simpler to convert both handles (int) to prim paths (str) for comparison
+                # convert handles to prim paths for comparison
                 c = [*c] # CsRawData enforces body0 and body1 types to be ints, but we want strings
                 c[2] = self._cs.decode_body_name(c[2])
                 c[3] = self._cs.decode_body_name(c[3])
@@ -249,21 +231,14 @@ def set_linear_velocity(self, velocity):
         Args:
             velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,).
         """
-        if self.dc_is_accessible:
-            self._dc.set_rigid_body_linear_velocity(self._handle, velocity)
-        else:
-            self._rigid_api.GetVelocityAttr().Set(Gf.Vec3f(velocity.tolist()))
+        self._rigid_prim_view.set_linear_velocities(velocity[None, :])
 
     def get_linear_velocity(self):
         """
         Returns:
             np.ndarray: current linear velocity of the the rigid prim. Shape (3,).
         """
-        if self.dc_is_accessible:
-            lin_vel = np.array(self._dc.get_rigid_body_linear_velocity(self._handle))
-        else:
-            lin_vel = self._rigid_api.GetVelocityAttr().Get()
-        return np.array(lin_vel)
+        return self._rigid_prim_view.get_linear_velocities()[0]
 
     def set_angular_velocity(self, velocity):
         """
@@ -272,99 +247,41 @@ def set_angular_velocity(self, velocity):
         Args:
             velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,).
         """
-        if self.dc_is_accessible:
-            self._dc.set_rigid_body_angular_velocity(self._handle, velocity)
-        else:
-            self._rigid_api.GetAngularVelocityAttr().Set(Gf.Vec3f(velocity.tolist()))
+        self._rigid_prim_view.set_angular_velocities(velocity[None, :])
 
     def get_angular_velocity(self):
         """
         Returns:
             np.ndarray: current angular velocity of the the rigid prim. Shape (3,).
         """
-        if self.dc_is_accessible:
-            return np.array(self._dc.get_rigid_body_angular_velocity(self._handle))
-        else:
-            return np.array(self._rigid_api.GetAngularVelocityAttr().Get())
+        return self._rigid_prim_view.get_angular_velocities()[0]
 
     def set_position_orientation(self, position=None, orientation=None):
-        if self.dc_is_accessible:
-            current_position, current_orientation = self.get_position_orientation()
-            if position is None:
-                position = current_position
-            if orientation is None:
-                orientation = current_orientation
+        if position is not None:
+            position = position[None, :]
+        if orientation is not None:
             assert np.isclose(np.linalg.norm(orientation), 1, atol=1e-3), \
                 f"{self.prim_path} desired orientation {orientation} is not a unit quaternion."
-            pose = _dynamic_control.Transform(position, orientation)
-            self._dc.set_rigid_body_pose(self._handle, pose)
-        else:
-            # Call super method by default
-            super().set_position_orientation(position=position, orientation=orientation)
+            orientation = orientation[None, :]
+        self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation)
 
     def get_position_orientation(self):
-        if self.dc_is_accessible:
-            pose = self._dc.get_rigid_body_pose(self._handle)
-            pos, ori = np.asarray(pose.p), np.asarray(pose.r)
-        else:
-            # Call super method by default
-            pos, ori = super().get_position_orientation()
+        pos, ori = self._rigid_prim_view.get_world_poses()
 
         assert np.isclose(np.linalg.norm(ori), 1, atol=1e-3), \
             f"{self.prim_path} orientation {ori} is not a unit quaternion."
-        return pos, ori
+        return pos[0], ori[0]
 
     def set_local_pose(self, translation=None, orientation=None):
-        if self.dc_is_accessible:
-            current_translation, current_orientation = self.get_local_pose()
-            translation = current_translation if translation is None else translation
-            orientation = current_orientation if orientation is None else orientation
-            orientation = orientation[[3, 0, 1, 2]]  # Flip from x,y,z,w to w,x,y,z
-            local_transform = tf_matrix_from_pose(translation=translation, orientation=orientation)
-            parent_world_tf = UsdGeom.Xformable(get_prim_parent(self._prim)).ComputeLocalToWorldTransform(
-                Usd.TimeCode.Default()
-            )
-            my_world_transform = np.matmul(parent_world_tf, local_transform)
-            transform = Gf.Transform()
-            transform.SetMatrix(Gf.Matrix4d(np.transpose(my_world_transform)))
-            calculated_position = transform.GetTranslation()
-            calculated_orientation = transform.GetRotation().GetQuat()
-            self.set_position_orientation(
-                position=np.array(calculated_position), orientation=gf_quat_to_np_array(calculated_orientation)
-            )
-        else:
-            # Call super method by default
-            super().set_local_pose(translation=translation, orientation=orientation)
+        if translation is not None:
+            translation = translation[None, :]
+        if orientation is not None:
+            orientation = orientation[None, :]
+        self._articulation_view.set_local_poses(translation, orientation)
 
     def get_local_pose(self):
-        if self.dc_is_accessible:
-            parent_world_tf = UsdGeom.Xformable(get_prim_parent(self._prim)).ComputeLocalToWorldTransform(
-                Usd.TimeCode.Default()
-            )
-            world_position, world_orientation = self.get_position_orientation()
-            world_orientation = world_orientation[[3, 0, 1, 2]]  # Flip from x,y,z,w to w,x,y,z
-            my_world_transform = tf_matrix_from_pose(translation=world_position, orientation=world_orientation)
-            local_transform = np.matmul(np.linalg.inv(np.transpose(parent_world_tf)), my_world_transform)
-            transform = Gf.Transform()
-            transform.SetMatrix(Gf.Matrix4d(np.transpose(local_transform)))
-            calculated_translation = transform.GetTranslation()
-            calculated_orientation = transform.GetRotation().GetQuat()
-            pos, ori = np.array(calculated_translation), gf_quat_to_np_array(calculated_orientation)[[1, 2, 3, 0]] # Flip from w,x,y,z to x,y,z,w to
-        else:
-            # Call super method by default
-            pos, ori = super().get_local_pose()
-
-        return np.array(pos), np.array(ori)
-
-    @property
-    def handle(self):
-        """
-        Handle used by Isaac Sim's dynamic control module to reference this rigid prim
-
-        Returns:
-            int: ID handle assigned to this prim from dynamic_control interface
-        """
-        return self._handle
+        positions, orientations = self._articulation_view.get_local_poses()
+        return positions[0], orientations[0]
 
     @property
     def body_name(self):
@@ -453,15 +370,11 @@ def mass(self):
         Returns:
             float: mass of the rigid body in kg.
         """
-        raw_usd_mass = self._mass_api.GetMassAttr().Get()
-        # If our raw_usd_mass isn't specified, we check dynamic control if possible (sim is playing),
-        # otherwise we fallback to analytical computation of volume * density
-        if raw_usd_mass != 0:
-            mass = raw_usd_mass
-        elif self.dc_is_accessible:
-            mass = self.rigid_body_properties.mass
-        else:
-            mass = self.volume * self.density
+        mass = self._rigid_prim_view.get_masses()[0]
+
+        # Fallback to analytical computation of volume * density
+        if mass == 0:
+            return self.volume * self.density
 
         return mass
 
@@ -620,39 +533,17 @@ def contact_reporting_enabled(self):
         """
         return self._prim.HasAPI(PhysxSchema.PhysxContactReportAPI)
 
-    @property
-    def rigid_body_properties(self):
-        """
-        Returns:
-            None or RigidBodyProperty: Properties for this rigid body, if accessible. If they do not exist or
-                dc cannot be queried, this will return None
-        """
-        return self._dc.get_rigid_body_properties(self._handle) if self.dc_is_accessible else None
-
-    @property
-    def dc_is_accessible(self):
-        """
-        Checks if dynamic control interface is accessible (checks whether we have a dc handle for this body
-        and if dc is simulating)
-
-        Returns:
-            bool: Whether dc interface can be used or not
-        """
-        return self._handle is not None and self._dc.is_simulating() and not self.kinematic_only
-
     def enable_gravity(self):
         """
         Enables gravity for this rigid body
         """
-        self.set_attribute("physxRigidBody:disableGravity", False)
-        # self._dc.set_rigid_body_disable_gravity(self._handle, False)
+        self._rigid_prim_view.enable_gravities()
 
     def disable_gravity(self):
         """
         Disables gravity for this rigid body
         """
-        self.set_attribute("physxRigidBody:disableGravity", True)
-        # self._dc.set_rigid_body_disable_gravity(self._handle, True)
+        self._rigid_prim_view.disable_gravities()
 
     def wake(self):
         """

From b5796efc9a2f94fc85b4e21e5584728e4a998fa0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 15:01:00 -0800
Subject: [PATCH 05/76] Some fixes to entity prim

---
 omnigibson/prims/entity_prim.py | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 7d22787ac..3a3acbb92 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -778,7 +778,11 @@ def get_angular_velocity(self):
         return self.root_link.get_angular_velocity()
 
     def set_position_orientation(self, position=None, orientation=None):
-        self._articulation_view.set_world_poses(position[None, :], orientation[None, :])
+        if position is not None:
+            position = position[None, :]
+        if orientation is not None:
+            orientation = orientation[None, :]
+        self._articulation_view.set_world_poses(position, orientation)
         BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
@@ -786,7 +790,11 @@ def get_position_orientation(self):
         return positions[0], orientations[0]
 
     def set_local_pose(self, translation=None, orientation=None):
-        self._articulation_view.set_local_poses(translation[None, :], orientation[None, :])
+        if translation is not None:
+            translation = translation[None, :]
+        if orientation is not None:
+            orientation = orientation[None, :]
+        self._articulation_view.set_local_poses(translation, orientation)
         BoundingBoxAPI.clear()
 
     def get_local_pose(self):

From 6bff1d706d438065d45745b54c83fb6a4dac263f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 15:01:09 -0800
Subject: [PATCH 06/76] Some fixes and questions on Tiago

---
 omnigibson/robots/tiago.py | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py
index d727ee429..ee61055a7 100644
--- a/omnigibson/robots/tiago.py
+++ b/omnigibson/robots/tiago.py
@@ -3,6 +3,7 @@
 import numpy as np
 from pxr import Gf
 
+import omnigibson as og
 from omnigibson.macros import gm
 import omnigibson.utils.transform_utils as T
 from omnigibson.macros import create_module_macros
@@ -696,13 +697,9 @@ def arm_workspace_range(self):
         }
 
     def get_position_orientation(self):
-        # If the simulator is playing, return the pose of the base_footprint link frame
-        if self._dc is not None and self._dc.is_simulating():
-            return self.base_footprint_link.get_position_orientation()
-
-        # Else, return the pose of the robot frame
-        else:
-            return super().get_position_orientation()
+        # TODO: This is a bad idea. If we need the pose to be here, let's put the base there.
+        # We shouldn't have a different behavior for only this object.
+        return self.base_footprint_link.get_position_orientation()
 
     def set_position_orientation(self, position=None, orientation=None):
         current_position, current_orientation = self.get_position_orientation()
@@ -713,8 +710,9 @@ def set_position_orientation(self, position=None, orientation=None):
         assert np.isclose(np.linalg.norm(orientation), 1, atol=1e-3), \
             f"{self.name} desired orientation {orientation} is not a unit quaternion."
 
+        # TODO: Reconsider the need for this. Why can't these behaviors be unified? Does the joint really need to move?
         # If the simulator is playing, set the 6 base joints to achieve the desired pose of base_footprint link frame
-        if self._dc is not None and self._dc.is_simulating():
+        if og.sim.is_playing():
             # Find the relative transformation from base_footprint_link ("base_footprint") frame to root_link
             # ("base_footprint_x") frame. Assign it to the 6 1DoF joints that control the base.
             # Note that the 6 1DoF joints are originated from the root_link ("base_footprint_x") frame.

From cb7e298ef3bdf1fa4ac15c4db43088ba1165c4f5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 15:03:03 -0800
Subject: [PATCH 07/76] Update scene_base.py

---
 omnigibson/scenes/scene_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/scenes/scene_base.py b/omnigibson/scenes/scene_base.py
index 87d84636a..62e155776 100644
--- a/omnigibson/scenes/scene_base.py
+++ b/omnigibson/scenes/scene_base.py
@@ -143,7 +143,7 @@ def object_registry_unique_keys(self):
         """
         Returns:
             list of str: Keys with which to index into the object registry. These should be valid public attributes of
-                prims that we can use as unique IDs to reference prims, e.g., prim.prim_path, prim.name, prim.handle, etc.
+                prims that we can use as unique IDs to reference prims, e.g., prim.prim_path, prim.name, etc.
         """
         return ["name", "prim_path", "uuid"]
 

From bb8b6490f11f7dd7248ddf2125c86f3b20f95039 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 16:38:47 -0800
Subject: [PATCH 08/76] Start implementing JointPrim stuff with extended
 articulation view

---
 omnigibson/prims/entity_prim.py               |  12 +-
 omnigibson/prims/joint_prim.py                | 115 +++++++-----------
 .../utils/extended_articulation_view.py       |  65 ++++++++++
 3 files changed, 114 insertions(+), 78 deletions(-)
 create mode 100644 omnigibson/utils/extended_articulation_view.py

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 3a3acbb92..ce66c5a94 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -1,6 +1,5 @@
 import numpy as np
 
-from omni.isaac.core.articulations import ArticulationView
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
 from omni.isaac.core.utils.stage import get_current_stage
 from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, PhysicsSchemaTools
@@ -15,6 +14,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.extended_articulation_view import ExtendedArticulationView
 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
@@ -90,7 +90,7 @@ def _load(self):
 
     def _post_load(self):
         # Prepare the articulation view (at this point only working via the USD interface)
-        self._articulation_view = ArticulationView(self._prim_path)
+        self._articulation_view = ExtendedArticulationView(self._prim_path)
 
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
@@ -234,13 +234,13 @@ def update_joints(self):
                 for i in range(self._articulation_view._metadata.joint_count):
                     joint_name = self._articulation_view._metadata.joint_names[i]
                     joint_dof_offset = self._articulation_view._metadata.joint_dof_offsets[i]
-                    joint_path = self._articulation_view._metadata.dof_paths[0][joint_dof_offset]
+                    joint_path = self._articulation_view.dof_paths[0][joint_dof_offset]
                     # Only add the joint if it's not fixed (i.e.: it has DOFs > 0)
                     if self._articulation_view._metadata.joint_dof_counts[i] > 0:
                         joint = JointPrim(
                             prim_path=joint_path,
                             name=f"{self._name}:joint_{joint_name}",
-                            articulation=self._articulation_view,
+                            articulation_view=self._articulation_view,
                         )
                         joint.initialize()
                         self._joints[joint_name] = joint
@@ -273,8 +273,8 @@ def _update_joint_limits(self):
             else:
                 assert not self.initialized, \
                     "Cannot update joint limits for a non-uniformly scaled object when already initialized."
-                for link_name, link in self.links.items():
-                    if joint.parent_name == link_name:
+                for link in self.links.values():
+                    if joint.body0 == link.prim_path:
                         # Find the parent link frame orientation in the object frame
                         _, link_local_orn = link.get_local_pose()
 
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 0ab38b477..dc569ec41 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -79,10 +79,10 @@ def __init__(
         prim_path,
         name,
         load_config=None,
-        articulation=None,
+        articulation_view=None,
     ):
         # Grab dynamic control reference and set properties
-        self._art = articulation
+        self._articulation_view = articulation_view
 
         # Other values that will be filled in at runtime
         self._joint_type = None
@@ -92,11 +92,10 @@ def __init__(
         self._driven = None
 
         # The following values will only be valid if this joint is part of an articulation
-        self._dc = None
-        self._handle = None
         self._n_dof = None
+        self._joint_idx = None
+        self._joint_dof_offset = None
         self._joint_name = None
-        self._dof_handles = None
 
         # Run super method
         super().__init__(
@@ -146,31 +145,16 @@ def _initialize(self):
         # Always run super first
         super()._initialize()
 
+        # Update the joint indices etc.
+        self.update_handles()
+
         # Initialize dynamic control references if this joint is articulated
         if self.articulated:
-            self._dc = _dynamic_control.acquire_dynamic_control_interface()
-            # TODO: A bit hacky way to get the joint handle, ideally we'd simply do dc.get_joint(), but this doesn't seem to work as expected?
-            for i in range(self._dc.get_articulation_joint_count(self._art)):
-                joint_handle = self._dc.get_articulation_joint(self._art, i)
-                joint_path = self._dc.get_joint_path(joint_handle)
-                if joint_path == self._prim_path:
-                    self._handle = joint_handle
-                    break
-            assert self._handle is not None, f"Did not find valid articulated joint with path: {self._prim_path}"
-
-            # Grab DOF info / handles
-            self._joint_name = self._dc.get_joint_name(self._handle)
-            self._n_dof = self._dc.get_joint_dof_count(self._handle)
-            self._dof_handles = []
-            self._dof_properties = []
             control_types = []
-            for i in range(self._n_dof):
-                dof_handle = self._dc.get_joint_dof(self._handle, i)
-                dof_props = self._dc.get_dof_properties(dof_handle)
-                self._dof_handles.append(dof_handle)
-                self._dof_properties.append(dof_props)
+            stifnesses, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)
+            for i, (kp, kd) in enumerate(zip(stifnesses[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
-                kp, kd = dof_props.stiffness, dof_props.damping
+                # TODO: Maybe assert mutual exclusiveness here?
                 if not self._driven:
                     control_type = ControlType.NONE
                 elif kp == 0.0:
@@ -187,15 +171,13 @@ def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
         """
-        # TODO: A bit hacky way to get the joint handle, ideally we'd simply do dc.get_joint(), but this doesn't seem to work as expected?
-        self._handle = None
-        for i in range(self._dc.get_articulation_joint_count(self._art)):
-            joint_handle = self._dc.get_articulation_joint(self._art, i)
-            joint_path = self._dc.get_joint_path(joint_handle)
-            if joint_path == self._prim_path:
-                self._handle = joint_handle
-                break
-
+        # 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)
+        self._joint_idx = list(self._articulation_view._metadata.joint_dof_offsets).index(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.
@@ -224,11 +206,13 @@ def set_control_type(self, control_type, kp=None, kd=None):
             kp, kd = 0.0, 0.0
 
         # Set values
-        if self._dc:
-            for dof_handle, dof_property in zip(self._dof_handles, self._dof_properties):
-                dof_property.stiffness = kp
-                dof_property.damping = kd
-                self._dc.set_dof_properties(dof_handle, dof_property)
+        kps = np.full((1, self._n_dof), kp)
+        kds = np.full((1, self._n_dof), kd)
+        self._articulation_view.set_gains(kps=kps, kds=kds, joint_indices=self.dof_indices, save_to_usd=True)
+
+        # Assert no weirdness happening around the save_to_usd business.
+        kps, kds = self._articulation_view.get_gains(joint_indices=self.dof_indices)
+        assert np.allclose(kps, kp) and np.allclose(kds, kd), "Something went wrong with setting the gains!"
 
         # Update control type
         self._control_type = control_type
@@ -281,26 +265,6 @@ def body1(self, body1):
         assert is_prim_path_valid(body1), f"Invalid body1 path specified: {body1}"
         self._prim.GetRelationship("physics:body1").SetTargets([Sdf.Path(body1)])
 
-    @property
-    def parent_name(self):
-        """
-        Gets this joint's parent body name, if it exists
-
-        Returns:
-            str: Joint's parent body name
-        """
-        return self._dc.get_rigid_body_name(self._dc.get_joint_parent_body(self._handle))
-
-    @property
-    def child_name(self):
-        """
-        Gets this joint's child body name, if it exists
-
-        Returns:
-            str: Joint's child body name
-        """
-        return self._dc.get_rigid_body_name(self._dc.get_joint_child_body(self._handle))
-
     @property
     def local_orientation(self):
         """
@@ -386,8 +350,7 @@ def max_velocity(self, vel):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        self._dof_properties[0].max_velocity = vel
-        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])
+        self._articulation_view.set_max
 
     @property
     def max_effort(self):
@@ -426,7 +389,8 @@ 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!"
-        return self._dof_properties[0].stiffness
+        stiffnesses, _ = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0]
+        return stiffnesses[0][0]
 
     @stiffness.setter
     def stiffness(self, stiffness):
@@ -438,8 +402,7 @@ def stiffness(self, stiffness):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        self._dof_properties[0].stiffness = stiffness
-        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])
+        self._articulation_view.set_gains(kps=np.array([[stiffness]]), joint_indices=self.dof_indices)
 
     @property
     def damping(self):
@@ -451,7 +414,8 @@ 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!"
-        return self._dof_properties[0].damping
+        _, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0]
+        return dampings[0][0]
 
     @damping.setter
     def damping(self, damping):
@@ -463,8 +427,7 @@ def damping(self, damping):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        self._dof_properties[0].damping = damping
-        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])
+        self._articulation_view.set_gains(kds=np.array([[damping]]), joint_indices=self.dof_indices)
 
     @property
     def friction(self):
@@ -474,7 +437,7 @@ def friction(self):
         Returns:
             float: friction for this joint
         """
-        return self.get_attribute("physxJoint:jointFriction")
+        return self._articulation_view.get_friction_coefficients(joint_indices=self.dof_indices)[0][0]
 
     @friction.setter
     def friction(self, friction):
@@ -484,7 +447,7 @@ def friction(self, friction):
         Args:
             friction (float): friction to set
         """
-        self.set_attribute("physxJoint:jointFriction", friction)
+        self._articulation_view.set_friction_coefficients(np.array([[friction]]), joint_indices=self.dof_indices)[0][0]
 
     @property
     def lower_limit(self):
@@ -498,7 +461,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._dof_properties[0].lower, self._dof_properties[0].upper
+        raw_pos_lower, raw_pos_upper = self._articulation_view.get_dof_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
@@ -531,7 +494,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._dof_properties[0].lower, self._dof_properties[0].upper
+        raw_pos_lower, raw_pos_upper = self._articulation_view.get_dof_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
@@ -595,6 +558,14 @@ def n_dof(self):
             int: Number of degrees of freedom this joint has
         """
         return self._n_dof
+    
+    @property
+    def dof_indices(self):
+        """
+        Returns:
+            list of int: Indices of this joint's DOFs in the parent articulation's DOF array
+        """
+        return list(range(self._joint_dof_offset, self._joint_dof_offset + self._n_dof))
 
     @property
     def articulated(self):
diff --git a/omnigibson/utils/extended_articulation_view.py b/omnigibson/utils/extended_articulation_view.py
new file mode 100644
index 000000000..42af3c6f5
--- /dev/null
+++ b/omnigibson/utils/extended_articulation_view.py
@@ -0,0 +1,65 @@
+from typing import List, Optional, Tuple, Union
+import carb
+import omni.timeline
+from omni.isaac.core.utils.prims import get_prim_at_path
+from pxr import PhysxSchema, Usd, UsdGeom, UsdPhysics
+import numpy as np
+import torch
+import warp as wp
+
+from omni.isaac.core.articulations import ArticulationView
+
+class ExtendedArticulationView(ArticulationView):
+    """ArticulationView with some additional functionality implemented."""
+
+    def set_joint_limits(
+        self,
+        values: Union[np.ndarray, torch.Tensor],
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+    ) -> None:
+        """Sets joint limits for articulation joints in the view.
+
+        Args:
+            values (Union[np.ndarray, torch.Tensor, wp.array]): joint limits for articulations in the view. shape (M, K, 2).
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to manipulate. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to manipulate. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
+            new_values = self._physics_view.get_dof_limits()
+            values = self._backend_utils.move_data(values, device="cpu")
+            new_values = self._backend_utils.assign(
+                values,
+                new_values,
+                [self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices],
+            )
+            self._physics_view.set_dof_limits(new_values, indices)
+        else:
+            indices = self._backend_utils.to_list(
+                self._backend_utils.resolve_indices(indices, self.count, self._device)
+            )
+            joint_indices = self._backend_utils.to_list(
+                self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            )
+            values = self._backend_utils.to_list(values)
+            articulation_read_idx = 0
+            for i in indices:
+                dof_read_idx = 0
+                for dof_index in joint_indices:
+                    prim = get_prim_at_path(self._dof_paths[i][dof_index])
+                    prim.GetAttribute("physics:lowerLimit").Set(values[articulation_read_idx][dof_read_idx][0])
+                    prim.GetAttribute("physics:upperLimit").Set(values[articulation_read_idx][dof_read_idx][1])
+                    dof_read_idx += 1
+                articulation_read_idx += 1
+        return
\ No newline at end of file

From 17572313aeadd3ae379db7176c1335559b0f7de0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 6 Dec 2023 16:39:10 -0800
Subject: [PATCH 09/76] Remove simulator DC handle

---
 omnigibson/simulator.py | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py
index 8204917e0..f33a1d847 100644
--- a/omnigibson/simulator.py
+++ b/omnigibson/simulator.py
@@ -12,7 +12,6 @@
 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.physx.bindings._physx import ContactEventType, SimulationEvent
 import omni.kit.loop._loop as omni_loop
 from pxr import Usd, PhysicsSchemaTools, UsdUtils
@@ -806,14 +805,6 @@ def __del__(self):
         Simulator._world_initialized = None
         return
 
-    @property
-    def dc(self):
-        """
-        Returns:
-            _dynamic_control.DynamicControl: Dynamic control (dc) interface
-        """
-        return self._dynamic_control
-
     @property
     def pi(self):
         """

From 1d923354a8e636422495ea446edfd96acda4d385 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 8 Dec 2023 16:18:47 -0800
Subject: [PATCH 10/76] Implement the remaining articulation view features

---
 omnigibson/prims/joint_prim.py | 60 +++++++++++++++-------------------
 1 file changed, 26 insertions(+), 34 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index dc569ec41..c6aaf6af4 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -129,7 +129,7 @@ def _post_load(self):
         if self.is_single_dof:
             state_type = "angular" if self._joint_type == JointType.JOINT_REVOLUTE else "linear"
             # We MUST already have the joint state API defined beforehand in the USD
-            # This is because dc complains if we try to add physx APIs AFTER a simulation step occurs, which
+            # This is because physx complains if we try to add physx APIs AFTER a simulation step occurs, which
             # happens because joint prims are usually created externally during an EntityPrim's initialization phase
             assert self._prim.HasAPI(PhysxSchema.JointStateAPI), \
                 "Revolute or Prismatic joints must already have JointStateAPI added!"
@@ -339,6 +339,7 @@ def max_velocity(self):
         raw_vel = self._dof_properties[0].max_velocity
         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
+        # TODO: Implement this
 
     @max_velocity.setter
     def max_velocity(self, vel):
@@ -350,7 +351,7 @@ def max_velocity(self, vel):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        self._articulation_view.set_max
+        # TODO: Implement this
 
     @property
     def max_effort(self):
@@ -363,7 +364,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_force = self._dof_properties[0].max_effort
+        raw_force = self._articulation_view.get_max_efforts(joint_indices=self.dof_indices)[0][0]
         return m.DEFAULT_MAX_EFFORT if raw_force is None or np.abs(raw_force) > m.INF_EFFORT_THRESHOLD else raw_force
 
     @max_effort.setter
@@ -376,8 +377,7 @@ def max_effort(self, force):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        self._dof_properties[0].max_effort = force
-        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])
+        self._articulation_view.set_max_efforts(np.array([[force]]), joint_indices=self.dof_indices)
 
     @property
     def stiffness(self):
@@ -476,9 +476,8 @@ def lower_limit(self, lower_limit):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        # Set dc properties
-        self._dof_properties[0].lower = lower_limit
-        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])
+        # TODO: Implement this w/ articulation view
+
         # Set USD properties
         lower_limit = T.rad2deg(lower_limit) if self.is_revolute else lower_limit
         self.set_attribute("physics:lowerLimit", lower_limit)
@@ -509,9 +508,8 @@ def upper_limit(self, upper_limit):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        # Set dc properties
-        self._dof_properties[0].upper = upper_limit
-        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])
+        # TODO: Implement this w/ articulation view
+
         # Set USD properties
         upper_limit = T.rad2deg(upper_limit) if self.is_revolute else upper_limit
         self.set_attribute("physics:upperLimit", upper_limit)
@@ -616,12 +614,9 @@ def get_state(self, normalized=False):
         self.assert_articulated()
 
         # Grab raw states
-        pos, vel, effort = np.zeros(self.n_dof), np.zeros(self.n_dof), np.zeros(self.n_dof)
-        for i, dof_handle in enumerate(self._dof_handles):
-            dof_state = self._dc.get_dof_state(dof_handle, _dynamic_control.STATE_ALL)
-            pos[i] = dof_state.pos
-            vel[i] = dof_state.vel
-            effort[i] = dof_state.effort
+        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:
@@ -645,10 +640,9 @@ def get_target(self, normalized=False):
         self.assert_articulated()
 
         # Grab raw states
-        pos, vel = np.zeros(self.n_dof), np.zeros(self.n_dof)
-        for i, dof_handle in enumerate(self._dof_handles):
-            pos[i] = self._dc.get_dof_position_target(dof_handle)
-            vel[i] = self._dc.get_dof_velocity_target(dof_handle)
+        targets = self._articulation_view.get_applied_actions()
+        pos = targets.joint_positions
+        vel = targets.joint_velocities
 
         # Potentially normalize if requested
         if normalized:
@@ -766,13 +760,12 @@ def set_pos(self, pos, normalized=False, drive=False):
             pos = self._denormalize_pos(pos)
 
         # Set the DOF(s) in this joint
-        for dof_handle, p in zip(self._dof_handles, pos):
-            if not drive:
-                self._dc.set_dof_position(dof_handle, p)
-                BoundingBoxAPI.clear()
+        if not drive:
+            self._articulation_view.set_joint_positions(positions=pos, joint_indices=self.dof_indices)
+            BoundingBoxAPI.clear()
 
-            # We set the position target in either case
-            self._dc.set_dof_position_target(dof_handle, p)
+        # Also set the target
+        self._articulation_view.set_joint_position_targets(positions=pos, joint_indices=self.dof_indices)
 
     def set_vel(self, vel, normalized=False, drive=False):
         """
@@ -802,11 +795,11 @@ def set_vel(self, vel, normalized=False, drive=False):
             vel = self._denormalize_vel(vel)
 
         # Set the DOF(s) in this joint
-        for dof_handle, v in zip(self._dof_handles, vel):
-            if not drive:
-                self._dc.set_dof_velocity(dof_handle, v)
-            # We set the target in either case
-            self._dc.set_dof_velocity_target(dof_handle, v)
+        if not drive:
+            self._articulation_view.set_joint_velocities(velocities=vel, joint_indices=self.dof_indices)
+
+        # Also set the target
+        self._articulation_view.set_joint_velocity_targets(velocities=vel, joint_indices=self.dof_indices)
 
     def set_effort(self, effort, normalized=False):
         """
@@ -830,8 +823,7 @@ def set_effort(self, effort, normalized=False):
             effort = self._denormalize_effort(effort)
 
         # Set the DOF(s) in this joint
-        for dof_handle, e in zip(self._dof_handles, effort):
-            self._dc.set_dof_effort(dof_handle, e)
+        self._articulation_view.set_joint_efforts(velocities=effort, joint_indices=self.dof_indices)
 
     def keep_still(self):
         """

From e23f42cb27be887c443dc243103af96285332df6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Mon, 11 Dec 2023 15:47:14 -0800
Subject: [PATCH 11/76] Finish implementing articulationview stuff

---
 omnigibson/prims/joint_prim.py                |  21 +--
 .../utils/extended_articulation_view.py       | 175 +++++++++++++++++-
 2 files changed, 178 insertions(+), 18 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index c6aaf6af4..35d0b898e 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -336,10 +336,9 @@ 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._dof_properties[0].max_velocity
+        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
-        # TODO: Implement this
 
     @max_velocity.setter
     def max_velocity(self, vel):
@@ -351,7 +350,7 @@ def max_velocity(self, vel):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        # TODO: Implement this
+        self._articulation_view.set_max_velocities(np.array([[vel]]), joint_indices=self.dof_indices)
 
     @property
     def max_effort(self):
@@ -461,7 +460,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_dof_limits(joint_indices=self.dof_indices).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
@@ -476,11 +475,7 @@ def lower_limit(self, lower_limit):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        # TODO: Implement this w/ articulation view
-
-        # Set USD properties
-        lower_limit = T.rad2deg(lower_limit) if self.is_revolute else lower_limit
-        self.set_attribute("physics:lowerLimit", lower_limit)
+        self._articulation_view.set_joint_limits(np.array([[lower_limit, self.upper_limit]]), joint_indices=self.dof_indices)
 
     @property
     def upper_limit(self):
@@ -493,7 +488,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_dof_limits(joint_indices=self.dof_indices).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
@@ -508,11 +503,7 @@ def upper_limit(self, upper_limit):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        # TODO: Implement this w/ articulation view
-
-        # Set USD properties
-        upper_limit = T.rad2deg(upper_limit) if self.is_revolute else upper_limit
-        self.set_attribute("physics:upperLimit", upper_limit)
+        self._articulation_view.set_joint_limits(np.array([[self.lower_limit, upper_limit]]), joint_indices=self.dof_indices)
 
     @property
     def has_limit(self):
diff --git a/omnigibson/utils/extended_articulation_view.py b/omnigibson/utils/extended_articulation_view.py
index 42af3c6f5..30d297dcb 100644
--- a/omnigibson/utils/extended_articulation_view.py
+++ b/omnigibson/utils/extended_articulation_view.py
@@ -6,9 +6,12 @@
 import numpy as np
 import torch
 import warp as wp
+import math
 
 from omni.isaac.core.articulations import ArticulationView
 
+DEG2RAD = math.pi / 180.0
+
 class ExtendedArticulationView(ArticulationView):
     """ArticulationView with some additional functionality implemented."""
 
@@ -49,6 +52,7 @@ def set_joint_limits(
             indices = self._backend_utils.to_list(
                 self._backend_utils.resolve_indices(indices, self.count, self._device)
             )
+            dof_types = self._backend_utils.to_list(self.get_dof_types())
             joint_indices = self._backend_utils.to_list(
                 self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
             )
@@ -57,9 +61,174 @@ def set_joint_limits(
             for i in indices:
                 dof_read_idx = 0
                 for dof_index in joint_indices:
+                    dof_val = values[articulation_read_idx][dof_read_idx]
+                    if dof_types[dof_index] == omni.physics.tensors.DofType.Rotation:
+                        dof_val /= DEG2RAD
                     prim = get_prim_at_path(self._dof_paths[i][dof_index])
-                    prim.GetAttribute("physics:lowerLimit").Set(values[articulation_read_idx][dof_read_idx][0])
-                    prim.GetAttribute("physics:upperLimit").Set(values[articulation_read_idx][dof_read_idx][1])
+                    prim.GetAttribute("physics:lowerLimit").Set(dof_val[0])
+                    prim.GetAttribute("physics:upperLimit").Set(dof_val[1])
                     dof_read_idx += 1
                 articulation_read_idx += 1
-        return
\ No newline at end of file
+        return
+    
+    def get_joint_limits(
+        self,
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        clone: bool = True,
+    ) -> Union[np.ndarray, torch.Tensor, wp.array]:
+        """Gets joint limits for articulation in the view.
+
+        Args:
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to query. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to query. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+            clone (Optional[bool]): True to return a clone of the internal buffer. Otherwise False. Defaults to True.
+
+        Returns:
+            Union[np.ndarray, torch.Tensor, wp.indexedarray]: joint limits for articulations in the view. shape (M, K).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return None
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            values = self._backend_utils.move_data(self._physics_view.get_dof_limits(), self._device)
+            if clone:
+                values = self._backend_utils.clone_tensor(values, device=self._device)
+            result = values[
+                self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices
+            ]
+            return result
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            dof_types = self._backend_utils.to_list(self.get_dof_types())
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            values = np.zeros(shape=(indices.shape[0], joint_indices.shape[0], 2), dtype="float32")
+            articulation_write_idx = 0
+            indices = self._backend_utils.to_list(indices)
+            joint_indices = self._backend_utils.to_list(joint_indices)
+            for i in indices:
+                dof_write_idx = 0
+                for dof_index in joint_indices:
+                    prim = get_prim_at_path(self._dof_paths[i][dof_index])
+                    values[articulation_write_idx][dof_write_idx][0] = prim.GetAttribute("physics:lowerLimit").Get()
+                    values[articulation_write_idx][dof_write_idx][1] = prim.GetAttribute("physics:upperLimit").Get()
+                    if dof_types[dof_index] == omni.physics.tensors.DofType.Rotation:
+                        values[articulation_write_idx][dof_write_idx] = values[articulation_write_idx][dof_write_idx] * DEG2RAD
+                    dof_write_idx += 1
+                articulation_write_idx += 1
+            values = self._backend_utils.convert(values, dtype="float32", device=self._device, indexed=True)
+            return values
+        
+    def set_max_velocities(
+        self,
+        values: Union[np.ndarray, torch.Tensor, wp.array],
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+    ) -> None:
+        """Sets maximum velocities for articulation in the view.
+
+        Args:
+            values (Union[np.ndarray, torch.Tensor, wp.array]): maximum velocities for articulations in the view. shape (M, K).
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to manipulate. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to manipulate. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
+            new_values = self._physics_view.get_dof_max_velocities()
+            new_values = self._backend_utils.assign(
+                self._backend_utils.move_data(values, device="cpu"),
+                new_values,
+                [self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices],
+            )
+            self._physics_view.set_dof_max_velocities(new_values, indices)
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            articulation_read_idx = 0
+            indices = self._backend_utils.to_list(indices)
+            joint_indices = self._backend_utils.to_list(joint_indices)
+            values = self._backend_utils.to_list(values)
+            for i in indices:
+                dof_read_idx = 0
+                for dof_index in joint_indices:
+                    prim = PhysxSchema.PhysxJointAPI(get_prim_at_path(self._dof_paths[i][dof_index]))
+                    if not prim.GetMaxJointVelocityAttr():
+                        prim.CreateMaxJointVelocityAttr().Set(values[articulation_read_idx][dof_read_idx])
+                    else:
+                        prim.GetMaxJointVelocityAttr().Set(values[articulation_read_idx][dof_read_idx])
+                    dof_read_idx += 1
+                articulation_read_idx += 1
+        return
+
+    def get_max_velocities(
+        self,
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        clone: bool = True,
+    ) -> Union[np.ndarray, torch.Tensor, wp.indexedarray]:
+        """Gets maximum velocities for articulation in the view.
+
+        Args:
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to query. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to query. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+            clone (Optional[bool]): True to return a clone of the internal buffer. Otherwise False. Defaults to True.
+
+        Returns:
+            Union[np.ndarray, torch.Tensor, wp.indexedarray]: maximum velocities for articulations in the view. shape (M, K).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return None
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
+            max_velocities = self._physics_view.get_dof_max_velocities()
+            if clone:
+                max_velocities = self._backend_utils.clone_tensor(max_velocities, device="cpu")
+            result = self._backend_utils.move_data(
+                max_velocities[
+                    self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices
+                ],
+                device=self._device,
+            )
+            return result
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            max_velocities = np.zeros(shape=(indices.shape[0], joint_indices.shape[0]), dtype="float32")
+            indices = self._backend_utils.to_list(indices)
+            joint_indices = self._backend_utils.to_list(joint_indices)
+            articulation_write_idx = 0
+            for i in indices:
+                dof_write_idx = 0
+                for dof_index in joint_indices:
+                    prim = PhysxSchema.PhysxJointAPI(get_prim_at_path(self._dof_paths[i][dof_index]))
+                    max_velocities[articulation_write_idx][dof_write_idx] = prim.GetMaxJointVelocityAttr().Get()
+                    dof_write_idx += 1
+                articulation_write_idx += 1
+            max_velocities = self._backend_utils.convert(max_velocities, dtype="float32", device=self._device, indexed=True)
+            return max_velocities
\ No newline at end of file

From 76f1165c65bd669fa13a7ca9bbf3993aa34e4009 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Mon, 11 Dec 2023 16:08:28 -0800
Subject: [PATCH 12/76] Use a joint graph to filter AG candidates

---
 omnigibson/prims/entity_prim.py         | 21 +++++++++++++++
 omnigibson/robots/manipulation_robot.py | 34 ++++++++++---------------
 2 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index ce66c5a94..fa7e7aa76 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -1,4 +1,5 @@
 import numpy as np
+import networkx as nx
 
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
 from omni.isaac.core.utils.stage import get_current_stage
@@ -417,6 +418,26 @@ def links(self):
             dict: Dictionary mapping link names (str) to link prims (RigidPrim) owned by this articulation
         """
         return self._links
+    
+    @property
+    def articulation_tree(self):
+        """
+        Get a graph of the articulation tree, where nodes are links (RigidPrim) and edges
+        correspond to joints (JointPrim), where the JointPrim is accessible on the `joint`
+        data field of the edge.
+        """
+        G = nx.DiGraph()
+        rename_later = {}
+        for link in self.links.values():
+            prim_path = link.prim_path
+            G.add_node(prim_path)
+            rename_later[prim_path] = link
+        for joint in self.joints.values():
+            if joint.body0 not in G.nodes or joint.body1 not in G.nodes:
+                continue
+            G.add_edge(joint.body0, joint.body1, joint=joint)
+        nx.relabel_nodes(G, rename_later)
+        return G
 
     @property
     def materials(self):
diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index c99cc6553..7a8234856 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -1,6 +1,7 @@
 from abc import abstractmethod
 from collections import namedtuple
 import numpy as np
+import networkx as nx
 
 import omnigibson as og
 from omnigibson.macros import gm, create_module_macros
@@ -783,12 +784,11 @@ def _calculate_in_hand_object_rigid(self, arm="default"):
 
         candidate_data = []
         for prim_path in candidates_set:
-            # Calculate position of the object link
-            # Note: this assumes the simulator is playing!
-            rb_handle = self._dc.get_rigid_body(prim_path)
-            pose = self._dc.get_rigid_body_pose(rb_handle)
-            link_pos = np.asarray(pose.p)
-            dist = np.linalg.norm(np.array(link_pos) - np.array(gripper_center_pos))
+            # Calculate position of the object link. Only allow this for objects currently.
+            obj_prim_path, link_name = prim_path.rsplit("/", 1)[-1]
+            candidate_obj = og.sim.scene.object_registry("prim_path", obj_prim_path)
+            candidate_link = candidate_obj.links[link_name]
+            dist = np.linalg.norm(np.array(candidate_link.get_position()) - np.array(gripper_center_pos))
             candidate_data.append((prim_path, dist))
 
         candidate_data = sorted(candidate_data, key=lambda x: x[-1])
@@ -1055,22 +1055,14 @@ def _get_assisted_grasp_joint_type(self, ag_obj, ag_link):
             return None
         
         # Otherwise, compute the joint type. We use a fixed joint unless the link is a non-fixed link.
-        # TODO: Make this not use the handle!
+        # A link is non-fixed if it has any non-fixed parent joints.
         joint_type = "FixedJoint"
-        if ag_obj.root_link != ag_link:
-            # We search up the tree path from the ag_link until we encounter the root (joint == 0) or a non fixed
-            # joint (e.g.: revolute or fixed)
-            link_handle = ag_link.handle
-            joint_handle = self._dc.get_rigid_body_parent_joint(link_handle)
-            while joint_handle != 0:
-                # If this joint type is not fixed, we've encountered a valid moving joint
-                # So we create a spherical joint rather than fixed joint
-                if self._dc.get_joint_type(joint_handle) != JointType.JOINT_FIXED:
-                    joint_type = "SphericalJoint"
-                    break
-                # Grab the parent link and its parent joint for the link
-                link_handle = self._dc.get_joint_parent_body(joint_handle)
-                joint_handle = self._dc.get_rigid_body_parent_joint(link_handle)
+        articulation_graph = ag_obj.articulation_graph
+        for edge in nx.edge_dfs(articulation_graph, ag_link, orientation="reverse"):
+            joint = articulation_graph.edges[edge]["joint"]
+            if joint.joint_type != JointType.JOINT_FIXED:
+                joint_type = "SphericalJoint"
+                break
 
         return joint_type
 

From fb1aa643d5ab612cd3d89588bf3896622d6417d1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Mon, 11 Dec 2023 16:08:36 -0800
Subject: [PATCH 13/76] Remove DC reference

---
 omnigibson/objects/controllable_object.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py
index d92e39128..9b78562d1 100644
--- a/omnigibson/objects/controllable_object.py
+++ b/omnigibson/objects/controllable_object.py
@@ -242,7 +242,7 @@ def reload_controllers(self, controller_config=None):
             else self._create_continuous_action_space()
 
     def reset(self):
-        # Make sure simulation is playing, otherwise, we cannot reset because DC requires active running
+        # Make sure simulation is playing, otherwise, we cannot reset because physx requires active running
         # simulation in order to set joints
         assert og.sim.is_playing(), "Simulator must be playing in order to reset controllable object's joints!"
 

From 0a254955f38831d2954d8ed5c059f27e923e1363 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 12 Dec 2023 13:59:56 -0800
Subject: [PATCH 14/76] Match set_local_pose signature to other
 position/orientation by renaming translation to position

---
 omnigibson/object_states/particle_modifier.py |  2 +-
 omnigibson/object_states/toggle.py            |  2 +-
 omnigibson/prims/entity_prim.py               |  8 ++++----
 omnigibson/prims/xform_prim.py                | 12 ++++++------
 4 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/omnigibson/object_states/particle_modifier.py b/omnigibson/object_states/particle_modifier.py
index c6676fba9..04bd4da05 100644
--- a/omnigibson/object_states/particle_modifier.py
+++ b/omnigibson/object_states/particle_modifier.py
@@ -348,7 +348,7 @@ def overlap_callback(hit):
             z_offset = 0.0 if self._projection_mesh_params["type"] == "Sphere" else self._projection_mesh_params["extents"][2] / 2
 
             self.projection_mesh.set_local_pose(
-                translation=np.array([0, 0, -z_offset]),
+                position=np.array([0, 0, -z_offset]),
                 orientation=T.euler2quat([0, 0, 0]),
             )
 
diff --git a/omnigibson/object_states/toggle.py b/omnigibson/object_states/toggle.py
index 781c12139..8292a7b5f 100644
--- a/omnigibson/object_states/toggle.py
+++ b/omnigibson/object_states/toggle.py
@@ -72,7 +72,7 @@ def _initialize(self):
         self.visual_marker.visible = True
 
         # Make sure the marker isn't translated at all
-        self.visual_marker.set_local_pose(translation=np.zeros(3), orientation=np.array([0, 0, 0, 1.0]))
+        self.visual_marker.set_local_pose(position=np.zeros(3), orientation=np.array([0, 0, 0, 1.0]))
 
         # Store the projection mesh's IDs
         projection_mesh_ids = PhysicsSchemaTools.encodeSdfPath(self.visual_marker.prim_path)
diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index fa7e7aa76..9fad39098 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -810,12 +810,12 @@ def get_position_orientation(self):
         positions, orientations = self._articulation_view.get_world_poses()
         return positions[0], orientations[0]
 
-    def set_local_pose(self, translation=None, orientation=None):
-        if translation is not None:
-            translation = translation[None, :]
+    def set_local_pose(self, position=None, orientation=None):
+        if position is not None:
+            position = position[None, :]
         if orientation is not None:
             orientation = orientation[None, :]
-        self._articulation_view.set_local_poses(translation, orientation)
+        self._articulation_view.set_local_poses(position, orientation)
         BoundingBoxAPI.clear()
 
     def get_local_pose(self):
diff --git a/omnigibson/prims/xform_prim.py b/omnigibson/prims/xform_prim.py
index 041aa9e6c..ca1b44d61 100644
--- a/omnigibson/prims/xform_prim.py
+++ b/omnigibson/prims/xform_prim.py
@@ -175,7 +175,7 @@ def set_position_orientation(self, position=None, orientation=None):
         calculated_translation = transform.GetTranslation()
         calculated_orientation = transform.GetRotation().GetQuat()
         self.set_local_pose(
-            translation=np.array(calculated_translation), orientation=gf_quat_to_np_array(calculated_orientation)[[1, 2, 3, 0]]     # Flip from w,x,y,z to x,y,z,w
+            position=np.array(calculated_translation), orientation=gf_quat_to_np_array(calculated_orientation)[[1, 2, 3, 0]]     # Flip from w,x,y,z to x,y,z,w
         )
 
     def get_position_orientation(self):
@@ -268,24 +268,24 @@ def get_local_pose(self):
         xform_orient_op = self.get_attribute("xformOp:orient")
         return np.array(xform_translate_op), gf_quat_to_np_array(xform_orient_op)[[1, 2, 3, 0]]
 
-    def set_local_pose(self, translation=None, orientation=None):
+    def set_local_pose(self, position=None, orientation=None):
         """
         Sets prim's pose with respect to the local frame (the prim's parent frame).
 
         Args:
-            translation (None or 3-array): if specified, (x,y,z) translation in the local frame of the prim
+            position (None or 3-array): if specified, (x,y,z) position in the local frame of the prim
                 (with respect to its parent prim). Default is None, which means left unchanged.
             orientation (None or 4-array): if specified, (x,y,z,w) quaternion orientation in the local frame of the prim
                 (with respect to its parent prim). Default is None, which means left unchanged.
         """
         properties = self.prim.GetPropertyNames()
-        if translation is not None:
-            translation = Gf.Vec3d(*np.array(translation, dtype=float))
+        if position is not None:
+            position = Gf.Vec3d(*np.array(position, dtype=float))
             if "xformOp:translate" not in properties:
                 carb.log_error(
                     "Translate property needs to be set for {} before setting its position".format(self.name)
                 )
-            self.set_attribute("xformOp:translate", translation)
+            self.set_attribute("xformOp:translate", position)
         if orientation is not None:
             orientation = np.array(orientation, dtype=float)[[3, 0, 1, 2]]
             if "xformOp:orient" not in properties:

From 85c364b422635946841b60dfc711f51735e249d4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 12 Dec 2023 14:06:20 -0800
Subject: [PATCH 15/76] Address comments

---
 omnigibson/prims/joint_prim.py | 14 +++++---------
 omnigibson/prims/rigid_prim.py |  8 ++++----
 omnigibson/robots/tiago.py     |  3 +--
 3 files changed, 10 insertions(+), 15 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 35d0b898e..c05118578 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -92,10 +92,10 @@ def __init__(
         self._driven = None
 
         # The following values will only be valid if this joint is part of an articulation
-        self._n_dof = None
-        self._joint_idx = None
-        self._joint_dof_offset = None
-        self._joint_name = None
+        self._n_dof = None  # The number of degrees of freedom this joint provides
+        self._joint_idx = None  # The index of this joint in the parent articulation's joint array
+        self._joint_dof_offset = None  # The starting index of the DOFs for this joint in the parent articulation's DOF array
+        self._joint_name = None  # The name of this joint in the parent's articulation tree
 
         # Run super method
         super().__init__(
@@ -208,11 +208,7 @@ def set_control_type(self, control_type, kp=None, kd=None):
         # Set values
         kps = np.full((1, self._n_dof), kp)
         kds = np.full((1, self._n_dof), kd)
-        self._articulation_view.set_gains(kps=kps, kds=kds, joint_indices=self.dof_indices, save_to_usd=True)
-
-        # Assert no weirdness happening around the save_to_usd business.
-        kps, kds = self._articulation_view.get_gains(joint_indices=self.dof_indices)
-        assert np.allclose(kps, kp) and np.allclose(kds, kd), "Something went wrong with setting the gains!"
+        self._articulation_view.set_gains(kps=kps, kds=kds, joint_indices=self.dof_indices)
 
         # Update control type
         self._control_type = control_type
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 6d451a0cf..2c77e4889 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -272,12 +272,12 @@ def get_position_orientation(self):
             f"{self.prim_path} orientation {ori} is not a unit quaternion."
         return pos[0], ori[0]
 
-    def set_local_pose(self, translation=None, orientation=None):
-        if translation is not None:
-            translation = translation[None, :]
+    def set_local_pose(self, position=None, orientation=None):
+        if position is not None:
+            position = position[None, :]
         if orientation is not None:
             orientation = orientation[None, :]
-        self._articulation_view.set_local_poses(translation, orientation)
+        self._articulation_view.set_local_poses(position, orientation)
 
     def get_local_pose(self):
         positions, orientations = self._articulation_view.get_local_poses()
diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py
index ee61055a7..2021806af 100644
--- a/omnigibson/robots/tiago.py
+++ b/omnigibson/robots/tiago.py
@@ -697,8 +697,7 @@ def arm_workspace_range(self):
         }
 
     def get_position_orientation(self):
-        # TODO: This is a bad idea. If we need the pose to be here, let's put the base there.
-        # We shouldn't have a different behavior for only this object.
+        # TODO: Investigate the need for this custom behavior.
         return self.base_footprint_link.get_position_orientation()
 
     def set_position_orientation(self, position=None, orientation=None):

From 0e93f0708159c1b19849b221b62c97936f0a4b15 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 12 Dec 2023 14:21:25 -0800
Subject: [PATCH 16/76] Reorder view creation

---
 omnigibson/prims/rigid_prim.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 2c77e4889..9162878e7 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -77,12 +77,12 @@ def __init__(
         )
 
     def _post_load(self):
-        # run super first
-        super()._post_load()
-
         # Create the view
         self._rigid_prim_view = RigidPrimView(self._prim_path)
 
+        # run super first
+        super()._post_load()
+
         # Only create contact report api if we're not visual only
         if not self._visual_only:
             PhysxSchema.PhysxContactReportAPI(self._prim) if \

From 826593621420a97ba4a3e53e522ed8225981a25d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 12 Dec 2023 14:23:30 -0800
Subject: [PATCH 17/76] Remove references to mass API

---
 omnigibson/prims/rigid_prim.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 9162878e7..e31f79154 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -384,7 +384,7 @@ def mass(self, mass):
         Args:
             mass (float): mass of the rigid body in kg.
         """
-        self._mass_api.GetMassAttr().Set(mass)
+        self._rigid_prim_view.set_masses([mass])
 
     @property
     def density(self):
@@ -392,7 +392,7 @@ def density(self):
         Returns:
             float: density of the rigid body in kg / m^3.
         """
-        raw_usd_mass = self._mass_api.GetMassAttr().Get()
+        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
@@ -400,7 +400,7 @@ def density(self):
         if raw_usd_mass != 0:
             density = raw_usd_mass / self.volume
         else:
-            density = self._mass_api.GetDensityAttr().Get()
+            density = self._rigid_prim_view.get_densities()[0]
             if density == 0:
                 density = 1000.0
 
@@ -412,7 +412,7 @@ def density(self, density):
         Args:
             density (float): density of the rigid body in kg / m^3.
         """
-        self._mass_api.GetDensityAttr().Set(density)
+        self._rigid_prim_view.set_densities([density])
 
     @property
     def kinematic_only(self):

From 757b144a977ee9af40560cbb1f543ce7e6b1e52d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 13 Dec 2023 10:55:39 -0800
Subject: [PATCH 18/76] Some fixes

---
 omnigibson/prims/entity_prim.py | 4 ++--
 omnigibson/prims/joint_prim.py  | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 9fad39098..610ac5f39 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -91,7 +91,7 @@ def _load(self):
 
     def _post_load(self):
         # Prepare the articulation view (at this point only working via the USD interface)
-        self._articulation_view = ExtendedArticulationView(self._prim_path)
+        self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
 
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
@@ -235,7 +235,7 @@ def update_joints(self):
                 for i in range(self._articulation_view._metadata.joint_count):
                     joint_name = self._articulation_view._metadata.joint_names[i]
                     joint_dof_offset = self._articulation_view._metadata.joint_dof_offsets[i]
-                    joint_path = self._articulation_view.dof_paths[0][joint_dof_offset]
+                    joint_path = self._articulation_view._dof_paths[0][joint_dof_offset]
                     # Only add the joint if it's not fixed (i.e.: it has DOFs > 0)
                     if self._articulation_view._metadata.joint_dof_counts[i] > 0:
                         joint = JointPrim(
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index c05118578..ed481334f 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -173,7 +173,7 @@ def update_handles(self):
         """
         # 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)
+        self._joint_dof_offset = list(self._articulation_view._dof_paths[0]).index(self._prim_path)
         self._joint_idx = list(self._articulation_view._metadata.joint_dof_offsets).index(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]
@@ -558,7 +558,7 @@ def articulated(self):
         Returns:
              bool: Whether this joint is articulated or not
         """
-        return self._art is not None
+        return self.n_dof > 0
 
     @property
     def is_revolute(self):

From 2c8119551cacc0487a82adda75fbb92501214112 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 10:58:53 -0800
Subject: [PATCH 19/76] Remove trivial assert function

---
 omnigibson/prims/joint_prim.py | 18 ++++++------------
 1 file changed, 6 insertions(+), 12 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index ed481334f..cbce1bd6e 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -550,6 +550,7 @@ def dof_indices(self):
         Returns:
             list of int: Indices of this joint's DOFs in the parent articulation's DOF array
         """
+        assert self.articulated, "Can only get DOF indices for articulated joints!"
         return list(range(self._joint_dof_offset, self._joint_dof_offset + self._n_dof))
 
     @property
@@ -576,13 +577,6 @@ def is_single_dof(self):
         """
         return self._joint_type in {JointType.JOINT_REVOLUTE, JointType.JOINT_PRISMATIC}
 
-    def assert_articulated(self):
-        """
-        Sanity check to make sure this joint is articulated. Used as a gatekeeping function to prevent non-intended
-        behavior (e.g.: trying to grab this joint's state if it's not articulated)
-        """
-        assert self.articulated, "Tried to call method not intended for non-articulated joint!"
-
     def get_state(self, normalized=False):
         """
         (pos, vel, effort) state of this joint
@@ -598,7 +592,7 @@ def get_state(self, normalized=False):
                 - n-array: effort of this joint, where n = number of DOF for this joint
         """
         # Make sure we only call this if we're an articulated joint
-        self.assert_articulated()
+        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]
@@ -624,7 +618,7 @@ def get_target(self, normalized=False):
                 - n-array: target velocity of this joint, where n = number of DOF for this joint
         """
         # Make sure we only call this if we're an articulated joint
-        self.assert_articulated()
+        assert self.articulated, "Can only get targets for articulated joints!"
 
         # Grab raw states
         targets = self._articulation_view.get_applied_actions()
@@ -733,7 +727,7 @@ def set_pos(self, pos, normalized=False, drive=False):
                 instantaneous setting of the position
         """
         # Sanity checks -- make sure we're the correct control type if we're setting a target and that we're articulated
-        self.assert_articulated()
+        assert self.articulated, "Can only set position for articulated joints!"
         if drive:
             assert self._driven, "Can only use set_pos with drive=True if this joint is driven!"
             assert self._control_type == ControlType.POSITION, \
@@ -768,7 +762,7 @@ def set_vel(self, vel, normalized=False, drive=False):
                 instantaneous setting of the velocity
         """
         # Sanity checks -- make sure we're the correct control type if we're setting a target and that we're articulated
-        self.assert_articulated()
+        assert self.articulated, "Can only set velocity for articulated joints!"
         if drive:
             assert self._driven, "Can only use set_vel with drive=True if this joint is driven!"
             assert self._control_type == ControlType.VELOCITY, \
@@ -800,7 +794,7 @@ def set_effort(self, effort, normalized=False):
         """
         # Sanity checks -- make sure that we're articulated (no control type check like position and velocity
         # because we can't set effort targets) and that we're driven
-        self.assert_articulated()
+        assert self.articulated, "Can only set effort for articulated joints!"
 
         # Standardize input
         effort = np.array([effort]) if self._n_dof == 1 and not isinstance(effort, Iterable) else np.array(effort)

From 7348f8dd2c5436db28c5e4f0ccd80ed7af42f9b9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:08:47 -0800
Subject: [PATCH 20/76] Fix targets

---
 omnigibson/prims/joint_prim.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index cbce1bd6e..94b3fc8b2 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -622,8 +622,8 @@ def get_target(self, normalized=False):
 
         # Grab raw states
         targets = self._articulation_view.get_applied_actions()
-        pos = targets.joint_positions
-        vel = targets.joint_velocities
+        pos = targets.joint_positions[0][self.dof_indices]
+        vel = targets.joint_velocities[0][self.dof_indices]
 
         # Potentially normalize if requested
         if normalized:

From dac0927285099b5efc1c4778a5b9962c539e1630 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:31:58 -0800
Subject: [PATCH 21/76] Remove articulation tree, which does not work because
 fixed links are not included in tree

---
 omnigibson/prims/entity_prim.py | 21 ---------------------
 1 file changed, 21 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 610ac5f39..299abd971 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -229,7 +229,6 @@ def update_joints(self):
         if not self.kinematic_only:
             self._n_dof = self._articulation_view.num_dof
 
-            # TODO: This still includes fixed joints for objects that also have non-fixed?
             # Additionally grab DOF info if we have non-fixed joints
             if self._n_dof > 0:
                 for i in range(self._articulation_view._metadata.joint_count):
@@ -419,26 +418,6 @@ def links(self):
         """
         return self._links
     
-    @property
-    def articulation_tree(self):
-        """
-        Get a graph of the articulation tree, where nodes are links (RigidPrim) and edges
-        correspond to joints (JointPrim), where the JointPrim is accessible on the `joint`
-        data field of the edge.
-        """
-        G = nx.DiGraph()
-        rename_later = {}
-        for link in self.links.values():
-            prim_path = link.prim_path
-            G.add_node(prim_path)
-            rename_later[prim_path] = link
-        for joint in self.joints.values():
-            if joint.body0 not in G.nodes or joint.body1 not in G.nodes:
-                continue
-            G.add_edge(joint.body0, joint.body1, joint=joint)
-        nx.relabel_nodes(G, rename_later)
-        return G
-
     @property
     def materials(self):
         """

From 00ce7c52c81eeb6dfe9b998bc5d452e2d2080466 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:32:07 -0800
Subject: [PATCH 22/76] Fix bug in joint finding

---
 omnigibson/prims/joint_prim.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 94b3fc8b2..8f6b778ca 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -174,7 +174,10 @@ def update_handles(self):
         # 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)
-        self._joint_idx = list(self._articulation_view._metadata.joint_dof_offsets).index(self._joint_dof_offset)
+        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]
         

From 4b6ecc16b1396a06029150a0f69f8b8407761b2b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:32:19 -0800
Subject: [PATCH 23/76] Temporarily disable tree-based AG type selection

---
 omnigibson/robots/manipulation_robot.py | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index 7a8234856..4484b0f92 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -1057,12 +1057,13 @@ def _get_assisted_grasp_joint_type(self, ag_obj, ag_link):
         # Otherwise, compute the joint type. We use a fixed joint unless the link is a non-fixed link.
         # A link is non-fixed if it has any non-fixed parent joints.
         joint_type = "FixedJoint"
-        articulation_graph = ag_obj.articulation_graph
-        for edge in nx.edge_dfs(articulation_graph, ag_link, orientation="reverse"):
-            joint = articulation_graph.edges[edge]["joint"]
-            if joint.joint_type != JointType.JOINT_FIXED:
-                joint_type = "SphericalJoint"
-                break
+        # TODO: Add this logic back w/o the graph using just the articulationview.
+        # articulation_graph = ag_obj.articulation_graph
+        # for edge in nx.edge_dfs(articulation_graph, ag_link, orientation="reverse"):
+        #     joint = articulation_graph.edges[edge]["joint"]
+        #     if joint.joint_type != JointType.JOINT_FIXED:
+        #         joint_type = "SphericalJoint"
+        #         break
 
         return joint_type
 

From 3462fccb559c3f0909552d93caaf3ec2de503d92 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:45:03 -0800
Subject: [PATCH 24/76] Fix has_limits

---
 omnigibson/prims/joint_prim.py | 15 +--------------
 1 file changed, 1 insertion(+), 14 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 8f6b778ca..e5be6dda8 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -87,8 +87,6 @@ def __init__(
         # Other values that will be filled in at runtime
         self._joint_type = None
         self._control_type = None
-        self._dof_properties = None
-        self._joint_state_api = None
         self._driven = None
 
         # The following values will only be valid if this joint is part of an articulation
@@ -133,7 +131,6 @@ def _post_load(self):
             # happens because joint prims are usually created externally during an EntityPrim's initialization phase
             assert self._prim.HasAPI(PhysxSchema.JointStateAPI), \
                 "Revolute or Prismatic joints must already have JointStateAPI added!"
-            self._joint_state_api = PhysxSchema.JointStateAPI(self._prim, state_type)
 
         # Possibly set the bodies
         if "body0" in self._load_config and self._load_config["body0"] is not None:
@@ -314,16 +311,6 @@ def control_type(self):
         """
         return self._control_type
 
-    @property
-    def dof_properties(self):
-        """
-        Returns:
-            list of DOFProperties: Per-DOF properties for this joint.
-                See https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.dynamic_control/docs/index.html#omni.isaac.dynamic_control._dynamic_control.DofProperties
-                for more information.
-        """
-        return self._dof_properties
-
     @property
     def max_velocity(self):
         """
@@ -512,7 +499,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 self._dof_properties[0].has_limits
+        return np.all(np.abs(self._articulation_view.get_joint_limits(joint_indices=self.dof_indices)) < m.INF_POS_THRESHOLD)
 
     @property
     def axis(self):

From 6e3195333f4ba1766b6e29f8fe940cc577032ea2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:59:31 -0800
Subject: [PATCH 25/76] Better compute of eef link idx

---
 omnigibson/robots/manipulation_robot.py | 12 ++----------
 1 file changed, 2 insertions(+), 10 deletions(-)

diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index 4484b0f92..11a1040b9 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -151,9 +151,6 @@ def __init__(
         self._grasping_mode = grasping_mode
         self._disable_grasp_handling = disable_grasp_handling
 
-        # Other internal variables initialized later
-        self._eef_link_idxs = {arm: None for arm in self.arm_names}
-
         # Initialize other variables used for assistive grasping
         self._ag_freeze_joint_pos = {
             arm: {} for arm in self.arm_names
@@ -219,12 +216,6 @@ def _validate_configuration(self):
     def _initialize(self):
         super()._initialize()
 
-        # Store index of EEF within link count
-        offset = -1 if self.fixed_base else 0
-        link_path_to_idx = {path: i for i, path in enumerate(self._physics_view.link_paths[0])}
-        for arm in self.arm_names:
-            self._eef_link_idxs[arm] = link_path_to_idx[self.eef_links[arm].prim_path] + offset
-
         if gm.AG_CLOTH:
             for arm in self.arm_names:
                 self._ag_check_in_volume[arm], self._ag_calculate_volume[arm] = \
@@ -399,7 +390,8 @@ def get_control_dict(self):
             # -n_joints because there may be an additional 6 entries at the beginning of the array, if this robot does
             # not have a fixed base (i.e.: the 6DOF --> "floating" joint)
             # see self.get_relative_jacobian() for more info
-            dic[f"eef_{arm}_jacobian_relative"] = self.get_relative_jacobian()[self._eef_link_idxs[arm], :, -self.n_joints:]
+            eef_link_idx = self._articulation_view.get_body_index(self.eef_links[arm].body_name)
+            dic[f"eef_{arm}_jacobian_relative"] = self.get_relative_jacobian()[eef_link_idx, :, -self.n_joints:]
 
         return dic
 

From 66a46a8f6ee799ef6d2d9f66ed538f5dcb488075 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 11:59:39 -0800
Subject: [PATCH 26/76] Remove references to physics view

---
 omnigibson/prims/entity_prim.py | 12 ++++--------
 1 file changed, 4 insertions(+), 8 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 299abd971..4028e1982 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -1084,8 +1084,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!"
-        self._verify_physics_view_is_valid()
-        return self._physics_view.get_coriolis_and_centrifugal_forces().reshape(self.n_dof)
+        return self._articulation_view.get_coriolis_and_centrifugal_forces().reshape(self.n_dof)
 
     def get_generalized_gravity_forces(self):
         """
@@ -1093,8 +1092,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!"
-        self._verify_physics_view_is_valid()
-        return self._physics_view.get_generalized_gravity_forces().reshape(self.n_dof)
+        return self._articulation_view.get_generalized_gravity_forces().reshape(self.n_dof)
 
     def get_mass_matrix(self):
         """
@@ -1102,8 +1100,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!"
-        self._verify_physics_view_is_valid()
-        return self._physics_view.get_mass_matrices().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):
         """
@@ -1113,8 +1110,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!"
-        self._verify_physics_view_is_valid()
-        return self._physics_view.get_jacobians().squeeze(axis=0)
+        return self._articulation_view.get_jacobians().squeeze(axis=0)
 
     def get_relative_jacobian(self):
         """

From b922a5fbefe86e67b41bc8c1e34750c8d17beed8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 12:07:24 -0800
Subject: [PATCH 27/76] Some fixes

---
 omnigibson/prims/joint_prim.py | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index e5be6dda8..ca351d1d2 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -148,8 +148,8 @@ def _initialize(self):
         # Initialize dynamic control references if this joint is articulated
         if self.articulated:
             control_types = []
-            stifnesses, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)
-            for i, (kp, kd) in enumerate(zip(stifnesses[0], dampings[0])):
+            stiffnesses, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)
+            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:
@@ -344,25 +344,25 @@ def max_effort(self):
         Gets this joint's maximum effort
 
         Returns:
-            float: maximum force for this joint
+            float: maximum effort for this joint
         """
         # 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_force = self._articulation_view.get_max_efforts(joint_indices=self.dof_indices)[0][0]
-        return m.DEFAULT_MAX_EFFORT if raw_force is None or np.abs(raw_force) > m.INF_EFFORT_THRESHOLD else raw_force
+        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
-    def max_effort(self, force):
+    def max_effort(self, effort):
         """
         Sets this joint's maximum effort
 
         Args:
-            force (float): Force to set
+            effort (float): effort to set
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        self._articulation_view.set_max_efforts(np.array([[force]]), joint_indices=self.dof_indices)
+        self._articulation_view.set_max_efforts(np.array([[effort]]), joint_indices=self.dof_indices)
 
     @property
     def stiffness(self):
@@ -432,7 +432,7 @@ def friction(self, friction):
         Args:
             friction (float): friction to set
         """
-        self._articulation_view.set_friction_coefficients(np.array([[friction]]), joint_indices=self.dof_indices)[0][0]
+        self._articulation_view.set_friction_coefficients(np.array([[friction]]), joint_indices=self.dof_indices)
 
     @property
     def lower_limit(self):
@@ -794,7 +794,7 @@ def set_effort(self, effort, normalized=False):
             effort = self._denormalize_effort(effort)
 
         # Set the DOF(s) in this joint
-        self._articulation_view.set_joint_efforts(velocities=effort, joint_indices=self.dof_indices)
+        self._articulation_view.set_joint_efforts(efforts=effort, joint_indices=self.dof_indices)
 
     def keep_still(self):
         """

From fe44403131431e5dc9dbe81304c00254c6e1ec0b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 13:48:01 -0800
Subject: [PATCH 28/76] Fix some naiveties

---
 omnigibson/prims/entity_prim.py | 8 ++++----
 omnigibson/prims/rigid_prim.py  | 8 ++++----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 4028e1982..6909f4460 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -779,9 +779,9 @@ def get_angular_velocity(self):
 
     def set_position_orientation(self, position=None, orientation=None):
         if position is not None:
-            position = position[None, :]
+            position = np.asarray(position)[None, :]
         if orientation is not None:
-            orientation = orientation[None, :]
+            orientation = np.asarray(orientation)[None, :]
         self._articulation_view.set_world_poses(position, orientation)
         BoundingBoxAPI.clear()
 
@@ -791,9 +791,9 @@ def get_position_orientation(self):
 
     def set_local_pose(self, position=None, orientation=None):
         if position is not None:
-            position = position[None, :]
+            position = np.asarray(position)[None, :]
         if orientation is not None:
-            orientation = orientation[None, :]
+            orientation = np.asarray(orientation)[None, :]
         self._articulation_view.set_local_poses(position, orientation)
         BoundingBoxAPI.clear()
 
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index e31f79154..082e97123 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -258,11 +258,11 @@ def get_angular_velocity(self):
 
     def set_position_orientation(self, position=None, orientation=None):
         if position is not None:
-            position = position[None, :]
+            position = np.asarray(position)[None, :]
         if orientation is not None:
             assert np.isclose(np.linalg.norm(orientation), 1, atol=1e-3), \
                 f"{self.prim_path} desired orientation {orientation} is not a unit quaternion."
-            orientation = orientation[None, :]
+            orientation = np.asarray(orientation)[None, :]
         self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation)
 
     def get_position_orientation(self):
@@ -274,9 +274,9 @@ def get_position_orientation(self):
 
     def set_local_pose(self, position=None, orientation=None):
         if position is not None:
-            position = position[None, :]
+            position = np.asarray(position)[None, :]
         if orientation is not None:
-            orientation = orientation[None, :]
+            orientation = np.asarray(orientation)[None, :]
         self._articulation_view.set_local_poses(position, orientation)
 
     def get_local_pose(self):

From bd548382e135cdddf6475820bf7a62d4170a533e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 13:56:07 -0800
Subject: [PATCH 29/76] Some fixes

---
 omnigibson/prims/rigid_prim.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 082e97123..78d9c7a3d 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -277,10 +277,10 @@ def set_local_pose(self, position=None, orientation=None):
             position = np.asarray(position)[None, :]
         if orientation is not None:
             orientation = np.asarray(orientation)[None, :]
-        self._articulation_view.set_local_poses(position, orientation)
+        self._rigid_prim_view.set_local_poses(position, orientation)
 
     def get_local_pose(self):
-        positions, orientations = self._articulation_view.get_local_poses()
+        positions, orientations = self._rigid_prim_view.get_local_poses()
         return positions[0], orientations[0]
 
     @property

From d774505a839a974aeadd32eb23c06ada90cec5ca Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 14:22:02 -0800
Subject: [PATCH 30/76] Make nonarticulated entityprims defer to xformprim for
 position setting and getting

---
 omnigibson/prims/entity_prim.py | 27 ++++++++++++++++++++++-----
 1 file changed, 22 insertions(+), 5 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 6909f4460..339d5650f 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -91,7 +91,8 @@ def _load(self):
 
     def _post_load(self):
         # Prepare the articulation view (at this point only working via the USD interface)
-        self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
+        if self.n_joints > 0:
+            self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
 
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
@@ -226,7 +227,7 @@ def update_joints(self):
         self.update_handles()
 
         # Handle case separately based on whether we are actually articulated or not
-        if not self.kinematic_only:
+        if self._articulation_view and not self.kinematic_only:
             self._n_dof = self._articulation_view.num_dof
 
             # Additionally grab DOF info if we have non-fixed joints
@@ -315,8 +316,7 @@ def articulated(self):
         Returns:
              bool: Whether this prim is articulated or not
         """
-        # An invalid handle implies that there is no articulation available for this object
-        return self.articulation_root_path is not None
+        return self.n_joints > 0
 
     @property
     def articulation_root_path(self):
@@ -675,7 +675,8 @@ def update_handles(self):
         assert og.sim.is_playing(), "Simulator must be playing if updating handles!"
 
         # Reinitialize the articulation view
-        self._articulation_view.initialize(og.sim.physics_sim_view)
+        if self._articulation_view is not None:
+            self._articulation_view.initialize(og.sim.physics_sim_view)
 
         # Update all links and joints as well
         for link in self._links.values():
@@ -778,6 +779,10 @@ def get_angular_velocity(self):
         return self.root_link.get_angular_velocity()
 
     def set_position_orientation(self, position=None, orientation=None):
+        # Delegate to XFormPrim if we are not articulated
+        if self._articulation_view is None:
+            return super().set_position_orientation(position=position, orientation=orientation)
+        
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -786,10 +791,18 @@ def set_position_orientation(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
+        # Delegate to XFormPrim if we are not articulated
+        if self._articulation_view is None:
+            return super().get_position_orientation()
+
         positions, orientations = self._articulation_view.get_world_poses()
         return positions[0], orientations[0]
 
     def set_local_pose(self, position=None, orientation=None):
+        # Delegate to XFormPrim if we are not articulated
+        if self._articulation_view is None:
+            return super().set_local_pose(position=position, orientation=orientation)
+        
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -798,6 +811,10 @@ def set_local_pose(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_local_pose(self):
+        # Delegate to XFormPrim if we are not articulated
+        if self._articulation_view is None:
+            return super().get_local_pose()
+        
         positions, orientations = self._articulation_view.get_local_poses()
         return positions[0], orientations[0]
 

From dd49f351555dfb972a7f4d54fc8d35170e8d8968 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 14:52:36 -0800
Subject: [PATCH 31/76] Reorder some things

---
 omnigibson/prims/entity_prim.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 339d5650f..7f4d6fb94 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -90,7 +90,11 @@ def _load(self):
         raise NotImplementedError("By default, an entity prim cannot be created from scratch.")
 
     def _post_load(self):
-        # Prepare the articulation view (at this point only working via the USD interface)
+        # Setup links info FIRST before running any other post loading behavior
+        # We pass in scale explicitly so that the generated links can leverage the desired entity scale
+        self.update_links(load_config=dict(scale=self._load_config.get("scale", None)))
+
+        # Prepare the articulation view.
         if self.n_joints > 0:
             self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
 
@@ -118,10 +122,6 @@ def _post_load(self):
             omni.kit.commands.execute("CopyPrim", path_from=cloth_mesh_prim.GetPath(), path_to=new_path)
             omni.kit.commands.execute("DeletePrims", paths=[old_link_prim.GetPath()], destructive=False)
 
-        # Setup links info FIRST before running any other post loading behavior
-        # We pass in scale explicitly so that the generated links can leverage the desired entity scale
-        self.update_links(load_config=dict(scale=self._load_config.get("scale", None)))
-
         # Set visual only flag
         # This automatically handles setting collisions / gravity appropriately per-link
         self.visual_only = self._load_config["visual_only"] if \

From 8b8eef50b597356c877da90934fefab9710baa5a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 15:17:36 -0800
Subject: [PATCH 32/76] Implement proper is_articulated checks for entity and
 joint

---
 omnigibson/prims/entity_prim.py | 2 +-
 omnigibson/prims/joint_prim.py  | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 7f4d6fb94..300ef9486 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -316,7 +316,7 @@ def articulated(self):
         Returns:
              bool: Whether this prim is articulated or not
         """
-        return self.n_joints > 0
+        return self._prim.HasAPI(UsdPhysics.ArticulationRootAPI)
 
     @property
     def articulation_root_path(self):
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index ca351d1d2..465e6ac7e 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -145,7 +145,7 @@ def _initialize(self):
         # Update the joint indices etc.
         self.update_handles()
 
-        # Initialize dynamic control references if this joint is articulated
+        # Get control type
         if self.articulated:
             control_types = []
             stiffnesses, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)
@@ -549,7 +549,7 @@ def articulated(self):
         Returns:
              bool: Whether this joint is articulated or not
         """
-        return self.n_dof > 0
+        return self._articulation_view is not None
 
     @property
     def is_revolute(self):

From e5dd359a005b5110a262413081f022b743dcbde8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 15:24:07 -0800
Subject: [PATCH 33/76] Back to root path

---
 omnigibson/prims/entity_prim.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 300ef9486..526824c51 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -316,7 +316,9 @@ def articulated(self):
         Returns:
              bool: Whether this prim is articulated or not
         """
-        return self._prim.HasAPI(UsdPhysics.ArticulationRootAPI)
+        # Note that this is not equivalent to self.n_joints > 0 because articulation root path is
+        # overridden by the object classes
+        return self.articulation_root_path is not None
 
     @property
     def articulation_root_path(self):

From 9e43641abb38850ba8f22a9fef3374bd7a1d254e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 15:31:34 -0800
Subject: [PATCH 34/76] Don't read info about fixed joints

---
 omnigibson/prims/entity_prim.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 526824c51..171a02c18 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -233,11 +233,11 @@ def update_joints(self):
             # Additionally grab DOF info if we have non-fixed joints
             if self._n_dof > 0:
                 for i in range(self._articulation_view._metadata.joint_count):
-                    joint_name = self._articulation_view._metadata.joint_names[i]
-                    joint_dof_offset = self._articulation_view._metadata.joint_dof_offsets[i]
-                    joint_path = self._articulation_view._dof_paths[0][joint_dof_offset]
                     # Only add the joint if it's not fixed (i.e.: it has DOFs > 0)
                     if self._articulation_view._metadata.joint_dof_counts[i] > 0:
+                        joint_name = self._articulation_view._metadata.joint_names[i]
+                        joint_dof_offset = self._articulation_view._metadata.joint_dof_offsets[i]
+                        joint_path = self._articulation_view._dof_paths[0][joint_dof_offset]
                         joint = JointPrim(
                             prim_path=joint_path,
                             name=f"{self._name}:joint_{joint_name}",

From 6b0a3f1639ae6f2b6232de7f7764ed2431093cd5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 16:11:40 -0800
Subject: [PATCH 35/76] Disable a bunch of APIs for kinematic-only bodies.

---
 omnigibson/prims/rigid_prim.py | 32 ++++++++++++++++++++++++++++++--
 1 file changed, 30 insertions(+), 2 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 78d9c7a3d..765d4a063 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -78,7 +78,8 @@ def __init__(
 
     def _post_load(self):
         # Create the view
-        self._rigid_prim_view = RigidPrimView(self._prim_path)
+        if not self.kinematic_only:
+            self._rigid_prim_view = RigidPrimView(self._prim_path)
 
         # run super first
         super()._post_load()
@@ -203,7 +204,8 @@ def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
         """
-        self._rigid_prim_view.initialize(og.sim.physics_sim_view)
+        if self._rigid_prim_view is not None:
+            self._rigid_prim_view.initialize(og.sim.physics_sim_view)
 
     def contact_list(self):
         """
@@ -231,6 +233,7 @@ def set_linear_velocity(self, velocity):
         Args:
             velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,).
         """
+        assert not self.kinematic_only, "Cannot set linear velocity for a kinematic-only body!"
         self._rigid_prim_view.set_linear_velocities(velocity[None, :])
 
     def get_linear_velocity(self):
@@ -238,6 +241,7 @@ def get_linear_velocity(self):
         Returns:
             np.ndarray: current linear velocity of the the rigid prim. Shape (3,).
         """
+        assert not self.kinematic_only, "Cannot get linear velocity for a kinematic-only body!"
         return self._rigid_prim_view.get_linear_velocities()[0]
 
     def set_angular_velocity(self, velocity):
@@ -247,6 +251,7 @@ def set_angular_velocity(self, velocity):
         Args:
             velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,).
         """
+        assert not self.kinematic_only, "Cannot set angular velocity for a kinematic-only body!"
         self._rigid_prim_view.set_angular_velocities(velocity[None, :])
 
     def get_angular_velocity(self):
@@ -254,9 +259,14 @@ def get_angular_velocity(self):
         Returns:
             np.ndarray: current angular velocity of the the rigid prim. Shape (3,).
         """
+        assert not self.kinematic_only, "Cannot get angular velocity for a kinematic-only body!"
         return self._rigid_prim_view.get_angular_velocities()[0]
 
     def set_position_orientation(self, position=None, orientation=None):
+        # Fallback to the xformprim method for kinematic-only objects
+        if self.kinematic_only:
+            return super().set_position_orientation(position=position, orientation=orientation)
+
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -266,6 +276,10 @@ def set_position_orientation(self, position=None, orientation=None):
         self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation)
 
     def get_position_orientation(self):
+        # Fallback to the xformprim method for kinematic-only objects
+        if self.kinematic_only:
+            return super().get_position_orientation()
+        
         pos, ori = self._rigid_prim_view.get_world_poses()
 
         assert np.isclose(np.linalg.norm(ori), 1, atol=1e-3), \
@@ -273,6 +287,10 @@ def get_position_orientation(self):
         return pos[0], ori[0]
 
     def set_local_pose(self, position=None, orientation=None):
+        # Fallback to the xformprim method for kinematic-only objects
+        if self.kinematic_only:
+            return super().set_local_pose(position=position, orientation=orientation)
+        
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -280,6 +298,10 @@ def set_local_pose(self, position=None, orientation=None):
         self._rigid_prim_view.set_local_poses(position, orientation)
 
     def get_local_pose(self):
+        # Fallback to the xformprim method for kinematic-only objects
+        if self.kinematic_only:
+            return super().get_local_pose()
+
         positions, orientations = self._rigid_prim_view.get_local_poses()
         return positions[0], orientations[0]
 
@@ -370,6 +392,7 @@ def mass(self):
         Returns:
             float: mass of the rigid body in kg.
         """
+        assert not self.kinematic_only, "Cannot get mass for a kinematic-only body!"
         mass = self._rigid_prim_view.get_masses()[0]
 
         # Fallback to analytical computation of volume * density
@@ -384,6 +407,7 @@ def mass(self, mass):
         Args:
             mass (float): mass of the rigid body in kg.
         """
+        assert not self.kinematic_only, "Cannot set mass for a kinematic-only body!"
         self._rigid_prim_view.set_masses([mass])
 
     @property
@@ -392,6 +416,7 @@ def density(self):
         Returns:
             float: density of the rigid body in kg / m^3.
         """
+        assert not self.kinematic_only, "Cannot get density for a kinematic-only body!"
         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
@@ -412,6 +437,7 @@ def density(self, density):
         Args:
             density (float): density of the rigid body in kg / m^3.
         """
+        assert not self.kinematic_only, "Cannot set density for a kinematic-only body!"
         self._rigid_prim_view.set_densities([density])
 
     @property
@@ -537,12 +563,14 @@ def enable_gravity(self):
         """
         Enables gravity for this rigid body
         """
+        assert not self.kinematic_only, "Cannot enable gravity for a kinematic-only body!"
         self._rigid_prim_view.enable_gravities()
 
     def disable_gravity(self):
         """
         Disables gravity for this rigid body
         """
+        assert not self.kinematic_only, "Cannot disable gravity for a kinematic-only body!"
         self._rigid_prim_view.disable_gravities()
 
     def wake(self):

From b610f6d37536096ea4e30465e0be813348d1b34c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 16:55:33 -0800
Subject: [PATCH 36/76] Create the views during initialization time so that we
 can use all information from post_load

---
 omnigibson/prims/entity_prim.py | 10 ++++------
 omnigibson/prims/rigid_prim.py  |  9 ++++-----
 2 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 171a02c18..2fc4acefe 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -94,10 +94,6 @@ def _post_load(self):
         # We pass in scale explicitly so that the generated links can leverage the desired entity scale
         self.update_links(load_config=dict(scale=self._load_config.get("scale", None)))
 
-        # Prepare the articulation view.
-        if self.n_joints > 0:
-            self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
-
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
             # Verify only a single link and a single mesh exists
@@ -676,8 +672,10 @@ def update_handles(self):
         """
         assert og.sim.is_playing(), "Simulator must be playing if updating handles!"
 
-        # Reinitialize the articulation view
-        if self._articulation_view is not None:
+        # Create the articulation view
+        if self.n_joints > 0:
+            if self._articulation_view is None:
+                self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
             self._articulation_view.initialize(og.sim.physics_sim_view)
 
         # Update all links and joints as well
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 765d4a063..a40e42a35 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -77,10 +77,6 @@ def __init__(
         )
 
     def _post_load(self):
-        # Create the view
-        if not self.kinematic_only:
-            self._rigid_prim_view = RigidPrimView(self._prim_path)
-
         # run super first
         super()._post_load()
 
@@ -204,7 +200,10 @@ def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
         """
-        if self._rigid_prim_view is not None:
+        # Create the view
+        if not self.kinematic_only:
+            if self._rigid_prim_view is None:
+                self._rigid_prim_view = RigidPrimView(self._prim_path)
             self._rigid_prim_view.initialize(og.sim.physics_sim_view)
 
     def contact_list(self):

From 4d85830b954f5a5f70a4a1def6693f80c824a477 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 17:02:29 -0800
Subject: [PATCH 37/76] Allow the use of XFormPrim methods until the rigid body
 is initialized.

---
 omnigibson/prims/rigid_prim.py | 28 ++++++++++++++--------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index a40e42a35..72e26486d 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -232,7 +232,7 @@ def set_linear_velocity(self, velocity):
         Args:
             velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot set linear velocity for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot set linear velocity for a kinematic-only or uninitialized body!"
         self._rigid_prim_view.set_linear_velocities(velocity[None, :])
 
     def get_linear_velocity(self):
@@ -240,7 +240,7 @@ def get_linear_velocity(self):
         Returns:
             np.ndarray: current linear velocity of the the rigid prim. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot get linear velocity for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot get linear velocity for a kinematic-only or uninitialized body!"
         return self._rigid_prim_view.get_linear_velocities()[0]
 
     def set_angular_velocity(self, velocity):
@@ -250,7 +250,7 @@ def set_angular_velocity(self, velocity):
         Args:
             velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot set angular velocity for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot set angular velocity for a kinematic-only or uninitialized body!"
         self._rigid_prim_view.set_angular_velocities(velocity[None, :])
 
     def get_angular_velocity(self):
@@ -258,12 +258,12 @@ def get_angular_velocity(self):
         Returns:
             np.ndarray: current angular velocity of the the rigid prim. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot get angular velocity for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot get angular velocity for a kinematic-only or uninitialized body!"
         return self._rigid_prim_view.get_angular_velocities()[0]
 
     def set_position_orientation(self, position=None, orientation=None):
         # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
+        if self._rigid_prim_view is None:
             return super().set_position_orientation(position=position, orientation=orientation)
 
         if position is not None:
@@ -276,7 +276,7 @@ def set_position_orientation(self, position=None, orientation=None):
 
     def get_position_orientation(self):
         # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
+        if self._rigid_prim_view is None:
             return super().get_position_orientation()
         
         pos, ori = self._rigid_prim_view.get_world_poses()
@@ -287,7 +287,7 @@ def get_position_orientation(self):
 
     def set_local_pose(self, position=None, orientation=None):
         # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
+        if self._rigid_prim_view is None:
             return super().set_local_pose(position=position, orientation=orientation)
         
         if position is not None:
@@ -298,7 +298,7 @@ def set_local_pose(self, position=None, orientation=None):
 
     def get_local_pose(self):
         # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
+        if self._rigid_prim_view is None:
             return super().get_local_pose()
 
         positions, orientations = self._rigid_prim_view.get_local_poses()
@@ -391,7 +391,7 @@ def mass(self):
         Returns:
             float: mass of the rigid body in kg.
         """
-        assert not self.kinematic_only, "Cannot get mass for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot get mass for a kinematic-only or uninitialized body!"
         mass = self._rigid_prim_view.get_masses()[0]
 
         # Fallback to analytical computation of volume * density
@@ -406,7 +406,7 @@ def mass(self, mass):
         Args:
             mass (float): mass of the rigid body in kg.
         """
-        assert not self.kinematic_only, "Cannot set mass for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot set mass for a kinematic-only or uninitialized body!"
         self._rigid_prim_view.set_masses([mass])
 
     @property
@@ -415,7 +415,7 @@ def density(self):
         Returns:
             float: density of the rigid body in kg / m^3.
         """
-        assert not self.kinematic_only, "Cannot get density for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot get density for a kinematic-only or uninitialized body!"
         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
@@ -436,7 +436,7 @@ def density(self, density):
         Args:
             density (float): density of the rigid body in kg / m^3.
         """
-        assert not self.kinematic_only, "Cannot set density for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot set density for a kinematic-only or uninitialized body!"
         self._rigid_prim_view.set_densities([density])
 
     @property
@@ -562,14 +562,14 @@ def enable_gravity(self):
         """
         Enables gravity for this rigid body
         """
-        assert not self.kinematic_only, "Cannot enable gravity for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot enable gravity for a kinematic-only or uninitialized body!"
         self._rigid_prim_view.enable_gravities()
 
     def disable_gravity(self):
         """
         Disables gravity for this rigid body
         """
-        assert not self.kinematic_only, "Cannot disable gravity for a kinematic-only body!"
+        assert self._rigid_prim_view is not None, "Cannot disable gravity for a kinematic-only or uninitialized body!"
         self._rigid_prim_view.disable_gravities()
 
     def wake(self):

From fb752fb7a9900606341be30589bedb6cfca95358 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 17:12:33 -0800
Subject: [PATCH 38/76] Revert "Allow the use of XFormPrim methods until the
 rigid body is initialized."

This reverts commit 4d85830b954f5a5f70a4a1def6693f80c824a477.
---
 omnigibson/prims/rigid_prim.py | 28 ++++++++++++++--------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 72e26486d..a40e42a35 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -232,7 +232,7 @@ def set_linear_velocity(self, velocity):
         Args:
             velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,).
         """
-        assert self._rigid_prim_view is not None, "Cannot set linear velocity for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot set linear velocity for a kinematic-only body!"
         self._rigid_prim_view.set_linear_velocities(velocity[None, :])
 
     def get_linear_velocity(self):
@@ -240,7 +240,7 @@ def get_linear_velocity(self):
         Returns:
             np.ndarray: current linear velocity of the the rigid prim. Shape (3,).
         """
-        assert self._rigid_prim_view is not None, "Cannot get linear velocity for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot get linear velocity for a kinematic-only body!"
         return self._rigid_prim_view.get_linear_velocities()[0]
 
     def set_angular_velocity(self, velocity):
@@ -250,7 +250,7 @@ def set_angular_velocity(self, velocity):
         Args:
             velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,).
         """
-        assert self._rigid_prim_view is not None, "Cannot set angular velocity for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot set angular velocity for a kinematic-only body!"
         self._rigid_prim_view.set_angular_velocities(velocity[None, :])
 
     def get_angular_velocity(self):
@@ -258,12 +258,12 @@ def get_angular_velocity(self):
         Returns:
             np.ndarray: current angular velocity of the the rigid prim. Shape (3,).
         """
-        assert self._rigid_prim_view is not None, "Cannot get angular velocity for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot get angular velocity for a kinematic-only body!"
         return self._rigid_prim_view.get_angular_velocities()[0]
 
     def set_position_orientation(self, position=None, orientation=None):
         # Fallback to the xformprim method for kinematic-only objects
-        if self._rigid_prim_view is None:
+        if self.kinematic_only:
             return super().set_position_orientation(position=position, orientation=orientation)
 
         if position is not None:
@@ -276,7 +276,7 @@ def set_position_orientation(self, position=None, orientation=None):
 
     def get_position_orientation(self):
         # Fallback to the xformprim method for kinematic-only objects
-        if self._rigid_prim_view is None:
+        if self.kinematic_only:
             return super().get_position_orientation()
         
         pos, ori = self._rigid_prim_view.get_world_poses()
@@ -287,7 +287,7 @@ def get_position_orientation(self):
 
     def set_local_pose(self, position=None, orientation=None):
         # Fallback to the xformprim method for kinematic-only objects
-        if self._rigid_prim_view is None:
+        if self.kinematic_only:
             return super().set_local_pose(position=position, orientation=orientation)
         
         if position is not None:
@@ -298,7 +298,7 @@ def set_local_pose(self, position=None, orientation=None):
 
     def get_local_pose(self):
         # Fallback to the xformprim method for kinematic-only objects
-        if self._rigid_prim_view is None:
+        if self.kinematic_only:
             return super().get_local_pose()
 
         positions, orientations = self._rigid_prim_view.get_local_poses()
@@ -391,7 +391,7 @@ def mass(self):
         Returns:
             float: mass of the rigid body in kg.
         """
-        assert self._rigid_prim_view is not None, "Cannot get mass for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot get mass for a kinematic-only body!"
         mass = self._rigid_prim_view.get_masses()[0]
 
         # Fallback to analytical computation of volume * density
@@ -406,7 +406,7 @@ def mass(self, mass):
         Args:
             mass (float): mass of the rigid body in kg.
         """
-        assert self._rigid_prim_view is not None, "Cannot set mass for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot set mass for a kinematic-only body!"
         self._rigid_prim_view.set_masses([mass])
 
     @property
@@ -415,7 +415,7 @@ def density(self):
         Returns:
             float: density of the rigid body in kg / m^3.
         """
-        assert self._rigid_prim_view is not None, "Cannot get density for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot get density for a kinematic-only body!"
         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
@@ -436,7 +436,7 @@ def density(self, density):
         Args:
             density (float): density of the rigid body in kg / m^3.
         """
-        assert self._rigid_prim_view is not None, "Cannot set density for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot set density for a kinematic-only body!"
         self._rigid_prim_view.set_densities([density])
 
     @property
@@ -562,14 +562,14 @@ def enable_gravity(self):
         """
         Enables gravity for this rigid body
         """
-        assert self._rigid_prim_view is not None, "Cannot enable gravity for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot enable gravity for a kinematic-only body!"
         self._rigid_prim_view.enable_gravities()
 
     def disable_gravity(self):
         """
         Disables gravity for this rigid body
         """
-        assert self._rigid_prim_view is not None, "Cannot disable gravity for a kinematic-only or uninitialized body!"
+        assert not self.kinematic_only, "Cannot disable gravity for a kinematic-only body!"
         self._rigid_prim_view.disable_gravities()
 
     def wake(self):

From 2a078d8ec5b3991c1df6c00be1ccadf53b146041 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 17:12:43 -0800
Subject: [PATCH 39/76] Revert "Create the views during initialization time so
 that we can use all information from post_load"

This reverts commit b610f6d37536096ea4e30465e0be813348d1b34c.
---
 omnigibson/prims/entity_prim.py | 10 ++++++----
 omnigibson/prims/rigid_prim.py  |  9 +++++----
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 2fc4acefe..171a02c18 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -94,6 +94,10 @@ def _post_load(self):
         # We pass in scale explicitly so that the generated links can leverage the desired entity scale
         self.update_links(load_config=dict(scale=self._load_config.get("scale", None)))
 
+        # Prepare the articulation view.
+        if self.n_joints > 0:
+            self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
+
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
             # Verify only a single link and a single mesh exists
@@ -672,10 +676,8 @@ def update_handles(self):
         """
         assert og.sim.is_playing(), "Simulator must be playing if updating handles!"
 
-        # Create the articulation view
-        if self.n_joints > 0:
-            if self._articulation_view is None:
-                self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
+        # Reinitialize the articulation view
+        if self._articulation_view is not None:
             self._articulation_view.initialize(og.sim.physics_sim_view)
 
         # Update all links and joints as well
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index a40e42a35..765d4a063 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -77,6 +77,10 @@ def __init__(
         )
 
     def _post_load(self):
+        # Create the view
+        if not self.kinematic_only:
+            self._rigid_prim_view = RigidPrimView(self._prim_path)
+
         # run super first
         super()._post_load()
 
@@ -200,10 +204,7 @@ def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
         """
-        # Create the view
-        if not self.kinematic_only:
-            if self._rigid_prim_view is None:
-                self._rigid_prim_view = RigidPrimView(self._prim_path)
+        if self._rigid_prim_view is not None:
             self._rigid_prim_view.initialize(og.sim.physics_sim_view)
 
     def contact_list(self):

From ff36fdb17f1e1c55e01a6aa728f733a41703c966 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 17:18:40 -0800
Subject: [PATCH 40/76] Make n_joints not depend on _links

---
 omnigibson/prims/entity_prim.py | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 171a02c18..f9ef3b646 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -250,6 +250,10 @@ def update_joints(self):
             # We assume this object contains a single rigid body
             self._n_dof = 0
 
+        assert self.n_joints == len(self._joints), \
+            f"Number of joints inferred from prim tree ({self.n_joints}) does not match number of joints " \
+            f"found in the articulation view ({len(self._joints)})!"
+
         self._update_joint_limits()
 
     def _update_joint_limits(self):
@@ -373,11 +377,10 @@ def n_joints(self):
         else:
             # Manually iterate over all links and check for any joints that are not fixed joints!
             num = 0
-            for link in self._links.values():
-                for child_prim in link.prim.GetChildren():
-                    prim_type = child_prim.GetPrimTypeInfo().GetTypeName().lower()
-                    if "joint" in prim_type and "fixed" not in prim_type:
-                        num += 1
+            for child_prim in self.prim.GetDescendants():
+                prim_type = child_prim.GetPrimTypeInfo().GetTypeName().lower()
+                if "joint" in prim_type and "fixed" not in prim_type:
+                    num += 1
         return num
 
     @property
@@ -388,11 +391,10 @@ def n_fixed_joints(self):
         """
         # Manually iterate over all links and check for any joints that are not fixed joints!
         num = 0
-        for link in self._links.values():
-            for child_prim in link.prim.GetChildren():
-                prim_type = child_prim.GetPrimTypeInfo().GetTypeName().lower()
-                if "joint" in prim_type and "fixed" in prim_type:
-                    num += 1
+        for child_prim in self.prim.GetDescendants():
+            prim_type = child_prim.GetPrimTypeInfo().GetTypeName().lower()
+            if "joint" in prim_type and "fixed" in prim_type:
+                num += 1
 
         return num
 

From d785ccd7a4323042d3f2b2120ff6449c9182f87e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 18:16:53 -0800
Subject: [PATCH 41/76] Move the kinematic_only decision mechanism

---
 omnigibson/objects/dataset_object.py   |  5 +++
 omnigibson/objects/object_base.py      | 47 ++++++++++++++++++--------
 omnigibson/objects/primitive_object.py |  5 +++
 omnigibson/objects/stateful_object.py  |  5 +++
 omnigibson/objects/usd_object.py       |  5 +++
 omnigibson/prims/entity_prim.py        | 11 ------
 omnigibson/prims/rigid_prim.py         | 12 -------
 omnigibson/systems/system_base.py      |  2 ++
 8 files changed, 54 insertions(+), 38 deletions(-)

diff --git a/omnigibson/objects/dataset_object.py b/omnigibson/objects/dataset_object.py
index 37e582e70..3b0da0fd7 100644
--- a/omnigibson/objects/dataset_object.py
+++ b/omnigibson/objects/dataset_object.py
@@ -50,6 +50,7 @@ def __init__(
         visible=True,
         fixed_base=False,
         visual_only=False,
+        kinematic_only=None,
         self_collisions=False,
         prim_type=PrimType.RIGID,
         load_config=None,
@@ -83,6 +84,9 @@ def __init__(
             visible (bool): whether to render this object or not in the stage
             fixed_base (bool): whether to fix the base of this object or not
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
+            kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
+                collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
+                are satisfied, else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
@@ -136,6 +140,7 @@ def __init__(
             visible=visible,
             fixed_base=fixed_base,
             visual_only=visual_only,
+            kinematic_only=kinematic_only,
             self_collisions=self_collisions,
             prim_type=prim_type,
             include_default_states=include_default_states,
diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py
index 7c1206022..687dbed82 100644
--- a/omnigibson/objects/object_base.py
+++ b/omnigibson/objects/object_base.py
@@ -47,6 +47,7 @@ def __init__(
             visible=True,
             fixed_base=False,
             visual_only=False,
+            kinematic_only=None,
             self_collisions=False,
             prim_type=PrimType.RIGID,
             load_config=None,
@@ -68,6 +69,9 @@ def __init__(
             visible (bool): whether to render this object or not in the stage
             fixed_base (bool): whether to fix the base of this object or not
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
+            kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
+                collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
+                are satisfied, else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
@@ -104,6 +108,7 @@ def __init__(
         load_config["scale"] = np.array(scale) if isinstance(scale, Iterable) else scale
         load_config["visible"] = visible
         load_config["visual_only"] = visual_only
+        load_config["kinematic_only"] = kinematic_only
         load_config["self_collisions"] = self_collisions
         load_config["prim_type"] = prim_type
 
@@ -136,27 +141,16 @@ def remove(self):
         log.info(f"Removed {self.name} from {self.prim_path}")
 
     def _post_load(self):
-        # Run super first
-        super()._post_load()
-
-        # Set visibility
-        if "visible" in self._load_config and self._load_config["visible"] is not None:
-            self.visible = self._load_config["visible"]
-
-        # First, remove any articulation root API that already exists at the object-level prim
-        if self._prim.HasAPI(UsdPhysics.ArticulationRootAPI):
-            self._prim.RemoveAPI(UsdPhysics.ArticulationRootAPI)
-            self._prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI)
-
-        # Add fixed joint if we're fixing the base
+        # 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
-            if self.n_joints == 0 and (np.all(np.isclose(self.scale, 1.0, atol=1e-3)) or self.n_fixed_joints == 0):
-                self.kinematic_only = True
+            if self.n_joints == 0 and (np.all(np.isclose(self._load_config["scale"], 1.0, atol=1e-3)) or self.n_fixed_joints == 0) and (self.load_config["kinematic_only"] != False):
+                kinematic_only = True
             else:
                 # Create fixed joint, and set Body0 to be this object's root prim
                 create_joint(
@@ -164,6 +158,29 @@ def _post_load(self):
                     joint_type="FixedJoint",
                     body1=f"{self._prim_path}/{self._root_link_name}",
                 )
+        
+        # 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
+        if kinematic_only:
+            for prim in self._prim.GetChildren():
+                if prim.GetPrimTypeInfo().GetTypeName() == "Xform":
+                    prim.GetAttribute("physics:kinematicEnabled").Set(True)
+                    prim.GetAttribute("physics:rigidBodyEnabled").Set(False)
+
+        # Run super first
+        super()._post_load()
+
+        # Set visibility
+        if "visible" in self._load_config and self._load_config["visible"] is not None:
+            self.visible = self._load_config["visible"]
+
+        # First, remove any articulation root API that already exists at the object-level prim
+        if self._prim.HasAPI(UsdPhysics.ArticulationRootAPI):
+            self._prim.RemoveAPI(UsdPhysics.ArticulationRootAPI)
+            self._prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI)
 
         # Potentially add articulation root APIs and also set self collisions
         root_prim = None if self.articulation_root_path is None else get_prim_at_path(self.articulation_root_path)
diff --git a/omnigibson/objects/primitive_object.py b/omnigibson/objects/primitive_object.py
index 54d52bf93..724c4191c 100644
--- a/omnigibson/objects/primitive_object.py
+++ b/omnigibson/objects/primitive_object.py
@@ -37,6 +37,7 @@ def __init__(
         visible=True,
         fixed_base=False,
         visual_only=False,
+        kinematic_only=None,
         self_collisions=False,
         prim_type=PrimType.RIGID,
         load_config=None,
@@ -66,6 +67,9 @@ def __init__(
             visible (bool): whether to render this object or not in the stage
             fixed_base (bool): whether to fix the base of this object or not
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
+            kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
+                collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
+                are satisfied, else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
@@ -110,6 +114,7 @@ def __init__(
             visible=visible,
             fixed_base=fixed_base,
             visual_only=visual_only,
+            kinematic_only=kinematic_only,
             self_collisions=self_collisions,
             prim_type=prim_type,
             include_default_states=include_default_states,
diff --git a/omnigibson/objects/stateful_object.py b/omnigibson/objects/stateful_object.py
index 87ec940df..f4757efda 100644
--- a/omnigibson/objects/stateful_object.py
+++ b/omnigibson/objects/stateful_object.py
@@ -66,6 +66,7 @@ def __init__(
             visible=True,
             fixed_base=False,
             visual_only=False,
+            kinematic_only=None,
             self_collisions=False,
             prim_type=PrimType.RIGID,
             load_config=None,
@@ -89,6 +90,9 @@ def __init__(
             visible (bool): whether to render this object or not in the stage
             fixed_base (bool): whether to fix the base of this object or not
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
+            kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
+                collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
+                are satisfied, else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
@@ -129,6 +133,7 @@ def __init__(
             visible=visible,
             fixed_base=fixed_base,
             visual_only=visual_only,
+            kinematic_only=kinematic_only,
             self_collisions=self_collisions,
             prim_type=prim_type,
             load_config=load_config,
diff --git a/omnigibson/objects/usd_object.py b/omnigibson/objects/usd_object.py
index 052f368f3..337e5bde5 100644
--- a/omnigibson/objects/usd_object.py
+++ b/omnigibson/objects/usd_object.py
@@ -27,6 +27,7 @@ def __init__(
         visible=True,
         fixed_base=False,
         visual_only=False,
+        kinematic_only=None,
         self_collisions=False,
         prim_type=PrimType.RIGID,
         load_config=None,
@@ -52,6 +53,9 @@ def __init__(
             visible (bool): whether to render this object or not in the stage
             fixed_base (bool): whether to fix the base of this object or not
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
+            kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
+                collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
+                are satisfied, else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
@@ -77,6 +81,7 @@ def __init__(
             visible=visible,
             fixed_base=fixed_base,
             visual_only=visual_only,
+            kinematic_only=kinematic_only,
             self_collisions=self_collisions,
             prim_type=prim_type,
             include_default_states=include_default_states,
diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index f9ef3b646..c676cef3e 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -1073,17 +1073,6 @@ def kinematic_only(self):
         """
         return self.root_link.kinematic_only
 
-    @kinematic_only.setter
-    def kinematic_only(self, val):
-        """
-        Args:
-            val (bool): Whether this object is a kinematic-only object (otherwise, it is a rigid body). A kinematic-only
-                object is not subject to simulator dynamics, and remains fixed unless the user explicitly sets the
-                body's pose / velocities. See https://docs.omniverse.nvidia.com/app_create/prod_extensions/ext_physics/rigid-bodies.html?highlight=rigid%20body%20enabled#kinematic-rigid-bodies
-                for more information
-        """
-        self.root_link.kinematic_only = val
-
     @property
     def aabb(self):
         # If we're a cloth prim type, we compute the bounding box from the limits of the particles. Otherwise, use the
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 765d4a063..8f34869c9 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -451,18 +451,6 @@ def kinematic_only(self):
         """
         return self.get_attribute("physics:kinematicEnabled")
 
-    @kinematic_only.setter
-    def kinematic_only(self, val):
-        """
-        Args:
-            val (bool): Whether this object is a kinematic-only object (otherwise, it is a rigid body). A kinematic-only
-                object is not subject to simulator dynamics, and remains fixed unless the user explicitly sets the
-                body's pose / velocities. See https://docs.omniverse.nvidia.com/app_create/prod_extensions/ext_physics/rigid-bodies.html?highlight=rigid%20body%20enabled#kinematic-rigid-bodies
-                for more information
-        """
-        self.set_attribute("physics:kinematicEnabled", val)
-        self.set_attribute("physics:rigidBodyEnabled", not val)
-
     @property
     def solver_position_iteration_count(self):
         """
diff --git a/omnigibson/systems/system_base.py b/omnigibson/systems/system_base.py
index b0f0dd560..9740f1d74 100644
--- a/omnigibson/systems/system_base.py
+++ b/omnigibson/systems/system_base.py
@@ -1154,6 +1154,7 @@ def generate_particle_template_fcn():
                         visible=False,
                         fixed_base=False,
                         visual_only=True,
+                        kinematic_only=True,
                         include_default_states=False,
                         abilities={},
                     )
@@ -1169,6 +1170,7 @@ def generate_particle_template_fcn():
                         visible=False,
                         fixed_base=False,
                         visual_only=True,
+                        kinematic_only=True,
                         include_default_states=False,
                         abilities={},
                     )

From e5748b4bba9e3b062865afe52ba1683159eb35c9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 18:22:28 -0800
Subject: [PATCH 42/76] Remove unavailable API call

---
 omnigibson/prims/entity_prim.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index c676cef3e..30e74018f 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -377,7 +377,10 @@ def n_joints(self):
         else:
             # Manually iterate over all links and check for any joints that are not fixed joints!
             num = 0
-            for child_prim in self.prim.GetDescendants():
+            children = list(self.prim.GetChildren())
+            while children:
+                child_prim = children.pop()
+                children.extend(child_prim.GetChildren())
                 prim_type = child_prim.GetPrimTypeInfo().GetTypeName().lower()
                 if "joint" in prim_type and "fixed" not in prim_type:
                     num += 1
@@ -391,7 +394,10 @@ def n_fixed_joints(self):
         """
         # Manually iterate over all links and check for any joints that are not fixed joints!
         num = 0
-        for child_prim in self.prim.GetDescendants():
+        children = list(self.prim.GetChildren())
+        while children:
+            child_prim = children.pop()
+            children.extend(child_prim.GetChildren())
             prim_type = child_prim.GetPrimTypeInfo().GetTypeName().lower()
             if "joint" in prim_type and "fixed" in prim_type:
                 num += 1

From 91f6da52adcc39b8136fcf452c1a483d90a5bbe3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 18:26:02 -0800
Subject: [PATCH 43/76] Fix typos

---
 omnigibson/objects/object_base.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py
index 687dbed82..c2fecb4bc 100644
--- a/omnigibson/objects/object_base.py
+++ b/omnigibson/objects/object_base.py
@@ -149,7 +149,7 @@ def _post_load(self):
             # 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
-            if self.n_joints == 0 and (np.all(np.isclose(self._load_config["scale"], 1.0, atol=1e-3)) or self.n_fixed_joints == 0) and (self.load_config["kinematic_only"] != False):
+            if self.n_joints == 0 and (np.all(np.isclose(self._load_config["scale"], 1.0, atol=1e-3)) or self.n_fixed_joints == 0) and (self._load_config["kinematic_only"] != False):
                 kinematic_only = True
             else:
                 # Create fixed joint, and set Body0 to be this object's root prim
@@ -160,8 +160,8 @@ def _post_load(self):
                 )
         
         # 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']}"
+        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
         if kinematic_only:

From d403e6d202a048f75aab46851fa7c919706b1e7a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 22:02:23 -0800
Subject: [PATCH 44/76] Don't touch gravity on kinematic-only objects

---
 omnigibson/prims/rigid_prim.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 8f34869c9..a6f67df58 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -358,10 +358,12 @@ def visual_only(self, val):
         # Set gravity and collisions based on value
         if val:
             self.disable_collisions()
-            self.disable_gravity()
+            if not self.kinematic_only:
+                self.disable_gravity()
         else:
             self.enable_collisions()
-            self.enable_gravity()
+            if not self.kinematic_only:
+                self.enable_gravity()
 
         # Also set the internal value
         self._visual_only = val

From 5c2fe60f2facc0bb683e22f9b26a9e18c8193a9e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 22:05:35 -0800
Subject: [PATCH 45/76] Don't set mass/density for kinematic-only objects

---
 omnigibson/prims/rigid_prim.py | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index a6f67df58..3b69ab8ca 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -96,14 +96,15 @@ def _post_load(self):
         self.update_meshes()
 
         # Possibly set the mass / density
-        if not self.has_collision_meshes:
-            # A meta (virtual) link has no collision meshes; set a negligible mass and a zero density (ignored)
-            self.mass = 1e-6
-            self.density = 0.0
-        elif "mass" in self._load_config and self._load_config["mass"] is not None:
-            self.mass = self._load_config["mass"]
-        if "density" in self._load_config and self._load_config["density"] is not None:
-            self.density = self._load_config["density"]
+        if not self.kinematic_only:
+            if not self.has_collision_meshes:
+                # A meta (virtual) link has no collision meshes; set a negligible mass and a zero density (ignored)
+                self.mass = 1e-6
+                self.density = 0.0
+            elif "mass" in self._load_config and self._load_config["mass"] is not None:
+                self.mass = self._load_config["mass"]
+            if "density" in self._load_config and self._load_config["density"] is not None:
+                self.density = self._load_config["density"]
 
         # Set the visual-only attribute
         # This automatically handles setting collisions / gravity appropriately

From ffd84bca516d796bacc1eeab5f5c82e73aa5940a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Wed, 13 Dec 2023 22:11:59 -0800
Subject: [PATCH 46/76] Do not initialize rigid prim view for kinematic-only
 objects.

---
 omnigibson/prims/rigid_prim.py | 61 ++++++++++------------------------
 1 file changed, 18 insertions(+), 43 deletions(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 3b69ab8ca..8daaa5bd5 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -78,8 +78,7 @@ def __init__(
 
     def _post_load(self):
         # Create the view
-        if not self.kinematic_only:
-            self._rigid_prim_view = RigidPrimView(self._prim_path)
+        self._rigid_prim_view = RigidPrimView(self._prim_path)
 
         # run super first
         super()._post_load()
@@ -96,15 +95,14 @@ def _post_load(self):
         self.update_meshes()
 
         # Possibly set the mass / density
-        if not self.kinematic_only:
-            if not self.has_collision_meshes:
-                # A meta (virtual) link has no collision meshes; set a negligible mass and a zero density (ignored)
-                self.mass = 1e-6
-                self.density = 0.0
-            elif "mass" in self._load_config and self._load_config["mass"] is not None:
-                self.mass = self._load_config["mass"]
-            if "density" in self._load_config and self._load_config["density"] is not None:
-                self.density = self._load_config["density"]
+        if not self.has_collision_meshes:
+            # A meta (virtual) link has no collision meshes; set a negligible mass and a zero density (ignored)
+            self.mass = 1e-6
+            self.density = 0.0
+        elif "mass" in self._load_config and self._load_config["mass"] is not None:
+            self.mass = self._load_config["mass"]
+        if "density" in self._load_config and self._load_config["density"] is not None:
+            self.density = self._load_config["density"]
 
         # Set the visual-only attribute
         # This automatically handles setting collisions / gravity appropriately
@@ -205,7 +203,10 @@ def update_handles(self):
         """
         Updates all internal handles for this prim, in case they change since initialization
         """
-        if self._rigid_prim_view is not None:
+        # We only do this for non-kinematic objects, because while the USD APIs for kinematic-only
+        # and dynamic objects are the same, physx tensor APIs do NOT exist for kinematic-only
+        # objects, meaning initializing the view actively breaks the view.
+        if not self.kinematic_only:
             self._rigid_prim_view.initialize(og.sim.physics_sim_view)
 
     def contact_list(self):
@@ -234,7 +235,6 @@ def set_linear_velocity(self, velocity):
         Args:
             velocity (np.ndarray): linear velocity to set the rigid prim to. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot set linear velocity for a kinematic-only body!"
         self._rigid_prim_view.set_linear_velocities(velocity[None, :])
 
     def get_linear_velocity(self):
@@ -242,7 +242,6 @@ def get_linear_velocity(self):
         Returns:
             np.ndarray: current linear velocity of the the rigid prim. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot get linear velocity for a kinematic-only body!"
         return self._rigid_prim_view.get_linear_velocities()[0]
 
     def set_angular_velocity(self, velocity):
@@ -252,7 +251,6 @@ def set_angular_velocity(self, velocity):
         Args:
             velocity (np.ndarray): angular velocity to set the rigid prim to. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot set angular velocity for a kinematic-only body!"
         self._rigid_prim_view.set_angular_velocities(velocity[None, :])
 
     def get_angular_velocity(self):
@@ -260,13 +258,11 @@ def get_angular_velocity(self):
         Returns:
             np.ndarray: current angular velocity of the the rigid prim. Shape (3,).
         """
-        assert not self.kinematic_only, "Cannot get angular velocity for a kinematic-only body!"
         return self._rigid_prim_view.get_angular_velocities()[0]
 
     def set_position_orientation(self, position=None, orientation=None):
         # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
-            return super().set_position_orientation(position=position, orientation=orientation)
+        return super().set_position_orientation(position=position, orientation=orientation)
 
         if position is not None:
             position = np.asarray(position)[None, :]
@@ -277,10 +273,6 @@ def set_position_orientation(self, position=None, orientation=None):
         self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation)
 
     def get_position_orientation(self):
-        # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
-            return super().get_position_orientation()
-        
         pos, ori = self._rigid_prim_view.get_world_poses()
 
         assert np.isclose(np.linalg.norm(ori), 1, atol=1e-3), \
@@ -288,10 +280,6 @@ def get_position_orientation(self):
         return pos[0], ori[0]
 
     def set_local_pose(self, position=None, orientation=None):
-        # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
-            return super().set_local_pose(position=position, orientation=orientation)
-        
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -299,10 +287,6 @@ def set_local_pose(self, position=None, orientation=None):
         self._rigid_prim_view.set_local_poses(position, orientation)
 
     def get_local_pose(self):
-        # Fallback to the xformprim method for kinematic-only objects
-        if self.kinematic_only:
-            return super().get_local_pose()
-
         positions, orientations = self._rigid_prim_view.get_local_poses()
         return positions[0], orientations[0]
 
@@ -359,12 +343,10 @@ def visual_only(self, val):
         # Set gravity and collisions based on value
         if val:
             self.disable_collisions()
-            if not self.kinematic_only:
-                self.disable_gravity()
+            self.disable_gravity()
         else:
             self.enable_collisions()
-            if not self.kinematic_only:
-                self.enable_gravity()
+            self.enable_gravity()
 
         # Also set the internal value
         self._visual_only = val
@@ -395,7 +377,6 @@ def mass(self):
         Returns:
             float: mass of the rigid body in kg.
         """
-        assert not self.kinematic_only, "Cannot get mass for a kinematic-only body!"
         mass = self._rigid_prim_view.get_masses()[0]
 
         # Fallback to analytical computation of volume * density
@@ -410,7 +391,6 @@ def mass(self, mass):
         Args:
             mass (float): mass of the rigid body in kg.
         """
-        assert not self.kinematic_only, "Cannot set mass for a kinematic-only body!"
         self._rigid_prim_view.set_masses([mass])
 
     @property
@@ -419,7 +399,6 @@ def density(self):
         Returns:
             float: density of the rigid body in kg / m^3.
         """
-        assert not self.kinematic_only, "Cannot get density for a kinematic-only body!"
         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
@@ -440,7 +419,6 @@ def density(self, density):
         Args:
             density (float): density of the rigid body in kg / m^3.
         """
-        assert not self.kinematic_only, "Cannot set density for a kinematic-only body!"
         self._rigid_prim_view.set_densities([density])
 
     @property
@@ -554,14 +532,12 @@ def enable_gravity(self):
         """
         Enables gravity for this rigid body
         """
-        assert not self.kinematic_only, "Cannot enable gravity for a kinematic-only body!"
         self._rigid_prim_view.enable_gravities()
 
     def disable_gravity(self):
         """
         Disables gravity for this rigid body
         """
-        assert not self.kinematic_only, "Cannot disable gravity for a kinematic-only body!"
         self._rigid_prim_view.disable_gravities()
 
     def wake(self):
@@ -591,9 +567,8 @@ def _load_state(self, state):
         super()._load_state(state=state)
 
         # Set velocities if not kinematic
-        if not self.kinematic_only:
-            self.set_linear_velocity(np.array(state["lin_vel"]))
-            self.set_angular_velocity(np.array(state["ang_vel"]))
+        self.set_linear_velocity(np.array(state["lin_vel"]))
+        self.set_angular_velocity(np.array(state["ang_vel"]))
 
     def _serialize(self, state):
         # Run super first

From 19b768931f3d5677d9e39bd0752bb9c535ac61c4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 15 Dec 2023 13:29:43 -0800
Subject: [PATCH 47/76] Some fixes

---
 omnigibson/objects/object_base.py |  3 ++-
 omnigibson/prims/entity_prim.py   | 10 +++++-----
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py
index c2fecb4bc..d09d55087 100644
--- a/omnigibson/objects/object_base.py
+++ b/omnigibson/objects/object_base.py
@@ -149,7 +149,8 @@ def _post_load(self):
             # 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
-            if self.n_joints == 0 and (np.all(np.isclose(self._load_config["scale"], 1.0, atol=1e-3)) or self.n_fixed_joints == 0) and (self._load_config["kinematic_only"] != False):
+            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
             else:
                 # Create fixed joint, and set Body0 to be this object's root prim
diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 30e74018f..e86c7715b 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -513,11 +513,11 @@ def set_joint_positions(self, positions, indices=None, normalized=False, drive=F
 
         # Set the DOF states
         if not drive:
-            self._articulation_view.set_joint_positions(positions=positions, joint_indices=indices)
+            self._articulation_view.set_joint_positions(positions, joint_indices=indices)
             BoundingBoxAPI.clear()
 
         # Also set the target
-        self._articulation_view.set_joint_position_targets(positions=positions, joint_indices=indices)
+        self._articulation_view.set_joint_position_targets(positions, joint_indices=indices)
 
     def set_joint_velocities(self, velocities, indices=None, normalized=False, drive=False):
         """
@@ -545,10 +545,10 @@ def set_joint_velocities(self, velocities, indices=None, normalized=False, drive
 
         # Set the DOF states
         if not drive:
-            self._articulation_view.set_joint_velocities(velocities=velocities, joint_indices=indices)
+            self._articulation_view.set_joint_velocities(velocities, joint_indices=indices)
 
         # Also set the target
-        self._articulation_view.set_joint_velocity_targets(velocities=velocities, joint_indices=indices)
+        self._articulation_view.set_joint_velocity_targets(velocities, joint_indices=indices)
 
     def set_joint_efforts(self, efforts, indices=None, normalized=False):
         """
@@ -572,7 +572,7 @@ def set_joint_efforts(self, efforts, indices=None, normalized=False):
             efforts = self._denormalize_efforts(efforts=efforts, indices=indices)
 
         # Set the DOF states
-        self._articulation_view.set_joint_efforts(velocities=efforts, joint_indices=indices)
+        self._articulation_view.set_joint_efforts(efforts, joint_indices=indices)
 
     def _normalize_positions(self, positions, indices=None):
         """

From 371496fa7e6a6b75b2b7a99f2f35c0a74bbda949 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 15 Dec 2023 14:21:14 -0800
Subject: [PATCH 48/76] Some fixes

---
 omnigibson/objects/object_base.py             |  22 +-
 omnigibson/prims/entity_prim.py               |   9 +-
 omnigibson/prims/rigid_prim.py                |   6 +
 omnigibson/utils/deprecated_utils.py          | 295 +++++++++++++++++-
 .../utils/extended_articulation_view.py       | 234 --------------
 5 files changed, 315 insertions(+), 251 deletions(-)
 delete mode 100644 omnigibson/utils/extended_articulation_view.py

diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py
index d09d55087..f1ca153b1 100644
--- a/omnigibson/objects/object_base.py
+++ b/omnigibson/objects/object_base.py
@@ -152,28 +152,26 @@ def _post_load(self):
             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
-            else:
-                # Create fixed joint, and set Body0 to be this object's root prim
-                create_joint(
-                    prim_path=f"{self._prim_path}/rootJoint",
-                    joint_type="FixedJoint",
-                    body1=f"{self._prim_path}/{self._root_link_name}",
-                )
         
         # 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
-        if kinematic_only:
-            for prim in self._prim.GetChildren():
-                if prim.GetPrimTypeInfo().GetTypeName() == "Xform":
-                    prim.GetAttribute("physics:kinematicEnabled").Set(True)
-                    prim.GetAttribute("physics:rigidBodyEnabled").Set(False)
+        self._load_config["kinematic_only"] = kinematic_only
 
         # Run super first
         super()._post_load()
 
+        # If the object is fixed_base but kinematic only is false, create the joint
+        if self.fixed_base and not self.kinematic_only:
+            # Create fixed joint, and set Body0 to be this object's root prim
+            create_joint(
+                prim_path=f"{self._prim_path}/rootJoint",
+                joint_type="FixedJoint",
+                body1=f"{self._prim_path}/{self._root_link_name}",
+            )
+
         # Set visibility
         if "visible" in self._load_config and self._load_config["visible"] is not None:
             self.visible = self._load_config["visible"]
diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index e86c7715b..b7b11a3e2 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.extended_articulation_view import ExtendedArticulationView
+from omnigibson.utils.deprecated_utils import ArticulationView
 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
@@ -92,11 +92,14 @@ def _load(self):
     def _post_load(self):
         # Setup links info FIRST before running any other post loading behavior
         # We pass in scale explicitly so that the generated links can leverage the desired entity scale
-        self.update_links(load_config=dict(scale=self._load_config.get("scale", None)))
+        self.update_links(load_config=dict(
+            scale=self._load_config.get("scale", None),
+            kinematic_only=self._load_config.get("kinematic_only", None)
+        ))
 
         # Prepare the articulation view.
         if self.n_joints > 0:
-            self._articulation_view = ExtendedArticulationView(self._prim_path + "/base_link")
+            self._articulation_view = ArticulationView(self._prim_path + "/base_link")
 
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 8daaa5bd5..7c9704ddb 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -52,6 +52,7 @@ class RigidPrim(XFormPrim):
             density (None or float): If specified, density of this body in kg / m^3
             visual_only (None or bool): If specified, whether this prim should include collisions or not.
                 Default is True.
+            kinematic_only (None or bool): If specified, whether this prim should be kinematic-only or not.
     """
 
     def __init__(
@@ -80,6 +81,11 @@ def _post_load(self):
         # Create the view
         self._rigid_prim_view = RigidPrimView(self._prim_path)
 
+        # Set it to be kinematic if necessary
+        if "kinematic_only" in self._load_config and self._load_config["kinematic_only"]:
+            self.set_attribute("physics:kinematicEnabled", True)
+            self.set_attribute("physics:rigidBodyEnabled", False)
+
         # run super first
         super()._post_load()
 
diff --git a/omnigibson/utils/deprecated_utils.py b/omnigibson/utils/deprecated_utils.py
index b4df02c6b..df2321836 100644
--- a/omnigibson/utils/deprecated_utils.py
+++ b/omnigibson/utils/deprecated_utils.py
@@ -2,15 +2,25 @@
 A set of utility functions slated to be deprecated once Omniverse bugs are fixed
 """
 import carb
-from typing import Callable
+from typing import List, Optional, Tuple, Union, Callable
 import omni.usd as ou
 from omni.particle.system.core.scripts.core import Core as OmniCore
 from omni.particle.system.core.scripts.utils import Utils as OmniUtils
-from pxr import Sdf, UsdShade
+from pxr import Sdf, UsdShade, PhysxSchema, Usd, UsdGeom, UsdPhysics
 import omni
 import omni.graph.core as ogc
 from omni.kit.primitive.mesh.command import _get_all_evaluators
 from omni.kit.primitive.mesh.command import CreateMeshPrimWithDefaultXformCommand as CMPWDXC
+import omni.timeline
+from omni.isaac.core.utils.prims import get_prim_at_path
+import numpy as np
+import torch
+import warp as wp
+import math
+from omni.isaac.core.articulations import ArticulationView as _ArticulationView
+from omni.isaac.core.prims import RigidPrimView as _RigidPrimView
+
+DEG2RAD = math.pi / 180.0
 
 
 class CreateMeshPrimWithDefaultXformCommand(CMPWDXC):
@@ -185,3 +195,284 @@ def get_compute_graph(self, selected_paths, create_new_graph=True, created_paths
             carb.log_info(f"No ComputeGraph selected. A new graph has been created at {graph.get_path_to_graph()}")
 
         return graph
+
+
+class ArticulationView(_ArticulationView):
+    """ArticulationView with some additional functionality implemented."""
+
+    def set_joint_limits(
+        self,
+        values: Union[np.ndarray, torch.Tensor],
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+    ) -> None:
+        """Sets joint limits for articulation joints in the view.
+
+        Args:
+            values (Union[np.ndarray, torch.Tensor, wp.array]): joint limits for articulations in the view. shape (M, K, 2).
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to manipulate. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to manipulate. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
+            new_values = self._physics_view.get_dof_limits()
+            values = self._backend_utils.move_data(values, device="cpu")
+            new_values = self._backend_utils.assign(
+                values,
+                new_values,
+                [self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices],
+            )
+            self._physics_view.set_dof_limits(new_values, indices)
+        else:
+            indices = self._backend_utils.to_list(
+                self._backend_utils.resolve_indices(indices, self.count, self._device)
+            )
+            dof_types = self._backend_utils.to_list(self.get_dof_types())
+            joint_indices = self._backend_utils.to_list(
+                self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            )
+            values = self._backend_utils.to_list(values)
+            articulation_read_idx = 0
+            for i in indices:
+                dof_read_idx = 0
+                for dof_index in joint_indices:
+                    dof_val = values[articulation_read_idx][dof_read_idx]
+                    if dof_types[dof_index] == omni.physics.tensors.DofType.Rotation:
+                        dof_val /= DEG2RAD
+                    prim = get_prim_at_path(self._dof_paths[i][dof_index])
+                    prim.GetAttribute("physics:lowerLimit").Set(dof_val[0])
+                    prim.GetAttribute("physics:upperLimit").Set(dof_val[1])
+                    dof_read_idx += 1
+                articulation_read_idx += 1
+        return
+    
+    def get_joint_limits(
+        self,
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        clone: bool = True,
+    ) -> Union[np.ndarray, torch.Tensor, wp.array]:
+        """Gets joint limits for articulation in the view.
+
+        Args:
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to query. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to query. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+            clone (Optional[bool]): True to return a clone of the internal buffer. Otherwise False. Defaults to True.
+
+        Returns:
+            Union[np.ndarray, torch.Tensor, wp.indexedarray]: joint limits for articulations in the view. shape (M, K).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return None
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            values = self._backend_utils.move_data(self._physics_view.get_dof_limits(), self._device)
+            if clone:
+                values = self._backend_utils.clone_tensor(values, device=self._device)
+            result = values[
+                self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices
+            ]
+            return result
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            dof_types = self._backend_utils.to_list(self.get_dof_types())
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            values = np.zeros(shape=(indices.shape[0], joint_indices.shape[0], 2), dtype="float32")
+            articulation_write_idx = 0
+            indices = self._backend_utils.to_list(indices)
+            joint_indices = self._backend_utils.to_list(joint_indices)
+            for i in indices:
+                dof_write_idx = 0
+                for dof_index in joint_indices:
+                    prim = get_prim_at_path(self._dof_paths[i][dof_index])
+                    values[articulation_write_idx][dof_write_idx][0] = prim.GetAttribute("physics:lowerLimit").Get()
+                    values[articulation_write_idx][dof_write_idx][1] = prim.GetAttribute("physics:upperLimit").Get()
+                    if dof_types[dof_index] == omni.physics.tensors.DofType.Rotation:
+                        values[articulation_write_idx][dof_write_idx] = values[articulation_write_idx][dof_write_idx] * DEG2RAD
+                    dof_write_idx += 1
+                articulation_write_idx += 1
+            values = self._backend_utils.convert(values, dtype="float32", device=self._device, indexed=True)
+            return values
+        
+    def set_max_velocities(
+        self,
+        values: Union[np.ndarray, torch.Tensor, wp.array],
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+    ) -> None:
+        """Sets maximum velocities for articulation in the view.
+
+        Args:
+            values (Union[np.ndarray, torch.Tensor, wp.array]): maximum velocities for articulations in the view. shape (M, K).
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to manipulate. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to manipulate. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
+            new_values = self._physics_view.get_dof_max_velocities()
+            new_values = self._backend_utils.assign(
+                self._backend_utils.move_data(values, device="cpu"),
+                new_values,
+                [self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices],
+            )
+            self._physics_view.set_dof_max_velocities(new_values, indices)
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            articulation_read_idx = 0
+            indices = self._backend_utils.to_list(indices)
+            joint_indices = self._backend_utils.to_list(joint_indices)
+            values = self._backend_utils.to_list(values)
+            for i in indices:
+                dof_read_idx = 0
+                for dof_index in joint_indices:
+                    prim = PhysxSchema.PhysxJointAPI(get_prim_at_path(self._dof_paths[i][dof_index]))
+                    if not prim.GetMaxJointVelocityAttr():
+                        prim.CreateMaxJointVelocityAttr().Set(values[articulation_read_idx][dof_read_idx])
+                    else:
+                        prim.GetMaxJointVelocityAttr().Set(values[articulation_read_idx][dof_read_idx])
+                    dof_read_idx += 1
+                articulation_read_idx += 1
+        return
+
+    def get_max_velocities(
+        self,
+        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
+        clone: bool = True,
+    ) -> Union[np.ndarray, torch.Tensor, wp.indexedarray]:
+        """Gets maximum velocities for articulation in the view.
+
+        Args:
+            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to query. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
+                                                                                 to query. Shape (K,).
+                                                                                 Where K <= num of dofs.
+                                                                                 Defaults to None (i.e: all dofs).
+            clone (Optional[bool]): True to return a clone of the internal buffer. Otherwise False. Defaults to True.
+
+        Returns:
+            Union[np.ndarray, torch.Tensor, wp.indexedarray]: maximum velocities for articulations in the view. shape (M, K).
+        """
+        if not self._is_initialized:
+            carb.log_warn("ArticulationView needs to be initialized.")
+            return None
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
+            max_velocities = self._physics_view.get_dof_max_velocities()
+            if clone:
+                max_velocities = self._backend_utils.clone_tensor(max_velocities, device="cpu")
+            result = self._backend_utils.move_data(
+                max_velocities[
+                    self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices
+                ],
+                device=self._device,
+            )
+            return result
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
+            max_velocities = np.zeros(shape=(indices.shape[0], joint_indices.shape[0]), dtype="float32")
+            indices = self._backend_utils.to_list(indices)
+            joint_indices = self._backend_utils.to_list(joint_indices)
+            articulation_write_idx = 0
+            for i in indices:
+                dof_write_idx = 0
+                for dof_index in joint_indices:
+                    prim = PhysxSchema.PhysxJointAPI(get_prim_at_path(self._dof_paths[i][dof_index]))
+                    max_velocities[articulation_write_idx][dof_write_idx] = prim.GetMaxJointVelocityAttr().Get()
+                    dof_write_idx += 1
+                articulation_write_idx += 1
+            max_velocities = self._backend_utils.convert(max_velocities, dtype="float32", device=self._device, indexed=True)
+            return max_velocities
+        
+
+class RigidPrimView(_RigidPrimView):
+    def enable_gravities(self, indices: Optional[Union[np.ndarray, list, torch.Tensor, wp.array]] = None) -> None:
+        """Enable gravity on rigid bodies (enabled by default).
+
+        Args:
+            indices (Optional[Union[np.ndarray, list, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to manipulate. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+        """
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+            data = self._physics_view.get_disable_gravities().reshape(self._count)
+            data = self._backend_utils.assign(
+                self._backend_utils.create_tensor_from_list([False] * len(indices), dtype="uint8"), data, indices
+            )
+            self._physics_view.set_disable_gravities(data, indices)
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            indices = self._backend_utils.to_list(indices)
+            for i in indices:
+                if self._physx_rigid_body_apis[i] is None:
+                    if self._prims[i].HasAPI(PhysxSchema.PhysxRigidBodyAPI):
+                        rigid_api = PhysxSchema.PhysxRigidBodyAPI(self._prims[i])
+                    else:
+                        rigid_api = PhysxSchema.PhysxRigidBodyAPI.Apply(self._prims[i])
+                    self._physx_rigid_body_apis[i] = rigid_api
+                self._physx_rigid_body_apis[i].GetDisableGravityAttr().Set(False)
+
+    def disable_gravities(self, indices: Optional[Union[np.ndarray, list, torch.Tensor, wp.array]] = None) -> None:
+        """Disable gravity on rigid bodies (enabled by default).
+
+        Args:
+            indices (Optional[Union[np.ndarray, list, torch.Tensor, wp.array]], optional): indicies to specify which prims
+                                                                                 to manipulate. Shape (M,).
+                                                                                 Where M <= size of the encapsulated prims in the view.
+                                                                                 Defaults to None (i.e: all prims in the view).
+        """
+        indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
+        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
+            data = self._physics_view.get_disable_gravities().reshape(self._count)
+            data = self._backend_utils.assign(
+                self._backend_utils.create_tensor_from_list([True] * len(indices), dtype="uint8"), data, indices
+            )
+            self._physics_view.set_disable_gravities(data, indices)
+        else:
+            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
+            indices = self._backend_utils.to_list(indices)
+            for i in indices:
+                if self._physx_rigid_body_apis[i] is None:
+                    if self._prims[i].HasAPI(PhysxSchema.PhysxRigidBodyAPI):
+                        rigid_api = PhysxSchema.PhysxRigidBodyAPI(self._prims[i])
+                    else:
+                        rigid_api = PhysxSchema.PhysxRigidBodyAPI.Apply(self._prims[i])
+                    self._physx_rigid_body_apis[i] = rigid_api
+                self._physx_rigid_body_apis[i].GetDisableGravityAttr().Set(True)
+            return
\ No newline at end of file
diff --git a/omnigibson/utils/extended_articulation_view.py b/omnigibson/utils/extended_articulation_view.py
deleted file mode 100644
index 30d297dcb..000000000
--- a/omnigibson/utils/extended_articulation_view.py
+++ /dev/null
@@ -1,234 +0,0 @@
-from typing import List, Optional, Tuple, Union
-import carb
-import omni.timeline
-from omni.isaac.core.utils.prims import get_prim_at_path
-from pxr import PhysxSchema, Usd, UsdGeom, UsdPhysics
-import numpy as np
-import torch
-import warp as wp
-import math
-
-from omni.isaac.core.articulations import ArticulationView
-
-DEG2RAD = math.pi / 180.0
-
-class ExtendedArticulationView(ArticulationView):
-    """ArticulationView with some additional functionality implemented."""
-
-    def set_joint_limits(
-        self,
-        values: Union[np.ndarray, torch.Tensor],
-        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-    ) -> None:
-        """Sets joint limits for articulation joints in the view.
-
-        Args:
-            values (Union[np.ndarray, torch.Tensor, wp.array]): joint limits for articulations in the view. shape (M, K, 2).
-            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
-                                                                                 to manipulate. Shape (M,).
-                                                                                 Where M <= size of the encapsulated prims in the view.
-                                                                                 Defaults to None (i.e: all prims in the view).
-            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
-                                                                                 to manipulate. Shape (K,).
-                                                                                 Where K <= num of dofs.
-                                                                                 Defaults to None (i.e: all dofs).
-        """
-        if not self._is_initialized:
-            carb.log_warn("ArticulationView needs to be initialized.")
-            return
-        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
-            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
-            new_values = self._physics_view.get_dof_limits()
-            values = self._backend_utils.move_data(values, device="cpu")
-            new_values = self._backend_utils.assign(
-                values,
-                new_values,
-                [self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices],
-            )
-            self._physics_view.set_dof_limits(new_values, indices)
-        else:
-            indices = self._backend_utils.to_list(
-                self._backend_utils.resolve_indices(indices, self.count, self._device)
-            )
-            dof_types = self._backend_utils.to_list(self.get_dof_types())
-            joint_indices = self._backend_utils.to_list(
-                self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
-            )
-            values = self._backend_utils.to_list(values)
-            articulation_read_idx = 0
-            for i in indices:
-                dof_read_idx = 0
-                for dof_index in joint_indices:
-                    dof_val = values[articulation_read_idx][dof_read_idx]
-                    if dof_types[dof_index] == omni.physics.tensors.DofType.Rotation:
-                        dof_val /= DEG2RAD
-                    prim = get_prim_at_path(self._dof_paths[i][dof_index])
-                    prim.GetAttribute("physics:lowerLimit").Set(dof_val[0])
-                    prim.GetAttribute("physics:upperLimit").Set(dof_val[1])
-                    dof_read_idx += 1
-                articulation_read_idx += 1
-        return
-    
-    def get_joint_limits(
-        self,
-        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-        clone: bool = True,
-    ) -> Union[np.ndarray, torch.Tensor, wp.array]:
-        """Gets joint limits for articulation in the view.
-
-        Args:
-            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
-                                                                                 to query. Shape (M,).
-                                                                                 Where M <= size of the encapsulated prims in the view.
-                                                                                 Defaults to None (i.e: all prims in the view).
-            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
-                                                                                 to query. Shape (K,).
-                                                                                 Where K <= num of dofs.
-                                                                                 Defaults to None (i.e: all dofs).
-            clone (Optional[bool]): True to return a clone of the internal buffer. Otherwise False. Defaults to True.
-
-        Returns:
-            Union[np.ndarray, torch.Tensor, wp.indexedarray]: joint limits for articulations in the view. shape (M, K).
-        """
-        if not self._is_initialized:
-            carb.log_warn("ArticulationView needs to be initialized.")
-            return None
-        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
-            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
-            values = self._backend_utils.move_data(self._physics_view.get_dof_limits(), self._device)
-            if clone:
-                values = self._backend_utils.clone_tensor(values, device=self._device)
-            result = values[
-                self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices
-            ]
-            return result
-        else:
-            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
-            dof_types = self._backend_utils.to_list(self.get_dof_types())
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
-            values = np.zeros(shape=(indices.shape[0], joint_indices.shape[0], 2), dtype="float32")
-            articulation_write_idx = 0
-            indices = self._backend_utils.to_list(indices)
-            joint_indices = self._backend_utils.to_list(joint_indices)
-            for i in indices:
-                dof_write_idx = 0
-                for dof_index in joint_indices:
-                    prim = get_prim_at_path(self._dof_paths[i][dof_index])
-                    values[articulation_write_idx][dof_write_idx][0] = prim.GetAttribute("physics:lowerLimit").Get()
-                    values[articulation_write_idx][dof_write_idx][1] = prim.GetAttribute("physics:upperLimit").Get()
-                    if dof_types[dof_index] == omni.physics.tensors.DofType.Rotation:
-                        values[articulation_write_idx][dof_write_idx] = values[articulation_write_idx][dof_write_idx] * DEG2RAD
-                    dof_write_idx += 1
-                articulation_write_idx += 1
-            values = self._backend_utils.convert(values, dtype="float32", device=self._device, indexed=True)
-            return values
-        
-    def set_max_velocities(
-        self,
-        values: Union[np.ndarray, torch.Tensor, wp.array],
-        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-    ) -> None:
-        """Sets maximum velocities for articulation in the view.
-
-        Args:
-            values (Union[np.ndarray, torch.Tensor, wp.array]): maximum velocities for articulations in the view. shape (M, K).
-            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
-                                                                                 to manipulate. Shape (M,).
-                                                                                 Where M <= size of the encapsulated prims in the view.
-                                                                                 Defaults to None (i.e: all prims in the view).
-            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
-                                                                                 to manipulate. Shape (K,).
-                                                                                 Where K <= num of dofs.
-                                                                                 Defaults to None (i.e: all dofs).
-        """
-        if not self._is_initialized:
-            carb.log_warn("ArticulationView needs to be initialized.")
-            return
-        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
-            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
-            new_values = self._physics_view.get_dof_max_velocities()
-            new_values = self._backend_utils.assign(
-                self._backend_utils.move_data(values, device="cpu"),
-                new_values,
-                [self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices],
-            )
-            self._physics_view.set_dof_max_velocities(new_values, indices)
-        else:
-            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
-            articulation_read_idx = 0
-            indices = self._backend_utils.to_list(indices)
-            joint_indices = self._backend_utils.to_list(joint_indices)
-            values = self._backend_utils.to_list(values)
-            for i in indices:
-                dof_read_idx = 0
-                for dof_index in joint_indices:
-                    prim = PhysxSchema.PhysxJointAPI(get_prim_at_path(self._dof_paths[i][dof_index]))
-                    if not prim.GetMaxJointVelocityAttr():
-                        prim.CreateMaxJointVelocityAttr().Set(values[articulation_read_idx][dof_read_idx])
-                    else:
-                        prim.GetMaxJointVelocityAttr().Set(values[articulation_read_idx][dof_read_idx])
-                    dof_read_idx += 1
-                articulation_read_idx += 1
-        return
-
-    def get_max_velocities(
-        self,
-        indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-        joint_indices: Optional[Union[np.ndarray, List, torch.Tensor, wp.array]] = None,
-        clone: bool = True,
-    ) -> Union[np.ndarray, torch.Tensor, wp.indexedarray]:
-        """Gets maximum velocities for articulation in the view.
-
-        Args:
-            indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): indicies to specify which prims
-                                                                                 to query. Shape (M,).
-                                                                                 Where M <= size of the encapsulated prims in the view.
-                                                                                 Defaults to None (i.e: all prims in the view).
-            joint_indices (Optional[Union[np.ndarray, List, torch.Tensor, wp.array]], optional): joint indicies to specify which joints
-                                                                                 to query. Shape (K,).
-                                                                                 Where K <= num of dofs.
-                                                                                 Defaults to None (i.e: all dofs).
-            clone (Optional[bool]): True to return a clone of the internal buffer. Otherwise False. Defaults to True.
-
-        Returns:
-            Union[np.ndarray, torch.Tensor, wp.indexedarray]: maximum velocities for articulations in the view. shape (M, K).
-        """
-        if not self._is_initialized:
-            carb.log_warn("ArticulationView needs to be initialized.")
-            return None
-        if not omni.timeline.get_timeline_interface().is_stopped() and self._physics_view is not None:
-            indices = self._backend_utils.resolve_indices(indices, self.count, "cpu")
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, "cpu")
-            max_velocities = self._physics_view.get_dof_max_velocities()
-            if clone:
-                max_velocities = self._backend_utils.clone_tensor(max_velocities, device="cpu")
-            result = self._backend_utils.move_data(
-                max_velocities[
-                    self._backend_utils.expand_dims(indices, 1) if self._backend != "warp" else indices, joint_indices
-                ],
-                device=self._device,
-            )
-            return result
-        else:
-            indices = self._backend_utils.resolve_indices(indices, self.count, self._device)
-            joint_indices = self._backend_utils.resolve_indices(joint_indices, self.num_dof, self._device)
-            max_velocities = np.zeros(shape=(indices.shape[0], joint_indices.shape[0]), dtype="float32")
-            indices = self._backend_utils.to_list(indices)
-            joint_indices = self._backend_utils.to_list(joint_indices)
-            articulation_write_idx = 0
-            for i in indices:
-                dof_write_idx = 0
-                for dof_index in joint_indices:
-                    prim = PhysxSchema.PhysxJointAPI(get_prim_at_path(self._dof_paths[i][dof_index]))
-                    max_velocities[articulation_write_idx][dof_write_idx] = prim.GetMaxJointVelocityAttr().Get()
-                    dof_write_idx += 1
-                articulation_write_idx += 1
-            max_velocities = self._backend_utils.convert(max_velocities, dtype="float32", device=self._device, indexed=True)
-            return max_velocities
\ No newline at end of file

From 45010bc81477431f7433ea98b53913171cea4a3d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 15 Dec 2023 14:49:23 -0800
Subject: [PATCH 49/76] Use custom rigid prim class

---
 omnigibson/prims/rigid_prim.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 7c9704ddb..ba0aaa9a7 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -1,4 +1,3 @@
-from omni.isaac.core.prims import RigidPrimView
 from omni.isaac.core.utils.prims import get_prim_at_path, get_prim_parent
 from omni.isaac.core.utils.transformations import tf_matrix_from_pose
 from omni.isaac.core.utils.rotations import gf_quat_to_np_array
@@ -11,6 +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.sim_utils import CsRawData
 from omnigibson.utils.usd_utils import get_mesh_volume_and_com
 import omnigibson.utils.transform_utils as T

From e9eda29d6b36bbbba92866405437b477d95360ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 15 Dec 2023 17:02:16 -0800
Subject: [PATCH 50/76] Bunch of fixes

---
 omnigibson/prims/entity_prim.py | 72 +++++++++++++++++++++------------
 omnigibson/prims/joint_prim.py  | 16 +++++++-
 omnigibson/prims/rigid_prim.py  | 33 ++++++++++-----
 3 files changed, 85 insertions(+), 36 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index b7b11a3e2..e0a7cfd99 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -54,7 +54,7 @@ def __init__(
         self._joints = None
         self._materials = None
         self._visual_only = None
-        self._articulation_view = None
+        self._articulation_view_direct = None
 
         # This needs to be initialized to be used for _load() of PrimitiveObject
         self._prim_type = load_config["prim_type"] if load_config is not None and "prim_type" in load_config else PrimType.RIGID
@@ -92,14 +92,11 @@ def _load(self):
     def _post_load(self):
         # Setup links info FIRST before running any other post loading behavior
         # We pass in scale explicitly so that the generated links can leverage the desired entity scale
-        self.update_links(load_config=dict(
-            scale=self._load_config.get("scale", None),
-            kinematic_only=self._load_config.get("kinematic_only", None)
-        ))
+        self.update_links()
 
         # Prepare the articulation view.
         if self.n_joints > 0:
-            self._articulation_view = ArticulationView(self._prim_path + "/base_link")
+            self._articulation_view_direct = ArticulationView(self._prim_path + "/base_link")
 
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
@@ -162,15 +159,15 @@ def _post_load(self):
 
         self._materials = materials
 
-    def update_links(self, load_config=None):
+    def update_links(self):
         """
         Helper function to refresh owned joints. Useful for synchronizing internal data if
         additional bodies are added manually
-
-        Args:
-            load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
-            loading each of the link's rigid prims
         """
+        load_config = {
+            "scale": self._load_config.get("scale", None),
+        }
+
         # Make sure to clean up all pre-existing names for all links
         if self._links is not None:
             for link in self._links.values():
@@ -178,8 +175,8 @@ def update_links(self, load_config=None):
 
         # We iterate over all children of this object's prim,
         # and grab any that are presumed to be rigid bodies (i.e.: other Xforms)
-        self._links = dict()
         joint_children = set()
+        links_to_create = {}
         for prim in self._prim.GetChildren():
             link_cls = None
             link_name = prim.GetName()
@@ -200,21 +197,32 @@ def update_links(self, load_config=None):
                 # For cloth object, process prims that are Meshes
                 link_cls = ClothPrim
 
+            # Keep track of all the links we will create. We can't create that just yet because we need to find
+            # the base link first.
             if link_cls is not None:
-                self._links[link_name] = link_cls(
-                    prim_path=prim.GetPrimPath().__str__(),
-                    name=f"{self._name}:{link_name}",
-                    load_config=load_config,
-                )
+                links_to_create[link_name] = (link_cls, prim)
 
         # Infer the correct root link name -- this corresponds to whatever link does not have any joint existing
         # in the children joints
-        valid_root_links = list(set(self._links.keys()) - joint_children)
+        valid_root_links = list(set(links_to_create.keys()) - joint_children)
 
         assert len(valid_root_links) == 1, f"Only a single root link should have been found for this entity prim, " \
                                            f"but found multiple instead: {valid_root_links}"
         self._root_link_name = valid_root_links[0] if len(valid_root_links) == 1 else "base_link"
 
+        # Now actually create the links
+        self._links = dict()
+        for link_name, (link_cls, prim) in links_to_create.items():
+            link_load_config = {
+                "kinematic_only": self._load_config["kinematic_only"] if link_name == self._root_link_name else False,
+            }
+            link_load_config.update(load_config)
+            self._links[link_name] = link_cls(
+                prim_path=prim.GetPrimPath().__str__(),
+                name=f"{self._name}:{link_name}",
+                load_config=link_load_config,
+            )
+
     def update_joints(self):
         """
         Helper function to refresh owned joints. Useful for synchronizing internal data if
@@ -244,7 +252,7 @@ def update_joints(self):
                         joint = JointPrim(
                             prim_path=joint_path,
                             name=f"{self._name}:joint_{joint_name}",
-                            articulation_view=self._articulation_view,
+                            articulation_view=self._articulation_view_direct,
                         )
                         joint.initialize()
                         self._joints[joint_name] = joint
@@ -309,6 +317,20 @@ def _update_joint_limits(self):
             joint.lower_limit = joint.lower_limit * scale_along_axis
             joint.upper_limit = joint.upper_limit * scale_along_axis
 
+    @property
+    def _articulation_view(self):
+        if self._articulation_view_direct is None:
+            return None
+
+        # Validate that the articulation view is initialized and that if physics is running, the
+        # view is valid.
+        if og.sim.is_playing():
+            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!"
+        
+        return self._articulation_view_direct
+
     @property
     def prim_type(self):
         """
@@ -688,8 +710,8 @@ def update_handles(self):
         assert og.sim.is_playing(), "Simulator must be playing if updating handles!"
 
         # Reinitialize the articulation view
-        if self._articulation_view is not None:
-            self._articulation_view.initialize(og.sim.physics_sim_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():
@@ -799,7 +821,7 @@ def set_position_orientation(self, position=None, orientation=None):
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
-            orientation = np.asarray(orientation)[None, :]
+            orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
         self._articulation_view.set_world_poses(position, orientation)
         BoundingBoxAPI.clear()
 
@@ -809,7 +831,7 @@ def get_position_orientation(self):
             return super().get_position_orientation()
 
         positions, orientations = self._articulation_view.get_world_poses()
-        return positions[0], orientations[0]
+        return positions[0], orientations[0][[1, 2, 3, 0]]
 
     def set_local_pose(self, position=None, orientation=None):
         # Delegate to XFormPrim if we are not articulated
@@ -819,7 +841,7 @@ def set_local_pose(self, position=None, orientation=None):
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
-            orientation = np.asarray(orientation)[None, :]
+            orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
         self._articulation_view.set_local_poses(position, orientation)
         BoundingBoxAPI.clear()
 
@@ -829,7 +851,7 @@ def get_local_pose(self):
             return super().get_local_pose()
         
         positions, orientations = self._articulation_view.get_local_poses()
-        return positions[0], orientations[0]
+        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
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 465e6ac7e..8d988118d 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -82,7 +82,7 @@ def __init__(
         articulation_view=None,
     ):
         # Grab dynamic control reference and set properties
-        self._articulation_view = articulation_view
+        self._articulation_view_direct = articulation_view
 
         # Other values that will be filled in at runtime
         self._joint_type = None
@@ -213,6 +213,20 @@ def set_control_type(self, control_type, kp=None, kd=None):
         # Update control type
         self._control_type = control_type
 
+    @property
+    def _articulation_view(self):
+        if self._articulation_view_direct is None:
+            return None
+
+        # Validate that the articulation view is initialized and that if physics is running, the
+        # view is valid.
+        if og.sim.is_playing():
+            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!"
+        
+        return self._articulation_view_direct
+
     @property
     def body0(self):
         """
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index ba0aaa9a7..85676585a 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -62,7 +62,7 @@ def __init__(
         load_config=None,
     ):
         # Other values that will be filled in at runtime
-        self._rigid_prim_view = None
+        self._rigid_prim_view_direct = None
         self._cs = None                     # Contact sensor interface
         self._body_name = None
 
@@ -79,7 +79,7 @@ def __init__(
 
     def _post_load(self):
         # Create the view
-        self._rigid_prim_view = RigidPrimView(self._prim_path)
+        self._rigid_prim_view_direct = RigidPrimView(self._prim_path)
 
         # Set it to be kinematic if necessary
         if "kinematic_only" in self._load_config and self._load_config["kinematic_only"]:
@@ -213,7 +213,7 @@ def update_handles(self):
         # and dynamic objects are the same, physx tensor APIs do NOT exist for kinematic-only
         # objects, meaning initializing the view actively breaks the view.
         if not self.kinematic_only:
-            self._rigid_prim_view.initialize(og.sim.physics_sim_view)
+            self._rigid_prim_view_direct.initialize(og.sim.physics_sim_view)
 
     def contact_list(self):
         """
@@ -267,15 +267,12 @@ def get_angular_velocity(self):
         return self._rigid_prim_view.get_angular_velocities()[0]
 
     def set_position_orientation(self, position=None, orientation=None):
-        # Fallback to the xformprim method for kinematic-only objects
-        return super().set_position_orientation(position=position, orientation=orientation)
-
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
             assert np.isclose(np.linalg.norm(orientation), 1, atol=1e-3), \
                 f"{self.prim_path} desired orientation {orientation} is not a unit quaternion."
-            orientation = np.asarray(orientation)[None, :]
+            orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
         self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation)
 
     def get_position_orientation(self):
@@ -283,18 +280,34 @@ def get_position_orientation(self):
 
         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]
+        return pos[0], ori[0][[1, 2, 3, 0]]
 
     def set_local_pose(self, position=None, orientation=None):
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
-            orientation = np.asarray(orientation)[None, :]
+            orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
         self._rigid_prim_view.set_local_poses(position, orientation)
 
     def get_local_pose(self):
         positions, orientations = self._rigid_prim_view.get_local_poses()
-        return positions[0], orientations[0]
+        return positions[0], orientations[0][[1, 2, 3, 0]]
+
+    @property
+    def _rigid_prim_view(self):
+        if self._rigid_prim_view_direct is None:
+            return None
+
+        # Validate that the if physics is running, the view is valid.
+        if not self.kinematic_only and og.sim.is_playing():
+            assert self._rigid_prim_view_direct.is_physics_handle_valid() and \
+                self._rigid_prim_view_direct._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_direct.is_valid), \
+            "Rigid prim view must be valid if physics is running!"
+        
+        return self._rigid_prim_view_direct
 
     @property
     def body_name(self):

From 8934baa21f77a4715a057f5b757ef49c2f2dc965 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 15 Dec 2023 17:33:46 -0800
Subject: [PATCH 51/76] More verbose error

---
 omnigibson/prims/entity_prim.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index e0a7cfd99..00350c7d2 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -206,7 +206,7 @@ def update_links(self):
         # in the children joints
         valid_root_links = list(set(links_to_create.keys()) - joint_children)
 
-        assert len(valid_root_links) == 1, f"Only a single root link should have been found for this entity prim, " \
+        assert len(valid_root_links) == 1, f"Only a single root link should have been found for {self.name}, " \
                                            f"but found multiple instead: {valid_root_links}"
         self._root_link_name = valid_root_links[0] if len(valid_root_links) == 1 else "base_link"
 

From 25beea75d0064258a457e32ebf792e729a6047a8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Fri, 15 Dec 2023 17:47:58 -0800
Subject: [PATCH 52/76] Unbreak cloth

---
 omnigibson/prims/entity_prim.py | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 00350c7d2..6dff107e1 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -90,14 +90,6 @@ def _load(self):
         raise NotImplementedError("By default, an entity prim cannot be created from scratch.")
 
     def _post_load(self):
-        # Setup links info FIRST before running any other post loading behavior
-        # We pass in scale explicitly so that the generated links can leverage the desired entity scale
-        self.update_links()
-
-        # Prepare the articulation view.
-        if self.n_joints > 0:
-            self._articulation_view_direct = ArticulationView(self._prim_path + "/base_link")
-
         # If this is a cloth, delete the root link and replace it with the single nested mesh
         if self._prim_type == PrimType.CLOTH:
             # Verify only a single link and a single mesh exists
@@ -122,6 +114,14 @@ def _post_load(self):
             omni.kit.commands.execute("CopyPrim", path_from=cloth_mesh_prim.GetPath(), path_to=new_path)
             omni.kit.commands.execute("DeletePrims", paths=[old_link_prim.GetPath()], destructive=False)
 
+        # Setup links info FIRST before running any other post loading behavior
+        # We pass in scale explicitly so that the generated links can leverage the desired entity scale
+        self.update_links()
+
+        # Prepare the articulation view.
+        if self.n_joints > 0:
+            self._articulation_view_direct = ArticulationView(self._prim_path + "/base_link")
+
         # Set visual only flag
         # This automatically handles setting collisions / gravity appropriately per-link
         self.visual_only = self._load_config["visual_only"] if \

From 5bd567493cab8e2509e63d44d54b75cbb732382e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Mon, 18 Dec 2023 21:42:21 -0800
Subject: [PATCH 53/76] Don't forget to clear the xform API

---
 omnigibson/prims/xform_prim.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/omnigibson/prims/xform_prim.py b/omnigibson/prims/xform_prim.py
index ca1b44d61..5d9b62f4c 100644
--- a/omnigibson/prims/xform_prim.py
+++ b/omnigibson/prims/xform_prim.py
@@ -298,6 +298,7 @@ def set_local_pose(self, position=None, orientation=None):
             else:
                 rotq = Gf.Quatd(*orientation)
             xform_op.Set(rotq)
+        BoundingBoxAPI.clear()
         return
 
     def get_world_scale(self):

From ae0f5ec498e0ee47828a22393149e1f7930c561b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Mon, 18 Dec 2023 21:48:06 -0800
Subject: [PATCH 54/76] Delegate to RigidPrim instead of XFormPrim if a root
 link exists

---
 omnigibson/prims/entity_prim.py | 30 +++++++++++++++++++++---------
 1 file changed, 21 insertions(+), 9 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 6dff107e1..3e50928c4 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -814,9 +814,12 @@ def get_angular_velocity(self):
         return self.root_link.get_angular_velocity()
 
     def set_position_orientation(self, position=None, orientation=None):
-        # Delegate to XFormPrim if we are not articulated
+        # Delegate to RigidPrim or XFormPrim if we are not articulated
         if self._articulation_view is None:
-            return super().set_position_orientation(position=position, orientation=orientation)
+            if self.root_link is not None:
+                return self.root_link.set_position_orientation(position=position, orientation=orientation)
+            else:
+                return super().set_position_orientation(position=position, orientation=orientation)
         
         if position is not None:
             position = np.asarray(position)[None, :]
@@ -826,18 +829,24 @@ def set_position_orientation(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
-        # Delegate to XFormPrim if we are not articulated
+        # Delegate to RigidPrim or XFormPrim if we are not articulated
         if self._articulation_view is None:
-            return super().get_position_orientation()
+            if self.root_link is not None:
+                return self.root_link.get_position_orientation()
+            else:
+                return super().get_position_orientation()
 
         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 XFormPrim if we are not articulated
+        # Delegate to RigidPrim or XFormPrim if we are not articulated
         if self._articulation_view is None:
-            return super().set_local_pose(position=position, orientation=orientation)
-        
+            if self.root_link is not None:
+                return self.root_link.set_local_pose(position=position, orientation=orientation)
+            else:
+                return super().set_local_pose(position=position, orientation=orientation)
+
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -846,9 +855,12 @@ def set_local_pose(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_local_pose(self):
-        # Delegate to XFormPrim if we are not articulated
+        # Delegate to RigidPrim or XFormPrim if we are not articulated
         if self._articulation_view is None:
-            return super().get_local_pose()
+            if self.root_link is not None:
+                return self.root_link.get_local_pose()
+            else:
+                return super().get_local_pose()
         
         positions, orientations = self._articulation_view.get_local_poses()
         return positions[0], orientations[0][[1, 2, 3, 0]]

From dafe3f747885dc86381ed153369022c1d3c3c527 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Mon, 18 Dec 2023 21:48:52 -0800
Subject: [PATCH 55/76] Update entity_prim.py

---
 omnigibson/prims/entity_prim.py | 30 +++++++++---------------------
 1 file changed, 9 insertions(+), 21 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 3e50928c4..f8f7cdb6b 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -814,12 +814,9 @@ def get_angular_velocity(self):
         return self.root_link.get_angular_velocity()
 
     def set_position_orientation(self, position=None, orientation=None):
-        # Delegate to RigidPrim or XFormPrim if we are not articulated
+        # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
-            if self.root_link is not None:
-                return self.root_link.set_position_orientation(position=position, orientation=orientation)
-            else:
-                return super().set_position_orientation(position=position, orientation=orientation)
+            return self.root_link.set_position_orientation(position=position, orientation=orientation)
         
         if position is not None:
             position = np.asarray(position)[None, :]
@@ -829,24 +826,18 @@ def set_position_orientation(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
-        # Delegate to RigidPrim or XFormPrim if we are not articulated
+        # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
-            if self.root_link is not None:
-                return self.root_link.get_position_orientation()
-            else:
-                return super().get_position_orientation()
+            return self.root_link.get_position_orientation()
 
         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 or XFormPrim if we are not articulated
+        # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
-            if self.root_link is not None:
-                return self.root_link.set_local_pose(position=position, orientation=orientation)
-            else:
-                return super().set_local_pose(position=position, orientation=orientation)
-
+            return self.root_link.set_local_pose(position=position, orientation=orientation)
+        
         if position is not None:
             position = np.asarray(position)[None, :]
         if orientation is not None:
@@ -855,12 +846,9 @@ def set_local_pose(self, position=None, orientation=None):
         BoundingBoxAPI.clear()
 
     def get_local_pose(self):
-        # Delegate to RigidPrim or XFormPrim if we are not articulated
+        # Delegate to RigidPrim if we are not articulated
         if self._articulation_view is None:
-            if self.root_link is not None:
-                return self.root_link.get_local_pose()
-            else:
-                return super().get_local_pose()
+            return self.root_link.get_local_pose()
         
         positions, orientations = self._articulation_view.get_local_poses()
         return positions[0], orientations[0][[1, 2, 3, 0]]

From 9dea92200138bb235621234941d5eaec30cb427d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Mon, 18 Dec 2023 22:04:58 -0800
Subject: [PATCH 56/76] Make RigidPrim reset BoundingBoxAPI too

---
 omnigibson/prims/rigid_prim.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 85676585a..bafa7e67f 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -12,7 +12,7 @@
 from omnigibson.utils.constants import GEOM_TYPES
 from omnigibson.utils.deprecated_utils import RigidPrimView
 from omnigibson.utils.sim_utils import CsRawData
-from omnigibson.utils.usd_utils import get_mesh_volume_and_com
+from omnigibson.utils.usd_utils import BoundingBoxAPI, get_mesh_volume_and_com
 import omnigibson.utils.transform_utils as T
 from omnigibson.utils.ui_utils import create_module_logger
 
@@ -274,6 +274,7 @@ def set_position_orientation(self, position=None, orientation=None):
                 f"{self.prim_path} desired orientation {orientation} is not a unit quaternion."
             orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
         self._rigid_prim_view.set_world_poses(positions=position, orientations=orientation)
+        BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
         pos, ori = self._rigid_prim_view.get_world_poses()
@@ -288,6 +289,7 @@ def set_local_pose(self, position=None, orientation=None):
         if orientation is not None:
             orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
         self._rigid_prim_view.set_local_poses(position, orientation)
+        BoundingBoxAPI.clear()
 
     def get_local_pose(self):
         positions, orientations = self._rigid_prim_view.get_local_poses()

From 5eacdce3a98281048131a8ff3b1ffc80f403e614 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Mon, 18 Dec 2023 23:34:16 -0800
Subject: [PATCH 57/76] Make particles fixed base

---
 omnigibson/systems/system_base.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/omnigibson/systems/system_base.py b/omnigibson/systems/system_base.py
index 9740f1d74..149669e75 100644
--- a/omnigibson/systems/system_base.py
+++ b/omnigibson/systems/system_base.py
@@ -1152,7 +1152,7 @@ def generate_particle_template_fcn():
                         encrypted=True,
                         category=system_name,
                         visible=False,
-                        fixed_base=False,
+                        fixed_base=True,
                         visual_only=True,
                         kinematic_only=True,
                         include_default_states=False,
@@ -1168,7 +1168,7 @@ def generate_particle_template_fcn():
                         category=system_name,
                         radius=0.015,
                         visible=False,
-                        fixed_base=False,
+                        fixed_base=True,
                         visual_only=True,
                         kinematic_only=True,
                         include_default_states=False,

From de1ee674de5877148afbd76ec3fb64dfdc56f581 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Mon, 18 Dec 2023 23:40:30 -0800
Subject: [PATCH 58/76] Don't set kinematic only manually for particles, this
 is now auto-computed

---
 omnigibson/systems/macro_particle_system.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/omnigibson/systems/macro_particle_system.py b/omnigibson/systems/macro_particle_system.py
index 13dc767c6..a4e047c3a 100644
--- a/omnigibson/systems/macro_particle_system.py
+++ b/omnigibson/systems/macro_particle_system.py
@@ -56,7 +56,6 @@ def initialize(cls):
         # Load the particle template, and make it kinematic only because it's not interacting with anything
         particle_template = cls._create_particle_template()
         og.sim.import_object(obj=particle_template, register=False)
-        particle_template.kinematic_only = True
 
         # Make sure template scaling is [1, 1, 1] -- any particle scaling should be done via cls.min/max_scale
         assert np.all(particle_template.scale == 1.0)

From 86373ee3dfa8f569f2ef59ea38e5aadcdce992c4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 00:01:00 -0800
Subject: [PATCH 59/76] Some fixes to manipulation robot

---
 omnigibson/robots/manipulation_robot.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index 11a1040b9..25b80b6c1 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -1,7 +1,6 @@
 from abc import abstractmethod
 from collections import namedtuple
 import numpy as np
-import networkx as nx
 
 import omnigibson as og
 from omnigibson.macros import gm, create_module_macros
@@ -778,7 +777,9 @@ def _calculate_in_hand_object_rigid(self, arm="default"):
         for prim_path in candidates_set:
             # Calculate position of the object link. Only allow this for objects currently.
             obj_prim_path, link_name = prim_path.rsplit("/", 1)[-1]
-            candidate_obj = og.sim.scene.object_registry("prim_path", obj_prim_path)
+            candidate_obj = og.sim.scene.object_registry("prim_path", obj_prim_path, None)
+            if candidate_obj is None or link_name not in candidate_obj.links:
+                continue
             candidate_link = candidate_obj.links[link_name]
             dist = np.linalg.norm(np.array(candidate_link.get_position()) - np.array(gripper_center_pos))
             candidate_data.append((prim_path, dist))

From f1ace518f1dc2c0d5481cf7dfae19fe810b55c26 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 11:11:17 -0800
Subject: [PATCH 60/76] better update handles logic

---
 omnigibson/simulator.py | 33 ++++++++++++++++++++-------------
 1 file changed, 20 insertions(+), 13 deletions(-)

diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py
index f33a1d847..b8e3c89a4 100644
--- a/omnigibson/simulator.py
+++ b/omnigibson/simulator.py
@@ -393,9 +393,8 @@ def remove_object(self, obj):
         self._scene.remove_object(obj)
         self.app.update()
 
-        # Re-initialize the physics view if we're playing because the number of objects has changed
-        if og.sim.is_playing():
-            RigidContactAPI.initialize_view()
+        # Update all handles that are now broken because objects have changed
+        self._update_handles()
 
         # Refresh all current rules
         TransitionRuleAPI.prune_active_rules()
@@ -405,6 +404,22 @@ def _reset_variables(self):
         Reset internal variables when a new stage is loaded
         """
 
+    def _update_handles(self):
+        # First, refresh the physics sim view
+        self._physics_sim_view = omni.physics.tensors.create_simulation_view(self.backend)
+        self._physics_sim_view.set_subspace_roots("/")
+
+        # Then update the handles for all objects
+        if self.scene is not None and self.scene.initialized:
+            for obj in self.scene.objects:
+                # Only need to update if object is already initialized as well
+                if obj.initialized:
+                    obj.update_handles()
+
+        # Finally update any unified views
+        if og.sim.is_playing():
+            RigidContactAPI.initialize_view()
+
     def _non_physics_step(self):
         """
         Complete any non-physics steps such as state updates.
@@ -438,7 +453,7 @@ 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
-                RigidContactAPI.initialize_view()
+                self._update_handles()
 
                 # Also refresh the transition rules that are currently active
                 TransitionRuleAPI.refresh_all_rules()
@@ -498,20 +513,12 @@ def play(self):
             # handles are updated, since updating the physics view makes the per-object physics view invalid
             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("/")
-
             # Take a render step -- this is needed so that certain (unknown, maybe omni internal state?) is populated
             # correctly.
             self.render()
 
             # Update all object handles
-            if self.scene is not None and self.scene.initialized:
-                for obj in self.scene.objects:
-                    # Only need to update if object is already initialized as well
-                    if obj.initialized:
-                        obj.update_handles()
+            self._update_handles()
 
             if was_stopped:
                 # We need to update controller mode because kp and kd were set to the original (incorrect) values when

From 53c62a685d0e4f6ca1e4184525a6e19f202f4181 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 11:15:15 -0800
Subject: [PATCH 61/76] Quick fix

---
 omnigibson/simulator.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py
index b8e3c89a4..85a800179 100644
--- a/omnigibson/simulator.py
+++ b/omnigibson/simulator.py
@@ -417,7 +417,7 @@ def _update_handles(self):
                     obj.update_handles()
 
         # Finally update any unified views
-        if og.sim.is_playing():
+        if self.is_playing():
             RigidContactAPI.initialize_view()
 
     def _non_physics_step(self):

From 68a6db8ccf1ab941a049a3b1b4fc770d1b1338b2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 11:24:13 -0800
Subject: [PATCH 62/76] Make the contact API use the sensor paths from the view

---
 omnigibson/utils/usd_utils.py | 28 ++++------------------------
 1 file changed, 4 insertions(+), 24 deletions(-)

diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py
index f8fe10c8a..a147f4e00 100644
--- a/omnigibson/utils/usd_utils.py
+++ b/omnigibson/utils/usd_utils.py
@@ -253,19 +253,6 @@ def initialize_view(cls):
         """
         assert og.sim.is_playing(), "Cannot create rigid contact view while sim is not playing!"
 
-        # Compile deterministic mapping from rigid body path to idx
-        # Note that omni's ordering is based on the top-down object ordering path on the USD stage, which coincidentally
-        # matches the same ordering we store objects in our registry. So the mapping we generate from our registry
-        # mapping aligns with omni's ordering!
-        i = 0
-        cls._PATH_TO_IDX = dict()
-        for obj in og.sim.scene.objects:
-            if obj.prim_type == PrimType.RIGID:
-                for link in obj.links.values():
-                    if not link.kinematic_only:
-                        cls._PATH_TO_IDX[link.prim_path] = i
-                        i += 1
-
         # If there are no valid objects, clear the view and terminate early
         if i == 0:
             cls._CONTACT_VIEW = None
@@ -276,17 +263,9 @@ def initialize_view(cls):
         # We also suppress the omni tensor plugin from giving warnings we expect
         og.sim.pi.update_simulation(elapsedStep=0, currentTime=og.sim.current_time)
         with suppress_omni_log(channels=["omni.physx.tensors.plugin"]):
-            cls._CONTACT_VIEW = og.sim.physics_sim_view.create_rigid_contact_view(
-                pattern="/World/*/*",
-                filter_patterns=list(cls._PATH_TO_IDX.keys()),
-            )
-
-        # Sanity check generated view -- this should generate square matrices of shape (N, N, 3)
-        n_bodies = len(cls._PATH_TO_IDX)
-        # from IPython import embed; embed()
-        assert cls._CONTACT_VIEW.sensor_count == n_bodies and cls._CONTACT_VIEW.filter_count == n_bodies, \
-            f"Got unexpected contact view shape. Expected: ({n_bodies}, {n_bodies}); " \
-            f"got: ({cls._CONTACT_VIEW.sensor_count}, {cls._CONTACT_VIEW.filter_count})"
+            cls._CONTACT_VIEW = og.sim.physics_sim_view.create_rigid_contact_view(pattern="/World/*/*")
+        
+        cls._PATH_TO_IDX = {prim_path: idx for idx, prim_path in enumerate(cls._CONTACT_VIEW.sensor_paths)}
 
     @classmethod
     def get_body_idx(cls, prim_path):
@@ -359,6 +338,7 @@ def clear(cls):
         Clears the internal contact matrix and cache
         """
         cls._CONTACT_MATRIX = None
+        cls._PATH_TO_IDX = None
         cls._CONTACT_CACHE = dict()
 
 

From 1d8a2ec5c4d22f335b9dcdee44f2e0ad44e6dedf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 13:20:55 -0800
Subject: [PATCH 63/76] Some fixes

---
 omnigibson/simulator.py       | 5 +++--
 omnigibson/utils/usd_utils.py | 9 ++++-----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py
index 85a800179..f1eae1279 100644
--- a/omnigibson/simulator.py
+++ b/omnigibson/simulator.py
@@ -517,8 +517,9 @@ def play(self):
             # correctly.
             self.render()
 
-            # Update all object handles
-            self._update_handles()
+            # 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
diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py
index a147f4e00..8c8e6bc12 100644
--- a/omnigibson/utils/usd_utils.py
+++ b/omnigibson/utils/usd_utils.py
@@ -253,17 +253,16 @@ def initialize_view(cls):
         """
         assert og.sim.is_playing(), "Cannot create rigid contact view while sim is not playing!"
 
-        # If there are no valid objects, clear the view and terminate early
-        if i == 0:
-            cls._CONTACT_VIEW = None
-            return
-
         # Generate rigid body view, making sure to update the simulation first (without physics) so that the physx
         # backend is synchronized with any newly added objects
         # We also suppress the omni tensor plugin from giving warnings we expect
         og.sim.pi.update_simulation(elapsedStep=0, currentTime=og.sim.current_time)
         with suppress_omni_log(channels=["omni.physx.tensors.plugin"]):
             cls._CONTACT_VIEW = og.sim.physics_sim_view.create_rigid_contact_view(pattern="/World/*/*")
+
+        if cls._CONTACT_VIEW is None or cls._CONTACT_VIEW._backend is None:
+            cls._CONTACT_VIEW = None
+            return
         
         cls._PATH_TO_IDX = {prim_path: idx for idx, prim_path in enumerate(cls._CONTACT_VIEW.sensor_paths)}
 

From bca5b738a4bc40f98752393c3a880f5b773ceb88 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 13:35:39 -0800
Subject: [PATCH 64/76] Some fixes

---
 omnigibson/prims/prim_base.py | 3 +++
 omnigibson/simulator.py       | 8 ++++----
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/omnigibson/prims/prim_base.py b/omnigibson/prims/prim_base.py
index e2483afbc..71d8e2ff4 100644
--- a/omnigibson/prims/prim_base.py
+++ b/omnigibson/prims/prim_base.py
@@ -128,6 +128,9 @@ def remove(self):
         # Also clear the name so we can reuse this later
         self.remove_names(include_all_owned=True)
 
+        # Make the simulator refresh its handles
+        og.sim.update_handles()
+
     @abstractmethod
     def _load(self):
         """
diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py
index f1eae1279..e5a393860 100644
--- a/omnigibson/simulator.py
+++ b/omnigibson/simulator.py
@@ -394,7 +394,7 @@ def remove_object(self, obj):
         self.app.update()
 
         # Update all handles that are now broken because objects have changed
-        self._update_handles()
+        self.update_handles()
 
         # Refresh all current rules
         TransitionRuleAPI.prune_active_rules()
@@ -404,7 +404,7 @@ def _reset_variables(self):
         Reset internal variables when a new stage is loaded
         """
 
-    def _update_handles(self):
+    def update_handles(self):
         # First, refresh the physics sim view
         self._physics_sim_view = omni.physics.tensors.create_simulation_view(self.backend)
         self._physics_sim_view.set_subspace_roots("/")
@@ -453,7 +453,7 @@ 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()
+                self.update_handles()
 
                 # Also refresh the transition rules that are currently active
                 TransitionRuleAPI.refresh_all_rules()
@@ -519,7 +519,7 @@ def play(self):
 
             # Update all object handles, unless this is a play during initialization
             if og.sim is not None:
-                self._update_handles()
+                self.update_handles()
 
             if was_stopped:
                 # We need to update controller mode because kp and kd were set to the original (incorrect) values when

From a298dc393c4cc3b1039df518173c80898061f9ad Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 15:14:22 -0800
Subject: [PATCH 65/76] Check if sim is playing

---
 omnigibson/simulator.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py
index e5a393860..5f3066b37 100644
--- a/omnigibson/simulator.py
+++ b/omnigibson/simulator.py
@@ -405,6 +405,10 @@ def _reset_variables(self):
         """
 
     def update_handles(self):
+        # Handles are only relevant when physx is running
+        if not self.is_playing():
+            return
+
         # First, refresh the physics sim view
         self._physics_sim_view = omni.physics.tensors.create_simulation_view(self.backend)
         self._physics_sim_view.set_subspace_roots("/")
@@ -417,8 +421,7 @@ def update_handles(self):
                     obj.update_handles()
 
         # Finally update any unified views
-        if self.is_playing():
-            RigidContactAPI.initialize_view()
+        RigidContactAPI.initialize_view()
 
     def _non_physics_step(self):
         """

From 8158f55b912152e5cbe8c0b52df6861c8af256da Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Tue, 19 Dec 2023 15:49:16 -0800
Subject: [PATCH 66/76] Apply the valid view requirement only for initialized
 objects

---
 omnigibson/prims/entity_prim.py | 2 +-
 omnigibson/prims/joint_prim.py  | 2 +-
 omnigibson/prims/rigid_prim.py  | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index f8f7cdb6b..e9405c543 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -324,7 +324,7 @@ 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():
+        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!"
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 8d988118d..da39dceef 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -220,7 +220,7 @@ 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():
+        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!"
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index bafa7e67f..d6d93d51b 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -301,7 +301,7 @@ def _rigid_prim_view(self):
             return None
 
         # Validate that the if physics is running, the view is valid.
-        if not self.kinematic_only and og.sim.is_playing():
+        if not self.kinematic_only and og.sim.is_playing() and self.initialized:
             assert self._rigid_prim_view_direct.is_physics_handle_valid() and \
                 self._rigid_prim_view_direct._physics_view.check(), \
                 "Rigid prim view must be valid if physics is running!"

From 87ebe880bdd492db8264ab2d36b3ca23341aa381 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 17:17:00 -0800
Subject: [PATCH 67/76] Revert "Remove articulation tree, which does not work
 because fixed links are not included in tree"

This reverts commit dac0927285099b5efc1c4778a5b9962c539e1630.
---
 omnigibson/prims/entity_prim.py | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index e9405c543..06e92051f 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -241,6 +241,7 @@ def update_joints(self):
         if self._articulation_view and not self.kinematic_only:
             self._n_dof = self._articulation_view.num_dof
 
+            # TODO: This still includes fixed joints for objects that also have non-fixed?
             # Additionally grab DOF info if we have non-fixed joints
             if self._n_dof > 0:
                 for i in range(self._articulation_view._metadata.joint_count):
@@ -453,6 +454,26 @@ def links(self):
         """
         return self._links
     
+    @property
+    def articulation_tree(self):
+        """
+        Get a graph of the articulation tree, where nodes are links (RigidPrim) and edges
+        correspond to joints (JointPrim), where the JointPrim is accessible on the `joint`
+        data field of the edge.
+        """
+        G = nx.DiGraph()
+        rename_later = {}
+        for link in self.links.values():
+            prim_path = link.prim_path
+            G.add_node(prim_path)
+            rename_later[prim_path] = link
+        for joint in self.joints.values():
+            if joint.body0 not in G.nodes or joint.body1 not in G.nodes:
+                continue
+            G.add_edge(joint.body0, joint.body1, joint=joint)
+        nx.relabel_nodes(G, rename_later)
+        return G
+
     @property
     def materials(self):
         """

From c98979af1b15d23477a5dac829744ef6f80a771c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 17:45:38 -0800
Subject: [PATCH 68/76] Reimplement articulation graph

---
 omnigibson/prims/entity_prim.py         | 57 +++++++++++++++++++------
 omnigibson/robots/manipulation_robot.py | 13 +++---
 2 files changed, 51 insertions(+), 19 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 06e92051f..027a6acea 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -54,6 +54,7 @@ def __init__(
         self._joints = None
         self._materials = None
         self._visual_only = None
+        self._articulation_tree = None
         self._articulation_view_direct = None
 
         # This needs to be initialized to be used for _load() of PrimitiveObject
@@ -267,6 +268,7 @@ def update_joints(self):
             f"found in the articulation view ({len(self._joints)})!"
 
         self._update_joint_limits()
+        self._compute_articulation_tree()
 
     def _update_joint_limits(self):
         """
@@ -454,25 +456,56 @@ def links(self):
         """
         return self._links
     
-    @property
-    def articulation_tree(self):
+    def _compute_articulation_tree(self):
         """
-        Get a graph of the articulation tree, where nodes are links (RigidPrim) and edges
-        correspond to joints (JointPrim), where the JointPrim is accessible on the `joint`
-        data field of the edge.
+        Get a graph of the articulation tree, where nodes are link names and edges
+        correspond to joint names, where the joint name is accessible on the `joint_name`
+        data field of the edge, and the joint type on the `joint_type` field.
         """
         G = nx.DiGraph()
         rename_later = {}
-        for link in self.links.values():
+
+        # Add the links
+        for link_name, link in self.links.items():
             prim_path = link.prim_path
             G.add_node(prim_path)
-            rename_later[prim_path] = link
-        for joint in self.joints.values():
-            if joint.body0 not in G.nodes or joint.body1 not in G.nodes:
-                continue
-            G.add_edge(joint.body0, joint.body1, joint=joint)
+            rename_later[prim_path] = link_name
+
+        # Add the joints
+        children = list(self.prim.GetChildren())
+        while children:
+            child_prim = children.pop()
+            children.extend(child_prim.GetChildren())
+            prim_type = child_prim.GetPrimTypeInfo().GetTypeName()
+            if "Joint" in prim_type:
+                # Get body 0
+                body0_targets = self._prim.GetRelationship("physics:body0").GetTargets()
+                if not body0_targets:
+                    continue
+                body0 = str(body0_targets[0])
+
+                # Get body 1
+                body1_targets = self._prim.GetRelationship("physics:body1").GetTargets()
+                if not body1_targets:
+                    continue
+                body1 = str(body1_targets[0])
+
+                # Assert both bodies in links
+                if body0 not in G.nodes or body1 not in G.nodes:
+                    continue
+            
+                # Add the joint
+                joint_type_str = "JOINT_" + prim_type.replace("Joint", "").upper()
+                G.add_edge(body0, body1, joint_name=child_prim.GetName(), joint_type=JointType[joint_type_str])
+
+        # Relabel nodes to use link name instead of prim path
         nx.relabel_nodes(G, rename_later)
-        return G
+        
+        self._articulation_tree = G
+
+    @property
+    def articulation_tree(self):
+        return self._articulation_tree
 
     @property
     def materials(self):
diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index 25b80b6c1..d3a71b411 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -1,6 +1,7 @@
 from abc import abstractmethod
 from collections import namedtuple
 import numpy as np
+import networkx as nx
 
 import omnigibson as og
 from omnigibson.macros import gm, create_module_macros
@@ -1050,13 +1051,11 @@ def _get_assisted_grasp_joint_type(self, ag_obj, ag_link):
         # Otherwise, compute the joint type. We use a fixed joint unless the link is a non-fixed link.
         # A link is non-fixed if it has any non-fixed parent joints.
         joint_type = "FixedJoint"
-        # TODO: Add this logic back w/o the graph using just the articulationview.
-        # articulation_graph = ag_obj.articulation_graph
-        # for edge in nx.edge_dfs(articulation_graph, ag_link, orientation="reverse"):
-        #     joint = articulation_graph.edges[edge]["joint"]
-        #     if joint.joint_type != JointType.JOINT_FIXED:
-        #         joint_type = "SphericalJoint"
-        #         break
+        for edge in nx.edge_dfs(ag_obj.articulation_tree, ag_link, orientation="reverse"):
+            joint = ag_obj.articulation_tree.edges[edge]["joint"]
+            if joint.joint_type != JointType.JOINT_FIXED:
+                joint_type = "SphericalJoint"
+                break
 
         return joint_type
 

From 73fdb8303e12f21dd8cd2989ddb4e4848cc11217 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 18:24:39 -0800
Subject: [PATCH 69/76] Fix articulation tree

---
 omnigibson/prims/entity_prim.py | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 027a6acea..313ef4029 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -479,13 +479,13 @@ def _compute_articulation_tree(self):
             prim_type = child_prim.GetPrimTypeInfo().GetTypeName()
             if "Joint" in prim_type:
                 # Get body 0
-                body0_targets = self._prim.GetRelationship("physics:body0").GetTargets()
+                body0_targets = child_prim.GetRelationship("physics:body0").GetTargets()
                 if not body0_targets:
                     continue
                 body0 = str(body0_targets[0])
 
                 # Get body 1
-                body1_targets = self._prim.GetRelationship("physics:body1").GetTargets()
+                body1_targets = child_prim.GetRelationship("physics:body1").GetTargets()
                 if not body1_targets:
                     continue
                 body1 = str(body1_targets[0])
@@ -495,11 +495,17 @@ def _compute_articulation_tree(self):
                     continue
             
                 # Add the joint
-                joint_type_str = "JOINT_" + prim_type.replace("Joint", "").upper()
+                joint_type_str = "JOINT_" + prim_type.replace("PhysicsJoint", "").upper()
                 G.add_edge(body0, body1, joint_name=child_prim.GetName(), joint_type=JointType[joint_type_str])
 
         # Relabel nodes to use link name instead of prim path
         nx.relabel_nodes(G, rename_later)
+
+        # Assert all nodes have in-degree of 1 except root
+        in_degrees = {node: G.in_degree(node) for node in G.nodes}
+        assert in_degrees[self.root_link_name] == 0, "Root link should have in-degree of 0!"
+        assert all([in_degrees[node] == 1 for node in G.nodes if node != self.root_link_name]), \
+            "All non-root links should have in-degree of 1!"
         
         self._articulation_tree = G
 

From 10804c176dc294f0a3e1322021618111961720f1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 18:35:10 -0800
Subject: [PATCH 70/76] Small fix

---
 omnigibson/prims/entity_prim.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 313ef4029..d720d8fec 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -499,7 +499,7 @@ def _compute_articulation_tree(self):
                 G.add_edge(body0, body1, joint_name=child_prim.GetName(), joint_type=JointType[joint_type_str])
 
         # Relabel nodes to use link name instead of prim path
-        nx.relabel_nodes(G, rename_later)
+        nx.relabel_nodes(G, rename_later, copy=False)
 
         # Assert all nodes have in-degree of 1 except root
         in_degrees = {node: G.in_degree(node) for node in G.nodes}

From 588ffabe1ce0b85f9fc1023e152fb51d7a96c5b7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 18:54:07 -0800
Subject: [PATCH 71/76] Fix joint type

---
 omnigibson/prims/entity_prim.py | 4 ++--
 omnigibson/prims/joint_prim.py  | 1 -
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index d720d8fec..2473f238f 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -495,8 +495,8 @@ def _compute_articulation_tree(self):
                     continue
             
                 # Add the joint
-                joint_type_str = "JOINT_" + prim_type.replace("PhysicsJoint", "").upper()
-                G.add_edge(body0, body1, joint_name=child_prim.GetName(), joint_type=JointType[joint_type_str])
+                joint_type = JointType.get_type(prim_type.split("Physics")[-1])
+                G.add_edge(body0, body1, joint_name=child_prim.GetName(), joint_type=joint_type)
 
         # Relabel nodes to use link name instead of prim path
         nx.relabel_nodes(G, rename_later, copy=False)
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index da39dceef..348597b3a 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -125,7 +125,6 @@ def _post_load(self):
         # Add joint state API if this is a revolute or prismatic joint
         self._joint_type = JointType.get_type(self._prim.GetTypeName().split("Physics")[-1])
         if self.is_single_dof:
-            state_type = "angular" if self._joint_type == JointType.JOINT_REVOLUTE else "linear"
             # We MUST already have the joint state API defined beforehand in the USD
             # This is because physx complains if we try to add physx APIs AFTER a simulation step occurs, which
             # happens because joint prims are usually created externally during an EntityPrim's initialization phase

From 263fa519bae3f656dc7a7741be4472a9c4fcd934 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 19:08:18 -0800
Subject: [PATCH 72/76] Fix RigidContactAPI

---
 omnigibson/utils/usd_utils.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py
index 8c8e6bc12..6001a4df0 100644
--- a/omnigibson/utils/usd_utils.py
+++ b/omnigibson/utils/usd_utils.py
@@ -258,7 +258,10 @@ def initialize_view(cls):
         # We also suppress the omni tensor plugin from giving warnings we expect
         og.sim.pi.update_simulation(elapsedStep=0, currentTime=og.sim.current_time)
         with suppress_omni_log(channels=["omni.physx.tensors.plugin"]):
-            cls._CONTACT_VIEW = og.sim.physics_sim_view.create_rigid_contact_view(pattern="/World/*/*")
+            cls._CONTACT_VIEW = og.sim.physics_sim_view.create_rigid_contact_view(
+                pattern="/World/*/*",
+                filter_patterns=["/World/*/*"],
+            )
 
         if cls._CONTACT_VIEW is None or cls._CONTACT_VIEW._backend is None:
             cls._CONTACT_VIEW = None

From 508db28c96516009b37aea07f5ff9d511224aae0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 19:31:44 -0800
Subject: [PATCH 73/76] Undo changes to RigidContactAPI

---
 omnigibson/utils/usd_utils.py | 32 +++++++++++++++++++++++++-------
 1 file changed, 25 insertions(+), 7 deletions(-)

diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py
index 6001a4df0..f8fe10c8a 100644
--- a/omnigibson/utils/usd_utils.py
+++ b/omnigibson/utils/usd_utils.py
@@ -253,6 +253,24 @@ def initialize_view(cls):
         """
         assert og.sim.is_playing(), "Cannot create rigid contact view while sim is not playing!"
 
+        # Compile deterministic mapping from rigid body path to idx
+        # Note that omni's ordering is based on the top-down object ordering path on the USD stage, which coincidentally
+        # matches the same ordering we store objects in our registry. So the mapping we generate from our registry
+        # mapping aligns with omni's ordering!
+        i = 0
+        cls._PATH_TO_IDX = dict()
+        for obj in og.sim.scene.objects:
+            if obj.prim_type == PrimType.RIGID:
+                for link in obj.links.values():
+                    if not link.kinematic_only:
+                        cls._PATH_TO_IDX[link.prim_path] = i
+                        i += 1
+
+        # If there are no valid objects, clear the view and terminate early
+        if i == 0:
+            cls._CONTACT_VIEW = None
+            return
+
         # Generate rigid body view, making sure to update the simulation first (without physics) so that the physx
         # backend is synchronized with any newly added objects
         # We also suppress the omni tensor plugin from giving warnings we expect
@@ -260,14 +278,15 @@ def initialize_view(cls):
         with suppress_omni_log(channels=["omni.physx.tensors.plugin"]):
             cls._CONTACT_VIEW = og.sim.physics_sim_view.create_rigid_contact_view(
                 pattern="/World/*/*",
-                filter_patterns=["/World/*/*"],
+                filter_patterns=list(cls._PATH_TO_IDX.keys()),
             )
 
-        if cls._CONTACT_VIEW is None or cls._CONTACT_VIEW._backend is None:
-            cls._CONTACT_VIEW = None
-            return
-        
-        cls._PATH_TO_IDX = {prim_path: idx for idx, prim_path in enumerate(cls._CONTACT_VIEW.sensor_paths)}
+        # Sanity check generated view -- this should generate square matrices of shape (N, N, 3)
+        n_bodies = len(cls._PATH_TO_IDX)
+        # from IPython import embed; embed()
+        assert cls._CONTACT_VIEW.sensor_count == n_bodies and cls._CONTACT_VIEW.filter_count == n_bodies, \
+            f"Got unexpected contact view shape. Expected: ({n_bodies}, {n_bodies}); " \
+            f"got: ({cls._CONTACT_VIEW.sensor_count}, {cls._CONTACT_VIEW.filter_count})"
 
     @classmethod
     def get_body_idx(cls, prim_path):
@@ -340,7 +359,6 @@ def clear(cls):
         Clears the internal contact matrix and cache
         """
         cls._CONTACT_MATRIX = None
-        cls._PATH_TO_IDX = None
         cls._CONTACT_CACHE = dict()
 
 

From f94536191a8d45fc77f9d33e7b6bab49ebd0c4b4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Tue, 19 Dec 2023 19:44:10 -0800
Subject: [PATCH 74/76] Fix AG mechanism

---
 omnigibson/robots/manipulation_robot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index d3a71b411..a2deb02b8 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -1051,7 +1051,7 @@ def _get_assisted_grasp_joint_type(self, ag_obj, ag_link):
         # Otherwise, compute the joint type. We use a fixed joint unless the link is a non-fixed link.
         # A link is non-fixed if it has any non-fixed parent joints.
         joint_type = "FixedJoint"
-        for edge in nx.edge_dfs(ag_obj.articulation_tree, ag_link, orientation="reverse"):
+        for edge in nx.edge_dfs(ag_obj.articulation_tree, ag_link.body_name, orientation="reverse"):
             joint = ag_obj.articulation_tree.edges[edge]["joint"]
             if joint.joint_type != JointType.JOINT_FIXED:
                 joint_type = "SphericalJoint"

From 2174058ac96ea79512746f0bdca33ec13eadaea9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?=
 <1408354+cgokmen@users.noreply.github.com>
Date: Wed, 20 Dec 2023 15:57:08 -0800
Subject: [PATCH 75/76] Small bugfix

---
 omnigibson/robots/manipulation_robot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index a2deb02b8..a5e4ba59e 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -777,7 +777,7 @@ def _calculate_in_hand_object_rigid(self, arm="default"):
         candidate_data = []
         for prim_path in candidates_set:
             # Calculate position of the object link. Only allow this for objects currently.
-            obj_prim_path, link_name = prim_path.rsplit("/", 1)[-1]
+            obj_prim_path, link_name = prim_path.rsplit("/", 1)
             candidate_obj = og.sim.scene.object_registry("prim_path", obj_prim_path, None)
             if candidate_obj is None or link_name not in candidate_obj.links:
                 continue

From 7958f5c390b3a4b43984cf0459ff77561cda0862 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <cem@cemgokmen.com>
Date: Thu, 21 Dec 2023 12:56:09 -0800
Subject: [PATCH 76/76] Address comments

---
 omnigibson/objects/dataset_object.py   | 2 +-
 omnigibson/objects/object_base.py      | 2 +-
 omnigibson/objects/primitive_object.py | 2 +-
 omnigibson/objects/stateful_object.py  | 2 +-
 omnigibson/objects/usd_object.py       | 2 +-
 omnigibson/prims/entity_prim.py        | 1 -
 omnigibson/prims/rigid_prim.py         | 6 +++---
 omnigibson/utils/deprecated_utils.py   | 2 +-
 8 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/omnigibson/objects/dataset_object.py b/omnigibson/objects/dataset_object.py
index 3b0da0fd7..7db6bf02a 100644
--- a/omnigibson/objects/dataset_object.py
+++ b/omnigibson/objects/dataset_object.py
@@ -86,7 +86,7 @@ def __init__(
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
             kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
                 collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
-                are satisfied, else False.
+                are satisfied (see object_base.py post_load function), else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py
index f1ca153b1..faa08ef9a 100644
--- a/omnigibson/objects/object_base.py
+++ b/omnigibson/objects/object_base.py
@@ -71,7 +71,7 @@ def __init__(
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
             kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
                 collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
-                are satisfied, else False.
+                are satisfied (see object_base.py post_load function), else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
diff --git a/omnigibson/objects/primitive_object.py b/omnigibson/objects/primitive_object.py
index 724c4191c..73ae3826c 100644
--- a/omnigibson/objects/primitive_object.py
+++ b/omnigibson/objects/primitive_object.py
@@ -69,7 +69,7 @@ def __init__(
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
             kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
                 collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
-                are satisfied, else False.
+                are satisfied (see object_base.py post_load function), else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
diff --git a/omnigibson/objects/stateful_object.py b/omnigibson/objects/stateful_object.py
index f4757efda..9d8aecb9e 100644
--- a/omnigibson/objects/stateful_object.py
+++ b/omnigibson/objects/stateful_object.py
@@ -92,7 +92,7 @@ def __init__(
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
             kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
                 collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
-                are satisfied, else False.
+                are satisfied (see object_base.py post_load function), else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
diff --git a/omnigibson/objects/usd_object.py b/omnigibson/objects/usd_object.py
index 337e5bde5..4e07eb1d7 100644
--- a/omnigibson/objects/usd_object.py
+++ b/omnigibson/objects/usd_object.py
@@ -55,7 +55,7 @@ def __init__(
             visual_only (bool): Whether this object should be visual only (and not collide with any other objects)
             kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any
                 collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria
-                are satisfied, else False.
+                are satisfied (see object_base.py post_load function), else False.
             self_collisions (bool): Whether to enable self collisions for this object
             prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH}
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py
index 2473f238f..06bf07a78 100644
--- a/omnigibson/prims/entity_prim.py
+++ b/omnigibson/prims/entity_prim.py
@@ -242,7 +242,6 @@ def update_joints(self):
         if self._articulation_view and not self.kinematic_only:
             self._n_dof = self._articulation_view.num_dof
 
-            # TODO: This still includes fixed joints for objects that also have non-fixed?
             # Additionally grab DOF info if we have non-fixed joints
             if self._n_dof > 0:
                 for i in range(self._articulation_view._metadata.joint_count):
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index d6d93d51b..dbc530012 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -82,9 +82,9 @@ def _post_load(self):
         self._rigid_prim_view_direct = RigidPrimView(self._prim_path)
 
         # Set it to be kinematic if necessary
-        if "kinematic_only" in self._load_config and self._load_config["kinematic_only"]:
-            self.set_attribute("physics:kinematicEnabled", True)
-            self.set_attribute("physics:rigidBodyEnabled", False)
+        kinematic_only = "kinematic_only" in self._load_config and self._load_config["kinematic_only"]
+        self.set_attribute("physics:kinematicEnabled", kinematic_only)
+        self.set_attribute("physics:rigidBodyEnabled", not kinematic_only)
 
         # run super first
         super()._post_load()
diff --git a/omnigibson/utils/deprecated_utils.py b/omnigibson/utils/deprecated_utils.py
index df2321836..755d9a4ae 100644
--- a/omnigibson/utils/deprecated_utils.py
+++ b/omnigibson/utils/deprecated_utils.py
@@ -475,4 +475,4 @@ def disable_gravities(self, indices: Optional[Union[np.ndarray, list, torch.Tens
                         rigid_api = PhysxSchema.PhysxRigidBodyAPI.Apply(self._prims[i])
                     self._physx_rigid_body_apis[i] = rigid_api
                 self._physx_rigid_body_apis[i].GetDisableGravityAttr().Set(True)
-            return
\ No newline at end of file
+            return