Merge pull request #659 from StanfordVL/fix/updates-from-sampling

changes from sampling branch

omnigibson/macros.py

@@ -88,6 +88,12 @@
 # Maximum particle contacts allowed
 gm.GPU_MAX_PARTICLE_CONTACTS = 1024 * 1024
 
+# Maximum rigid contacts -- 524288 is default value from omni, but increasing too much can sometimes lead to crashes
+gm.GPU_MAX_RIGID_CONTACT_COUNT = 524288 * 4
+
+# Maximum rigid patches -- 81920 is default value from omni, but increasing too much can sometimes lead to crashes
+gm.GPU_MAX_RIGID_PATCH_COUNT = 81920 * 4
+
 # Whether to enable object state logic or not
 gm.ENABLE_OBJECT_STATES = True
 

omnigibson/object_states/attached_to.py

@@ -14,6 +14,7 @@
 from omnigibson.utils.usd_utils import create_joint
 from omnigibson.utils.ui_utils import create_module_logger
 from omnigibson.utils.python_utils import classproperty
+from omnigibson.utils.usd_utils import CollisionAPI
 
 # Create module logger
 log = create_module_logger(module_name=__name__)
@@ -30,7 +31,15 @@
 m.DEFAULT_BREAK_FORCE = 1000  # Newton
 m.DEFAULT_BREAK_TORQUE = 1000  # Newton-Meter
 
-
+# TODO: Make AttachedTo into a global state that manages all the attachments in the scene.
+# When an attachment of a child and a parent is about to happen:
+# 1. stop the sim
+# 2. remove all existing attachment joints (and save information to restore later)
+# 3. disable collision between the child and the parent
+# 4. play the sim
+# 5. reload the state
+# 6. restore all existing attachment joints
+# 7. create the joint
 class AttachedTo(RelativeObjectState, BooleanStateMixin, ContactSubscribedStateMixin, JointBreakSubscribedStateMixin, LinkBasedStateMixin):
     """
         Handles attachment between two rigid objects, by creating a fixed/spherical joint between self.obj (child) and
@@ -43,6 +52,20 @@ class AttachedTo(RelativeObjectState, BooleanStateMixin, ContactSubscribedStateM
         on_contact function attempts to attach self.obj to other when a CONTACT_FOUND event happens
         on_joint_break function breaks the current attachment
     """
+    # This is to force the __init__ args to be "self" and "obj" only.
+    # Otherwise, it will inherit from LinkBasedStateMixin and the __init__ args will be "self", "args", "kwargs".
+    def __init__(self, obj):
+        # Run super method
+        super().__init__(obj=obj)
+
+    def initialize(self):
+        super().initialize()
+        og.sim.add_callback_on_stop(name=f"{self.obj.name}_detach", callback=self._detach)
+        self.parents_disabled_collisions = set()
+
+    def remove(self):
+        super().remove()
+        og.sim.remove_callback_on_stop(name=f"{self.obj.name}_detach")
 
     @classproperty
     def metalink_prefix(cls):
@@ -89,24 +112,54 @@ def on_contact(self, other, contact_headers, contact_data):
                 if self.set_value(other, True):
                     break
 
-    def _set_value(self, other, new_value, bypass_alignment_checking=False):
+    def _set_value(self, other, new_value, bypass_alignment_checking=False, check_physics_stability=False, can_joint_break=True):
+        """
+        Args:
+            other (DatasetObject): parent object to attach to.
+            new_value (bool): whether to attach or detach.
+            bypass_alignment_checking (bool): whether to bypass alignment checking when finding attachment links.
+                Normally when finding attachment links, we check if the child and parent links have aligned positions
+                or poses. This flag allows users to bypass this check and find attachment links solely based on the
+                attachment meta link types. Default is False.
+            check_physics_stability (bool): whether to check if the attachment is stable after attachment.
+                If True, it will check if the child object is not colliding with other objects except the parent object.
+                If False, it will not check the stability and simply attach the child to the parent.
+                Default is False.
+            can_joint_break (bool): whether the joint can break or not.
+
+        Returns:
+            bool: whether the attachment setting was successful or not.
+        """
         # Attempt to attach
         if new_value:
             if self.parent == other:
                 # Already attached to this object. Do nothing.
                 return True
-            elif self.parent is None:
-                # Find attachment links that satisfy the proximity requirements
-                child_link, parent_link = self._find_attachment_links(other, bypass_alignment_checking)
-                if child_link is not None:
-                    self._attach(other, child_link, parent_link)
-                    return True
-                else:
-                    return False
-            else:
+            elif self.parent is not None:
                 log.debug(f"Trying to attach object {self.obj.name} to object {other.name},"
                           f"but it is already attached to object {self.parent.name}. Try detaching first.")
                 return False
