pydrake.geometry

pydrake.geometry.ConnectDrakeVisualizer(*args, **kwargs)

Overloaded function.

  1. ConnectDrakeVisualizer(builder: pydrake.systems.framework.DiagramBuilder_[float], scene_graph: pydrake.geometry.SceneGraph, lcm: pydrake.lcm.DrakeLcmInterface = None) -> pydrake.systems.lcm.LcmPublisherSystem

Extends a Diagram with the required components to interface with drake_visualizer. This must be called during Diagram building and uses the given builder to add relevant subsystems and connections.

This is a convenience method to simplify some common boilerplate for adding visualization capability to a Diagram. What it does is:

  • adds an initialization event that sends the required load message to set up

the visualizer with the relevant geometry, - adds systems PoseBundleToDrawMessage and LcmPublisherSystem to the Diagram and connects the draw message output to the publisher input, - connects the scene_graph pose bundle output to the PoseBundleToDrawMessage system, and - sets the publishing rate to 1/60 of a second (simulated time).

The visualization mechanism depends on the illustration role (see geometry_roles for details). Specifically, only geometries with the illustration role assigned will be included. The visualization function looks for the following properties in the IllustrationProperties instance. | Group name | Required | Property Name | Property Type | Property Description | | :——–: | :——: | :———–: | :————-: | :——————- | | phong | no | diffuse | Eigen::Vector4d | The rgba value of the object surface | See MakeDrakeVisualizerProperties() to facilitate making a compliant set of illustration properties.

You can then connect source output ports for visualization like this:

builder->Connect(pose_output_port,
scene_graph.get_source_pose_port(source_id));

Note

The initialization event occurs when Simulator::Initialize() is called (explicitly or implicitly at the start of a simulation). If you aren’t going to be using a Simulator, use DispatchLoadMessage() to send the message yourself.

Parameter builder:
The diagram builder being used to construct the Diagram.
Parameter scene_graph:
The System in builder containing the geometry to be visualized.
Parameter lcm:
An optional lcm interface through which lcm messages will be dispatched. Will be allocated internally if none is supplied.
Precondition:
This method has not been previously called while building the builder’s current Diagram.
Precondition:
The given scene_graph must be contained within the supplied DiagramBuilder.
Returns:the LcmPublisherSystem (in case callers, e.g., need to change the default publishing rate).

See also

geometry::DispatchLoadMessage()

  1. ConnectDrakeVisualizer(builder: pydrake.systems.framework.DiagramBuilder_[float], scene_graph: pydrake.geometry.SceneGraph, pose_bundle_output_port: pydrake.systems.framework.OutputPort_[float], lcm: pydrake.lcm.DrakeLcmInterface = None) -> pydrake.systems.lcm.LcmPublisherSystem

Implements ConnectDrakeVisualizer, but using pose_bundle_output_port to explicitly specify the output port used to get pose bundles for scene_graph. This is required, for instance, when the SceneGraph is inside a Diagram, and the Diagram exports the pose bundle port.

Precondition:
pose_bundle_output_port must be connected directly to the pose_bundle_output_port of scene_graph.

See also

ConnectDrakeVisualizer().

pydrake.geometry.DispatchLoadMessage(scene_graph: pydrake.geometry.SceneGraph, lcm: pydrake.lcm.DrakeLcmInterface) → None

(Advanced) Explicitly dispatches an LCM load message based on the registered geometry. Normally this is done automatically at Simulator initialization. But if you have to do it yourself (likely because you are not using a Simulator), it should be invoked after registration is complete. Typically this is used after ConnectDrakeVisualizer() has been used to add visualization to the Diagram that contains the given scene_graph. The message goes to LCM channel “DRAKE_VIEWER_LOAD_ROBOT”.

See also

geometry::ConnectDrakeVisualizer()

class pydrake.geometry.FrameId
__init__(self: pydrake.geometry.FrameId) → None

Default constructor; the result is an invalid identifier. This only exists to satisfy demands of working with various container classes.

get_new_id() → pydrake.geometry.FrameId

Generates a new identifier for this id type. This new identifier will be different from all previous identifiers created. This method does not make any guarantees about the values of ids from successive invocations. This method is guaranteed to be thread safe.

get_value(self: pydrake.geometry.FrameId) → int

