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/query_object.h>

Public Member Functions

 QueryObject (const QueryObject &other)
 
QueryObjectoperator= (const QueryObject &)
 
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 φ(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.

Note
The signed distance function is a continuous function with respect to the pose of the objects.
std::vector< SignedDistancePair< double > > ComputeSignedDistancePairwiseClosestPoints () const
 Computes the signed distance together with the nearest points across all pairs of geometries in the world. 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 ( const QueryObject< T > &  other)

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

◆ inspector()

const SceneGraphInspector<T>& inspector ( ) const
inline

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

◆ operator=()

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

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: