Merge pull request #628 from StanfordVL/mesh-scaling-v2

Mesh scaling v2

omnigibson/object_states/overlaid.py

@@ -8,8 +8,7 @@
 
 import omnigibson as og
 
-from scipy.spatial import ConvexHull, HalfspaceIntersection
-from scipy.spatial.qhull import QhullError
+from scipy.spatial import ConvexHull, HalfspaceIntersection, QhullError
 import numpy as np
 import trimesh
 import itertools

omnigibson/object_states/particle_modifier.py

@@ -298,9 +298,9 @@ def _initialize(self):
         # Sanity check scale if requested
         if self.requires_overlap:
             # Run sanity check to make sure compatibility with omniverse physx
-            if self.method == ParticleModifyMethod.PROJECTION and self.obj.scale.max() != self.obj.scale.min():
+            if self.method == ParticleModifyMethod.PROJECTION and not np.isclose(self.obj.scale.max(), self.obj.scale.min(), atol=1e-04):
                 raise ValueError(f"{self.__class__.__name__} for obj {self.obj.name} using PROJECTION method cannot be "
-                                 f"created with non-uniform scale and sample_with_raycast! Got scale: {self.obj.scale}")
+                                 f"created with non-uniform scale and requires_overlap! Got scale: {self.obj.scale}")
 
         # Initialize internal variables
         self._current_step = 0
@@ -631,7 +631,7 @@ def requires_overlap(self):
         Returns:
             bool: Whether overlap checks should be executed as a guard condition against modifying particles
         """
-        return True
+        raise NotImplementedError()
 
     @classproperty
     def supported_active_systems(cls):

omnigibson/prims/cloth_prim.py

@@ -19,6 +19,7 @@
 import omnigibson as og
 
 import numpy as np
+from collections.abc import Iterable
 
 
 # Create settings for this module

omnigibson/prims/entity_prim.py

@@ -123,6 +123,10 @@ def _post_load(self):
         # We pass in scale explicitly so that the generated links can leverage the desired entity scale
         self.update_links()
 
+        # Optionally set the scale
+        if "scale" in self._load_config and self._load_config["scale"] is not None:
+            self.scale = self._load_config["scale"]
+
         # Prepare the articulation view.
         if self.n_joints > 0:
             # Import now to avoid too-eager load of Omni classes due to inheritance
@@ -185,10 +189,6 @@ def update_links(self):
         Helper function to refresh owned joints. Useful for synchronizing internal data if
         additional bodies are added manually
         """
-        load_config = {
-            "scale": self._load_config.get("scale", None),
-        }
-
         # Make sure to clean up all pre-existing names for all links
         if self._links is not None:
             for link in self._links.values():
@@ -234,11 +234,12 @@ def update_links(self):
         # Now actually create the links
         self._links = dict()
         for link_name, (link_cls, prim) in links_to_create.items():
+            # Fixed child links of kinematic-only objects are not kinematic-only, to avoid the USD error:
+            # PhysicsUSD: CreateJoint - cannot create a joint between static bodies, joint prim: ...
             link_load_config = {
                 "kinematic_only": self._load_config.get("kinematic_only", False)
                 if link_name == self._root_link_name else False,
             }
