The QueryObject serves as a mechanism to perform geometry queries on the world's geometry.
The SceneGraph has an abstractvalued port that contains a QueryObject (i.e., a QueryObjectvalued output port).
To perform geometry queries on SceneGraph:
const QueryObject&
in return, and, finally,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 reevaluate 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.
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, geometrypair, 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:
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 
Configurationdependent 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 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 nonempty 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 nonempty 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 nonzero 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  
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 

default 
Constructs a default QueryObject (all pointers are null).
QueryObject  (  const QueryObject< T > &  other  ) 

default 
std::enable_if_t<scalar_predicate<T1>::is_bool, std::vector<ContactSurface<T> > > ComputeContactSurfaces  (  HydroelasticContactRepresentation  representation  )  const 
Reports pairwise intersections and characterizes each nonempty 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:
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ᶜ 
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.)
representation  Controls the mesh representation of the contact surface. See contact surface representation for more details. 
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 nonempty 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.
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.
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. 
surfaces
nor point_pairs
is nullptr. std::exception  for the reasons described in ComputeContactSurfaces() and ComputePointPairPenetration(). 
surfaces
and point_pairs
are output pointers in C++, but are return values in the Python bindings. 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 nondeformable geometry. This function only supports double as the scalar type.
[out]  deformable_contact  Contains all deformable contact data on output. Any data passed in is cleared before the computation. 
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.
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  2e15  ░░░░░░  ░░░░░  ░░░░░░░  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Capsule  3e5ᶜ  2e5ᶜ  ░░░░░  ░░░░░░░  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Convex  2e15ᶜ  3e5ᶜ  2e15ᶜ  ░░░░░░░  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Cylinder  1e3ᶜ  4e5ᶜ  1e3ᶜ  2e3ᶜ  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Ellipsoid  4e4ᶜ  2e4ᶜ  4e4ᶜ  2e3ᶜ  5e4ᶜ  ░░░░░░  ░░░░░  ░░░░░ 
HalfSpace  6e15  4e15  3e15ᶜ  4e15  3e15  throwsᵃ  ░░░░░  ░░░░░ 
Mesh  ᵇ  ᵇ  ᵇ  ᵇ  ᵇ  ᵇ  ᵇ  ░░░░░ 
Sphere  3e15  5e15  3e5ᶜ  5e15  2e4ᶜ  3e15  ᵇ  5e15 
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  2e15  3e15  throwsᵈ  2e15  throwsᵈ  2e15  ᵇ  5e15 
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.
T
= AutoDiffXd at this time.T
= drake::symbolic::Expression at this time.std::exception  if a ShapeShape pair is in collision and indicated as throws in the support table above. 
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().
This method merely exercises the same mechanisms as ComputeSignedDistancePairwiseClosestPoints() for evaluating signed distance. Refer to the table for ComputeSignedDistancePairwiseClosestPoints() for details.
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. 
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.
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.
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.
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  4e15  ░░░░░░  ░░░░░  ░░░░░░░  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Capsule  3e6  2e5  ░░░░░  ░░░░░░░  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Convex  3e15  2e5  3e15  ░░░░░░░  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Cylinder  6e6  1e5  6e6  2e5  ░░░░░░  ░░░░░░  ░░░░░  ░░░░░ 
Ellipsoid  9e6  5e6  9e6  5e5  2e5  ░░░░░░  ░░░░░  ░░░░░ 
HalfSpace  throwsᵃ  throwsᵃ  throwsᵃ  throwsᵃ  throwsᵃ  throwsᵃ  ░░░░░  ░░░░░ 
Mesh  ᶜ  ᶜ  ᶜ  ᶜ  ᶜ  throwsᵃ  ᶜ  ░░░░░ 
Sphere  3e15  6e15  3e6  5e15  4e5  3e15  ᶜ  6e15 
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  2e15  throwsᵇ  throwsᵇ  throwsᵇ  throwsᵇ  2e15  ᶜ  5e15 
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.
T
= AutoDiffXd at this time.T
= drake::symbolic::Expression at this time.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 signeddistance gradients when two geometries touch at zero distance.
max_distance  The maximum distance at which distance data is reported. 
max_distance
. The ordering of the results is guaranteed to be consistent – for fixed geometry poses, the results will remain the same. std::exception  as indicated in the table above. 
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.
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 zeroradius sphere.
Scalar  Box  Capsule  Convex  Cylinder  Ellipsoid  HalfSpace  Mesh  Sphere 

double  2e15  4e15  ᵃ  3e15  3e5ᵇ  5e15  ᵃ  4e15 
AutoDiffXd  1e15  4e15  ᵃ  ᵃ  ᵃ  5e15  ᵃ  3e15 
Expression  ᵃ  ᵃ  ᵃ  ᵃ  ᵃ  ᵃ  ᵃ  ᵃ 
Table 8: Worst observed error (in m) for 2mm penetration/separation between geometry approximately 20cm in size and a point.
[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. 
signed_distances  A vector populated with perobject 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. 
std::vector<SortedPair<GeometryId> > FindCollisionCandidates  (  )  const 
Applies a conservative culling mechanism to create a subset of all possible geometry pairs based on nonzero 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.
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.
std::exception  if the geometry deformable_geometry_id is not valid or is not a deformable geometry. 
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.
std::exception  if the geometry associated with deformable_geometry_id is not a registered deformable geometry with the given role. 
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().
std::exception  if the frame frame_id is not valid. 
const math::RigidTransform<T>& GetPoseInWorld  (  FrameId  frame_id  )  const 
Reports the position of the frame indicated by frame_id
relative to the world frame.
std::exception  if the frame frame_id is not valid. 
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).
std::exception  if the geometry geometry_id is not valid or if it is deformable. 
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.
bool HasCollisions  (  )  const 
Reports true if there are any collisions between unfiltered pairs in the world.
const SceneGraphInspector<T>& inspector  (  )  const 
Provides an inspector for the topological structure of the underlying scene graph data (see SceneGraphInspector for details).
QueryObject& operator=  (  const QueryObject< T > &  ) 

default 
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.
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. 
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.
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. 
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.
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. 

friend 

friend 