curobo.wrap.model.robot_world module

This module has differentiable layers built from CuRobo’s core features for use in Pytorch.

class RobotWorldConfig(
kinematics: curobo.cuda_robot_model.cuda_robot_model.CudaRobotModel,
sampler: curobo.util.sample_lib.HaltonGenerator,
bound_scale: torch.Tensor,
bound_cost: curobo.rollout.cost.bound_cost.BoundCost,
pose_cost: curobo.rollout.cost.pose_cost.PoseCost,
self_collision_cost: curobo.rollout.cost.self_collision_cost.SelfCollisionCost | None = None,
collision_cost: curobo.rollout.cost.primitive_collision_cost.PrimitiveCollisionCost | None = None,
collision_constraint: curobo.rollout.cost.primitive_collision_cost.PrimitiveCollisionCost | None = None,
world_model: curobo.geom.sdf.world.WorldCollision | None = None,
rejection_ratio: int = 10,
tensor_args: curobo.types.base.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),
contact_distance: float = 0.0,
)

Bases: object

kinematics: CudaRobotModel
sampler: HaltonGenerator
bound_scale: Tensor
bound_cost: BoundCost
pose_cost: PoseCost
self_collision_cost: SelfCollisionCost | None = None
collision_cost: PrimitiveCollisionCost | None = None
collision_constraint: PrimitiveCollisionCost | None = None
world_model: WorldCollision | None = None
rejection_ratio: int = 10
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)
contact_distance: float = 0.0
static load_from_config(
robot_config: RobotConfig | str = 'franka.yml',
world_model: None | str | Dict | WorldConfig | List[WorldConfig] | List[str] = 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),
n_envs: int = 1,
n_meshes: int = 50,
n_cuboids: int = 50,
collision_activation_distance: float = 0.2,
self_collision_activation_distance: float = 0.0,
max_collision_distance: float = 1.0,
collision_checker_type: CollisionCheckerType = CollisionCheckerType.MESH,
world_collision_checker: WorldCollision | None = None,
pose_weight: List[float] = [1, 1, 1, 1],
)
class RobotWorld(
config: RobotWorldConfig,
)

Bases: RobotWorldConfig

get_kinematics(
q: Tensor,
) → CudaRobotModelState
update_world(
world_config: WorldConfig,
)
clear_world_cache()
get_collision_distance(
x_sph: Tensor,
env_query_idx: Tensor | None = None,
) → Tensor

Get collision distance

Parameters:

x_sph (torch.Tensor) – batch, horizon, n_spheres, 4

Returns:

_description_

Return type:

torch.Tensor

get_collision_constraint(
x_sph: Tensor,
env_query_idx: Tensor | None = None,
) → Tensor

Get collision distance

Parameters:

x_sph (torch.Tensor) – batch, horizon, n_spheres, 4

Returns:

_description_

Return type:

torch.Tensor

get_self_collision_distance(
x_sph: Tensor,
) → Tensor
get_self_collision(
x_sph: Tensor,
) → Tensor
get_collision_vector(
x_sph: Tensor,
env_query_idx: Tensor | None = None,
)

NOTE: This function is not differentiable

get_world_self_collision_distance_from_joints(
q: Tensor,
env_query_idx: Tensor | None = None,
) → Tuple[Tensor, Tensor]

q : batch, dof

get_world_self_collision_distance_from_joint_trajectory(
q: Tensor,
env_query_idx: Tensor | None = None,
) → Tuple[Tensor, Tensor]

q : batch, horizon, dof

get_bound(
q: Tensor,
) → Tensor
sample(
n: int,
mask_valid: bool = True,
env_query_idx: Tensor | None = None,
)

This does not support batched environments, use sample_trajectory instead.

validate(
q: Tensor,
env_query_idx: Tensor | None = None,
)

This does not support batched environments, use validate_trajectory instead

sample_trajectory(
batch: int,
horizon: int,
mask_valid: bool = True,
env_query_idx: Tensor | None = None,
)
validate_trajectory(
q: Tensor,
env_query_idx: Tensor | None = None,
)

q: batch , horizon, dof env_query_idx: batch, 1

pose_distance(
x_des: Pose,
x_current: Pose,
resize: bool = False,
)
get_point_robot_distance(
points: Tensor,
q: Tensor,
)

Compute distance from the robot at q joint configuration to points (e.g., pointcloud)

Parameters:

  • points – [b,n,3]

  • q – [1, dof]

Returns:

[b,1] Positive is in collision with robot

Return type:

distance

NOTE: This currently does not support batched robot but can be done easily.

get_active_js(
full_js: JointState,
)
collision_constraint: PrimitiveCollisionCost | None = None
collision_cost: PrimitiveCollisionCost | None = None
contact_distance: float = 0.0
static load_from_config(
robot_config: RobotConfig | str = 'franka.yml',
world_model: None | str | Dict | WorldConfig | List[WorldConfig] | List[str] = 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),
n_envs: int = 1,
n_meshes: int = 50,
n_cuboids: int = 50,
collision_activation_distance: float = 0.2,
self_collision_activation_distance: float = 0.0,
max_collision_distance: float = 1.0,
collision_checker_type: CollisionCheckerType = CollisionCheckerType.MESH,
world_collision_checker: WorldCollision | None = None,
pose_weight: List[float] = [1, 1, 1, 1],
)
rejection_ratio: int = 10
self_collision_cost: SelfCollisionCost | 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)
world_model: WorldCollision | None = None
kinematics: CudaRobotModel
sampler: HaltonGenerator
bound_scale: Tensor
bound_cost: BoundCost
pose_cost: PoseCost
sum_mask(d1, d2, d3)
mask(d1, d2, d3)
point_robot_distance(
link_spheres_tensor,
points,
)

Compute distance between robot and points

Parameters:

  • link_spheres_tensor – [batch_robot, n_robot_spheres, 4]

  • points – [batch_points, n_points, 3]

Returns:

[batch_points, n_points]

Return type:

distance