Extracts the underlying representation from the identifier. This is considered invalid for invalid ids and is strictly enforced in Debug builds.

is_valid(self: pydrake.geometry.FrameId) → bool

Reports if the id is valid.

class pydrake.geometry.GeometryId
__init__(self: pydrake.geometry.GeometryId) → None

Default constructor; the result is an invalid identifier. This only exists to satisfy demands of working with various container classes.

get_new_id() → pydrake.geometry.GeometryId

Generates a new identifier for this id type. This new identifier will be different from all previous identifiers created. This method does not make any guarantees about the values of ids from successive invocations. This method is guaranteed to be thread safe.

get_value(self: pydrake.geometry.GeometryId) → int

Extracts the underlying representation from the identifier. This is considered invalid for invalid ids and is strictly enforced in Debug builds.

is_valid(self: pydrake.geometry.GeometryId) → bool

Reports if the id is valid.

class pydrake.geometry.PenetrationAsPointPair
__init__(self: pydrake.geometry.PenetrationAsPointPair) → None
depth

The penetration depth.

id_A

The id of the first geometry in the contact.

id_B

The id of the second geometry in the contact.

nhat_BA_W

The unit-length normal which defines the penetration direction, pointing from geometry B into geometry A, measured and expressed in the world frame. It approximates the normal to the plane on which the contact patch lies.

p_WCa

The point on A that most deeply penetrates B, measured and expressed in the world frame.

p_WCb

The point on B that most deeply penetrates A, measured and expressed in the world frame.

class pydrake.geometry.QueryObject

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 parameter T:
The 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.

ComputePointPairPenetration(self: pydrake.geometry.QueryObject) → List[drake::geometry::PenetrationAsPointPair<double>]

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(self: pydrake.geometry.QueryObject) → List[drake::geometry::SignedDistancePair<double>]

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.

Returns near_pairs:
The signed distance for all unfiltered geometry pairs.
__init__

x.__init__(…) initializes x; see help(type(x)) for signature

inspector(self: pydrake.geometry.QueryObject) → pydrake.geometry.SceneGraphInspector

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

class pydrake.geometry.SceneGraph

Bases: pydrake.systems.framework.LeafSystem_[float]

SceneGraph serves as the nexus for all geometry (and geometry-based operations) in a Diagram. Through SceneGraph, other systems that introduce geometry can register that geometry as part of a common global domain, including it in geometric queries (e.g., cars controlled by one LeafSystem can be observed by a different sensor system). SceneGraph provides the interface for registering the geometry, updating its position based on the current context, and performing geometric queries.

Only registered “geometry sources” can introduce geometry into SceneGraph. Geometry sources will typically be other leaf systems, but, in the case of anchored (i.e., stationary) geometry, it could also be some other block of code (e.g., adding a common ground plane with which all systems’ geometries interact). For dynamic geometry (geometry whose pose depends on a Context), the geometry source must also provide pose values for all of the geometries the source owns, via a port connection on SceneGraph.

The basic workflow for interacting with SceneGraph is:

  • Register as a geometry source, acquiring a unique SourceId.
  • Register geometry (anchored and dynamic) with the system.
  • Connect source’s geometry output ports to the corresponding SceneGraph

input ports. - Implement appropriate Calc* methods on the geometry output ports to update geometry pose values.

For each registered geometry source, there is one input port for each order of kinematics values (e.g., pose, velocity, and acceleration). If a source registers a frame, it must connect to these ports (although, in the current version, only pose is supported). Failure to connect to the port (or to provide valid kinematics values) will lead to runtime exceptions.

pose port: An abstract-valued port providing an instance of FramePoseVector. For each registered frame, this “pose vector” maps the registered FrameId to a pose value. All registered frames must be accounted for and only frames registered by a source can be included in its output port. See the details in FrameKinematicsVector for details on how to allocate and calculate this port.

SceneGraph has two output ports:

query port: An abstract-valued port containing an instance of QueryObject. It provides a “ticket” for downstream LeafSystem instances to perform geometric queries on the SceneGraph. To perform geometric queries, downstream LeafSystem instances acquire the QueryObject from SceneGraph’s output port and provide it as a parameter to one of SceneGraph’s query methods (e.g., SceneGraph::ComputeContact()). This assumes that the querying system has access to a const pointer to the connected SceneGraph instance. Use get_query_output_port() to acquire the output port for the query handle.

