update to 0.6.2

src/curobo/geom/sdf/world.py

@@ -848,13 +848,13 @@ class WorldPrimitiveCollision(WorldCollision):
         trange = [h - l for l, h in zip(low, high)]
 
         x = torch.linspace(
-            -bounds[0], bounds[0], int(trange[0] // voxel_size), device=self.tensor_args.device
+            -bounds[0], bounds[0], int(trange[0] // voxel_size) + 1, device=self.tensor_args.device
         )
         y = torch.linspace(
-            -bounds[1], bounds[1], int(trange[1] // voxel_size), device=self.tensor_args.device
+            -bounds[1], bounds[1], int(trange[1] // voxel_size) + 1, device=self.tensor_args.device
         )
         z = torch.linspace(
-            -bounds[2], bounds[2], int(trange[2] // voxel_size), device=self.tensor_args.device
+            -bounds[2], bounds[2], int(trange[2] // voxel_size) + 1, device=self.tensor_args.device
         )
         w, l, h = x.shape[0], y.shape[0], z.shape[0]
         xyz = (
@@ -893,8 +893,11 @@ class WorldPrimitiveCollision(WorldCollision):
         voxel_size: float = 0.02,
     ) -> Mesh:
         voxels = self.get_voxels_in_bounding_box(cuboid, voxel_size)
+        # voxels = voxels.cpu().numpy()
+        # cuboids = [Cuboid(name="c_"+str(x), pose=[voxels[x,0],voxels[x,1],voxels[x,2], 1,0,0,0], dims=[voxel_size, voxel_size, voxel_size]) for x in range(voxels.shape[0])]
+        # mesh = WorldConfig(cuboid=cuboids).get_mesh_world(True).mesh[0]
         mesh = Mesh.from_pointcloud(
             voxels[:, :3].detach().cpu().numpy(),
-            pitch=voxel_size,
+            pitch=voxel_size * 1.1,
         )
         return mesh

src/curobo/geom/sdf/world_blox.py

@@ -76,8 +76,8 @@ class WorldBloxCollision(WorldMeshCollision):
                 self._blox_mapper = Mapper(
                     voxel_sizes=voxel_sizes,
                     integrator_types=integrator_types,
-                    cuda_device_id=self.tensor_args.device.index,
                     free_on_destruction=False,
+                    cuda_device_id=self.tensor_args.device.index,
                 )
                 self._blox_voxel_sizes = voxel_sizes
             # load map from file if it exists:
@@ -412,6 +412,8 @@ class WorldBloxCollision(WorldMeshCollision):
         index = self._blox_names.index(layer_name)
         pose_mat = camera_observation.pose.get_matrix().view(4, 4)
         if camera_observation.rgb_image is not None:
+            if camera_observation.rgb_image.shape[-1] != 4:
+                log_error("nvblox color should be of shape HxWx4")
             with profiler.record_function("world_blox/add_color_frame"):
                 self._blox_mapper.add_color_frame(
                     camera_observation.rgb_image,
@@ -473,6 +475,11 @@ class WorldBloxCollision(WorldMeshCollision):
             )
         return mesh
 
+    def save_layer(self, layer_name: str, file_name: str) -> bool:
+        index = self._blox_names.index(layer_name)
+        status = self._blox_mapper.save_map(file_name, index)
+        return status
+
     def decay_layer(self, layer_name: str):
         index = self._blox_names.index(layer_name)
         self._blox_mapper.decay_occupancy(mapper_id=index)

src/curobo/geom/types.py

@@ -391,9 +391,9 @@ class Mesh(Obstacle):
                 vertex_normals=self.vertex_normals,
                 face_colors=self.face_colors,
             )
-        # if self.scale is not None:
-        #    m.vertices = np.ravel(self.scale) * m.vertices
-
+        if self.scale is not None:
+            m.vertices = np.ravel(self.scale) * m.vertices
+            self.scale = None
         return m
 
     def update_material(self):