Drake
Drake C++ Documentation
QueryObject< T > Class Template Reference

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
TThe 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)
 
QueryObjectoperator= (const QueryObject &)
 
 QueryObject (QueryObject &&)=default
 
QueryObjectoperator= (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::RenderEngineGetRenderEngineByName (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
representationControls 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
representationControls the mesh representation of the contact surface. See contact surface representation for more details.
[out]surfacesThe vector that contact surfaces will be added to. The vector will not be cleared.
[out]point_pairsThe 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::exceptionfor 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_contactContains 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::exceptionif 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::exceptionif 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_distanceThe 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::exceptionas 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_WQPosition of a query point Q in world frame W.
[in]thresholdWe 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_distancesA 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::exceptionif 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::exceptionif 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::exceptionif 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::exceptionif 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::exceptionif 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
cameraThe camera to render from.
parent_frameThe id for the camera's parent frame.
X_PCThe pose of the camera body in the parent frame.
[out]color_image_outThe 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
cameraThe camera to render from.
parent_frameThe id for the camera's parent frame.
X_PCThe pose of the camera body in the parent frame.
[out]depth_image_outThe 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
cameraThe camera to render from.
parent_frameThe id for the camera's parent frame.
X_PCThe pose of the camera body in the parent frame.
[out]label_image_outThe 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: