temporarily fix attachment (demo and test pass), and fix bugs in RigidContactAPI)

omnigibson/examples/object_states/attachment_demo.py

@@ -37,7 +37,6 @@ def main(random_selection=False, headless=False, short_exec=False):
             category="shelf_back_panel",
             model="gjsnrt",
             position=[0, 0, 0.01],
-            fixed_base=True,
             abilities={"attachable": {}},
         )
     )
@@ -99,7 +98,7 @@ def main(random_selection=False, headless=False, short_exec=False):
             name=f"shelf_baseboard",
             category="shelf_baseboard",
             model="hlhneo",
-            position=[0, -0.97884506, base_z + delta_z * idx],
+            position=[0, -10.97884506, base_z + delta_z * idx],
             abilities={"attachable": {}},
         )
     )
@@ -119,13 +118,10 @@ def main(random_selection=False, headless=False, short_exec=False):
         env.step([])
 
     shelf_baseboard = env.scene.object_registry("name", "shelf_baseboard")
-    shelf_baseboard.set_position_orientation(position=[0, -0.979, 0.26], orientation=[0, 0, 0, 1])
+    shelf_baseboard.set_position_orientation(position=[0, -0.979, 0.21], orientation=[0, 0, 0, 1])
     shelf_baseboard.keep_still()
