Classes
class	BodySpheres
	Container for all spheres belonging to a single body's collision model. More...
class	DistanceAndGradient
	Wrapper that combines a distance value and gradient. More...
class	MbpEnvironmentCollisionChecker
	Sphere-model robot collision checker using MbP/SG to model environment geometry. More...
class	Placeholder
	This is a stub class to exercise the build system. More...
class	PointSignedDistanceAndGradientResult
	Wrapper for multiple distance and gradient values. More...
class	SphereRobotModelCollisionChecker
	Base class for collision checkers using a sphere-geometry robot model. More...
class	SphereSpecification
	Class modelling collision spheres used for collision checking. More...
class	VoxelizedEnvironmentCollisionChecker
	Collision checker using a voxelized environment model. More...
class	VoxelOccupancyMap
class	VoxelSignedDistanceField
	Container for voxelized signed distance fields. More...
class	VoxelTaggedObjectOccupancyMap

Functions
void	SelfFilter (const SphereRobotModelCollisionChecker &collision_checker, const Eigen::VectorXd &q, double padding, multibody::BodyIndex grid_body_index, VoxelOccupancyMap *occupancy_map, Parallelism parallelism, std::optional< int > context_number=std::nullopt)
	Self-filter implementation for VoxelOccupancyMap environments.
void	SelfFilter (const SphereRobotModelCollisionChecker &collision_checker, const Eigen::VectorXd &q, double padding, multibody::BodyIndex grid_body_index, VoxelTaggedObjectOccupancyMap *occupancy_map, Parallelism parallelism, std::optional< int > context_number=std::nullopt)
	Self-filter implementation for VoxelTaggedObjectOccupancyMap environments.
VoxelOccupancyMap	BuildOccupancyMap (const multibody::MultibodyPlant< double > &plant, const systems::Context< double > &plant_context, const std::unordered_set< geometry::GeometryId > &geometries_to_ignore, const std::string &parent_body_name, const math::RigidTransformd &X_PG, const Eigen::Vector3d &grid_dimensions, const double grid_resolution, const std::optional< multibody::BodyIndex > &override_parent_body_index={}, Parallelism parallelism=Parallelism::Max())
	Builds a VoxelOccupancyMap using the parameters specified.
VoxelTaggedObjectOccupancyMap	BuildTaggedObjectOccupancyMap (const multibody::MultibodyPlant< double > &plant, const systems::Context< double > &plant_context, const std::unordered_set< geometry::GeometryId > &geometries_to_ignore, const std::string &parent_body_name, const math::RigidTransformd &X_PG, const Eigen::Vector3d &grid_dimensions, const double grid_resolution, const std::optional< multibody::BodyIndex > &override_parent_body_index={}, Parallelism parallelism=Parallelism::Max())
	Builds a VoxelTaggedObjectOccupancyMap using the parameters specified.
void	FillOccupancyMap (const multibody::MultibodyPlant< double > &plant, const systems::Context< double > &plant_context, const std::unordered_set< geometry::GeometryId > &geometries_to_ignore, VoxelOccupancyMap *occupancy_map, const std::optional< multibody::BodyIndex > &override_parent_body_index={}, Parallelism parallelism=Parallelism::Max())
	Fills an initialized VoxelOccupancyMap.
void	FillTaggedObjectOccupancyMap (const multibody::MultibodyPlant< double > &plant, const systems::Context< double > &plant_context, const std::unordered_set< geometry::GeometryId > &geometries_to_ignore, VoxelTaggedObjectOccupancyMap *occupancy_map, const std::optional< multibody::BodyIndex > &override_parent_body_index={}, Parallelism parallelism=Parallelism::Max())
	Fills an initialized VoxelTaggedObjectOccupancyMap.

Function Documentation

◆ BuildOccupancyMap()

VoxelOccupancyMap BuildOccupancyMap	(	const multibody::MultibodyPlant< double > &	plant,
		const systems::Context< double > &	plant_context,
		const std::unordered_set< geometry::GeometryId > &	geometries_to_ignore,
		const std::string &	parent_body_name,
		const math::RigidTransformd &	X_PG,
		const Eigen::Vector3d &	grid_dimensions,
		const double	grid_resolution,
		const std::optional< multibody::BodyIndex > &	override_parent_body_index = {},
		Parallelism	parallelism = Parallelism::Max() )

