storm_kit.geom.sdf.robot_world module

class RobotWorldCollision(robot_collision, world_collision)[source]

Bases: object

update_world_robot_pose(w_pos, w_rot)[source]
class RobotWorldCollisionCapsule(robot_collision_params, world_collision_params, robot_batch_size=1, world_batch_size=1, tensor_args={'device': 'cpu', 'dtype': torch.float32})[source]

Bases: storm_kit.geom.sdf.robot_world.RobotWorldCollision

Collision checking between capsule robot and sphere world

get_signed_distance()[source]
class RobotWorldCollisionPrimitive(robot_collision_params, world_collision_params, robot_batch_size=1, world_batch_size=1, tensor_args={'device': 'cpu', 'dtype': torch.float32}, bounds=None, grid_resolution=None)[source]

Bases: storm_kit.geom.sdf.robot_world.RobotWorldCollision

build_batch_features(batch_size, clone_pose=True, clone_points=True)[source]
check_robot_sphere_collisions(link_trans, link_rot)[source]

get signed distance from stored grid [very fast]

Parameters
  • link_trans (tensor) – [b,3]

  • link_rot (tensor) – [b,3,3]

Returns

signed distance [b,1]

Return type

tensor

get_robot_env_sdf(link_trans, link_rot)[source]

Compute signed distance via analytic functino

Parameters
  • link_trans (tensor) – [b,3]

  • link_rot (tensor) – [b,3,3]

Returns

signed distance [b,1]

Return type

tensor

class RobotWorldCollisionVoxel(robot_collision_params, batch_size, label_map, bounds=None, grid_resolution=0.02, tensor_args={'device': device(type='cpu'), 'dtype': torch.float32})[source]

Bases: object

This class can check collision between robot and sdf grid of camera pointcloud.

build_batch_features(batch_size, clone_pose=True, clone_points=True)[source]
check_robot_mesh_collisions(link_trans, link_rot, threshold=0.45, return_point_values=False)[source]

Checks collision between robot represented by sampling points on mesh surface and pointcloud sdf.

Parameters
  • link_trans ([type]) – [description]

  • link_rot ([type]) – [description]

  • threshold (float, optional) – [description]. Defaults to 0.45.

  • return_point_values (bool, optional) – [description]. Defaults to False.

Returns

[description]

Return type

[type]

check_robot_sphere_collisions(link_trans, link_rot)[source]

Checks collision between robot spheres and pointcloud sdf grid

Parameters
  • link_trans ([type]) – [description]

  • link_rot ([type]) – [description]

Returns

[description]

Return type

[type]

set_scene(camera_pointcloud, scene_labels)[source]

Loads scene pointcloud of the environment

Parameters
  • camera_pointcloud – pointcloud of scene from camera [nx3]

  • scene_labels – labels of pointcloud [“robot”, “ground”]

set_world_transform(robot_table_trans, robot_R_table, robot_c_trans, robot_R_c)[source]
transform_to_table(link_trans, link_rot)[source]