Drake

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 
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.
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.
T  The scalar type. Must be a valid Eigen scalar. 
Instantiated templates for the following kinds of T's are provided:
They are already available to link against in the containing library. No other values for T are currently supported.

default 
Constructs a default QueryObject (all pointers are null).
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.
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.
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.
near_pairs  The signed distance for all unfiltered geometry pairs. 
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.
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.
[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). See SignedDistanceToPoint. 

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

friend 

friend 