-    shelf_baseboard.set_linear_velocity(th.tensor([-0.2, 0, 0]))
+    # Lower the mass of the baseboard - otherwise, the gravity will create enough torque to break the joint
-
+    shelf_baseboard.root_link.mass = 0.1
-    shelf_side_left = env.scene.object_registry("name", "shelf_side_left")
-    shelf_side_left.set_position_orientation(position=[-0.4, 0.0, 0.2], orientation=[0, 0, 0, 1])
-    shelf_side_left.keep_still()
 
     input(
         "\n\nShelf parts fall to their correct poses and get automatically attached to the back panel.\n"

omnigibson/object_states/attached_to.py

@@ -316,7 +316,7 @@ class AttachedTo(
             _, 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)
+        # self._disable_collision_between_child_and_parent(child=self.obj, parent=other)
 
         # Set the parent references
         self.parent = other
@@ -341,6 +341,7 @@ class AttachedTo(
             joint_frame_in_parent_frame_quat=parent_local_quat,
             joint_frame_in_child_frame_pos=th.zeros(3),
             joint_frame_in_child_frame_quat=th.tensor([0.0, 0.0, 0.0, 1.0]),
+            exclude_from_articulation=True,
             **kwargs,
         )
 

omnigibson/robots/manipulation_robot.py

@@ -273,32 +273,38 @@ class ManipulationRobot(BaseRobot):
                     set of unique robot link prim_paths that it is in contact with
         """
         arm = self.default_arm if arm == "default" else arm
-        # Get robot contact links
+
+        # Get robot finger links
+        finger_paths = set([link.prim_path for link in self.finger_links[arm]])
+
+        # Get robot links
         link_paths = set(self.link_prim_paths)
 
         if not return_contact_positions:
             raw_contact_data = {
                 (row, col)
-                for row, col in GripperRigidContactAPI.get_contact_pairs(self.scene.idx, column_prim_paths=link_paths)
+                for row, col in GripperRigidContactAPI.get_contact_pairs(self.scene.idx, column_prim_paths=finger_paths)
+                if row not in link_paths
+            }
+        else:
+            raw_contact_data = {
+                (row, col, point)
+                for row, col, force, point, normal, sep in GripperRigidContactAPI.get_contact_data(
+                    self.scene.idx, column_prim_paths=finger_paths
+                )
                 if row not in link_paths
             }
 
-            # Translate that to robot contact data
+        # Translate to robot contact data
-            robot_contact_links = {}
+        robot_contact_links = dict()
-            for con_data in raw_contact_data:
+        contact_data = set()
+        for con_data in raw_contact_data:
+            if not return_contact_positions:
                 other_contact, link_contact = con_data
-                if other_contact not in robot_contact_links:
+                contact_data.add(other_contact)
-                    robot_contact_links[other_contact] = set()
+            else:
-                robot_contact_links[other_contact].add(link_contact)
+                other_contact, link_contact, point = con_data
-
+                contact_data.add((other_contact, point))
-            return {other for other, _ in raw_contact_data}, robot_contact_links
-
-        # Otherwise, we rely on the simpler, but more costly, get_contact_data API.
-        contacts = GripperRigidContactAPI.get_contact_data(self.scene.idx, column_prim_paths=link_paths)
-        contact_data = {(contact[0], contact[3]) for contact in contacts}
-        robot_contact_links = {}
-        for con_data in contacts:
-            other_contact, link_contact = con_data[:2]
             if other_contact not in robot_contact_links:
                 robot_contact_links[other_contact] = set()
             robot_contact_links[other_contact].add(link_contact)
@@ -867,7 +873,7 @@ class ManipulationRobot(BaseRobot):
             if candidate_obj is None or link_name not in candidate_obj.links:
                 continue
             candidate_link = candidate_obj.links[link_name]
-            dist = th.norm(th.tensor(candidate_link.get_position_orientation()[0]) - th.tensor(gripper_center_pos))
+            dist = th.norm(candidate_link.get_position_orientation()[0] - gripper_center_pos)
             candidate_data.append((prim_path, dist))
 
         if not candidate_data:
@@ -1240,9 +1246,14 @@ class ManipulationRobot(BaseRobot):
             force_data, _ = self._find_gripper_contacts(arm=arm, return_contact_positions=True)
             for c_link_prim_path, c_contact_pos in force_data:
                 if c_link_prim_path == ag_link.prim_path:
-                    contact_pos = th.tensor(c_contact_pos)
+                    contact_pos = c_contact_pos
                     break
-        assert contact_pos is not None
+
+        assert contact_pos is not None, (
+            f"contact_pos in self._find_gripper_contacts(return_contact_positions=True) is not found in "
+            f"self._find_gripper_contacts(return_contact_positions=False). This is likely because "
+            f"GripperRigidContactAPI.get_contact_pairs and get_contact_data return inconsistent results."
+        )
 
         # Joint frame set at the contact point
         # Need to find distance between robot and contact point in robot link's local frame and
@@ -1266,6 +1277,7 @@ class ManipulationRobot(BaseRobot):
             body0=self.eef_links[arm].prim_path,
             body1=ag_link.prim_path,
             enabled=True,
+            exclude_from_articulation=True,
             joint_frame_in_parent_frame_pos=parent_frame_pos / self.scale,
             joint_frame_in_parent_frame_quat=parent_frame_orn,
             joint_frame_in_child_frame_pos=child_frame_pos / ag_obj.scale,
@@ -1448,6 +1460,7 @@ class ManipulationRobot(BaseRobot):
             body0=ag_link.prim_path,
             body1=None,
             enabled=False,
+            exclude_from_articulation=True,
             joint_frame_in_child_frame_pos=attachment_point_pos,
         )
 

omnigibson/utils/physx_utils.py

@@ -262,10 +262,16 @@ def create_physx_particleset_pointinstancer(
 
 
 def apply_force_at_pos(prim, force, pos):
+    if isinstance(force, th.Tensor):
+        force = force.cpu().numpy()
+    if isinstance(pos, th.Tensor):
+        pos = pos.cpu().numpy()
     prim_id = lazy.pxr.PhysicsSchemaTools.sdfPathToInt(prim.prim_path)
     og.sim.psi.apply_force_at_pos(og.sim.stage_id, prim_id, force, pos)
 
 
 def apply_torque(prim, foward_vect, roll_torque_scalar):
+    if isinstance(foward_vect, th.Tensor):
+        foward_vect = foward_vect.cpu().numpy()
     prim_id = lazy.pxr.PhysicsSchemaTools.sdfPathToInt(prim.prim_path)
     og.sim.psi.apply_torque(og.sim.stage_id, prim_id, foward_vect * roll_torque_scalar)

omnigibson/utils/usd_utils.py

@@ -89,6 +89,7 @@ def create_joint(
     body0=None,
     body1=None,
     enabled=True,
+    exclude_from_articulation=False,
     joint_frame_in_parent_frame_pos=None,
     joint_frame_in_parent_frame_quat=None,
     joint_frame_in_child_frame_pos=None,
@@ -107,6 +108,7 @@ def create_joint(
         body0 (str or None): absolute path to the first body's prim. At least @body0 or @body1 must be specified.
         body1 (str or None): absolute path to the second body's prim. At least @body0 or @body1 must be specified.
         enabled (bool): whether to enable this joint or not.
+        exclude_from_articulation (bool): whether to exclude this joint from the articulation or not.
         joint_frame_in_parent_frame_pos (th.tensor or None): relative position of the joint frame to the parent frame (body0).
         joint_frame_in_parent_frame_quat (th.tensor or None): relative orientation of the joint frame to the parent frame (body0).
         joint_frame_in_child_frame_pos (th.tensor or None): relative position of the joint frame to the child frame (body1).
@@ -168,6 +170,9 @@ def create_joint(
     # Possibly (un-/)enable this joint
     joint_prim.GetAttribute("physics:jointEnabled").Set(enabled)
 
+    # Possibly exclude this joint from the articulation
+    joint_prim.GetAttribute("physics:excludeFromArticulation").Set(exclude_from_articulation)
+
     # We update the simulation now without stepping physics if sim is playing so we can bypass the snapping warning from PhysicsUSD
     if og.sim.is_playing():
         with suppress_omni_log(channels=["omni.physx.plugin"]):
@@ -389,12 +394,14 @@ class RigidContactAPIImpl:
         # Get all of the (row, col) pairs where the impulse is greater than 0
         return {
             (interesting_row_paths[row], interesting_col_paths[col])
-            for row, col in th.nonzero(interesting_impulses > 0, as_tuple=True)
+            for row, col in zip(*th.nonzero(interesting_impulses > 0, as_tuple=True))
         }
 
     def get_contact_data(self, scene_idx, row_prim_paths=None, column_prim_paths=None):
         # First check if the object has any contacts
-        impulses = self.get_all_impulses(scene_idx)
+        impulses = th.norm(self.get_all_impulses(scene_idx), dim=-1)
+        assert impulses.ndim == 2, f"Impulse matrix should be 2D, found shape {impulses.shape}"
+
         row_idx = (
             list(range(impulses.shape[0]))
             if row_prim_paths is None
@@ -406,6 +413,8 @@ class RigidContactAPIImpl:
             else [self.get_body_col_idx(path)[1] for path in column_prim_paths]
         )
         relevant_impulses = impulses[row_idx][:, col_idx]
+
+        # Early return if not in contact.
         if not th.any(relevant_impulses > 0):
             return []
 

tests/test_object_states.py

@@ -716,19 +716,24 @@ def test_toggled_on(env):
     assert not stove.states[ToggledOn].get_value()
 
 
-@pytest.mark.skip(reason="skipping attachment for now")
 @og_test
 def test_attached_to(env):
     shelf_back_panel = env.scene.object_registry("name", "shelf_back_panel")
     shelf_shelf = env.scene.object_registry("name", "shelf_shelf")
     shelf_baseboard = env.scene.object_registry("name", "shelf_baseboard")
 
+    # Lower the mass of the shelf - otherwise, the gravity will create enough torque to break the joint
+    shelf_shelf.root_link.mass = 0.1
+
     shelf_back_panel.set_position_orientation(position=[0, 0, 0.01], orientation=[0, 0, 0, 1])
     shelf_back_panel.keep_still()
     shelf_shelf.set_position_orientation(position=[0, 0.03, 0.17], orientation=[0, 0, 0, 1])
     shelf_shelf.keep_still()
 
+    og.sim.step()
+
     # The shelf should not be attached to the back panel (no contact yet)
+    assert not shelf_shelf.states[Touching].get_value(shelf_back_panel)
     assert not shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
 
     # Let the shelf fall
@@ -736,16 +741,20 @@ def test_attached_to(env):
         og.sim.step()
 
     # The shelf should be attached to the back panel
+    assert shelf_shelf.states[Touching].get_value(shelf_back_panel)
     assert shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
 
+    # Try to attach again (should be no-op)
     assert shelf_shelf.states[AttachedTo].set_value(shelf_back_panel, True)
     # The shelf should still be attached to the back panel
     assert shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
 
+    # Detach
     assert shelf_shelf.states[AttachedTo].set_value(shelf_back_panel, False)
     # The shelf should not be attached to the back panel
     assert not shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
 
+    # Attach again
     assert shelf_shelf.states[AttachedTo].set_value(shelf_back_panel, True)
     # shelf should be attached to the back panel
     assert shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)