Merge pull request #626 from StanfordVL/mesh-scaling

Accommodate non-uniformly scaled meshes

omnigibson/objects/object_base.py

@@ -254,11 +254,7 @@ def volume(self):
         Returns:
              float: Cumulative volume of this potentially articulated object.
         """
-        volume = 0.0
-        for link in self._links.values():
-            volume += link.volume
-
-        return volume
+        return sum(link.volume for link in self._links.values())
 
     @volume.setter
     def volume(self, volume):

omnigibson/objects/primitive_object.py

@@ -146,18 +146,6 @@ def _load(self):
         return prim
 
     def _post_load(self):
-        # Run super first
-        super()._post_load()
-
-        # Set the collision approximation appropriately
-        if self._primitive_type == "Sphere":
-            col_approximation = "boundingSphere"
-        elif self._primitive_type == "Cube":
-            col_approximation = "boundingCube"
-        else:
-            col_approximation = "convexHull"
-        self.root_link.collision_meshes["collisions"].set_collision_approximation(col_approximation)
-
         # Possibly set scalings (only if the scale value is not set)
         if self._load_config["scale"] is not None:
             log.warning("Custom scale specified for primitive object, so ignoring radius, height, and size arguments!")
@@ -169,6 +157,21 @@ def _post_load(self):
             if self._load_config["size"] is not None:
                 self.size = self._load_config["size"]
 
+        # This step might will perform cloth remeshing if self._prim_type == PrimType.CLOTH.
+        # Therefore, we need to apply size, radius, and height before this to scale the points properly.
+        super()._post_load()
+
+        # Cloth primitive does not have collision meshes
+        if self._prim_type != PrimType.CLOTH:
+            # Set the collision approximation appropriately
+            if self._primitive_type == "Sphere":
+                col_approximation = "boundingSphere"
+            elif self._primitive_type == "Cube":
+                col_approximation = "boundingCube"
+            else:
+                col_approximation = "convexHull"
+            self.root_link.collision_meshes["collisions"].set_collision_approximation(col_approximation)
+
     def _initialize(self):
         # Run super first
         super()._initialize()

omnigibson/prims/cloth_prim.py

@@ -15,7 +15,6 @@
 import omnigibson.utils.transform_utils as T
 from omnigibson.utils.sim_utils import CsRawData
 from omnigibson.utils.usd_utils import array_to_vtarray, mesh_prim_to_trimesh_mesh, sample_mesh_keypoints
-from omnigibson.utils.constants import GEOM_TYPES
 from omnigibson.utils.python_utils import classproperty
 import omnigibson as og
 
@@ -355,26 +354,8 @@ def update_handles(self):
 
     @property
     def volume(self):
-        mesh = self.prim
-        mesh_type = mesh.GetPrimTypeInfo().GetTypeName()
-        assert mesh_type in GEOM_TYPES, f"Invalid collision mesh type: {mesh_type}"
-        if mesh_type == "Mesh":
-            # We construct a trimesh object from this mesh in order to infer its volume
-            trimesh_mesh = mesh_prim_to_trimesh_mesh(mesh)
-            mesh_volume = trimesh_mesh.volume if trimesh_mesh.is_volume else trimesh_mesh.convex_hull.volume
-        elif mesh_type == "Sphere":
-            mesh_volume = 4 / 3 * np.pi * (mesh.GetAttribute("radius").Get() ** 3)
-        elif mesh_type == "Cube":
-            mesh_volume = mesh.GetAttribute("size").Get() ** 3
-        elif mesh_type == "Cone":
-            mesh_volume = np.pi * (mesh.GetAttribute("radius").Get() ** 2) * mesh.GetAttribute("height").Get() / 3
-        elif mesh_type == "Cylinder":
-            mesh_volume = np.pi * (mesh.GetAttribute("radius").Get() ** 2) * mesh.GetAttribute("height").Get()
-        else:
-            raise ValueError(f"Cannot compute volume for mesh of type: {mesh_type}")
-
-        mesh_volume *= np.product(self.get_world_scale())
-        return mesh_volume
+        mesh = mesh_prim_to_trimesh_mesh(self.prim, include_normals=False, include_texcoord=False, world_frame=True)
+        return mesh.volume if mesh.is_volume else mesh.convex_hull.volume
 
     @volume.setter
     def volume(self, volume):

omnigibson/prims/entity_prim.py

@@ -10,7 +10,7 @@
 from omnigibson.prims.joint_prim import JointPrim
 from omnigibson.prims.rigid_prim import RigidPrim
 from omnigibson.prims.xform_prim import XFormPrim
-from omnigibson.utils.constants import PrimType, GEOM_TYPES, JointType, JointAxis
+from omnigibson.utils.constants import PrimType, JointType, JointAxis
 from omnigibson.utils.ui_utils import suppress_omni_log
 from omnigibson.utils.usd_utils import PoseAPI
 

omnigibson/prims/geom_prim.py

@@ -7,7 +7,7 @@
 from omnigibson.macros import gm
 from omnigibson.prims.xform_prim import XFormPrim
 from omnigibson.utils.python_utils import assert_valid_key
-from omnigibson.utils.usd_utils import PoseAPI
+from omnigibson.utils.usd_utils import PoseAPI, mesh_prim_shape_to_trimesh_mesh
 import omnigibson.utils.transform_utils as T
 
 
@@ -131,32 +131,11 @@ def points(self):
         mesh = self.prim
         mesh_type = mesh.GetPrimTypeInfo().GetTypeName()
         if mesh_type == "Mesh":
+            # If the geom is a mesh we can directly return its points.
             return np.array(self.prim.GetAttribute("points").Get())
-
-        # Generate a trimesh for other shapes
-        if mesh_type == "Sphere":
-            radius = mesh.GetAttribute("radius").Get()
-            mesh = trimesh.creation.icosphere(subdivision=3, radius=radius)
-        elif mesh_type == "Cube":
-            extent = mesh.GetAttribute("size").Get()
-            mesh = trimesh.creation.box([extent]*3)
-        elif mesh_type == "Cone":
-            radius = mesh.GetAttribute("radius").Get()
-            height = mesh.GetAttribute("height").Get()
-            mesh = trimesh.creation.cone(radius=radius, height=height)
-
-            # Trimesh cones are centered at the base. We'll move them down by half the height.
-            transform = trimesh.transformations.translation_matrix([0, 0, -height / 2])
-            mesh.apply_transform(transform)
-        elif mesh_type == "Cylinder":
-            radius = mesh.GetAttribute("radius").Get()
-            height = mesh.GetAttribute("height").Get()
-            mesh = trimesh.creation.cylinder(radius=radius, height=height)
         else:
-            raise ValueError(f"Cannot compute volume for mesh of type: {mesh_type}")
-
-        # Return the vertices of the trimesh
-        return np.array(mesh.vertices)
+            # Return the vertices of the trimesh
+            return np.array(mesh_prim_shape_to_trimesh_mesh(mesh).vertices)
 
     @property
     def points_in_parent_frame(self):

omnigibson/prims/rigid_prim.py

@@ -182,7 +182,8 @@ def update_meshes(self):
             for child in prim.GetChildren():
                 prims_to_check.append(child)
         for prim in prims_to_check:
-            if prim.GetPrimTypeInfo().GetTypeName() in GEOM_TYPES:
+            mesh_type = prim.GetPrimTypeInfo().GetTypeName()
+            if mesh_type in GEOM_TYPES:
                 mesh_name, mesh_path = prim.GetName(), prim.GetPrimPath().__str__()
                 mesh_prim = lazy.omni.isaac.core.utils.prims.get_prim_at_path(prim_path=mesh_path)
                 is_collision = mesh_prim.HasAPI(lazy.pxr.UsdPhysics.CollisionAPI)
@@ -195,10 +196,10 @@ def update_meshes(self):
                     mesh.set_rest_offset(m.DEFAULT_REST_OFFSET)
                     self._collision_meshes[mesh_name] = mesh
 
-                    is_volume, volume, com = get_mesh_volume_and_com(mesh_prim)
-                    vols.append(volume)
-                    # We need to translate the center of mass from the mesh's local frame to the link's local frame
+                    volume, com = get_mesh_volume_and_com(mesh_prim)
+                    # We need to transform the volume and CoM from the mesh's local frame to the link's local frame
                     local_pos, local_orn = mesh.get_local_pose()
+                    vols.append(volume * np.product(mesh.scale))
                     coms.append(T.quat2mat(local_orn) @ (com * mesh.scale) + local_pos)
                     # If the ratio between the max extent and min radius is too large (i.e. shape too oblong), use
                     # boundingCube approximation for the underlying collision approximation for GPU compatibility
@@ -208,6 +209,7 @@ def update_meshes(self):
                 else:
                     self._visual_meshes[mesh_name] = VisualGeomPrim(**mesh_kwargs)
 
+
         # If we have any collision meshes, we aggregate their center of mass and volume values to set the center of mass
         # for this link
         if len(coms) > 0:
@@ -429,12 +431,7 @@ def volume(self):
             float: total volume of all the collision meshes of the rigid body in m^3.
         """
         # TODO (eric): revise this once omni exposes API to query volume of GeomPrims
