curobo.geom.sdf.world_blox module¶

World represented by ESDF layers of nvblox.

class WorldBloxCollision( config: WorldCollisionConfig, )¶

Bases: WorldVoxelCollision

World Collision Representaiton using Nvidia’s nvblox library.

This class depends on pytorch wrapper for nvblox. Additionally, this representation does not support batched environments as we only store one world via nvblox.

There are two ways to use nvblox, one is by loading maps from disk and the other is by creating maps online. In both these instances, we might load more than one map and need to check collisions against all maps.To facilitate online map creation and updation, we build apis in this class to process depth images.

Initialize with a world collision configuration.

load_collision_model( world_model: WorldConfig, fix_cache_reference: bool = False, )¶

Load collision model from world obstacles. Only 1 environment is supported.

Parameters:

world_model – Obstacles in world to load.
fix_cache_reference – If True, throws error if number of obstacles is greater than cache. If False, creates a larger cache. Note that when using collision checker inside a recorded cuda graph, recreating the cache will break the graph as the reference pointer to the cache will change.

clear_cache()¶: Clear obstacle cache, clears nvblox maps and other obstacles.

clear_blox_layer( layer_name: str, )¶: Clear a specific blox layer.

get_sphere_distance( query_sphere: Tensor, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, activation_distance: Tensor, env_query_idx: Tensor | None = None, return_loss: bool = False, sum_collisions: bool = True, compute_esdf: bool = False, ) → Tensor¶

Compute the signed distance between query spheres and world obstacles.

This distance can be used as a collision cost for optimization.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Buffer to store collision query results.
weight – Weight of the collision cost.
activation_distance – Distance outside the object to start computing the cost.
env_query_idx – Environment index for each batch of query spheres.
return_loss – If the returned tensor will be scaled or changed before calling backward, set this to True. If the returned tensor will be used directly through addition, set this to False.
sum_collisions – Sum the collision cost across all obstacles. This variable is currently not passed to the underlying CUDA kernel as setting this to False caused poor performance.
compute_esdf – Compute Euclidean signed distance instead of collision cost. When True, the returned tensor will be the signed distance with positive values inside an obstacle and negative values outside obstacles.

Returns:

Signed distance between query spheres and world obstacles.

get_sphere_collision(

query_sphere,

collision_query_buffer: CollisionQueryBuffer,

weight: Tensor,

activation_distance: Tensor,

env_query_idx=None,

return_loss: bool = False,

**kwargs,

) → Tensor¶

Compute binary collision between query spheres and world obstacles.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Collision query buffer to store the results.
weight – Weight to scale the collision cost.
activation_distance – Distance outside the object to start computing the cost.
env_query_idx – Environment index for each batch of query spheres.
return_loss – True is not supported for binary classification. Set to False.

Returns:

Tensor with binary collision results.

get_swept_sphere_distance( query_sphere, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, activation_distance: Tensor, speed_dt: Tensor, sweep_steps: int, enable_speed_metric=False, env_query_idx: Tensor | None = None, return_loss: bool = False, sum_collisions: bool = True, ) → Tensor¶

Compute the signed distance between trajectory of spheres and world obstacles.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Collision query buffer to store the results.
weight – Collision cost weight.
activation_distance – Distance outside the object to start computing the cost. A smooth scaling is applied to the cost starting from this distance. See Technical Report for more details.
speed_dt – Length of time (seconds) to use when calculating the speed of the sphere using finite difference.
sweep_steps – Number of steps to sweep the sphere along the trajectory. More steps will allow for catching small obstacles, taking more time to compute.
enable_speed_metric – True will scale the collision cost by the speed of the sphere. This has the effect of slowing down the robot when near obstacles. This also has shown to improve convergence from poor initialization.
env_query_idx – Environment index for each batch of query spheres.
return_loss – If the returned tensor will be scaled or changed before calling backward, set this to True. If the returned tensor will be used directly through addition, set this to False.
sum_collisions – Sum the collision cost across all obstacles. This variable is currently not passed to the underlying CUDA kernel as setting this to False caused poor performance.

Returns:

