Detailed Description

template<typename T>
class drake::geometry::QueryObject< T >

The QueryObject serves as a mechanism to perform geometry queries on the world's geometry.

The SceneGraph has an abstract-valued port that contains a QueryObject (i.e., a QueryObject-valued output port).

To perform geometry queries on SceneGraph:

a LeafSystem must have a QueryObject-valued input port and connect it to the corresponding query output port on SceneGraph,
the querying LeafSystem can evaluate the input port, retrieving a const QueryObject& in return, and, finally,
invoke the appropriate method on the QueryObject.

The const reference returned by the input port is considered "live" - it is linked to the context, system, and cache (making full use of all of those mechanisms). This const reference should never be persisted; doing so can lead to erroneous query results. It is simpler and more advisable to acquire it for evaluation in a limited scope (e.g., CalcTimeDerivatives()) and then discard it. If a QueryObject is needed for many separate functions in a LeafSystem, each should re-evaluate the input port. The underlying caching mechanism should make the cost of this negligible.

The QueryObject can be copied. The copied instance is no longer "live"; it is now "baked". Essentially, it freezes the state of the live scene graph in its current configuration and disconnects it from the system and context. This means, even if the original context changes values, the copied/baked instance will always reproduce the same query results. This baking process is not cheap and should not be done without consideration.

Queries and scalar type

A QueryObject cannot be converted to a different scalar type. A QueryObject of scalar type T can only be acquired from the output port of a SceneGraph of type T evaluated on a corresponding Context, also of type T.

QueryObject's support for arbitrary scalar type is incomplete. Not all queries support all scalar types to the same degree. Furthermore, the queries are typically served by families of algorithms. The evaluation of a query between a particular pair of geometries will depend on the query, the pair of geometry types involved, and the scalar type. From query to query, the treatment of a geometry (or geometry pair) for a given scalar type can vary in many ways, including but not limited to: ignoring the geometry, throwing an exception, results with limited precision, or full, accurate support. The queries below provide tables to help inform your expectations when evaluating queries. The tables all use a common methodology and admit a common interpretation.

For each (query, geometry-pair, scalar) combination, we create a set of geometric configurations with known answers. We evaluate the precision of the query result (if supported at all) over the entire set and report the worst observed error. This is a purely empirical approach and doesn't fully characterize the families of underlying algorithms, and the reported error may be misleading in that we've missed far worse latent error or that the error reported doesn't well represent the average case.

The families of algorithms also differ in how their inherent errors scale with the scale of the problem (e.g., size of geometries, magnitude of distance/depth, etc.) Attempting to fully characterize that aspect is both arduous and problematic, so, we've chosen a more representative approach.

Because Drake is primarily intended for robot simulation, we've created geometric configurations on the scale of common robot manipulators (on the order of 20cm). We position them with a known penetration depth (or separating distance) of 2 mm. The error reported is the deviation from that expected result.

When interpreting the tables, keep the following in mind:

The table illustrates trends in broad strokes, only. It does not represent an exhaustive analysis.
If your problem involves larger geometries, greater penetration depths, or larger separating distances, the error will vary. Do not assume that observed error in this context is necessarily relative – there truly is that much variability in the families of algorithms.
These values are an attempt to capture the worst case outcome. The error shown is a single-significant-digit approximation of that observed error.
These values may not actually represent the true worst case; discovering the true worst case is generally challenging. These represent our best effort to date. If you find outcomes that are worse those reported here, please submit a bug.
These tables represent Drake's transient state. The eventual goal is to report no more than 1e-14 error across all supportable geometry pairs and scalars. At that point, the table will simply disappear.

Template Parameters

T	The scalar type, which must be one of the default scalars.

#include <drake/geometry/query_object.h>

Public Member Functions
	QueryObject ()=default
	Constructs a default QueryObject (all pointers are null). More...

const SceneGraphInspector< T > &	inspector () const
	Provides an inspector for the topological structure of the underlying scene graph data (see SceneGraphInspector for details). More...

Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
Calling the copy constructor or assignment will turn a live QueryObject into a baked QueryObject (an expensive operation). Copying baked QueryObjects is cheap.
	QueryObject (const QueryObject &other)

QueryObject &	operator= (const QueryObject &)

	QueryObject (QueryObject &&)=default

QueryObject &	operator= (QueryObject &&)=default

Configuration-dependent Introspection
These methods provide access to introspect geometry and frame quantities that directly depend on the poses of the frames or configurations of deformable geometries. For geometry and frame quantities that do not depend on the poses of frames, such as X_FG, use inspector() to access the SceneGraphInspector.
const math::RigidTransform< T > &	GetPoseInWorld (FrameId frame_id) const
	Reports the position of the frame indicated by `frame_id` relative to the world frame. More...

const math::RigidTransform< T > &	GetPoseInParent (FrameId frame_id) const
	Reports the position of the frame indicated by `frame_id` relative to its parent frame. More...

