Detailed Description

Interface for collision checkers to use.

This interface builds on the basic multi-threading idiom of Drake: one context per thread. It offers two models to achieve multi-threaded parallel collision checking:

using thread pools (e.g. OpenMP or similar) and "implicit contexts" managed by this object
using arbitrary threads and "explicit contexts" created by this object

Implicit Context Parallelism

Many methods of this class aren't designed for entry from arbitrary threads (e.g. from std::async threads), but rather are designed for use with a main thread and various thread-pool-parallel operations achieved by using directives like omp parallel. To support this usage, the base class AllocateContexts() protected method establishes a pool of contexts to support the implicit context parallelism specified in the constructor. (Note: if the collision checker declares that parallel checking is not supported, only one implict context will be allocated). AllocateContexts() must be called and only be called as part of the constructor of a derived class defined as final.

Once the context pool is created, clients can access a thread-associated context by using model_context(optional<int> context_number) and related methods. These methods may be called in two ways:

without a context number, the association between thread and context uses the OpenMP notion of thread number
with a context number, the method uses the context corresponding to the provided number

Without a context number, these context access methods are only safe under the following conditions:

the caller is the "main thread"
the caller is an OpenMP team thread during execution of a parallel region

With a context number, these context access methods are only safe under the following conditions:

no two or more threads simultaneously use the same context number

Methods supporting implicit context parallelism are noted below by having a reference to this section; as a rule of thumb, any public method that takes a context_number argument uses implicit context parallelism.

Users of this class (derived classes and others) can write their own parallel operations using implicit contexts, provided they limit parallel blocks to only use const methods or methods marked to support implicit contexts parallelism, and the parallel operations are:

without a context number, only with parallelism using OpenMP directives
with a context number, via a parallelization method that provides a notion of thread numbers similar in behavior to OpenMP's (i.e. a thread number in [0, number of threads), not arbitrary values like std::this_thread::get_id())

To determine the greatest implicit context parallelism that can be achieved in a parallelized operation, GetNumberOfThreads(Parallelism parallelize) returns the lesser of the provided parallelism and the supported implicit context parallelism.

Explicit Context Parallelism

It is also possible to use arbitrary thread models to perform collision checking in parallel using explicitly created contexts from this class. Contexts returned from MakeStandaloneModelContext() may be used in any thread, using only const methods of this class, or "explicit context" methods.

Explicit contexts are tracked by this class using std::weak_ptr to track their lifetimes. This mechanism is used by PerformOperationAgainstAllModelContexts to map an operation over all collision contexts, whether explicit or implicit.

Methods supporting explicit context parallelism are noted below by having a reference to this section; as a rule of thumb, any public method that takes a model_context first argument uses explicit context parallelism.

In practice, multi-threaded collision checking with explicit contexts may look something like the example below.

const Eigen::VectorXd start_q ...
const Eigen::VectorXd sample_q1 ...
const Eigen::VectorXd sample_q2 ...
const auto check_edge_to = [&collision_checker, &start_q] (
    const Eigen::VectorXd& sample_q,
    CollisionCheckerContext* explicit_context) {
  return collision_checker.CheckContextEdgeCollisionFree(
      explicit_context, start_q, sample_q);
};
const auto context_1 = collision_checker.MakeStandaloneModelContext();
const auto context_2 = collision_checker.MakeStandaloneModelContext();
auto future_q1 = std::async(std::launch::async, check_edge_to, sample_q1,
                            context_1.get());
auto future_q2 = std::async(std::launch::async, check_edge_to, sample_q2,
                            context_2.get());
const double edge_1_valid = future_q1.get();
const double edge_2_valid = future_q2.get();

Mixing Threading Models

It is possible to support mixed threading models, i.e., using both OpenMP thread pools and arbitrary threads. In this case, each arbitrary thread (say, from std::async) should have its own instance of a collision checker made using Clone(). Then each arbitrary thread will have its own implicit context pool.

Implementing Derived Classes

Collision checkers deriving from CollisionChecker must support parallel operations from both of the above parallelism models. This is generally accomplished by placing all mutable state within the per-thread context. If this cannot be accomplished, the shared mutable state must be accessed in a thread-safe manner. There are APIs that depend on SupportsParallelChecking() (e.g., CheckConfigsCollisionFree(), CheckEdgeCollisionFreeParallel(), CheckEdgesCollisionFree(), etc); a derived implementation should return false from SupportsParallelChecking() if there is no meaningful benefit to attempting to do work in parallel (e.g., they must fully serialize on shared state).

#include <drake/planning/collision_checker.h>

Classes
struct	AddedShape
	Representation of an "added" shape. More...

Public Member Functions
virtual	~CollisionChecker ()

std::unique_ptr< CollisionChecker >	Clone () const

bool	SupportsParallelChecking () const
	Does the collision checker support true parallel collision checks? More...

Does not allow copy, move, or assignment.
	CollisionChecker (CollisionChecker &&)=delete

CollisionChecker &	operator= (const CollisionChecker &)=delete

CollisionChecker &	operator= (CollisionChecker &&)=delete

Robot Model
These methods all provide access to the underlying robot model's contents.
const RobotDiagram< double > &	model () const

const multibody::MultibodyPlant< double > &	plant () const

const multibody::RigidBody< double > &	get_body (multibody::BodyIndex body_index) const

const std::vector< multibody::ModelInstanceIndex > &	robot_model_instances () const
	Gets the set of model instances belonging to the robot. More...

bool	IsPartOfRobot (const multibody::RigidBody< double > &body) const

bool	IsPartOfRobot (multibody::BodyIndex body_index) const

const Eigen::VectorXd &	GetZeroConfiguration () const

Context management
int	num_allocated_contexts () const

const CollisionCheckerContext &	model_context (std::optional< int > context_number=std::nullopt) const
	Accesses a collision checking context from within the implicit context pool owned by this collision checker. More...

const systems::Context< double > &	plant_context (std::optional< int > context_number=std::nullopt) const
	Accesses a multibody plant sub-context context from within the implicit context pool owned by this collision checker. More...

const systems::Context< double > &	UpdatePositions (const Eigen::VectorXd &q, std::optional< int > context_number=std::nullopt) const
	Updates the generalized positions `q` in the implicit context specified and returns a reference to the MultibodyPlant's now-updated context. More...

const systems::Context< double > &	UpdateContextPositions (CollisionCheckerContext *model_context, const Eigen::VectorXd &q) const
	Explicit Context-based version of UpdatePositions(). More...

std::shared_ptr< CollisionCheckerContext >	MakeStandaloneModelContext () const
	Make and track a CollisionCheckerContext. More...

void	PerformOperationAgainstAllModelContexts (const std::function< void(const RobotDiagram< double > &, CollisionCheckerContext *)> &operation)
	Allows externally-provided operations that must be performed against all contexts in the per-thread context pool, and any standalone contexts made with MakeStandaloneModelContext(). More...

Adding geometries to a body
While the underlying model will have some set of geometries to represent each body, it can be useful to extend the representative set of geometries. These APIs admit adding and removing such geometries to the underlying model. We'll distinguish the geometries added by the checker from those in the underlying model by referring to them as "checker" geometries and "model" geometries, respectively. For example, when an end effector has successfully grasped a manipuland, we can add additional geometry to the end effector body, representing the extent of the manipuland, causing the motion planning to likewise consider collisions with the manipuland. Note, this implicitly treats the manipuland as being rigidly affixed to the end effector. Checker geometries are managed in groups, identified by "group names". Each group can contain one or more checker geometries. This is particularly useful when multiple geometries should be added and removed as a whole. Simply by storing the shared group name, all checker geometries in the group can be removed with a single invocation, relying on the collision checker to do the book keeping for you. Note that different collision checker implementations may limit the set of supported Shape types; e.g. sphere-robot-model collision checkers only add sphere shapes to robot bodies, and voxel-based collision checkers cannot add individual collision geometries to the environment at all. Therefore, all methods for adding collision geometries report if the geometries were added. The derived classes should clearly document which shapes they support.
bool	AddCollisionShape (const std::string &group_name, const BodyShapeDescription &description)
	Requests the addition of a shape to a body, both given in `description`. More...