Collision cost between trajectory of spheres and world obstacles.

get_swept_sphere_collision( query_sphere, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, sweep_steps, activation_distance: Tensor, speed_dt: Tensor, enable_speed_metric=False, env_query_idx: Tensor | None = None, return_loss: bool = False, ) → Tensor¶

Get binary collision between trajectory of spheres and world obstacles.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Collision query buffer to store the results.
weight – Collision cost weight.
activation_distance – Distance outside the object to start computing the cost. A smooth scaling is applied to the cost starting from this distance. See Technical Report for more details.
speed_dt – Length of time (seconds) to use when calculating the speed of the sphere using finite difference. This is not used.
sweep_steps – Number of steps to sweep the sphere along the trajectory. More steps will allow for catching small obstacles, taking more time to compute.
enable_speed_metric – True will scale the collision cost by the speed of the sphere. This has the effect of slowing down the robot when near obstacles. This also has shown to improve convergence from poor initialization. This is not used.
env_query_idx – Environment index for each batch of query spheres.
return_loss – This is not supported for binary classification. Set to False.

Returns:

Collision value between trajectory of spheres and world obstacles.

enable_obstacle( name: str, enable: bool = True, env_idx: int = 0, )¶

Enable/Disable object in collision checking functions.

Parameters:

name – Name of the obstacle to enable.
enable – True to enable, False to disable.
env_idx – Index of the environment to enable the obstacle in.

enable_blox( enable: bool = True, name: str | None = None, )¶

Enable/Disable nvblox layer for collision checking.

Parameters:

enable – True to enable, False to disable.
name – Name of the nvblox layer to enable.

update_blox_pose( w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, name: str | None = None, )¶

Update pose of a nvblox layer.

Parameters:

w_obj_pose – Pose of layer in world frame.
obj_w_pose – Inverse pose of layer. If w_obj_pose is provided, this is not required.
name – Name of the nvblox layer to update.

clear_bounding_box( cuboid: Cuboid, layer_name: str | None = None, )¶

Clear occupied regions of a nvblox layer. Not implemented.

Parameters:

cuboid – Bounding box to clear.
layer_name – Name of nvblox layer.

get_bounding_spheres( bounding_box: Cuboid, obstacle_name: str | None = None, n_spheres: int = 1, surface_sphere_radius: float = 0.002, fit_type: SphereFitType = SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE, voxelize_method: str = 'ray', pre_transform_pose: Pose | None = None, clear_region: bool = False, clear_region_layer: str | None = None, ) → List[Sphere]¶

Get spheres within a bounding box.

Parameters:

bounding_box – Bounding box to find occupied region.
obstacle_name – Name to use for created occupied region. Not useful, use any random name.
n_spheres – Number of spheres to use for approximating the occupied region.
surface_sphere_radius – Radius to use for surface spheres.
fit_type – Sphere fit algorithm to use. See Geometry Approximation to Spheres for more details. The default method SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE voxelizes the volume of the objects and adds spheres representing the voxels, then samples points on the surface of the object, adds surface_sphere_radius to these points. This should be used for most cases.
voxelize_method – Method to use for voxelization, passed to trimesh.voxel.creation.voxelize.
pre_transform_pose – Optional pose to transform the bounding box before finding spheres.
clear_region – Clear region in nvblox layer. Not supported.
clear_region_layer – Layer of nvblox to clear region.

Returns:

Spheres approximating occupied region.

add_camera_frame( camera_observation: CameraObservation, layer_name: str, )¶

Add a camera image to nvblox layer.

Parameters:

camera_observation – New image to add to nvblox layer.
layer_name – Name of nvblox layer.

process_camera_frames( layer_name: str | None = None, process_aux: bool = False, )¶

Integrate ESDF data from camera frames into nvblox layer.

Parameters:

layer_name – Name of nvblox layer. If None, all layers are processed.
process_aux – Process color frames, useful for visualization.

update_blox_hashes()¶: Update hashmaps for nvblox layers. Required after processing camera frames.

update_blox_esdf( layer_name: str | None = None, )¶