const math::RigidTransform< T > &	GetPoseInWorld (GeometryId geometry_id) const
	Reports the position of the frame of the rigid geometry indicated by `geometry_id` relative to the world frame (X_WG). More...

const VectorX< T > &	GetConfigurationsInWorld (GeometryId deformable_geometry_id) const
	Reports the configuration of the deformable geometry indicated by `deformable_geometry_id` relative to the world frame. More...

std::vector< VectorX< T > >	GetDrivenMeshConfigurationsInWorld (GeometryId deformable_geometry_id, Role role) const
	Reports the configurations of the driven meshes associated with the given role for the deformable geometry indicated by `deformable_geometry_id` relative to the world frame if the deformable geometry has that role. More...

Collision Queries
These queries detect collisions between geometry. Two geometries collide if they overlap each other and are not explicitly excluded through collision filtering. These algorithms find those colliding cases, characterize them, and report the essential characteristics of that collision. For two colliding geometries g_A and g_B, it is guaranteed that they will map to `id_A` and `id_B` in a fixed, repeatable manner, where `id_A` and `id_B` are GeometryId's of geometries g_A and g_B respectively. These methods are affected by collision filtering; element pairs that have been filtered will not produce contacts, even if their collision geometry is penetrating.
std::vector< PenetrationAsPointPair< T > >	ComputePointPairPenetration () const
	Computes the penetrations across all pairs of geometries in the world with the penetrations characterized by pairs of points (see PenetrationAsPointPair), providing some measure of the penetration "depth" of the two objects, but not the overlapping volume. More...

template<typename T1 = T>
std::enable_if_t< scalar_predicate< T1 >::is_bool, std::vector< ContactSurface< T > > >	ComputeContactSurfaces (HydroelasticContactRepresentation representation) const
	Reports pairwise intersections and characterizes each non-empty intersection as a ContactSurface for hydroelastic contact model. More...

template<typename T1 = T>
std::enable_if_t< scalar_predicate< T1 >::is_bool, void >	ComputeContactSurfacesWithFallback (HydroelasticContactRepresentation representation, std::vector< ContactSurface< T >> surfaces, std::vector< PenetrationAsPointPair< T >> point_pairs) const
	Reports pairwise intersections and characterizes each non-empty intersection as a ContactSurface where possible and as a PenetrationAsPointPair where not. More...

template<typename T1 = T>
std::enable_if_t< std::is_same_v< T1, double >, void >	ComputeDeformableContact (internal::DeformableContact< T > *deformable_contact) const
	Reports contact information among all deformable geometries. More...

std::vector< SortedPair< GeometryId > >	FindCollisionCandidates () const
	Applies a conservative culling mechanism to create a subset of all possible geometry pairs based on non-zero intersections. More...

bool	HasCollisions () const
	Reports true if there are any collisions between unfiltered pairs in the world. More...

Signed Distance Queries
These queries provide the signed distance between two objects. Each query has a specific definition of the signed distance being positive, negative, or zero associated with some notions of being outside, inside, or on the boundary. These queries provide bookkeeping data like geometry id(s) of the geometries involved and the important locations on the boundaries of these geometries. The signed distance function is a continuous function. Its partial derivatives are continuous almost everywhere.
std::vector< SignedDistancePair< T > >	ComputeSignedDistancePairwiseClosestPoints (const double max_distance=std::numeric_limits< double >::infinity()) const
	Computes the signed distance together with the nearest points across all pairs of geometries in the world. More...

SignedDistancePair< T >	ComputeSignedDistancePairClosestPoints (GeometryId geometry_id_A, GeometryId geometry_id_B) const
	A variant of ComputeSignedDistancePairwiseClosestPoints() which computes the signed distance (and witnesses) between a specific pair of geometries indicated by id. More...

std::vector< SignedDistanceToPoint< T > >	ComputeSignedDistanceToPoint (const Vector3< T > &p_WQ, const double threshold=std::numeric_limits< double >::infinity()) const
	Computes the signed distances and gradients to a query point from each geometry in the scene. More...

Render Queries
The methods support queries along the lines of "What do I see?" They support simulation of sensors. External entities define a sensor camera – its extrinsic and intrinsic properties and QueryObject renders into the provided image.
void	RenderColorImage (const render::ColorRenderCamera &camera, FrameId parent_frame, const math::RigidTransformd &X_PC, systems::sensors::ImageRgba8U *color_image_out) const
	Renders an RGB image for the given `camera` posed with respect to the indicated parent frame P. More...

void	RenderDepthImage (const render::DepthRenderCamera &camera, FrameId parent_frame, const math::RigidTransformd &X_PC, systems::sensors::ImageDepth32F *depth_image_out) const
	Renders a depth image for the given `camera` posed with respect to the indicated parent frame P. More...

void	RenderLabelImage (const render::ColorRenderCamera &camera, FrameId parent_frame, const math::RigidTransformd &X_PC, systems::sensors::ImageLabel16I *label_image_out) const
	Renders a label image for the given `camera` posed with respect to the indicated parent frame P. More...

