curobo.geom.sdf.world module

World representations for computing signed distance are implemented in this module.

class CollisionBuffer(
distance_buffer: Tensor,
grad_distance_buffer: Tensor,
sparsity_index_buffer: Tensor,
shape: Size | None = None,
)

Bases: object

Helper class stores all buffers required to compute collision cost and gradients.

distance_buffer: Tensor

Buffer to store signed distance cost value for each query sphere.

grad_distance_buffer: Tensor

Buffer to store gradient of signed distance cost value for each query sphere.

sparsity_index_buffer: Tensor

Buffer to store sparsity index for each query sphere. If sphere’s value is not in collsiion, sparsity index is set to 0, else 1. Used to prevent rewriting 0 values in distance_buffer and grad_distance_buffer.

shape: Size | None = None

Shape of the distance buffer. This is used to check if the buffer needs to be recreated.

classmethod initialize_from_shape(
shape: Size,
tensor_args: TensorDeviceType,
) → CollisionBuffer

Initialize the CollisionBuffer from the given shape of query spheres.

Parameters:

  • shape – Input shape of the query spheres. The shape is (batch, horizon, n_spheres, 4).

  • tensor_args – Device and precision of the tensors.

Returns:

Initialized CollisionBuffer object.

Return type:

CollisionBuffer

_update_from_shape(
shape: Size,
tensor_args: TensorDeviceType,
)

Update shape of buffers.

Parameters:

  • shape – New shape of the query spheres.

  • tensor_args – device and precision of the tensors.

update_buffer_shape(
shape: Size,
tensor_args: TensorDeviceType,
)

Update the buffer shape if the shape of the query spheres changes.

Parameters:

  • shape – New shape of the query spheres.

  • tensor_args – device and precision of the tensors.

clone() → CollisionBuffer

Clone the CollisionBuffer object.

class CollisionQueryBuffer(
primitive_collision_buffer: CollisionBuffer | None = None,
mesh_collision_buffer: CollisionBuffer | None = None,
blox_collision_buffer: CollisionBuffer | None = None,
voxel_collision_buffer: CollisionBuffer | None = None,
shape: Size | None = None,
)

Bases: object

Class stores all buffers required to query collisions across world representations.

primitive_collision_buffer: CollisionBuffer | None = None

Buffer to store signed distance cost value for Cuboid world obstacles.

mesh_collision_buffer: CollisionBuffer | None = None

Buffer to store signed distance cost value for Mesh world obstacles.

blox_collision_buffer: CollisionBuffer | None = None

Buffer to store signed distance cost value for Blox world obstacles.

voxel_collision_buffer: CollisionBuffer | None = None

Buffer to store signed distance cost value for Voxel world obstacles.

shape: Size | None = None

Shape of the query spheres. This is used to check if the buffer needs to be recreated.

clone() → CollisionQueryBuffer

Clone the CollisionQueryBuffer object.

classmethod initialize_from_shape(
shape: Size,
tensor_args: TensorDeviceType,
collision_types: Dict[str, bool],
) → CollisionQueryBuffer

Initialize the CollisionQueryBuffer from the given shape of query spheres.

Parameters:

  • shape – Input shape of the query spheres. The shape is (batch, horizon, n_spheres, 4).

  • tensor_args – Device and precision of the tensors.

  • collision_types – Dictionary of collision types to initialize buffers for.

Returns:

Initialized CollisionQueryBuffer object.

Return type:

CollisionQueryBuffer

create_from_shape(
shape: Size,
tensor_args: TensorDeviceType,
collision_types: Dict[str, bool],
) → CollisionQueryBuffer

Create the CollisionQueryBuffer from the given shape of query spheres.

Parameters:

  • shape – Input shape of the query spheres. The shape is (batch, horizon, n_spheres, 4).

  • tensor_args – Device and precision of the tensors.

  • collision_types – Dictionary of collision types to initialize buffers for.

Returns:

Initialized CollisionQueryBuffer object.

Return type:

CollisionQueryBuffer

update_buffer_shape(
shape: Size,
tensor_args: TensorDeviceType,
collision_types: Dict[str, bool] | None,
)

Update buffer shape if it doesn’t match existing shape.

Parameters:

  • shape – New shape of the query spheres.

  • tensor_args – Device and precision of the tensors.

  • collision_types – Dictionary of collision types to update buffers for.

get_gradient_buffer() → Tensor | None

Compute the gradient buffer by summing the gradient buffers of all collision types.

Returns:

Gradient buffer for all collision types

Return type:

torch.Tensor

class CollisionCheckerType(value)

