Drake
GeometrySystem< T > Class Template Reference

GeometrySystem serves as a system-level wrapper for GeometryWorld. More...

#include <drake/geometry/geometry_state.h>

Inheritance diagram for GeometrySystem< T >:
[legend]
Collaboration diagram for GeometrySystem< T >:
[legend]

Public Member Functions

 GeometrySystem ()
 
 ~GeometrySystem () override
 
Does not allow copy, move, or assignment
 GeometrySystem (const GeometrySystem &)=delete
 
GeometrySystemoperator= (const GeometrySystem &)=delete
 
 GeometrySystem (GeometrySystem &&)=delete
 
GeometrySystemoperator= (GeometrySystem &&)=delete
 
Port management

Access to GeometrySystem's input/output ports.

This topic includes registration of geometry sources because the input ports are mapped to registered geometry sources.

A source that registers frames and geometries must connect outputs to the inputs associated with that source. Failure to do so will be treated as a runtime error during the evaluation of GeometrySystem. GeometrySystem will detect that frames have been registered but no values have been provided.

SourceId RegisterSource (const std::string &name="")
 Registers a new source to the geometry system (see GeometryWorld for the discussion of "geometry source"). More...
 
const systems::InputPortDescriptor< T > & get_source_frame_id_port (SourceId id)
 Given a valid source id, returns the "frame id" input port associated with that id. More...
 
const systems::InputPortDescriptor< T > & get_source_pose_port (SourceId id)
 Given a valid source id, returns a pose input port associated with that id. More...
 
const systems::OutputPort< T > & get_pose_bundle_output_port () const
 Returns the output port which produces the PoseBundle for LCM communication to drake visualizer. More...
 
const systems::OutputPort< T > & get_query_output_port () const
 Returns the output port which produces the QueryHandle for performing geometric queries. More...
 
Topology Manipulation

Topology manipulation consists of changing the data contained in GeometryWorld.

This includes registering a new geometry source, adding or removing frames, and adding or removing geometries.

Currently, the topology can only be manipulated during initialization. Eventually, the API will expand to include modifications of the topology during discrete updates.

The initialization phase begins with the instantiation of a GeometrySystem and ends when a context is allocated by the GeometrySystem instance. This is the only phase when geometry sources can be registered with GeometryWorld. Once a source is registered, it can register frames and geometries. Any frames and geometries registered during this phase become part of the default context state for GeometrySystem and calls to CreateDefaultContext() will produce identical contexts.

Every geometry must ultimately be associated with a parent frame. The position of every geometry in the world depends on a hierarchy of frames between it and the world. The pose of a geometry is described relative to its parent (a frame or another geometry). That parent may, in turn, also have a parent. So, the position of a particular geometry in the world frame depends on all of its ancestors which lie between it and the world frame. The act of assigning a frame or geometry as a child to another frame or geometry (as appropriate) and defining its pose, is referred to colloquially has "hanging" it on the parent.

Geometry sources can only hang frames or geometries onto other frames and/or geometries that it "owns".

FrameId RegisterFrame (SourceId source_id, const GeometryFrame &frame)
 Registers a new frame F on for this source. More...
 
FrameId RegisterFrame (SourceId source_id, FrameId parent_id, const GeometryFrame &frame)
 Registers a new frame F for this source. More...
 
GeometryId RegisterGeometry (SourceId source_id, FrameId frame_id, std::unique_ptr< GeometryInstance > geometry)
 Registers a new geometry G for this source. More...
 
GeometryId RegisterGeometry (SourceId source_id, GeometryId geometry_id, std::unique_ptr< GeometryInstance > geometry)
 Registers a new geometry G for this source. More...
 
GeometryId RegisterAnchoredGeometry (SourceId source_id, std::unique_ptr< GeometryInstance > geometry)
 Registers a new anchored geometry G for this source. More...
 
void ClearSource (SourceId source_id)
 Clears of all the registered frames and geometries from this source, but the source is still registered, allowing future registration of frames and geometries. More...
 
void RemoveFrame (SourceId source_id, FrameId frame_id)
 Removes the given frame F (indicated by frame_id) from the the given source's registered frames. More...
 
void RemoveGeometry (SourceId source_id, GeometryId geometry_id)
 Removes the given geometry G (indicated by geometry_id) from the the given source's registered geometries. More...
 
System Queries

These methods perform queries on the state of the geometry world including: proximity queries, contact queries, ray-casting queries, and look ups on geometry resources.

These operations require a QueryHandle instance. The caller must acquire one from the GeometrySystem by connecting to the output port that provides GeometryQuery instances.

const std::string & get_source_name (const QueryHandle< T > &handle, SourceId id) const
 Reports the name for the given source id. More...
 
bool SourceIsRegistered (SourceId id) const
 Reports if the given source id is registered. More...
 
FrameId GetFrameId (const QueryHandle< T > &handle, GeometryId geometry_id) const
 Reports the frame to which this geometry is registered. More...
 
std::vector< PenetrationAsPointPair< T > > ComputePenetration (const QueryHandle< T > &handle) const
 Determines penetrations across all pairs of geometries in GeometryWorld. More...
 
- Public Member Functions inherited from LeafSystem< T >
 ~LeafSystem () override
 
std::unique_ptr< CompositeEventCollection< T > > AllocateCompositeEventCollection () const final
 Allocates a CompositeEventCollection object for this system. More...
 
std::unique_ptr< Context< T > > AllocateContext () const override
 Allocates a context, initialized with the correct numbers of concrete input ports and state variables for this System. More...
 