const render::RenderEngine *	GetRenderEngineByName (const std::string &name) const
	Returns the named render engine, if it exists. More...

Friends
class	SceneGraph< T >

class	QueryObjectTest

Constructor & Destructor Documentation

◆ QueryObject() [1/3]

QueryObject ( )

default

Constructs a default QueryObject (all pointers are null).

◆ QueryObject() [2/3]

QueryObject ( const QueryObject< T > & other )

◆ QueryObject() [3/3]

QueryObject ( QueryObject< T > && )

default

Member Function Documentation

◆ ComputeContactSurfaces()

std::enable_if_t<scalar_predicate<T1>::is_bool, std::vector<ContactSurface<T> > > ComputeContactSurfaces ( HydroelasticContactRepresentation representation ) const

Reports pairwise intersections and characterizes each non-empty intersection as a ContactSurface for hydroelastic contact model.

The computation is subject to collision filtering.

For two intersecting geometries g_A and g_B, it is guaranteed that they will map to id_A and id_B in a fixed, repeatable manner, where id_A and id_B are GeometryId's of geometries g_A and g_B respectively.

In the current incarnation, this function represents an incomplete implementation. That has several implications, as described below:

This table shows which shapes can be declared for use in hydroelastic contact, and what compliance can be assigned.

Shape	Compliant	Rigid
Sphere	yes	yes
Cylinder	yes	yes
Box	yes	yes
Capsule	yes	yes
Ellipsoid	yes	yes
HalfSpace	yes	yes
Mesh	yesᵃ	yesᵇ
Convex	yesᶜ	yesᶜ

ᵃ The exact representation of a compliant mesh depends on the type of mesh file it references:
- .obj: the convex hull of the mesh will be used (as if it were declared to be a Convex shape).
- .vtk: the tetrahedral mesh will be used directly. This external working document provides guidance how to generate a tetrahedral mesh in a VTK file from a surface mesh in an OBJ file.
ᵇ For rigid Mesh, please specify a surface mesh in an OBJ file in Mesh(filename). A tetrahedral mesh in a VTK file can also be specified.
ᶜ The Convex shape can reference either an .obj or a .vtk tetrahedral mesh. In both cases, its convex hull will be used to define the hydroelastic representation.
We do not support contact between two rigid geometries. One geometry must* be compliant, and the other could be rigid or compliant. If two rigid geometries collide, an exception will be thrown. More particularly, if such a geometry pair cannot be culled an exception will be thrown. No exception is thrown if the pair has been filtered.
If you need all combinations of rigid-rigid contact, rigid-compliant contact, and compliant-compliant contact, you might consider ComputeContactSurfacesWithFallback().
The hydroelastic modulus (N/m^2) of each compliant geometry is set in ProximityProperties by AddCompliantHydroelasticProperties().
The tessellation of the corresponding meshes is controlled by the resolution hint (where appropriate), as defined by AddCompliantHydroelasticProperties() and AddRigidHydroelasticProperties().

Scalar support

This method provides support for both double and AutoDiffXd, but not Expression. Like with the other proximity queries, derivatives can only be introduced via geometry poses. We cannot differentiate w.r.t. geometric properties (e.g., radius, length, etc.)

Parameters

representation Controls the mesh representation of the contact surface. See contact surface representation for more details.

Returns: A vector populated with all detected intersections characterized as contact surfaces. The ordering of the results is guaranteed to be consistent – for fixed geometry poses, the results will remain the same.

◆ ComputeContactSurfacesWithFallback()

std::enable_if_t<scalar_predicate<T1>::is_bool, void> ComputeContactSurfacesWithFallback	(	HydroelasticContactRepresentation	representation,
		std::vector< ContactSurface< T >> *	surfaces,
		std::vector< PenetrationAsPointPair< T >> *	point_pairs
	)		const

Reports pairwise intersections and characterizes each non-empty intersection as a ContactSurface where possible and as a PenetrationAsPointPair where not.

This method can be thought of as a combination of ComputeContactSurfaces() and ComputePointPairPenetration(). For each geometry pair, we attempt to compute a ContactSurface. If that fails, rather than throwing, we attempt to characterize the contact as a point pair. If that fails, we throw. See the documentation of those constituent methods to understand the circumstances in which they fail.

The ordering of the added results is guaranteed to be consistent – for fixed geometry poses, the results will remain the same.

Scalar support

The scalar support is a combination of the scalar support offered by ComputeContactSurfaces() and ComputePointPairPenetration(). This method supports double and AutoDiffXd to the extent that those constituent methods do, but does not support Expression.

Parameters

	representation	Controls the mesh representation of the contact surface. See contact surface representation for more details.
[out]	surfaces	The vector that contact surfaces will be added to. The vector will not be cleared.
[out]	point_pairs	The vector that fall back point pair data will be added to. The vector will not be cleared.