Integrate ESDF data from camera frames into nvblox layer.

Parameters:: layer_name – Name of nvblox layer. If None, all layers are processed.

update_blox_mesh( layer_name: str | None = None, )¶

Update mesh data for nvblox layer. Requires RGB data.

Parameters:: layer_name – Name of nvblox layer. If None, all layers are processed.

get_mesh_from_blox_layer( layer_name: str, mode: str = 'nvblox', ) → Mesh¶

Get Mesh from nvblox layer.

Parameters:

layer_name – Name of nvblox layer.
mode – If mode is “nvblox”, mesh is generated using nvblox’s internal mesh construction method. This relies on RGB data from camera frames. If mode is “voxel”, mesh is generated using occupancy.

Returns:

Mesh object.

save_layer( layer_name: str, file_name: str, ) → bool¶

Save nvblox layer to disk.

Parameters:

layer_name – Name of nvblox layer.
file_name – File path to save layer.

Returns:

True if successful, False otherwise.

decay_layer( layer_name: str, )¶

Decay nvblox layer. This will remove any stale voxels in the layer.

Parameters:: layer_name – Name of nvblox layer to decay.

get_obstacle_names( env_idx: int = 0, ) → List[str]¶

Get names of all obstacles in the environment.

Parameters:: env_idx – Environment index to get obstacles from.
Returns:: List of obstacle names.

_get_blox_sdf( query_spheres: Tensor, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, activation_distance: Tensor, return_loss: bool = False, compute_esdf: bool = False, ) → Tensor¶

Compute the signed distance between query spheres and world obstacles.

This distance can be used as a collision cost for optimization.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Buffer to store collision query results.
weight – Weight of the collision cost.
activation_distance – Distance outside the object to start computing the cost.
env_query_idx – Environment index for each batch of query spheres.
return_loss – If the returned tensor will be scaled or changed before calling backward, set this to True. If the returned tensor will be used directly through addition, set this to False.
compute_esdf – Compute Euclidean signed distance instead of collision cost. When True, the returned tensor will be the signed distance with positive values inside an obstacle and negative values outside obstacles.

Returns:

Signed distance between query spheres and world obstacles.

_get_blox_swept_sdf( query_spheres: Tensor, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, activation_distance: Tensor, speed_dt: Tensor, sweep_steps: int, enable_speed_metric: bool, return_loss: bool = False, ) → Tensor¶

Compute the signed distance between trajectory of spheres and world obstacles.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Collision query buffer to store the results.
weight – Collision cost weight.
activation_distance – Distance outside the object to start computing the cost. A smooth scaling is applied to the cost starting from this distance. See Technical Report for more details.
speed_dt – Length of time (seconds) to use when calculating the speed of the sphere using finite difference.
sweep_steps – Number of steps to sweep the sphere along the trajectory. More steps will allow for catching small obstacles, taking more time to compute.
enable_speed_metric – True will scale the collision cost by the speed of the sphere. This has the effect of slowing down the robot when near obstacles. This also has shown to improve convergence from poor initialization.
return_loss – If the returned tensor will be scaled or changed before calling backward, set this to True. If the returned tensor will be used directly through addition, set this to False.

Returns:

Collision cost between trajectory of spheres and world obstacles.

_batch_tensor_voxel( pose: List[List[float]], dims: List[float], voxel_size: List[float], ) → List[Tensor]¶

Create a list of tensors that represent the voxel parameters.

Parameters:

pose – Pose of voxel grids.
dims – Dimensions of voxel grids.
voxel_size – Resolution of voxel grids.

Returns:

List of tensors representing the voxel parameters.

_create_mesh_cache( mesh_cache: int, )¶

Create cache for mesh obstacles.

Parameters:: mesh_cache – Number of mesh obstacles to cache.

_create_obb_cache( obb_cache: int, )¶

Create cache for cuboid (oriented bounding box) obstacles.

Parameters:: obb_cache – Number of cuboids to cache for collision checking.

_create_voxel_cache( voxel_cache: Dict[str, Any], )¶

Create a cache for voxel grid representation of the world.