int	AddCollisionShapes (const std::string &group_name, const std::vector< BodyShapeDescription > &descriptions)
	Requests the addition of N shapes to N bodies, each given in the set of `descriptions`. More...

std::map< std::string, int >	AddCollisionShapes (const std::map< std::string, std::vector< BodyShapeDescription >> &geometry_groups)
	Requests the addition of a collection of shapes to bodies across multiple geometry groups. More...

bool	AddCollisionShapeToFrame (const std::string &group_name, const multibody::Frame< double > &frameA, const geometry::Shape &shape, const math::RigidTransform< double > &X_AG)
	Requests the addition of `shape` to the frame A in the checker's model. More...

bool	AddCollisionShapeToBody (const std::string &group_name, const multibody::RigidBody< double > &bodyA, const geometry::Shape &shape, const math::RigidTransform< double > &X_AG)
	Requests the addition of `shape` to the body A in the checker's model The added `shape` will belong to the named geometry group. More...

std::map< std::string, std::vector< BodyShapeDescription > >	GetAllAddedCollisionShapes () const
	Gets all checker geometries currently added across the whole checker. More...

void	RemoveAllAddedCollisionShapes (const std::string &group_name)
	Removes all added checker geometries which belong to the named group. More...

void	RemoveAllAddedCollisionShapes ()
	Removes all added checker geometries from all geometry groups. More...

Padding the distance between bodies
Ultimately, the model's bodies are represented with geometries. The distance between bodies is the distance between their representative geometries. However, the exact distance between those geometries is not necessarily helpful in planning. For example, You may want to add padding when real-world objects differ from the planning geometry. In some cases, limited penetration is expected, and possibly desirable, such as when grasping in clutter. Padding is a mechanism where these distances can be "fudged" without modifying the underlying model. The reported distance between two bodies can be decreased by adding positive padding or increased by adding negative* padding. One could think of it as padding the geometries – making the objects larger (positive) or smaller (negative). This isn't quite true. Padding is defined for pairs of geometries. Ultimately, the padding data is stored in an NxN matrix (where the underlying model has N total bodies). The matrix is symmetric and the entry at (i, j) – and (j, i) – is the padding value to be applied to distance measurements between bodies i and j. It is meaningless to apply padding between a body and itself. Furthermore, as CollisionChecker is concerned with the state of the robot, it is equally meaningless to apply padding between environment* bodies. To avoid the illusion of padding, those entries on the diagonal and corresponding to environment body pairs are kept at zero. Configuring padding Padding can be applied at different levels of scope: For all pairs, via SetPaddingMatrix(). For all (robot, environment) or (robot, robot) pairs via SetPaddingAllRobotEnvironmentPairs() and SetPaddingAllRobotRobotPairs(). For all (robot i, environment) pairs via SetPaddingOneRobotBodyAllEnvironmentPairs(). For pair (body i, body j) via SetPaddingBetween(). Invocations of these methods make immediate changes to the underlying padding data. Changing the order of invocations will change the final padding state. In other words, setting padding for a particular pair (A, B) followed by calling, for example, SetPaddingOneRobotBodyAllEnvironmentPairs(A) could erase the effect of the first invocation. Configuration prerequisites In all these configuration methods, there are some specific requirements: The padding value must always be finite. Body indices must be in range (i.e., in [0, N) for a model with N total bodies). If the parameters include one or more more body indices, at least one of them must reference a robot body. Introspection The current state of collision padding can be introspected via a number of methods: View the underlying NxN padding matrix via GetPaddingMatrix(). Find out if padding values are heterogeneous via MaybeGetUniformRobotEnvironmentPadding() and MaybeGetUniformRobotRobotPadding(). Query the specific padding value between a pair of bodies via GetPaddingBetween(). Find out the maximum padding value via GetLargestPadding().
std::optional< double >	MaybeGetUniformRobotEnvironmentPadding () const
	If the padding between all robot bodies and environment bodies is the same, returns the common padding value. More...

std::optional< double >	MaybeGetUniformRobotRobotPadding () const
	If the padding between all pairs of robot bodies is the same, returns the common padding value. More...

double	GetPaddingBetween (multibody::BodyIndex bodyA_index, multibody::BodyIndex bodyB_index) const
	Gets the padding value for the pair of bodies specified. More...

double	GetPaddingBetween (const multibody::RigidBody< double > &bodyA, const multibody::RigidBody< double > &bodyB) const
	Overload that uses body references. More...

void	SetPaddingBetween (multibody::BodyIndex bodyA_index, multibody::BodyIndex bodyB_index, double padding)
	Sets the padding value for the pair of bodies specified. More...

void	SetPaddingBetween (const multibody::RigidBody< double > &bodyA, const multibody::RigidBody< double > &bodyB, double padding)
	Overload that uses body references. More...

const Eigen::MatrixXd &	GetPaddingMatrix () const
	Gets the collision padding matrix. More...

void	SetPaddingMatrix (const Eigen::MatrixXd &collision_padding)
	Sets the collision padding matrix. More...

double	GetLargestPadding () const
	Gets the current largest collision padding across all (robot, *) body pairs. More...

void	SetPaddingOneRobotBodyAllEnvironmentPairs (multibody::BodyIndex body_index, double padding)
	Sets the environment collision padding for the provided robot body with respect to all environment bodies. More...

void	SetPaddingAllRobotEnvironmentPairs (double padding)
	Sets the padding for all (robot, environment) pairs. More...

void	SetPaddingAllRobotRobotPairs (double padding)
	Sets the padding for all (robot, robot) pairs. More...

Collision filtering
The CollisionChecker adapts the idea of "collision filtering" to bodies (see geometry::CollisionFilterManager). In addition to whatever collision filters have been declared within the underlying model, CollisionChecker provides mechanisms to layer additional filters by specifying a pair of bodies as being "filtered". No collisions or distance measurements are reported on filtered body pairs. The "filter" state of all possible body pairs are stored in a symmetric NxN integer-valued matrix, where N is the number of bodies reported by the plant owned by this collision checker. The matrix can only contain one of three values: 0, 1, and -1. For the (i, j) entry in the matrix, each value would be interpreted as follows: 0: The collision is not filtered. Collision checker will report collisions and clearance between bodies I and J. 1: The collision is filtered. Collision checker will not report collisions and clearance between bodies I and J. -1: The collision is filtered by definition. Collision checker will not* report collisions and clearance between bodies I and J and the user cannot change this state. CollisionChecker limits itself to characterizing the state of the robot. As such, it always filters collisions between pairs of environment bodies. It also filters collisions between a body and itself as nonsensical. Therefore, the matrix will always have immutable -1s along the diagonal and for every cell representing an environment-environment pair. The collision filter matrix must remain consistent. Only valid values for body pairs are accepted. I.e., assigning an (environment, environment) pair a value of 0 or 1 is "inconsistent". Likewise doing the same on the diagonal. Alternatively, assigning a -1 to any pair with a robot body would be inconsistent. The functions for configuring collision filters will throw if explicitly asked to make an inconsistent change. The CollisionChecker's definition of filtered body pairs is initially drawn from the underlying model's configuration (see GetNominalFilteredCollisionMatrix()). However, the CollisionChecker's definition of the set of collision filters is independent of the model after construction and can be freely modified to allow for greater freedom in determining which bodies can affect each other.
const Eigen::MatrixXi &	GetNominalFilteredCollisionMatrix () const
	Returns the "nominal" collision filter matrix. More...

const Eigen::MatrixXi &	GetFilteredCollisionMatrix () const
	Gets the "active" collision filter matrix. More...

void	SetCollisionFilterMatrix (const Eigen::MatrixXi &filter_matrix)
	Sets the "active" collision filter matrix. More...

bool	IsCollisionFilteredBetween (multibody::BodyIndex bodyA_index, multibody::BodyIndex bodyB_index) const
	Checks if collision is filtered between the two bodies specified. More...