-            link_load_config.update(load_config)
             self._links[link_name] = link_cls(
                 prim_path=prim.GetPrimPath().__str__(),
                 name=f"{self._name}:{link_name}",
@@ -1084,16 +1085,19 @@ def disabled_collision_pairs(self):
 
     @property
     def scale(self):
-        # Since all rigid bodies owned by this object prim have the same scale, we simply grab it from the root prim
-        return self.root_link.scale
+        # For the EntityPrim (object) level, @self.scale represents the scale with respect to the original scale of
+        # the link (RigidPrim or ClothPrim), which might not be uniform ([1, 1, 1]) itself.
+        return self.root_link.scale / self.root_link.original_scale
 
     @scale.setter
     def scale(self, scale):
+        # For the EntityPrim (object) level, @self.scale represents the scale with respect to the original scale of
+        # the link (RigidPrim or ClothPrim), which might not be uniform ([1, 1, 1]) itself.
         # We iterate over all rigid bodies owned by this object prim and set their individual scales
         # We do this because omniverse cannot scale orientation of an articulated prim, so we get mesh mismatches as
-        # they rotate in the world
+        # they rotate in the world.
         for link in self._links.values():
-            link.scale = scale
+            link.scale = scale * link.original_scale
 
     @property
     def solver_position_iteration_count(self):

omnigibson/prims/rigid_prim.py

@@ -1,5 +1,5 @@
 from functools import cached_property
-import scipy
+from scipy.spatial import ConvexHull, QhullError
 import numpy as np
 
 import omnigibson as og
@@ -63,7 +63,7 @@ def __init__(
         self._visual_only = None
         self._collision_meshes = None
         self._visual_meshes = None
-        
+
         # Caches for kinematic-only objects
         # This exists because RigidPrimView uses USD pose read, which is very slow
         self._kinematic_world_pose_cache = None
@@ -623,7 +623,7 @@ def _compute_points_on_convex_hull(self, visual):
         points = np.concatenate(points, axis=0)
 
         try:
-            hull = scipy.spatial.ConvexHull(points)
+            hull = ConvexHull(points)
             return points[hull.vertices, :]
         except:
             # Handle the case where a convex hull cannot be formed (e.g., collinear points)

omnigibson/prims/xform_prim.py

@@ -40,6 +40,7 @@ def __init__(
         self._binding_api = None
         self._material = None
         self._collision_filter_api = None
+        self.original_scale = None
 
         # Run super method
         super().__init__(
@@ -58,6 +59,10 @@ def _post_load(self):
         # Make sure all xforms have pose and scaling info
         self._set_xform_properties()
 
+        # Cache the original scale from the USD so that when EntityPrim sets the scale for each link (Rigid/ClothPrim),
+        # the new scale is with respect to the original scale. XFormPrim's scale always matches the scale in the USD.
+        self.original_scale = np.array(self.get_attribute("xformOp:scale"))
+
         # Create collision filter API
         self._collision_filter_api = lazy.pxr.UsdPhysics.FilteredPairsAPI(self._prim) if \
             self._prim.HasAPI(lazy.pxr.UsdPhysics.FilteredPairsAPI) else lazy.pxr.UsdPhysics.FilteredPairsAPI.Apply(self._prim)

omnigibson/scenes/scene_base.py

@@ -442,7 +442,7 @@ def add_object(self, obj, register=True, _is_call_from_simulator=False):
         # This is to account for cases such as Tiago, which has a fixed base which is needed for its global base joints
         # We do this by adding the object to our tracked collision groups
         structure_categories = {"walls", "floors", "ceilings"}
-        if obj.fixed_base and obj.category != robot_macros.ROBOT_CATEGORY:
+        if obj.fixed_base and obj.category != robot_macros.ROBOT_CATEGORY and not obj.visual_only:
             # TODO: Remove structure hotfix once asset collision meshes are fixed!!
             if obj.category in structure_categories:
                 CollisionAPI.add_to_collision_group(col_group="structures", prim_path=obj.prim_path)

omnigibson/simulator.py

@@ -680,10 +680,6 @@ def play(self):
                 with suppress_omni_log(channels=channels):
                     super().play()
 
-                # If we're stopped, take a physics step and update the physics sim view. This must happen BEFORE the
-                # handles are updated, since updating the physics view makes the per-object physics view invalid
-                self.step_physics()
-
                 # Take a render step -- this is needed so that certain (unknown, maybe omni internal state?) is populated
                 # correctly.
                 self.render()

omnigibson/systems/macro_particle_system.py

@@ -228,7 +228,7 @@ def add_particle(cls, prim_path, scale, idn=None):
         new_particle = cls._load_new_particle(prim_path=f"{prim_path}/{name}", name=name)
 
         # Set the scale and make sure the particle is visible
-        new_particle.scale = scale
+        new_particle.scale *= scale
         new_particle.visible = True
 
         # Track this particle as well
@@ -1165,7 +1165,7 @@ def process_particle_object(cls):
         super().process_particle_object()
 
         # Compute particle radius
-        vertices = np.array(cls.particle_object.get_attribute("points")) * cls.max_scale.reshape(1, 3)
+        vertices = np.array(cls.particle_object.get_attribute("points")) * cls.particle_object.scale * cls.max_scale.reshape(1, 3)
         cls._particle_offset, cls._particle_radius = trimesh.nsphere.minimum_nsphere(trimesh.Trimesh(vertices=vertices))
 
     @classmethod

omnigibson/systems/micro_particle_system.py

@@ -1475,7 +1475,7 @@ def _create_particle_prototypes(cls):
 
         # Wrap it with VisualGeomPrim with the correct scale
         prototype = VisualGeomPrim(prim_path=prototype_path, name=prototype_path)
-        prototype.scale = cls.max_scale
+        prototype.scale *= cls.max_scale
         prototype.visible = False
         lazy.omni.isaac.core.utils.semantics.add_update_semantics(
             prim=prototype.prim,

tests/test_object_states.py

@@ -1073,37 +1073,31 @@ def test_filled():
             og.sim.step()
 
         assert stockpot.states[Filled].set_value(system, True)
-
-        for _ in range(5):
-            og.sim.step()
-
+        og.sim.step()
         assert stockpot.states[Filled].get_value(system)
 
         # Cannot set Filled state False
         with pytest.raises(NotImplementedError):
             stockpot.states[Filled].set_value(system, False)
 
         system.remove_all_particles()
-
-        for _ in range(5):
-            og.sim.step()
-
+        og.sim.step()
         assert not stockpot.states[Filled].get_value(system)
 
 @og_test
 def test_contains():
     stockpot = og.sim.scene.object_registry("name", "stockpot")
     systems = [get_system(system_name) for system_name, system_class in SYSTEM_EXAMPLES.items()]
     for system in systems:
+        print(f"Testing Contains {stockpot.name} with {system.name}")
         stockpot.set_position_orientation(position=np.ones(3) * 50.0, orientation=[0, 0, 0, 1.0])
         place_obj_on_floor_plane(stockpot)
         for _ in range(5):
             og.sim.step()
 
         # Sample single particle
         if is_physical_particle_system(system_name=system.name):
-            system.generate_particles(positions=[np.array([0, 0, stockpot.aabb[1][2] + system.particle_radius * 1.01])])
-            assert not stockpot.states[Contains].get_value(system)
+            system.generate_particles(positions=[np.array([0, 0, stockpot.aabb[1][2] - 0.1])])
         else:
             if system.get_group_name(stockpot) not in system.groups:
                 system.create_attachment_group(stockpot)
@@ -1113,9 +1107,7 @@ def test_contains():
                 link_prim_paths=[stockpot.root_link.prim_path],
             )
 
-        for _ in range(10):
-            og.sim.step()
-
+        og.sim.step()
         assert stockpot.states[Contains].get_value(system)
 
         # Remove all particles and make sure contains returns False
@@ -1132,10 +1124,10 @@ def test_contains():
 @og_test
 def test_covered():
     bracelet = og.sim.scene.object_registry("name", "bracelet")
-    oyster = og.sim.scene.object_registry("name", "oyster")
+    bowl = og.sim.scene.object_registry("name", "bowl")
     microwave = og.sim.scene.object_registry("name", "microwave")
     systems = [get_system(system_name) for system_name, system_class in SYSTEM_EXAMPLES.items()]
-    for obj in (bracelet, oyster, microwave):
+    for obj in (bracelet, bowl, microwave):
         for system in systems:
             # bracelet is too small to sample physical particles on it
             sampleable = is_visual_particle_system(system.name) or obj != bracelet

tests/test_robot_states.py

@@ -65,8 +65,8 @@ def camera_pose_test(flatcache):
     sensor_world_pos_gt = np.array([150.16513062, 150.0, 101.3833847])
     sensor_world_ori_gt = np.array([-0.29444987, 0.29444981, 0.64288363, -0.64288352])
 
-    assert np.allclose(sensor_world_pos, sensor_world_pos_gt)
-    assert np.allclose(sensor_world_ori, sensor_world_ori_gt)
+    assert np.allclose(sensor_world_pos, sensor_world_pos_gt, atol=1e-3)
+    assert np.allclose(sensor_world_ori, sensor_world_ori_gt, atol=1e-3)
 
     # Now, we want to move the robot and check if the sensor pose has been updated
     old_camera_local_pose = vision_sensor.get_local_pose()
@@ -76,9 +76,9 @@ def camera_pose_test(flatcache):
     new_camera_world_pose = vision_sensor.get_position_orientation()
     robot_pose_mat = pose2mat(robot.get_position_orientation())
     expected_camera_world_pos, expected_camera_world_ori = mat2pose(robot_pose_mat @ robot_to_sensor_mat)
-    assert np.allclose(old_camera_local_pose[0], new_camera_local_pose[0])
-    assert np.allclose(new_camera_world_pose[0], expected_camera_world_pos)
-    assert np.allclose(new_camera_world_pose[1], expected_camera_world_ori)
+    assert np.allclose(old_camera_local_pose[0], new_camera_local_pose[0], atol=1e-3)
+    assert np.allclose(new_camera_world_pose[0], expected_camera_world_pos, atol=1e-3)
+    assert np.allclose(new_camera_world_pose[1], expected_camera_world_ori, atol=1e-3)
 
     # Then, we want to move the local pose of the camera and check 
     # 1) if the world pose is updated 2) if the robot stays in the same position
@@ -89,9 +89,9 @@ def camera_pose_test(flatcache):
     camera_parent_path = str(camera_parent_prim.GetPath())
     camera_parent_world_transform = PoseAPI.get_world_pose_with_scale(camera_parent_path)
     expected_new_camera_world_pos, expected_new_camera_world_ori = mat2pose(camera_parent_world_transform @ pose2mat([[10, 10, 10], [0, 0, 0, 1]]))
-    assert np.allclose(new_camera_world_pose[0], expected_new_camera_world_pos)
-    assert np.allclose(new_camera_world_pose[1], expected_new_camera_world_ori)
-    assert np.allclose(robot.get_position(), [100, 100, 100])
+    assert np.allclose(new_camera_world_pose[0], expected_new_camera_world_pos, atol=1e-3)
+    assert np.allclose(new_camera_world_pose[1], expected_new_camera_world_ori, atol=1e-3)
+    assert np.allclose(robot.get_position(), [100, 100, 100], atol=1e-3)
 
 
     # Finally, we want to move the world pose of the camera and check
@@ -100,17 +100,17 @@ def camera_pose_test(flatcache):
     old_camera_local_pose = vision_sensor.get_local_pose()
     vision_sensor.set_position_orientation([150, 150, 101.36912537], [-0.29444987, 0.29444981, 0.64288363, -0.64288352])
     new_camera_local_pose = vision_sensor.get_local_pose()
-    assert not np.allclose(old_camera_local_pose[0], new_camera_local_pose[0])
-    assert not np.allclose(old_camera_local_pose[1], new_camera_local_pose[1])
-    assert np.allclose(robot.get_position(), [150, 150, 100])
+    assert not np.allclose(old_camera_local_pose[0], new_camera_local_pose[0], atol=1e-3)
+    assert not np.allclose(old_camera_local_pose[1], new_camera_local_pose[1], atol=1e-3)
+    assert np.allclose(robot.get_position(), [150, 150, 100], atol=1e-3)
 
     # Another test we want to try is setting the camera's parent scale and check if the world pose is updated
     camera_parent_prim.GetAttribute('xformOp:scale').Set(lazy.pxr.Gf.Vec3d([2.0, 2.0, 2.0]))
     camera_parent_world_transform = PoseAPI.get_world_pose_with_scale(camera_parent_path)
     camera_local_pose = vision_sensor.get_local_pose()
     expected_new_camera_world_pos, _ = mat2pose(camera_parent_world_transform @ pose2mat(camera_local_pose))
     new_camera_world_pose = vision_sensor.get_position_orientation()
-    assert np.allclose(new_camera_world_pose[0], expected_new_camera_world_pos)
+    assert np.allclose(new_camera_world_pose[0], expected_new_camera_world_pos, atol=1e-3)
 
     og.sim.clear()