Drake
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 nonsymbolic scalars.

#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...
 
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
 
Pose-dependent Introspection

These methods provide access to introspect geometry and frame quantities that directly depend on the poses of the frames.

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 geometry indicated by geometry_id relative to the world frame. 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...
 
std::vector< ContactSurface< T > > ComputeContactSurfaces () const
 Reports pairwise intersections and characterizes each non-empty intersection as a ContactSurface for hydroelastic contact model. More...
 
void ComputeContactSurfacesWithFallback (std::vector< ContactSurface< T >> *surfaces, std::vector< PenetrationAsPointPair< T >> *point_pairs) const
 Reports pair-wise intersections and characterizes each non-empty intersection as a ContactSurface where possible and as a PenetrationAsPointPair where not. 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::vector<ContactSurface<T> > ComputeContactSurfaces ( ) 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 Soft Rigid
Sphere yes yes
Cylinder yes yes
Box yes yes
Capsule yes yes
Ellipsoid yes yes
HalfSpace yes yes
Mesh no yes
Convex no yes
  • We do not currently support contact between two geometries with the same* compliance; one geometry must be soft, and the other must be rigid. If geometries with the same compliance 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.
  • The elasticity modulus E (N/m^2) of each geometry is set in ProximityProperties (see AddContactMaterial()).
  • The tessellation of the corresponding meshes is controlled by the resolution hint (where appropriate), as defined by AddSoftHydroelasticProperties() and AddRigidHydroelasticProperties().

Scalar support

This method provides support for both double and AutoDiffXd. 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.)

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()

void ComputeContactSurfacesWithFallback ( std::vector< ContactSurface< T >> *  surfaces,
std::vector< PenetrationAsPointPair< T >> *  point_pairs 
) const

Reports pair-wise 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.

Parameters
[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().

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

  • ᵃ 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.
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().

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, the pair (A, B) has been marked as filtered, or according to 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.

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 3: 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 ░░░░░
Sphere 2e-15 throwsᵇ throwsᵇ throwsᵇ throwsᵇ 2e-15 5e-15

Table 4: Worst observed error (in m) for 2mm penetration/separation between geometries approximately 20cm in size for T = AutoDiffXd.

  • ᵃ 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.
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.
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 5e-15 4e-15
ADXd 1e-15 4e-15 5e-15 3e-15

Table 5: 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.
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 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.

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

◆ 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().

Note
This is analogous to but distinct from SceneGraphInspector::GetPoseInParent(). In this case, the pose will always be relative to another frame.
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 geometry indicated by geometry_id relative to the world frame.

Exceptions
std::exceptionif the geometry geometry_id is not valid.

◆ 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: