From 3ac33e0dbb1654d8a72659467eca3eb1ba21f7d9 Mon Sep 17 00:00:00 2001 From: Chengshu Li Date: Tue, 24 Sep 2024 15:35:47 -0700 Subject: [PATCH 1/3] fix joint state caching in KinematicMixin --- omnigibson/object_states/kinematics_mixin.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/object_states/kinematics_mixin.py b/omnigibson/object_states/kinematics_mixin.py index 327afde89..29d112c09 100644 --- a/omnigibson/object_states/kinematics_mixin.py +++ b/omnigibson/object_states/kinematics_mixin.py @@ -29,7 +29,7 @@ class KinematicsMixin(BaseObjectState): info[self.obj] = {"q": self.obj.states[Joint].get_value(), "p": self.obj.states[Pose].get_value()} for arg in get_value_args: if isinstance(arg, StatefulObject): - info[arg] = {"q": self.obj.states[Joint].get_value(), "p": arg.states[Pose].get_value()} + info[arg] = {"q": arg.states[Joint].get_value(), "p": arg.states[Pose].get_value()} return info From cf6e32cd28226b2043399c7555e1914aee935436 Mon Sep 17 00:00:00 2001 From: Chengshu Li Date: Tue, 24 Sep 2024 15:36:50 -0700 Subject: [PATCH 2/3] when removing ancestral prims (all scene objects now), we deactivate them --- omnigibson/prims/prim_base.py | 9 ++++----- omnigibson/utils/sim_utils.py | 28 --------------------------- omnigibson/utils/usd_utils.py | 36 +++++++++++++++++++++++++++++++++++ 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/omnigibson/prims/prim_base.py b/omnigibson/prims/prim_base.py index 9596443da..1d40b60dc 100644 --- a/omnigibson/prims/prim_base.py +++ b/omnigibson/prims/prim_base.py @@ -4,9 +4,8 @@ from abc import ABC, abstractmethod import omnigibson as og import omnigibson.lazy as lazy from omnigibson.utils.python_utils import Recreatable, Serializable -from omnigibson.utils.sim_utils import check_deletable_prim from omnigibson.utils.ui_utils import create_module_logger -from omnigibson.utils.usd_utils import scene_relative_prim_path_to_absolute +from omnigibson.utils.usd_utils import delete_or_deactivate_prim, scene_relative_prim_path_to_absolute # Create module logger log = create_module_logger(module_name=__name__) @@ -133,9 +132,9 @@ class BasePrim(Serializable, Recreatable, ABC): if not self._loaded: raise ValueError("Cannot remove a prim that was never loaded.") - # Remove prim if it can be deleted - if check_deletable_prim(self.prim_path): - lazy.omni.isaac.core.utils.prims.delete_prim(self.prim_path) + # Remove or deactivate prim if it's possible + if not delete_or_deactivate_prim(self.prim_path): + log.warning(f"Prim {self.name} at prim_path {self.prim_path} could not be deleted or deactivated.") def _load(self): """ diff --git a/omnigibson/utils/sim_utils.py b/omnigibson/utils/sim_utils.py index 1a0265c56..1ccb9c4bf 100644 --- a/omnigibson/utils/sim_utils.py +++ b/omnigibson/utils/sim_utils.py @@ -55,34 +55,6 @@ def set_carb_setting(carb_settings, setting, value): raise TypeError(f"Value of type {type(value)} is not supported.") -def check_deletable_prim(prim_path): - """ - Checks whether the prim defined at @prim_path can be deleted. - - Args: - prim_path (str): Path defining which prim should be checked for deletion - - Returns: - bool: Whether the prim can be deleted or not - """ - if not lazy.omni.isaac.core.utils.prims.is_prim_path_valid(prim_path): - return False - if lazy.omni.isaac.core.utils.prims.is_prim_no_delete(prim_path): - return False - if lazy.omni.isaac.core.utils.prims.is_prim_ancestral(prim_path): - return False - if lazy.omni.isaac.core.utils.prims.get_prim_type_name(prim_path=prim_path) == "PhysicsScene": - return False - if prim_path == "/World": - return False - if prim_path == "/": - return False - # Don't remove any /Render prims as that can cause crashes - if prim_path.startswith("/Render"): - return False - return True - - def prims_to_rigid_prim_set(inp_prims): """ Converts prims @inp_prims into its corresponding set of rigid prims diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index 589e0e337..3df7fc318 100644 --- a/omnigibson/utils/usd_utils.py +++ b/omnigibson/utils/usd_utils.py @@ -1743,3 +1743,39 @@ def deep_copy_prim(source_root_prim, dest_stage, dest_root_path): for child in source_prim.GetAllChildren(): new_dest_path = dest_path + "/" + child.GetName() queue.append((child, new_dest_path)) + + +def delete_or_deactivate_prim(prim_path): + """ + Attept to delete or deactivate the prim defined at @prim_path. + + Args: + prim_path (str): Path defining which prim should be deleted or deactivated + + Returns: + bool: Whether the operation was successful or not + """ + if not lazy.omni.isaac.core.utils.prims.is_prim_path_valid(prim_path): + return False + if lazy.omni.isaac.core.utils.prims.is_prim_no_delete(prim_path): + return False + if lazy.omni.isaac.core.utils.prims.get_prim_type_name(prim_path=prim_path) == "PhysicsScene": + return False + if prim_path == "/World": + return False + if prim_path == "/": + return False + # Don't remove any /Render prims as that can cause crashes + if prim_path.startswith("/Render"): + return False + + # If the prim is not ancestral, we can delete it. + if not lazy.omni.isaac.core.utils.prims.is_prim_ancestral(prim_path): + lazy.omni.usd.commands.DeletePrimsCommand([prim_path], destructive=True).do() + + # Otherwise, we can only deactivate it, which essentially serves the same purpose. + # All objects that are originally in the scene are ancestral because we add the pre-build scene to the stage. + else: + lazy.omni.usd.commands.DeletePrimsCommand([prim_path], destructive=False).do() + + return True From 458d32a0c269e121fc21a5007b7b85ce34f286c1 Mon Sep 17 00:00:00 2001 From: Chengshu Li Date: Wed, 25 Sep 2024 17:22:45 -0700 Subject: [PATCH 3/3] temporarily fix attachment (demo and test pass), and fix bugs in RigidContactAPI) --- .../examples/object_states/attachment_demo.py | 12 ++--- omnigibson/object_states/attached_to.py | 3 +- omnigibson/robots/manipulation_robot.py | 53 ++++++++++++------- omnigibson/utils/physx_utils.py | 6 +++ omnigibson/utils/usd_utils.py | 13 ++++- tests/test_object_states.py | 11 +++- 6 files changed, 66 insertions(+), 32 deletions(-) diff --git a/omnigibson/examples/object_states/attachment_demo.py b/omnigibson/examples/object_states/attachment_demo.py index 129e520f6..e91c58818 100644 --- a/omnigibson/examples/object_states/attachment_demo.py +++ b/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])) - - 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() + # Lower the mass of the baseboard - otherwise, the gravity will create enough torque to break the joint + shelf_baseboard.root_link.mass = 0.1 input( "\n\nShelf parts fall to their correct poses and get automatically attached to the back panel.\n" diff --git a/omnigibson/object_states/attached_to.py b/omnigibson/object_states/attached_to.py index 41b355629..904d3c51b 100644 --- a/omnigibson/object_states/attached_to.py +++ b/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, ) diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index b4d76f5c7..de8b6d954 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/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 - robot_contact_links = {} - for con_data in raw_contact_data: + # Translate to robot contact data + robot_contact_links = dict() + 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: - robot_contact_links[other_contact] = set() - robot_contact_links[other_contact].add(link_contact) - - 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] + contact_data.add(other_contact) + else: + other_contact, link_contact, point = con_data + contact_data.add((other_contact, point)) 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, ) diff --git a/omnigibson/utils/physx_utils.py b/omnigibson/utils/physx_utils.py index 6260a6867..ef2503cc2 100644 --- a/omnigibson/utils/physx_utils.py +++ b/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) diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py index 3df7fc318..6582474c4 100644 --- a/omnigibson/utils/usd_utils.py +++ b/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 [] diff --git a/tests/test_object_states.py b/tests/test_object_states.py index cbaf3b4b2..14509c543 100644 --- a/tests/test_object_states.py +++ b/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)