# geometry_utils

A set of helper utility functions for dealing with 3D geometry

## `check_points_in_cone(size, pos, quat, scale, particle_positions)`

Checks which points are within a cone with specified size @size.

NOTE: Assumes the cone and positions are expressed in the same coordinate frame such that the cone's height is aligned with the z-axis

Parameters:

Name Type Description Default
`size` `2 - array`

(radius, height) dimensions of the cone, specified in its local frame

required
`pos` `3 - array`

(x,y,z) local location of the cone

required
`quat` `4 - array`

(x,y,z,w) local orientation of the cone

required
`scale` `3 - array`

(x,y,z) local scale of the cone, specified in its local frame

required
`particle_positions` `N, 3) array`

positions to check for whether it is in the cone

required

Returns:

Type Description

(N,) array: boolean numpy array specifying whether each point lies in the cone.

Source code in `omnigibson/utils/geometry_utils.py`
 ``` 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115``` ``````def check_points_in_cone(size, pos, quat, scale, particle_positions): """ Checks which points are within a cone with specified size @size. NOTE: Assumes the cone and positions are expressed in the same coordinate frame such that the cone's height is aligned with the z-axis Args: size (2-array): (radius, height) dimensions of the cone, specified in its local frame pos (3-array): (x,y,z) local location of the cone quat (4-array): (x,y,z,w) local orientation of the cone scale (3-array): (x,y,z) local scale of the cone, specified in its local frame particle_positions ((N, 3) array): positions to check for whether it is in the cone Returns: (N,) array: boolean numpy array specifying whether each point lies in the cone. """ particle_positions = get_particle_positions_in_frame( pos=pos, quat=quat, scale=scale, particle_positions=particle_positions, ) radius, height = size in_height = (-height / 2.0 < particle_positions[:, -1]) & (particle_positions[:, -1] < height / 2.0) in_radius = np.linalg.norm(particle_positions[:, :-1], axis=-1) < \ (radius * (1 - (particle_positions[:, -1] + height / 2.0) / height )) return in_height & in_radius ``````

## `check_points_in_convex_hull_mesh(mesh_face_centroids, mesh_face_normals, pos, quat, scale, particle_positions)`

Checks which points are within a sphere with specified size @size.

NOTE: Assumes the mesh and positions are expressed in the same coordinate frame

Parameters:

Name Type Description Default
`mesh_face_centroids` `(D, 3)`

(x,y,z) location of the centroid of each mesh face, expressed in its local frame

required
`mesh_face_normals` `(D, 3)`

(x,y,z) normalized direction vector of each mesh face, expressed in its local frame

required
`pos` `3 - array`

(x,y,z) local location of the mesh

required
`quat` `4 - array`

(x,y,z,w) local orientation of the mesh

required
`scale` `3 - array`

(x,y,z) local scale of the cube, specified in its local frame

required
`particle_positions` `N, 3) array`

positions to check for whether it is in the mesh

required

Returns:

Type Description

(N,) array: boolean numpy array specifying whether each point lies in the mesh

Source code in `omnigibson/utils/geometry_utils.py`
 ```172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207``` ``````def check_points_in_convex_hull_mesh(mesh_face_centroids, mesh_face_normals, pos, quat, scale, particle_positions): """ Checks which points are within a sphere with specified size @size. NOTE: Assumes the mesh and positions are expressed in the same coordinate frame Args: mesh_face_centroids (D, 3): (x,y,z) location of the centroid of each mesh face, expressed in its local frame mesh_face_normals (D, 3): (x,y,z) normalized direction vector of each mesh face, expressed in its local frame pos (3-array): (x,y,z) local location of the mesh quat (4-array): (x,y,z,w) local orientation of the mesh scale (3-array): (x,y,z) local scale of the cube, specified in its local frame particle_positions ((N, 3) array): positions to check for whether it is in the mesh Returns: (N,) array: boolean numpy array specifying whether each point lies in the mesh """ particle_positions = get_particle_positions_in_frame( pos=pos, quat=quat, scale=scale, particle_positions=particle_positions, ) # For every mesh point / normal and particle position pair, we check whether it is "inside" (i.e.: the point lies # BEHIND the normal plane -- this is easily done by taking the dot product with the vector from the point to the # particle position with the normal, and validating that the value is < 0) D, _ = mesh_face_centroids.shape N, _ = particle_positions.shape mesh_points = np.tile(mesh_face_centroids.reshape(1, D, 3), (N, 1, 1)) mesh_normals = np.tile(mesh_face_normals.reshape(1, D, 3), (N, 1, 1)) particle_positions = np.tile(particle_positions.reshape(N, 1, 3), (1, D, 1)) # All arrays are now (N, D, 3) shape -- efficient for batching in_range = ((particle_positions - mesh_points) * mesh_normals).sum(axis=-1) < 0 # shape (N, D) # All D normals must be satisfied for a single point to be considered inside the hull in_range = in_range.sum(axis=-1) == D return in_range ``````

