From 70fb145c0d19e6f721266fe82f4c07995a8b45d9 Mon Sep 17 00:00:00 2001 From: Jianghanxiao Date: Tue, 24 Dec 2024 20:07:44 -0500 Subject: [PATCH 01/38] fix cloth loading problems: caused by non-watertight, and potential multiple pieces --- omnigibson/objects/dataset_object.py | 10 ------- omnigibson/systems/micro_particle_system.py | 31 ++++++++++++++++----- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/omnigibson/objects/dataset_object.py b/omnigibson/objects/dataset_object.py index 11b525b83..117960635 100644 --- a/omnigibson/objects/dataset_object.py +++ b/omnigibson/objects/dataset_object.py @@ -119,16 +119,6 @@ def __init__( assert len(available_models) > 0, f"No available models found for category {category}!" model = random.choice(available_models) - # If the model is in BAD_CLOTH_MODELS, raise an error for now -- this is a model that's unstable and needs to be fixed - # TODO: Remove this once the asset is fixed! - from omnigibson.utils.bddl_utils import BAD_CLOTH_MODELS - - if prim_type == PrimType.CLOTH and model in BAD_CLOTH_MODELS.get(category, dict()): - raise ValueError( - f"Cannot create cloth object category: {category}, model: {model} because it is " - f"currently broken ): This will be fixed in the next release!" - ) - self._model = model usd_path = self.get_usd_path(category=category, model=model, dataset_type=dataset_type) diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index a9322638a..6702f8131 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -32,18 +32,16 @@ # Create settings for this module m = create_module_macros(module_path=__file__) - -# TODO: Tune these default values! # TODO (eric): figure out whether one offset can fit all m.MAX_CLOTH_PARTICLES = 20000 # Comes from a limitation in physx - do not increase m.CLOTH_PARTICLE_CONTACT_OFFSET = 0.0075 m.CLOTH_REMESHING_ERROR_THRESHOLD = 0.05 -m.CLOTH_STRETCH_STIFFNESS = 10000.0 -m.CLOTH_BEND_STIFFNESS = 200.0 -m.CLOTH_SHEAR_STIFFNESS = 100.0 -m.CLOTH_DAMPING = 0.2 +m.CLOTH_STRETCH_STIFFNESS = 100.0 +m.CLOTH_BEND_STIFFNESS = 50.0 +m.CLOTH_SHEAR_STIFFNESS = 70.0 +m.CLOTH_DAMPING = 0.02 m.CLOTH_FRICTION = 0.4 -m.CLOTH_DRAG = 0.001 +m.CLOTH_DRAG = 0.02 m.CLOTH_LIFT = 0.003 m.MIN_PARTICLE_CONTACT_OFFSET = 0.005 # Minimum particle contact offset for physical micro particles m.FLUID_PARTICLE_PARTICLE_DISTANCE_SCALE = 0.8 # How much overlap expected between fluid particles at rest @@ -1716,6 +1714,25 @@ def clothify_mesh_prim(self, mesh_prim, remesh=True, particle_distance=None): ms.meshing_isotropic_explicit_remeshing( iterations=5, adaptive=True, targetlen=pymeshlab.AbsoluteValue(particle_distance) ) + # Make sure the clothes is watertight + ms.meshing_repair_non_manifold_edges() + ms.meshing_repair_non_manifold_vertices() + + # If the cloth has multiple pieces, only keep the largest one + ms.generate_splitting_by_connected_components(delete_source_mesh=True) + if len(ms) > 1: + log.warning(f"The cloth mesh has {len(ms)} disconnected pieces. To simplify, we only keep the mesh with largest face number.") + biggest_face_num = 0 + for split_mesh in ms: + face_num = split_mesh.face_number() + if face_num > biggest_face_num: + biggest_face_num = face_num + new_ms = pymeshlab.MeshSet() + for split_mesh in ms: + if split_mesh.face_number() == biggest_face_num: + new_ms.add_mesh(split_mesh) + ms = new_ms + avg_edge_percentage_mismatch = abs( 1.0 - particle_distance / ms.get_geometric_measures()["avg_edge_length"] ) From ea8d531d09b0436a2f37833d509bf454947c68ef Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 25 Dec 2024 01:11:12 +0000 Subject: [PATCH 02/38] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/systems/micro_particle_system.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index 6702f8131..30dee69d7 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -1717,11 +1717,13 @@ def clothify_mesh_prim(self, mesh_prim, remesh=True, particle_distance=None): # Make sure the clothes is watertight ms.meshing_repair_non_manifold_edges() ms.meshing_repair_non_manifold_vertices() - + # If the cloth has multiple pieces, only keep the largest one ms.generate_splitting_by_connected_components(delete_source_mesh=True) if len(ms) > 1: - log.warning(f"The cloth mesh has {len(ms)} disconnected pieces. To simplify, we only keep the mesh with largest face number.") + log.warning( + f"The cloth mesh has {len(ms)} disconnected pieces. To simplify, we only keep the mesh with largest face number." + ) biggest_face_num = 0 for split_mesh in ms: face_num = split_mesh.face_number() From bb927b4df6425f12da7e33bca0e3361ff8cce49d Mon Sep 17 00:00:00 2001 From: Jianghanxiao Date: Fri, 24 Jan 2025 13:40:26 -0500 Subject: [PATCH 03/38] add the script for squeezing and folding the cloth --- state_fold.py | 125 +++++++++++++++++++++++++++++++++++++ state_squeeze.py | 157 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 282 insertions(+) create mode 100644 state_fold.py create mode 100644 state_squeeze.py diff --git a/state_fold.py b/state_fold.py new file mode 100644 index 000000000..6ca97b4dd --- /dev/null +++ b/state_fold.py @@ -0,0 +1,125 @@ +from omnigibson.utils.constants import PrimType +from omnigibson.object_states import Folded, Unfolded +from omnigibson.macros import gm +import numpy as np + +import omnigibson as og +import json +import torch + +# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) +gm.ENABLE_OBJECT_STATES = True +gm.USE_GPU_DYNAMICS = True + + +def main(random_selection=False, headless=False, short_exec=False): + """ + Demo of cloth objects that can potentially be folded. + """ + og.log.info(f"Demo {__file__}\n " + "*" * 80 + "\n Description:\n" + main.__doc__ + "*" * 80) + + cloth_category_models = [ + ("hoodie", "agftpm"), + ] + + for cloth in cloth_category_models: + category = cloth[0] + model = cloth[1] + if model != "agftpm": + continue + print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") + # Create the scene config to load -- empty scene + custom cloth object + cfg = { + "scene": { + "type": "Scene", + }, + "objects": [ + { + "type": "DatasetObject", + "name": model, + "category": category, + "model": model, + # "bounding_box": [0.897, 0.568, 0.012], + "prim_type": PrimType.CLOTH, + "abilities": {"cloth": {}}, + # "position": [0, 0, 0.5], + "orientation": [0.7071, 0.0, 0.7071, 0.0], + }, + ], + } + + # Create the environment + env = og.Environment(configs=cfg) + + # Grab object references + carpet = env.scene.object_registry("name", model) + objs = [carpet] + + # Set viewer camera + og.sim.viewer_camera.set_position_orientation( + position=np.array([0.46382895, -2.66703958, 1.22616824]), + orientation=np.array([0.58779174, -0.00231237, -0.00318273, 0.80900271]), + ) + + for _ in range(100): + og.sim.step() + + print("\nCloth state:\n") + + if not short_exec: + # Get particle positions + increments = 100 + obj = objs[0] + + # Fold - first stage + pos = np.asarray(obj.root_link.compute_particle_positions()) + y_min = np.min(pos[:, 1]) + y_max = np.max(pos[:, 1]) + y_mid_bottom = y_min + (y_max - y_min) / 3 + y_mid_top = y_min + 2 * (y_max - y_min) / 3 + bottom_indices = np.where(pos[:, 1] < y_mid_bottom)[0] + bottom_start = np.copy(pos[bottom_indices]) + bottom_end = np.copy(bottom_start) + bottom_end[:, 1] = 2 * y_mid_bottom - bottom_end[:, 1] # Mirror bottom to the above of y_mid_bottom + for ctrl_pts in np.linspace(bottom_start, bottom_end, increments): + obj.root_link.set_particle_positions(torch.tensor(ctrl_pts), idxs=bottom_indices) + og.sim.step() + + # Fold - second stage + pos = np.asarray(obj.root_link.compute_particle_positions()) + y_min = np.min(pos[:, 1]) + y_max = np.max(pos[:, 1]) + y_mid_bottom = y_min + (y_max - y_min) / 3 + y_mid_top = y_min + 2 * (y_max - y_min) / 3 + top_indices = np.where(pos[:, 1] > y_mid_top)[0] + top_start = np.copy(pos[top_indices]) + top_end = np.copy(top_start) + top_end[:, 1] = 2 * y_mid_top - top_end[:, 1] # Mirror top to the below of y_mid_top + for ctrl_pts in np.linspace(top_start, top_end, increments): + obj.root_link.set_particle_positions(torch.tensor(ctrl_pts), idxs=top_indices) + og.sim.step() + + # Fold - third stage + pos = np.asarray(obj.root_link.compute_particle_positions()) + x_min = np.min(pos[:, 0]) + x_max = np.max(pos[:, 0]) + x_mid = (x_min + x_max) / 2 + indices = np.argsort(pos, axis=0)[:, 0][-len(pos) // 2 :] + start = np.copy(pos[indices]) + end = np.copy(start) + end[:, 0] = 2 * x_mid - end[:, 0] + for ctrl_pts in np.linspace(start, end, increments): + obj.root_link.set_particle_positions(torch.tensor(ctrl_pts), idxs=indices) + og.sim.step() + + while True: + print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") + env.step(np.array([])) + + # Shut down env at the end + print() + env.close() + + +if __name__ == "__main__": + main() diff --git a/state_squeeze.py b/state_squeeze.py new file mode 100644 index 000000000..985169f6f --- /dev/null +++ b/state_squeeze.py @@ -0,0 +1,157 @@ +from omnigibson.utils.constants import PrimType +from omnigibson.object_states import Folded, Unfolded +from omnigibson.macros import gm +import numpy as np + +import omnigibson as og +import omnigibson.lazy as lazy +import json +import torch +import time +from omnigibson.prims.xform_prim import XFormPrim +import omnigibson.utils.transform_utils as T + +# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) +gm.ENABLE_OBJECT_STATES = True +gm.USE_GPU_DYNAMICS = True + +# Set up the 4 walls +def generate_box(box_half_extent=torch.tensor([1, 1, 1], dtype=torch.float32), visualize_wall=False): + # Temp function to generate the walls for squeezing the cloth + # The floor plane already exists + # We just need to generate the side planes + plane_centers = ( + torch.tensor( + [ + [1, 0, 1], + [0, 1, 1], + [-1, 0, 1], + [0, -1, 1], + ] + ) + * box_half_extent + ) + plane_prims = [] + plane_motions = [] + for i, pc in enumerate(plane_centers): + + plane = lazy.omni.isaac.core.objects.ground_plane.GroundPlane( + prim_path=f"/World/plane_{i}", + name=f"plane_{i}", + z_position=0, + size=box_half_extent[2].item(), + color=None, + visible=visualize_wall, + ) + + plane_as_prim = XFormPrim( + relative_prim_path=f"/plane_{i}", + name=plane.name, + ) + plane_as_prim.load(None) + + # Build the plane orientation from the plane normal + horiz_dir = pc - torch.tensor([0, 0, box_half_extent[2]]) + plane_z = -1 * horiz_dir / torch.norm(horiz_dir) + plane_x = torch.tensor([0, 0, 1], dtype=torch.float32) + plane_y = torch.cross(plane_z, plane_x) + plane_mat = torch.stack([plane_x, plane_y, plane_z], dim=1) + plane_quat = T.mat2quat(plane_mat) + plane_as_prim.set_position_orientation(pc, plane_quat) + + plane_prims.append(plane_as_prim) + plane_motions.append(plane_z) + return plane_prims, plane_motions + +def main(visualize_wall=False): + """ + Demo of cloth objects that can be wall squeezed + """ + + cloth_category_models = [ + ("bandana", "wbhliu"), + ] + # cloth_category_models = [ + # ("hoodie", "agftpm"), + # ] + + for cloth in cloth_category_models: + category = cloth[0] + model = cloth[1] + print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") + # Create the scene config to load -- empty scene + custom cloth object + cfg = { + "scene": { + "type": "Scene", + }, + "objects": [ + { + "type": "DatasetObject", + "name": model, + "category": category, + "model": model, + "prim_type": PrimType.CLOTH, + "abilities": {"cloth": {}}, + }, + ], + } + + # Create the environment + env = og.Environment(configs=cfg) + + plane_prims, plane_motions = generate_box(visualize_wall=visualize_wall) + + # Grab object references + carpet = env.scene.object_registry("name", model) + objs = [carpet] + + # Set viewer camera + og.sim.viewer_camera.set_position_orientation( + position=np.array([0.46382895, -2.66703958, 1.22616824]), + orientation=np.array([0.58779174, -0.00231237, -0.00318273, 0.80900271]), + ) + + for _ in range(100): + og.sim.step() + + # Calculate end positions for all walls + end_positions = [] + for i in range(4): + plane_prim = plane_prims[i] + position = plane_prim.get_position() + end_positions.append(np.array(position) + np.array(plane_motions[i])) + + increments = 1000 + for step in range(increments): + # Move all walls a small amount + for i in range(4): + plane_prim = plane_prims[i] + current_pos = np.linspace(plane_prim.get_position(), end_positions[i], increments)[step] + plane_prim.set_position_orientation(position=current_pos) + + og.sim.step() + + # Check cloth height + cloth_positions = objs[0].root_link.compute_particle_positions() + # import pdb; pdb.set_trace() + max_height = torch.max(cloth_positions[:, 2]) + + # Get distance between facing walls (assuming walls 0-2 and 1-3 are facing pairs) + wall_dist_1 = torch.linalg.norm(plane_prims[0].get_position() - plane_prims[2].get_position()) + wall_dist_2 = torch.linalg.norm(plane_prims[1].get_position() - plane_prims[3].get_position()) + min_wall_dist = min(wall_dist_1, wall_dist_2) + + # Stop if cloth height exceeds wall distance + if max_height > min_wall_dist: + print(f"Stopping: Cloth height ({max_height:.3f}) exceeds wall distance ({min_wall_dist:.3f})") + break + + + # TODO: save the cloth in the squeezed state + + # Shut down env at the end + env.close() + + +if __name__ == "__main__": + main() From 57d7f52a71498599973c6e98eaafd9057cacd232 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 24 Jan 2025 18:41:55 +0000 Subject: [PATCH 04/38] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- state_squeeze.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/state_squeeze.py b/state_squeeze.py index 985169f6f..a650f632a 100644 --- a/state_squeeze.py +++ b/state_squeeze.py @@ -15,6 +15,7 @@ gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True + # Set up the 4 walls def generate_box(box_half_extent=torch.tensor([1, 1, 1], dtype=torch.float32), visualize_wall=False): # Temp function to generate the walls for squeezing the cloth @@ -34,7 +35,6 @@ def generate_box(box_half_extent=torch.tensor([1, 1, 1], dtype=torch.float32), v plane_prims = [] plane_motions = [] for i, pc in enumerate(plane_centers): - plane = lazy.omni.isaac.core.objects.ground_plane.GroundPlane( prim_path=f"/World/plane_{i}", name=f"plane_{i}", @@ -63,6 +63,7 @@ def generate_box(box_half_extent=torch.tensor([1, 1, 1], dtype=torch.float32), v plane_motions.append(plane_z) return plane_prims, plane_motions + def main(visualize_wall=False): """ Demo of cloth objects that can be wall squeezed @@ -145,7 +146,6 @@ def main(visualize_wall=False): if max_height > min_wall_dist: print(f"Stopping: Cloth height ({max_height:.3f}) exceeds wall distance ({min_wall_dist:.3f})") break - # TODO: save the cloth in the squeezed state From 0a88b683e7c6c1f3adcb144f71e290da5fcb7ac5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 3 Feb 2025 16:35:47 -0800 Subject: [PATCH 05/38] Go back to torch.jit.script --- omnigibson/utils/asset_conversion_utils.py | 4 +- omnigibson/utils/transform_utils.py | 112 ++++++++++----------- 2 files changed, 58 insertions(+), 58 deletions(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 2936f2814..8dff43110 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -1472,8 +1472,8 @@ def _add_metalinks_to_urdf(urdf_path, obj_category, obj_model, dataset_root): attrs_list = [attrs_list[k] for k in keys] for i, attrs in enumerate(attrs_list): - pos = attrs["position"] - quat = attrs["orientation"] + pos = th.as_tensor(attrs["position"]) + quat = th.as_tensor(attrs["orientation"]) # For particle applier only, the orientation of the meta link matters (particle should shoot towards the negative z-axis) # If the meta link is created based on the orientation of the first mesh that is a cone, we need to rotate it by 180 degrees diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index dd167e4fe..39e43be64 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -43,27 +43,27 @@ } -@torch.compile +@torch.jit.script def copysign(a, b): # type: (float, torch.Tensor) -> torch.Tensor a = torch.tensor(a, device=b.device, dtype=torch.float).repeat(b.shape[0]) return torch.abs(a) * torch.sign(b) -@torch.compile +@torch.jit.script def anorm(x: torch.Tensor, dim: Optional[int] = None, keepdim: bool = False) -> torch.Tensor: """Compute L2 norms along specified axes.""" return torch.norm(x, dim=dim, keepdim=keepdim) -@torch.compile +@torch.jit.script def normalize(v: torch.Tensor, dim: Optional[int] = None, eps: float = 1e-10) -> torch.Tensor: """L2 Normalize along specified axes.""" norm = anorm(v, dim=dim, keepdim=True) return v / torch.where(norm < eps, torch.full_like(norm, eps), norm) -@torch.compile +@torch.jit.script def dot(v1, v2, dim=-1, keepdim=False): """ Computes dot product between two vectors along the provided dim, optionally keeping the dimension @@ -81,7 +81,7 @@ def dot(v1, v2, dim=-1, keepdim=False): return torch.sum(v1 * v2, dim=dim, keepdim=keepdim) -@torch.compile +@torch.jit.script def unit_vector(data: torch.Tensor, dim: Optional[int] = None, out: Optional[torch.Tensor] = None) -> torch.Tensor: """ Returns tensor normalized by length, i.e. Euclidean norm, along axis. @@ -116,7 +116,7 @@ def unit_vector(data: torch.Tensor, dim: Optional[int] = None, out: Optional[tor return data -@torch.compile +@torch.jit.script def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """ Apply a quaternion rotation to a vector (equivalent to R.from_quat(x).apply(y)) @@ -161,7 +161,7 @@ def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: return result.squeeze() -@torch.compile +@torch.jit.script def convert_quat(q: torch.Tensor, to: str = "xyzw") -> torch.Tensor: """ Converts quaternion from one convention to another. @@ -183,7 +183,7 @@ def convert_quat(q: torch.Tensor, to: str = "xyzw") -> torch.Tensor: raise ValueError("convert_quat: choose a valid `to` argument (xyzw or wxyz)") -@torch.compile +@torch.jit.script def quat_multiply(quaternion1: torch.Tensor, quaternion0: torch.Tensor) -> torch.Tensor: """ Return multiplication of two quaternions (q1 * q0). @@ -209,7 +209,7 @@ def quat_multiply(quaternion1: torch.Tensor, quaternion0: torch.Tensor) -> torch ) -@torch.compile +@torch.jit.script def quat_conjugate(quaternion: torch.Tensor) -> torch.Tensor: """ Return conjugate of quaternion. @@ -223,7 +223,7 @@ def quat_conjugate(quaternion: torch.Tensor) -> torch.Tensor: return torch.cat([-quaternion[:3], quaternion[3:]]) -@torch.compile +@torch.jit.script def quat_inverse(quaternion: torch.Tensor) -> torch.Tensor: """ Return inverse of quaternion. @@ -243,7 +243,7 @@ def quat_inverse(quaternion: torch.Tensor) -> torch.Tensor: return quat_conjugate(quaternion) / torch.dot(quaternion, quaternion) -@torch.compile +@torch.jit.script def quat_distance(quaternion1, quaternion0): """ Returns distance between two quaternions, such that distance * quaternion0 = quaternion1 @@ -258,7 +258,7 @@ def quat_distance(quaternion1, quaternion0): return quat_multiply(quaternion1, quat_inverse(quaternion0)) -@torch.compile +@torch.jit.script def quat_slerp(quat0, quat1, frac, shortestpath=True, eps=1.0e-15): """ Return spherical linear interpolation between two quaternions. @@ -310,7 +310,7 @@ def quat_slerp(quat0, quat1, frac, shortestpath=True, eps=1.0e-15): return val.reshape(list(quat_shape)) -@torch.compile +@torch.jit.script def random_quaternion(num_quaternions: int = 1) -> torch.Tensor: """ Generate random rotation quaternions, uniformly distributed over SO(3). @@ -335,7 +335,7 @@ def random_quaternion(num_quaternions: int = 1) -> torch.Tensor: return quaternions -@torch.compile +@torch.jit.script def random_axis_angle(angle_limit: float = 2.0 * math.pi): """ Samples an axis-angle rotation by first sampling a random axis @@ -357,7 +357,7 @@ def random_axis_angle(angle_limit: float = 2.0 * math.pi): return random_axis, random_angle.item() -@torch.compile +@torch.jit.script def quat2mat(quaternion): """ Convert quaternions into rotation matrices. @@ -400,7 +400,7 @@ def quat2mat(quaternion): return rotation_matrix -@torch.compile +@torch.jit.script def mat2quat(rmat: torch.Tensor) -> torch.Tensor: """ Converts given rotation matrix to quaternion. @@ -485,7 +485,7 @@ def mat2quat_batch(rmat: torch.Tensor) -> torch.Tensor: return mat2quat(rmat) -@torch.compile +@torch.jit.script def mat2pose(hmat): """ Converts a homogeneous 4x4 matrix into pose. @@ -507,7 +507,7 @@ def mat2pose(hmat): return pos, orn -@torch.compile +@torch.jit.script def vec2quat(vec: torch.Tensor, up: torch.Tensor = torch.tensor([0.0, 0.0, 1.0])) -> torch.Tensor: """ Converts given 3d-direction vector @vec to quaternion orientation with respect to another direction vector @up @@ -536,7 +536,7 @@ def vec2quat(vec: torch.Tensor, up: torch.Tensor = torch.tensor([0.0, 0.0, 1.0]) return mat2quat(rotation_matrix) -@torch.compile +@torch.jit.script def euler2quat(euler: torch.Tensor) -> torch.Tensor: """ Converts euler angles into quaternion form @@ -573,7 +573,7 @@ def euler2quat(euler: torch.Tensor) -> torch.Tensor: return torch.stack([qx, qy, qz, qw], dim=-1) -@torch.compile +@torch.jit.script def quat2euler(q): single_dim = q.dim() == 1 @@ -602,7 +602,7 @@ def quat2euler(q): return euler -@torch.compile +@torch.jit.script def euler2mat(euler): """ Converts euler angles into rotation matrix form @@ -626,7 +626,7 @@ def euler2mat(euler): return quat2mat(quat) -@torch.compile +@torch.jit.script def mat2euler(rmat): """ Converts given rotation matrix to euler angles in radian. @@ -652,7 +652,7 @@ def mat2euler(rmat): return torch.stack([roll, pitch, yaw], dim=-1) -@torch.compile +@torch.jit.script def pose2mat(pose: Tuple[torch.Tensor, torch.Tensor]) -> torch.Tensor: pos, orn = pose @@ -671,7 +671,7 @@ def pose2mat(pose: Tuple[torch.Tensor, torch.Tensor]) -> torch.Tensor: return homo_pose_mat -@torch.compile +@torch.jit.script def quat2axisangle(quat): """ Converts quaternion to axis-angle format. @@ -705,7 +705,7 @@ def quat2axisangle(quat): return ret -@torch.compile +@torch.jit.script def axisangle2quat(vec, eps=1e-6): """ Converts scaled axis-angle to quat. @@ -744,7 +744,7 @@ def axisangle2quat(vec, eps=1e-6): return quat -@torch.compile +@torch.jit.script def pose_in_A_to_pose_in_B(pose_A, pose_A_in_B): """ Converts a homogenous matrix corresponding to a point C in frame A @@ -766,7 +766,7 @@ def pose_in_A_to_pose_in_B(pose_A, pose_A_in_B): return torch.matmul(pose_A_in_B, pose_A) -@torch.compile +@torch.jit.script def pose_inv(pose_mat): """ Computes the inverse of a homogeneous matrix corresponding to the pose of some @@ -799,7 +799,7 @@ def pose_inv(pose_mat): return pose_inv -@torch.compile +@torch.jit.script def pose_transform(pos1, quat1, pos0, quat0): """ Conducts forward transform from pose (pos0, quat0) to pose (pos1, quat1): @@ -825,7 +825,7 @@ def pose_transform(pos1, quat1, pos0, quat0): return mat2pose(mat1 @ mat0) -@torch.compile +@torch.jit.script def invert_pose_transform(pos, quat): """ Inverts a pose transform @@ -846,7 +846,7 @@ def invert_pose_transform(pos, quat): return mat2pose(pose_inv(mat)) -@torch.compile +@torch.jit.script def relative_pose_transform(pos1, quat1, pos0, quat0): """ Computes relative forward transform from pose (pos0, quat0) to pose (pos1, quat1), i.e.: solves: @@ -872,7 +872,7 @@ def relative_pose_transform(pos1, quat1, pos0, quat0): return mat2pose(pose_inv(mat0) @ mat1) -@torch.compile +@torch.jit.script def _skew_symmetric_translation(pos_A_in_B): """ Helper function to get a skew symmetric translation matrix for converting quantities @@ -895,7 +895,7 @@ def _skew_symmetric_translation(pos_A_in_B): ) -@torch.compile +@torch.jit.script def vel_in_A_to_vel_in_B(vel_A, ang_vel_A, pose_A_in_B): """ Converts linear and angular velocity of a point in frame A to the equivalent in frame B. @@ -919,7 +919,7 @@ def vel_in_A_to_vel_in_B(vel_A, ang_vel_A, pose_A_in_B): return vel_B, ang_vel_B -@torch.compile +@torch.jit.script def force_in_A_to_force_in_B(force_A, torque_A, pose_A_in_B): """ Converts linear and rotational force at a point in frame A to the equivalent in frame B. @@ -943,7 +943,7 @@ def force_in_A_to_force_in_B(force_A, torque_A, pose_A_in_B): return force_B, torque_B -@torch.compile +@torch.jit.script def rotation_matrix(angle: float, direction: torch.Tensor) -> torch.Tensor: """ Returns a 3x3 rotation matrix to rotate about the given axis. @@ -980,7 +980,7 @@ def rotation_matrix(angle: float, direction: torch.Tensor) -> torch.Tensor: return R -@torch.compile +@torch.jit.script def transformation_matrix(angle: float, direction: torch.Tensor, point: Optional[torch.Tensor] = None) -> torch.Tensor: """ Returns a 4x4 homogeneous transformation matrix to rotate about axis defined by point and direction. @@ -1003,7 +1003,7 @@ def transformation_matrix(angle: float, direction: torch.Tensor, point: Optional return M -@torch.compile +@torch.jit.script def clip_translation(dpos, limit): """ Limits a translation (delta position) to a specified limit @@ -1024,7 +1024,7 @@ def clip_translation(dpos, limit): return (dpos * limit / input_norm, True) if input_norm > limit else (dpos, False) -@torch.compile +@torch.jit.script def clip_rotation(quat, limit): """ Limits a (delta) rotation to a specified limit @@ -1068,7 +1068,7 @@ def clip_rotation(quat, limit): return quat, clipped -@torch.compile +@torch.jit.script def make_pose(translation, rotation): """ Makes a homogeneous pose matrix from a translation vector and a rotation matrix. @@ -1087,7 +1087,7 @@ def make_pose(translation, rotation): return pose -@torch.compile +@torch.jit.script def get_orientation_error(desired, current): """ This function calculates a 3-dimensional orientation error vector, where inputs are quaternions @@ -1108,7 +1108,7 @@ def get_orientation_error(desired, current): return (q_r[:, 0:3] * torch.sign(q_r[:, 3]).unsqueeze(-1)).reshape(list(input_shape) + [3]) -@torch.compile +@torch.jit.script def get_orientation_diff_in_radian(orn0: torch.Tensor, orn1: torch.Tensor) -> torch.Tensor: """ Returns the difference between two quaternion orientations in radians. @@ -1130,7 +1130,7 @@ def get_orientation_diff_in_radian(orn0: torch.Tensor, orn1: torch.Tensor) -> to return torch.norm(axis_angle) -@torch.compile +@torch.jit.script def get_pose_error(target_pose, current_pose): """ Computes the error corresponding to target pose - current pose as a 6-dim vector. @@ -1165,7 +1165,7 @@ def get_pose_error(target_pose, current_pose): return error -@torch.compile +@torch.jit.script def matrix_inverse(matrix): """ Helper function to have an efficient matrix inversion function. @@ -1179,7 +1179,7 @@ def matrix_inverse(matrix): return torch.linalg.inv_ex(matrix).inverse -@torch.compile +@torch.jit.script def vecs2axisangle(vec0, vec1): """ Converts the angle from unnormalized 3D vectors @vec0 to @vec1 into an axis-angle representation of the angle @@ -1196,7 +1196,7 @@ def vecs2axisangle(vec0, vec1): return torch.linalg.cross(vec0, vec1) * torch.arccos((vec0 * vec1).sum(-1, keepdim=True)) -@torch.compile +@torch.jit.script def vecs2quat(vec0: torch.Tensor, vec1: torch.Tensor, normalized: bool = False) -> torch.Tensor: """ Converts the angle from unnormalized 3D vectors @vec0 to @vec1 into a quaternion representation of the angle @@ -1232,7 +1232,7 @@ def vecs2quat(vec0: torch.Tensor, vec1: torch.Tensor, normalized: bool = False) # Ref: https://github.com/scipy/scipy/blob/9974222eb58ec3eafe5d12f25ee960f3170c277a/scipy/spatial/transform/_rotation.pyx#L3249 -@torch.compile +@torch.jit.script def align_vector_sets(vec_set1: torch.Tensor, vec_set2: torch.Tensor) -> torch.Tensor: """ Computes a single quaternion representing the rotation that best aligns vec_set1 to vec_set2. @@ -1261,13 +1261,13 @@ def align_vector_sets(vec_set1: torch.Tensor, vec_set2: torch.Tensor) -> torch.T return mat2quat(C) -@torch.compile +@torch.jit.script def l2_distance(v1: torch.Tensor, v2: torch.Tensor) -> torch.Tensor: """Returns the L2 distance between vector v1 and v2.""" return torch.norm(v1 - v2) -@torch.compile +@torch.jit.script def cartesian_to_polar(x, y): """Convert cartesian coordinate to polar coordinate""" rho = torch.sqrt(x**2 + y**2) @@ -1283,7 +1283,7 @@ def rad2deg(rad): return rad * 180.0 / math.pi -@torch.compile +@torch.jit.script def check_quat_right_angle(quat: torch.Tensor, atol: float = 5e-2) -> torch.Tensor: """ Check by making sure the quaternion is some permutation of +/- (1, 0, 0, 0), @@ -1303,14 +1303,14 @@ def check_quat_right_angle(quat: torch.Tensor, atol: float = 5e-2) -> torch.Tens return torch.any(torch.abs(l1_norm.unsqueeze(-1) - reference_norms) < atol, dim=-1) -@torch.compile +@torch.jit.script def z_angle_from_quat(quat): """Get the angle around the Z axis produced by the quaternion.""" rotated_X_axis = quat_apply(quat, torch.tensor([1, 0, 0], dtype=torch.float32)) return torch.arctan2(rotated_X_axis[1], rotated_X_axis[0]) -@torch.compile +@torch.jit.script def integer_spiral_coordinates(n: int) -> Tuple[int, int]: """A function to map integers to 2D coordinates in a spiral pattern around the origin.""" # Map integers from Z to Z^2 in a spiral pattern around the origin. @@ -1323,7 +1323,7 @@ def integer_spiral_coordinates(n: int) -> Tuple[int, int]: return int(x), int(y) -@torch.compile +@torch.jit.script def transform_points(points: torch.Tensor, matrix: torch.Tensor, translate: bool = True) -> torch.Tensor: """ Returns points rotated by a homogeneous @@ -1359,7 +1359,7 @@ def transform_points(points: torch.Tensor, matrix: torch.Tensor, translate: bool return torch.mm(matrix[:dim, :dim], points.t()).t() -@torch.compile +@torch.jit.script def quaternions_close(q1: torch.Tensor, q2: torch.Tensor, atol: float = 1e-3) -> bool: """ Whether two quaternions represent the same rotation, @@ -1380,7 +1380,7 @@ def quaternions_close(q1: torch.Tensor, q2: torch.Tensor, atol: float = 1e-3) -> return torch.allclose(q1, q2, atol=atol) or torch.allclose(q1, -q2, atol=atol) -@torch.compile +@torch.jit.script def orientation_error(desired, current): """ This function calculates a 3-dimensional orientation error vector for use in the @@ -1410,7 +1410,7 @@ def orientation_error(desired, current): return error.reshape(desired.shape[:-2] + (3,)) -@torch.compile +@torch.jit.script def delta_rotation_matrix(omega, delta_t): """ Compute the delta rotation matrix given angular velocity and time elapsed. @@ -1445,7 +1445,7 @@ def delta_rotation_matrix(omega, delta_t): return R -@torch.compile +@torch.jit.script def mat2euler_intrinsic(rmat): """ Converts given rotation matrix to intrinsic euler angles in radian. @@ -1471,7 +1471,7 @@ def mat2euler_intrinsic(rmat): return torch.tensor([roll, pitch, yaw], dtype=torch.float32) -@torch.compile +@torch.jit.script def euler_intrinsic2mat(euler): """ Converts intrinsic euler angles into rotation matrix form From 15036558c3fdcb3de9923d02b2e3e012f71d10ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 3 Feb 2025 17:09:34 -0800 Subject: [PATCH 06/38] Small fixes --- omnigibson/utils/transform_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index 39e43be64..d43f9660d 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -1436,7 +1436,7 @@ def delta_rotation_matrix(omega, delta_t): axis = omega / omega_magnitude # Skew-symmetric matrix K - u_x, u_y, u_z = axis + u_x, u_y, u_z = axis[0], axis[1], axis[2] K = torch.tensor([[0, -u_z, u_y], [u_z, 0, -u_x], [-u_y, u_x, 0]]) # Rodrigues' rotation formula From 0bad3ee8cc1ec715b00edda8d9746656d145de61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 3 Feb 2025 17:28:46 -0800 Subject: [PATCH 07/38] More fixes --- omnigibson/utils/transform_utils.py | 30 ++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index d43f9660d..535c7b160 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -383,21 +383,21 @@ def quat2mat(quaternion): yw = outer[..., 1, 3] zw = outer[..., 2, 3] - rotation_matrix = torch.empty(quaternion.shape[:-1] + (3, 3), dtype=quaternion.dtype, device=quaternion.device) + rmat = torch.empty(quaternion.shape[:-1] + (3, 3), dtype=quaternion.dtype, device=quaternion.device) - rotation_matrix[..., 0, 0] = 1 - 2 * (yy + zz) - rotation_matrix[..., 0, 1] = 2 * (xy - zw) - rotation_matrix[..., 0, 2] = 2 * (xz + yw) + rmat[..., 0, 0] = 1 - 2 * (yy + zz) + rmat[..., 0, 1] = 2 * (xy - zw) + rmat[..., 0, 2] = 2 * (xz + yw) - rotation_matrix[..., 1, 0] = 2 * (xy + zw) - rotation_matrix[..., 1, 1] = 1 - 2 * (xx + zz) - rotation_matrix[..., 1, 2] = 2 * (yz - xw) + rmat[..., 1, 0] = 2 * (xy + zw) + rmat[..., 1, 1] = 1 - 2 * (xx + zz) + rmat[..., 1, 2] = 2 * (yz - xw) - rotation_matrix[..., 2, 0] = 2 * (xz - yw) - rotation_matrix[..., 2, 1] = 2 * (yz + xw) - rotation_matrix[..., 2, 2] = 1 - 2 * (xx + yy) + rmat[..., 2, 0] = 2 * (xz - yw) + rmat[..., 2, 1] = 2 * (yz + xw) + rmat[..., 2, 2] = 1 - 2 * (xx + yy) - return rotation_matrix + return rmat @torch.jit.script @@ -531,9 +531,9 @@ def vec2quat(vec: torch.Tensor, up: torch.Tensor = torch.tensor([0.0, 0.0, 1.0]) s_n = torch.cross(up_n, vec_n, dim=-1) u_n = torch.cross(vec_n, s_n, dim=-1) - rotation_matrix = torch.stack([vec_n, s_n, u_n], dim=-1) + rmat = torch.stack([vec_n, s_n, u_n], dim=-1) - return mat2quat(rotation_matrix) + return mat2quat(rmat) @torch.jit.script @@ -1464,8 +1464,8 @@ def mat2euler_intrinsic(rmat): yaw = torch.arctan2(-rmat[0, 1], rmat[0, 0]) else: # Gimbal lock case - pitch = math.pi / 2 if rotation_matrix[0, 2] == 1 else -math.pi / 2 - roll = torch.arctan2(rotation_matrix[1, 0], rotation_matrix[1, 1]) + pitch = math.pi / 2 if rmat[0, 2] == 1 else -math.pi / 2 + roll = torch.arctan2(rmat[1, 0], rmat[1, 1]) yaw = 0 # Can set yaw to 0 in gimbal lock return torch.tensor([roll, pitch, yaw], dtype=torch.float32) From 3f92e6bea87ed07101c1c363b415c220510f95a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 3 Feb 2025 17:31:11 -0800 Subject: [PATCH 08/38] More fixes --- omnigibson/utils/transform_utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index 535c7b160..48e2ceccc 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -1464,9 +1464,9 @@ def mat2euler_intrinsic(rmat): yaw = torch.arctan2(-rmat[0, 1], rmat[0, 0]) else: # Gimbal lock case - pitch = math.pi / 2 if rmat[0, 2] == 1 else -math.pi / 2 + pitch = torch.pi / 2 if rmat[0, 2] == 1 else -torch.pi / 2 roll = torch.arctan2(rmat[1, 0], rmat[1, 1]) - yaw = 0 # Can set yaw to 0 in gimbal lock + yaw = torch.tensor(0) # Can set yaw to 0 in gimbal lock return torch.tensor([roll, pitch, yaw], dtype=torch.float32) From 4799e827ee40e5aa5e066d01291a35f6c3cff9fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 3 Feb 2025 17:34:09 -0800 Subject: [PATCH 09/38] More fixes --- omnigibson/utils/transform_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index 48e2ceccc..1a68b781e 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -1464,7 +1464,7 @@ def mat2euler_intrinsic(rmat): yaw = torch.arctan2(-rmat[0, 1], rmat[0, 0]) else: # Gimbal lock case - pitch = torch.pi / 2 if rmat[0, 2] == 1 else -torch.pi / 2 + pitch = torch.tensor(math.pi / 2 if rmat[0, 2] == 1 else -math.pi / 2) roll = torch.arctan2(rmat[1, 0], rmat[1, 1]) yaw = torch.tensor(0) # Can set yaw to 0 in gimbal lock From 4cc028e053123cc2702a46b426c3144fdb4875a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 3 Feb 2025 17:48:14 -0800 Subject: [PATCH 10/38] More fixes --- omnigibson/utils/transform_utils.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index 1a68b781e..7dcc56aac 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -553,7 +553,8 @@ def euler2quat(euler: torch.Tensor) -> torch.Tensor: assert euler.shape[-1] == 3, "Invalid input shape" # Unpack roll, pitch, yaw - roll, pitch, yaw = euler.unbind(-1) + unbound_euler = euler.unbind(-1) + roll, pitch, yaw = unbound_euler[0], unbound_euler[1], unbound_euler[2] # Compute sines and cosines of half angles cy = torch.cos(yaw * 0.5) @@ -1482,7 +1483,7 @@ def euler_intrinsic2mat(euler): Returns: torch.tensor: 3x3 rotation matrix """ - roll, pitch, yaw = euler + roll, pitch, yaw = euler[0], euler[1], euler[2] # Rotation matrix around X-axis Rx = torch.tensor([[1, 0, 0], [0, torch.cos(roll), -torch.sin(roll)], [0, torch.sin(roll), torch.cos(roll)]]) From 79e8818891756f40dc6e682b60d51d2b7e349d56 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 4 Feb 2025 12:31:34 -0800 Subject: [PATCH 11/38] Updates to asset conversion --- omnigibson/utils/asset_conversion_utils.py | 91 +++++++++++----------- 1 file changed, 47 insertions(+), 44 deletions(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 8dff43110..3f957137e 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -437,7 +437,7 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat # default_mat_is_used = False # Grab all visual objs for this object - urdf_path = f"{dataset_root}/objects/{obj_category}/{obj_model}/{obj_model}_with_metalinks.urdf" + urdf_path = f"{dataset_root}/objects/{obj_category}/{obj_model}/urdf/{obj_model}_with_meta_links.urdf" visual_objs = _get_visual_objs_from_urdf(urdf_path) # Extract absolute paths to mtl files for each link @@ -630,13 +630,14 @@ def _add_xform_properties(prim): xformable.SetXformOpOrder([xform_op_translate, xform_op_rot, xform_op_scale]) -def _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos): +def _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_link_type, meta_link_infos): """ Process a meta link by creating visual meshes or lights below it. Args: stage (pxr.Usd.Stage): The USD stage where the meta link will be processed. obj_model (str): The object model name. + link_name (str): Name of the meta link's parent link (e.g. what part of the object the meta link is attached to). meta_link_type (str): The type of the meta link. Must be one of the allowed meta types. meta_link_infos (dict): A dictionary containing meta link information. The keys are link IDs and the values are lists of mesh information dictionaries. @@ -648,18 +649,12 @@ def _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos): ValueError: If an invalid light type or mesh type is encountered. Notes: - - Temporarily disables importing of fillable meshes for "container" meta link type. - Handles specific meta link types such as "togglebutton", "particleapplier", "particleremover", "particlesink", and "particlesource". - For "particleapplier" meta link type, adjusts the orientation if the mesh type is "cone". - Creates lights or primitive shapes based on the meta link type and mesh information. - Sets various attributes for lights and meshes, including color, intensity, size, and scale. - Makes meshes invisible and sets their local pose. """ - # TODO: Reenable after fillable meshes are backported into 3ds Max. - # Temporarily disable importing of fillable meshes. - if meta_link_type in ["container"]: - return - assert meta_link_type in _ALLOWED_META_TYPES if _ALLOWED_META_TYPES[meta_link_type] not in ["primitive", "light"] and meta_link_type != "particlesource": return @@ -686,13 +681,13 @@ def _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos): assert len(mesh_info_list) == 1, f"Invalid number of meshes for {meta_link_type}" meta_link_in_parent_link_pos, meta_link_in_parent_link_orn = ( - mesh_info_list[0]["position"], - mesh_info_list[0]["orientation"], + th.tensor(mesh_info_list[0]["position"]), + th.tensor(mesh_info_list[0]["orientation"]), ) # For particle applier only, the orientation of the meta link matters (particle should shoot towards the negative z-axis) # If the meta link is created based on the orientation of the first mesh that is a cone, we need to rotate it by 180 degrees - # because the cone is pointing in the wrong direction. This is already done in update_obj_urdf_with_metalinks; + # because the cone is pointing in the wrong direction. This is already done in update_obj_urdf_with_meta_links; # we just need to make sure meta_link_in_parent_link_orn is updated correctly. if meta_link_type == "particleapplier" and mesh_info_list[0]["type"] == "cone": meta_link_in_parent_link_orn = T.quat_multiply( @@ -704,7 +699,7 @@ def _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos): if is_light: # Create a light light_type = _LIGHT_MAPPING[mesh_info["type"]] - prim_path = f"/{obj_model}/lights_{link_id}_0_link/light_{i}" + prim_path = f"/{obj_model}/__meta_{link_name}_lights_{link_id}_0_link/light_{i}" prim = getattr(lazy.pxr.UsdLux, f"{light_type}Light").Define(stage, prim_path).GetPrim() lazy.pxr.UsdLux.ShapingAPI.Apply(prim).GetShapingConeAngleAttr().Set(180.0) else: @@ -713,7 +708,7 @@ def _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos): else: # Create a primitive shape mesh_type = mesh_info["type"].capitalize() if mesh_info["type"] != "box" else "Cube" - prim_path = f"/{obj_model}/{meta_link_type}_{link_id}_0_link/mesh_{i}" + prim_path = f"/{obj_model}/__meta_{link_name}_{meta_link_type}_{link_id}_0_link/mesh_{i}" assert hasattr(lazy.pxr.UsdGeom, mesh_type) # togglebutton has to be a sphere if meta_link_type in ["togglebutton"]: @@ -817,9 +812,6 @@ def _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos): else: raise ValueError(f"Invalid mesh type: {mesh_type}") - # Make invisible - lazy.pxr.UsdGeom.Imageable(xform_prim.prim).MakeInvisible() - xform_prim.set_local_pose( translation=mesh_in_meta_link_pos, orientation=mesh_in_meta_link_orn[[3, 0, 1, 2]], @@ -942,10 +934,32 @@ def import_obj_metadata(usd_path, obj_category, obj_model, dataset_root, import_ log.debug("Process meta links") - # TODO: Use parent link name + # Convert primitive meta links for link_name, link_metadata in meta_links.items(): for meta_link_type, meta_link_infos in link_metadata.items(): - _process_meta_link(stage, obj_model, meta_link_type, meta_link_infos) + _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_link_type, meta_link_infos) + + # Get all meta links, set them to guide purpose, and add some metadata + # Here we want to include every link that has the __meta prefix. + meta_link_prims = [p for p in prim.GetChildren() if p.GetName().startswith("__meta_")] + for meta_prim in meta_link_prims: + # Get meta link information + unparsed_meta = meta_prim.GetName()[7:-5] # remove __meta_ and _link + link_name, meta_link_type, link_id, link_sub_id = unparsed_meta.rsplit("_", 3) + + # Add the is_meta_link, meta_link_type, and meta_link_id attributes + meta_prim.CreateAttribute("ig:isMetaLink", lazy.pxr.Sdf.ValueTypeNames.Bool) + meta_prim.GetAttribute("ig:isMetaLink").Set(True) + meta_prim.CreateAttribute("ig:metaLinkType", lazy.pxr.Sdf.ValueTypeNames.String) + meta_prim.GetAttribute("ig:metaLinkType").Set(meta_link_type) + meta_prim.CreateAttribute("ig:metaLinkId", lazy.pxr.Sdf.ValueTypeNames.String) + meta_prim.GetAttribute("ig:metaLinkId").Set(link_id) + meta_prim.CreateAttribute("ig:metaLinkSubId", lazy.pxr.Sdf.ValueTypeNames.Int) + meta_prim.GetAttribute("ig:metaLinkId").Set(int(link_sub_id)) + + # Set the purpose of the meta link + purpose_attr = lazy.pxr.UsdGeom.Imageable(meta_prim).CreatePurposeAttr() + purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) log.debug("Done processing meta links") @@ -1152,8 +1166,8 @@ def import_obj_urdf( - str: Absolute path to post-processed URDF file used to generate USD - str: Absolute path to the imported USD file """ - # Preprocess input URDF to account for metalinks - urdf_path = _add_metalinks_to_urdf( + # Preprocess input URDF to account for meta links + urdf_path = _add_meta_links_to_urdf( urdf_path=urdf_path, obj_category=obj_category, obj_model=obj_model, dataset_root=dataset_root ) # Import URDF @@ -1323,9 +1337,9 @@ def _create_urdf_link(name, subelements=None, mass=None, inertia=None): return link -def _create_urdf_metalink( +def _create_urdf_meta_link( root_element, - metalink_name, + meta_link_name, parent_link_name="base_link", pos=(0, 0, 0), rpy=(0, 0, 0), @@ -1334,8 +1348,8 @@ def _create_urdf_metalink( Creates the appropriate URDF joint and link for a meta link and appends it to the root element. Args: - root_element (Element): The root XML element to which the metalink will be appended. - metalink_name (str): The name of the metalink to be created. + root_element (Element): The root XML element to which the meta link will be appended. + meta_link_name (str): The name of the meta link to be created. parent_link_name (str, optional): The name of the parent link. Defaults to "base_link". pos (tuple, optional): The position of the joint in the form (x, y, z). Defaults to (0, 0, 0). rpy (tuple, optional): The roll, pitch, and yaw of the joint in the form (r, p, y). Defaults to (0, 0, 0). @@ -1345,16 +1359,16 @@ def _create_urdf_metalink( """ # Create joint jnt = _create_urdf_joint( - name=f"{metalink_name}_joint", + name=f"{meta_link_name}_joint", parent=parent_link_name, - child=f"{metalink_name}_link", + child=f"{meta_link_name}_link", pos=pos, rpy=rpy, joint_type="fixed", ) # Create child link link = _create_urdf_link( - name=f"{metalink_name}_link", + name=f"{meta_link_name}_link", mass=0.0001, inertia=[0.00001, 0.00001, 0.00001, 0, 0, 0], ) @@ -1393,7 +1407,7 @@ def _save_xmltree_as_urdf(root_element, name, dirpath, unique_urdf=False): return fpath -def _add_metalinks_to_urdf(urdf_path, obj_category, obj_model, dataset_root): +def _add_meta_links_to_urdf(urdf_path, obj_category, obj_model, dataset_root): """ Adds meta links to a URDF file based on metadata. @@ -1446,11 +1460,6 @@ def _add_metalinks_to_urdf(urdf_path, obj_category, obj_model, dataset_root): meta_link_name in _ALLOWED_META_TYPES ), f"meta_link_name {meta_link_name} not in {_ALLOWED_META_TYPES}" - # TODO: Reenable after fillable meshes are backported into 3ds Max. - # Temporarily disable importing of fillable meshes. - if meta_link_name in ["container"]: - continue - for ml_id, attrs_list in ml_attrs.items(): if len(attrs_list) > 0: if _ALLOWED_META_TYPES[meta_link_name] != "dimensionless": @@ -1465,12 +1474,6 @@ def _add_metalinks_to_urdf(urdf_path, obj_category, obj_model, dataset_root): len(attrs_list) == 1 ), f"Expected only one instance for meta_link {meta_link_name}_{ml_id}, but found {len(attrs_list)}" - # TODO: Remove this after this is fixed. - if type(attrs_list) is dict: - keys = [str(x) for x in range(len(attrs_list))] - assert set(attrs_list.keys()) == set(keys), "Unexpected keys" - attrs_list = [attrs_list[k] for k in keys] - for i, attrs in enumerate(attrs_list): pos = th.as_tensor(attrs["position"]) quat = th.as_tensor(attrs["orientation"]) @@ -1484,10 +1487,10 @@ def _add_metalinks_to_urdf(urdf_path, obj_category, obj_model, dataset_root): ), f"Expected only one instance for meta_link {meta_link_name}_{ml_id}, but found {len(attrs_list)}" quat = T.quat_multiply(quat, T.axisangle2quat(th.tensor([math.pi, 0.0, 0.0]))) - # Create metalink - _create_urdf_metalink( + # Create meta link + _create_urdf_meta_link( root_element=root, - metalink_name=f"{meta_link_name}_{ml_id}_{i}", + meta_link_name=f"{meta_link_name}_{ml_id}_{i}", parent_link_name=parent_link_name, pos=pos, rpy=T.quat2euler(quat), @@ -1496,7 +1499,7 @@ def _add_metalinks_to_urdf(urdf_path, obj_category, obj_model, dataset_root): # Export this URDF return _save_xmltree_as_urdf( root_element=root, - name=f"{obj_model}_with_metalinks", + name=f"{obj_model}_with_meta_links", dirpath=f"{model_root_path}/urdf", unique_urdf=False, ) @@ -2382,7 +2385,7 @@ def import_og_asset_from_urdf( merge_fixed_joints=merge_fixed_joints, ) - # Copy metalinks URDF to original name of object model + # Copy meta links URDF to original name of object model shutil.copy2(urdf_path, os.path.join(dataset_root, "objects", category, model, "urdf", f"{model}.urdf")) prim = import_obj_metadata( From 71f5789020b59fbc965c22bfcfebbdb360d1643a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 4 Feb 2025 14:11:58 -0800 Subject: [PATCH 12/38] Support opacity properly --- omnigibson/utils/asset_conversion_utils.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 3f957137e..943bb558c 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -282,6 +282,12 @@ def _set_mtl_opacity(mtl_prim, texture): lazy.omni.usd.create_material_input(mtl_prim, mtl, texture, lazy.pxr.Sdf.ValueTypeNames.Asset) lazy.omni.usd.create_material_input(mtl_prim, "enable_opacity", True, lazy.pxr.Sdf.ValueTypeNames.Bool) lazy.omni.usd.create_material_input(mtl_prim, "enable_opacity_texture", True, lazy.pxr.Sdf.ValueTypeNames.Bool) + + # Set the opacity to use the alpha channel for its mono-channel value. + # This defaults to some other value, which takes opaque black channels in the + # image to be fully transparent. This is not what we want. + lazy.omni.usd.create_material_input(mtl_prim, "opacity_mode", 0, lazy.pxr.Sdf.ValueTypeNames.Int) + # Verify it was set shade = lazy.omni.usd.get_shader_from_material(mtl_prim) log.debug(f"mtl {mtl}: {shade.GetInput(mtl).Get()}") @@ -699,7 +705,7 @@ def _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_ if is_light: # Create a light light_type = _LIGHT_MAPPING[mesh_info["type"]] - prim_path = f"/{obj_model}/__meta_{link_name}_lights_{link_id}_0_link/light_{i}" + prim_path = f"/{obj_model}/meta__{link_name}_lights_{link_id}_0_link/light_{i}" prim = getattr(lazy.pxr.UsdLux, f"{light_type}Light").Define(stage, prim_path).GetPrim() lazy.pxr.UsdLux.ShapingAPI.Apply(prim).GetShapingConeAngleAttr().Set(180.0) else: @@ -708,7 +714,7 @@ def _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_ else: # Create a primitive shape mesh_type = mesh_info["type"].capitalize() if mesh_info["type"] != "box" else "Cube" - prim_path = f"/{obj_model}/__meta_{link_name}_{meta_link_type}_{link_id}_0_link/mesh_{i}" + prim_path = f"/{obj_model}/meta__{link_name}_{meta_link_type}_{link_id}_0_link/mesh_{i}" assert hasattr(lazy.pxr.UsdGeom, mesh_type) # togglebutton has to be a sphere if meta_link_type in ["togglebutton"]: @@ -940,11 +946,13 @@ def import_obj_metadata(usd_path, obj_category, obj_model, dataset_root, import_ _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_link_type, meta_link_infos) # Get all meta links, set them to guide purpose, and add some metadata - # Here we want to include every link that has the __meta prefix. - meta_link_prims = [p for p in prim.GetChildren() if p.GetName().startswith("__meta_")] + # Here we want to include every link that has the meta__ prefix. + # This includes meta links that get added into the URDF in earlier + # stages. + meta_link_prims = [p for p in prim.GetChildren() if p.GetName().startswith("meta__")] for meta_prim in meta_link_prims: # Get meta link information - unparsed_meta = meta_prim.GetName()[7:-5] # remove __meta_ and _link + unparsed_meta = meta_prim.GetName()[6:-5] # remove meta__ and _link link_name, meta_link_type, link_id, link_sub_id = unparsed_meta.rsplit("_", 3) # Add the is_meta_link, meta_link_type, and meta_link_id attributes From 325b6887aec798454d6eb143263eea6c23f4c32a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 4 Feb 2025 21:34:17 -0800 Subject: [PATCH 13/38] Improve material handling in asset conversion --- omnigibson/utils/asset_conversion_utils.py | 119 +++++---------------- 1 file changed, 26 insertions(+), 93 deletions(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 943bb558c..18c5d5b5d 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -350,31 +350,6 @@ def _get_visual_objs_from_urdf(urdf_path): return visual_objs -def _copy_object_state_textures(obj_category, obj_model, dataset_root): - """ - Copies specific object state texture files from the old material directory to the new material directory. - - Args: - obj_category (str): The category of the object. - obj_model (str): The model of the object. - dataset_root (str): The root directory of the dataset. - - Returns: - None - """ - obj_root_dir = f"{dataset_root}/objects/{obj_category}/{obj_model}" - old_mat_fpath = f"{obj_root_dir}/material" - new_mat_fpath = f"{obj_root_dir}/usd/materials" - for mat_file in os.listdir(old_mat_fpath): - should_copy = False - for object_state in _OBJECT_STATE_TEXTURES: - if object_state in mat_file.lower(): - should_copy = True - break - if should_copy: - shutil.copy(f"{old_mat_fpath}/{mat_file}", new_mat_fpath) - - def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_path, usd_path, dataset_root): """ Imports and binds rendering channels for a given object in an Omniverse USD stage. @@ -400,15 +375,8 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat AssertionError: If a valid visual prim is not found for a mesh. """ usd_dir = os.path.dirname(usd_path) - # # mat_dir = f"{model_root_path}/material/{obj_category}" if \ - # # obj_category in {"ceilings", "walls", "floors"} else f"{model_root_path}/material" - # mat_dir = f"{model_root_path}/material" - # # Compile all material files we have - # mat_files = set(os.listdir(mat_dir)) # Remove the material prims as we will create them explictly later. - # TODO: Be a bit more smart about this. a material procedurally generated will lose its material without it having - # be regenerated stage = lazy.omni.usd.get_context().get_stage() for prim in obj_prim.GetChildren(): looks_prim = None @@ -427,20 +395,10 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat stage.RemovePrim(subprim.GetPath()), ) - # # Create new default material for this object. - # mtl_created_list = [] - # lazy.omni.kit.commands.execute( - # "CreateAndBindMdlMaterialFromLibrary", - # mdl_name="OmniPBR.mdl", - # mtl_name="OmniPBR", - # mtl_created_list=mtl_created_list, - # ) - # default_mat = lazy.omni.isaac.core.utils.prims.get_prim_at_path(mtl_created_list[0]) - # default_mat = rename_prim(prim=default_mat, name=f"default_material") - # log.debug("Created default material:", default_mat.GetPath()) - # - # # We may delete this default material if it's never used - # default_mat_is_used = False + # Remove the materials copied over by the URDF importer + urdf_importer_mtl_dir = os.path.join(usd_dir, "materials") + if os.path.exists(urdf_importer_mtl_dir): + shutil.rmtree(urdf_importer_mtl_dir) # Grab all visual objs for this object urdf_path = f"{dataset_root}/objects/{obj_category}/{obj_model}/urdf/{obj_model}_with_meta_links.urdf" @@ -449,15 +407,12 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat # Extract absolute paths to mtl files for each link link_mtl_files = OrderedDict() # maps link name to dictionary mapping mesh name to mtl file mtl_infos = OrderedDict() # maps mtl name to dictionary mapping material channel name to png file - mat_files = OrderedDict() # maps mtl name to corresponding list of material filenames - mtl_old_dirs = OrderedDict() # maps mtl name to corresponding directory where the mtl file exists - mat_old_paths = OrderedDict() # maps mtl name to corresponding list of relative mat paths from mtl directory for link_name, link_meshes in visual_objs.items(): link_mtl_files[link_name] = OrderedDict() for mesh_name, obj_file in link_meshes.items(): # Get absolute path and open the obj file if it exists: if obj_file is not None: - obj_path = f"{dataset_root}/objects/{obj_category}/{obj_model}/{obj_file}" + obj_path = os.path.abspath(f"{dataset_root}/objects/{obj_category}/{obj_model}/urdf/{obj_file}") with open(obj_path, "r") as f: mtls = [] for line in f.readlines(): @@ -469,30 +424,30 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat mtl = mtls[0] # TODO: Make name unique mtl_name = ".".join(os.path.basename(mtl).split(".")[:-1]).replace("-", "_").replace(".", "_") - mtl_old_dir = os.path.dirname(obj_path) + mtl_dir = os.path.dirname(obj_path) link_mtl_files[link_name][mesh_name] = mtl_name mtl_infos[mtl_name] = OrderedDict() - mtl_old_dirs[mtl_name] = mtl_old_dir - mat_files[mtl_name] = [] - mat_old_paths[mtl_name] = [] # Open the mtl file - mtl_path = f"{mtl_old_dir}/{mtl}" + mtl_path = os.path.join(mtl_dir, mtl) with open(mtl_path, "r") as f: # Read any lines beginning with map that aren't commented out for line in f.readlines(): if line[:4] == "map_": - map_type, map_file = line.split(" ") - map_file = map_file.split("\n")[0] - map_filename = os.path.basename(map_file) - mat_files[mtl_name].append(map_filename) - mat_old_paths[mtl_name].append(map_file) - mtl_infos[mtl_name][_MTL_MAP_TYPE_MAPPINGS[map_type.lower()]] = map_filename + map_type, map_path_relative_to_mtl_dir = line.split(" ") + map_path_relative_to_mtl_dir = map_path_relative_to_mtl_dir.split("\n")[0] + print("Found map path in file as ", map_path_relative_to_mtl_dir) + map_path_absolute = os.path.abspath(os.path.join(mtl_dir, map_path_relative_to_mtl_dir)) + print("Absolute map path is ", map_path_absolute) + map_path_relative_to_usd_dir = os.path.relpath(map_path_absolute, usd_dir) + print("USD path is ", usd_dir) + print("Material path relative to USD is", map_path_relative_to_usd_dir) + mtl_infos[mtl_name][_MTL_MAP_TYPE_MAPPINGS[map_type.lower()]] = ( + map_path_relative_to_usd_dir + ) print("Found material file:", mtl_name, mtl_infos[mtl_name]) - # Next, for each material information, we create a new material and port the material files to the USD directory - mat_new_fpath = os.path.join(usd_dir, "materials") - Path(mat_new_fpath).mkdir(parents=True, exist_ok=True) + # Next, for each material information, we create a new material shaders = OrderedDict() # maps mtl name to shader prim rendering_channel_mappings = { "diffuse": _set_mtl_albedo, @@ -505,9 +460,6 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat "emission": _set_mtl_emission, } for mtl_name, mtl_info in mtl_infos.items(): - for mat_old_path in mat_old_paths[mtl_name]: - shutil.copy(os.path.join(mtl_old_dirs[mtl_name], mat_old_path), mat_new_fpath) - # Create the new material mtl_created_list = [] lazy.omni.kit.commands.execute( @@ -522,7 +474,7 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat for mat_type, mat_file in mtl_info.items(): render_channel_fcn = rendering_channel_mappings.get(mat_type, None) if render_channel_fcn is not None: - render_channel_fcn(mat, os.path.join("materials", mat_file)) + render_channel_fcn(mat, mat_file) else: # Warn user that we didn't find the correct rendering channel log.debug(f"Warning: could not find rendering channel function for material: {mat_type}, skipping") @@ -563,9 +515,6 @@ def _import_rendering_channels(obj_prim, obj_category, obj_model, model_root_pat shaders[mtl_name], lazy.pxr.UsdShade.Tokens.strongerThanDescendants ) - # Lastly, we copy object_state texture maps that are state-conditioned; e.g.: cooked, soaked, etc. - _copy_object_state_textures(obj_category=obj_category, obj_model=obj_model, dataset_root=dataset_root) - def _add_xform_properties(prim): """ @@ -963,11 +912,12 @@ def import_obj_metadata(usd_path, obj_category, obj_model, dataset_root, import_ meta_prim.CreateAttribute("ig:metaLinkId", lazy.pxr.Sdf.ValueTypeNames.String) meta_prim.GetAttribute("ig:metaLinkId").Set(link_id) meta_prim.CreateAttribute("ig:metaLinkSubId", lazy.pxr.Sdf.ValueTypeNames.Int) - meta_prim.GetAttribute("ig:metaLinkId").Set(int(link_sub_id)) + meta_prim.GetAttribute("ig:metaLinkSubId").Set(int(link_sub_id)) - # Set the purpose of the meta link - purpose_attr = lazy.pxr.UsdGeom.Imageable(meta_prim).CreatePurposeAttr() - purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) + # Set the purpose of the visual meshes to be guide + for visual_mesh in meta_prim.GetChild("visuals").GetChildren(): + purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_mesh).CreatePurposeAttr() + purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) log.debug("Done processing meta links") @@ -1134,7 +1084,7 @@ def _create_urdf_import_config( import_config.set_merge_fixed_joints(merge_fixed_joints) import_config.set_convex_decomp(use_convex_decomposition) import_config.set_fix_base(False) - import_config.set_import_inertia_tensor(False) + import_config.set_import_inertia_tensor(True) import_config.set_distance_scale(1.0) import_config.set_density(0.0) import_config.set_default_drive_type(drive_mode.JOINT_DRIVE_NONE) @@ -2117,23 +2067,6 @@ def generate_urdf_for_obj( ), f"Something's wrong: there's more than 1 material file in {list(temp_dir_path.iterdir())}" original_material_filename = material_files[0].name - # Fix texture file paths if necessary. - # original_material_dir = G.nodes[link_node]["material_dir"] - # if original_material_dir: - # for src_texture_file in original_material_dir.iterdir(): - # fname = src_texture_file - # # fname is in the same format as room_light-0-0_VRayAOMap.png - # vray_name = fname[fname.index("VRay") : -4] if "VRay" in fname else None - # if vray_name in VRAY_MAPPING: - # dst_fname = VRAY_MAPPING[vray_name] - # else: - # raise ValueError(f"Unknown texture map: {fname}") - - # dst_texture_file = f"{obj_name}-base_link-{dst_fname}.png" - - # # Load the image - # shutil.copy2(original_material_dir / src_texture_file, obj_link_material_folder / dst_texture_file) - # Modify MTL reference in OBJ file mtl_name = f"{obj_name}-base_link.mtl" with open(obj_temp_path, "r") as f: From 58008ffef8ebdd2ec9c2e051ef415f29358a6508 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Wed, 5 Feb 2025 17:29:33 -0800 Subject: [PATCH 14/38] Finish implementing cloth prim features --- omnigibson/__init__.py | 26 +- omnigibson/prims/cloth_prim.py | 398 +++++++++++++++++++- omnigibson/systems/micro_particle_system.py | 136 +------ state_fold.py | 107 ++---- state_squeeze.py | 157 -------- 5 files changed, 450 insertions(+), 374 deletions(-) delete mode 100644 state_squeeze.py diff --git a/omnigibson/__init__.py b/omnigibson/__init__.py index 62f0273d9..36097227c 100644 --- a/omnigibson/__init__.py +++ b/omnigibson/__init__.py @@ -82,15 +82,15 @@ def clear( import omnigibson.lazy as lazy # First save important simulator settings - init_kwargs = dict( - gravity=sim.gravity if gravity is None else gravity, - physics_dt=sim.get_physics_dt() if physics_dt is None else physics_dt, - rendering_dt=sim.get_rendering_dt() if rendering_dt is None else rendering_dt, - sim_step_dt=sim.get_sim_step_dt() if sim_step_dt is None else sim_step_dt, - viewer_width=sim.viewer_width if viewer_width is None else viewer_width, - viewer_height=sim.viewer_height if viewer_height is None else viewer_height, - device=sim.device if device is None else device, - ) + # init_kwargs = dict( + # gravity=sim.gravity if gravity is None else gravity, + # physics_dt=sim.get_physics_dt() if physics_dt is None else physics_dt, + # rendering_dt=sim.get_rendering_dt() if rendering_dt is None else rendering_dt, + # sim_step_dt=sim.get_sim_step_dt() if sim_step_dt is None else sim_step_dt, + # viewer_width=sim.viewer_width if viewer_width is None else viewer_width, + # viewer_height=sim.viewer_height if viewer_height is None else viewer_height, + # device=sim.device if device is None else device, + # ) # First let the simulator clear everything it owns. sim._partial_clear() @@ -101,12 +101,12 @@ def clear( lazy.omni.isaac.core.simulation_context.SimulationContext.clear_instance() # Then relaunch the simulator. - launch(**init_kwargs) + launch() # **init_kwargs) # Check that the device remains the same - assert ( - sim.device == init_kwargs["device"] - ), f"Device changed from {init_kwargs['device']} to {sim.device} after clear." + # assert ( + # sim.device == init_kwargs["device"] + # ), f"Device changed from {init_kwargs['device']} to {sim.device} after clear." def cleanup(*args, **kwargs): diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 43c4481f2..ae35aabf0 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -9,8 +9,17 @@ import math from functools import cached_property - +import os +from pathlib import Path +import tempfile +from typing import Literal +import uuid + +from omnigibson.prims.xform_prim import XFormPrim +from omnigibson.utils.python_utils import multi_dim_linspace import torch as th +from omnigibson.utils.ui_utils import create_module_logger +import trimesh import omnigibson as og import omnigibson.lazy as lazy @@ -19,15 +28,26 @@ from omnigibson.prims.geom_prim import GeomPrim from omnigibson.utils.numpy_utils import vtarray_to_torch from omnigibson.utils.sim_utils import CsRawData -from omnigibson.utils.usd_utils import mesh_prim_to_trimesh_mesh, sample_mesh_keypoints +from omnigibson.utils.usd_utils import PoseAPI, mesh_prim_to_trimesh_mesh, sample_mesh_keypoints + +# Create module logger +log = create_module_logger(module_name=__name__) # Create settings for this module m = create_module_macros(module_path=__file__) +CLOTH_CONFIGURATIONS = ["default", "settled", "folded", "crumpled"] + # Subsample cloth particle points to boost performance +m.ALLOW_MULTIPLE_CLOTH_MESH_COMPONENTS = False m.N_CLOTH_KEYPOINTS = 1000 +m.FOLDING_INCREMENTS = 100 +m.CRUMPLING_INCREMENTS = 100 m.KEYPOINT_COVERAGE_THRESHOLD = 0.75 m.N_CLOTH_KEYFACES = 500 +m.MAX_CLOTH_PARTICLES = 20000 # Comes from a limitation in physx - do not increase +m.CLOTH_PARTICLE_CONTACT_OFFSET = 0.0075 +m.CLOTH_REMESHING_ERROR_THRESHOLD = 0.05 class ClothPrim(GeomPrim): @@ -88,8 +108,25 @@ def _post_load(self): if "mass" in self._load_config and self._load_config["mass"] is not None: self.mass = self._load_config["mass"] + # Save the default point configuration + self.save_configuration("default", self.points) + + # Remesh the object if necessary + if self._load_config.get("force_remesh", False) or ( + self._load_config.get("remesh", True) and not th.allclose(self.scale, th.ones(3)) + ): + # Remesh the object if necessary + log.warning( + f"Remeshing cloth {self.name}. This happens when cloth is loaded with non-unit scale or forced using the forced_remesh argument. It invalidates precached info for settled, folded, and crumpled configurations." + ) + self._remesh() + + # Set the mesh to use the desired configuration + points_configuration = self._load_config.get("default_point_configuration", "default") + self.reset_points_to_configuration(configuration=points_configuration) + # Clothify this prim, which is assumed to be a mesh - self.cloth_system.clothify_mesh_prim(mesh_prim=self._prim, remesh=self._load_config.get("remesh", True)) + self.cloth_system.clothify_mesh_prim(mesh_prim=self._prim) # Track generated particle count positions = self.compute_particle_positions() @@ -136,6 +173,361 @@ def _post_load(self): # Store the default position of the points in the local frame self._default_positions = vtarray_to_torch(self.get_attribute(attr="points")) + def _remesh(self): + assert self.prim is not None, "Cannot remesh a non-existent prim!" + has_uv_mapping = self.prim.GetAttribute("primvars:st").Get() is not None + + # We will remesh in pymeshlab, but it doesn't allow programmatic construction of a mesh with texcoords so + # we convert our mesh into a trimesh mesh, then export it to a temp file, then load it into pymeshlab + scaled_world_transform = PoseAPI.get_world_pose_with_scale(self.prim.GetPath().pathString) + # Convert to trimesh mesh (in world frame) + tm = mesh_prim_to_trimesh_mesh( + mesh_prim=self.prim, include_normals=True, include_texcoord=True, world_frame=True + ) + # Tmp file written to: {tmp_dir}/{tmp_fname}/{tmp_fname}.obj + tmp_name = str(uuid.uuid4()) + tmp_dir = os.path.join(tempfile.gettempdir(), tmp_name) + tmp_fpath = os.path.join(tmp_dir, f"{tmp_name}.obj") + Path(tmp_dir).mkdir(parents=True, exist_ok=True) + tm.export(tmp_fpath) + + # Start with the default particle distance + particle_distance = self.cloth_system.particle_contact_offset * 2 / 1.5 + + # Repetitively re-mesh at lower resolution until we have a mesh that has less than MAX_CLOTH_PARTICLES vertices + import pymeshlab # We import this here because it takes a few seconds to load. + + for _ in range(10): + ms = pymeshlab.MeshSet() + ms.load_new_mesh(tmp_fpath) + + # Re-mesh based on @particle_distance - distance chosen such that at rest particles should be just touching + # each other. The 1.5 magic number comes from the particle cloth demo from omni + # Note that this means that the particles will overlap with each other, since at dist = 2 * contact_offset + # the particles are just touching each other at rest + + avg_edge_percentage_mismatch = 1.0 + # Loop re-meshing until average edge percentage is within error threshold or we reach the max number of tries + for _ in range(5): + if avg_edge_percentage_mismatch <= m.CLOTH_REMESHING_ERROR_THRESHOLD: + break + + ms.meshing_isotropic_explicit_remeshing( + iterations=5, adaptive=True, targetlen=pymeshlab.AbsoluteValue(particle_distance) + ) + # Make sure the clothes is watertight + ms.meshing_repair_non_manifold_edges() + ms.meshing_repair_non_manifold_vertices() + + # If the cloth has multiple pieces, only keep the largest one + ms.generate_splitting_by_connected_components(delete_source_mesh=True) + if len(ms) > 1: + assert m.ALLOW_MULTIPLE_CLOTH_MESH_COMPONENTS, "Cloth mesh has multiple components!" + + log.warning( + f"The cloth mesh has {len(ms)} disconnected components. To simplify, we only keep the mesh with largest face number." + ) + biggest_face_num = 0 + for split_mesh in ms: + face_num = split_mesh.face_number() + if face_num > biggest_face_num: + biggest_face_num = face_num + new_ms = pymeshlab.MeshSet() + for split_mesh in ms: + if split_mesh.face_number() == biggest_face_num: + new_ms.add_mesh(split_mesh) + ms = new_ms + + avg_edge_percentage_mismatch = abs( + 1.0 - particle_distance / ms.get_geometric_measures()["avg_edge_length"] + ) + else: + # Terminate anyways, but don't fail + log.warning("The generated cloth may not have evenly distributed particles.") + + # Check if we have too many vertices + cm = ms.current_mesh() + if cm.vertex_number() > m.MAX_CLOTH_PARTICLES: + # We have too many vertices, so we will re-mesh again + particle_distance *= math.sqrt(2) # halve the number of vertices + log.warning( + f"Too many vertices ({cm.vertex_number()})! Re-meshing with particle distance {particle_distance}..." + ) + else: + break + else: + raise ValueError(f"Could not remesh with less than MAX_CLOTH_PARTICLES ({m.MAX_CLOTH_PARTICLES}) vertices!") + + # Re-write data to @mesh_prim + new_faces = cm.face_matrix() + new_vertices = cm.vertex_matrix() + new_normals = cm.vertex_normal_matrix() + texcoord = cm.wedge_tex_coord_matrix() if has_uv_mapping else None + tm = trimesh.Trimesh( + vertices=new_vertices, + faces=new_faces, + vertex_normals=new_normals, + ) + # Apply the inverse of the world transform to get the mesh back into its local frame + tm.apply_transform(th.linalg.inv_ex(scaled_world_transform).inverse) + + # Update the mesh prim to store the new information. First update the non-configuration- + # dependent fields + face_vertex_counts = th.tensor([len(face) for face in tm.faces], dtype=int).cpu().numpy() + self.prim.GetAttribute("faceVertexCounts").Set(face_vertex_counts) + self.prim.GetAttribute("faceVertexIndices").Set(tm.faces.flatten()) + self.prim.GetAttribute("normals").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(tm.vertex_normals)) + if has_uv_mapping: + self.prim.GetAttribute("primvars:st").Set(lazy.pxr.Vt.Vec2fArray.FromNumpy(texcoord)) + + # Remove the properties for all configurations + for config in CLOTH_CONFIGURATIONS: + attr_name = f"points_{config}" + if self.prim.HasAttribute(attr_name): + self.prim.RemoveProperty(attr_name) + + # Then update the configuration-dependent fields + self.save_configuration("default", th.tensor(tm.vertices, dtype=th.float32)) + + # Then update the points to the default configuration + self.reset_points_to_configuration("default") + + def generate_settled_configuration(self): + """ + Generate a settled configuration for the cloth by running a few steps of simulation to let the cloth settle + """ + # Reset position first (moving to a position where the AABB is just on the floor) + self.reset_points_to_configuration("default") + self.set_position_orientation(position=th.zeros(3), orientation=th.tensor([0, 0, 0, 1.0])) + self.set_position_orientation( + position=th.tensor([0, 0, self.aabb_extent[2] / 2.0 - self.aabb_center[2]]), + orientation=th.tensor([0, 0, 0, 1.0]), + ) + + # Run a few steps of simulation to let the cloth settle + for _ in range(300): + og.sim.step() + + # Save the settled configuration + self.save_configuration("settled", self.points) + + def generate_folded_configuration(self): + # Settle and reset position first (moving to a position where the AABB is just on the floor) + self.reset_points_to_configuration("settled") + self.set_position_orientation(position=th.zeros(3), orientation=th.tensor([0, 0, 0, 1.0])) + self.set_position_orientation( + position=th.tensor([0, 0, self.aabb_extent[2] / 2.0 - self.aabb_center[2]]), + orientation=th.tensor([0, 0, 0, 1.0]), + ) + + for _ in range(100): + og.sim.step() + + # Fold - first stage. Take the bottom third of Y positions and fold them over towards + # the middle third. + pos = self.compute_particle_positions() + y_min = th.min(pos[:, 1]) + y_max = th.max(pos[:, 1]) + y_bottom_third = y_min + (y_max - y_min) / 3 + y_top_third = y_min + 2 * (y_max - y_min) / 3 + first_folding_indices = th.where(pos[:, 1] < y_bottom_third)[0] + first_folding_initial_pos = th.clone(pos[first_folding_indices]) + first_staying_pos = pos[pos[:, 1] >= y_bottom_third] + first_staying_z_height = th.max(first_staying_pos[:, 2]) - th.min(first_staying_pos[:, 2]) + first_folding_final_pos = th.clone(first_folding_initial_pos) + first_folding_final_pos[:, 1] = ( + 2 * y_bottom_third - first_folding_final_pos[:, 1] + ) # Mirror bottom to the above of y_mid_bottom + first_folding_final_pos[:, 2] += first_staying_z_height # Add a Z offset to keep particles from overlapping + for ctrl_pts in multi_dim_linspace(first_folding_initial_pos, first_folding_final_pos, m.FOLDING_INCREMENTS): + all_pts = th.clone(pos) + all_pts[first_folding_indices] = ctrl_pts + self.set_particle_positions(all_pts) + og.sim.step() + + # Fold - second stage. Take the top third of original Y positions and fold them over towards the + # middle too. + pos = self.compute_particle_positions() + second_folding_indices = th.where(pos[:, 1] > y_top_third)[0] + second_folding_initial_pos = th.clone(pos[second_folding_indices]) + second_staying_pos = pos[pos[:, 1] <= y_top_third] + second_staying_z_height = th.max(second_staying_pos[:, 2]) - th.min(second_staying_pos[:, 2]) + second_folding_final_pos = th.clone(second_folding_initial_pos) + second_folding_final_pos[:, 1] = ( + 2 * y_top_third - second_folding_final_pos[:, 1] + ) # Mirror top to the below of y_mid + second_folding_final_pos[:, 2] += second_staying_z_height # Add a Z offset to keep particles from overlapping + for ctrl_pts in multi_dim_linspace(second_folding_initial_pos, second_folding_final_pos, m.FOLDING_INCREMENTS): + all_pts = th.clone(pos) + all_pts[second_folding_indices] = ctrl_pts + self.set_particle_positions(all_pts) + og.sim.step() + + # Fold - third stage. Fold along the X axis, in half. + pos = self.compute_particle_positions() + x_min = th.min(pos[:, 0]) + x_max = th.max(pos[:, 0]) + x_mid = (x_min + x_max) / 2 + third_folding_indices = th.where(pos[:, 0] > x_mid)[0] + third_folding_initial_pos = th.clone(pos[third_folding_indices]) + third_staying_pos = pos[pos[:, 0] <= x_mid] + third_staying_z_height = th.max(third_staying_pos[:, 2]) - th.min(third_staying_pos[:, 2]) + third_folding_final_pos = th.clone(third_folding_initial_pos) + third_folding_final_pos[:, 0] = 2 * x_mid - third_folding_final_pos[:, 0] + third_folding_final_pos[:, 2] += third_staying_z_height # Add a Z offset to keep particles from overlapping + for ctrl_pts in multi_dim_linspace(third_folding_initial_pos, third_folding_final_pos, m.FOLDING_INCREMENTS): + all_pts = th.clone(pos) + all_pts[third_folding_indices] = ctrl_pts + self.set_particle_positions(all_pts) + og.sim.step() + + # Let things settle + for _ in range(100): + og.sim.step() + + # Save the folded configuration + self.save_configuration("folded", self.points) + + def generate_crumpled_configuration(self): + # Settle and reset position first (moving to a position where the AABB is just on the floor) + self.reset_points_to_configuration("settled") + self.set_position_orientation(position=th.zeros(3), orientation=th.tensor([0, 0, 0, 1.0])) + self.set_position_orientation( + position=th.tensor([0, 0, self.aabb_extent[2] / 2.0 - self.aabb_center[2]]), + orientation=th.tensor([0, 0, 0, 1.0]), + ) + for _ in range(100): + og.sim.step() + + # We just need to generate the side planes + box_half_extent = self.aabb_extent / 2 + plane_centers = ( + th.tensor( + [ + [1, 0, 1], + [0, 1, 1], + [-1, 0, 1], + [0, -1, 1], + ] + ) + * box_half_extent + ) + plane_prims = [] + plane_motions = [] + for i, pc in enumerate(plane_centers): + plane = lazy.omni.isaac.core.objects.ground_plane.GroundPlane( + prim_path=f"/World/plane_{i}", + name=f"plane_{i}", + z_position=0, + size=box_half_extent[2].item(), + color=None, + visible=False, + ) + + plane_as_prim = XFormPrim( + relative_prim_path=f"/plane_{i}", + name=plane.name, + ) + plane_as_prim.load(None) + + # Build the plane orientation from the plane normal + horiz_dir = pc - th.tensor([0, 0, box_half_extent[2]]) + plane_z = -1 * horiz_dir / th.norm(horiz_dir) + plane_x = th.tensor([0, 0, 1], dtype=th.float32) + plane_y = th.cross(plane_z, plane_x) + plane_mat = th.stack([plane_x, plane_y, plane_z], dim=1) + plane_quat = T.mat2quat(plane_mat) + plane_as_prim.set_position_orientation(pc, plane_quat) + + plane_prims.append(plane_as_prim) + plane_motions.append(plane_z) + + # Calculate end positions for all walls + end_positions = [] + for i in range(4): + plane_prim = plane_prims[i] + position = plane_prim.get_position_orientation()[0] + end_positions.append(position + plane_motions[i] * box_half_extent) + + for step in range(m.CRUMPLING_INCREMENTS): + # Move all walls a small amount + for i in range(4): + plane_prim = plane_prims[i] + current_pos = multi_dim_linspace( + plane_prim.get_position_orientation()[0], end_positions[i], m.CRUMPLING_INCREMENTS + )[step] + plane_prim.set_position_orientation(position=current_pos) + + og.sim.step() + + # Check cloth height + cloth_positions = self.compute_particle_positions() + max_height = th.max(cloth_positions[:, 2]) + + # Get distance between facing walls (assuming walls 0-2 and 1-3 are facing pairs) + wall_dist_1 = th.linalg.norm( + plane_prims[0].get_position_orientation()[0] - plane_prims[2].get_position_orientation()[0] + ) + wall_dist_2 = th.linalg.norm( + plane_prims[1].get_position_orientation()[0] - plane_prims[3].get_position_orientation()[0] + ) + min_wall_dist = min(wall_dist_1, wall_dist_2) + + # Stop if cloth height exceeds wall distance + if max_height > min_wall_dist: + break + + # Let things settle + for _ in range(100): + og.sim.step() + + # Save the folded configuration + self.save_configuration("crumpled", self.points) + + # Remove the planes + for plane_prim in plane_prims: + lazy.omni.isaac.core.utils.prims.delete_prim(plane_prim.prim_path) + + def get_available_configurations(self): + """ + Returns: + list: List of available configurations for this cloth prim + """ + return [x for x in CLOTH_CONFIGURATIONS if self.prim.HasAttribute(f"points_{x}")] + + def save_configuration(self, configuration: Literal["default", "settled", "folded", "crumpled"], points: th.tensor): + """ + Save the current configuration of the cloth to a specific configuration + + Args: + configuration (Literal["default", "settled", "folded", "crumpled"]): Configuration to save the cloth to + """ + # Get the points arguments stored on the USD prim + assert configuration in CLOTH_CONFIGURATIONS, f"Invalid cloth configuration {configuration}!" + assert self.prim is not None, "Cannot save configuration for a non-existent prim!" + attr_name = f"points_{configuration}" + points_default_attrib = ( + self.prim.GetAttribute(attr_name) + if self.prim.HasAttribute(attr_name) + else self.prim.CreateAttribute(attr_name, lazy.pxr.Sdf.ValueTypeNames.Float3Array) + ) + points_default_attrib.Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(points.cpu().numpy())) + + def reset_points_to_configuration(self, configuration: Literal["default", "settled", "folded", "crumpled"]): + """ + Reset the cloth to a specific configuration + + Args: + configuration (Literal["default", "settled", "folded", "crumpled"]): Configuration to reset the cloth to + """ + # Get the points arguments stored on the USD prim + assert ( + configuration in self.get_available_configurations() + ), f"Invalid or unavailable cloth configuration {configuration}!" + attr_name = f"points_{configuration}" + points = self.get_attribute(attr=attr_name) + self.set_attribute(attr="points", val=points) + # For cloth, points should NOT be @cached_property because their local poses change over time @property def points(self): diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index 68eee9c72..8ebad467c 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -1,8 +1,4 @@ import math -import os -import tempfile -import uuid -from pathlib import Path import torch as th import trimesh @@ -19,9 +15,7 @@ from omnigibson.utils.python_utils import assert_valid_key, torch_delete from omnigibson.utils.ui_utils import create_module_logger from omnigibson.utils.usd_utils import ( - PoseAPI, absolute_prim_path_to_scene_relative, - mesh_prim_to_trimesh_mesh, scene_relative_prim_path_to_absolute, ) @@ -31,10 +25,7 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -# TODO (eric): figure out whether one offset can fit all -m.MAX_CLOTH_PARTICLES = 20000 # Comes from a limitation in physx - do not increase m.CLOTH_PARTICLE_CONTACT_OFFSET = 0.0075 -m.CLOTH_REMESHING_ERROR_THRESHOLD = 0.05 m.CLOTH_STRETCH_STIFFNESS = 100.0 m.CLOTH_BEND_STIFFNESS = 50.0 m.CLOTH_SHEAR_STIFFNESS = 70.0 @@ -1647,137 +1638,14 @@ def remove_all_particles(self): # Override base method since there are no particles to be deleted pass - def clothify_mesh_prim(self, mesh_prim, remesh=True, particle_distance=None): + def clothify_mesh_prim(self, mesh_prim): """ Clothifies @mesh_prim by applying the appropriate Cloth API, optionally re-meshing the mesh so that the resulting generated particles are roughly @particle_distance apart from each other. Args: mesh_prim (Usd.Prim): Mesh prim to clothify - remesh (bool): If True, will remesh the input mesh before converting it into a cloth - particle_distance (None or float): If set and @remesh is True, specifies the absolute target distance - between generated cloth particles. If None, a value is automatically chosen such that the generated - cloth particles are roughly touching each other, given self._particle_contact_offset and - @mesh_prim's scale - """ - has_uv_mapping = mesh_prim.GetAttribute("primvars:st").Get() is not None - if not remesh: - # We always load into trimesh to remove redundant particles (since natively omni redundantly represents - # the number of vertices as 6x the total unique number of vertices) - tm = mesh_prim_to_trimesh_mesh( - mesh_prim=mesh_prim, include_normals=True, include_texcoord=True, world_frame=False - ) - texcoord = ( - vtarray_to_torch(mesh_prim.GetAttribute("primvars:st").Get(), dtype=th.float32) - if has_uv_mapping - else None - ) - else: - # We will remesh in pymeshlab, but it doesn't allow programmatic construction of a mesh with texcoords so - # we convert our mesh into a trimesh mesh, then export it to a temp file, then load it into pymeshlab - scaled_world_transform = PoseAPI.get_world_pose_with_scale(mesh_prim.GetPath().pathString) - # Convert to trimesh mesh (in world frame) - tm = mesh_prim_to_trimesh_mesh( - mesh_prim=mesh_prim, include_normals=True, include_texcoord=True, world_frame=True - ) - # Tmp file written to: {tmp_dir}/{tmp_fname}/{tmp_fname}.obj - tmp_name = str(uuid.uuid4()) - tmp_dir = os.path.join(tempfile.gettempdir(), tmp_name) - tmp_fpath = os.path.join(tmp_dir, f"{tmp_name}.obj") - Path(tmp_dir).mkdir(parents=True, exist_ok=True) - tm.export(tmp_fpath) - - # Start with the default particle distance - particle_distance = ( - self._particle_contact_offset * 2 / 1.5 if particle_distance is None else particle_distance - ) - - # Repetitively re-mesh at lower resolution until we have a mesh that has less than MAX_CLOTH_PARTICLES vertices - import pymeshlab # We import this here because it takes a few seconds to load. - - for _ in range(10): - ms = pymeshlab.MeshSet() - ms.load_new_mesh(tmp_fpath) - - # Re-mesh based on @particle_distance - distance chosen such that at rest particles should be just touching - # each other. The 1.5 magic number comes from the particle cloth demo from omni - # Note that this means that the particles will overlap with each other, since at dist = 2 * contact_offset - # the particles are just touching each other at rest - - avg_edge_percentage_mismatch = 1.0 - # Loop re-meshing until average edge percentage is within error threshold or we reach the max number of tries - for _ in range(5): - if avg_edge_percentage_mismatch <= m.CLOTH_REMESHING_ERROR_THRESHOLD: - break - - ms.meshing_isotropic_explicit_remeshing( - iterations=5, adaptive=True, targetlen=pymeshlab.AbsoluteValue(particle_distance) - ) - # Make sure the clothes is watertight - ms.meshing_repair_non_manifold_edges() - ms.meshing_repair_non_manifold_vertices() - - # If the cloth has multiple pieces, only keep the largest one - ms.generate_splitting_by_connected_components(delete_source_mesh=True) - if len(ms) > 1: - log.warning( - f"The cloth mesh has {len(ms)} disconnected pieces. To simplify, we only keep the mesh with largest face number." - ) - biggest_face_num = 0 - for split_mesh in ms: - face_num = split_mesh.face_number() - if face_num > biggest_face_num: - biggest_face_num = face_num - new_ms = pymeshlab.MeshSet() - for split_mesh in ms: - if split_mesh.face_number() == biggest_face_num: - new_ms.add_mesh(split_mesh) - ms = new_ms - - avg_edge_percentage_mismatch = abs( - 1.0 - particle_distance / ms.get_geometric_measures()["avg_edge_length"] - ) - else: - # Terminate anyways, but don't fail - log.warning("The generated cloth may not have evenly distributed particles.") - - # Check if we have too many vertices - cm = ms.current_mesh() - if cm.vertex_number() > m.MAX_CLOTH_PARTICLES: - # We have too many vertices, so we will re-mesh again - particle_distance *= math.sqrt(2) # halve the number of vertices - log.warning( - f"Too many vertices ({cm.vertex_number()})! Re-meshing with particle distance {particle_distance}..." - ) - else: - break - else: - raise ValueError( - f"Could not remesh with less than MAX_CLOTH_PARTICLES ({m.MAX_CLOTH_PARTICLES}) vertices!" - ) - - # Re-write data to @mesh_prim - new_faces = cm.face_matrix() - new_vertices = cm.vertex_matrix() - new_normals = cm.vertex_normal_matrix() - texcoord = cm.wedge_tex_coord_matrix() if has_uv_mapping else None - tm = trimesh.Trimesh( - vertices=new_vertices, - faces=new_faces, - vertex_normals=new_normals, - ) - # Apply the inverse of the world transform to get the mesh back into its local frame - tm.apply_transform(th.linalg.inv_ex(scaled_world_transform).inverse) - - # Update the mesh prim - face_vertex_counts = th.tensor([len(face) for face in tm.faces], dtype=int).cpu().numpy() - mesh_prim.GetAttribute("faceVertexCounts").Set(face_vertex_counts) - mesh_prim.GetAttribute("points").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(tm.vertices)) - mesh_prim.GetAttribute("faceVertexIndices").Set(tm.faces.flatten()) - mesh_prim.GetAttribute("normals").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(tm.vertex_normals)) - if has_uv_mapping: - mesh_prim.GetAttribute("primvars:st").Set(lazy.pxr.Vt.Vec2fArray.FromNumpy(texcoord)) - + """ # Convert into particle cloth lazy.omni.physx.scripts.particleUtils.add_physx_particle_cloth( stage=og.sim.stage, diff --git a/state_fold.py b/state_fold.py index 6ca97b4dd..20cbfb9e8 100644 --- a/state_fold.py +++ b/state_fold.py @@ -1,15 +1,15 @@ from omnigibson.utils.constants import PrimType -from omnigibson.object_states import Folded, Unfolded from omnigibson.macros import gm -import numpy as np import omnigibson as og -import json -import torch +import omnigibson.lazy as lazy +from omnigibson.utils.ui_utils import KeyboardEventHandler +import torch as th # Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True +gm.DATASET_PATH = "/scr/cloth-test" def main(random_selection=False, headless=False, short_exec=False): @@ -19,14 +19,12 @@ def main(random_selection=False, headless=False, short_exec=False): og.log.info(f"Demo {__file__}\n " + "*" * 80 + "\n Description:\n" + main.__doc__ + "*" * 80) cloth_category_models = [ - ("hoodie", "agftpm"), + ("jeans", "nmvvil"), ] for cloth in cloth_category_models: category = cloth[0] model = cloth[1] - if model != "agftpm": - continue print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") # Create the scene config to load -- empty scene + custom cloth object cfg = { @@ -39,11 +37,13 @@ def main(random_selection=False, headless=False, short_exec=False): "name": model, "category": category, "model": model, - # "bounding_box": [0.897, 0.568, 0.012], "prim_type": PrimType.CLOTH, "abilities": {"cloth": {}}, - # "position": [0, 0, 0.5], - "orientation": [0.7071, 0.0, 0.7071, 0.0], + "position": [0, 0, 0.5], + "orientation": [0, 0, 0, 1], + "load_config": { + "default_configuration": "settled", + }, }, ], } @@ -52,69 +52,42 @@ def main(random_selection=False, headless=False, short_exec=False): env = og.Environment(configs=cfg) # Grab object references - carpet = env.scene.object_registry("name", model) - objs = [carpet] + obj = env.scene.object_registry("name", model) # Set viewer camera og.sim.viewer_camera.set_position_orientation( - position=np.array([0.46382895, -2.66703958, 1.22616824]), - orientation=np.array([0.58779174, -0.00231237, -0.00318273, 0.80900271]), + position=th.tensor([0.46382895, -2.66703958, 1.22616824]), + orientation=th.tensor([0.58779174, -0.00231237, -0.00318273, 0.80900271]), ) - for _ in range(100): - og.sim.step() - - print("\nCloth state:\n") - - if not short_exec: - # Get particle positions - increments = 100 - obj = objs[0] - - # Fold - first stage - pos = np.asarray(obj.root_link.compute_particle_positions()) - y_min = np.min(pos[:, 1]) - y_max = np.max(pos[:, 1]) - y_mid_bottom = y_min + (y_max - y_min) / 3 - y_mid_top = y_min + 2 * (y_max - y_min) / 3 - bottom_indices = np.where(pos[:, 1] < y_mid_bottom)[0] - bottom_start = np.copy(pos[bottom_indices]) - bottom_end = np.copy(bottom_start) - bottom_end[:, 1] = 2 * y_mid_bottom - bottom_end[:, 1] # Mirror bottom to the above of y_mid_bottom - for ctrl_pts in np.linspace(bottom_start, bottom_end, increments): - obj.root_link.set_particle_positions(torch.tensor(ctrl_pts), idxs=bottom_indices) - og.sim.step() - - # Fold - second stage - pos = np.asarray(obj.root_link.compute_particle_positions()) - y_min = np.min(pos[:, 1]) - y_max = np.max(pos[:, 1]) - y_mid_bottom = y_min + (y_max - y_min) / 3 - y_mid_top = y_min + 2 * (y_max - y_min) / 3 - top_indices = np.where(pos[:, 1] > y_mid_top)[0] - top_start = np.copy(pos[top_indices]) - top_end = np.copy(top_start) - top_end[:, 1] = 2 * y_mid_top - top_end[:, 1] # Mirror top to the below of y_mid_top - for ctrl_pts in np.linspace(top_start, top_end, increments): - obj.root_link.set_particle_positions(torch.tensor(ctrl_pts), idxs=top_indices) - og.sim.step() - - # Fold - third stage - pos = np.asarray(obj.root_link.compute_particle_positions()) - x_min = np.min(pos[:, 0]) - x_max = np.max(pos[:, 0]) - x_mid = (x_min + x_max) / 2 - indices = np.argsort(pos, axis=0)[:, 0][-len(pos) // 2 :] - start = np.copy(pos[indices]) - end = np.copy(start) - end[:, 0] = 2 * x_mid - end[:, 0] - for ctrl_pts in np.linspace(start, end, increments): - obj.root_link.set_particle_positions(torch.tensor(ctrl_pts), idxs=indices) - og.sim.step() + def reset_points_to_configuration(configuration): + obj.root_link.reset_points_to_configuration(configuration) + obj.set_position_orientation(position=th.zeros(3), orientation=th.tensor([0.0, 0.0, 0.0, 1.0])) + obj.set_position_orientation( + position=th.tensor([0, 0, obj.aabb_extent[2] / 2.0 - obj.aabb_center[2]]), + orientation=th.tensor([0.0, 0.0, 0.0, 1.0]), + ) + + KeyboardEventHandler.initialize() + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.Q, + callback_fn=lambda: reset_points_to_configuration("default"), + ) + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.W, + callback_fn=lambda: reset_points_to_configuration("settled"), + ) + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.E, + callback_fn=lambda: reset_points_to_configuration("folded"), + ) + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.R, + callback_fn=lambda: reset_points_to_configuration("crumpled"), + ) - while True: - print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") - env.step(np.array([])) + while True: + og.sim.render() # Shut down env at the end print() diff --git a/state_squeeze.py b/state_squeeze.py deleted file mode 100644 index a650f632a..000000000 --- a/state_squeeze.py +++ /dev/null @@ -1,157 +0,0 @@ -from omnigibson.utils.constants import PrimType -from omnigibson.object_states import Folded, Unfolded -from omnigibson.macros import gm -import numpy as np - -import omnigibson as og -import omnigibson.lazy as lazy -import json -import torch -import time -from omnigibson.prims.xform_prim import XFormPrim -import omnigibson.utils.transform_utils as T - -# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True - - -# Set up the 4 walls -def generate_box(box_half_extent=torch.tensor([1, 1, 1], dtype=torch.float32), visualize_wall=False): - # Temp function to generate the walls for squeezing the cloth - # The floor plane already exists - # We just need to generate the side planes - plane_centers = ( - torch.tensor( - [ - [1, 0, 1], - [0, 1, 1], - [-1, 0, 1], - [0, -1, 1], - ] - ) - * box_half_extent - ) - plane_prims = [] - plane_motions = [] - for i, pc in enumerate(plane_centers): - plane = lazy.omni.isaac.core.objects.ground_plane.GroundPlane( - prim_path=f"/World/plane_{i}", - name=f"plane_{i}", - z_position=0, - size=box_half_extent[2].item(), - color=None, - visible=visualize_wall, - ) - - plane_as_prim = XFormPrim( - relative_prim_path=f"/plane_{i}", - name=plane.name, - ) - plane_as_prim.load(None) - - # Build the plane orientation from the plane normal - horiz_dir = pc - torch.tensor([0, 0, box_half_extent[2]]) - plane_z = -1 * horiz_dir / torch.norm(horiz_dir) - plane_x = torch.tensor([0, 0, 1], dtype=torch.float32) - plane_y = torch.cross(plane_z, plane_x) - plane_mat = torch.stack([plane_x, plane_y, plane_z], dim=1) - plane_quat = T.mat2quat(plane_mat) - plane_as_prim.set_position_orientation(pc, plane_quat) - - plane_prims.append(plane_as_prim) - plane_motions.append(plane_z) - return plane_prims, plane_motions - - -def main(visualize_wall=False): - """ - Demo of cloth objects that can be wall squeezed - """ - - cloth_category_models = [ - ("bandana", "wbhliu"), - ] - # cloth_category_models = [ - # ("hoodie", "agftpm"), - # ] - - for cloth in cloth_category_models: - category = cloth[0] - model = cloth[1] - print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") - # Create the scene config to load -- empty scene + custom cloth object - cfg = { - "scene": { - "type": "Scene", - }, - "objects": [ - { - "type": "DatasetObject", - "name": model, - "category": category, - "model": model, - "prim_type": PrimType.CLOTH, - "abilities": {"cloth": {}}, - }, - ], - } - - # Create the environment - env = og.Environment(configs=cfg) - - plane_prims, plane_motions = generate_box(visualize_wall=visualize_wall) - - # Grab object references - carpet = env.scene.object_registry("name", model) - objs = [carpet] - - # Set viewer camera - og.sim.viewer_camera.set_position_orientation( - position=np.array([0.46382895, -2.66703958, 1.22616824]), - orientation=np.array([0.58779174, -0.00231237, -0.00318273, 0.80900271]), - ) - - for _ in range(100): - og.sim.step() - - # Calculate end positions for all walls - end_positions = [] - for i in range(4): - plane_prim = plane_prims[i] - position = plane_prim.get_position() - end_positions.append(np.array(position) + np.array(plane_motions[i])) - - increments = 1000 - for step in range(increments): - # Move all walls a small amount - for i in range(4): - plane_prim = plane_prims[i] - current_pos = np.linspace(plane_prim.get_position(), end_positions[i], increments)[step] - plane_prim.set_position_orientation(position=current_pos) - - og.sim.step() - - # Check cloth height - cloth_positions = objs[0].root_link.compute_particle_positions() - # import pdb; pdb.set_trace() - max_height = torch.max(cloth_positions[:, 2]) - - # Get distance between facing walls (assuming walls 0-2 and 1-3 are facing pairs) - wall_dist_1 = torch.linalg.norm(plane_prims[0].get_position() - plane_prims[2].get_position()) - wall_dist_2 = torch.linalg.norm(plane_prims[1].get_position() - plane_prims[3].get_position()) - min_wall_dist = min(wall_dist_1, wall_dist_2) - - # Stop if cloth height exceeds wall distance - if max_height > min_wall_dist: - print(f"Stopping: Cloth height ({max_height:.3f}) exceeds wall distance ({min_wall_dist:.3f})") - break - - # TODO: save the cloth in the squeezed state - - # Shut down env at the end - env.close() - - -if __name__ == "__main__": - main() From fbbb09f3603415ab14a81cf1a967bdc3152d3472 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Wed, 5 Feb 2025 17:46:42 -0800 Subject: [PATCH 15/38] Include root-level visuals prim in guide purpose conversion --- omnigibson/utils/asset_conversion_utils.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 18c5d5b5d..5b7bfd1b4 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -915,7 +915,10 @@ def import_obj_metadata(usd_path, obj_category, obj_model, dataset_root, import_ meta_prim.GetAttribute("ig:metaLinkSubId").Set(int(link_sub_id)) # Set the purpose of the visual meshes to be guide - for visual_mesh in meta_prim.GetChild("visuals").GetChildren(): + visual_prim = meta_prim.GetChild("visuals") + purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_prim).CreatePurposeAttr() + purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) + for visual_mesh in visual_prim.GetChildren(): purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_mesh).CreatePurposeAttr() purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) From b3a75a728c0c651dd08aabfd5c3a037d2a389c6a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 7 Feb 2025 15:27:38 -0800 Subject: [PATCH 16/38] Learn new meta link format --- .../object_states/link_based_state_mixin.py | 11 +++++++---- omnigibson/prims/rigid_prim.py | 19 +++++++++++++++++++ 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/omnigibson/object_states/link_based_state_mixin.py b/omnigibson/object_states/link_based_state_mixin.py index b2b10924d..abf59a827 100644 --- a/omnigibson/object_states/link_based_state_mixin.py +++ b/omnigibson/object_states/link_based_state_mixin.py @@ -27,12 +27,12 @@ def is_compatible(cls, obj, **kwargs): return True, None metalink_prefix = cls.metalink_prefix for link in obj.links.values(): - if metalink_prefix in link.name: + if link.is_meta_link and link.meta_link_type == metalink_prefix: return True, None return ( False, - f"LinkBasedStateMixin {cls.__name__} requires metalink with prefix {cls.metalink_prefix} " + f"LinkBasedStateMixin {cls.__name__} requires meta link with type {cls.metalink_prefix} " f"for obj {obj.name} but none was found! To get valid compatible object models, please use " f"omnigibson.utils.asset_utils.get_all_object_category_models_with_abilities(...)", ) @@ -50,12 +50,15 @@ def is_compatible_asset(cls, prim, **kwargs): metalink_prefix = cls.metalink_prefix for child in prim.GetChildren(): if child.GetTypeName() == "Xform": - if metalink_prefix in child.GetName(): + if ( + child.HasAttribute("ig:metaLinkType") + and child.GetAttribute("ig:metaLinkType").Get() == metalink_prefix + ): return True, None return ( False, - f"LinkBasedStateMixin {cls.__name__} requires metalink with prefix {cls.metalink_prefix} " + f"LinkBasedStateMixin {cls.__name__} requires meta link with prefix {cls.metalink_prefix} " f"for asset prim {prim.GetName()} but none was found! To get valid compatible object models, " f"please use omnigibson.utils.asset_utils.get_all_object_category_models_with_abilities(...)", ) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 1d547fdd7..9d4c7cca4 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -796,6 +796,25 @@ def visual_aabb_center(self): min_corner, max_corner = self.visual_aabb return (max_corner + min_corner) / 2.0 + @cached_property + def is_meta_link(self): + return self.prim.HasAttribute("ig:isMetaLink") and self.get_attribute("ig:isMetaLink") + + @cached_property + def meta_link_type(self): + assert self.is_meta_link, f"{self.name} is not a meta link" + return self.get_attribute("ig:metaLinkType") + + @cached_property + def meta_link_id(self): + assert self.is_meta_link, f"{self.name} is not a meta link" + return self.get_attribute("ig:metaLinkId") + + @cached_property + def meta_link_sub_id(self): + assert self.is_meta_link, f"{self.name} is not a meta link" + return self.get_attribute("ig:metaLinkSubId") + def enable_gravity(self): """ Enables gravity for this rigid body From 2b9fa6b82f95ad4b544baea64a410b50b131de7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 7 Feb 2025 15:31:37 -0800 Subject: [PATCH 17/38] Standardize spelling of meta link --- omnigibson/object_states/attached_to.py | 6 +-- omnigibson/object_states/contains.py | 2 +- omnigibson/object_states/factory.py | 6 +-- .../object_states/heat_source_or_sink.py | 8 ++-- .../object_states/link_based_state_mixin.py | 38 +++++++++---------- omnigibson/object_states/on_fire.py | 4 +- omnigibson/object_states/particle_modifier.py | 20 +++++----- .../object_states/particle_source_or_sink.py | 16 ++++---- omnigibson/object_states/toggle.py | 2 +- omnigibson/objects/stateful_object.py | 8 ++-- omnigibson/utils/asset_utils.py | 14 +++---- omnigibson/utils/bddl_utils.py | 10 ++--- 12 files changed, 66 insertions(+), 68 deletions(-) diff --git a/omnigibson/object_states/attached_to.py b/omnigibson/object_states/attached_to.py index 30a7b35fb..2e151d291 100644 --- a/omnigibson/object_states/attached_to.py +++ b/omnigibson/object_states/attached_to.py @@ -76,10 +76,10 @@ def remove(self): og.sim.remove_callback_on_stop(name=f"{self.obj.name}_detach") @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): """ Returns: - str: Unique keyword that defines the metalink associated with this object state + str: Unique keyword that defines the meta link associated with this object state """ return m.ATTACHMENT_LINK_PREFIX @@ -375,7 +375,7 @@ def _disable_collision_between_child_and_parent(self, child, parent): 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. + # Otherwise, the parent meta link will oscillate due to the gravity force of the child. if parent.kinematic_only: child.disable_gravity() diff --git a/omnigibson/object_states/contains.py b/omnigibson/object_states/contains.py index e95bb55d7..7a5921818 100644 --- a/omnigibson/object_states/contains.py +++ b/omnigibson/object_states/contains.py @@ -37,7 +37,7 @@ def __init__(self, obj): self._compute_info = None # Intermediate computation information to store @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.CONTAINER_LINK_PREFIX def _get_value(self, system): diff --git a/omnigibson/object_states/factory.py b/omnigibson/object_states/factory.py index 9df7c96ab..0e58d8ac0 100644 --- a/omnigibson/object_states/factory.py +++ b/omnigibson/object_states/factory.py @@ -200,11 +200,11 @@ def get_states_by_dependency_order(states=None): return list(reversed(list(nx.algorithms.topological_sort(get_state_dependency_graph(states))))) -# Define all metalinks -METALINK_PREFIXES = set() +# Define all meta links +META_LINK_TYPES = set() for state in get_states_by_dependency_order(): if issubclass(state, LinkBasedStateMixin): try: - METALINK_PREFIXES.add(state.metalink_prefix) + META_LINK_TYPES.add(state.meta_link_type) except NotImplementedError: pass diff --git a/omnigibson/object_states/heat_source_or_sink.py b/omnigibson/object_states/heat_source_or_sink.py index 3644bfab1..46ff1391f 100644 --- a/omnigibson/object_states/heat_source_or_sink.py +++ b/omnigibson/object_states/heat_source_or_sink.py @@ -116,12 +116,12 @@ def is_compatible_asset(cls, prim, **kwargs): return True, None @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.HEATSOURCE_LINK_PREFIX @classmethod - def requires_metalink(cls, **kwargs): - # No metalink required if inside + def requires_meta_link(cls, **kwargs): + # No meta link required if inside return not kwargs.get("requires_inside", False) @property @@ -251,7 +251,7 @@ def overlap_callback(hit): affected_objects.remove(obj) else: - # Position is either the AABB center of the default link or the metalink position itself + # Position is either the AABB center of the default link or the meta link position itself heat_source_pos = ( self.link.aabb_center if self.link == self._default_link diff --git a/omnigibson/object_states/link_based_state_mixin.py b/omnigibson/object_states/link_based_state_mixin.py index abf59a827..0dc1dae6b 100644 --- a/omnigibson/object_states/link_based_state_mixin.py +++ b/omnigibson/object_states/link_based_state_mixin.py @@ -22,17 +22,17 @@ def is_compatible(cls, obj, **kwargs): if not compatible: return compatible, reason - # Check whether this state requires metalink - if not cls.requires_metalink(**kwargs): + # Check whether this state requires meta link + if not cls.requires_meta_link(**kwargs): return True, None - metalink_prefix = cls.metalink_prefix + meta_link_type = cls.meta_link_type for link in obj.links.values(): - if link.is_meta_link and link.meta_link_type == metalink_prefix: + if link.is_meta_link and link.meta_link_type == meta_link_type: return True, None return ( False, - f"LinkBasedStateMixin {cls.__name__} requires meta link with type {cls.metalink_prefix} " + f"LinkBasedStateMixin {cls.__name__} requires meta link with type {cls.meta_link_type} " f"for obj {obj.name} but none was found! To get valid compatible object models, please use " f"omnigibson.utils.asset_utils.get_all_object_category_models_with_abilities(...)", ) @@ -44,38 +44,38 @@ def is_compatible_asset(cls, prim, **kwargs): if not compatible: return compatible, reason - # Check whether this state requires metalink - if not cls.requires_metalink(**kwargs): + # Check whether this state requires meta link + if not cls.requires_meta_link(**kwargs): return True, None - metalink_prefix = cls.metalink_prefix + meta_link_type = cls.meta_link_type for child in prim.GetChildren(): if child.GetTypeName() == "Xform": if ( child.HasAttribute("ig:metaLinkType") - and child.GetAttribute("ig:metaLinkType").Get() == metalink_prefix + and child.GetAttribute("ig:metaLinkType").Get() == meta_link_type ): return True, None return ( False, - f"LinkBasedStateMixin {cls.__name__} requires meta link with prefix {cls.metalink_prefix} " + f"LinkBasedStateMixin {cls.__name__} requires meta link with prefix {cls.meta_link_type} " f"for asset prim {prim.GetName()} but none was found! To get valid compatible object models, " f"please use omnigibson.utils.asset_utils.get_all_object_category_models_with_abilities(...)", ) @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): """ Returns: - str: Unique keyword that defines the metalink associated with this object state + str: Unique keyword that defines the meta link associated with this object state """ NotImplementedError() @classmethod - def requires_metalink(cls, **kwargs): + def requires_meta_link(cls, **kwargs): """ Returns: - Whether an object state instantiated with constructor arguments **kwargs will require a metalink + Whether an object state instantiated with constructor arguments **kwargs will require a meta link or not """ # True by default @@ -94,7 +94,7 @@ def link(self): def links(self): """ Returns: - dict: mapping from link names to links that match the metalink_prefix + dict: mapping from link names to links that match the meta_link_type """ return self._links @@ -103,7 +103,7 @@ def _default_link(self): """ Returns: None or RigidPrim: If supported, the fallback link associated with this link-based state if - no valid metalink is found + no valid meta link is found """ # No default link by default return None @@ -111,11 +111,9 @@ def _default_link(self): def initialize_link_mixin(self): assert not self._initialized - # TODO: Extend logic to account for multiple instances of the same metalink? e.g: _0, _1, ... suffixes + # TODO: Extend logic to account for multiple instances of the same meta link? e.g: _0, _1, ... suffixes for name, link in self.obj.links.items(): - if self.metalink_prefix in name or ( - self._default_link is not None and link.name == self._default_link.name - ): + if self.meta_link_type in name or (self._default_link is not None and link.name == self._default_link.name): self._links[name] = link # Make sure the scale is similar if the link is not a cloth prim if not isinstance(link, ClothPrim): diff --git a/omnigibson/object_states/on_fire.py b/omnigibson/object_states/on_fire.py index 65695f6b7..5d246fe0a 100644 --- a/omnigibson/object_states/on_fire.py +++ b/omnigibson/object_states/on_fire.py @@ -60,8 +60,8 @@ def __init__( self.ignition_temperature = ignition_temperature @classmethod - def requires_metalink(cls, **kwargs): - # Does not require metalink to be specified + def requires_meta_link(cls, **kwargs): + # Does not require meta link to be specified return False @property diff --git a/omnigibson/object_states/particle_modifier.py b/omnigibson/object_states/particle_modifier.py index 053baf1b6..6f016f2c5 100644 --- a/omnigibson/object_states/particle_modifier.py +++ b/omnigibson/object_states/particle_modifier.py @@ -410,7 +410,7 @@ def overlap_callback(hit): len(self.link.visual_meshes) == 1 ), f"Expected only a single projection mesh for {self.link}, got: {len(self.link.visual_meshes)}" - # Make sure the mesh is translated so that its tip lies at the metalink origin, and rotated so the vector + # Make sure the mesh is translated so that its tip lies at the meta link origin, and rotated so the vector # from tip to tail faces the positive x axis z_offset = ( 0.0 @@ -924,12 +924,12 @@ def requires_overlap(self): return False @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.REMOVAL_LINK_PREFIX @classmethod - def requires_metalink(cls, **kwargs): - # No metalink required for adjacency + def requires_meta_link(cls, **kwargs): + # No meta link required for adjacency return kwargs.get("method", ParticleModifyMethod.ADJACENCY) != ParticleModifyMethod.ADJACENCY @property @@ -1088,15 +1088,15 @@ def _initialize(self): # Make sure the meta mesh is aligned with the meta link if visualizing # This corresponds to checking (a) position of tip of projection mesh should align with origin of - # metalink, and (b) zero relative orientation between the metalink and the projection mesh + # meta link, and (b) zero relative orientation between the meta link and the projection mesh local_pos, local_quat = self.projection_mesh.get_position_orientation(frame="parent") assert th.all( th.isclose(local_pos + th.tensor([0, 0, height / 2.0]), th.zeros_like(local_pos)) - ), "Projection mesh tip should align with metalink position!" + ), "Projection mesh tip should align with meta link position!" local_euler = T.quat2euler(local_quat) assert th.all( th.isclose(local_euler, th.zeros_like(local_euler)) - ), "Projection mesh orientation should align with metalink orientation!" + ), "Projection mesh orientation should align with meta link orientation!" # Store which method to use for sampling particle locations if self._sample_with_raycast: @@ -1492,12 +1492,12 @@ def projection_is_active(self): return self.projection_emitter.GetProperty("inputs:active").Get() @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.APPLICATION_LINK_PREFIX @classmethod - def requires_metalink(cls, **kwargs): - # No metalink required for adjacency + def requires_meta_link(cls, **kwargs): + # No meta link required for adjacency return kwargs.get("method", ParticleModifyMethod.ADJACENCY) != ParticleModifyMethod.ADJACENCY @classmethod diff --git a/omnigibson/object_states/particle_source_or_sink.py b/omnigibson/object_states/particle_source_or_sink.py index e4ef621f0..4c4bb09a1 100644 --- a/omnigibson/object_states/particle_source_or_sink.py +++ b/omnigibson/object_states/particle_source_or_sink.py @@ -36,7 +36,7 @@ class ParticleSource(ParticleApplier): """ ParticleApplier where physical particles are spawned continuously in a cylindrical fashion from the - metalink pose. + meta link pose. Args: obj (StatefulObject): Object to which this state will be applied @@ -121,8 +121,8 @@ def _get_max_particles_limit_per_step(self, system): return m.MAX_SOURCE_PARTICLES_PER_STEP @classmethod - def requires_metalink(cls, **kwargs): - # Always requires metalink since projection is used + def requires_meta_link(cls, **kwargs): + # Always requires meta link since projection is used return True @property @@ -131,7 +131,7 @@ def visualize(self): return False @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.SOURCE_LINK_PREFIX @property @@ -146,7 +146,7 @@ def physical_particle_modification_limit(self): class ParticleSink(ParticleRemover): """ ParticleRemover where physical particles are removed continuously within a cylindrical volume located - at the metalink pose. + at the meta link pose. Args: obj (StatefulObject): Object to which this state will be applied @@ -235,12 +235,12 @@ def requires_overlap(self): return False @classmethod - def requires_metalink(cls, **kwargs): - # Always requires metalink since projection is used + def requires_meta_link(cls, **kwargs): + # Always requires meta link since projection is used return True @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.SINK_LINK_PREFIX @property diff --git a/omnigibson/object_states/toggle.py b/omnigibson/object_states/toggle.py index 297a37cae..cc63a7e4c 100644 --- a/omnigibson/object_states/toggle.py +++ b/omnigibson/object_states/toggle.py @@ -78,7 +78,7 @@ def global_update(cls): cls._finger_contact_objs.add(obj) @classproperty - def metalink_prefix(cls): + def meta_link_type(cls): return m.TOGGLE_LINK_PREFIX def _get_value(self): diff --git a/omnigibson/objects/stateful_object.py b/omnigibson/objects/stateful_object.py index 54e3ba96f..3c0eef163 100644 --- a/omnigibson/objects/stateful_object.py +++ b/omnigibson/objects/stateful_object.py @@ -296,11 +296,11 @@ def _create_emitter_apis(self, emitter_type): emitter_config = {} bbox_extent_local = self.native_bbox if hasattr(self, "native_bbox") else self.aabb_extent / self.scale if emitter_type == EmitterType.FIRE: - fire_at_metalink = True + fire_at_meta_link = True if OnFire in self.states: # Note whether the heat source link is explicitly set link = self.states[OnFire].link - fire_at_metalink = link != self.root_link + fire_at_meta_link = link != self.root_link elif HeatSourceOrSink in self.states: # Only apply fire to non-root-link (i.e.: explicitly specified) heat source links # Otherwise, immediately return @@ -313,7 +313,7 @@ def _create_emitter_apis(self, emitter_type): emitter_config["name"] = "flowEmitterSphere" emitter_config["type"] = "FlowEmitterSphere" emitter_config["position"] = ( - (0.0, 0.0, 0.0) if fire_at_metalink else (0.0, 0.0, bbox_extent_local[2] * m.FIRE_EMITTER_HEIGHT_RATIO) + (0.0, 0.0, 0.0) if fire_at_meta_link else (0.0, 0.0, bbox_extent_local[2] * m.FIRE_EMITTER_HEIGHT_RATIO) ) emitter_config["fuel"] = 0.6 emitter_config["coupleRateFuel"] = 1.2 @@ -391,7 +391,7 @@ def _create_emitter_apis(self, emitter_type): if emitter_type == EmitterType.FIRE: # Radius is in the absolute world coordinate even though the fire is under the link frame. # In other words, scaling the object doesn't change the fire radius. - if fire_at_metalink: + if fire_at_meta_link: # TODO: get radius of heat_source_link from metadata. radius = 0.05 else: diff --git a/omnigibson/utils/asset_utils.py b/omnigibson/utils/asset_utils.py index 37a1d4169..6ee0f6a48 100644 --- a/omnigibson/utils/asset_utils.py +++ b/omnigibson/utils/asset_utils.py @@ -297,16 +297,16 @@ def supports_abilities(info, obj_prim): return valid_models -def get_attachment_metalinks(category, model): +def get_attachment_meta_links(category, model): """ - Get attachment metalinks for an object model + Get attachment meta links for an object model Args: category (str): Object category name model (str): Object model name Returns: - list of str: all attachment metalinks for the object model + list of str: all attachment meta links for the object model """ # Avoid circular imports from omnigibson.object_states import AttachedTo @@ -317,12 +317,12 @@ def get_attachment_metalinks(category, model): with decrypted(usd_path) as fpath: stage = lazy.pxr.Usd.Stage.Open(fpath) prim = stage.GetDefaultPrim() - attachment_metalinks = [] + attachment_meta_links = [] for child in prim.GetChildren(): if child.GetTypeName() == "Xform": - if AttachedTo.metalink_prefix in child.GetName(): - attachment_metalinks.append(child.GetName()) - return attachment_metalinks + if AttachedTo.meta_link_type in child.GetName(): + attachment_meta_links.append(child.GetName()) + return attachment_meta_links def get_og_assets_version(): diff --git a/omnigibson/utils/bddl_utils.py b/omnigibson/utils/bddl_utils.py index 829916ca7..2fe661632 100644 --- a/omnigibson/utils/bddl_utils.py +++ b/omnigibson/utils/bddl_utils.py @@ -21,7 +21,7 @@ from omnigibson.utils.asset_utils import ( get_all_object_categories, get_all_object_category_models_with_abilities, - get_attachment_metalinks, + get_attachment_meta_links, ) from omnigibson.utils.constants import PrimType from omnigibson.utils.python_utils import Wrapper @@ -1111,7 +1111,7 @@ def _consolidate_room_instance(self, filtered_object_scope, condition_type): def _filter_model_choices_by_attached_states(self, model_choices, category, obj_inst): # If obj_inst is a child object that depends on a parent object that has been imported or exists in the scene, - # we filter in only models that match the parent object's attachment metalinks. + # we filter in only models that match the parent object's attachment meta links. if obj_inst in self._attached_objects: parent_insts = self._attached_objects[obj_inst] parent_objects = [] @@ -1144,14 +1144,14 @@ def can_attach(child_attachment_links, parent_attachment_links): # Filter out models that don't support the attached states new_model_choices = set() for model_choice in model_choices: - child_attachment_links = get_attachment_metalinks(category, model_choice) + child_attachment_links = get_attachment_meta_links(category, model_choice) # The child model choice needs to be able to attach to all parent instances. # For in-room parent instances, there might be multiple parent objects (e.g. different wall nails), # and the child object needs to be able to attach to at least one of them. if all( any( can_attach( - child_attachment_links, get_attachment_metalinks(parent_obj.category, parent_obj.model) + child_attachment_links, get_attachment_meta_links(parent_obj.category, parent_obj.model) ) for parent_obj in parent_objs_per_inst ) @@ -1167,7 +1167,7 @@ def can_attach(child_attachment_links, parent_attachment_links): # Filter out models that don't support the attached states new_model_choices = set() for model_choice in model_choices: - if len(get_attachment_metalinks(category, model_choice)) > 0: + if len(get_attachment_meta_links(category, model_choice)) > 0: new_model_choices.add(model_choice) return new_model_choices From 92a7684e6e636e4057535460db9306eb50b0dd9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 7 Feb 2025 15:31:49 -0800 Subject: [PATCH 18/38] Standardize spelling of meta link --- omnigibson/object_states/particle_source_or_sink.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/object_states/particle_source_or_sink.py b/omnigibson/object_states/particle_source_or_sink.py index 4c4bb09a1..cb8df37a5 100644 --- a/omnigibson/object_states/particle_source_or_sink.py +++ b/omnigibson/object_states/particle_source_or_sink.py @@ -11,7 +11,7 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -# Metalink naming prefixes +# Meta link naming prefixes m.SOURCE_LINK_PREFIX = "particlesource" m.SINK_LINK_PREFIX = "particlesink" From 9eb10ff346fcbe2b541319d9355fe8e53938b74b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 7 Feb 2025 15:41:28 -0800 Subject: [PATCH 19/38] Fix some issues --- omnigibson/utils/asset_conversion_utils.py | 27 ++++++++++++++++------ state_fold.py | 2 +- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 5b7bfd1b4..0662c2f57 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -916,11 +916,15 @@ def import_obj_metadata(usd_path, obj_category, obj_model, dataset_root, import_ # Set the purpose of the visual meshes to be guide visual_prim = meta_prim.GetChild("visuals") - purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_prim).CreatePurposeAttr() - purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) - for visual_mesh in visual_prim.GetChildren(): - purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_mesh).CreatePurposeAttr() - purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) + if visual_prim.IsValid(): + # If it's an imageable, set the purpose to guide + if visual_prim.GetTypeName() == "Mesh": + purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_prim).CreatePurposeAttr() + purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) + for visual_mesh in visual_prim.GetChildren(): + if visual_mesh.GetTypeName() == "Mesh": + purpose_attr = lazy.pxr.UsdGeom.Imageable(visual_mesh).CreatePurposeAttr() + purpose_attr.Set(lazy.pxr.UsdGeom.Tokens.guide) log.debug("Done processing meta links") @@ -1422,6 +1426,15 @@ def _add_meta_links_to_urdf(urdf_path, obj_category, obj_model, dataset_root): ), f"meta_link_name {meta_link_name} not in {_ALLOWED_META_TYPES}" for ml_id, attrs_list in ml_attrs.items(): + # If the attrs list is a dictionary (legacy format), convert it to a list + if isinstance(attrs_list, dict): + keys = [int(k) for k in attrs_list.keys()] + assert set(keys) == set( + range(len(keys)) + ), f"Expected keys to be 0-indexed integers, but got {keys}" + int_key_dict = {int(k): v for k, v in attrs_list.items()} + attrs_list = [int_key_dict[i] for i in range(len(keys))] + if len(attrs_list) > 0: if _ALLOWED_META_TYPES[meta_link_name] != "dimensionless": # If not dimensionless, we create one meta link for a list of meshes below it @@ -1484,7 +1497,7 @@ def convert_scene_urdf_to_json(urdf, json_path): # Play the simulator, then save og.sim.play() Path(os.path.dirname(json_path)).mkdir(parents=True, exist_ok=True) - og.sim.save(json_path=json_path) + og.sim.save(json_paths=[json_path]) # Load the json, remove the init_info because we don't need it, then save it again with open(json_path, "r") as f: @@ -1535,7 +1548,7 @@ def _load_scene_from_urdf(urdf): name=obj_name, **obj_info["cfg"], ) - og.sim.import_object(obj) + scene.add_object(obj) obj.set_bbox_center_position_orientation(position=obj_info["bbox_pos"], orientation=obj_info["bbox_quat"]) except Exception as e: raise ValueError(f"Failed to load object {obj_name}") from e diff --git a/state_fold.py b/state_fold.py index 20cbfb9e8..d9eede701 100644 --- a/state_fold.py +++ b/state_fold.py @@ -19,7 +19,7 @@ def main(random_selection=False, headless=False, short_exec=False): og.log.info(f"Demo {__file__}\n " + "*" * 80 + "\n Description:\n" + main.__doc__ + "*" * 80) cloth_category_models = [ - ("jeans", "nmvvil"), + ("jeans", "pvzxyp"), ] for cloth in cloth_category_models: From cf40a1ed2624e20307938f03d581884cdceb4f16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 7 Feb 2025 17:44:55 -0800 Subject: [PATCH 20/38] Don't default to default purpose, accept w/e is in the USD --- omnigibson/prims/geom_prim.py | 3 --- state_fold.py | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/omnigibson/prims/geom_prim.py b/omnigibson/prims/geom_prim.py index 1e16e65e3..17c8832a4 100644 --- a/omnigibson/prims/geom_prim.py +++ b/omnigibson/prims/geom_prim.py @@ -50,9 +50,6 @@ def _post_load(self): # run super first super()._post_load() - # By default, GeomPrim shows up in the rendering. - self.purpose = "default" - @property def purpose(self): """ diff --git a/state_fold.py b/state_fold.py index d9eede701..6e4b122b7 100644 --- a/state_fold.py +++ b/state_fold.py @@ -9,7 +9,7 @@ # Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True -gm.DATASET_PATH = "/scr/cloth-test" +# gm.DATASET_PATH = "/scr/cloth-test" def main(random_selection=False, headless=False, short_exec=False): From 73d4a4c412866794fede9628f3dc4388793fd880 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 7 Feb 2025 17:45:19 -0800 Subject: [PATCH 21/38] Fixes to USD conversion --- omnigibson/utils/asset_conversion_utils.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 0662c2f57..e78be9282 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -654,7 +654,8 @@ def _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_ if is_light: # Create a light light_type = _LIGHT_MAPPING[mesh_info["type"]] - prim_path = f"/{obj_model}/meta__{link_name}_lights_{link_id}_0_link/light_{i}" + stage.DefinePrim(f"/{obj_model}/meta__{link_name}_lights_{link_id}_0_link/lights", "Scope") + prim_path = f"/{obj_model}/meta__{link_name}_lights_{link_id}_0_link/lights/light_{i}" prim = getattr(lazy.pxr.UsdLux, f"{light_type}Light").Define(stage, prim_path).GetPrim() lazy.pxr.UsdLux.ShapingAPI.Apply(prim).GetShapingConeAngleAttr().Set(180.0) else: @@ -663,7 +664,9 @@ def _generate_meshes_for_primitive_meta_links(stage, obj_model, link_name, meta_ else: # Create a primitive shape mesh_type = mesh_info["type"].capitalize() if mesh_info["type"] != "box" else "Cube" - prim_path = f"/{obj_model}/meta__{link_name}_{meta_link_type}_{link_id}_0_link/mesh_{i}" + # Create the visuals prim + stage.DefinePrim(f"/{obj_model}/meta__{link_name}_{meta_link_type}_{link_id}_0_link/visuals", "Scope") + prim_path = f"/{obj_model}/meta__{link_name}_{meta_link_type}_{link_id}_0_link/visuals/mesh_{i}" assert hasattr(lazy.pxr.UsdGeom, mesh_type) # togglebutton has to be a sphere if meta_link_type in ["togglebutton"]: @@ -898,11 +901,15 @@ def import_obj_metadata(usd_path, obj_category, obj_model, dataset_root, import_ # Here we want to include every link that has the meta__ prefix. # This includes meta links that get added into the URDF in earlier # stages. - meta_link_prims = [p for p in prim.GetChildren() if p.GetName().startswith("meta__")] + meta_link_prims = [ + p for p in prim.GetChildren() if p.GetName().startswith("meta__") and p.GetName().endswith("_link") + ] for meta_prim in meta_link_prims: # Get meta link information unparsed_meta = meta_prim.GetName()[6:-5] # remove meta__ and _link - link_name, meta_link_type, link_id, link_sub_id = unparsed_meta.rsplit("_", 3) + meta_parts = unparsed_meta.rsplit("_", 3) + assert len(meta_parts) == 4, f"Invalid meta link name: {unparsed_meta}" + link_name, meta_link_type, link_id, link_sub_id = meta_parts # Add the is_meta_link, meta_link_type, and meta_link_id attributes meta_prim.CreateAttribute("ig:isMetaLink", lazy.pxr.Sdf.ValueTypeNames.Bool) @@ -1464,7 +1471,7 @@ def _add_meta_links_to_urdf(urdf_path, obj_category, obj_model, dataset_root): # Create meta link _create_urdf_meta_link( root_element=root, - meta_link_name=f"{meta_link_name}_{ml_id}_{i}", + meta_link_name=f"meta__{parent_link_name}_{meta_link_name}_{ml_id}_{i}", parent_link_name=parent_link_name, pos=pos, rpy=T.quat2euler(quat), From bb7f0661be7eb34bbf6f401422de47853e8ffd71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 10 Feb 2025 21:41:51 -0800 Subject: [PATCH 22/38] Some more fixes --- omnigibson/__init__.py | 26 +++++++++++----------- omnigibson/prims/geom_prim.py | 4 ++++ omnigibson/prims/material_prim.py | 14 ++++++++++++ omnigibson/utils/asset_conversion_utils.py | 1 + 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/omnigibson/__init__.py b/omnigibson/__init__.py index 36097227c..62f0273d9 100644 --- a/omnigibson/__init__.py +++ b/omnigibson/__init__.py @@ -82,15 +82,15 @@ def clear( import omnigibson.lazy as lazy # First save important simulator settings - # init_kwargs = dict( - # gravity=sim.gravity if gravity is None else gravity, - # physics_dt=sim.get_physics_dt() if physics_dt is None else physics_dt, - # rendering_dt=sim.get_rendering_dt() if rendering_dt is None else rendering_dt, - # sim_step_dt=sim.get_sim_step_dt() if sim_step_dt is None else sim_step_dt, - # viewer_width=sim.viewer_width if viewer_width is None else viewer_width, - # viewer_height=sim.viewer_height if viewer_height is None else viewer_height, - # device=sim.device if device is None else device, - # ) + init_kwargs = dict( + gravity=sim.gravity if gravity is None else gravity, + physics_dt=sim.get_physics_dt() if physics_dt is None else physics_dt, + rendering_dt=sim.get_rendering_dt() if rendering_dt is None else rendering_dt, + sim_step_dt=sim.get_sim_step_dt() if sim_step_dt is None else sim_step_dt, + viewer_width=sim.viewer_width if viewer_width is None else viewer_width, + viewer_height=sim.viewer_height if viewer_height is None else viewer_height, + device=sim.device if device is None else device, + ) # First let the simulator clear everything it owns. sim._partial_clear() @@ -101,12 +101,12 @@ def clear( lazy.omni.isaac.core.simulation_context.SimulationContext.clear_instance() # Then relaunch the simulator. - launch() # **init_kwargs) + launch(**init_kwargs) # Check that the device remains the same - # assert ( - # sim.device == init_kwargs["device"] - # ), f"Device changed from {init_kwargs['device']} to {sim.device} after clear." + assert ( + sim.device == init_kwargs["device"] + ), f"Device changed from {init_kwargs['device']} to {sim.device} after clear." def cleanup(*args, **kwargs): diff --git a/omnigibson/prims/geom_prim.py b/omnigibson/prims/geom_prim.py index 17c8832a4..318ae80df 100644 --- a/omnigibson/prims/geom_prim.py +++ b/omnigibson/prims/geom_prim.py @@ -50,6 +50,10 @@ def _post_load(self): # run super first super()._post_load() + # Temporarily set the opacity threshold to be 0.1. This will be automated in later exports. + if self.material: + self.material.opacity_threshold = 0.1 + @property def purpose(self): """ diff --git a/omnigibson/prims/material_prim.py b/omnigibson/prims/material_prim.py index 3098658a3..6bd0e9b42 100644 --- a/omnigibson/prims/material_prim.py +++ b/omnigibson/prims/material_prim.py @@ -231,6 +231,20 @@ def set_input(self, inp, val): val (any): Value to set for the input. This should be the valid type for that attribute. """ # Make sure the input exists first, so we avoid segfaults with "invalid null prim" + if inp not in self.shader_input_names: + input_type = None + if isinstance(val, str): + input_type = lazy.pxr.Sdf.ValueTypeNames.String + elif isinstance(val, int): + input_type = lazy.pxr.Sdf.ValueTypeNames.Int + elif isinstance(val, float): + input_type = lazy.pxr.Sdf.ValueTypeNames.Float + elif isinstance(val, bool): + input_type = lazy.pxr.Sdf.ValueTypeNames.Bool + else: + raise ValueError(f"Invalid input type {type(val)} for input {inp}") + + lazy.omni.usd.create_material_input(self.prim, "opacity_threshold", val, input_type) assert ( inp in self.shader_input_names ), f"Got invalid shader input to set! Current inputs are: {self.shader_input_names}. Got: {inp}" diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index e78be9282..8ca132137 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -287,6 +287,7 @@ def _set_mtl_opacity(mtl_prim, texture): # This defaults to some other value, which takes opaque black channels in the # image to be fully transparent. This is not what we want. lazy.omni.usd.create_material_input(mtl_prim, "opacity_mode", 0, lazy.pxr.Sdf.ValueTypeNames.Int) + lazy.omni.usd.create_material_input(mtl_prim, "opacity_threshold", 0.1, lazy.pxr.Sdf.ValueTypeNames.Float) # Verify it was set shade = lazy.omni.usd.get_shader_from_material(mtl_prim) From 37261be02a61347265fa312ea4d5564cec6ce2c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 10 Feb 2025 22:00:05 -0800 Subject: [PATCH 23/38] Add backwards compatibility --- .../object_states/link_based_state_mixin.py | 10 +++++- omnigibson/prims/rigid_prim.py | 36 ++++++++++++++++--- 2 files changed, 41 insertions(+), 5 deletions(-) diff --git a/omnigibson/object_states/link_based_state_mixin.py b/omnigibson/object_states/link_based_state_mixin.py index 0dc1dae6b..d09d40a2a 100644 --- a/omnigibson/object_states/link_based_state_mixin.py +++ b/omnigibson/object_states/link_based_state_mixin.py @@ -50,12 +50,17 @@ def is_compatible_asset(cls, prim, **kwargs): meta_link_type = cls.meta_link_type for child in prim.GetChildren(): if child.GetTypeName() == "Xform": + # With the new format, we can know for sure by checking the meta link type if ( child.HasAttribute("ig:metaLinkType") and child.GetAttribute("ig:metaLinkType").Get() == meta_link_type ): return True, None + # Until the next dataset release, also accept the old format + # TODO: Remove this block after the next dataset release + if meta_link_type in child.GetName(): + return True, None return ( False, f"LinkBasedStateMixin {cls.__name__} requires meta link with prefix {cls.meta_link_type} " @@ -113,7 +118,10 @@ def initialize_link_mixin(self): # TODO: Extend logic to account for multiple instances of the same meta link? e.g: _0, _1, ... suffixes for name, link in self.obj.links.items(): - if self.meta_link_type in name or (self._default_link is not None and link.name == self._default_link.name): + is_appropriate_meta_link = link.is_meta_link and link.meta_link_type == self.meta_link_type + # TODO: Remove support for this default meta link logic after the next dataset release + is_default_link = self._default_link is not None and link.name == self._default_link.name + if is_appropriate_meta_link or is_default_link: self._links[name] = link # Make sure the scale is similar if the link is not a cloth prim if not isinstance(link, ClothPrim): diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 9d4c7cca4..96fbd41f2 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -1,5 +1,6 @@ import math from functools import cached_property +import re from typing import Literal import torch as th @@ -29,6 +30,7 @@ m.DEFAULT_CONTACT_OFFSET = 0.001 m.DEFAULT_REST_OFFSET = 0.0 +m.META_LINK_PATTERN = re.compile(r"(\w+)_(\d+)_(\d+)_link") class RigidPrim(XFormPrim): @@ -798,22 +800,48 @@ def visual_aabb_center(self): @cached_property def is_meta_link(self): - return self.prim.HasAttribute("ig:isMetaLink") and self.get_attribute("ig:isMetaLink") + # Check using the new format first + new_format = self.prim.HasAttribute("ig:isMetaLink") and self.get_attribute("ig:isMetaLink") + if new_format: + return True + + # Check using the old format. + # TODO: Remove this after the next dataset release + old_format = m.META_LINK_PATTERN.fullmatch(self.name) is not None + if old_format: + return True + + return False @cached_property def meta_link_type(self): assert self.is_meta_link, f"{self.name} is not a meta link" - return self.get_attribute("ig:metaLinkType") + if self.prim.HasAttribute("ig:metaLinkType"): + return self.get_attribute("ig:metaLinkType") + + # Check using the old format. + # TODO: Remove this after the next dataset release + return m.META_LINK_PATTERN.fullmatch(self.name).group(1) @cached_property def meta_link_id(self): assert self.is_meta_link, f"{self.name} is not a meta link" - return self.get_attribute("ig:metaLinkId") + if self.prim.HasAttribute("ig:metaLinkId"): + return self.get_attribute("ig:metaLinkId") + + # Check using the old format. + # TODO: Remove this after the next dataset release + return m.META_LINK_PATTERN.fullmatch(self.name).group(2) @cached_property def meta_link_sub_id(self): assert self.is_meta_link, f"{self.name} is not a meta link" - return self.get_attribute("ig:metaLinkSubId") + if self.prim.HasAttribute("ig:metaLinkSubId"): + return int(self.get_attribute("ig:metaLinkSubId")) + + # Check using the old format. + # TODO: Remove this after the next dataset release + return int(m.META_LINK_PATTERN.fullmatch(self.name).group(3)) def enable_gravity(self): """ From 923d258213c0096820c6be177c514aa4cd2f7102 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Mon, 10 Feb 2025 22:14:57 -0800 Subject: [PATCH 24/38] Add cloth config demo --- .../objects/view_cloth_configurations.py | 119 ++++++++++++++++++ state_fold.py | 98 --------------- 2 files changed, 119 insertions(+), 98 deletions(-) create mode 100644 omnigibson/examples/objects/view_cloth_configurations.py delete mode 100644 state_fold.py diff --git a/omnigibson/examples/objects/view_cloth_configurations.py b/omnigibson/examples/objects/view_cloth_configurations.py new file mode 100644 index 000000000..7f01bc5fa --- /dev/null +++ b/omnigibson/examples/objects/view_cloth_configurations.py @@ -0,0 +1,119 @@ +from omnigibson.utils.asset_utils import get_all_object_categories, get_all_object_category_models +from omnigibson.macros import gm + +import omnigibson as og +import omnigibson.lazy as lazy +from omnigibson.utils.constants import PrimType +from omnigibson.utils.ui_utils import KeyboardEventHandler, choose_from_options +import torch as th +from bddl.object_taxonomy import ObjectTaxonomy + +# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) +gm.ENABLE_OBJECT_STATES = True +gm.USE_GPU_DYNAMICS = True + + +def main(random_selection=False, headless=False, short_exec=False): + """ + Demo showing the multiple configurations stored on each cloth object. + """ + og.log.info(f"Demo {__file__}\n " + "*" * 80 + "\n Description:\n" + main.__doc__ + "*" * 80) + + # Select a category to load + available_obj_categories = get_all_object_categories() + object_taxonomy = ObjectTaxonomy() + cloth_obj_categories = [ + category + for category in available_obj_categories + if object_taxonomy.get_synset_from_category(category) + and "cloth" in object_taxonomy.get_abilities(object_taxonomy.get_synset_from_category(category)) + ] + obj_category = choose_from_options( + options=cloth_obj_categories, name="object category", random_selection=random_selection + ) + + # Select a model to load + available_obj_models = get_all_object_category_models(obj_category) + obj_model = choose_from_options( + options=available_obj_models, name="object model", random_selection=random_selection + ) + + # Create and load this object into the simulator + obj_cfg = { + "type": "DatasetObject", + "name": "cloth", + "category": obj_category, + "model": obj_model, + "prim_type": PrimType.CLOTH, + "position": [0, 0, 0.5], + "orientation": [0, 0, 0, 1], + "load_config": { + "default_configuration": "settled", + }, + } + + # Create the scene config to load -- empty scene + custom cloth object + cfg = { + "scene": { + "type": "Scene", + }, + "objects": [obj_cfg], + } + + # Create the environment + env = og.Environment(configs=cfg) + + # Grab object references + obj = env.scene.object_registry("name", "cloth") + + # Set viewer camera + og.sim.viewer_camera.set_position_orientation( + position=th.tensor([0.46382895, -2.66703958, 1.22616824]), + orientation=th.tensor([0.58779174, -0.00231237, -0.00318273, 0.80900271]), + ) + + def reset_points_to_configuration(configuration): + print(f"Resetting to {configuration} configuration") + obj.root_link.reset_points_to_configuration(configuration) + obj.set_position_orientation(position=th.zeros(3), orientation=th.tensor([0.0, 0.0, 0.0, 1.0])) + obj.set_position_orientation( + position=th.tensor([0, 0, obj.aabb_extent[2] / 2.0 - obj.aabb_center[2]]), + orientation=th.tensor([0.0, 0.0, 0.0, 1.0]), + ) + + KeyboardEventHandler.initialize() + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.Q, + callback_fn=lambda: reset_points_to_configuration("default"), + ) + print("Press Q to reset to default configuration") + + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.W, + callback_fn=lambda: reset_points_to_configuration("settled"), + ) + print("Press W to reset to settled configuration") + + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.E, + callback_fn=lambda: reset_points_to_configuration("folded"), + ) + print("Press E to reset to folded configuration") + + KeyboardEventHandler.add_keyboard_callback( + key=lazy.carb.input.KeyboardInput.R, + callback_fn=lambda: reset_points_to_configuration("crumpled"), + ) + print("Press R to reset to crumpled configuration") + + # Step through the environment + max_steps = 100 if short_exec else 10000 + for i in range(max_steps): + env.step(th.empty(0)) + + # Always close the environment at the end + og.clear() + + +if __name__ == "__main__": + main() diff --git a/state_fold.py b/state_fold.py deleted file mode 100644 index 6e4b122b7..000000000 --- a/state_fold.py +++ /dev/null @@ -1,98 +0,0 @@ -from omnigibson.utils.constants import PrimType -from omnigibson.macros import gm - -import omnigibson as og -import omnigibson.lazy as lazy -from omnigibson.utils.ui_utils import KeyboardEventHandler -import torch as th - -# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth) -gm.ENABLE_OBJECT_STATES = True -gm.USE_GPU_DYNAMICS = True -# gm.DATASET_PATH = "/scr/cloth-test" - - -def main(random_selection=False, headless=False, short_exec=False): - """ - Demo of cloth objects that can potentially be folded. - """ - og.log.info(f"Demo {__file__}\n " + "*" * 80 + "\n Description:\n" + main.__doc__ + "*" * 80) - - cloth_category_models = [ - ("jeans", "pvzxyp"), - ] - - for cloth in cloth_category_models: - category = cloth[0] - model = cloth[1] - print(f"\nCategory: {category}, Model: {model}!!!!!!!!!!!!!!!!!!!!!!!!!!") - # Create the scene config to load -- empty scene + custom cloth object - cfg = { - "scene": { - "type": "Scene", - }, - "objects": [ - { - "type": "DatasetObject", - "name": model, - "category": category, - "model": model, - "prim_type": PrimType.CLOTH, - "abilities": {"cloth": {}}, - "position": [0, 0, 0.5], - "orientation": [0, 0, 0, 1], - "load_config": { - "default_configuration": "settled", - }, - }, - ], - } - - # Create the environment - env = og.Environment(configs=cfg) - - # Grab object references - obj = env.scene.object_registry("name", model) - - # Set viewer camera - og.sim.viewer_camera.set_position_orientation( - position=th.tensor([0.46382895, -2.66703958, 1.22616824]), - orientation=th.tensor([0.58779174, -0.00231237, -0.00318273, 0.80900271]), - ) - - def reset_points_to_configuration(configuration): - obj.root_link.reset_points_to_configuration(configuration) - obj.set_position_orientation(position=th.zeros(3), orientation=th.tensor([0.0, 0.0, 0.0, 1.0])) - obj.set_position_orientation( - position=th.tensor([0, 0, obj.aabb_extent[2] / 2.0 - obj.aabb_center[2]]), - orientation=th.tensor([0.0, 0.0, 0.0, 1.0]), - ) - - KeyboardEventHandler.initialize() - KeyboardEventHandler.add_keyboard_callback( - key=lazy.carb.input.KeyboardInput.Q, - callback_fn=lambda: reset_points_to_configuration("default"), - ) - KeyboardEventHandler.add_keyboard_callback( - key=lazy.carb.input.KeyboardInput.W, - callback_fn=lambda: reset_points_to_configuration("settled"), - ) - KeyboardEventHandler.add_keyboard_callback( - key=lazy.carb.input.KeyboardInput.E, - callback_fn=lambda: reset_points_to_configuration("folded"), - ) - KeyboardEventHandler.add_keyboard_callback( - key=lazy.carb.input.KeyboardInput.R, - callback_fn=lambda: reset_points_to_configuration("crumpled"), - ) - - while True: - og.sim.render() - - # Shut down env at the end - print() - env.close() - - -if __name__ == "__main__": - main() From 75546266fb74d83c4bbaab5db80eb4ac5a46ecaa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Tue, 11 Feb 2025 12:45:31 -0800 Subject: [PATCH 25/38] Fix JIT issues --- omnigibson/utils/transform_utils.py | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index 7dcc56aac..aaea779ec 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -1438,7 +1438,13 @@ def delta_rotation_matrix(omega, delta_t): # Skew-symmetric matrix K u_x, u_y, u_z = axis[0], axis[1], axis[2] - K = torch.tensor([[0, -u_z, u_y], [u_z, 0, -u_x], [-u_y, u_x, 0]]) + K = torch.zeros((3, 3)) + K[0, 1] = -u_z + K[0, 2] = u_y + K[1, 0] = u_z + K[1, 2] = -u_x + K[2, 0] = -u_y + K[2, 1] = u_x # Rodrigues' rotation formula R = torch.eye(3) + torch.sin(theta) * K + (1 - torch.cos(theta)) * (K @ K) @@ -1484,14 +1490,21 @@ def euler_intrinsic2mat(euler): torch.tensor: 3x3 rotation matrix """ roll, pitch, yaw = euler[0], euler[1], euler[2] + # Rotation matrix around X-axis - Rx = torch.tensor([[1, 0, 0], [0, torch.cos(roll), -torch.sin(roll)], [0, torch.sin(roll), torch.cos(roll)]]) + euler_x = torch.zeros(3) + euler_x[0] = roll + Rx = euler2mat(euler_x) # Rotation matrix around Y-axis - Ry = torch.tensor([[torch.cos(pitch), 0, torch.sin(pitch)], [0, 1, 0], [-torch.sin(pitch), 0, torch.cos(pitch)]]) + euler_y = torch.zeros(3) + euler_y[1] = pitch + Ry = euler2mat(euler_y) # Rotation matrix around Z-axis - Rz = torch.tensor([[torch.cos(yaw), -torch.sin(yaw), 0], [torch.sin(yaw), torch.cos(yaw), 0], [0, 0, 1]]) + euler_z = torch.zeros(3) + euler_z[2] = yaw + Rz = euler2mat(euler_z) # Combine the rotation matrices # Intrinsic x-y-z is the same as extrinsic z-y-x From 92008241d4597a9dfda5b0a9cc6ae401f044a1ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 11 Feb 2025 13:14:50 -0800 Subject: [PATCH 26/38] Small fixes --- omnigibson/prims/rigid_prim.py | 2 +- omnigibson/utils/transform_utils.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 96fbd41f2..953ce3b92 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -30,7 +30,7 @@ m.DEFAULT_CONTACT_OFFSET = 0.001 m.DEFAULT_REST_OFFSET = 0.0 -m.META_LINK_PATTERN = re.compile(r"(\w+)_(\d+)_(\d+)_link") +m.META_LINK_PATTERN = re.compile(r".*:(\w+)_(\d+)_(\d+)_link") class RigidPrim(XFormPrim): diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py index aaea779ec..81415d746 100644 --- a/omnigibson/utils/transform_utils.py +++ b/omnigibson/utils/transform_utils.py @@ -1473,9 +1473,9 @@ def mat2euler_intrinsic(rmat): # Gimbal lock case pitch = torch.tensor(math.pi / 2 if rmat[0, 2] == 1 else -math.pi / 2) roll = torch.arctan2(rmat[1, 0], rmat[1, 1]) - yaw = torch.tensor(0) # Can set yaw to 0 in gimbal lock + yaw = torch.tensor(0.0) # Can set yaw to 0 in gimbal lock - return torch.tensor([roll, pitch, yaw], dtype=torch.float32) + return torch.stack([roll, pitch, yaw]) @torch.jit.script From 8bc2362ec3ba966f0cb3e44807e756a8a2c051c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 11 Feb 2025 14:29:16 -0800 Subject: [PATCH 27/38] Backwards compatibility for cloth too --- omnigibson/prims/cloth_prim.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index ae35aabf0..13ddf1a9e 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -112,9 +112,15 @@ def _post_load(self): self.save_configuration("default", self.points) # Remesh the object if necessary - if self._load_config.get("force_remesh", False) or ( - self._load_config.get("remesh", True) and not th.allclose(self.scale, th.ones(3)) - ): + force_remesh = self._load_config.get("force_remesh", False) + should_remesh_because_of_scale = self._load_config.get("remesh", True) and not th.allclose( + self.scale, th.ones(3) + ) + # TODO: Remove the legacy check after the next dataset release + should_remesh_because_legacy = self._load_config.get( + "remesh", True + ) and self.get_available_configurations() == ["default"] + if should_remesh_because_of_scale or should_remesh_because_legacy or force_remesh: # Remesh the object if necessary log.warning( f"Remeshing cloth {self.name}. This happens when cloth is loaded with non-unit scale or forced using the forced_remesh argument. It invalidates precached info for settled, folded, and crumpled configurations." From 383981db833ca6bc41b22a6e94d4f76658ff4aad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 11 Feb 2025 15:45:36 -0800 Subject: [PATCH 28/38] Fix meta link pattern --- omnigibson/prims/rigid_prim.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 953ce3b92..372bf271c 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -30,7 +30,7 @@ m.DEFAULT_CONTACT_OFFSET = 0.001 m.DEFAULT_REST_OFFSET = 0.0 -m.META_LINK_PATTERN = re.compile(r".*:(\w+)_(\d+)_(\d+)_link") +m.LEGACY_META_LINK_PATTERN = re.compile(r".*:(\w+)_([A-Za-z0-9]+)_(\d+)_link") class RigidPrim(XFormPrim): @@ -807,7 +807,7 @@ def is_meta_link(self): # Check using the old format. # TODO: Remove this after the next dataset release - old_format = m.META_LINK_PATTERN.fullmatch(self.name) is not None + old_format = m.LEGACY_META_LINK_PATTERN.fullmatch(self.name) is not None if old_format: return True @@ -821,7 +821,7 @@ def meta_link_type(self): # Check using the old format. # TODO: Remove this after the next dataset release - return m.META_LINK_PATTERN.fullmatch(self.name).group(1) + return m.LEGACY_META_LINK_PATTERN.fullmatch(self.name).group(1) @cached_property def meta_link_id(self): @@ -831,7 +831,7 @@ def meta_link_id(self): # Check using the old format. # TODO: Remove this after the next dataset release - return m.META_LINK_PATTERN.fullmatch(self.name).group(2) + return m.LEGACY_META_LINK_PATTERN.fullmatch(self.name).group(2) @cached_property def meta_link_sub_id(self): @@ -841,7 +841,7 @@ def meta_link_sub_id(self): # Check using the old format. # TODO: Remove this after the next dataset release - return int(m.META_LINK_PATTERN.fullmatch(self.name).group(3)) + return int(m.LEGACY_META_LINK_PATTERN.fullmatch(self.name).group(3)) def enable_gravity(self): """ From b92acf08b13c8a697b1b78a7117c4372bf053c29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 11 Feb 2025 16:27:45 -0800 Subject: [PATCH 29/38] Fix accidental hardcoded input name --- omnigibson/prims/material_prim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/prims/material_prim.py b/omnigibson/prims/material_prim.py index 6bd0e9b42..1b05d24e8 100644 --- a/omnigibson/prims/material_prim.py +++ b/omnigibson/prims/material_prim.py @@ -244,7 +244,7 @@ def set_input(self, inp, val): else: raise ValueError(f"Invalid input type {type(val)} for input {inp}") - lazy.omni.usd.create_material_input(self.prim, "opacity_threshold", val, input_type) + lazy.omni.usd.create_material_input(self.prim, inp, val, input_type) assert ( inp in self.shader_input_names ), f"Got invalid shader input to set! Current inputs are: {self.shader_input_names}. Got: {inp}" From 247d1e82c218a4d8cf6ceda78b241702e738d3f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Wed, 12 Feb 2025 01:33:28 -0800 Subject: [PATCH 30/38] Change prefix to meta link type across classes --- omnigibson/object_states/attached_to.py | 4 ++-- omnigibson/object_states/contains.py | 4 ++-- omnigibson/object_states/heat_source_or_sink.py | 4 ++-- omnigibson/object_states/particle_modifier.py | 8 ++++---- omnigibson/object_states/particle_source_or_sink.py | 8 ++++---- omnigibson/object_states/toggle.py | 4 ++-- 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/omnigibson/object_states/attached_to.py b/omnigibson/object_states/attached_to.py index 2e151d291..67f1b0a44 100644 --- a/omnigibson/object_states/attached_to.py +++ b/omnigibson/object_states/attached_to.py @@ -23,7 +23,7 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -m.ATTACHMENT_LINK_PREFIX = "attachment" +m.ATTACHMENT_META_LINK_TYPE = "attachment" m.DEFAULT_POSITION_THRESHOLD = 0.05 # 5cm m.DEFAULT_ORIENTATION_THRESHOLD = th.deg2rad(th.tensor([5.0])).item() # 5 degrees @@ -81,7 +81,7 @@ def meta_link_type(cls): Returns: str: Unique keyword that defines the meta link associated with this object state """ - return m.ATTACHMENT_LINK_PREFIX + return m.ATTACHMENT_META_LINK_TYPE @classmethod def get_dependencies(cls): diff --git a/omnigibson/object_states/contains.py b/omnigibson/object_states/contains.py index 7a5921818..2067579ea 100644 --- a/omnigibson/object_states/contains.py +++ b/omnigibson/object_states/contains.py @@ -12,7 +12,7 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -m.CONTAINER_LINK_PREFIX = "container" +m.CONTAINER_META_LINK_TYPE = "container" m.VISUAL_PARTICLE_OFFSET = 0.01 # Offset to visual particles' poses when checking overlaps with container volume @@ -38,7 +38,7 @@ def __init__(self, obj): @classproperty def meta_link_type(cls): - return m.CONTAINER_LINK_PREFIX + return m.CONTAINER_META_LINK_TYPE def _get_value(self, system): """ diff --git a/omnigibson/object_states/heat_source_or_sink.py b/omnigibson/object_states/heat_source_or_sink.py index 46ff1391f..1e076455e 100644 --- a/omnigibson/object_states/heat_source_or_sink.py +++ b/omnigibson/object_states/heat_source_or_sink.py @@ -15,7 +15,7 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -m.HEATSOURCE_LINK_PREFIX = "heatsource" +m.HEATSOURCE_META_LINK_TYPE = "heatsource" m.HEATING_ELEMENT_MARKER_SCALE = [1.0] * 3 # TODO: Delete default values for this and make them required. @@ -117,7 +117,7 @@ def is_compatible_asset(cls, prim, **kwargs): @classproperty def meta_link_type(cls): - return m.HEATSOURCE_LINK_PREFIX + return m.HEATSOURCE_META_LINK_TYPE @classmethod def requires_meta_link(cls, **kwargs): diff --git a/omnigibson/object_states/particle_modifier.py b/omnigibson/object_states/particle_modifier.py index 6f016f2c5..d85c527bc 100644 --- a/omnigibson/object_states/particle_modifier.py +++ b/omnigibson/object_states/particle_modifier.py @@ -35,8 +35,8 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -m.APPLICATION_LINK_PREFIX = "particleapplier" -m.REMOVAL_LINK_PREFIX = "particleremover" +m.APPLICATION_META_LINK_TYPE = "particleapplier" +m.REMOVAL_META_LINK_TYPE = "particleremover" # How many samples within the application area to generate per update step m.MAX_VISUAL_PARTICLES_APPLIED_PER_STEP = 2 @@ -925,7 +925,7 @@ def requires_overlap(self): @classproperty def meta_link_type(cls): - return m.REMOVAL_LINK_PREFIX + return m.REMOVAL_META_LINK_TYPE @classmethod def requires_meta_link(cls, **kwargs): @@ -1493,7 +1493,7 @@ def projection_is_active(self): @classproperty def meta_link_type(cls): - return m.APPLICATION_LINK_PREFIX + return m.APPLICATION_META_LINK_TYPE @classmethod def requires_meta_link(cls, **kwargs): diff --git a/omnigibson/object_states/particle_source_or_sink.py b/omnigibson/object_states/particle_source_or_sink.py index cb8df37a5..725e2e02a 100644 --- a/omnigibson/object_states/particle_source_or_sink.py +++ b/omnigibson/object_states/particle_source_or_sink.py @@ -12,8 +12,8 @@ m = create_module_macros(module_path=__file__) # Meta link naming prefixes -m.SOURCE_LINK_PREFIX = "particlesource" -m.SINK_LINK_PREFIX = "particlesink" +m.SOURCE_META_LINK_TYPE = "particlesource" +m.SINK_META_LINK_TYPE = "particlesink" # Default radius and height m.DEFAULT_SOURCE_RADIUS = 0.0125 @@ -132,7 +132,7 @@ def visualize(self): @classproperty def meta_link_type(cls): - return m.SOURCE_LINK_PREFIX + return m.SOURCE_META_LINK_TYPE @property def n_steps_per_modification(self): @@ -241,7 +241,7 @@ def requires_meta_link(cls, **kwargs): @classproperty def meta_link_type(cls): - return m.SINK_LINK_PREFIX + return m.SINK_META_LINK_TYPE @property def n_steps_per_modification(self): diff --git a/omnigibson/object_states/toggle.py b/omnigibson/object_states/toggle.py index cc63a7e4c..881c1c88b 100644 --- a/omnigibson/object_states/toggle.py +++ b/omnigibson/object_states/toggle.py @@ -15,7 +15,7 @@ # Create settings for this module m = create_module_macros(module_path=__file__) -m.TOGGLE_LINK_PREFIX = "togglebutton" +m.TOGGLE_META_LINK_TYPE = "togglebutton" m.DEFAULT_SCALE = 0.1 m.CAN_TOGGLE_STEPS = 5 @@ -79,7 +79,7 @@ def global_update(cls): @classproperty def meta_link_type(cls): - return m.TOGGLE_LINK_PREFIX + return m.TOGGLE_META_LINK_TYPE def _get_value(self): return self.value From f13aeb4f540e4625adae95c591d842d1d832435c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Wed, 12 Feb 2025 01:47:55 -0800 Subject: [PATCH 31/38] Address comments --- omnigibson/prims/geom_prim.py | 4 ++++ omnigibson/prims/rigid_prim.py | 14 ++++++++++++++ omnigibson/utils/asset_conversion_utils.py | 4 ++++ 3 files changed, 22 insertions(+) diff --git a/omnigibson/prims/geom_prim.py b/omnigibson/prims/geom_prim.py index 318ae80df..e37b29fb8 100644 --- a/omnigibson/prims/geom_prim.py +++ b/omnigibson/prims/geom_prim.py @@ -51,6 +51,10 @@ def _post_load(self): super()._post_load() # Temporarily set the opacity threshold to be 0.1. This will be automated in later exports. + # We do this because in previous exports we included continuous alpha values for opacity + # but the ray tracing renderer can only handle binary opacity values. The default threshold + # leaves most objects entirely transparent, so we try to avoid that here. + # TODO: Remove this after the next dataset export if self.material: self.material.opacity_threshold = 0.1 diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 372bf271c..6680be22b 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -825,6 +825,14 @@ def meta_link_type(self): @cached_property def meta_link_id(self): + """The meta link id of this link, if the link is a meta link. + + The meta link ID is a semantic identifier for the meta link within the meta link type. It is + used when an object has multiple meta links of the same type. It can be just a numerical index, + or for some objects, it will be a string that can be matched to other meta links. For example, + a stove might have toggle buttons named "left" and "right", and heat sources named "left" and + "right". The meta link ID can be used to match the toggle button to the heat source. + """ assert self.is_meta_link, f"{self.name} is not a meta link" if self.prim.HasAttribute("ig:metaLinkId"): return self.get_attribute("ig:metaLinkId") @@ -835,6 +843,12 @@ def meta_link_id(self): @cached_property def meta_link_sub_id(self): + """The integer meta link sub id of this link, if the link is a meta link. + + The meta link sub ID identifies this link as one of the parts of a meta link. For example, an + attachment meta link's ID will be the attachment pair name, and each attachment point that + works with that pair will show up as a separate link with a unique sub ID. + """ assert self.is_meta_link, f"{self.name} is not a meta link" if self.prim.HasAttribute("ig:metaLinkSubId"): return int(self.get_attribute("ig:metaLinkSubId")) diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index 8ca132137..dd44aa51b 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -287,6 +287,10 @@ def _set_mtl_opacity(mtl_prim, texture): # This defaults to some other value, which takes opaque black channels in the # image to be fully transparent. This is not what we want. lazy.omni.usd.create_material_input(mtl_prim, "opacity_mode", 0, lazy.pxr.Sdf.ValueTypeNames.Int) + + # We also need to set an opacity threshold. Our objects can include continuous alpha values for opacity + # but the ray tracing renderer can only handle binary opacity values. The default threshold + # leaves most objects entirely transparent, so we try to avoid that here. lazy.omni.usd.create_material_input(mtl_prim, "opacity_threshold", 0.1, lazy.pxr.Sdf.ValueTypeNames.Float) # Verify it was set From eea95d25e1fa5351e82e92ceb9b52ef0347e9d48 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 13 Feb 2025 12:03:52 -0800 Subject: [PATCH 32/38] Change reset implementation --- omnigibson/prims/cloth_prim.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 13ddf1a9e..6e38831fb 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -176,9 +176,6 @@ def _post_load(self): dists = th.norm(positions - aabb_center.reshape(1, 3), dim=-1) self._centroid_idx = th.argmin(dists) - # Store the default position of the points in the local frame - self._default_positions = vtarray_to_torch(self.get_attribute(attr="points")) - def _remesh(self): assert self.prim is not None, "Cannot remesh a non-existent prim!" has_uv_mapping = self.prim.GetAttribute("primvars:st").Get() is not None @@ -533,6 +530,7 @@ def reset_points_to_configuration(self, configuration: Literal["default", "settl attr_name = f"points_{configuration}" points = self.get_attribute(attr=attr_name) self.set_attribute(attr="points", val=points) + self.particle_velocities = th.zeros((self._n_particles, 3)) # For cloth, points should NOT be @cached_property because their local poses change over time @property @@ -1023,5 +1021,5 @@ def reset(self): Reset the points to their default positions in the local frame, and also zeroes out velocities """ if self.initialized: - self.set_attribute(attr="points", val=lazy.pxr.Vt.Vec3fArray(self._default_positions.tolist())) - self.particle_velocities = th.zeros((self._n_particles, 3)) + points_configuration = self._load_config.get("default_point_configuration", "default") + self.reset_points_to_configuration(points_configuration) From e03d3ed29363af948676ba797b7d0faf0aafeca8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 13 Feb 2025 12:04:13 -0800 Subject: [PATCH 33/38] Use settled configuration in sampling when available --- omnigibson/utils/object_state_utils.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/omnigibson/utils/object_state_utils.py b/omnigibson/utils/object_state_utils.py index 9e6852bba..1f7a6c94d 100644 --- a/omnigibson/utils/object_state_utils.py +++ b/omnigibson/utils/object_state_utils.py @@ -267,8 +267,11 @@ def sample_cloth_on_rigid(obj, other, max_trials=40, z_offset=0.05, randomize_xy state = og.sim.dump_state(serialized=False) - # Reset the cloth - obj.root_link.reset() + # Reset the cloth to the settled configuration if available + if "settled" in obj.root_link.get_available_configurations(): + obj.root_link.reset_points_to_configuration("settled") + else: + obj.root_link.reset() obj_aabb_low, obj_aabb_high = obj.states[AABB].get_value() other_aabb_low, other_aabb_high = other.states[AABB].get_value() From ccf015b6bc4831fdb9e9b205db7a2e903e4a6ed3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 13 Feb 2025 14:38:55 -0800 Subject: [PATCH 34/38] Fix velocity issue --- omnigibson/prims/cloth_prim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index 6e38831fb..fbabc7dd4 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -530,7 +530,7 @@ def reset_points_to_configuration(self, configuration: Literal["default", "settl attr_name = f"points_{configuration}" points = self.get_attribute(attr=attr_name) self.set_attribute(attr="points", val=points) - self.particle_velocities = th.zeros((self._n_particles, 3)) + self.set_attribute(attr="velocities", val=lazy.pxr.Vt.Vec3fArray(th.zeros((len(points), 3)).tolist())) # For cloth, points should NOT be @cached_property because their local poses change over time @property From c637235439f96041b0741da59f2b6dd90d02937b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Thu, 13 Feb 2025 23:59:11 -0800 Subject: [PATCH 35/38] Fix weird remeshing behavior --- omnigibson/prims/cloth_prim.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index fbabc7dd4..a6a311ec6 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -218,9 +218,6 @@ def _remesh(self): ms.meshing_isotropic_explicit_remeshing( iterations=5, adaptive=True, targetlen=pymeshlab.AbsoluteValue(particle_distance) ) - # Make sure the clothes is watertight - ms.meshing_repair_non_manifold_edges() - ms.meshing_repair_non_manifold_vertices() # If the cloth has multiple pieces, only keep the largest one ms.generate_splitting_by_connected_components(delete_source_mesh=True) From d3ed4860ca86b91ef47bfce9c5b33aa4ab872cf0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Fri, 14 Feb 2025 00:57:15 -0800 Subject: [PATCH 36/38] Re-reduce drag temporarily until meeting --- omnigibson/systems/micro_particle_system.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/systems/micro_particle_system.py b/omnigibson/systems/micro_particle_system.py index 8ebad467c..cc3141727 100644 --- a/omnigibson/systems/micro_particle_system.py +++ b/omnigibson/systems/micro_particle_system.py @@ -31,7 +31,7 @@ m.CLOTH_SHEAR_STIFFNESS = 70.0 m.CLOTH_DAMPING = 0.02 m.CLOTH_FRICTION = 0.4 -m.CLOTH_DRAG = 0.02 +m.CLOTH_DRAG = 0.001 m.CLOTH_LIFT = 0.003 m.MIN_PARTICLE_CONTACT_OFFSET = 0.005 # Minimum particle contact offset for physical micro particles m.FLUID_PARTICLE_PARTICLE_DISTANCE_SCALE = 0.8 # How much overlap expected between fluid particles at rest From 278e61cc7a418cf4fa43691c69aa6e832b035fde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 14 Feb 2025 01:05:08 -0800 Subject: [PATCH 37/38] Undo temporary changes since we're reexporting the RC --- omnigibson/prims/geom_prim.py | 12 ------------ omnigibson/prims/material_prim.py | 14 -------------- 2 files changed, 26 deletions(-) diff --git a/omnigibson/prims/geom_prim.py b/omnigibson/prims/geom_prim.py index e37b29fb8..e60f1381c 100644 --- a/omnigibson/prims/geom_prim.py +++ b/omnigibson/prims/geom_prim.py @@ -46,18 +46,6 @@ def _load(self): # This should not be called, because this prim cannot be instantiated from scratch! raise NotImplementedError("By default, a geom prim cannot be created from scratch.") - def _post_load(self): - # run super first - super()._post_load() - - # Temporarily set the opacity threshold to be 0.1. This will be automated in later exports. - # We do this because in previous exports we included continuous alpha values for opacity - # but the ray tracing renderer can only handle binary opacity values. The default threshold - # leaves most objects entirely transparent, so we try to avoid that here. - # TODO: Remove this after the next dataset export - if self.material: - self.material.opacity_threshold = 0.1 - @property def purpose(self): """ diff --git a/omnigibson/prims/material_prim.py b/omnigibson/prims/material_prim.py index 1b05d24e8..3098658a3 100644 --- a/omnigibson/prims/material_prim.py +++ b/omnigibson/prims/material_prim.py @@ -231,20 +231,6 @@ def set_input(self, inp, val): val (any): Value to set for the input. This should be the valid type for that attribute. """ # Make sure the input exists first, so we avoid segfaults with "invalid null prim" - if inp not in self.shader_input_names: - input_type = None - if isinstance(val, str): - input_type = lazy.pxr.Sdf.ValueTypeNames.String - elif isinstance(val, int): - input_type = lazy.pxr.Sdf.ValueTypeNames.Int - elif isinstance(val, float): - input_type = lazy.pxr.Sdf.ValueTypeNames.Float - elif isinstance(val, bool): - input_type = lazy.pxr.Sdf.ValueTypeNames.Bool - else: - raise ValueError(f"Invalid input type {type(val)} for input {inp}") - - lazy.omni.usd.create_material_input(self.prim, inp, val, input_type) assert ( inp in self.shader_input_names ), f"Got invalid shader input to set! Current inputs are: {self.shader_input_names}. Got: {inp}" From 19770bb5be09c41aad78f07d84b10f79283244c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 14 Feb 2025 09:19:29 -0800 Subject: [PATCH 38/38] Make velocity reset optional --- omnigibson/prims/cloth_prim.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index a6a311ec6..b9112c14c 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -527,7 +527,10 @@ def reset_points_to_configuration(self, configuration: Literal["default", "settl attr_name = f"points_{configuration}" points = self.get_attribute(attr=attr_name) self.set_attribute(attr="points", val=points) - self.set_attribute(attr="velocities", val=lazy.pxr.Vt.Vec3fArray(th.zeros((len(points), 3)).tolist())) + + # Reset velocities to zero if velocities are present + if self.prim.HasAttribute("velocities"): + self.set_attribute(attr="velocities", val=lazy.pxr.Vt.Vec3fArray(th.zeros((len(points), 3)).tolist())) # For cloth, points should NOT be @cached_property because their local poses change over time @property