lcm visualization port: An abstract-valued port containing an instance of PoseBundle. This is a convenience port designed to feed LCM update messages to drake_visualizer for the purpose of visualizing the state of the world’s geometry. Additional uses of this port are strongly discouraged; instead, use an appropriate geometric query to obtain the state of the world’s geometry.

LeafSystem instances can relate to SceneGraph in one of two ways: as a consumer that performs queries, or as a producer that introduces geometry into the shared world and defines its context-dependent kinematics values. It is reasonable for systems to perform either role singly, or both.

Consumer

Consumers perform geometric queries upon the world geometry. SceneGraph serves those queries. As indicated above, in order for a LeafSystem to act as a consumer, it must: 1. define a QueryObject-valued input port and connect it to SceneGraph’s corresponding output port, and 2. have a reference to the connected SceneGraph instance.

With those two requirements satisfied, a LeafSystem can perform geometry queries by: 1. evaluating the QueryObject input port, and 2. passing the returned query object into the appropriate query method on SceneGraph (e.g., SceneGraph::ComputeContact()).

Producer

All producers introduce geometry into the shared geometric world. This is called registering geometry. Depending on what exactly has been registered, a producer may also have to update kinematics. Producers themselves must be registered with SceneGraph as producers (a.k.a. geometry sources). They do this by acquiring a SourceId (via SceneGraph::RegisterSource()). The SourceId serves as a unique handle through which the producer’s identity is validated and its ownership of its registered geometry is maintained.

Registering Geometry

SceneGraph cannot know what geometry should be part of the shared world. Other systems are responsible for introducing geometry into the world. This process (defining geometry and informing SceneGraph) is called registering the geometry. The source that registers the geometry “owns” the geometry; the source’s unique SourceId is required to perform any operations on the geometry registered with that SourceId. Geometry can be registered as anchored or dynamic.

Dynamic geometry can move; more specifically, its kinematics (e.g., pose) depends on a system’s Context. Particularly, dynamic geometry is fixed to a frame whose kinematics values depend on a context. As the frame moves, the geometries fixed to it move with it. Therefore, to register dynamic geometry a frame must be registered first. These registered frames serve as the basis for repositioning geometry in the shared world. The geometry source is responsible for providing up-to-date kinematics values for those registered frames upon request (via an appropriate output port on the source LeafSystem connecting to the appropriate input port on SceneGraph). The work flow is as follows: 1. A LeafSystem registers itself as a geometry source, acquiring a SourceId (RegisterSource()). 2. The source registers a frame (GeometrySource::RegisterFrame()). - A frame always has a “parent” frame. It can implicitly be the world frame, or another frame registered by the source. 3. Register one or more geometries to a frame (GeometrySource::RegisterGeometry()). - The registered geometry is posed relative to the frame to which it is fixed. - The geometry can also be posed relative to another registered geometry. It will be affixed to that geometry’s frame.

Anchored geometry is independent of the context (i.e., it doesn’t move). Anchored geometries are always affixed to the immobile world frame. As such, registering a frame is not required for registering anchored geometry (see GeometrySource::RegisterAnchoredGeometry()). However, the source still “owns” the anchored geometry.

Updating Kinematics

Registering dynamic geometry implies a contract between the geometry source and SceneGraph. The geometry source must do the following: - It must provide, populate, and connect two output ports: the “id” port and the “pose” port. - The id port must contain all the frame ids returned as a result of frame registration. - The pose port must contain one pose per registered frame; the pose value is expressed relative to the registered frame’s parent frame. As mentioned above, the iᵗʰ pose value should describe the frame indicated by the iᵗʰ id in the id output port.

Failure to meet these requirements will lead to a run-time error.

Many (and eventually all) methods that configure the population of SceneGraph have two variants that differ by whether they accept a mutable Context or not. When no Context is provided, this SceneGraph instance’s underlying model is modified. When the SceneGraph instance allocates a context, its model is copied into that context.

The second variant causes SceneGraph to modify the data stored in the provided Context to be modified instead of the internal model.

Note

In this initial version, the only methods with the Context-modifying variant are those methods that do not change the the semantics of the input or output ports. Modifications that make such changes must be coordinated across systems.

Template parameter T:
The 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.