## `check_points_in_cube(size, pos, quat, scale, particle_positions)`

Checks which points are within a cube with specified size @size.

NOTE: Assumes the cube and positions are expressed in the same coordinate frame such that the cube's dimensions are axis-aligned with (x,y,z)

Parameters:

Name Type Description Default
`size` `float`

length of each side of the cube, specified in its local frame

required
`pos` `3 - array`

(x,y,z) local location of the cube

required
`quat` `4 - array`

(x,y,z,w) local orientation of the cube

required
`scale` `3 - array`

(x,y,z) local scale of the cube, specified in its local frame

required
`particle_positions` `N, 3) array`

positions to check for whether it is in the cube

required

Returns:

Type Description

(N,) array: boolean numpy array specifying whether each point lies in the cube.

Source code in `omnigibson/utils/geometry_utils.py`
 ```62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85``` ``````def check_points_in_cube(size, pos, quat, scale, particle_positions): """ Checks which points are within a cube with specified size @size. NOTE: Assumes the cube and positions are expressed in the same coordinate frame such that the cube's dimensions are axis-aligned with (x,y,z) Args: size float: length of each side of the cube, specified in its local frame pos (3-array): (x,y,z) local location of the cube quat (4-array): (x,y,z,w) local orientation of the cube scale (3-array): (x,y,z) local scale of the cube, specified in its local frame particle_positions ((N, 3) array): positions to check for whether it is in the cube Returns: (N,) array: boolean numpy array specifying whether each point lies in the cube. """ particle_positions = get_particle_positions_in_frame( pos=pos, quat=quat, scale=scale, particle_positions=particle_positions, ) return ((-size / 2.0 < particle_positions) & (particle_positions < size / 2.0)).sum(axis=-1) == 3 ``````

## `check_points_in_cylinder(size, pos, quat, scale, particle_positions)`

Checks which points are within a cylinder with specified size @size.

NOTE: Assumes the cylinder and positions are expressed in the same coordinate frame such that the cylinder's height is aligned with the z-axis

Parameters:

Name Type Description Default
`size` `2 - array`

(radius, height) dimensions of the cylinder, specified in its local frame

required
`pos` `3 - array`

(x,y,z) local location of the cylinder

required
`quat` `4 - array`

(x,y,z,w) local orientation of the cylinder

required
`scale` `3 - array`

(x,y,z) local scale of the cube, specified in its local frame

required
`particle_positions` `N, 3) array`

positions to check for whether it is in the cylinder

required

Returns:

Type Description

(N,) array: boolean numpy array specifying whether each point lies in the cylinder.

Source code in `omnigibson/utils/geometry_utils.py`
 ```118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144``` ``````def check_points_in_cylinder(size, pos, quat, scale, particle_positions): """ Checks which points are within a cylinder with specified size @size. NOTE: Assumes the cylinder and positions are expressed in the same coordinate frame such that the cylinder's height is aligned with the z-axis Args: size (2-array): (radius, height) dimensions of the cylinder, specified in its local frame pos (3-array): (x,y,z) local location of the cylinder quat (4-array): (x,y,z,w) local orientation of the cylinder scale (3-array): (x,y,z) local scale of the cube, specified in its local frame particle_positions ((N, 3) array): positions to check for whether it is in the cylinder Returns: (N,) array: boolean numpy array specifying whether each point lies in the cylinder. """ particle_positions = get_particle_positions_in_frame( pos=pos, quat=quat, scale=scale, particle_positions=particle_positions, ) radius, height = size in_height = (-height / 2.0 < particle_positions[:, -1]) & (particle_positions[:, -1] < height / 2.0) in_radius = np.linalg.norm(particle_positions[:, :-1], axis=-1) < radius return in_height & in_radius ``````

## `check_points_in_sphere(size, pos, quat, scale, particle_positions)`

Checks which points are within a sphere with specified size @size.

NOTE: Assumes the sphere and positions are expressed in the same coordinate frame