Precondition: Neither surfaces nor point_pairs is nullptr.

Exceptions

std::exception for the reasons described in ComputeContactSurfaces() and ComputePointPairPenetration().

Note: The surfaces and point_pairs are output pointers in C++, but are return values in the Python bindings.

◆ ComputeDeformableContact()

std::enable_if_t<std::is_same_v<T1, double>, void> ComputeDeformableContact ( internal::DeformableContact< T > * deformable_contact ) const

Reports contact information among all deformable geometries.

It includes contacts between two deformable geometries or contacts between a deformable geometry and a non-deformable geometry. This function only supports double as the scalar type.

Parameters

[out] deformable_contact Contains all deformable contact data on output. Any data passed in is cleared before the computation.

Precondition: deformable_contact != nullptr.

Warning: This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

◆ ComputePointPairPenetration()

std::vector<PenetrationAsPointPair<T> > ComputePointPairPenetration ( ) const

Computes the penetrations across all pairs of geometries in the world with the penetrations characterized by pairs of points (see PenetrationAsPointPair), providing some measure of the penetration "depth" of the two objects, but not the overlapping volume.

Only reports results for penetrating geometries; if two geometries are separated, there will be no result for that pair. Geometries whose surfaces are just touching (osculating) are not considered in penetration. Surfaces whose penetration is within an epsilon of osculation, are likewise not considered penetrating. Pairs of anchored geometry are also not reported. This method is affected by collision filtering.

For two penetrating geometries g_A and g_B, it is guaranteed that they will map to id_A and id_B in a fixed, repeatable manner.

Characterizing the returned values

As discussed in the class's documentation, these tables document the support given by this query for pairs of geometry types and scalar. See the description in the link for details on how to interpret the tables' results. The query is symmetric with respect to shape ordering, the pair (ShapeA, ShapeB) will be the same as (ShapeB, ShapeA), so we only fill in half of each table.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	2e-15	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	3e-5ᶜ	2e-5ᶜ	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	2e-15ᶜ	3e-5ᶜ	2e-15ᶜ	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	1e-3ᶜ	4e-5ᶜ	1e-3ᶜ	2e-3ᶜ	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	4e-4ᶜ	2e-4ᶜ	4e-4ᶜ	2e-3ᶜ	5e-4ᶜ	░░░░░░	░░░░░	░░░░░
HalfSpace	6e-15	4e-15	3e-15ᶜ	4e-15	3e-15	throwsᵃ	░░░░░	░░░░░
Mesh	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	░░░░░
Sphere	3e-15	5e-15	3e-5ᶜ	5e-15	2e-4ᶜ	3e-15	ᵇ	5e-15

Table 1: Worst observed error (in m) for 2mm penetration between geometries approximately 20cm in size for T = double.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	throwsᵈ	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	throwsᵈ	throwsᵈ	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	throwsᵈ	throwsᵈ	throwsᵈ	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	░░░░░░	░░░░░	░░░░░
HalfSpace	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵃ	░░░░░	░░░░░
Mesh	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	░░░░░
Sphere	2e-15	3e-15	throwsᵈ	2e-15	throwsᵈ	2e-15	ᵇ	5e-15

Table 2: Worst observed error (in m) for 2mm penetration between geometries approximately 20cm in size for T = AutoDiffXd.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	throwsᵉ	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	throwsᵉ	throwsᵉ	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	throwsᵉ	throwsᵉ	throwsᵉ	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	░░░░░░	░░░░░	░░░░░
HalfSpace	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵃ	░░░░░	░░░░░
Mesh	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	ᵇ	░░░░░
Sphere	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	throwsᵉ	ᵇ	throwsᵉ

Table 3: Support for T = drake::symbolic::Expression.

ᵃ Penetration depth between two HalfSpace instances has no meaning; either they don't intersect, or they have infinite penetration.
ᵇ Meshes are represented by the convex hull of the mesh, therefore the results for Mesh are assumed to be the same as for Convex.
ᶜ These results are computed using an iterative algorithm. For particular configurations, the solution may be correct to machine precision. The values reported here are confirmed, observed worst case answers.
ᵈ These results are simply not supported for T = AutoDiffXd at this time.
ᵉ These results are simply not supported for T = drake::symbolic::Expression at this time.

Returns: A vector populated with all detected penetrations characterized as point pairs. The ordering of the results is guaranteed to be consistent – for fixed geometry poses, the results will remain the same.

Warning: For Mesh shapes, their convex hulls are used in this query. It is not* computationally efficient or particularly accurate.

Exceptions

std::exception if a Shape-Shape pair is in collision and indicated as throws in the support table above.

◆ ComputeSignedDistancePairClosestPoints()

SignedDistancePair<T> ComputeSignedDistancePairClosestPoints	(	GeometryId	geometry_id_A,
		GeometryId	geometry_id_B
	)		const