void SetDefaultState (const Context< T > &context, State< T > *state) const override
 Default implementation: sets all continuous and discrete state variables to zero. More...
 
virtual void SetDefaultParameters (const LeafContext< T > &context, Parameters< T > *parameters) const
 Default implementation: sets all numeric parameters to the model vector given to DeclareNumericParameter, or else if no model was provided sets the numeric parameter to one. More...
 
void SetDefaults (Context< T > *context) const final
 
std::unique_ptr< SystemOutput< T > > AllocateOutput (const Context< T > &context) const final
 Returns a container that can hold the values of all of this System's output ports. More...
 
std::unique_ptr< ContinuousState< T > > AllocateTimeDerivatives () const override
 Returns the AllocateContinuousState value, which must not be nullptr. More...
 
std::unique_ptr< DiscreteValues< T > > AllocateDiscreteVariables () const override
 Returns the AllocateDiscreteState value, which must not be nullptr. More...
 
std::multimap< int, intGetDirectFeedthroughs () const final
 Reports all direct feedthroughs from input ports to output ports. More...
 
 LeafSystem (const LeafSystem &)=delete
 
LeafSystemoperator= (const LeafSystem &)=delete
 
 LeafSystem (LeafSystem &&)=delete
 
LeafSystemoperator= (LeafSystem &&)=delete
 
- Public Member Functions inherited from System< T >
virtual ~System ()
 
void GetWitnessFunctions (const Context< T > &context, std::vector< const WitnessFunction< T > * > *w) const
 Gets the witness functions active at the beginning of a continuous time interval. More...
 
EvaluateWitness (const Context< T > &context, const WitnessFunction< T > &witness_func) const
 Evaluates a witness function at the given context. More...
 
std::string GetSystemIdString () const
 Returns a string suitable for identifying this particular System in error messages, when it is a subsystem of a larger Diagram. More...
 
 System (const System &)=delete
 
Systemoperator= (const System &)=delete
 
 System (System &&)=delete
 
Systemoperator= (System &&)=delete
 
std::unique_ptr< BasicVector< T > > AllocateInputVector (const InputPortDescriptor< T > &descriptor) const
 Given a port descriptor, allocates the vector storage. More...
 
std::unique_ptr< AbstractValueAllocateInputAbstract (const InputPortDescriptor< T > &descriptor) const
 Given a port descriptor, allocates the abstract storage. More...
 
std::unique_ptr< Context< T > > CreateDefaultContext () const
 This convenience method allocates a context using AllocateContext() and sets its default values using SetDefaults(). More...
 
void AllocateFreestandingInputs (Context< T > *context) const
 For each input port, allocates a freestanding input of the concrete type that this System requires, and binds it to the port, disconnecting any prior input. More...
 
bool HasAnyDirectFeedthrough () const
 Returns true if any of the inputs to the system might be directly fed through to any of its outputs and false otherwise. More...
 
bool HasDirectFeedthrough (int output_port) const
 Returns true if there might be direct-feedthrough from any input port to the given output_port, and false otherwise. More...
 
bool HasDirectFeedthrough (int input_port, int output_port) const
 Returns true if there might be direct-feedthrough from the given input_port to the given output_port, and false otherwise. More...
 
void Publish (const Context< T > &context, const EventCollection< PublishEvent< T >> &events) const
 This method is the public entry point for dispatching all publish event handlers. More...
 
void Publish (const Context< T > &context) const
 Forces a publish on the system, given a context. More...
 
const T & EvalConservativePower (const Context< T > &context) const
 Returns a reference to the cached value of the conservative power. More...
 
const T & EvalNonConservativePower (const Context< T > &context) const
 Returns a reference to the cached value of the non-conservative power. More...
 
template<template< typename > class Vec = BasicVector>
const Vec< T > * EvalVectorInput (const Context< T > &context, int port_index) const
 Causes the vector-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
Eigen::VectorBlock< const VectorX< T > > EvalEigenVectorInput (const Context< T > &context, int port_index) const
 Causes the vector-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
const AbstractValueEvalAbstractInput (const Context< T > &context, int port_index) const
 Causes the abstract-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
template<typename V >
const V * EvalInputValue (const Context< T > &context, int port_index) const
 Causes the abstract-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