Bases: Enum

Type of collision checker to use.

PRIMITIVE = 'PRIMITIVE'
BLOX = 'BLOX'
MESH = 'MESH'
VOXEL = 'VOXEL'
class WorldCollisionConfig(
tensor_args: TensorDeviceType,
world_model: List[WorldConfig] | WorldConfig | None = None,
cache: Dict[Obstacle, int] | None = None,
n_envs: int = 1,
checker_type: CollisionCheckerType = CollisionCheckerType.PRIMITIVE,
max_distance: Tensor | float = 0.1,
max_esdf_distance: Tensor | float = 100.0,
)

Bases: object

Configuration parameters for the WorldCollision object.

tensor_args: TensorDeviceType

Device and precision of the tensors.

world_model: List[WorldConfig] | WorldConfig | None = None

World obstacles to load for collision checking.

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.

n_envs: int = 1

Number of environments to use for collision checking.

checker_type: CollisionCheckerType = 'PRIMITIVE'

Type of collision checker to use.

max_distance: 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: Tensor | float = 100.0

Maximum distance outside an obstacle to use when computing euclidean signed distance field (ESDF) from different world representations.

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

class WorldCollision(
config: WorldCollisionConfig | None = None,
)

Bases: WorldCollisionConfig

Base class for computing signed distance between query spheres and world obstacles.

Initialize the WorldCollision object.

Parameters:

config – Configuration parameters for the WorldCollision object.

load_collision_model(
world_model: WorldConfig,
)

Load the world obstacles for collision checking.

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.

get_sphere_distance(
query_sphere,
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,
)

Compute the 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: Tensor | None = None,
return_loss: bool = False,
)

Compute binary collision between query spheres and world obstacles.

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,
)

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

get_swept_sphere_collision(
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,
)

Compute binary collision between trajectory of spheres and world obstacles.

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.

clear_voxelization_cache()

Clear cache that contains voxelization locations.

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.

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_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:
Returns:

Voxels with the Euclidean signed distance in the bounding box.

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_obstacle_names(
env_idx: int = 0,
) → List[str]

Get the names of the obstacles in the world.

Parameters:

env_idx – Environment index to get the obstacle names.

Returns:

Obstacle names in the world.

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.

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.

checker_type: CollisionCheckerType = 'PRIMITIVE'

Type of collision checker to use.

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: 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: 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.

world_model: List[WorldConfig] | WorldConfig | None = None

World obstacles to load for collision checking.

tensor_args: TensorDeviceType

Device and precision of the tensors.

class WorldPrimitiveCollision(
config: WorldCollisionConfig,
)

Bases: WorldCollision

World collision checking with oriented bounding boxes (cuboids) for obstacles.

Initialize the WorldPrimitiveCollision object.

Parameters:

config – Configuration parameters for the WorldPrimitiveCollision object.

_init_cache()

Initialize obstacles cache to allow for dynamic addition of obstacles.

load_collision_model(
world_config: WorldConfig,
env_idx=0,
fix_cache_reference: bool = False,
)

Load world obstacles into collision checker.

Parameters:

  • world_config – Obstacles to load into the collision checker.

  • env_idx – Environment index to load the obstacles.

  • 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.

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

Get the names of the obstacles in the world.

Parameters:

env_idx – Environment index to get the obstacle names.

Returns:

Obstacle names in the world.

load_batch_collision_model(
world_config_list: List[WorldConfig],
)

Load a batch of collision environments from a list of world configs.

Parameters:

world_config_list – list of world configs to load from.

_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.

_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.

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.

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.

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.

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_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.

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

Update pose of an existing obstacle.

Parameters:

  • name – Name of obstacle.

  • w_obj_pose – Pose of the obstacle in world frame.

  • env_idx – Index of the environment to update the 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_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.

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_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_sphere_distance(
query_sphere: Tensor,
collision_query_buffer: CollisionQueryBuffer,
weight: Tensor,
activation_distance: Tensor,
env_query_idx: Tensor | None = None,
return_loss=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: Tensor,
collision_query_buffer: CollisionQueryBuffer,
weight: Tensor,
activation_distance: Tensor,
env_query_idx: Tensor | None = None,
return_loss=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: Tensor,
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=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: Tensor,
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=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.

clear_cache()

Delete all cuboid obstacles from 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.

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:
Returns:

Voxels with the Euclidean signed distance in the bounding box.

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_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_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.

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: 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: 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_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.

world_model: List[WorldConfig] | WorldConfig | None = None

World obstacles to load for collision checking.

tensor_args: TensorDeviceType

Device and precision of the tensors.