Parameters:

Name Type Description Default
`size` `float`

radius dimensions of the sphere

required
`pos` `3 - array`

(x,y,z) local location of the sphere

required
`quat` `4 - array`

(x,y,z,w) local orientation of the sphere

required
`scale` `3 - array`

(x,y,z) local scale of the sphere, specified in its local frame

required
`particle_positions` `N, 3) array`

positions to check for whether it is in the sphere

required

Returns:

Type Description

(N,) array: boolean numpy array specifying whether each point lies in the sphere

Source code in `omnigibson/utils/geometry_utils.py`
 ```147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169``` ``````def check_points_in_sphere(size, pos, quat, scale, particle_positions): """ Checks which points are within a sphere with specified size @size. NOTE: Assumes the sphere and positions are expressed in the same coordinate frame Args: size (float): radius dimensions of the sphere pos (3-array): (x,y,z) local location of the sphere quat (4-array): (x,y,z,w) local orientation of the sphere scale (3-array): (x,y,z) local scale of the sphere, specified in its local frame particle_positions ((N, 3) array): positions to check for whether it is in the sphere Returns: (N,) array: boolean numpy array specifying whether each point lies in the sphere """ particle_positions = get_particle_positions_in_frame( pos=pos, quat=quat, scale=scale, particle_positions=particle_positions, ) return np.linalg.norm(particle_positions, axis=-1) < size ``````

## `generate_points_in_volume_checker_function(obj, volume_link, use_visual_meshes=True, mesh_name_prefixes=None)`

Generates a function for quickly checking which of a group of points are contained within any container volumes. Four volume types are supported: "Cylinder" - Cylinder volume "Cube" - Cube volume "Sphere" - Sphere volume "Mesh" - Convex hull volume

@volume_link should have any number of nested, visual-only meshes of types {Sphere, Cylinder, Cube, Mesh} with naming prefix "container[...]"

Parameters:

Name Type Description Default
`obj` `EntityPrim`

Object which contains @volume_link as one of its links

required
`volume_link` `RigidPrim`

Link to use to grab container volumes composing the values for checking the points

required
`use_visual_meshes` `bool`

Whether to use @volume_link's visual or collision meshes to generate points fcn

`True`
`mesh_name_prefixes` `None or str`

If specified, specifies the substring that must exist in @volume_link's mesh names in order for that mesh to be included in the volume checker function. If None, no filtering will be used.

`None`

Returns:

Type Description

2-tuple: - function: Function with signature:

``````in_range = check_in_volumes(particle_positions)
``````

where @in_range is a N-array boolean numpy array, (True where the particle is in the volume), and @particle_positions is a (N, 3) array specifying the particle positions in global coordinates

• function: Function for grabbing real-time global scale volume of the container. Signature:

vol = total_volume()

where @vol is the total volume being checked (expressed in global scale) aggregated across all container sub-volumes