Parameters:: voxel_cache – Parameters for the voxel grid representation. The dictionary should contain the following keys: layers, dims, voxel_size, feature_dtype. Current implementation assumes that all voxel grids have the same number of voxels. Though different layers can have different resolutions, this is not yet thoroughly tested.

classmethod _get_obstacle_poses( w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, )¶

Get pose of world from obstacle frame of reference.

Parameters:

w_obj_pose – Pose of the obstacle in world frame.
obj_w_pose – Pose of world in obstacle frame of reference.

Returns:

Pose of world in obstacle frame of reference.

_get_sdf( query_spheres: Tensor, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, activation_distance: Tensor, env_query_idx: Tensor | None = None, return_loss: bool = False, compute_esdf: bool = False, ) → Tensor¶

Compute signed distance for mesh obstacles using warp kernel.

Parameters:

query_spheres – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Collision query buffer to store the results.
weight – Weight to scale the collision cost.
activation_distance – Distance outside the object to start computing the cost.
env_query_idx – Environment index for each batch of query spheres.
return_loss – If the returned tensor will be scaled or changed before calling backward, set this to True. If the returned tensor will be used directly through addition, set this to False.
compute_esdf – Compute Euclidean signed distance instead of collision cost. When True, the returned tensor will be the signed distance with positive values inside an obstacle and negative values outside obstacles.

Returns:

Collision cost between query spheres and world obstacles.

_get_swept_sdf( query_spheres: Tensor, collision_query_buffer: CollisionQueryBuffer, weight: Tensor, activation_distance: Tensor, speed_dt: Tensor, sweep_steps: int, enable_speed_metric=False, env_query_idx=None, return_loss: bool = False, ) → Tensor¶

Compute signed distance for mesh obstacles using warp kernel.

Parameters:

query_sphere – Input tensor with query spheres [batch, horizon, number of spheres, 4]. With [x, y, z, radius] as the last column for each sphere.
collision_query_buffer – Collision query buffer to store the results.
weight – Collision cost weight.
activation_distance – Distance outside the object to start computing the cost. A smooth scaling is applied to the cost starting from this distance. See Technical Report for more details.
speed_dt – Length of time (seconds) to use when calculating the speed of the sphere using finite difference.
sweep_steps – Number of steps to sweep the sphere along the trajectory. More steps will allow for catching small obstacles, taking more time to compute.
enable_speed_metric – True will scale the collision cost by the speed of the sphere. This has the effect of slowing down the robot when near obstacles. This also has shown to improve convergence from poor initialization.
env_query_idx – Environment index for each batch of query spheres.
return_loss – If the returned tensor will be scaled or changed before calling backward, set this to True. If the returned tensor will be used directly through addition, set this to False.

Returns:

Collision cost between trajectory of spheres and world obstacles.

_init_cache()¶: Initialize the cache for the world.

_load_batch_mesh_to_warp( mesh_list: List[Mesh], ) → List¶

Load multiple meshes into warp.

Parameters:: mesh_list – List of meshes to load.
Returns:: List of mesh names, mesh ids, and inverse poses.

_load_collision_model_in_cache( world_config: WorldConfig, env_idx: int = 0, fix_cache_reference: bool = False, )¶

Load world obstacles into collision checker cache. This only loads cuboids.

Parameters:

world_config – World obstacles to load into the collision checker.
env_idx – Environment index to load the obstacles.
fix_cache_reference – If True, does not allow to load more obstacles than cache size.

_load_mesh_into_cache( mesh: Mesh, ) → WarpMeshData¶

Load cuRobo mesh into cache.

Parameters:: mesh – mesh instance to load.
Returns:: loaded mesh data.

_load_mesh_to_warp( mesh: Mesh, ) → WarpMeshData¶

Load cuRobo mesh into warp.

Parameters:: mesh – mesh instance to load.
Returns:: loaded mesh data.

_load_voxel_collision_model_in_cache( world_config: WorldConfig, env_idx: int = 0, fix_cache_reference: bool = False, )¶

Load voxel grid representation of the world into the cache.

Parameters:

