curobo.geom.sdf.world_mesh module

World represented as Meshes can be used with this module for collision checking.

class WarpMeshData(
name: str,
m_id: int,
vertices: array,
faces: array,
mesh: Mesh,
)

Bases: object

Data helper to use with warp for representing a mesh

name: str

Name of the mesh.

m_id: int

Mesh ID, created by warp once mesh is loaded to device.

vertices: array

Vertices of the mesh.

faces: array

Faces of the mesh.

mesh: Mesh

Warp mesh instance.

class WorldMeshCollision(
config: WorldCollisionConfig,
)

Bases: WorldPrimitiveCollision

World Mesh Collision using Nvidia’s warp library.

Initialize World Mesh Collision with given configuration.

tensor_args: TensorDeviceType

Device and precision of the tensors.

_init_cache()

Initialize cache for storing meshes.

load_collision_model(
world_model: WorldConfig,
env_idx: int = 0,
load_obb_obs: bool = True,
fix_cache_reference: bool = False,
)

Load world obstacles into collision checker.

Parameters:

  • world_model – Obstacles to load.

  • env_idx – Environment index to load obstacles into.

  • load_obb_obs – Load OBB (cuboid) 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.

load_batch_collision_model(
world_config_list: List[WorldConfig],
)

Load multiple world obstacles into collision checker across environments.

Parameters:

world_config_list – List of obstacles to load.

_load_mesh_to_warp(
mesh: Mesh,
) → WarpMeshData

Load cuRobo mesh into warp.

Parameters:

mesh – mesh instance to load.

Returns:

loaded mesh data.

_load_mesh_into_cache(
mesh: Mesh,
) → WarpMeshData

Load cuRobo mesh into cache.

Parameters:

mesh – mesh instance to load.

Returns:

loaded mesh data.

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

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.

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.

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.

_create_mesh_cache(
mesh_cache: int,
)

Create cache for mesh obstacles.

Parameters:

mesh_cache – Number of mesh obstacles to cache.

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

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.

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.

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.

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

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=False,
**kwargs,
)

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

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

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 and mesh obstacles from the world.

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

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

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.

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

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.

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.

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