curobo.geom.sdf.world module

class curobo.geom.sdf.world.CollisionBuffer(distance_buffer: torch.Tensor, grad_distance_buffer: torch.Tensor, sparsity_index_buffer: torch.Tensor, shape: torch.Size | None = None)

Bases: object

Parameters:
  • distance_buffer (Tensor) –

  • grad_distance_buffer (Tensor) –

  • sparsity_index_buffer (Tensor) –

  • shape (Size | None) –

distance_buffer: Tensor
grad_distance_buffer: Tensor
sparsity_index_buffer: Tensor
shape: Size | None = None
classmethod initialize_from_shape(shape, tensor_args)
Parameters:
_update_from_shape(shape, tensor_args)
Parameters:
update_buffer_shape(shape, tensor_args)
Parameters:
clone()
class curobo.geom.sdf.world.CollisionQueryBuffer(primitive_collision_buffer=None, mesh_collision_buffer=None, blox_collision_buffer=None, shape=None)

Bases: object

Class stores all buffers required to query collisions This class currently has three main buffers. We initialize the required buffers based on ?

Parameters:
primitive_collision_buffer: CollisionBuffer | None = None
mesh_collision_buffer: CollisionBuffer | None = None
blox_collision_buffer: CollisionBuffer | None = None
shape: Size | None = None
clone()
classmethod initialize_from_shape(shape, tensor_args, collision_types)
Parameters:
  • shape (Size) –

  • tensor_args (TensorDeviceType) –

  • collision_types (Dict[str, bool]) –

create_from_shape(shape, tensor_args, collision_types)
Parameters:
  • shape (Size) –

  • tensor_args (TensorDeviceType) –

  • collision_types (Dict[str, bool]) –

update_buffer_shape(shape, tensor_args, collision_types)
Parameters:
  • shape (Size) –

  • tensor_args (TensorDeviceType) –

  • collision_types (Dict[str, bool] | None) –

get_gradient_buffer()
class curobo.geom.sdf.world.CollisionCheckerType(value)

Bases: Enum

Type of collision checker to use. :param Enum: _description_ :type Enum: _type_

PRIMITIVE = 'PRIMITIVE'
BLOX = 'BLOX'
MESH = 'MESH'
class curobo.geom.sdf.world.WorldCollisionConfig(tensor_args: curobo.types.base.TensorDeviceType, world_model: Union[List[curobo.geom.types.WorldConfig], curobo.geom.types.WorldConfig, NoneType] = None, cache: Optional[Dict[curobo.geom.types.Obstacle, int]] = None, n_envs: int = 1, checker_type: curobo.geom.sdf.world.CollisionCheckerType = <CollisionCheckerType.PRIMITIVE: 'PRIMITIVE'>, max_distance: float = 0.01)

Bases: object

Parameters:
tensor_args: TensorDeviceType
world_model: List[WorldConfig] | WorldConfig | None = None
cache: Dict[Obstacle, int] | None = None
n_envs: int = 1
checker_type: CollisionCheckerType = 'PRIMITIVE'
max_distance: float = 0.01
static load_from_dict(world_coll_checker_dict, world_model_dict=None, tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32))
Parameters:
class curobo.geom.sdf.world.WorldCollision(config=None)

Bases: WorldCollisionConfig

Parameters:

config (WorldCollisionConfig | None) –

load_collision_model(world_model)
Parameters:

world_model (WorldConfig) –

get_sphere_distance(query_sphere, collision_query_buffer, weight, activation_distance, env_query_idx=None, return_loss=False)

Computes the signed distance via analytic function Args: tensor_sphere: b, n, 4

Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • env_query_idx (Tensor | None) –

  • return_loss (bool) –

get_sphere_collision(query_sphere, collision_query_buffer, weight, activation_distance, env_query_idx=None, return_loss=False)

Computes the signed distance via analytic function Args: tensor_sphere: b, n, 4 we assume we don’t need gradient for this function. If you need gradient, use get_sphere_distance

Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • env_query_idx (Tensor | None) –

  • return_loss (bool) –

get_swept_sphere_distance(query_sphere, collision_query_buffer, weight, activation_distance, speed_dt, sweep_steps, enable_speed_metric=False, env_query_idx=None, return_loss=False)
Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • speed_dt (Tensor) –

  • sweep_steps (int) –

  • env_query_idx (Tensor | None) –

  • return_loss (bool) –

get_swept_sphere_collision(query_sphere, collision_query_buffer, weight, activation_distance, speed_dt, sweep_steps, enable_speed_metric=False, env_query_idx=None, return_loss=False)
Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • speed_dt (Tensor) –

  • sweep_steps (int) –

  • env_query_idx (Tensor | None) –

  • return_loss (bool) –

get_sphere_trace(query_sphere, collision_query_buffer, weight, sweep_steps, env_query_idx=None, return_loss=False)
Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • sweep_steps (int) –

  • env_query_idx (Tensor | None) –

  • return_loss (bool) –

class curobo.geom.sdf.world.WorldPrimitiveCollision(config)

Bases: WorldCollision

World Oriented Bounding Box representation object

