Add re-timing, minimum dt robustness

src/curobo/geom/cv.py

@@ -47,7 +47,9 @@ def project_depth_to_pointcloud(depth_image: torch.Tensor, intrinsics_matrix: to
 
 
 @get_torch_jit_decorator()
-def get_projection_rays(height: int, width: int, intrinsics_matrix: torch.Tensor):
+def get_projection_rays(
+    height: int, width: int, intrinsics_matrix: torch.Tensor, depth_to_meter: float = 0.001
+):
     """Projects numpy depth image to point cloud.
 
     Args:
@@ -82,7 +84,7 @@ def get_projection_rays(height: int, width: int, intrinsics_matrix: torch.Tensor
     rays = torch.stack([output_x, output_y, input_z], -1).reshape(
         intrinsics_matrix.shape[0], width * height, 3
     )
-    rays = rays * (1.0 / 1000.0)
+    rays = rays * depth_to_meter
     return rays
 
 

src/curobo/geom/sdf/world.py

@@ -264,7 +264,7 @@ class WorldCollisionConfig:
     n_envs: int = 1
     checker_type: CollisionCheckerType = CollisionCheckerType.PRIMITIVE
     max_distance: Union[torch.Tensor, float] = 0.1
-    max_esdf_distance: Union[torch.Tensor, float] = 1000.0
+    max_esdf_distance: Union[torch.Tensor, float] = 100.0
 
     def __post_init__(self):
         if self.world_model is not None and isinstance(self.world_model, list):
@@ -398,6 +398,7 @@ class WorldCollision(WorldCollisionConfig):
             self._cache_voxelization is None
             or self._cache_voxelization.voxel_size != new_grid.voxel_size
             or self._cache_voxelization.dims != new_grid.dims
+            or self._cache_voxelization.xyzr_tensor is None
         ):
             self._cache_voxelization = new_grid
             self._cache_voxelization.xyzr_tensor = self._cache_voxelization.create_xyzr_tensor(
@@ -458,7 +459,6 @@ class WorldCollision(WorldCollisionConfig):
         self.update_cache_voxelization(new_grid)
 
         xyzr = self._cache_voxelization.xyzr_tensor
-        voxel_shape = xyzr.shape
         xyzr = xyzr.view(-1, 1, 1, 4)
 
         weight = self.tensor_args.to_device([1.0])

src/curobo/geom/sdf/world_blox.py

@@ -127,6 +127,7 @@ class WorldBloxCollision(WorldVoxelCollision):
         collision_query_buffer: CollisionQueryBuffer,
         weight,
         activation_distance,
+        return_loss: bool = False,
         compute_esdf: bool = False,
     ):
         d = self._blox_mapper.query_sphere_sdf_cost(
@@ -136,8 +137,11 @@ class WorldBloxCollision(WorldVoxelCollision):
             collision_query_buffer.blox_collision_buffer.sparsity_index_buffer,
             weight,
             activation_distance,
+            self.max_esdf_distance,
             self._blox_tensor_list[0],
             self._blox_tensor_list[1],
+            return_loss,
+            compute_esdf,
         )
         return d
 
@@ -191,12 +195,12 @@ class WorldBloxCollision(WorldVoxelCollision):
                 sum_collisions=sum_collisions,
                 compute_esdf=compute_esdf,
             )
-
         d = self._get_blox_sdf(
             query_sphere,
             collision_query_buffer,
             weight=weight,
             activation_distance=activation_distance,
+            return_loss=return_loss,
             compute_esdf=compute_esdf,
         )
 
@@ -246,6 +250,7 @@ class WorldBloxCollision(WorldVoxelCollision):
             collision_query_buffer,
             weight=weight,
             activation_distance=activation_distance,
+            return_loss=return_loss,
         )
         if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
             "mesh" not in self.collision_types or not self.collision_types["mesh"]
@@ -404,7 +409,7 @@ class WorldBloxCollision(WorldVoxelCollision):
         cuboid: Cuboid,
         layer_name: Optional[str] = None,
     ):
-        raise NotImplementedError
+        log_error("Not implemented")
 
     def get_bounding_spheres(
         self,
@@ -418,7 +423,8 @@ class WorldBloxCollision(WorldVoxelCollision):
         clear_region: bool = False,
     ) -> List[Sphere]:
         mesh = self.get_mesh_in_bounding_box(bounding_box, obstacle_name, clear_region=clear_region)
-
+        if clear_region:
+            self.clear_bounding_box(bounding_box, obstacle_name)
         spheres = mesh.get_bounding_spheres(
             n_spheres=n_spheres,
             surface_sphere_radius=surface_sphere_radius,

src/curobo/geom/sdf/world_voxel.py

@@ -80,6 +80,9 @@ class WorldVoxelCollision(WorldMeshCollision):
         if feature_dtype in [torch.float32, torch.float16, torch.bfloat16]:
             voxel_features[:] = -1.0 * self.max_esdf_distance
         else:
+            if self.max_esdf_distance > 100.0:
+                log_warn("Using fp8 for WorldVoxelCollision will reduce max_esdf_distance to 100")
+                self.max_esdf_distance = 100.0
             voxel_features = (voxel_features.to(dtype=torch.float16) - self.max_esdf_distance).to(
                 dtype=feature_dtype
             )
@@ -503,9 +506,10 @@ class WorldVoxelCollision(WorldMeshCollision):
             False,
             use_batch_env,
             False,
-            True,
+            False,
             False,
         )
+
         if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
             "mesh" not in self.collision_types or not self.collision_types["mesh"]
         ):

src/curobo/geom/sphere_fit.py

@@ -92,6 +92,7 @@ def voxel_fit_surface_mesh(
 
 
 def get_voxelgrid_from_mesh(mesh: trimesh.Trimesh, n_spheres: int, voxelize_method: str = "ray"):
+    """Get voxel grid from mesh using :py:func:`trimesh.voxel.creation.voxelize`."""
     pitch = get_voxel_pitch(mesh, n_spheres)
     radius = pitch / 2.0
     try:

src/curobo/geom/types.py

@@ -58,9 +58,9 @@ class Obstacle:
     texture: Optional[str] = None
 
     #: material properties to apply in visualization.
-    material: Material = Material()
+    material: Material = field(default_factory=Material)
 
-    tensor_args: TensorDeviceType = TensorDeviceType()
+    tensor_args: TensorDeviceType = field(default_factory=TensorDeviceType)
 
     def get_trimesh_mesh(self, process: bool = True, process_color: bool = True) -> trimesh.Trimesh:
         """Create a trimesh instance from the obstacle representation.