Merge branch 'og-develop' into doc-update

omnigibson/object_states/contact_particles.py

@@ -49,7 +49,7 @@ def report_hit(hit):
             return continue_traversal
 
         # Grab the relaxed AABB of this object or its link for coarse filtering of particles to ignore checking
-        lower, upper = self.obj.states[AABB].get_value() if link is None else link.aabb
+        lower, upper = self.obj.states[AABB].get_value() if link is None else link.visual_aabb
 
         # Add margin for filtering inbound
         lower = lower - (system.particle_radius + m.CONTACT_AABB_TOLERANCE)

omnigibson/object_states/inside.py

@@ -1,11 +1,11 @@
+import numpy as np
 import omnigibson as og
 from omnigibson.object_states.aabb import AABB
 from omnigibson.object_states.adjacency import HorizontalAdjacency, VerticalAdjacency, flatten_planes
 from omnigibson.object_states.kinematics_mixin import KinematicsMixin
 from omnigibson.object_states.object_state_base import BooleanStateMixin, RelativeObjectState
 from omnigibson.utils.object_state_utils import sample_kinematics
 from omnigibson.utils.constants import PrimType
-from omnigibson.utils.usd_utils import BoundingBoxAPI
 from omnigibson.utils.object_state_utils import m as os_m
 
 
@@ -41,9 +41,9 @@ def _get_value(self, other):
         # Since we usually check for a small set of outer objects, this is cheap
         aabb_lower, aabb_upper = self.obj.states[AABB].get_value()
         inner_object_pos = (aabb_lower + aabb_upper) / 2.0
-        outer_object_aabb = other.states[AABB].get_value()
-
-        if not BoundingBoxAPI.aabb_contains_point(inner_object_pos, outer_object_aabb):
+        outer_object_aabb_lo, outer_object_aabb_hi = other.states[AABB].get_value()
+        
+        if not (np.less_equal(outer_object_aabb_lo, inner_object_pos).all() and np.less_equal(inner_object_pos, outer_object_aabb_hi).all()):
             return False
 
         # Our definition of inside: an object A is inside an object B if there

omnigibson/object_states/particle_modifier.py

@@ -402,7 +402,7 @@ def check_overlap():
             # Define the function for checking whether points are within the adjacency mesh
             def check_in_adjacency_mesh(particle_positions):
                 # Define the AABB bounds
-                lower, upper = self.link.aabb
+                lower, upper = self.link.visual_aabb
                 # Add the margin
                 lower -= m.PARTICLE_MODIFIER_ADJACENCY_AREA_MARGIN
                 upper += m.PARTICLE_MODIFIER_ADJACENCY_AREA_MARGIN
@@ -413,7 +413,7 @@ def check_in_adjacency_mesh(particle_positions):
             def check_overlap():
                 nonlocal valid_hit
                 valid_hit = False