Builds a VoxelOccupancyMap using the parameters specified.

VoxelOccupancyMaps are a dense voxel grid where each cell is a voxelized_geometry_tools::OccupancyCell that stores P(occupancy) as a float. OccupancyMaps support computation of signed distance fields and topological invariants (# of components, # of holes, # of voids). Empty cells receive occupancy=0.0, filled cells receive occupancy=1.0.

Parameters

plant	MultibodyPlant model with a registered SceneGraph.
plant_context	Context of plant.
geometries_to_ignore	Set of geometries to ignore.
parent_body_name	Name of parent body in the MultibodyPlant, used as frame name in the constructed OccupancyMap. If this name is not unique, or does not correspond to an existing MbP body, use override_parent_body_index to specfiy the parent body directly.
X_PG	Pose of occupancy map frame G in frame of parent body P.
grid_dimensions	Size of occupancy map in meters. If you specify a grid_size that is not evenly divisible by grid_resolution, you will get a larger grid with num_cells = ceil(size/resolution).
grid_resolution	Cell size (in meters) for all Voxel grids used by the builder. All grids must have uniform cell sizes.

Precondition: grid_resolution > 0

Parameters

override_parent_body_index Optionally provide a body index to override using parent_body_name to identify the parent body. Use this if the parent body name is not unique, or if the frame name does not match an existing MbP body (e.g. the name is a TF-compatible name incompatible with GetBodyByName).

◆ BuildTaggedObjectOccupancyMap()

VoxelTaggedObjectOccupancyMap BuildTaggedObjectOccupancyMap	(	const multibody::MultibodyPlant< double > &	plant,
		const systems::Context< double > &	plant_context,
		const std::unordered_set< geometry::GeometryId > &	geometries_to_ignore,
		const std::string &	parent_body_name,
		const math::RigidTransformd &	X_PG,
		const Eigen::Vector3d &	grid_dimensions,
		const double	grid_resolution,
		const std::optional< multibody::BodyIndex > &	override_parent_body_index = {},
		Parallelism	parallelism = Parallelism::Max() )

Builds a VoxelTaggedObjectOccupancyMap using the parameters specified.

VoxelTaggedObjectOccupancyMaps are a dense voxel grid where each cell is a voxelized_geometry_tools::TaggedObjectOccupancyCell that stores P(occupancy) as a float and object_id as a uint32_t. TaggedObjectOccupancyMaps support computation of signed distance fields and the first three topological invariants (# of components, # of holes, # of voids) as well as a limited form of spatial partioning. Empty cells receive occupancy=0.0, filled cells receive occupancy=1.0. Filled cells of the environment receive object_id values corresponding to the integer values of the GeometryId in that location. If your environment has more than 2^32 GeometryIds, this will throw.

Parameters

plant	MultibodyPlant model with a registered SceneGraph.
plant_context	Context of plant.
geometries_to_ignore	Set of geometries to ignore.
parent_body_name	Name of parent body in the MultibodyPlant, used as frame name in the constructed OccupancyMap. If this name is not unique, or does not correspond to an existing MbP body, use override_parent_body_index to specfiy the parent body directly.
X_PG	Pose of occupancy map frame G in frame of parent body P.
grid_dimensions	Size of occupancy map in meters. If you specify a grid_size that is not evenly divisible by grid_resolution, you will get a larger grid with num_cells = ceil(size/resolution).
grid_resolution	Cell size (in meters) for all Voxel grids used by the builder. All grids must have uniform cell sizes.

Precondition: grid_resolution > 0

Parameters

override_parent_body_index Optionally provide a body index to override using parent_body_name to identify the parent body. Use this if the parent body name is not unique, or if the frame name does not match an existing MbP body (e.g. the name is a TF-compatible name incompatible with GetBodyByName).

◆ FillOccupancyMap()

void FillOccupancyMap	(	const multibody::MultibodyPlant< double > &	plant,
		const systems::Context< double > &	plant_context,
		const std::unordered_set< geometry::GeometryId > &	geometries_to_ignore,
		VoxelOccupancyMap *	occupancy_map,
		const std::optional< multibody::BodyIndex > &	override_parent_body_index = {},
		Parallelism	parallelism = Parallelism::Max() )

Fills an initialized VoxelOccupancyMap.

Empty cells receive occupancy=0.0, filled cells receive occupancy=1.0.

Parameters

plant	MultibodyPlant model with a registered SceneGraph.
plant_context	Context of plant.
geometries_to_ignore	Set of geometries to ignore.
occupancy_map	TaggedObjectOccupancyMapGrid to fill.
override_parent_body_index	Optionally provide a body index to override using the collision map frame name to identify the parent body. Use this if the frame name is not unique, or if the frame name does not match an existing MbP body (e.g. the name is a TF-compatible name incompatible with GetBodyByName).

◆ FillTaggedObjectOccupancyMap()

void FillTaggedObjectOccupancyMap	(	const multibody::MultibodyPlant< double > &	plant,
		const systems::Context< double > &	plant_context,
		const std::unordered_set< geometry::GeometryId > &	geometries_to_ignore,
		VoxelTaggedObjectOccupancyMap *	occupancy_map,
		const std::optional< multibody::BodyIndex > &	override_parent_body_index = {},
		Parallelism	parallelism = Parallelism::Max() )

Fills an initialized VoxelTaggedObjectOccupancyMap.

Empty cells receive occupancy=0.0, filled cells receive occupancy=1.0. Filled cells of the environment receive object_id values corresponding to the integer values of the GeometryId in that location. If your environment has more than 2^32 GeometryIds, this will throw.

Parameters

plant	MultibodyPlant model with a registered SceneGraph.
plant_context	Context of plant.
geometries_to_ignore	Set of geometries to ignore.
occupancy_map	TaggedObjectOccupancyMapGrid to fill.
override_parent_body_index	Optionally provide a body index to override using the collision map frame name to identify the parent body. Use this if the frame name is not unique, or if the frame name does not match an existing MbP body (e.g. the name is a TF-compatible name incompatible with GetBodyByName).

◆ SelfFilter() [1/2]

void SelfFilter	(	const SphereRobotModelCollisionChecker &	collision_checker,
		const Eigen::VectorXd &	q,
		double	padding,
		multibody::BodyIndex	grid_body_index,
		VoxelOccupancyMap *	occupancy_map,
		Parallelism	parallelism,
		std::optional< int >	context_number = std::nullopt )

Self-filter implementation for VoxelOccupancyMap environments.

Self-filter marks voxels belonging to the robot as empty so that they do not produce false collisions in a voxelized environment used for collision checking.

Parameters

collision_checker	Sphere-model collision checker that provides the sphere model of robot geometry and performs forward kinematics.
q	Current configuration of the robot.
padding	Padding to inflate the spheres of the collision model to use in the self-filter.

Precondition: >= 0.0.

Parameters

occupancy_map Current environment.

Precondition: != nullptr.

Parameters

parallelism	Parallelism to use.
context_number	Optional context number for use in parallel contexts.

◆ SelfFilter() [2/2]

void SelfFilter	(	const SphereRobotModelCollisionChecker &	collision_checker,
		const Eigen::VectorXd &	q,
		double	padding,
		multibody::BodyIndex	grid_body_index,
		VoxelTaggedObjectOccupancyMap *	occupancy_map,
		Parallelism	parallelism,
		std::optional< int >	context_number = std::nullopt )

Self-filter implementation for VoxelTaggedObjectOccupancyMap environments.

Self-filter marks voxels belonging to the robot as empty so that they do not produce false collisions in a voxelized environment used for collision checking.

Parameters

collision_checker	Sphere-model collision checker that provides the sphere model of robot geometry and performs forward kinematics.
q	Current configuration of the robot.
padding	Padding to inflate the spheres of the collision model to use in the self-filter.

Precondition: >= 0.0.

Parameters

occupancy_map Current environment.

Precondition: != nullptr.

Parameters

parallelism	Parallelism to use.
context_number	Optional context number for use in parallel contexts.

Classes

Functions

Function Documentation

◆ BuildOccupancyMap()

◆ BuildTaggedObjectOccupancyMap()

◆ FillOccupancyMap()

◆ FillTaggedObjectOccupancyMap()

◆ SelfFilter() [1/2]

◆ SelfFilter() [2/2]