int get_num_constraint_equations (const Context< T > &context) const
 Gets the number of constraint equations for this system using the given context (useful in case the number of constraints is dependent upon the current state (as might be the case with a system modeled using piecewise differential algebraic equations). More...
 
Eigen::VectorXd EvalConstraintEquations (const Context< T > &context) const
 Evaluates the constraint equations for the system at the generalized coordinates and generalized velocity specified by the context. More...
 
Eigen::VectorXd EvalConstraintEquationsDot (const Context< T > &context) const
 Computes the time derivative of each constraint equation, evaluated at the generalized coordinates and generalized velocity specified by the context. More...
 
Eigen::VectorXd CalcVelocityChangeFromConstraintImpulses (const Context< T > &context, const Eigen::MatrixXd &J, const Eigen::VectorXd &lambda) const
 Computes the change in velocity from applying the given constraint forces to the system at the given context. More...
 
double CalcConstraintErrorNorm (const Context< T > &context, const Eigen::VectorXd &error) const
 Computes the norm on constraint error (used as a metric for comparing errors between the outputs of algebraic equations applied to two different state variable instances). More...
 
void CalcTimeDerivatives (const Context< T > &context, ContinuousState< T > *derivatives) const
 Calculates the time derivatives xcdot of the continuous state xc. More...
 
void CalcDiscreteVariableUpdates (const Context< T > &context, const EventCollection< DiscreteUpdateEvent< T >> &events, DiscreteValues< T > *discrete_state) const
 This method is the public entry point for dispatching all discrete variable update event handlers. More...
 
void CalcDiscreteVariableUpdates (const Context< T > &context, DiscreteValues< T > *discrete_state) const
 This method forces a discrete update on the system given a context, and the updated discrete state is stored in discrete_state. More...
 
void CalcUnrestrictedUpdate (const Context< T > &context, const EventCollection< UnrestrictedUpdateEvent< T >> &events, State< T > *state) const
 This method is the public entry point for dispatching all unrestricted update event handlers. More...
 
void CalcUnrestrictedUpdate (const Context< T > &context, State< T > *state) const
 This method forces an unrestricted update on the system given a context, and the updated state is stored in discrete_state. More...
 
CalcNextUpdateTime (const Context< T > &context, CompositeEventCollection< T > *events) const
 This method is called by a Simulator during its calculation of the size of the next continuous step to attempt. More...
 
void GetPerStepEvents (const Context< T > &context, CompositeEventCollection< T > *events) const
 This method is called by Simulator::Initialize() to gather all update and publish events that are to be handled in StepTo() at the point before Simulator integrates continuous state. More...
 
void CalcOutput (const Context< T > &context, SystemOutput< T > *outputs) const
 Utility method that computes for every output port i the value y(i) that should result from the current contents of the given Context. More...
 
CalcPotentialEnergy (const Context< T > &context) const
 Calculates and returns the potential energy current stored in the configuration provided in context. More...
 
CalcKineticEnergy (const Context< T > &context) const
 Calculates and returns the kinetic energy currently present in the motion provided in the given Context. More...
 
CalcConservativePower (const Context< T > &context) const
 Calculates and returns the rate at which mechanical energy is being converted from potential energy to kinetic energy by this system in the given Context. More...
 
CalcNonConservativePower (const Context< T > &context) const
 Calculates and returns the rate at which mechanical energy is being generated (positive) or dissipated (negative) other than by conversion between potential and kinetic energy (in the given Context). More...
 
void MapVelocityToQDot (const Context< T > &context, const VectorBase< T > &generalized_velocity, VectorBase< T > *qdot) const
 Transforms a given generalized velocity v to the time derivative qdot of the generalized configuration q taken from the supplied Context. More...
 
void MapVelocityToQDot (const Context< T > &context, const Eigen::Ref< const VectorX< T >> &generalized_velocity, VectorBase< T > *qdot) const
 Transforms the given generalized velocity to the time derivative of generalized configuration. More...
 
void MapQDotToVelocity (const Context< T > &context, const VectorBase< T > &qdot, VectorBase< T > *generalized_velocity) const
 Transforms the time derivative qdot of the generalized configuration q to generalized velocities v. More...
 
void MapQDotToVelocity (const Context< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, VectorBase< T > *generalized_velocity) const
 Transforms the given time derivative qdot of generalized configuration q to generalized velocity v. More...
 
void set_name (const std::string &name)
 Sets the name of the system. More...
 
std::string get_name () const
 Returns the name last supplied to set_name(), or empty if set_name() was never called. More...
 
std::string GetMemoryObjectName () const
 Returns a name for this System based on a stringification of its type name and memory address. More...
 
void GetPath (std::stringstream *output) const
 Writes the full path of this System in the tree of Systems to output. More...
 
std::string GetPath () const
 
int get_num_input_ports () const
 Returns the number of input ports of the system. More...
 
int get_num_output_ports () const
 Returns the number of output ports of the system. More...
 
const InputPortDescriptor< T > & get_input_port (int port_index) const
 Returns the descriptor of the input port at index port_index. More...
 
const OutputPort< T > & get_output_port (int port_index) const
 Returns the output port at index port_index. More...
 
int get_num_constraints () const
 Returns the number of constraints specified for the system. More...
 
const SystemConstraint< T > & get_constraint (SystemConstraintIndex constraint_index) const
 Returns the constraint at index constraint_index. More...
 
int get_num_total_inputs () const
 Returns the total dimension of all of the input ports (as if they were muxed). More...
 
int get_num_total_outputs () const
 Returns the total dimension of all of the output ports (as if they were muxed). More...
 
void CheckValidOutput (const SystemOutput< T > *output) const
 Checks that output is consistent with the number and size of output ports declared by the system. More...
 
template<typename T1 = T>
void CheckValidContext (const Context< T1 > &context) const
 Checks that context is consistent for this System template. More...
 
VectorX< T > CopyContinuousStateVector (const Context< T > &context) const
 Returns a copy of the continuous state vector xc into an Eigen vector. More...
 
void set_parent (const detail::InputPortEvaluatorInterface< T > *parent)
 Declares that parent is the immediately enclosing Diagram. More...
 
std::string GetGraphvizString () const
 Returns a Graphviz string describing this System. More...
 
int64_t GetGraphvizId () const
 Returns an opaque integer that uniquely identifies this system in the Graphviz output. More...
 
void FixInputPortsFrom (const System< double > &other_system, const Context< double > &other_context, Context< T > *target_context) const
 Fixes all of the input ports in target_context to their current values in other_context, as evaluated by other_system. More...
 
const SystemScalarConverterget_system_scalar_converter () const
 (Advanced) Returns the SystemScalarConverter for this object. More...
 
std::unique_ptr< System< AutoDiffXd > > ToAutoDiffXd () const
 Creates a deep copy of this System, transmogrified to use the autodiff scalar type, with a dynamic-sized vector of partial derivatives. More...
 
std::unique_ptr< System< AutoDiffXd > > ToAutoDiffXdMaybe () const
 Creates a deep copy of this system exactly like ToAutoDiffXd(), but returns nullptr if this System does not support autodiff, instead of throwing an exception. More...
 
std::unique_ptr< System< symbolic::Expression > > ToSymbolic () const
 Creates a deep copy of this System, transmogrified to use the symbolic scalar type. More...
 
std::unique_ptr< System< symbolic::Expression > > ToSymbolicMaybe () const
 Creates a deep copy of this system exactly like ToSymbolic(), but returns nullptr if this System does not support symbolic, instead of throwing an exception. More...
 

Friends

class GeometrySystemTester
 
void DispatchLoadMessage (const GeometrySystem< double > &)
 Dispatches an LCM load message based on the registered geometry. More...
 

Additional Inherited Members

- Static Public Member Functions inherited from System< T >
template<template< typename > class S = ::drake::systems::System>
static std::unique_ptr< S< AutoDiffXd > > ToAutoDiffXd (const S< T > &from)
 Creates a deep copy of from, transmogrified to use the autodiff scalar type, with a dynamic-sized vector of partial derivatives. More...
 
template<template< typename > class S = ::drake::systems::System>
static std::unique_ptr< S< symbolic::Expression > > ToSymbolic (const S< T > &from)
 Creates a deep copy of from, transmogrified to use the symbolic scalar type. More...
 
- Protected Member Functions inherited from LeafSystem< T >
 LeafSystem ()
 Default constructor that declares no inputs, outputs, state, parameters, events, nor scalar-type conversion support (AutoDiff, etc.). More...
 
 LeafSystem (SystemScalarConverter converter)
 Constructor that declares no inputs, outputs, state, parameters, or events, but allows subclasses to declare scalar-type conversion support (AutoDiff, etc.). More...
 
std::unique_ptr< System< AutoDiffXd > > DoToAutoDiffXd () const final
 NVI implementation of ToAutoDiffXdMaybe. More...
 
std::unique_ptr< System< symbolic::Expression > > DoToSymbolic () const final
 NVI implementation of ToSymbolicMaybe. More...
 
DoEvaluateWitness (const Context< T > &context, const WitnessFunction< T > &witness_func) const final
 Derived classes will implement this method to evaluate a witness function at the given context. More...
 
void AddTriggeredWitnessFunctionToCompositeEventCollection (const WitnessFunction< T > &witness_func, CompositeEventCollection< T > *events) const final
 Add witness_func to events. More...
 
void DoCalcNextUpdateTime (const Context< T > &context, CompositeEventCollection< T > *events, T *time) const override
 Computes the next update time based on the configured periodic events, for scalar types that are arithmetic, or aborts for scalar types that are not arithmetic. More...
 
BasicVector< T > * DoAllocateInputVector (const InputPortDescriptor< T > &descriptor) const override
 Allocates a vector that is suitable as an input value for descriptor. More...
 
AbstractValueDoAllocateInputAbstract (const InputPortDescriptor< T > &descriptor) const override
 Allocates an AbstractValue suitable as an input value for descriptor. More...
 
void GetGraphvizFragment (std::stringstream *dot) const override
 Emits a graphviz fragment for this System. More...
 
void GetGraphvizInputPortToken (const InputPortDescriptor< T > &port, std::stringstream *dot) const final
 Appends a fragment to the dot stream identifying the graphviz node representing port. More...
 
void GetGraphvizOutputPortToken (const OutputPort< T > &port, std::stringstream *dot) const final
 Appends a fragment to the dot stream identifying the graphviz node representing port. More...
 
virtual std::unique_ptr< ContinuousState< T > > AllocateContinuousState () const
 Returns a ContinuousState used to implement both CreateDefaultContext and AllocateTimeDerivatives. More...
 
virtual std::unique_ptr< DiscreteValues< T > > AllocateDiscreteState () const
 Reserves the discrete state as required by CreateDefaultContext. More...
 
virtual std::unique_ptr< AbstractValuesAllocateAbstractState () const
 Reserves the abstract state as required by CreateDefaultContext. More...
 
virtual std::unique_ptr< Parameters< T > > AllocateParameters () const
 Reserves the parameters as required by CreateDefaultContext. More...
 
virtual optional< boolDoHasDirectFeedthrough (int input_port, int output_port) const
 Returns true if there is direct-feedthrough from the given input_port to the given output_port, false if there is not direct-feedthrough, or nullopt if unknown (in which case SystemSymbolicInspector will attempt to measure the feedthrough using symbolic form). More...
 
int DeclareNumericParameter (const BasicVector< T > &model_vector)
 Declares a numeric parameter using the given model_vector. More...
 
template<template< typename > class U = BasicVector>
const U< T > & GetNumericParameter (const Context< T > &context, int index) const
 Extracts the numeric parameters of type U from the context at index. More...
 
template<template< typename > class U = BasicVector>
U< T > * GetMutableNumericParameter (Context< T > *context, int index) const
 Extracts the numeric parameters of type U from the context at index. More...
 
template<typename EventType >
void DeclarePeriodicEvent (double period_sec, double offset_sec)
 Declares that this System has a simple, fixed-period event specified with no custom callback function, and its attribute field contains an Event<T>::PeriodicAttribute constructed from the specified period_sec and offset_sec. More...
 
template<typename EventType >
void DeclarePeriodicEvent (double period_sec, double offset_sec, const EventType &event)
 Declares that this System has a simple, fixed-period event specified by event. More...
 
void DeclarePeriodicDiscreteUpdate (double period_sec, double offset_sec=0)
 Declares a periodic discrete update event with period = period_sec and offset = offset_sec. More...
 
void DeclarePeriodicUnrestrictedUpdate (double period_sec, double offset_sec=0)
 Declares a periodic unrestricted update event with period = period_sec and offset = offset_sec. More...
 
void DeclarePeriodicPublish (double period_sec, double offset_sec=0)
 Declares a periodic publish event with period = period_sec and offset = offset_sec. More...
 
template<typename EventType >
void DeclarePerStepEvent (const EventType &event)
 Declares a per-step event using event, which is deep copied (the copy is maintained by this). More...
 
void DeclareContinuousState (int num_state_variables)
 Declares that this System should reserve continuous state with num_state_variables state variables, which have no second-order structure. More...
 
void DeclareContinuousState (int num_q, int num_v, int num_z)
 Declares that this System should reserve continuous state with num_q generalized positions, num_v generalized velocities, and num_z miscellaneous state variables. More...
 
void DeclareContinuousState (const BasicVector< T > &model_vector)
 Declares that this System should reserve continuous state with model_vector.size() miscellaneous state variables, stored in a vector Cloned from model_vector. More...
 
void DeclareContinuousState (const BasicVector< T > &model_vector, int num_q, int num_v, int num_z)
 Declares that this System should reserve continuous state with num_q generalized positions, num_v generalized velocities, and num_z miscellaneous state variables, stored in a vector Cloned from model_vector. More...
 
void DeclareContinuousState (std::unique_ptr< BasicVector< T >> model_vector, int num_q, int num_v, int num_z)
 Declares that this System should reserve continuous state with num_q generalized positions, num_v generalized velocities, and num_z miscellaneous state variables, stored in the a vector Cloned from model_vector. More...
 
void DeclareDiscreteState (int num_state_variables)
 Declares that this System should reserve discrete state with num_state_variables state variables. More...
 
int DeclareAbstractState (std::unique_ptr< AbstractValue > abstract_state)
 Declares an abstract state. More...
 
template<class MySystem >
SystemConstraintIndex DeclareEqualityConstraint (void(MySystem::*calc)(const Context< T > &, VectorX< T > *) const, int count, const std::string &description)
 Declares a system constraint of the form f(context) = 0 by specifying a member function to use to calculate the (VectorX) constraint value with a signature: More...
 
SystemConstraintIndex DeclareEqualityConstraint (typename SystemConstraint< T >::CalcCallback calc, int count, const std::string &description)
 Declares a system constraint of the form f(context) = 0 by specifying a std::function to use to calculate the (Vector) constraint value with a signature: More...
 
template<class MySystem >
SystemConstraintIndex DeclareInequalityConstraint (void(MySystem::*calc)(const Context< T > &, VectorX< T > *) const, int count, const std::string &description)
 Declares a system constraint of the form f(context) ≥ 0 by specifying a member function to use to calculate the (VectorX) constraint value with a signature: More...
 
SystemConstraintIndex DeclareInequalityConstraint (typename SystemConstraint< T >::CalcCallback calc, int count, const std::string &description)
 Declares a system constraint of the form f(context) ≥ 0 by specifying a std::function to use to calculate the (Vector) constraint value with a signature: More...
 
virtual void DoPublish (const Context< T > &context, const std::vector< const PublishEvent< T > * > &events) const
 Derived-class event handler for all simultaneous publish events in events. More...
 
virtual void DoCalcDiscreteVariableUpdates (const Context< T > &context, const std::vector< const DiscreteUpdateEvent< T > * > &events, DiscreteValues< T > *discrete_state) const
 Derived-class event handler for all simultaneous discrete update events. More...
 
virtual void DoCalcUnrestrictedUpdate (const Context< T > &context, const std::vector< const UnrestrictedUpdateEvent< T > * > &events, State< T > *state) const
 Derived-class event handler for all simultaneous unrestricted update events. More...
 
const InputPortDescriptor< T > & DeclareVectorInputPort (const BasicVector< T > &model_vector, optional< RandomDistribution > random_type=nullopt)
 Declares a vector-valued input port using the given model_vector. More...
 
const InputPortDescriptor< T > & DeclareAbstractInputPort (const AbstractValue &model_value)
 Declares an abstract-valued input port using the given model_value. More...
 
template<class MySystem , typename BasicVectorSubtype >
const OutputPort< T > & DeclareVectorOutputPort (const BasicVectorSubtype &model_vector, void(MySystem::*calc)(const Context< T > &, BasicVectorSubtype *) const)
 Declares a vector-valued output port by specifying (1) a model vector of type BasicVectorSubtype derived from BasicVector and initialized to the correct size and desired initial value, and (2) a calculator function that is a class member function (method) with signature: More...
 
template<class MySystem , typename BasicVectorSubtype >
const OutputPort< T > & DeclareVectorOutputPort (void(MySystem::*calc)(const Context< T > &, BasicVectorSubtype *) const)
 Declares a vector-valued output port by specifying only a calculator function that is a class member function (method) with signature: More...
 
const OutputPort< T > & DeclareVectorOutputPort (const BasicVector< T > &model_vector, typename LeafOutputPort< T >::CalcVectorCallback vector_calc_function)
 (Advanced) Declares a vector-valued output port using the given model_vector and a function for calculating the port's value at runtime. More...
 
template<class MySystem , typename OutputType >
const OutputPort< T > & DeclareAbstractOutputPort (const OutputType &model_value, void(MySystem::*calc)(const Context< T > &, OutputType *) const)
 Declares an abstract-valued output port by specifying a model value of concrete type OutputType and a calculator function that is a class member function (method) with signature: More...
 
template<class MySystem , typename OutputType >
const OutputPort< T > & DeclareAbstractOutputPort (void(MySystem::*calc)(const Context< T > &, OutputType *) const)
 Declares an abstract-valued output port by specifying only a calculator function that is a class member function (method) with signature: More...
 
template<class MySystem , typename OutputType >
const OutputPort< T > & DeclareAbstractOutputPort (OutputType(MySystem::*make)(const Context< T > &) const, void(MySystem::*calc)(const Context< T > &, OutputType *) const)
 Declares an abstract-valued output port by specifying member functions to use both for the allocator and calculator. More...
 
template<class MySystem , typename OutputType >
const OutputPort< T > & DeclareAbstractOutputPort (OutputType(MySystem::*make)() const, void(MySystem::*calc)(const Context< T > &, OutputType *) const)
 Declares an abstract-valued output port by specifying member functions to use both for the allocator and calculator. More...
 
const OutputPort< T > & DeclareAbstractOutputPort (typename LeafOutputPort< T >::AllocCallback alloc_function, typename LeafOutputPort< T >::CalcCallback calc_function)
 (Advanced) Declares an abstract-valued output port using the given allocator and calculator functions provided in their most generic forms. More...
 
- Protected Member Functions inherited from System< T >
virtual void DoGetWitnessFunctions (const Context< T > &, std::vector< const WitnessFunction< T > * > *) const
 Derived classes can override this method to provide witness functions active at the beginning of a continuous time interval. More...
 
SystemConstraintIndex AddConstraint (std::unique_ptr< SystemConstraint< T >> constraint)
 Adds an already-created constraint to the list of constraints for this System. More...
 
const EventCollection< PublishEvent< T > > & get_forced_publish_events () const
 
const EventCollection< DiscreteUpdateEvent< T > > & get_forced_discrete_update_events () const
 
const EventCollection< UnrestrictedUpdateEvent< T > > & get_forced_unrestricted_update_events () const
 
void set_forced_publish_events (std::unique_ptr< EventCollection< PublishEvent< T >>> forced)
 
void set_forced_discrete_update_events (std::unique_ptr< EventCollection< DiscreteUpdateEvent< T >>> forced)
 
void set_forced_unrestricted_update_events (std::unique_ptr< EventCollection< UnrestrictedUpdateEvent< T >>> forced)
 
 System (SystemScalarConverter converter)
 Constructs an empty System base class object, possibly supporting scalar-type conversion support (AutoDiff, etc.) using converter. More...
 
const InputPortDescriptor< T > & DeclareInputPort (PortDataType type, int size, optional< RandomDistribution > random_type=nullopt)
 Adds a port with the specified type and size to the input topology. More...
 
const InputPortDescriptor< T > & DeclareAbstractInputPort ()
 Adds an abstract-valued port to the input topology. More...
 
void CreateOutputPort (std::unique_ptr< OutputPort< T >> port)
 Adds an already-created output port to this System. More...
 
virtual void DoCalcTimeDerivatives (const Context< T > &context, ContinuousState< T > *derivatives) const
 Override this if you have any continuous state variables xc in your concrete System to calculate their time derivatives. More...
 
virtual T DoCalcPotentialEnergy (const Context< T > &context) const
 Override this method for physical systems to calculate the potential energy currently stored in the configuration provided in the given Context. More...
 
virtual T DoCalcKineticEnergy (const Context< T > &context) const
 Override this method for physical systems to calculate the kinetic energy currently present in the motion provided in the given Context. More...
 
virtual T DoCalcConservativePower (const Context< T > &context) const
 Override this method to return the rate at which mechanical energy is being converted from potential energy to kinetic energy by this system in the given Context. More...
 
virtual T DoCalcNonConservativePower (const Context< T > &context) const
 Override this method to return the rate at which mechanical energy is being generated (positive) or dissipated (negative) other than by conversion between potential and kinetic energy (in the given Context). More...
 
virtual void DoMapQDotToVelocity (const Context< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, VectorBase< T > *generalized_velocity) const
 Provides the substantive implementation of MapQDotToVelocity(). More...
 
virtual void DoMapVelocityToQDot (const Context< T > &context, const Eigen::Ref< const VectorX< T >> &generalized_velocity, VectorBase< T > *qdot) const
 Provides the substantive implementation of MapVelocityToQDot(). More...
 
virtual int do_get_num_constraint_equations (const Context< T > &context) const
 Gets the number of constraint equations for this system from the given context. More...
 
virtual Eigen::VectorXd DoEvalConstraintEquations (const Context< T > &context) const
 Evaluates the constraint equations for the system at the generalized coordinates and generalized velocity specified by the context. More...
 
virtual Eigen::VectorXd DoEvalConstraintEquationsDot (const Context< T > &context) const
 Computes the time derivative of each constraint equation, evaluated at the generalized coordinates and generalized velocity specified by the context. More...
 
virtual Eigen::VectorXd DoCalcVelocityChangeFromConstraintImpulses (const Context< T > &context, const Eigen::MatrixXd &J, const Eigen::VectorXd &lambda) const
 Computes the change in velocity from applying the given constraint forces to the system at the given context. More...
 
virtual double DoCalcConstraintErrorNorm (const Context< T > &context, const Eigen::VectorXd &error) const
 Computes the norm of the constraint error. More...
 
Eigen::VectorBlock< VectorX< T > > GetMutableOutputVector (SystemOutput< T > *output, int port_index) const
 Returns a mutable Eigen expression for a vector valued output port with index port_index in this system. More...
 
void EvalInputPort (const Context< T > &context, int port_index) const
 Causes an InputPortValue in the context to become up-to-date, delegating to the parent Diagram if necessary. More...
 

Detailed Description

template<typename T>
class drake::geometry::GeometrySystem< T >

GeometrySystem serves as a system-level wrapper for GeometryWorld.

It serves as the nexus for all geometry (and geometry-based operations) in a Diagram. Through GeometrySystem, 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). GeometrySystem 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 GeometrySystem. 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 GeometrySystem.