world_config – Obstacles in world to load.
env_idx – Environment index to load obstacles into.
fix_cache_reference – If True, throws error if number of obstacles is greater than cache. If False, creates a larger cache. Note that when using collision checker inside a recorded cuda graph, recreating the cache will break the graph as the reference pointer to the cache will change.

add_mesh( new_mesh: Mesh, env_idx: int = 0, )¶

Add a mesh to the world.

Parameters:

new_mesh – Mesh to add.
env_idx – Environment index to add mesh to.

add_obb( cuboid: Cuboid, env_idx: int = 0, ) → int¶

Add cuboid obstacle to world.

Parameters:

cuboid – Cuboid to add.
env_idx – Environment index to add the obstacle to.

Returns:

Index of the obstacle in the world.

add_obb_from_raw( name: str, dims: Tensor, env_idx: int, w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, ) → int¶

Add cuboid obstacle to world.

Parameters:

name – Name of the obstacle. Must be unique.
dims – Dimensions of the cuboid obstacle [length, width, height].
env_idx – Environment index to add the obstacle to.
w_obj_pose – Pose of the obstacle in world frame.
obj_w_pose – Inverse pose of the obstacle in world frame.

Returns:

Index of the obstacle in the world.

cache: Dict[Obstacle, int] | None = None¶: Number of obstacles to cache for collision checking across representations. Use this to create a fixed size buffer for collision checking, e.g, {‘obb’: 1000} will create a buffer of 1000 cuboids for each environment.

check_obstacle_exists( name: str, env_idx: int = 0, ) → bool¶

Check if an obstacle exists in the world by name.

Parameters:

name – Name of the obstacle to check.
env_idx – Environment index to check the obstacle.

Returns:

True if the obstacle exists in the world, else False.

checker_type: CollisionCheckerType = 'PRIMITIVE'¶: Type of collision checker to use.

clear_voxelization_cache()¶: Clear cache that contains voxelization locations.

create_collision_cache( mesh_cache: int | None = None, obb_cache: int | None = None, n_envs: int | None = None, )¶

Create cache of obstacles.

Parameters:

mesh_cache – Number of mesh obstacles to cache.
obb_cache – Number of OBB (cuboid) obstacles to cache.
n_envs – Number of environments to cache.

enable_mesh( enable: bool = True, name: str | None = None, env_mesh_idx: Tensor | None = None, env_idx: int = 0, )¶

Enable/Disable mesh in collision checking functions.

Parameters:

enable – True to enable, False to disable.
name – Name of the mesh to enable.
env_mesh_idx – Index of the mesh in environment. If name is given, this is ignored.
env_idx – Environment index to enable the mesh in.