A variant of ComputeSignedDistancePairwiseClosestPoints() which computes the signed distance (and witnesses) between a specific pair of geometries indicated by id.

This function has the same restrictions on scalar report as ComputeSignedDistancePairwiseClosestPoints().

Note: This query is unique among the distance queries in that it doesn't respect collision filters. As long as the geometry ids refer to geometries with proximity roles, signed distance can be computed (subject to supported scalar tables above).

Characterizing the returned values

This method merely exercises the same mechanisms as ComputeSignedDistancePairwiseClosestPoints() for evaluating signed distance. Refer to the table for ComputeSignedDistancePairwiseClosestPoints() for details.

Exceptions

std::exception if either geometry id is invalid (e.g., doesn't refer to an existing geometry, lacking proximity role, etc.), the pair is unsupported as indicated by the scalar support table.

Warning: For Mesh shapes, their convex hulls are used in this query. It is not computationally efficient or particularly accurate.

◆ ComputeSignedDistancePairwiseClosestPoints()

std::vector<SignedDistancePair<T> > ComputeSignedDistancePairwiseClosestPoints ( const double max_distance = std::numeric_limits< double >::infinity() ) const

Computes the signed distance together with the nearest points across all pairs of geometries in the world.

Reports both the separating geometries and penetrating geometries.

This query provides φ(A, B), the signed distance between two objects A and B.

If the objects do not overlap (i.e., A ⋂ B = ∅), φ > 0 and represents the minimal distance between the two objects. More formally: φ = min(|Aₚ - Bₚ|) ∀ Aₚ ∈ A and Bₚ ∈ B.

Note: The pair (Aₚ, Bₚ) is a "witness" of the distance. The pair need not be unique (think of two parallel planes).

If the objects touch or overlap (i.e., A ⋂ B ≠ ∅), φ ≤ 0 and can be interpreted as the negative penetration depth. It is the smallest length of the vector v, such that by shifting one object along that vector relative to the other, the two objects will no longer be overlapping. More formally, φ(A, B) = -min |v|. s.t (Tᵥ · A) ⋂ B = ∅ where Tᵥ is a rigid transformation that displaces A by the vector v, namely Tᵥ · A = {u + v | ∀ u ∈ A}. By implication, there exist points Aₚ and Bₚ on the surfaces of objects A and B, respectively, such that Aₚ + v = Bₚ, Aₚ ∈ A ∩ B, Bₚ ∈ A ∩ B. These points are the witnesses to the penetration.

This method is affected by collision filtering; geometry pairs that have been filtered will not produce signed distance query results.

For a geometry pair (A, B), the returned results will always be reported in a fixed order (e.g., always (A, B) and never (B, A)). The basis for the ordering is arbitrary (and therefore undocumented), but guaranteed to be fixed and repeatable.

Notice that this is an O(N²) operation, where N is the number of geometries remaining in the world after applying collision filter. We report the distance between dynamic objects, and between dynamic and anchored objects. We DO NOT report the distance between two anchored objects.

Using maximum distance

While the algorithm generally has O(N²) complexity in time and space, that can be reduced by the judicious use of the max_distance parameter. If φ(A, B) > max_distance, the pair (A, B) will not be included in the results (making it O(M²) in space where M < N). Furthermore, the broadphase culling algorithm can exploit max_distance to cheaply eliminate pairs of geometry that are "obviously" too far (likewise reducing the time complexity).

Passing max_distance = 0 is conceptually related to calling HasCollisions(). If contact is sparse (very few actually contacting geometry pairs), the two invocations will be quite similar in cost. However, the distinction between the two is that this method would have to include all pairs that satisfy φ(A, B) <= 0, whereas HasCollisions() stops at the first. So, the more actually colliding geometry pairs there are, the bigger the difference in cost between the two approaches.

Characterizing the returned values

As discussed in the class's documentation, this table documents the support given by this query for pairs of geometry types and scalar. See the description in the link for details on how to interpret the table results. The query is symmetric with respect to shape ordering, the pair (ShapeA, ShapeB) will be the same as (ShapeB, ShapeA), so we only fill in half the table.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	4e-15	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	3e-6	2e-5	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	3e-15	2e-5	3e-15	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	6e-6	1e-5	6e-6	2e-5	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	9e-6	5e-6	9e-6	5e-5	2e-5	░░░░░░	░░░░░	░░░░░
HalfSpace	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	░░░░░	░░░░░
Mesh	ᶜ	ᶜ	ᶜ	ᶜ	ᶜ	throwsᵃ	ᶜ	░░░░░
Sphere	3e-15	6e-15	3e-6	5e-15	4e-5	3e-15	ᶜ	6e-15

Table 4: Worst observed error (in m) for 2mm penetration/separation between geometries approximately 20cm in size for T = double.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	throwsᵇ	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	throwsᵇ	throwsᵇ	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	throwsᵇ	throwsᵇ	throwsᵇ	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	throwsᵇ	throwsᵇ	throwsᵇ	throwsᵇ	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	throwsᵇ	throwsᵇ	throwsᵇ	throwsᵇ	throwsᵇ	░░░░░░	░░░░░	░░░░░
HalfSpace	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	░░░░░	░░░░░
Mesh	ᶜ	ᶜ	ᶜ	ᶜ	ᶜ	throwsᵃ	ᶜ	░░░░░
Sphere	2e-15	throwsᵇ	throwsᵇ	throwsᵇ	throwsᵇ	2e-15	ᶜ	5e-15

Table 5: Worst observed error (in m) for 2mm penetration/separation between geometries approximately 20cm in size for T = AutoDiffXd.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	throwsᵈ	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	throwsᵈ	throwsᵈ	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	throwsᵈ	throwsᵈ	throwsᵈ	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	░░░░░░	░░░░░	░░░░░
HalfSpace	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	throwsᵃ	░░░░░	░░░░░
Mesh	ᶜ	ᶜ	ᶜ	ᶜ	ᶜ	throwsᵃ	ᶜ	░░░░░
Sphere	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	throwsᵈ	ᶜ	throwsᵈ

Table 6: Support for T = drake::symbolic::Expression.

ᵃ We don't currently support queries between HalfSpace and any other shape (except for Sphere).
ᵇ These results are simply not supported for T = AutoDiffXd at this time.
ᶜ Meshes are represented by the convex hull of the mesh, therefore the results for Mesh are the same as for Convex.
ᵈ These results are simply not supported for T = drake::symbolic::Expression at this time.

Characterizing the returned gradients

In most cases, the returned gradient vectors are the normalized displacement vectors between two witness points. However, when two geometries touch at zero distance, their witness points have a zero displacement vector that we cannot normalize.

When two geometries touch at zero distance, we have special implementation to choose reasonable gradients for some cases shown as "Ok" in the table below. Otherwise, they are "NaN" in the table. In general, we try to choose the gradient, when two geometries touch, in the most consistent way, but the problem sometimes doesn't have a unique solution. For example, any direction is qualified to be the gradient for two concentric spheres. Or two boxes touching at their vertices can pick a gradient from a continuous family of directions.

	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
Box	Ok	░░░░░░	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Capsule	NaN	NaN	░░░░░	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Convex	NaN	NaN	NaN	░░░░░░░	░░░░░░	░░░░░░	░░░░░	░░░░░
Cylinder	NaN	NaN	NaN	NaN	░░░░░░	░░░░░░	░░░░░	░░░░░
Ellipsoid	NaN	NaN	NaN	NaN	NaN	░░░░░░	░░░░░	░░░░░
HalfSpace	NaN	NaN	NaN	NaN	NaN	NaN	░░░░░	░░░░░
Mesh	NaN	NaN	NaN	NaN	NaN	NaN	NaN	░░░░░
Sphere	Ok	Ok	Okᵃ	Ok	Okᵃ	Ok	Okᵃ	Ok

Table 7: Support for signed-distance gradients when two geometries touch at zero distance.

ᵃ Return the gradient as a Vector3d of NaN if the sphere has zero radius.

Parameters

max_distance The maximum distance at which distance data is reported.

Returns: The signed distance (and supporting data) for all unfiltered geometry pairs whose distance is less than or equal to max_distance. The ordering of the results is guaranteed to be consistent – for fixed geometry poses, the results will remain the same.

Exceptions

std::exception as indicated in the table above.

Warning: For Mesh shapes, their convex hulls are used in this query. It is not* computationally efficient or particularly accurate.

◆ ComputeSignedDistanceToPoint()

std::vector<SignedDistanceToPoint<T> > ComputeSignedDistanceToPoint	(	const Vector3< T > &	p_WQ,
		const double	threshold = `std::numeric_limits< double >::infinity()`
	)		const

Computes the signed distances and gradients to a query point from each geometry in the scene.

This query provides φᵢ(p), φᵢ:ℝ³→ℝ, the signed distance to the position p of a query point from geometry Gᵢ in the scene. It returns an array of the signed distances from all geometries.

Optionally you can specify a threshold distance that will filter out any object beyond the threshold. By default, we report distances from the query point to every object.

This query also provides the gradient vector ∇φᵢ(p) of the signed distance function from geometry Gᵢ. Note that, in general, if p is outside Gᵢ, then ∇φᵢ(p) equals the unit vector in the direction from the nearest point Nᵢ on Gᵢ's surface to p. If p is inside Gᵢ, then ∇φᵢ(p) is in the direction from p to Nᵢ. This observation is written formally as:

∇φᵢ(p) = (p - Nᵢ)/|p - Nᵢ| if p is outside Gᵢ

∇φᵢ(p) = -(p - Nᵢ)/|p - Nᵢ| if p is inside Gᵢ

Note that ∇φᵢ(p) is also defined on Gᵢ's surface, but we cannot use the above formula.

Characterizing the returned values

This table is a variant of that described in this class's documentation. The query evaluates signed distance between one shape and a point (in contrast to other queries which involve two shapes). Therefore, we don't need a matrix of shape pairs, but a list of shapes. Otherwise, the methodology is the same as described, with the point being represented as a zero-radius sphere.

Scalar	Box	Capsule	Convex	Cylinder	Ellipsoid	HalfSpace	Mesh	Sphere
double	2e-15	4e-15	ᵃ	3e-15	3e-5ᵇ	5e-15	ᵃ	4e-15
AutoDiffXd	1e-15	4e-15	ᵃ	ᵃ	ᵃ	5e-15	ᵃ	3e-15
Expression	ᵃ	ᵃ	ᵃ	ᵃ	ᵃ	ᵃ	ᵃ	ᵃ

Table 8: Worst observed error (in m) for 2mm penetration/separation between geometry approximately 20cm in size and a point.

ᵃ Unsupported geometry/scalar combinations are simply ignored; no results are reported for that geometry.
ᵇ This uses an iterative algorithm which introduces a relatively large and variable error. For example, as the eccentricity of the ellipsoid increases, this error may get worse. It also depends on the location of the projection of the query point on the ellipsoid; the closer that point is to the high curvature area, the bigger the effect. It is not immediately clear how much worse the answer will get.

Note: For a sphere G, the signed distance function φᵢ(p) has an undefined gradient vector at the center of the sphere–every point on the sphere's surface has the same distance to the center. In this case, we will assign ∇φᵢ(p) the unit vector Gx (x-directional vector of G's frame) expressed in World frame.; For a box, at a point p on an edge or a corner of the box, the signed distance function φᵢ(p) has an undefined gradient vector. In this case, we will assign a unit vector in the direction of the average of the outward face unit normals of the incident faces of the edge or the corner. A point p is considered being on a face, or an edge, or a corner of the box if it lies within a certain tolerance from them.; For a box B, if a point p is inside the box, and it is equidistant to multiple nearest faces, the signed distance function φᵢ(p) at p will have an undefined gradient vector. There is a nearest point candidate associated with each nearest face. In this case, we arbitrarily pick the point Nᵢ associated with one of the nearest faces. Please note that, due to the possible round off error arising from applying a pose X_WG to B, there is no guarantee which of the nearest faces will be used.; The signed distance function is a continuous function with respect to the position of the query point, but its gradient vector field may not be continuous. Specifically at a position equidistant to multiple nearest points, its gradient vector field is not continuous.; For a convex object, outside the object at positive distance from the boundary, the signed distance function is smooth (having continuous first-order partial derivatives).