Source code in `omnigibson/utils/geometry_utils.py`
 ```263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406``` ``````def generate_points_in_volume_checker_function(obj, volume_link, use_visual_meshes=True, mesh_name_prefixes=None): """ Generates a function for quickly checking which of a group of points are contained within any container volumes. Four volume types are supported: "Cylinder" - Cylinder volume "Cube" - Cube volume "Sphere" - Sphere volume "Mesh" - Convex hull volume @volume_link should have any number of nested, visual-only meshes of types {Sphere, Cylinder, Cube, Mesh} with naming prefix "container[...]" Args: obj (EntityPrim): Object which contains @volume_link as one of its links volume_link (RigidPrim): Link to use to grab container volumes composing the values for checking the points use_visual_meshes (bool): Whether to use @volume_link's visual or collision meshes to generate points fcn mesh_name_prefixes (None or str): If specified, specifies the substring that must exist in @volume_link's mesh names in order for that mesh to be included in the volume checker function. If None, no filtering will be used. Returns: 2-tuple: - function: Function with signature: in_range = check_in_volumes(particle_positions) where @in_range is a N-array boolean numpy array, (True where the particle is in the volume), and @particle_positions is a (N, 3) array specifying the particle positions in global coordinates - function: Function for grabbing real-time global scale volume of the container. Signature: vol = total_volume() where @vol is the total volume being checked (expressed in global scale) aggregated across all container sub-volumes """ # Iterate through all visual meshes and keep track of any that are prefixed with container container_meshes = [] meshes = volume_link.visual_meshes if use_visual_meshes else volume_link.collision_meshes for container_mesh_name, container_mesh in meshes.items(): if mesh_name_prefixes is None or mesh_name_prefixes in container_mesh_name: container_meshes.append(container_mesh) # Programmatically define the volume checker functions based on each container found volume_checker_fcns = [] for sub_container_mesh in container_meshes: mesh_type = sub_container_mesh.prim.GetTypeName() if mesh_type == "Mesh": fcn, vol_fcn = _generate_convex_hull_volume_checker_functions(convex_hull_mesh=sub_container_mesh.prim) elif mesh_type == "Sphere": fcn = lambda mesh, particle_positions: check_points_in_sphere( size=mesh.GetAttribute("radius").Get(), pos=np.array(mesh.GetAttribute("xformOp:translate").Get()), quat=np.array([*(mesh.GetAttribute("xformOp:orient").Get().imaginary), mesh.GetAttribute("xformOp:orient").Get().real]), scale=np.array(mesh.GetAttribute("xformOp:scale").Get()), particle_positions=particle_positions, ) elif mesh_type == "Cylinder": fcn = lambda mesh, particle_positions: check_points_in_cylinder( size=[mesh.GetAttribute("radius").Get(), mesh.GetAttribute("height").Get()], pos=np.array(mesh.GetAttribute("xformOp:translate").Get()), quat=np.array([*(mesh.GetAttribute("xformOp:orient").Get().imaginary), mesh.GetAttribute("xformOp:orient").Get().real]), scale=np.array(mesh.GetAttribute("xformOp:scale").Get()), particle_positions=particle_positions, ) elif mesh_type == "Cone": fcn = lambda mesh, particle_positions: check_points_in_cone( size=[mesh.GetAttribute("radius").Get(), mesh.GetAttribute("height").Get()], pos=np.array(mesh.GetAttribute("xformOp:translate").Get()), quat=np.array([*(mesh.GetAttribute("xformOp:orient").Get().imaginary), mesh.GetAttribute("xformOp:orient").Get().real]), scale=np.array(mesh.GetAttribute("xformOp:scale").Get()), particle_positions=particle_positions, ) elif mesh_type == "Cube": fcn = lambda mesh, particle_positions: check_points_in_cube( size=mesh.GetAttribute("size").Get(), pos=np.array(mesh.GetAttribute("xformOp:translate").Get()), quat=np.array([*(mesh.GetAttribute("xformOp:orient").Get().imaginary), mesh.GetAttribute("xformOp:orient").Get().real]), scale=np.array(mesh.GetAttribute("xformOp:scale").Get()), particle_positions=particle_positions, ) else: raise ValueError(f"Cannot create volume checker function for mesh of type: {mesh_type}") volume_checker_fcns.append(fcn) # Define the actual volume checker function def check_points_in_volumes(particle_positions): # Algo # 1. Particles in global frame --> particles in volume link frame (including scaling) # 2. For each volume checker function, apply volume checking # 3. Aggregate across all functions with OR condition (any volume satisfied for that point) ###### n_particles = len(particle_positions) # Get pose of origin (global frame) in frame of volume link # NOTE: This assumes there is no relative scaling between obj and volume link volume_link_pos, volume_link_quat = volume_link.get_position_orientation() particle_positions = get_particle_positions_in_frame( pos=volume_link_pos, quat=volume_link_quat, scale=obj.scale, particle_positions=particle_positions, ) in_volumes = np.zeros(n_particles).astype(bool) for checker_fcn, mesh in zip(volume_checker_fcns, container_meshes): in_volumes |= checker_fcn(mesh.prim, particle_positions) return in_volumes # Define the actual volume calculator function def calculate_volume(precision=1e-5): # We use monte-carlo sampling to approximate the voluem up to @precision # NOTE: precision defines the RELATIVE precision of the volume computation -- i.e.: the relative error with # respect to the volume link's global AABB # Convert precision to minimum number of particles to sample min_n_particles = int(np.ceil(1. / precision)) # Make sure container meshes are visible so AABB computation is correct for mesh in container_meshes: mesh.visible = True # Determine equally-spaced sampling distance to achieve this minimum particle count aabb_volume = np.product(volume_link.visual_aabb_extent) sampling_distance = np.cbrt(aabb_volume / min_n_particles) low, high = volume_link.visual_aabb n_particles_per_axis = ((high - low) / sampling_distance).astype(int) + 1 assert np.all(n_particles_per_axis), "Must increase precision for calculate_volume -- too coarse for sampling!" # 1e-10 is added because the extent might be an exact multiple of particle radius arrs = [np.arange(l, h, sampling_distance) for l, h, n in zip(low, high, n_particles_per_axis)] # Generate 3D-rectangular grid of points, and only keep the ones inside the mesh points = np.stack([arr.flatten() for arr in np.meshgrid(*arrs)]).T # Re-hide container meshes for mesh in container_meshes: mesh.visible = False # Return the fraction of the link AABB's volume based on fraction of points enclosed within it return aabb_volume * np.mean(check_points_in_volumes(points)) return check_points_in_volumes, calculate_volume ``````

