Drake
QueryObject< T > Class Template Reference

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

#include <drake/geometry/scene_graph_inspector.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...
 
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.

std::vector< PenetrationAsPointPair< double > > ComputePointPairPenetration () const
 Computes the penetrations across all pairs of geometries 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< double > > ComputeSignedDistancePairwiseClosestPoints () const
 Computes the signed distance together with the nearest points across all pairs of geometries in the world. More...
 
std::vector< SignedDistanceToPoint< double > > ComputeSignedDistanceToPoint (const Vector3< double > &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...
 

Friends

class SceneGraph< T >
 
class QueryObjectTester
 

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.

In addition to not persisting the reference from the output port, the QueryObject shouldn't be copied. Strictly speaking, it is an allowed operation, but the result is not live, and any geometry query performed on the copy will throw an exception.

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

Template Parameters
TThe scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

  • double
  • AutoDiffXd

They are already available to link against in the containing library. No other values for T are currently supported.

Constructor & Destructor Documentation

◆ QueryObject()

QueryObject ( )
default

Constructs a default QueryObject (all pointers are null).

Member Function Documentation

◆ ComputePointPairPenetration()

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

Computes the penetrations across all pairs of geometries in the world.

Only reports results for penetrating geometries; if two geometries are separated, there will be no result for that pair. Pairs of anchored geometry are also not reported. The penetration between two geometries is characterized as a point pair (see PenetrationAsPointPair).

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

This method is affected by collision filtering; element pairs that have been filtered will not produce contacts, even if their collision geometry is penetrating.

Returns
A vector populated with all detected penetrations characterized as point pairs.

◆ ComputeSignedDistancePairwiseClosestPoints()

std::vector< SignedDistancePair< double > > ComputeSignedDistancePairwiseClosestPoints ( ) 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.

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.

Return values
near_pairsThe signed distance for all unfiltered geometry pairs.

◆ ComputeSignedDistanceToPoint()

std::vector< SignedDistanceToPoint< double > > ComputeSignedDistanceToPoint ( const Vector3< double > &  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.

Warning
Currently supports spheres and boxes only. Silently ignores other kinds of geometries, which will be added later.

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.

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 its lies within a certain tolerance from them.
For a box B, if a point p is inside the box, and it is equidistant to 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). See SignedDistanceToPoint.

◆ inspector()

const SceneGraphInspector<T>& inspector ( ) const
inline

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

Friends And Related Function Documentation

◆ QueryObjectTester

friend class QueryObjectTester
friend

◆ SceneGraph< T >

friend class SceneGraph< T >
friend

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