Parameters

[in]	p_WQ	Position of a query point Q in world frame W.
[in]	threshold	We ignore any object beyond this distance. By default, it is infinity, so we report distances from the query point to every object.

Return values

signed_distances A vector populated with per-object signed distance values (and supporting data) for every supported geometry as shown in the table. See SignedDistanceToPoint. The ordering of the results is guaranteed to be consistent – for fixed geometry poses, the results will remain the same.

◆ FindCollisionCandidates()

std::vector<SortedPair<GeometryId> > FindCollisionCandidates ( ) const

Applies a conservative culling mechanism to create a subset of all possible geometry pairs based on non-zero intersections.

A geometry pair that is absent from the results is either a) culled by collision filters or b) known to be separated. The caller is responsible for confirming that the remaining, unculled geometry pairs are actually in collision.

Returns: A vector populated with collision pair candidates (the order will remain constant for a fixed population but can change as geometry ids are added/removed).

◆ GetConfigurationsInWorld()

const VectorX<T>& GetConfigurationsInWorld ( GeometryId deformable_geometry_id ) const

Reports the configuration of the deformable geometry indicated by deformable_geometry_id relative to the world frame.

See also: GetPoseInWorld().

Exceptions

std::exception if the geometry deformable_geometry_id is not valid or is not a deformable geometry.