+            else:
+                # Find attachment links that satisfy the proximity requirements
+                child_link, parent_link = self._find_attachment_links(other, bypass_alignment_checking)
+                if child_link is None:
+                    return False
+                else:
+                    if check_physics_stability:
+                        state = og.sim.dump_state()
+                    self._attach(other, child_link, parent_link, can_joint_break=can_joint_break)
+                    if not check_physics_stability:
+                        return True
+                    else:
+                        og.sim.step_physics()
+                        # self.obj should not collide with other objects except the parent
+                        success = len(self.obj.states[ContactBodies].get_value(ignore_objs=(other,))) == 0
+                        if success:
+                            return True
+                        else:
+                            self._detach()
+                            og.sim.load_state(state)
+                            return False
 
         # Attempt to detach
         else:
@@ -190,7 +243,7 @@ def _get_parent_candidates(self, other):
     def attachment_joint_prim_path(self):
         return f"{self.parent_link.prim_path}/{self.obj.name}_attachment_joint" if self.parent_link is not None else None
 
-    def _attach(self, other, child_link, parent_link, joint_type=m.DEFAULT_JOINT_TYPE, break_force=m.DEFAULT_BREAK_FORCE, break_torque=m.DEFAULT_BREAK_TORQUE):
+    def _attach(self, other, child_link, parent_link, joint_type=m.DEFAULT_JOINT_TYPE, can_joint_break=True):
         """
         Creates a fixed or spherical joint between a male meta link of self.obj (@child_link) and a female meta link of
          @other (@parent_link) with a given @joint_type, @break_force and @break_torque
@@ -200,16 +253,9 @@ def _attach(self, other, child_link, parent_link, joint_type=m.DEFAULT_JOINT_TYP
             child_link (RigidPrim): male meta link of @self.obj.
             parent_link (RigidPrim): female meta link of @other.
             joint_type (JointType): joint type of the attachment, {JointType.JOINT_FIXED, JointType.JOINT_SPHERICAL}
-            break_force (float or None): break force for linear dofs, unit is Newton.
-            break_torque (float or None): break torque for angular dofs, unit is Newton-meter.
+            can_joint_break (bool): whether the joint can break or not.
         """
         assert joint_type in {JointType.JOINT_FIXED, JointType.JOINT_SPHERICAL}, f"Unsupported joint type {joint_type}"
-        # Set the parent references
-        self.parent = other
-        self.parent_link = parent_link
-
-        # Set the child reference for @other
-        other.states[AttachedTo].children[parent_link.body_name] = self.obj
 
         # Set pose for self.obj so that child_link and parent_link align (6dof alignment for FixedJoint and 3dof alignment for SphericalJoint)
         parent_pos, parent_quat = parent_link.get_position_orientation()