We represent the world with oriented bounding boxes. For speed, we assume there is a maximum number of obbs that can be instantiated. This number is read from the WorldCollisionConfig. If no cache is setup, we use the number from the first call of load_collision_model.

Parameters:

config (WorldCollisionConfig) –

_init_cache()
load_collision_model(world_config, env_idx=0, fix_cache_reference=False)
Parameters:
  • world_config (WorldConfig) –

  • fix_cache_reference (bool) –

load_batch_collision_model(world_config_list)

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

Parameters:

world_config_list (List[WorldConfig]) – list of world configs to load from.

_load_collision_model_in_cache(world_config, env_idx=0, fix_cache_reference=False)
Parameters:
  • world_config (WorldConfig) –

  • env_idx (int) –

  • fix_cache_reference (bool) –

_create_obb_cache(obb_cache)
add_obb_from_raw(name, dims, env_idx, w_obj_pose=None, obj_w_pose=None)

Args: dims: lenght, width, height position: x,y,z rotation: matrix (3x3)

Parameters:
  • name (str) –

  • dims (Tensor) –

  • env_idx (int) –

  • w_obj_pose (Pose | None) –

  • obj_w_pose (Pose | None) –

add_obb(cuboid, env_idx=0)
Parameters:
  • cuboid (Cuboid) –

  • env_idx (int) –

update_obb_dims(obj_dims, name=None, env_obj_idx=None, env_idx=0)

Update obstacle dimensions

Parameters:
  • obj_dims (torch.Tensor) – [dim.x,dim.y, dim.z], give as [b,3]

  • obj_idx (torch.Tensor or int) –

  • name (str | None) –

  • env_obj_idx (Tensor | None) –

  • env_idx (int) –

enable_obstacle(name, enable=True, env_idx=0)
Parameters:
  • name (str) –

  • enable (bool) –

  • env_idx (int) –

enable_obb(enable=True, name=None, env_obj_idx=None, env_idx=0)

Update obstacle dimensions

Parameters:
  • obj_dims (torch.Tensor) – [dim.x,dim.y, dim.z], give as [b,3]

  • obj_idx (torch.Tensor or int) –

  • enable (bool) –

  • name (str | None) –

  • env_obj_idx (Tensor | None) –

  • env_idx (int) –

update_obstacle_pose(name, w_obj_pose, env_idx=0)
Parameters:
  • name (str) –

  • w_obj_pose (Pose) –

  • env_idx (int) –

update_obb_pose(w_obj_pose=None, obj_w_pose=None, name=None, env_obj_idx=None, env_idx=0)

Update pose of a specific objects. This also updates the signed distance grid to account for the updated object pose. Args: obj_w_pose: Pose obj_idx:

Parameters:
  • w_obj_pose (Pose | None) –

  • obj_w_pose (Pose | None) –

  • name (str | None) –

  • env_obj_idx (Tensor | None) –

  • env_idx (int) –

classmethod _get_obstacle_poses(w_obj_pose=None, obj_w_pose=None)
Parameters:
  • w_obj_pose (Pose | None) –

  • obj_w_pose (Pose | None) –

get_obb_idx(name, env_idx=0)
Parameters:
  • name (str) –

  • env_idx (int) –

Return type:

int

get_sphere_distance(query_sphere, collision_query_buffer, weight, activation_distance, env_query_idx=None, return_loss=False)

Computes the signed distance via analytic function Args: tensor_sphere: b, n, 4

Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • env_query_idx (Tensor | None) –

get_sphere_collision(query_sphere, collision_query_buffer, weight, activation_distance, env_query_idx=None, return_loss=False)

Computes the signed distance via analytic function Args: tensor_sphere: b, n, 4 we assume we don’t need gradient for this function. If you need gradient, use get_sphere_distance

Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • env_query_idx (Tensor | None) –

get_swept_sphere_distance(query_sphere, collision_query_buffer, weight, activation_distance, speed_dt, sweep_steps, enable_speed_metric=False, env_query_idx=None, return_loss=False)

Computes the signed distance via analytic function Args: tensor_sphere: b, n, 4

Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • speed_dt (Tensor) –

  • sweep_steps (int) –

  • env_query_idx (Tensor | None) –

get_swept_sphere_collision(query_sphere, collision_query_buffer, weight, activation_distance, speed_dt, sweep_steps, enable_speed_metric=False, env_query_idx=None, return_loss=False)

Computes the signed distance via analytic function Args: tensor_sphere: b, n, 4

Parameters:
  • collision_query_buffer (CollisionQueryBuffer) –

  • weight (Tensor) –

  • activation_distance (Tensor) –

  • speed_dt (Tensor) –

  • sweep_steps (int) –

  • env_query_idx (Tensor | None) –

clear_cache()
get_voxels_in_bounding_box(cuboid, voxel_size=0.02)
Parameters:
  • cuboid (Cuboid) –

  • voxel_size (float) –

Return type:

List[Cuboid] | Tensor

get_mesh_in_bounding_box(cuboid, voxel_size=0.02)
Parameters:
  • cuboid (Cuboid) –

  • voxel_size (float) –

Return type:

Mesh