◆ GetDrivenMeshConfigurationsInWorld()

std::vector<VectorX<T> > GetDrivenMeshConfigurationsInWorld	(	GeometryId	deformable_geometry_id,
		Role	role
	)		const

Reports the configurations of the driven meshes associated with the given role for the deformable geometry indicated by deformable_geometry_id relative to the world frame if the deformable geometry has that role.

Exceptions

std::exception if the geometry associated with deformable_geometry_id is not a registered deformable geometry with the given role.

Warning: This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

◆ GetPoseInParent()

const math::RigidTransform<T>& GetPoseInParent ( FrameId frame_id ) const

Reports the position of the frame indicated by frame_id relative to its parent frame.

If the frame was registered with the world frame as its parent frame, this value will be identical to that returned by GetPoseInWorld().

Exceptions

std::exception if the frame frame_id is not valid.

◆ GetPoseInWorld() [1/2]

const math::RigidTransform<T>& GetPoseInWorld ( FrameId frame_id ) const

Reports the position of the frame indicated by frame_id relative to the world frame.

Exceptions

std::exception if the frame frame_id is not valid.

◆ GetPoseInWorld() [2/2]

const math::RigidTransform<T>& GetPoseInWorld ( GeometryId geometry_id ) const

Reports the position of the frame of the rigid geometry indicated by geometry_id relative to the world frame (X_WG).