The basic workflow for interacting with GeometrySystem 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 GeometrySystem input ports.
    • Implement appropriate Calc* methods on the geometry output ports to update geometry pose values.

Inputs

For each registered geometry source, there are two input ports: id and pose. Failing to connect to those ports or providing "bad" values on those ports will cause runtime errors to be thrown. The two ports work in tandem. Through these ports, the upstream source system communicates the poses of all of the frames it has registered with GeometrySystem (see RegisterFrame() for more details).

identifier port: An abstract-valued port containing an instance of FrameIdVector. It should contain the FrameId of each frame registered by the upstream source exactly once. The order of the ids is how the values in the pose port will be interpreted. Use get_source_frame_id_port() to acquire the port for a given source.

pose port: An abstract-valued port containing an instance of FramePoseVector. There should be one pose value for each id in the the identifier port value. The iᵗʰ pose belongs to the iᵗʰ id. Use get_source_pose_port() to acquire the port for a given source.

For source systems, there are some implicit assumptions regarding these input ports. Generally, we assume that the source system already has some logic for computing kinematics of the frames they've registered and an ordered data structure for organizing that data. These input ports rely on that. It is expected that the geometry source will define the frame identifiers in an order which matches the source's internal ordering (and never need to change that output value unless the topology changes). The values of the pose port can then simply be written by copying the ordered data from the internal ordering to the output ordering. This should facilitate translation from internal representation to GeometrySystem representation.