RegisterSource(self: pydrake.geometry.SceneGraph, name: unicode = u'') → pydrake.geometry.SourceId

Registers a new source to the geometry system. The caller must save the returned SourceId; it is the token by which all other operations on the geometry world are conducted.

This source id can be used to register arbitrary anchored geometry. But if dynamic geometry is registered (via RegisterGeometry/RegisterFrame), then the context-dependent pose values must be provided on an input port. See get_source_pose_port().

Parameter name:
The optional name of the source. If none is provided (or the empty string) a unique name will be defined by SceneGraph’s logic.
Raises:
  • RuntimeError if a context has already been allocated for this
  • SceneGraph.

See also

GeometryState::RegisterNewSource()

__init__(self: pydrake.geometry.SceneGraph) → None

Constructor used for scalar conversions. It should only be used to convert from double to other scalar types.

get_pose_bundle_output_port(self: pydrake.geometry.SceneGraph) → pydrake.systems.framework.OutputPort_[float]

Returns the output port which produces the PoseBundle for LCM communication to drake visualizer.

get_query_output_port(self: pydrake.geometry.SceneGraph) → pydrake.systems.framework.OutputPort_[float]

Returns the output port which produces the QueryObject for performing geometric queries.

get_source_pose_port(self: pydrake.geometry.SceneGraph, arg0: pydrake.geometry.SourceId) → pydrake.systems.framework.InputPort_[float]

Given a valid source id, returns a pose input port associated with that id. This port is used to communicate pose data for registered frames.

Raises:RuntimeError if the source_id is not recognized.
class pydrake.geometry.SceneGraphInspector

The SceneGraphInspector serves as a mechanism to query the topological structure of a SceneGraph instance. The topological structure consists of all of the SceneGraph data that does not depend on input pose data. Including, but not limited to:

  • names of frames and geometries
  • hierarchies (parents of geometries, parents of frames, etc.)
  • geometry parameters (e.g., contact, rendering, visualization)
  • fixed poses of geometries relative to frames

In contrast, the following pieces of data do depend on input pose data and cannot* be performed with the SceneGraphInspector (see the QueryObject instead):

  • world pose of frames or geometry
  • collision queries
  • proximity queries

A SceneGraphInspector cannot be instantiated explicitly. Nor can it be copied or moved. A reference to a SceneGraphInspector instance can be acquired from

  • a SceneGraph instance (to inspect the state of the system’s model), or
  • a QueryObject instance (to inspect the state of the scene graph data stored

in the context).

The reference should not be persisted (and, as previously indicated, cannot be copied). SceneGraphInspector instances are cheap; they can be created, queried, and thrown out. If there is any doubt about the valid lifespan of a SceneGraphInspector, throw out the old instance and request a new instance.

Template parameter T:
The scalar of the associated SceneGraph instance. The template parameter is provided for the sake of compatibility, although no queries (or their results) depend on the scalar.
GetFrameId(self: pydrake.geometry.SceneGraphInspector, geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.FrameId

Reports the id of the frame to which the given geometry id is registered.

Raises:RuntimeError if the geometry id is invalid.
__init__

x.__init__(…) initializes x; see help(type(x)) for signature

class pydrake.geometry.SignedDistancePair
__init__(self: pydrake.geometry.SignedDistancePair) → None
distance

The distance between p_A_A and p_B_B (measured in a common frame).

id_A

The id of the first geometry in the pair.

id_B

The id of the second geometry in the pair.

p_ACa

The witness point on geometry A’s surface, expressed in A’s frame.

p_BCb

The witness point on geometry B’s surface, expressed in B’s frame.

class pydrake.geometry.SourceId
__init__(self: pydrake.geometry.SourceId) → None

Default constructor; the result is an invalid identifier. This only exists to satisfy demands of working with various container classes.

get_new_id() → pydrake.geometry.SourceId

Generates a new identifier for this id type. This new identifier will be different from all previous identifiers created. This method does not make any guarantees about the values of ids from successive invocations. This method is guaranteed to be thread safe.

get_value(self: pydrake.geometry.SourceId) → int

Extracts the underlying representation from the identifier. This is considered invalid for invalid ids and is strictly enforced in Debug builds.

is_valid(self: pydrake.geometry.SourceId) → bool

Reports if the id is valid.