Note: This query is meaningless for deformable geometries. Their current state cannot be represented by a single rigid transformation. Instead, one should use GetConfigurationsInWorld() to get the current vertex positions of the deformable geometry in the world frame. On the other hand, it is meaningful to query the fixed pose of the reference geometry in its parent frame (see SceneGraphInspector::GetPoseInFrame()).

Exceptions

std::exception if the geometry geometry_id is not valid or if it is deformable.

◆ GetRenderEngineByName()

const render::RenderEngine* GetRenderEngineByName ( const std::string & name ) const

Returns the named render engine, if it exists.

The RenderEngine is guaranteed to be up to date w.r.t. the poses and data in the context.

◆ HasCollisions()

bool HasCollisions ( ) const

Reports true if there are any collisions between unfiltered pairs in the world.

Warning: For Mesh shapes, their convex hulls are used in this query. It is not* computationally efficient or particularly accurate.

◆ inspector()

const SceneGraphInspector<T>& inspector ( ) const

Provides an inspector for the topological structure of the underlying scene graph data (see SceneGraphInspector for details).

◆ operator=() [1/2]

QueryObject& operator= ( const QueryObject< T > & )

◆ operator=() [2/2]

QueryObject& operator= ( QueryObject< T > && )

default

◆ RenderColorImage()

void RenderColorImage	(	const render::ColorRenderCamera &	camera,
		FrameId	parent_frame,
		const math::RigidTransformd &	X_PC,
		systems::sensors::ImageRgba8U *	color_image_out
	)		const

Renders an RGB image for the given camera posed with respect to the indicated parent frame P.

Parameters

	camera	The camera to render from.
	parent_frame	The id for the camera's parent frame.
	X_PC	The pose of the camera body in the parent frame.
[out]	color_image_out	The rendered color image.

◆ RenderDepthImage()

void RenderDepthImage	(	const render::DepthRenderCamera &	camera,
		FrameId	parent_frame,
		const math::RigidTransformd &	X_PC,
		systems::sensors::ImageDepth32F *	depth_image_out
	)		const

Renders a depth image for the given camera posed with respect to the indicated parent frame P.

In contrast to the other rendering methods, rendering depth images doesn't provide the option to display the window; generally, basic depth images are not readily communicative to humans.

Parameters

	camera	The camera to render from.
	parent_frame	The id for the camera's parent frame.
	X_PC	The pose of the camera body in the parent frame.
[out]	depth_image_out	The rendered depth image.

◆ RenderLabelImage()

void RenderLabelImage	(	const render::ColorRenderCamera &	camera,
		FrameId	parent_frame,
		const math::RigidTransformd &	X_PC,
		systems::sensors::ImageLabel16I *	label_image_out
	)		const

Renders a label image for the given camera posed with respect to the indicated parent frame P.

Parameters

	camera	The camera to render from.
	parent_frame	The id for the camera's parent frame.
	X_PC	The pose of the camera body in the parent frame.
[out]	label_image_out	The rendered label image.

Friends And Related Function Documentation

◆ QueryObjectTest

friend class QueryObjectTest

friend

◆ SceneGraph< T >

friend class SceneGraph< T >

friend

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

drake/geometry/query_object.h

Detailed Description

template<typename T> class drake::geometry::QueryObject< T >

Queries and scalar type

Public Member Functions

Friends

Constructor & Destructor Documentation

◆ QueryObject() [1/3]

◆ QueryObject() [2/3]

◆ QueryObject() [3/3]

Member Function Documentation

◆ ComputeContactSurfaces()

Scalar support

◆ ComputeContactSurfacesWithFallback()

Scalar support

◆ ComputeDeformableContact()

◆ ComputePointPairPenetration()

Characterizing the returned values

◆ ComputeSignedDistancePairClosestPoints()

Characterizing the returned values

◆ ComputeSignedDistancePairwiseClosestPoints()

Using maximum distance

Characterizing the returned values

Characterizing the returned gradients

◆ ComputeSignedDistanceToPoint()

Characterizing the returned values

◆ FindCollisionCandidates()

◆ GetConfigurationsInWorld()

◆ GetDrivenMeshConfigurationsInWorld()

◆ GetPoseInParent()

◆ GetPoseInWorld() [1/2]

◆ GetPoseInWorld() [2/2]

◆ GetRenderEngineByName()

◆ HasCollisions()

◆ inspector()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ RenderColorImage()

◆ RenderDepthImage()

◆ RenderLabelImage()

Friends And Related Function Documentation

◆ QueryObjectTest

◆ SceneGraph< T >

template<typename T>
class drake::geometry::QueryObject< T >