## `get_particle_positions_from_frame(pos, quat, scale, particle_positions)`

Transforms particle positions @positions from the frame specified by @pos and @quat with new scale @scale.

This is similar to @get_particle_positions_in_frame, but does the reverse operation, inverting @pos and @quat

Parameters:

Name Type Description Default
`pos` `3 - array`

(x,y,z) pos of the local frame

required
`quat` `4 - array`

(x,y,z,w) quaternion orientation of the local frame

required
`scale` `3 - array`

(x,y,z) local scale of the local frame

required
`particle_positions` `N, 3) array`

positions

required

Returns:

Type Description

(N,) array: updated particle positions in the parent coordinate frame

Source code in `omnigibson/utils/geometry_utils.py`
 ```35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59``` ``````def get_particle_positions_from_frame(pos, quat, scale, particle_positions): """ Transforms particle positions @positions from the frame specified by @pos and @quat with new scale @scale. This is similar to @get_particle_positions_in_frame, but does the reverse operation, inverting @pos and @quat Args: pos (3-array): (x,y,z) pos of the local frame quat (4-array): (x,y,z,w) quaternion orientation of the local frame scale (3-array): (x,y,z) local scale of the local frame particle_positions ((N, 3) array): positions Returns: (N,) array: updated particle positions in the parent coordinate frame """ # Scale by the new scale particle_positions = particle_positions * scale.reshape(1, 3) # Get pose of origin (global frame) in new_frame origin_in_new_frame = T.pose2mat((pos, quat)) # Batch the transforms to get all particle points in the local link frame positions_tensor = np.tile(np.eye(4).reshape(1, 4, 4), (len(particle_positions), 1, 1)) # (N, 4, 4) # Scale by the new scale# positions_tensor[:, :3, 3] = particle_positions return (origin_in_new_frame @ positions_tensor)[:, :3, 3] # (N, 3) ``````

## `get_particle_positions_in_frame(pos, quat, scale, particle_positions)`

Transforms particle positions @positions into the frame specified by @pos and @quat with new scale @scale, where @pos and @quat are assumed to be specified in the same coordinate frame that @particle_positions is specified

Parameters:

Name Type Description Default
`pos` `3 - array`

(x,y,z) pos of the new frame

required
`quat` `4 - array`

(x,y,z,w) quaternion orientation of the new frame

required
`scale` `3 - array`

(x,y,z) local scale of the new frame

required
`particle_positions` `N, 3) array`

positions

required

Returns:

Type Description

(N,) array: updated particle positions in the new coordinate frame

Source code in `omnigibson/utils/geometry_utils.py`
 ``` 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32``` ``````def get_particle_positions_in_frame(pos, quat, scale, particle_positions): """ Transforms particle positions @positions into the frame specified by @pos and @quat with new scale @scale, where @pos and @quat are assumed to be specified in the same coordinate frame that @particle_positions is specified Args: pos (3-array): (x,y,z) pos of the new frame quat (4-array): (x,y,z,w) quaternion orientation of the new frame scale (3-array): (x,y,z) local scale of the new frame particle_positions ((N, 3) array): positions Returns: (N,) array: updated particle positions in the new coordinate frame """ # Get pose of origin (global frame) in new_frame origin_in_new_frame = T.pose_inv(T.pose2mat((pos, quat))) # Batch the transforms to get all particle points in the local link frame positions_tensor = np.tile(np.eye(4).reshape(1, 4, 4), (len(particle_positions), 1, 1)) # (N, 4, 4) # Scale by the new scale# positions_tensor[:, :3, 3] = particle_positions particle_positions = (origin_in_new_frame @ positions_tensor)[:, :3, 3] # (N, 3) # Scale by the new scale return particle_positions / scale.reshape(1, 3) ``````