-        volume = 0.0
-        for collision_mesh in self._collision_meshes.values():
-            _, mesh_volume, _ = get_mesh_volume_and_com(collision_mesh.prim)
-            volume += mesh_volume * np.product(collision_mesh.get_world_scale())
-
-        return volume
+        return sum(get_mesh_volume_and_com(collision_mesh.prim, world_frame=True)[0] for collision_mesh in self._collision_meshes.values())
 
     @volume.setter
     def volume(self, volume):

omnigibson/systems/micro_particle_system.py

@@ -9,7 +9,7 @@
 from omnigibson.utils.geometry_utils import generate_points_in_volume_checker_function
 from omnigibson.utils.python_utils import classproperty, assert_valid_key, subclass_factory, snake_case_to_camel_case
 from omnigibson.utils.sampling_utils import sample_cuboid_on_object_full_grid_topdown
-from omnigibson.utils.usd_utils import mesh_prim_to_trimesh_mesh
+from omnigibson.utils.usd_utils import mesh_prim_to_trimesh_mesh, PoseAPI
 from omnigibson.utils.physx_utils import create_physx_particle_system, create_physx_particleset_pointinstancer
 from omnigibson.utils.ui_utils import disclaimer, create_module_logger
 
@@ -1558,7 +1558,9 @@ def clothify_mesh_prim(cls, mesh_prim, remesh=True, particle_distance=None):
         if remesh:
             # 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