bool	IsCollisionFilteredBetween (const multibody::RigidBody< double > &bodyA, const multibody::RigidBody< double > &bodyB) const
	Overload that uses body references. More...

void	SetCollisionFilteredBetween (multibody::BodyIndex bodyA_index, multibody::BodyIndex bodyB_index, bool filter_collision)
	Declares the body pair (bodyA, bodyB) to be filtered (or not) based on `filter_collision`. More...

void	SetCollisionFilteredBetween (const multibody::RigidBody< double > &bodyA, const multibody::RigidBody< double > &bodyB, bool filter_collision)
	Overload that uses body references. More...

void	SetCollisionFilteredWithAllBodies (multibody::BodyIndex body_index)
	Declares that body pair (B, O) is filtered (for all bodies O in this checker's plant). More...

void	SetCollisionFilteredWithAllBodies (const multibody::RigidBody< double > &body)
	Overload that uses body references. More...

Configuration collision checking
bool	CheckConfigCollisionFree (const Eigen::VectorXd &q, std::optional< int > context_number=std::nullopt) const
	Checks a single configuration for collision using the current thread's associated context. More...

bool	CheckContextConfigCollisionFree (CollisionCheckerContext *model_context, const Eigen::VectorXd &q) const
	Explicit Context-based version of CheckConfigCollisionFree(). More...

std::vector< uint8_t >	CheckConfigsCollisionFree (const std::vector< Eigen::VectorXd > &configs, Parallelism parallelize=Parallelism::Max()) const
	Checks a vector of configurations for collision, evaluating in parallel when supported and enabled by `parallelize`. More...

Edge collision checking
These functions serve motion planning methods, such as sampling-based planners and shortcut smoothers, which are concerned about checking that an "edge" between two configurations, q1 and q2, is free of collision. These functions approximately answer the question: is the edge collision free. The answer is determined by sampling configurations along the edge and reporting the edge to be collision free if all samples are collision free. Otherwise, we report the fraction of samples (starting with the start configuration) that reported to be collision free. The tested samples include the start and end configurations, q1 and q2, respectively. CollisionChecker doesn't know how an edge is defined. An edge can be anything from a simple line segment (e.g. a linear path in C-space) to an arbitrarily complex path (e.g. a Reeds-Shepp path). The shape of the edge is defined implicitly by the ConfigurationInterpolationFunction as an interpolation between two configurations `q1` and `q2` (see SetConfigurationDistanceFunction() and SetConfigurationInterpolationFunction()). CollisionChecker picks configuration samples along an edge by uniformly sampling the interpolation parameter from zero to one. (By definition, the interpolation at zero is equal to `q1` and at one is equal to `q2`.) The number of samples is determined by the distance between `q1` and `q2`, as provided by a ConfigurationDistanceFunction, divided by "edge step size" (see set_edge_step_size()). As a result, the selection of interpolation function, distance function, and edge step size must be coordinated to ensure that edge collision checking is sufficiently accurate for your application. Default functions The configuration distance function is defined at construction (from CollisionCheckerParams). It can be as simple as `\|q1 - q2\|` or could be a weighted norm `\|wᵀ⋅(q1 − q2)\|` based on joint importance or unit reconciliation (e.g., some qs are translational and some are rotational). Because of this, the "distance" reported may have arbitrary units. Whatever the units are, the edge step size must match. The step size value and distance function will determine the number of samples on the edge. The smaller the step size, the more samples (and the more expensive the collision check becomes). If all joints are revolute joints, one reasonable distance function is the weighted function `\|wᵀ⋅(q1 − q2)\|` where the weights are based on joint speed. For joint dofs `J = [J₀, J₁, ...]` with corresponding positive maximum speeds `[s₀, s₁, ...]`, we identify the speed of the fastest joint `sₘₐₓ = maxᵢ(sᵢ)` and define the per-dof weights as `wᵢ = sₘₐₓ / sᵢ`. Intuitively, the more time a particular joint requires to cover an angular distance, the more significance we attribute to that distance – it's a time-biased weighting function. The weights are unitless so the reported distance is in radians. For some common arms (IIWA, Panda, UR, Jaco, etc.), we have found that an edge step size of 0.05 radians produces reasonable results. CollisionChecker has a default interpolation function, as defined by MakeDefaultConfigurationInterpolationFunction(). It performs Slerp for quaternion-valued dofs and linear interpolation for all other dofs. Note that this is not appropriate for all robots, (e.g. those using a BallRpyJoint, or any non-holonomic robot). You will need to provide your own interpolation function in such cases. Function-level parallelism Parallelization in some edge collision checks is provided using OpenMP and is enabled when both: (1) the collision checker declares that parallelization is supported (i.e. when SupportsParallelChecking() is true) and (2) when multiple OpenMP threads are available for execution. Due to this internal parallelism, special care must be paid when calling these methods from any thread that is not the main thread; ensure that, for a given collision checker instance, implicit context methods are only called from one non-OpenMP thread at a given time. Thoughts on configuring CollisionChecker for edge collision detection Because the edge collision check samples the edge, there is a perpetual trade off between the cost of evaluating the edge and the likelihood that a real collision is missed. In practice, the CollisionChecker should be configured to maximize performance for a reasonable level of collision detection reliability. There is no definitive method for tuning the parameters. Rather than advocating a tuning strategy, we'll simply elaborate on the available parameters and leave the actual tuning as an exercise for the reader. There are two properties that will most directly contribute to the accuracy of the edge tests: edge step size and padding. Ultimately, any obstacle in C-space whose measure is smaller than the edge step size is likely to be missed. The goal is to tune the parameters such that such features – located in an area of interest (i.e., where you want your robot to operate) – will have a low probability of being missed. Edge step size is very much a global parameter. It will increase the cost of every collision check. If you are unable to anticipate where the small features are, a small edge step size will be robust to that uncertainty. As such, it serves as a good backstop. It comes at a cost of increasing the cost of every test, even for large geometries with nothing but coarse features. Increasing the padding can likewise reduce the likelihood of missing features. Adding padding has the effect of increasing the size of the workspace obstacles in C-space. The primary benefit is that it can be applied locally. If there is a particular obstacle with fine features that your robot will be near, padding between robot and that obstacle can be added so that interactions between robot and obstacle are more likely to be caught. Doing so leaves the global cost low for coarse features. However, padding comes at the cost that physically free edges may no longer be considered free. The best tuning will likely include configuring both edge step size and applying appropriate padding.
void	SetDistanceAndInterpolationProvider (std::shared_ptr< const DistanceAndInterpolationProvider > provider)
	Sets the distance and interpolation provider to use. More...

const DistanceAndInterpolationProvider &	distance_and_interpolation_provider () const
	Gets the DistanceAndInterpolationProvider in use. More...

void	SetConfigurationDistanceFunction (const ConfigurationDistanceFunction &distance_function)
	Sets the configuration distance function to `distance_function`. More...

double	ComputeConfigurationDistance (const Eigen::VectorXd &q1, const Eigen::VectorXd &q2) const
	Computes configuration-space distance between the provided configurations `q1` and `q2`, using the distance function configured at construction- time or via SetConfigurationDistanceFunction(). More...

ConfigurationDistanceFunction	MakeStandaloneConfigurationDistanceFunction () const

void	SetConfigurationInterpolationFunction (const ConfigurationInterpolationFunction &interpolation_function)
	Sets the configuration interpolation function to `interpolation_function`. More...

Eigen::VectorXd	InterpolateBetweenConfigurations (const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, double ratio) const
	Interpolates between provided configurations `q1` and `q2`. More...

ConfigurationInterpolationFunction	MakeStandaloneConfigurationInterpolationFunction () const

double	edge_step_size () const
	Gets the current edge step size. More...

void	set_edge_step_size (double edge_step_size)
	Sets the edge step size to `edge_step_size`. More...

bool	CheckEdgeCollisionFree (const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, std::optional< int > context_number=std::nullopt) const
	Checks a single configuration-to-configuration edge for collision, using the current thread's associated context. More...

bool	CheckContextEdgeCollisionFree (CollisionCheckerContext *model_context, const Eigen::VectorXd &q1, const Eigen::VectorXd &q2) const
	Explicit Context-based version of CheckEdgeCollisionFree(). More...

bool	CheckEdgeCollisionFreeParallel (const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, Parallelism parallelize=Parallelism::Max()) const
	Checks a single configuration-to-configuration edge for collision. More...

std::vector< uint8_t >	CheckEdgesCollisionFree (const std::vector< std::pair< Eigen::VectorXd, Eigen::VectorXd >> &edges, Parallelism parallelize=Parallelism::Max()) const
	Checks multiple configuration-to-configuration edges for collision. More...

EdgeMeasure	MeasureEdgeCollisionFree (const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, std::optional< int > context_number=std::nullopt) const
	Checks a single configuration-to-configuration edge for collision, using the current thread's associated context. More...

EdgeMeasure	MeasureContextEdgeCollisionFree (CollisionCheckerContext *model_context, const Eigen::VectorXd &q1, const Eigen::VectorXd &q2) const
	Explicit Context-based version of MeasureEdgeCollisionFree(). More...

EdgeMeasure	MeasureEdgeCollisionFreeParallel (const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, Parallelism parallelize=Parallelism::Max()) const
	Checks a single configuration-to-configuration edge for collision. More...

std::vector< EdgeMeasure >	MeasureEdgesCollisionFree (const std::vector< std::pair< Eigen::VectorXd, Eigen::VectorXd >> &edges, Parallelism parallelize=Parallelism::Max()) const
	Checks multiple configuration-to-configuration edge for collision. More...

Robot collision state
These methods help characterize the robot's collision state with respect to a particular robot configuration. In this section, the "collision state" is characterized with two different APIs: "clearance", a measure of how near to collision the robot as computed by CalcRobotClearance(), and a boolean colliding state for each robot body as computed by ClassifyBodyCollisions().
RobotClearance	CalcRobotClearance (const Eigen::VectorXd &q, double influence_distance, std::optional< int > context_number=std::nullopt) const
	Calculates the distance, ϕ, and distance Jacobian, Jqᵣ_ϕ, for each potential collision whose distance is less than `influence_distance`, using the current thread's associated context. More...

RobotClearance	CalcContextRobotClearance (CollisionCheckerContext *model_context, const Eigen::VectorXd &q, double influence_distance) const
	Explicit Context-based version of CalcRobotClearance(). More...

int	MaxNumDistances (std::optional< int > context_number=std::nullopt) const
	Returns an upper bound on the number of distances returned by CalcRobotClearance(), using the current thread's associated context. More...

int	MaxContextNumDistances (const CollisionCheckerContext &model_context) const
	Explicit Context-based version of MaxNumDistances(). More...

std::vector< RobotCollisionType >	ClassifyBodyCollisions (const Eigen::VectorXd &q, std::optional< int > context_number=std::nullopt) const
	Classifies which robot bodies are in collision (and which type of collision) for the provided configuration `q`, using the current thread's associated context. More...

std::vector< RobotCollisionType >	ClassifyContextBodyCollisions (CollisionCheckerContext *model_context, const Eigen::VectorXd &q) const
	Explicit Context-based version of ClassifyBodyCollisions(). More...

Protected Member Functions
	CollisionChecker (CollisionCheckerParams params, bool supports_parallel_checking)
	Derived classes declare upon construction whether they support parallel checking (see SupportsParallelChecking()). More...

	CollisionChecker (const CollisionChecker &)
	To support Clone(), allow copying (but not move nor assign). More...

void	AllocateContexts ()
	Allocate the per-thread context pool, and discontinue mutable access to the robot model. More...

virtual std::unique_ptr< CollisionCheckerContext >	CreatePrototypeContext () const
	Collision checkers that use derived context types can override this implementation to allocate their context type instead. More...

bool	IsInitialSetup () const

RobotDiagram< double > &	GetMutableSetupModel ()

bool	CanEvaluateInParallel () const

std::string	CriticizePaddingMatrix () const

Internal overridable implementations of public methods.
virtual std::unique_ptr< CollisionChecker >	DoClone () const =0
	Derived collision checkers implement can make use of the protected copy constructor to implement DoClone(). More...

virtual void	DoUpdateContextPositions (CollisionCheckerContext *model_context) const =0
	Derived collision checkers can do further work in this function in response to updates to the MultibodyPlant positions. More...

virtual bool	DoCheckContextConfigCollisionFree (const CollisionCheckerContext &model_context) const =0
	Derived collision checkers are responsible for reporting the collision status of the configuration. More...

virtual std::optional< geometry::GeometryId >	DoAddCollisionShapeToBody (const std::string &group_name, const multibody::RigidBody< double > &bodyA, const geometry::Shape &shape, const math::RigidTransform< double > &X_AG)=0
	Does the work of adding a shape to be rigidly affixed to the body. More...

virtual void	RemoveAddedGeometries (const std::vector< AddedShape > &shapes)=0
	Removes all of the given added shapes (if they exist) from the checker. More...

virtual void	UpdateCollisionFilters ()=0
	Derived collision checkers can do further work in this function in response to changes in collision filters. More...

virtual RobotClearance	DoCalcContextRobotClearance (const CollisionCheckerContext &model_context, double influence_distance) const =0
	Derived collision checkers are responsible for defining the reported measurements. More...

virtual std::vector< RobotCollisionType >	DoClassifyContextBodyCollisions (const CollisionCheckerContext &model_context) const =0
	Derived collision checkers are responsible for choosing a collision type for each of the robot bodies. More...

virtual int	DoMaxContextNumDistances (const CollisionCheckerContext &model_context) const =0
	Derived collision checkers must implement the semantics documented for MaxNumDistances. More...

Constructor & Destructor Documentation

◆ CollisionChecker() [1/3]

CollisionChecker ( CollisionChecker && )

delete

◆ ~CollisionChecker()

virtual ~CollisionChecker ( )

virtual

◆ CollisionChecker() [2/3]

CollisionChecker	(	CollisionCheckerParams	params,
		bool	supports_parallel_checking
	)

protected

Derived classes declare upon construction whether they support parallel checking (see SupportsParallelChecking()).

If a derived class does not support parallel checking, it must set params.implicit_context_parallelism to Parallelism::None(); otherwise this constructor will throw.

Exceptions

std::exception if params is invalid.

See also: CollisionCheckerParams.

◆ CollisionChecker() [3/3]

CollisionChecker ( const CollisionChecker & )

protected

To support Clone(), allow copying (but not move nor assign).

Member Function Documentation

◆ AddCollisionShape()

bool AddCollisionShape	(	const std::string &	group_name,
		const BodyShapeDescription &	description
	)

Requests the addition of a shape to a body, both given in description.

If added, the shape will belong to the named geometry group.

Parameters

group_name	The name of the group to add the geometry to.
description	The data describing the shape and target body.

Returns: true if the shape was added.

◆ AddCollisionShapes() [1/2]

int AddCollisionShapes	(	const std::string &	group_name,
		const std::vector< BodyShapeDescription > &	descriptions
	)

Requests the addition of N shapes to N bodies, each given in the set of descriptions.

Each added shape will belong to the named geometry group.

Parameters

group_name	The name of the group to add the geometry to.
descriptions	The descriptions of N (shape, body) pairs.

Returns: The total number of shapes in descriptions that got added.

◆ AddCollisionShapes() [2/2]

std::map<std::string, int> AddCollisionShapes ( const std::map< std::string, std::vector< BodyShapeDescription >> & geometry_groups )

Requests the addition of a collection of shapes to bodies across multiple geometry groups.

geometry_groups specifies a collection of (shape, body) descriptors across multiple geometry groups.

Parameters

geometry_groups A map from a named geometry group to the (shape, body) pairs to add to that group.

Returns: A map from input named geometry group to the number of geometries added to that group.

◆ AddCollisionShapeToBody()

bool AddCollisionShapeToBody	(	const std::string &	group_name,
		const multibody::RigidBody< double > &	bodyA,
		const geometry::Shape &	shape,
		const math::RigidTransform< double > &	X_AG
	)

Requests the addition of shape to the body A in the checker's model The added shape will belong to the named geometry group.

Parameters

group_name	The name of the group to add the geometry to.
bodyA	The body the shape should be rigidly affixed to.
shape	The requested shape, defined in its canonical frame G.
X_AG	The pose of the shape in body A's frame.

Returns: true if the shape was added.

◆ AddCollisionShapeToFrame()

bool AddCollisionShapeToFrame	(	const std::string &	group_name,
		const multibody::Frame< double > &	frameA,
		const geometry::Shape &	shape,
		const math::RigidTransform< double > &	X_AG
	)

Requests the addition of shape to the frame A in the checker's model.

The added shape will belong to the named geometry group.

Parameters

group_name	The name of the group to add the geometry to.
frameA	The frame the shape should be rigidly affixed to.
shape	The requested shape, defined in its canonical frame G.
X_AG	The pose of the shape in the frame A.

Returns: true if the shape was added.

◆ AllocateContexts()

void AllocateContexts ( )

protected

Allocate the per-thread context pool, and discontinue mutable access to the robot model.

This must be called and only be called as part of the constructor in a derived class defined as final.

Precondition: This cannot have already been called for this instance.

◆ CalcContextRobotClearance()

RobotClearance CalcContextRobotClearance	(	CollisionCheckerContext *	model_context,
		const Eigen::VectorXd &	q,
		double	influence_distance
	)		const

Explicit Context-based version of CalcRobotClearance().

Exceptions

std::exception if model_context is nullptr.

See also: Explicit Context Parallelism.

◆ CalcRobotClearance()

RobotClearance CalcRobotClearance	(	const Eigen::VectorXd &	q,
		double	influence_distance,
		std::optional< int >	context_number = `std::nullopt`
	)		const

Calculates the distance, ϕ, and distance Jacobian, Jqᵣ_ϕ, for each potential collision whose distance is less than influence_distance, using the current thread's associated context.

Distances for filtered collisions will not be returned.

Distances between a pair of robot bodies (i.e., where collision_types() reports SelfCollision) report one body's index in robot_indices() and the the other body's in other_indices(); which body appears in which column is arbitrary.

The total number of rows can depend on how the model is defined and how a particular CollisionChecker instance is implemented (see MaxNumDistances()).

See also: RobotClearance for details on the quantities ϕ and Jqᵣ_ϕ (and other details).

Parameters

context_number Optional implicit context number.

See also: Implicit Context Parallelism.

◆ CanEvaluateInParallel()

bool CanEvaluateInParallel ( ) const

protected

Returns: true if this object SupportsParallelChecking() and more than one thread is available.

◆ CheckConfigCollisionFree()

bool CheckConfigCollisionFree	(	const Eigen::VectorXd &	q,
		std::optional< int >	context_number = `std::nullopt`
	)		const

Checks a single configuration for collision using the current thread's associated context.

Parameters

q	Configuration to check
context_number	Optional implicit context number.

Returns: true if collision free, false if in collision.

See also: Implicit Context Parallelism.

◆ CheckConfigsCollisionFree()

std::vector<uint8_t> CheckConfigsCollisionFree	(	const std::vector< Eigen::VectorXd > &	configs,
		Parallelism	parallelize = `Parallelism::Max()`
	)		const

Checks a vector of configurations for collision, evaluating in parallel when supported and enabled by parallelize.

Parallelization in configuration collision checks is provided using OpenMP and is supported when both: (1) the collision checker declares that parallelization is supported (i.e. when SupportsParallelChecking() is true) and (2) when multiple OpenMP threads are available for execution. See function-level parallelism for guidance on proper usage.

Parameters

configs	Configurations to check
parallelize	How much should collision checks be parallelized?

Returns: std::vector<uint8_t>, one for each configuration in configs. For each configuration, 1 if collision free, 0 if in collision.

◆ CheckContextConfigCollisionFree()

bool CheckContextConfigCollisionFree	(	CollisionCheckerContext *	model_context,
		const Eigen::VectorXd &	q
	)		const

Explicit Context-based version of CheckConfigCollisionFree().

Exceptions

std::exception if model_context is nullptr.

See also: Explicit Context Parallelism.

◆ CheckContextEdgeCollisionFree()

bool CheckContextEdgeCollisionFree	(	CollisionCheckerContext *	model_context,
		const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2
	)		const

Explicit Context-based version of CheckEdgeCollisionFree().

Exceptions

std::exception if model_context is nullptr.

See also: Explicit Context Parallelism.

◆ CheckEdgeCollisionFree()

bool CheckEdgeCollisionFree	(	const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2,
		std::optional< int >	context_number = `std::nullopt`
	)		const

Checks a single configuration-to-configuration edge for collision, using the current thread's associated context.

Parameters

q1	Start configuration for edge.
q2	End configuration for edge.
context_number	Optional implicit context number.

Returns: true if collision free, false if in collision.

See also: Implicit Context Parallelism.

◆ CheckEdgeCollisionFreeParallel()

bool CheckEdgeCollisionFreeParallel	(	const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2,
		Parallelism	parallelize = `Parallelism::Max()`
	)		const

Checks a single configuration-to-configuration edge for collision.

Collision check is parallelized via OpenMP when supported. See function-level parallelism for guidance on proper usage.

Parameters

q1	Start configuration for edge.
q2	End configuration for edge.
parallelize	How much should edge collision check be parallelized?

Returns: true if collision free, false if in collision.

◆ CheckEdgesCollisionFree()

std::vector<uint8_t> CheckEdgesCollisionFree	(	const std::vector< std::pair< Eigen::VectorXd, Eigen::VectorXd >> &	edges,
		Parallelism	parallelize = `Parallelism::Max()`
	)		const

Checks multiple configuration-to-configuration edges for collision.

Collision checks are parallelized via OpenMP when supported and enabled by parallelize. See function-level parallelism for guidance on proper usage.

Parameters

edges	Edges to check, each in the form of pair<q1, q2>.
parallelize	How much should edge collision checks be parallelized?

Returns: std::vector<uint8_t>, one for each edge in edges. For each edge, 1 if collision free, 0 if in collision.

◆ ClassifyBodyCollisions()

std::vector<RobotCollisionType> ClassifyBodyCollisions	(	const Eigen::VectorXd &	q,
		std::optional< int >	context_number = `std::nullopt`
	)		const

Classifies which robot bodies are in collision (and which type of collision) for the provided configuration q, using the current thread's associated context.

Parameters

context_number Optional implicit context number.

Returns: a vector of collision types arranged in body index order. Only entries for robot bodies are guaranteed to be valid; entries for environment bodies are populated with kNoCollision, regardless of their actual status.

See also: Implicit Context Parallelism.

◆ ClassifyContextBodyCollisions()

std::vector<RobotCollisionType> ClassifyContextBodyCollisions	(	CollisionCheckerContext *	model_context,
		const Eigen::VectorXd &	q
	)		const

Explicit Context-based version of ClassifyBodyCollisions().

Exceptions

std::exception if model_context is nullptr.

See also: Explicit Context Parallelism.

◆ Clone()

std::unique_ptr<CollisionChecker> Clone ( ) const

◆ ComputeConfigurationDistance()

double ComputeConfigurationDistance	(	const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2
	)		const

Computes configuration-space distance between the provided configurations q1 and q2, using the distance function configured at construction- time or via SetConfigurationDistanceFunction().

◆ CreatePrototypeContext()

virtual std::unique_ptr<CollisionCheckerContext> CreatePrototypeContext ( ) const

protectedvirtual

Collision checkers that use derived context types can override this implementation to allocate their context type instead.

◆ CriticizePaddingMatrix()

std::string CriticizePaddingMatrix ( ) const

protected

◆ distance_and_interpolation_provider()

const DistanceAndInterpolationProvider& distance_and_interpolation_provider ( ) const

Gets the DistanceAndInterpolationProvider in use.

◆ DoAddCollisionShapeToBody()

virtual std::optional<geometry::GeometryId> DoAddCollisionShapeToBody	(	const std::string &	group_name,
		const multibody::RigidBody< double > &	bodyA,
		const geometry::Shape &	shape,
		const math::RigidTransform< double > &	X_AG
	)

protectedpure virtual

Does the work of adding a shape to be rigidly affixed to the body.

Derived checkers can choose to ignore the request, but must return nullopt if they do so.

Implemented in UnimplementedCollisionChecker.

◆ DoCalcContextRobotClearance()

virtual RobotClearance DoCalcContextRobotClearance	(	const CollisionCheckerContext &	model_context,
		double	influence_distance
	)		const

protectedpure virtual

Derived collision checkers are responsible for defining the reported measurements.

But they must adhere to the characteristics documented on RobotClearance, e.g., one measurement per row. CollisionChecker guarantees that influence_distance is finite and non-negative.

Implemented in UnimplementedCollisionChecker.

◆ DoCheckContextConfigCollisionFree()

virtual bool DoCheckContextConfigCollisionFree ( const CollisionCheckerContext & model_context ) const

protectedpure virtual

Derived collision checkers are responsible for reporting the collision status of the configuration.

CollisionChecker guarantees that the passed model_context has been updated with the configuration q supplied to the public method.

Implemented in UnimplementedCollisionChecker.

◆ DoClassifyContextBodyCollisions()

virtual std::vector<RobotCollisionType> DoClassifyContextBodyCollisions ( const CollisionCheckerContext & model_context ) const

protectedpure virtual

Derived collision checkers are responsible for choosing a collision type for each of the robot bodies.

They should adhere to the semantics documented for ClassifyBodyCollisions. CollisionChecker guarantees that the passed model_context has been updated with the configuration q supplied to the public method.

Implemented in UnimplementedCollisionChecker.

◆ DoClone()

virtual std::unique_ptr<CollisionChecker> DoClone ( ) const

protectedpure virtual

Derived collision checkers implement can make use of the protected copy constructor to implement DoClone().

Implemented in UnimplementedCollisionChecker.

◆ DoMaxContextNumDistances()

virtual int DoMaxContextNumDistances ( const CollisionCheckerContext & model_context ) const

protectedpure virtual

Derived collision checkers must implement the semantics documented for MaxNumDistances.

CollisionChecker does nothing; it just calls this method.

Implemented in UnimplementedCollisionChecker.

◆ DoUpdateContextPositions()

virtual void DoUpdateContextPositions ( CollisionCheckerContext * model_context ) const

protectedpure virtual

Derived collision checkers can do further work in this function in response to updates to the MultibodyPlant positions.

CollisionChecker guarantees that model_context will not be nullptr and that the new positions are present in model_context->plant_context().

Implemented in UnimplementedCollisionChecker.

◆ edge_step_size()

double edge_step_size ( ) const

Gets the current edge step size.

◆ get_body()

const multibody::RigidBody<double>& get_body ( multibody::BodyIndex body_index ) const

Returns: a const body reference to a body in the full model's plant for the given body_index.

◆ GetAllAddedCollisionShapes()

std::map<std::string, std::vector<BodyShapeDescription> > GetAllAddedCollisionShapes ( ) const

Gets all checker geometries currently added across the whole checker.

Returns: A mapping from each geometry group name to the collection of (shape, body) descriptions in that group.

◆ GetFilteredCollisionMatrix()

const Eigen::MatrixXi& GetFilteredCollisionMatrix ( ) const

Gets the "active" collision filter matrix.

◆ GetLargestPadding()

double GetLargestPadding ( ) const

Gets the current largest collision padding across all (robot, *) body pairs.

This excludes the meaningless zeros on the diagonal and environment-environment pairs; the return value can be negative.

◆ GetMutableSetupModel()

RobotDiagram<double>& GetMutableSetupModel ( )

protected

Returns: a mutable reference to the robot model.

Exceptions

std::exception if IsInitialSetup() == false.

◆ GetNominalFilteredCollisionMatrix()

const Eigen::MatrixXi& GetNominalFilteredCollisionMatrix ( ) const

Returns the "nominal" collision filter matrix.

The nominal matrix is initialized at construction time and represents the configuration of the model's plant and scene graph. It serves as a reference point to assess any changes to collision filters beyond this checker's intrinsic model.

Collisions between bodies A and B are filtered in the following cases:

There exists a welded path between A and B.
SceneGraph has filtered the collisions between all pairs of geometries of A and B.

Note: SceneGraph allows arbitrary collision filter configuration at the geometry* level. The filters on one geometry of body need not be the same as another geometry on the same body. CollisionChecker is body centric. It requires all geometries on a body to be filtered homogeneously. A SceneGraph that violates this stricter requirement cannot be used in a CollisionChecker. It is highly unlikely that a SceneGraph instance will ever be in this configuration by accident.

◆ GetPaddingBetween() [1/2]

double GetPaddingBetween	(	multibody::BodyIndex	bodyA_index,
		multibody::BodyIndex	bodyB_index
	)		const

Gets the padding value for the pair of bodies specified.

If the body indices are the same, zero will always be returned.

Exceptions

std::exception if either body index is out of range.

◆ GetPaddingBetween() [2/2]

double GetPaddingBetween	(	const multibody::RigidBody< double > &	bodyA,
		const multibody::RigidBody< double > &	bodyB
	)		const

Overload that uses body references.

◆ GetPaddingMatrix()

const Eigen::MatrixXd& GetPaddingMatrix ( ) const

Gets the collision padding matrix.

◆ GetZeroConfiguration()

const Eigen::VectorXd& GetZeroConfiguration ( ) const

Returns: a generalized position vector, sized according to the full model, whose values are all zero.

Warning: A zero vector is not necessarily a valid configuration, e.g., in case the configuration has quaternions, or position constraints, or etc.

◆ InterpolateBetweenConfigurations()

Eigen::VectorXd InterpolateBetweenConfigurations	(	const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2,
		double	ratio
	)		const

Interpolates between provided configurations q1 and q2.

Parameters

ratio Interpolation ratio.

Returns: Interpolated configuration.

Exceptions

std::exception if ratio is not in range [0, 1].

See also: ConfigurationInterpolationFunction for more.

◆ IsCollisionFilteredBetween() [1/2]

bool IsCollisionFilteredBetween	(	multibody::BodyIndex	bodyA_index,
		multibody::BodyIndex	bodyB_index
	)		const

Checks if collision is filtered between the two bodies specified.

Note: collision between two environment bodies is always filtered.

Exceptions

std::exception if either body index is out of range.

◆ IsCollisionFilteredBetween() [2/2]

bool IsCollisionFilteredBetween	(	const multibody::RigidBody< double > &	bodyA,
		const multibody::RigidBody< double > &	bodyB
	)		const

Overload that uses body references.

◆ IsInitialSetup()

bool IsInitialSetup ( ) const

protected

Returns: true if called during initial setup (before AllocateContexts() is called).

◆ IsPartOfRobot() [1/2]

bool IsPartOfRobot ( const multibody::RigidBody< double > & body ) const

Returns: true if the indicated body is part of the robot.

◆ IsPartOfRobot() [2/2]

bool IsPartOfRobot ( multibody::BodyIndex body_index ) const

Returns: true if the indicated body is part of the robot.

◆ MakeStandaloneConfigurationDistanceFunction()

ConfigurationDistanceFunction MakeStandaloneConfigurationDistanceFunction ( ) const

Returns: a functor that captures this object, so it can be used like a free function. The returned functor is only valid during the lifetime of this object. The math of the function is equivalent to ComputeConfigurationDistance().

Warning: do not pass this standalone function back into SetConfigurationDistanceFunction() function; doing so would create an infinite loop.

◆ MakeStandaloneConfigurationInterpolationFunction()

ConfigurationInterpolationFunction MakeStandaloneConfigurationInterpolationFunction ( ) const

Returns: a functor that captures this object, so it can be used like a free function. The returned functor is only valid during the lifetime of this object. The math of the function is equivalent to InterpolateBetweenConfigurations().

Warning: do not pass this standalone function back into our SetConfigurationInterpolationFunction() function; doing so would create an infinite loop.

◆ MakeStandaloneModelContext()

std::shared_ptr<CollisionCheckerContext> MakeStandaloneModelContext ( ) const

Make and track a CollisionCheckerContext.

The returned context will participate in PerformOperationAgainstAllModelContexts() until it is destroyed.

◆ MaxContextNumDistances()

int MaxContextNumDistances ( const CollisionCheckerContext & model_context ) const

Explicit Context-based version of MaxNumDistances().

See also: Explicit Context Parallelism.

◆ MaxNumDistances()

int MaxNumDistances ( std::optional< int > context_number = std::nullopt ) const

Returns an upper bound on the number of distances returned by CalcRobotClearance(), using the current thread's associated context.

Parameters

context_number Optional implicit context number.

See also: Implicit Context Parallelism.

◆ MaybeGetUniformRobotEnvironmentPadding()

std::optional<double> MaybeGetUniformRobotEnvironmentPadding ( ) const

If the padding between all robot bodies and environment bodies is the same, returns the common padding value.

Returns nullopt otherwise.

◆ MaybeGetUniformRobotRobotPadding()

std::optional<double> MaybeGetUniformRobotRobotPadding ( ) const

If the padding between all pairs of robot bodies is the same, returns the common padding value.

Returns nullopt otherwise.

◆ MeasureContextEdgeCollisionFree()

EdgeMeasure MeasureContextEdgeCollisionFree	(	CollisionCheckerContext *	model_context,
		const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2
	)		const

Explicit Context-based version of MeasureEdgeCollisionFree().

Exceptions

std::exception if model_context is nullptr.

See also: Explicit Context Parallelism.

◆ MeasureEdgeCollisionFree()

EdgeMeasure MeasureEdgeCollisionFree	(	const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2,
		std::optional< int >	context_number = `std::nullopt`
	)		const

Checks a single configuration-to-configuration edge for collision, using the current thread's associated context.

Parameters

q1	Start configuration for edge.
q2	End configuration for edge.
context_number	Optional implicit context number.

Returns: A measure of how much of the edge is collision free.

See also: Implicit Context Parallelism.

◆ MeasureEdgeCollisionFreeParallel()

EdgeMeasure MeasureEdgeCollisionFreeParallel	(	const Eigen::VectorXd &	q1,
		const Eigen::VectorXd &	q2,
		Parallelism	parallelize = `Parallelism::Max()`
	)		const

Checks a single configuration-to-configuration edge for collision.

Collision check is parallelized via OpenMP when supported. See function-level parallelism for guidance on proper usage.

Parameters

q1	Start configuration for edge.
q2	End configuration for edge.
parallelize	How much should edge collision check be parallelized?

Returns: A measure of how much of the edge is collision free.

◆ MeasureEdgesCollisionFree()

std::vector<EdgeMeasure> MeasureEdgesCollisionFree	(	const std::vector< std::pair< Eigen::VectorXd, Eigen::VectorXd >> &	edges,
		Parallelism	parallelize = `Parallelism::Max()`
	)		const

Checks multiple configuration-to-configuration edge for collision.

Collision checks are parallelized via OpenMP when supported and enabled by parallelize. See function-level parallelism for guidance on proper usage.

Parameters

edges	Edges to check, each in the form of pair<q1, q2>.
parallelize	How much should edge collision checks be parallelized?

Returns: A measure of how much of each edge is collision free. The iᵗʰ entry is the result for the iᵗʰ edge.

◆ model()

const RobotDiagram<double>& model ( ) const

Returns: a const reference to the full model.

◆ model_context()

const CollisionCheckerContext& model_context ( std::optional< int > context_number = std::nullopt ) const

Accesses a collision checking context from within the implicit context pool owned by this collision checker.

Parameters

context_number Optional implicit context number.

Returns: a const reference to either the collision checking context given by the context_number, or when nullopt the context to be used with the current OpenMP thread.

See also: Implicit Context Parallelism.

◆ num_allocated_contexts()

int num_allocated_contexts ( ) const

Returns: the number of internal (not standalone) per-thread contexts.

◆ operator=() [1/2]

CollisionChecker& operator= ( const CollisionChecker & )

delete

◆ operator=() [2/2]

CollisionChecker& operator= ( CollisionChecker && )

delete

◆ PerformOperationAgainstAllModelContexts()

void PerformOperationAgainstAllModelContexts ( const std::function< void(const RobotDiagram< double > &, CollisionCheckerContext *)> & operation )

Allows externally-provided operations that must be performed against all contexts in the per-thread context pool, and any standalone contexts made with MakeStandaloneModelContext().

For any standalone contexts, note that it is illegal to mutate a context from two different threads. No other threads should be mutating any of our standalone contexts when this function is called.

◆ plant()

const multibody::MultibodyPlant<double>& plant ( ) const

Returns: a `const reference to the full model's plant.

◆ plant_context()

const systems::Context<double>& plant_context ( std::optional< int > context_number = std::nullopt ) const

Accesses a multibody plant sub-context context from within the implicit context pool owned by this collision checker.

Parameters

context_number Optional implicit context number.

Returns: a const reference to the multibody plant sub-context within the context given by the context_number, or when nullopt the context to be used with the current OpenMP thread.

See also: Implicit Context Parallelism.

◆ RemoveAddedGeometries()

virtual void RemoveAddedGeometries ( const std::vector< AddedShape > & shapes )

protectedpure virtual

Removes all of the given added shapes (if they exist) from the checker.

Implemented in UnimplementedCollisionChecker.

◆ RemoveAllAddedCollisionShapes() [1/2]

void RemoveAllAddedCollisionShapes ( const std::string & group_name )

Removes all added checker geometries which belong to the named group.

◆ RemoveAllAddedCollisionShapes() [2/2]

void RemoveAllAddedCollisionShapes ( )

Removes all added checker geometries from all geometry groups.

◆ robot_model_instances()

const std::vector<multibody::ModelInstanceIndex>& robot_model_instances ( ) const

Gets the set of model instances belonging to the robot.

The returned vector has no duplicates and is in sorted order.

◆ set_edge_step_size()

void set_edge_step_size ( double edge_step_size )

Sets the edge step size to edge_step_size.

Exceptions

std::exception if edge_step_size is not positive.

◆ SetCollisionFilteredBetween() [1/2]

void SetCollisionFilteredBetween	(	multibody::BodyIndex	bodyA_index,
		multibody::BodyIndex	bodyB_index,
		bool	filter_collision
	)

Declares the body pair (bodyA, bodyB) to be filtered (or not) based on filter_collision.

Parameters

filter_collision Sets the to body pair to be filtered if true.

Exceptions

std::exception	if either body index is out of range.
std::exception	if both indices refer to the same body.
std::exception	if both indices refer to environment bodies.

◆ SetCollisionFilteredBetween() [2/2]

void SetCollisionFilteredBetween	(	const multibody::RigidBody< double > &	bodyA,
		const multibody::RigidBody< double > &	bodyB,
		bool	filter_collision
	)

Overload that uses body references.

◆ SetCollisionFilteredWithAllBodies() [1/2]

void SetCollisionFilteredWithAllBodies ( multibody::BodyIndex body_index )

Declares that body pair (B, O) is filtered (for all bodies O in this checker's plant).

Exceptions

std::exception	if `body_index` is out of range.
std::exception	if `body_index` refers to an environment body.

◆ SetCollisionFilteredWithAllBodies() [2/2]

void SetCollisionFilteredWithAllBodies ( const multibody::RigidBody< double > & body )

Overload that uses body references.

◆ SetCollisionFilterMatrix()

void SetCollisionFilterMatrix ( const Eigen::MatrixXi & filter_matrix )

Sets the "active" collision filter matrix.

Parameters

filter_matrix must meet the above conditions to be a "consistent" collision filter matrix.

Exceptions

std::exception if the given matrix is incompatible with this collision checker, or if it is inconsistent.

◆ SetConfigurationDistanceFunction()

void SetConfigurationDistanceFunction ( const ConfigurationDistanceFunction & distance_function )

Sets the configuration distance function to distance_function.

Precondition: distance_function satisfies the requirements documented on ConfigurationDistanceFunction and a DistanceAndInterpolationProvider is not already in use.; the collision checker was created with separate distance and interpolation functions, not a combined DistanceAndInterpolationProvider.

Note: the distance_function object will be copied and retained by this collision checker, so if the function has any lambda-captured data then that data must outlive this collision checker.

◆ SetConfigurationInterpolationFunction()

void SetConfigurationInterpolationFunction ( const ConfigurationInterpolationFunction & interpolation_function )

Sets the configuration interpolation function to interpolation_function.

Parameters

interpolation_function a functor, or nullptr. If nullptr, the default function will be configured and used.

Precondition: interpolation_function satisfies the requirements documented on ConfigurationInterpolationFunction, or is nullptr and a DistanceAndInterpolationProvider is not already in use.; the collision checker was created with separate distance and interpolation functions, not a combined DistanceAndInterpolationProvider.

Note: the interpolation_function object will be copied and retained by this collision checker, so if the function has any lambda-captured data then that data must outlive this collision checker.; the default function uses linear interpolation for most variables, and uses slerp for quaternion valued variables.

◆ SetDistanceAndInterpolationProvider()

void SetDistanceAndInterpolationProvider ( std::shared_ptr< const DistanceAndInterpolationProvider > provider )

Sets the distance and interpolation provider to use.

Note that in case any of the (to-be-deprecated) separate distance and interpolation functions were in use, this supplants both of them.

Precondition: provider satisfies the requirements documents on DistanceAndInterpolationProvider.

◆ SetPaddingAllRobotEnvironmentPairs()

void SetPaddingAllRobotEnvironmentPairs ( double padding )

Sets the padding for all (robot, environment) pairs.

Exceptions

std::exception if the configuration prerequisites are not met.

◆ SetPaddingAllRobotRobotPairs()

void SetPaddingAllRobotRobotPairs ( double padding )

Sets the padding for all (robot, robot) pairs.

Exceptions

std::exception if the configuration prerequisites are not met.

◆ SetPaddingBetween() [1/2]

void SetPaddingBetween	(	multibody::BodyIndex	bodyA_index,
		multibody::BodyIndex	bodyB_index,
		double	padding
	)

Sets the padding value for the pair of bodies specified.

Exceptions

std::exception if the configuration prerequisites are not met or bodyA_index == bodyB_index.

◆ SetPaddingBetween() [2/2]

void SetPaddingBetween	(	const multibody::RigidBody< double > &	bodyA,
		const multibody::RigidBody< double > &	bodyB,
		double	padding
	)

Overload that uses body references.

◆ SetPaddingMatrix()

void SetPaddingMatrix ( const Eigen::MatrixXd & collision_padding )

Sets the collision padding matrix.

Note that this matrix contains all padding data, both robot-robot "self" padding, and robot-environment padding. collision_padding must have the following properties to be considered valid.

It is a square NxN matrix (where N is the total number of bodies).
Diagonal values are all zero.
Entries involving only environment bodies are all zero.
It is symmetric.
All values are finite.

Exceptions

std::exception if collision_padding doesn't have the enumerated properties.

◆ SetPaddingOneRobotBodyAllEnvironmentPairs()

void SetPaddingOneRobotBodyAllEnvironmentPairs	(	multibody::BodyIndex	body_index,
		double	padding
	)

Sets the environment collision padding for the provided robot body with respect to all environment bodies.

Exceptions

std::exception if the configuration prerequisites are not met.

◆ SupportsParallelChecking()

bool SupportsParallelChecking ( ) const

Does the collision checker support true parallel collision checks?

Returns: true if parallel checking is supported.

◆ UpdateCollisionFilters()

virtual void UpdateCollisionFilters ( )

protectedpure virtual

Derived collision checkers can do further work in this function in response to changes in collision filters.

This is called after any changes are made to the collision filter matrix.

Implemented in UnimplementedCollisionChecker.

◆ UpdateContextPositions()

const systems::Context<double>& UpdateContextPositions	(	CollisionCheckerContext *	model_context,
		const Eigen::VectorXd &	q
	)		const

Explicit Context-based version of UpdatePositions().

Exceptions

std::exception if model_context is nullptr.

See also: Explicit Context Parallelism.

◆ UpdatePositions()

const systems::Context<double>& UpdatePositions	(	const Eigen::VectorXd &	q,
		std::optional< int >	context_number = `std::nullopt`
	)		const

Updates the generalized positions q in the implicit context specified and returns a reference to the MultibodyPlant's now-updated context.

The implicit context is either that specified by context_number, or when nullopt the context to be used with the current OpenMP thread.

Parameters

context_number Optional implicit context number.

See also: Implicit Context Parallelism.

The documentation for this class was generated from the following file:

drake/planning/collision_checker.h

Detailed Description

Implicit Context Parallelism

Explicit Context Parallelism

Mixing Threading Models

Implementing Derived Classes

Classes

Public Member Functions

Protected Member Functions

Constructor & Destructor Documentation

◆ CollisionChecker() [1/3]

◆ ~CollisionChecker()

◆ CollisionChecker() [2/3]

◆ CollisionChecker() [3/3]

Member Function Documentation

◆ AddCollisionShape()

◆ AddCollisionShapes() [1/2]

◆ AddCollisionShapes() [2/2]

◆ AddCollisionShapeToBody()

◆ AddCollisionShapeToFrame()

◆ AllocateContexts()

◆ CalcContextRobotClearance()

◆ CalcRobotClearance()

◆ CanEvaluateInParallel()

◆ CheckConfigCollisionFree()

◆ CheckConfigsCollisionFree()

◆ CheckContextConfigCollisionFree()

◆ CheckContextEdgeCollisionFree()

◆ CheckEdgeCollisionFree()

◆ CheckEdgeCollisionFreeParallel()

◆ CheckEdgesCollisionFree()

◆ ClassifyBodyCollisions()

◆ ClassifyContextBodyCollisions()

◆ Clone()

◆ ComputeConfigurationDistance()

◆ CreatePrototypeContext()

◆ CriticizePaddingMatrix()

◆ distance_and_interpolation_provider()

◆ DoAddCollisionShapeToBody()

◆ DoCalcContextRobotClearance()

◆ DoCheckContextConfigCollisionFree()

◆ DoClassifyContextBodyCollisions()

◆ DoClone()

◆ DoMaxContextNumDistances()

◆ DoUpdateContextPositions()

◆ edge_step_size()

◆ get_body()

◆ GetAllAddedCollisionShapes()

◆ GetFilteredCollisionMatrix()

◆ GetLargestPadding()

◆ GetMutableSetupModel()

◆ GetNominalFilteredCollisionMatrix()

◆ GetPaddingBetween() [1/2]

◆ GetPaddingBetween() [2/2]

◆ GetPaddingMatrix()

◆ GetZeroConfiguration()

◆ InterpolateBetweenConfigurations()

◆ IsCollisionFilteredBetween() [1/2]

◆ IsCollisionFilteredBetween() [2/2]

◆ IsInitialSetup()

◆ IsPartOfRobot() [1/2]

◆ IsPartOfRobot() [2/2]

◆ MakeStandaloneConfigurationDistanceFunction()

◆ MakeStandaloneConfigurationInterpolationFunction()

◆ MakeStandaloneModelContext()

◆ MaxContextNumDistances()

◆ MaxNumDistances()

◆ MaybeGetUniformRobotEnvironmentPadding()

◆ MaybeGetUniformRobotRobotPadding()

◆ MeasureContextEdgeCollisionFree()

◆ MeasureEdgeCollisionFree()

◆ MeasureEdgeCollisionFreeParallel()

◆ MeasureEdgesCollisionFree()

◆ model()

◆ model_context()

◆ num_allocated_contexts()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ PerformOperationAgainstAllModelContexts()

◆ plant()

◆ plant_context()