Outputs

GeometrySystem has two output ports:

query port: An abstract-valued port containing an instance of QueryHandle. It provides a "ticket" for downstream LeafSystem instances to perform geometric queries on the GeometrySystem. To perform geometric queries, downstream LeafSystem instances acquire the QueryHandle from GeometrySystem's output port and provide it as a parameter to one of GeometrySystem's query methods (e.g., GeometrySystem::ComputeContact()). This assumes that the querying system has access to a const pointer to the connected GeometrySystem 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.

Working with GeometrySystem

LeafSystem instances can relate to GeometrySystem 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. GeometrySystem serves those queries. As indicated above, in order for a LeafSystem to act as a consumer, it must:

  1. define a QueryHandle-valued input port and connect it to GeometrySystem's corresponding output port, and
  2. have a reference to the connected GeometrySystem instance.

With those two requirements satisfied, a LeafSystem can perform geometry queries by:

  1. evaluating the QueryHandle input port, and
  2. passing the returned handle into the appropriate query method on GeometrySystem (e.g., GeometrySystem::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 GeometrySystem as producers (a.k.a. geometry sources). They do this by acquiring a SourceId (via GeometrySystem::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

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

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

Constructor & Destructor Documentation

GeometrySystem ( const GeometrySystem< T > &  )
delete
GeometrySystem ( GeometrySystem< T > &&  )
delete

Here is the call graph for this function:

~GeometrySystem ( )
inlineoverride

Here is the call graph for this function:

Member Function Documentation

void ClearSource ( SourceId  source_id)

Clears of all the registered frames and geometries from this source, but the source is still registered, allowing future registration of frames and geometries.

Parameters
source_idThe id of the source whose registered elements will be cleared.
Exceptions
std::logic_errorIf the source_id does not map to a registered source or if a context has been allocated.

Here is the caller graph for this function:

std::vector< PenetrationAsPointPair< T > > ComputePenetration ( const QueryHandle< T > &  handle) const

Determines penetrations across all pairs of geometries in GeometryWorld.

Parameters
handleThe QueryHandle produced by evaluating the connected input port on the querying LeafSystem.
Returns
A vector populated with all detected penetrations characterized as point pairs.

Here is the call graph for this function:

Here is the caller graph for this function:

const systems::OutputPort<T>& get_pose_bundle_output_port ( ) const
inline

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

Here is the call graph for this function:

const systems::OutputPort<T>& get_query_output_port ( ) const
inline

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

Here is the call graph for this function:

const systems::InputPortDescriptor< T > & get_source_frame_id_port ( SourceId  id)

Given a valid source id, returns the "frame id" input port associated with that id.

This port's value is an ordered list of frame ids; it is used to provide an interpretation on the pose values provided on the pose port.

Exceptions
std::logic_errorif the source_id is not recognized.

Here is the call graph for this function:

Here is the caller graph for this function:

const std::string & get_source_name ( const QueryHandle< T > &  handle,
SourceId  id 
) const

Reports the name for the given source id.

Parameters
handleThe QueryHandle produced by evaluating the connected input port on the querying LeafSystem.
idThe id of the source to query.

Here is the call graph for this function:

Here is the caller graph for this function:

const systems::InputPortDescriptor< T > & get_source_pose_port ( SourceId  id)

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.

Exceptions
std::logic_errorif the source_id is not recognized.

Here is the call graph for this function:

Here is the caller graph for this function:

FrameId GetFrameId ( const QueryHandle< T > &  handle,
GeometryId  geometry_id 
) const

Reports the frame to which this geometry is registered.

Parameters
handleThe QueryHandle produced by evaluating the connected input port on the querying LeafSystem.

Here is the call graph for this function:

Here is the caller graph for this function:

GeometrySystem& operator= ( GeometrySystem< T > &&  )
delete
GeometrySystem& operator= ( const GeometrySystem< T > &  )
delete
GeometryId RegisterAnchoredGeometry ( SourceId  source_id,
std::unique_ptr< GeometryInstance geometry 
)

Registers a new anchored geometry G for this source.

This hangs geometry G from the world frame (W). Its pose is defined in that frame (i.e., X_WG). Returns the corresponding unique geometry id.

Parameters
source_idThe id for the source registering the frame.
geometryThe anchored geometry G to add to the world.
Returns
The index for the added geometry.
Exceptions
std::logic_errorIf the source_id does not map to a registered source or a context has been allocated.

Here is the caller graph for this function:

FrameId RegisterFrame ( SourceId  source_id,
const GeometryFrame frame 
)

Registers a new frame F on for this source.

This hangs frame F on the world frame (W). Its pose is defined relative to the world frame (i.e, X_WF). Returns the corresponding unique frame id.

Parameters
source_idThe id for the source registering the frame.
frameThe definition of the frame to add.
Returns
A newly allocated frame id.
Exceptions
std::logic_errorIf the source_id does not map to a registered source or if a context has been allocated.

Here is the caller graph for this function:

FrameId RegisterFrame ( SourceId  source_id,
FrameId  parent_id,
const GeometryFrame frame 
)

Registers a new frame F for this source.

This hangs frame F on another previously registered frame P (indicated by parent_id). The pose of the new frame is defined relative to the parent frame (i.e., X_PF). Returns the corresponding unique frame id.

Parameters
source_idThe id for the source registering the frame.
parent_idThe id of the parent frame P.
frameThe frame to register.
Returns
A newly allocated frame id.
Exceptions
std::logic_error1. If the source_id does not map to a registered source,
  1. If the parent_id does not map to a known frame or does not belong to the source, or
  2. a context has been allocated.
GeometryId RegisterGeometry ( SourceId  source_id,
FrameId  frame_id,
std::unique_ptr< GeometryInstance geometry 
)

Registers a new geometry G for this source.

This hangs geometry G on a previously registered frame F (indicated by frame_id). The pose of the geometry is defined in a fixed pose relative to F (i.e., X_FG). Returns the corresponding unique geometry id.

Parameters
source_idThe id for the source registering the geometry.
frame_idThe id for the frame F to hang the geometry on.
geometryThe geometry G to affix to frame F.
Returns
A unique identifier for the added geometry.
Exceptions
std::logic_error1. the source_id does not map to a registered source,
  1. the frame_id doesn't belong to the source,
  2. the geometry is equal to nullptr,
  3. a context has been allocated.

Here is the caller graph for this function:

GeometryId RegisterGeometry ( SourceId  source_id,
GeometryId  geometry_id,
std::unique_ptr< GeometryInstance geometry 
)

Registers a new geometry G for this source.

This hangs geometry G on a previously registered geometry P (indicated by geometry_id). The pose of the geometry is defined in a fixed pose relative to geometry P (i.e., X_PG). By induction, this geometry is effectively rigidly affixed to the frame that P is affixed to. Returns the corresponding unique geometry id.

Parameters
source_idThe id for the source registering the geometry.
geometry_idThe id for the parent geometry P.
geometryThe geometry G to add.
Returns
A unique identifier for the added geometry.
Exceptions
std::logic_error1. the source_id does not map to a registered source,
  1. the geometry_id doesn't belong to the source,
  2. the geometry is equal to nullptr, or
  3. a context has been allocated.
SourceId RegisterSource ( const std::string &  name = "")

Registers a new source to the geometry system (see GeometryWorld for the discussion of "geometry source").

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_frame_id_port() and get_source_pose_port().

Parameters
nameThe optional name of the source. If none is provided (or the empty string) a unique name will be defined by GeometrySystem's logic.
Exceptions
std::logic_errorif a context has already been allocated for this GeometrySystem.
See also
GeometryState::RegisterNewSource()

Here is the call graph for this function:

Here is the caller graph for this function:

void RemoveFrame ( SourceId  source_id,
FrameId  frame_id 
)

Removes the given frame F (indicated by frame_id) from the the given source's registered frames.

All registered geometries connected to this frame will also be removed.

Parameters
source_idThe id for the owner geometry source.
frame_idThe id of the frame to remove.
Exceptions
std::logic_errorIf:
  1. The source_id is not a registered source,
  2. the frame_id doesn't belong to the source, or
  3. a context has been allocated.

Here is the caller graph for this function:

void RemoveGeometry ( SourceId  source_id,
GeometryId  geometry_id 
)

Removes the given geometry G (indicated by geometry_id) from the the given source's registered geometries.

All registered geometries hanging from this geometry will also be removed.

Parameters
source_idThe identifier for the owner geometry source.
geometry_idThe identifier of the geometry to remove.
Exceptions
std::logic_errorIf:
  1. The source_id is not a registered source,
  2. the geometry_id doesn't belong to the source, or
  3. a context has been allocated.

Here is the caller graph for this function:

bool SourceIsRegistered ( SourceId  id) const

Reports if the given source id is registered.

Parameters
idThe id of the source to query.

Here is the caller graph for this function:

Friends And Related Function Documentation

void DispatchLoadMessage ( const GeometrySystem< double > &  )
friend

Dispatches an LCM load message based on the registered geometry.

It should be invoked after registration is complete, but before context allocation.

Parameters
systemThe system whose geometry will be sent in an LCM message.
Exceptions
std::logic_errorif the system has already had its context allocated.
friend class GeometrySystemTester
friend

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