-            tm = mesh_prim_to_trimesh_mesh(mesh_prim=mesh_prim, include_normals=True, include_texcoord=True)
+            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)
@@ -1567,8 +1569,7 @@ def clothify_mesh_prim(cls, mesh_prim, remesh=True, particle_distance=None):
             tm.export(tmp_fpath)
 
             # Start with the default particle distance
-            particle_distance = cls.particle_contact_offset * 2 / (1.5 * np.mean(mesh_prim.GetAttribute("xformOp:scale").Get())) \
-                if particle_distance is None else particle_distance
+            particle_distance = cls.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
             for _ in range(3):
@@ -1604,16 +1605,27 @@ def clothify_mesh_prim(cls, mesh_prim, remesh=True, particle_distance=None):
                 raise ValueError(f"Could not remesh with less than MAX_CLOTH_PARTICLES ({m.MAX_CLOTH_PARTICLES}) vertices!")
 
             # Re-write data to @mesh_prim
-            new_face_vertex_ids = cm.face_matrix().flatten()
+            new_faces = cm.face_matrix()
+            new_face_vertex_ids = new_faces.flatten()
             new_texcoord = cm.wedge_tex_coord_matrix()
             new_vertices = cm.vertex_matrix()
             new_normals = cm.vertex_normal_matrix()
             n_faces = len(cm.face_matrix())
+            new_face_vertex_counts = np.ones(n_faces, dtype=int) * 3
 
-            mesh_prim.GetAttribute("faceVertexCounts").Set(np.ones(n_faces, dtype=int) * 3)
-            mesh_prim.GetAttribute("points").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(new_vertices))
+            tm_new = 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_new.apply_transform(np.linalg.inv(scaled_world_transform))
+
+            # Update the mesh prim
+            mesh_prim.GetAttribute("faceVertexCounts").Set(new_face_vertex_counts)
+            mesh_prim.GetAttribute("points").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(tm_new.vertices))
             mesh_prim.GetAttribute("faceVertexIndices").Set(new_face_vertex_ids)
-            mesh_prim.GetAttribute("normals").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(new_normals))
+            mesh_prim.GetAttribute("normals").Set(lazy.pxr.Vt.Vec3fArray.FromNumpy(tm_new.vertex_normals))
             mesh_prim.GetAttribute("primvars:st").Set(lazy.pxr.Vt.Vec2fArray.FromNumpy(new_texcoord))
 
         # Convert into particle cloth

omnigibson/utils/constants.py

@@ -101,7 +101,7 @@ class EmitterType(IntEnum):
 }
 
 # Valid geom types
-GEOM_TYPES = {"Sphere", "Cube", "Capsule", "Cone", "Cylinder", "Mesh"}
+GEOM_TYPES = {"Sphere", "Cube", "Cone", "Cylinder", "Mesh"}
 
 # Valid joint axis
 JointAxis = ["X", "Y", "Z"]

omnigibson/utils/geometry_utils.py

@@ -3,7 +3,7 @@
 """
 import numpy as np
 import omnigibson.utils.transform_utils as T
-from omnigibson.utils.usd_utils import mesh_prim_to_trimesh_mesh
+from omnigibson.utils.usd_utils import mesh_prim_mesh_to_trimesh_mesh
 
 
 def get_particle_positions_in_frame(pos, quat, scale, particle_positions):
@@ -238,7 +238,7 @@ def _generate_convex_hull_volume_checker_functions(convex_hull_mesh):
                 USD mesh
     """
     # For efficiency, we pre-compute the mesh using trimesh and find its corresponding faces and normals
-    trimesh_mesh = mesh_prim_to_trimesh_mesh(convex_hull_mesh, include_normals=False, include_texcoord=False).convex_hull
+    trimesh_mesh = mesh_prim_mesh_to_trimesh_mesh(convex_hull_mesh, include_normals=False, include_texcoord=False).convex_hull
     assert trimesh_mesh.is_convex, \
         f"Trying to generate a volume checker function for a non-convex mesh {convex_hull_mesh.GetPath().pathString}"
     face_centroids = trimesh_mesh.vertices[trimesh_mesh.faces].mean(axis=1)