@@ -229,6 +275,7 @@ def _attach(self, other, child_link, parent_link, joint_type=m.DEFAULT_JOINT_TYP
         # Actually move the object and also keep it still for stability purposes.
         self.obj.set_position_orientation(new_child_root_pos, new_child_root_quat)
         self.obj.keep_still()
+        other.keep_still()
 
         if joint_type == JointType.JOINT_FIXED:
             # FixedJoint: the parent link, the child link and the joint frame all align.
@@ -238,6 +285,18 @@ def _attach(self, other, child_link, parent_link, joint_type=m.DEFAULT_JOINT_TYP
             # The child link and the joint frame still align.
             _, parent_local_quat = T.relative_pose_transform([0, 0, 0], child_quat, [0, 0, 0], parent_quat)
 
+        # Disable collision between the parent and child objects
+        self._disable_collision_between_child_and_parent(child=self.obj, parent=other)
+
+        # Set the parent references
+        self.parent = other
+        self.parent_link = parent_link
+
+        # Set the child reference for @other
+        other.states[AttachedTo].children[parent_link.body_name] = self.obj
+
+        kwargs = {"break_force": m.DEFAULT_BREAK_FORCE, "break_torque": m.DEFAULT_BREAK_TORQUE} if can_joint_break else dict()
+
         # Create the joint
         create_joint(
             prim_path=self.attachment_joint_prim_path,
@@ -248,23 +307,61 @@ def _attach(self, other, child_link, parent_link, joint_type=m.DEFAULT_JOINT_TYP
             joint_frame_in_parent_frame_quat=parent_local_quat,
             joint_frame_in_child_frame_pos=np.zeros(3),
             joint_frame_in_child_frame_quat=np.array([0.0, 0.0, 0.0, 1.0]),
-            break_force=break_force,
-            break_torque=break_torque,
+            **kwargs
         )
 
+    def _disable_collision_between_child_and_parent(self, child, parent):
+        """
+        Disables collision between the child and parent objects
+        """
+        if parent in self.parents_disabled_collisions:
+            return
+        self.parents_disabled_collisions.add(parent)
+
+        was_playing = og.sim.is_playing()
+        if was_playing:
+            state = og.sim.dump_state()
+            og.sim.stop()
+
+        for child_link in child.links.values():
+            for parent_link in parent.links.values():
+                child_link.add_filtered_collision_pair(parent_link)
+
+        if parent.category == "wall_nail":
+            # Temporary hack to disable collision between the attached child object and all walls/floors so that objects
+            # attached to the wall_nails do not collide with the walls/floors.
+            for wall in og.sim.scene.object_registry("category", "walls", set()):
+                for wall_link in wall.links.values():
+                    for child_link in child.links.values():
+                        child_link.add_filtered_collision_pair(wall_link)
+            for wall in og.sim.scene.object_registry("category", "floors", set()):
+                for floor_link in wall.links.values():
+                    for child_link in child.links.values():
+                        child_link.add_filtered_collision_pair(floor_link)
+
+        # Temporary hack to disable gravity for the attached child object if the parent is kinematic_only
+        # Otherwise, the parent metalink will oscillate due to the gravity force of the child.
+        if parent.kinematic_only:
+            child.disable_gravity()
+
+        if was_playing:
+            og.sim.play()
+            og.sim.load_state(state)
+
     def _detach(self):
         """
         Removes the current attachment joint
         """
-        # Remove the attachment joint prim from the stage
-        og.sim.stage.RemovePrim(self.attachment_joint_prim_path)
+        if self.parent_link is not None:
+            # Remove the attachment joint prim from the stage
+            og.sim.stage.RemovePrim(self.attachment_joint_prim_path)
 
-        # Remove child reference from the parent object
-        self.parent.states[AttachedTo].children[self.parent_link.body_name] = None
+            # Remove child reference from the parent object
+            self.parent.states[AttachedTo].children[self.parent_link.body_name] = None
 
-        # Remove reference to the parent object and link
-        self.parent = None
-        self.parent_link = None
+            # Remove reference to the parent object and link
+            self.parent = None
+            self.parent_link = None
 
     @property
     def settable(self):
@@ -285,15 +382,20 @@ def _load_state(self, state):
             attached_obj = og.sim.scene.object_registry("uuid", uuid)
             assert attached_obj is not None, "attached_obj_uuid does not match any object in the scene."
 
-        # If it's currently attached to something, detach.
-        if self.parent is not None:
-            self.set_value(self.parent, False)
-            assert self.parent is None, "parent reference is not cleared after detachment"
-
-        # If the loaded state requires attachment, attach.
-        if attached_obj is not None:
-            self.set_value(attached_obj, True)
-            assert self.parent == attached_obj, "parent reference is not updated after attachment"
+        if self.parent != attached_obj:
+            # If it's currently attached to something else, detach.
+            if self.parent is not None:
+                self.set_value(self.parent, False)
+                # assert self.parent is None, "parent reference is not cleared after detachment"
+                if self.parent is not None:
+                    log.warning(f"parent reference is not cleared after detachment")
+
+            # If the loaded state requires attachment, attach.
+            if attached_obj is not None:
+                self.set_value(attached_obj, True, bypass_alignment_checking=True, check_physics_stability=False, can_joint_break=True)
+                # assert self.parent == attached_obj, "parent reference is not updated after attachment"
+                if self.parent != attached_obj:
+                    log.warning(f"parent reference is not updated after attachment")
 
     def _serialize(self, state):
         return np.array([state["attached_obj_uuid"]], dtype=float)

omnigibson/object_states/filled.py

@@ -3,12 +3,15 @@
 from omnigibson.object_states.contains import ContainedParticles
 from omnigibson.object_states.object_state_base import RelativeObjectState, BooleanStateMixin
 from omnigibson.systems.system_base import PhysicalParticleSystem, is_physical_particle_system
+from omnigibson.systems.macro_particle_system import MacroParticleSystem
 
 # Create settings for this module
 m = create_module_macros(module_path=__file__)
 
 # Proportion of object's volume that must be filled for object to be considered filled
 m.VOLUME_FILL_PROPORTION = 0.2
+m.N_MAX_MACRO_PARTICLE_SAMPLES = 500
+m.N_MAX_MICRO_PARTICLE_SAMPLES = 100000
 
 
 class Filled(RelativeObjectState, BooleanStateMixin):
@@ -20,7 +23,8 @@ def _get_value(self, system):
 
         # Check what volume is filled
         if system.n_particles > 0:
-            particle_volume = 4 / 3 * np.pi * (system.particle_radius ** 3)
+            # Treat particles as cubes
+            particle_volume = (system.particle_radius * 2) ** 3
             n_particles = self.obj.states[ContainedParticles].get_value(system).n_in_volume
             prop_filled = particle_volume * n_particles / self.obj.states[ContainedParticles].volume
             # If greater than threshold, then the volume is filled
@@ -50,6 +54,8 @@ def _set_value(self, system, new_value):
                     obj=self.obj,
                     link=contained_particles_state.link,
                     check_contact=True,
+                    max_samples=m.N_MAX_MACRO_PARTICLE_SAMPLES
+                    if issubclass(system, MacroParticleSystem) else m.N_MAX_MICRO_PARTICLE_SAMPLES
                 )
             else:
                 # Cannot set False

omnigibson/object_states/folded.py

@@ -1,6 +1,6 @@
 import numpy as np
 from collections import namedtuple
-from scipy.spatial import ConvexHull, distance_matrix
+from scipy.spatial import ConvexHull, distance_matrix, QhullError
 
 from omnigibson.macros import create_module_macros
 from omnigibson.object_states.object_state_base import BooleanStateMixin, AbsoluteObjectState
@@ -98,7 +98,13 @@ def calculate_projection_area_and_diagonal(self, dims):
         """
         cloth = self.obj.root_link
         points = cloth.keypoint_particle_positions[:, dims]
-        hull = ConvexHull(points)
+        try:
+            hull = ConvexHull(points)
+
+        # The points may be 2D-degenerate, so catch the error and return 0 if so
+        except QhullError:
+            # This is a degenerate hull, so return 0 area and diagonal
+            return 0.0, 0.0
 
         # When input points are 2-dimensional, this is the area of the convex hull.
         # Ref: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.ConvexHull.html

omnigibson/object_states/inside.py

@@ -16,7 +16,7 @@ def get_dependencies(cls):
         deps.update({AABB, HorizontalAdjacency, VerticalAdjacency})
         return deps
 
-    def _set_value(self, other, new_value):
+    def _set_value(self, other, new_value, reset_before_sampling=False):
         if not new_value:
             raise NotImplementedError("Inside does not support set_value(False)")
 
@@ -25,6 +25,10 @@ def _set_value(self, other, new_value):
 
         state = og.sim.dump_state(serialized=False)
 
+        # Possibly reset this object if requested
+        if reset_before_sampling:
+            self.obj.reset()
+
         for _ in range(os_m.DEFAULT_HIGH_LEVEL_SAMPLING_ATTEMPTS):
             if sample_kinematics("inside", self.obj, other) and self.get_value(other):
                 return True

omnigibson/object_states/on_top.py

@@ -16,7 +16,7 @@ def get_dependencies(cls):
         deps.update({Touching, VerticalAdjacency})
         return deps
 
-    def _set_value(self, other, new_value):
+    def _set_value(self, other, new_value, reset_before_sampling=False):
         if not new_value:
             raise NotImplementedError("OnTop does not support set_value(False)")
 
@@ -25,6 +25,10 @@ def _set_value(self, other, new_value):
 
         state = og.sim.dump_state(serialized=False)
 
+        # Possibly reset this object if requested
+        if reset_before_sampling:
+            self.obj.reset()
+
         for _ in range(os_m.DEFAULT_HIGH_LEVEL_SAMPLING_ATTEMPTS):
             if sample_kinematics("onTop", self.obj, other) and self.get_value(other):
                 return True

omnigibson/object_states/under.py

@@ -14,7 +14,7 @@ def get_dependencies(cls):
         deps.add(VerticalAdjacency)
         return deps
 
-    def _set_value(self, other, new_value):
+    def _set_value(self, other, new_value, reset_before_sampling=False):
         if not new_value:
             raise NotImplementedError("Under does not support set_value(False)")
 
@@ -23,6 +23,10 @@ def _set_value(self, other, new_value):
 
         state = og.sim.dump_state(serialized=False)
 
+        # Possibly reset this object if requested
+        if reset_before_sampling:
+            self.obj.reset()
+
         for _ in range(os_m.DEFAULT_HIGH_LEVEL_SAMPLING_ATTEMPTS):
             if sample_kinematics("under", self.obj, other) and self.get_value(other):
                 return True