-                aabb = self.link.aabb
+                aabb = self.link.visual_aabb
                 og.sim.psqi.overlap_box(
                     halfExtent=(aabb[1] - aabb[0]) / 2.0 + m.PARTICLE_MODIFIER_ADJACENCY_AREA_MARGIN,
                     pos=(aabb[1] + aabb[0]) / 2.0,
@@ -1120,7 +1120,7 @@ def _modify_particles(self, system):
                     system.create_attachment_group(obj=self.obj)
                 avg_scale = np.cbrt(np.product(self.obj.scale))
                 scales = system.sample_scales_by_group(group=group, n=len(start_points))
-                cuboid_dimensions = scales * system.particle_object.aabb_extent.reshape(1, 3) * avg_scale
+                cuboid_dimensions = scales * system.particle_object.extent.reshape(1, 3) * avg_scale
             else:
                 scales = None
                 cuboid_dimensions = np.zeros(3)
@@ -1317,7 +1317,7 @@ def _sample_particle_locations_from_adjacency_area(self, system):
         """
         # Randomly sample end points from within the object's AABB
         n_samples = self._get_max_particles_limit_per_step(system=system)
-        lower, upper = self.link.aabb
+        lower, upper = self.link.visual_aabb
         lower = lower.reshape(1, 3) - m.PARTICLE_MODIFIER_ADJACENCY_AREA_MARGIN
         upper = upper.reshape(1, 3) + m.PARTICLE_MODIFIER_ADJACENCY_AREA_MARGIN
         lower_upper = np.concatenate([lower, upper], axis=0)

omnigibson/objects/dataset_object.py

@@ -14,7 +14,6 @@
 from omnigibson.objects.usd_object import USDObject
 from omnigibson.utils.constants import AVERAGE_CATEGORY_SPECS, DEFAULT_JOINT_FRICTION, SPECIAL_JOINT_FRICTIONS, JointType
 import omnigibson.utils.transform_utils as T
-from omnigibson.utils.usd_utils import BoundingBoxAPI
 from omnigibson.utils.asset_utils import get_all_object_category_models
 from omnigibson.utils.constants import PrimType
 from omnigibson.macros import gm, create_module_macros
@@ -530,10 +529,8 @@ def get_base_aligned_bbox(self, link_name=None, visual=False, xy_aligned=False,
                     # If we're visual and the mesh is not visible, there is no fallback so continue
                     if bbox_type == "visual" and not np.all(tuple(mesh.visible for mesh in meshes.values())):
                         continue
-                    # If no BB annotation is available, get the AABB for this link.
-                    aabb_center, aabb_extent = BoundingBoxAPI.compute_center_extent(prim=link)
-                    aabb_vertices_in_world = aabb_center + np.array(list(itertools.product((1, -1), repeat=3))) * (
-                            aabb_extent / 2
+                    aabb_vertices_in_world = link.aabb_center + np.array(list(itertools.product((1, -1), repeat=3))) * (
+                            link.aabb_extent / 2
                     )
                     aabb_vertices_in_base_frame = trimesh.transformations.transform_points(
                         aabb_vertices_in_world, world_to_base_frame

omnigibson/prims/entity_prim.py

@@ -12,7 +12,6 @@
 from omnigibson.prims.xform_prim import XFormPrim
 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
 
 from omnigibson.macros import gm, create_module_macros
 
@@ -621,7 +620,6 @@ def set_joint_positions(self, positions, indices=None, normalized=False, drive=F
             self._articulation_view.set_joint_position_targets(positions, joint_indices=indices)
         else:
             self._articulation_view.set_joint_positions(positions, joint_indices=indices)
-            BoundingBoxAPI.clear()
 
     def set_joint_velocities(self, velocities, indices=None, normalized=False, drive=False):
         """
@@ -922,7 +920,6 @@ def set_position_orientation(self, position=None, orientation=None):
             if orientation is not None:
                 orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
             self._articulation_view.set_world_poses(position, orientation)
-            BoundingBoxAPI.clear()
 
     def get_position_orientation(self):
         # If the simulation isn't running, we should read from this prim's XForm (object-level) properties directly
@@ -953,7 +950,6 @@ def set_local_pose(self, position=None, orientation=None):
             if orientation is not None:
                 orientation = np.asarray(orientation)[None, [3, 0, 1, 2]]
             self._articulation_view.set_local_poses(position, orientation)
-            BoundingBoxAPI.clear()
 
     def get_local_pose(self):
         # If the simulation isn't running, we should read from this prim's XForm (object-level) properties directly
@@ -1241,11 +1237,46 @@ def aabb(self):
             aabb_lo, aabb_hi = np.min(particle_positions, axis=0) - particle_contact_offset, \
                                np.max(particle_positions, axis=0) + particle_contact_offset
         else:
-            aabb_lo, aabb_hi = super().aabb
-            aabb_lo, aabb_hi = np.array(aabb_lo), np.array(aabb_hi)
-
+            points_world = []
+            for link in self._links.values():
+                hull_points = link.collision_boundary_points
+                if hull_points is None:
+                    continue
+
+                position, orientation = link.get_position_orientation()
+                scale = link.scale
+                points_scaled = hull_points * scale
+                points_rotated = np.dot(T.quat2mat(orientation), points_scaled.T).T
+                points_transformed = points_rotated + position
+                points_world.append(points_transformed)
+
+            all_points = np.concatenate(points_world, axis=0)
+            aabb_lo = np.min(all_points, axis=0)
+            aabb_hi = np.max(all_points, axis=0)
         return aabb_lo, aabb_hi
 
+    @property
+    def aabb_extent(self):
+        """
+        Get this xform's actual bounding box extent
+
+        Returns:
+            3-array: (x,y,z) bounding box
+        """
+        min_corner, max_corner = self.aabb
+        return max_corner - min_corner
+
+    @property
+    def aabb_center(self):
+        """
+        Get this xform's actual bounding box center
+
+        Returns:
+            3-array: (x,y,z) bounding box center
+        """
+        min_corner, max_corner = self.aabb
+        return (max_corner + min_corner) / 2.0
+
     def get_coriolis_and_centrifugal_forces(self, clone=True):
         """
         Args:

omnigibson/prims/geom_prim.py

@@ -1,10 +1,13 @@
+from functools import cached_property
 import numpy as np
+import trimesh
 
 import omnigibson as og
 import omnigibson.lazy as lazy
 from omnigibson.macros import gm
 from omnigibson.prims.xform_prim import XFormPrim
 from omnigibson.utils.python_utils import assert_valid_key
+import omnigibson.utils.transform_utils as T
 
 
 class GeomPrim(XFormPrim):
@@ -116,6 +119,64 @@ def opacity(self, opacity):
             self.material.opacity_constant = opacity
         else:
             self.set_attribute("primvars:displayOpacity", np.array([opacity]))
+
+    @property
+    def points(self):
+        """
+        Returns:
+            np.ndarray: Local poses of all points
+        """
+        # If the geom is a mesh we can directly return its points.
+        mesh = self.prim
+        mesh_type = mesh.GetPrimTypeInfo().GetTypeName()
+        if mesh_type == "Mesh":
+            return self.prim.GetAttribute("points").Get()
+
+        # Generate a trimesh for other shapes
+        if mesh_type == "Sphere":
+            radius = mesh.GetAttribute("radius").Get()
+            mesh = trimesh.creation.icosphere(subdivision=3, radius=radius)
+        elif mesh_type == "Cube":
+            extent = mesh.GetAttribute("size").Get()
+            mesh = trimesh.creation.box([extent]*3)
+        elif mesh_type == "Cone":
+            radius = mesh.GetAttribute("radius").Get()
+            height = mesh.GetAttribute("height").Get()
+            mesh = trimesh.creation.cone(radius=radius, height=height)
+
+            # Trimesh cones are centered at the base. We'll move them down by half the height.
+            transform = trimesh.transformations.translation_matrix([0, 0, -height / 2])
+            mesh.apply_transform(transform)
+        elif mesh_type == "Cylinder":
+            radius = mesh.GetAttribute("radius").Get()
+            height = mesh.GetAttribute("height").Get()
+            mesh = trimesh.creation.cylinder(radius=radius, height=height)
+        else:
+            raise ValueError(f"Cannot compute volume for mesh of type: {mesh_type}")
+
+        # Return the vertices of the trimesh
+        return mesh.vertices
+
+    @property
+    def points_in_parent_frame(self):
+        points = self.points
+        if points is None:
+            return None
+        position, orientation = self.get_local_pose()
+        scale = self.scale
+        points_scaled = points * scale
+        points_rotated = np.dot(T.quat2mat(orientation), points_scaled.T).T
+        points_transformed = points_rotated + position
+        return points_transformed
+
+    @cached_property
+    def extent(self):
+        """
+        Returns:
+            np.ndarray: The unscaled 3d extent of the mesh in its local frame.
+        """
+        points = self.points
+        return np.max(points, axis=0) - np.min(points, axis=0)
 
 
 class CollisionGeomPrim(GeomPrim):

omnigibson/prims/joint_prim.py

@@ -8,7 +8,6 @@
 from omnigibson.utils.constants import JointType, JointAxis
 from omnigibson.utils.python_utils import assert_valid_key
 import omnigibson.utils.transform_utils as T
-from omnigibson.utils.usd_utils import BoundingBoxAPI
 
 from omnigibson.controllers.controller_base import ControlType
 
@@ -734,7 +733,6 @@ def set_pos(self, pos, normalized=False, drive=False):
         # Set the DOF(s) in this joint
         if not drive:
             self._articulation_view.set_joint_positions(positions=pos, joint_indices=self.dof_indices)
-            BoundingBoxAPI.clear()
 
         # Also set the target
         self._articulation_view.set_joint_position_targets(positions=pos, joint_indices=self.dof_indices)