enable_obb( enable: bool = True, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Enable/Disable cuboid in collision checking functions.

Parameters:

enable – True to enable, False to disable.
name – Name of the obstacle to enable.
env_obj_idx – Index of the obstacle to enable. Not required if name is provided.
env_idx – Index of the environment to enable the obstacle in.

enable_voxel( enable: bool = True, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Enable/Disable voxel grid in collision checking functions.

Parameters:

enable – True to enable, False to disable.
name – Name of voxel grid to enable.
env_obj_idx – Index of voxel grid. If name is provided, this is ignored.
env_idx – Environment index to enable the voxel grid in.

get_esdf_in_bounding_box( cuboid: Cuboid = Cuboid(name='test', pose=[0, 0, 0, 1, 0, 0, 0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[1, 1, 1]), voxel_size: float = 0.02, dtype=torch.float32, ) → VoxelGrid¶

Get the Euclidean signed distance in a grid bounded by the given cuboid.

Distance is positive inside obstacles and negative outside obstacles.

Parameters:

cuboid – Bounding cuboid to query signed distance.
voxel_size – Size of the voxels in meters.
dtype – Data type of the feature tensor. Use :var:`torch.bfloat16` or :var:`torch.float8_e4m3fn` for reduced memory usage.

Returns:

Voxels with the Euclidean signed distance in the bounding box.

get_mesh_idx( name: str, env_idx: int = 0, ) → int¶

Get index of mesh with given name.

Parameters:

name – Name of the mesh.
env_idx – Environment index to search in.

Returns:

Mesh index.

get_mesh_in_bounding_box( cuboid: Cuboid = Cuboid(name='test', pose=[0, 0, 0, 1, 0, 0, 0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[1, 1, 1]), voxel_size: float = 0.02, run_marching_cubes: bool = True, ) → Mesh¶

Get a mesh representation of the world obstacles based on occupancy in a bounding box.

This uses marching cubes to create a mesh representation of the world obstacles. Use this to debug world representations.

Parameters:

cuboid – Bounding box to get the mesh representation.
voxel_size – Size of the voxels in meters.
run_marching_cubes – Runs marching cubes over occupied voxels to generate a mesh. If set to False, then all occupied voxels are merged into a mesh and returned.

Returns:

Mesh representation of the world obstacles in the bounding box.

get_obb_idx( name: str, env_idx: int = 0, ) → int¶

Get index of the cuboid obstacle in the world.

Parameters:

name – Name of the obstacle to get the index.
env_idx – Environment index to get the obstacle index.

Returns:

Index of the obstacle in the world.

get_occupancy_in_bounding_box( cuboid: Cuboid = Cuboid(name='test', pose=[0, 0, 0, 1, 0, 0, 0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[1, 1, 1]), voxel_size: float = 0.02, ) → VoxelGrid¶

Get the occupancy of voxels in a grid bounded by the given cuboid.

Parameters:

cuboid – Cuboid to get the occupancy of voxels. Provide pose and dimenstions to create occupancy information.
voxel_size – Size in meters to use as the voxel size.

Returns:

Grid with the occupancy of voxels in the bounding box.

get_voxel_grid( name: str, env_idx: int = 0, ) → VoxelGrid¶

Get voxel grid from world obstacles.

Parameters:

name – Name of voxel grid.
env_idx – Environment index to get voxel grid from.

Returns:

Voxel grid object.

get_voxel_grid_shape( env_idx: int = 0, obs_idx: int = 0, name: str | None = None, ) → Size¶

Get dimensions of the voxel grid.

Parameters:

env_idx – Environment index.
obs_idx – Obstacle index.
name – Name of obstacle. When provided, obs_idx is ignored.

Returns:

Shape of the voxel grid.

get_voxel_idx( name: str, env_idx: int = 0, ) → int¶

Get index of voxel grid in the environment.

Parameters:

name – Name of the voxel grid.
env_idx – Environment index to get voxel grid from.

Returns:

Index of voxel grid.

get_voxels_in_bounding_box( cuboid: Cuboid = Cuboid(name='test', pose=[0, 0, 0, 1, 0, 0, 0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[1, 1, 1]), voxel_size: float = 0.02, ) → List[Cuboid] | Tensor¶

Get occupied voxels in a grid bounded by the given cuboid.

Parameters:

cuboid – Bounding box to get the occupied voxels.
voxel_size – Size of the voxel grid.

Returns:

Tensor with the occupied voxels in the bounding box.

load_batch_collision_model( world_config_list: List[WorldConfig], )¶

Load voxel grid for batched environments

Parameters:: world_config_list – List of world obstacles for each environment.

static load_from_dict( world_coll_checker_dict: Dict, world_model_dict: WorldConfig | Dict | List[WorldConfig] | None = None, tensor_args: TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), ) → WorldCollisionConfig¶

Load the WorldCollisionConfig from a dictionary.

Parameters:

world_coll_checker_dict – Dictionary containing the configuration parameters.
world_model_dict – Dictionary containing obstacles.
tensor_args – Device and precision of the tensors.

Returns:

Initialized WorldCollisionConfig object.

Return type:

WorldCollisionConfig

max_distance: torch.Tensor | float = 0.1¶: Maximum distance to compute collision checking cost outside the object. This value is added in addition to a query sphere radius and collision activation distance. A smaller value will speedup collision checking but can result in slower convergence with swept sphere collision checking.

max_esdf_distance: torch.Tensor | float = 100.0¶: Maximum distance outside an obstacle to use when computing euclidean signed distance field (ESDF) from different world representations.

n_envs: int = 1¶: Number of environments to use for collision checking.

update_cache_voxelization( new_grid: VoxelGrid, )¶

Update locaiton of voxels based on new grid parameters. Only for debugging.

Parameters:: new_grid – New grid to use for getting voxelized occupancy of current world obstacles.

update_mesh_from_warp( warp_mesh_idx: int, w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, obj_idx: int = 0, env_idx: int = 0, name: str | None = None, )¶

Update mesh information in world cache.

Parameters:

warp_mesh_idx – New warp mesh index.
w_obj_pose – Pose of the mesh in world frame.
obj_w_pose – Invserse pose of the mesh. Not required if w_obj_pose is given.
obj_idx – Index of mesh in environment. If name is given, this is ignored.
env_idx – Environment index to update mesh in.
name – Name of mesh to update.

update_mesh_pose( w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Update pose of mesh.

Parameters:

w_obj_pose – Pose of the mesh in world frame.
obj_w_pose – Inverse pose of the mesh.
name – Name of mesh to update.
env_obj_idx – Index of mesh in environment. If name is given, this is ignored.
env_idx – Environment index to update mesh in.

update_obb_dims( obj_dims: Tensor, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Update dimensinots of an existing cuboid obstacle.

Parameters:

obj_dims – [dim.x,dim.y, dim.z].
name – Name of the obstacle to update.
env_obj_idx – Index of the obstacle to update. Not required if name is provided.
env_idx – Environment index to update the obstacle.

update_obb_pose( w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Update pose of an existing cuboid obstacle.

Parameters:

w_obj_pose – Pose of the obstacle in world frame.
obj_w_pose – Inverse pose of the obstacle in world frame. Not required if w_obj_pose is provided.
name – Name of the obstacle to update.
env_obj_idx – Index of the obstacle to update. Not required if name is provided.
env_idx – Index of the environment to update the obstacle.
update_cpu_reference – If True, updates the CPU reference with the new pose. This is useful for debugging and visualization. Only supported for env_idx=0.

update_obstacle_pose( name: str, w_obj_pose: Pose, env_idx: int = 0, update_cpu_reference: bool = False, )¶

Update pose of obstacle.

Parameters:

name – Name of the obstacle.
w_obj_pose – Pose of obstacle in world frame.
env_idx – Environment index to update obstacle in.
update_cpu_reference – If True, updates the CPU reference with the new pose. This is useful for debugging and visualization. Only supported for env_idx=0.

update_obstacle_pose_in_world_model( name: str, pose: Pose, env_idx: int = 0, )¶

Update the pose of an obstacle in the world model.

Parameters:

name – Name of the obstacle to update.
pose – Pose to update the obstacle.
env_idx – Environment index to update the obstacle.

update_voxel_data( new_voxel: VoxelGrid, env_idx: int = 0, )¶

Update parameters of a voxel grid. This can also updates signed distance values.

Parameters:

new_voxel – New parameters.
env_idx – Environment index to update voxel grid in.

update_voxel_features( features: Tensor, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Update signed distance values in a voxel grid.

Parameters:

features – New signed distance values.
name – Name of voxel grid obstacle.
env_obj_idx – Index of voxel grid. If name is provided, this is ignored.
env_idx – Environment index to update voxel grid in.

update_voxel_pose( w_obj_pose: Pose | None = None, obj_w_pose: Pose | None = None, name: str | None = None, env_obj_idx: Tensor | None = None, env_idx: int = 0, )¶

Update pose of voxel grid.

Parameters:

w_obj_pose – Pose of voxel grid in world frame.
obj_w_pose – Inverse pose of voxel grid. If provided, w_obj_pose is ignored.
name – Name of the voxel grid.
env_obj_idx – Index of voxel grid. If name is provided, this is ignored.
env_idx – Environment index to update voxel grid in.

world_model: List[WorldConfig] | WorldConfig | None = None¶: World obstacles to load for collision checking.

tensor_args: TensorDeviceType¶: Device and precision of the tensors.