Drake
Drake C++ Documentation
DeformableModel< T > Class Template Referencefinal

Detailed Description

template<typename T>
class drake::multibody::DeformableModel< T >

DeformableModel implements the interface in PhysicalModel and provides the functionalities to specify deformable bodies.

Unlike rigid bodies, the shape of deformable bodies can change in a simulation. Each deformable body is modeled as a volumetric mesh with persisting topology, changing vertex positions, and an approximated signed distance field. A finite element model is built for each registered deformable body that is used to evaluate the dynamics of the body.

Warning
This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.
Template Parameters
TThe scalar type, which must be double.

#include <drake/multibody/plant/physical_model.h>

Public Member Functions

 DeformableModel (MultibodyPlant< T > *plant)
 (Internal only) Constructs a DeformableModel to be owned by the given MultibodyPlant. More...
 
 ~DeformableModel () final
 
int num_bodies () const
 Returns the number of deformable bodies registered with this DeformableModel. More...
 
DeformableBodyId RegisterDeformableBody (std::unique_ptr< geometry::GeometryInstance > geometry_instance, const fem::DeformableBodyConfig< T > &config, double resolution_hint)
 Registers a deformable body in this DeformableModel with the given GeometryInstance. More...
 
void SetWallBoundaryCondition (DeformableBodyId id, const Vector3< T > &p_WQ, const Vector3< T > &n_W)
 Sets wall boundary conditions for the body with the given id. More...
 
MultibodyConstraintId AddFixedConstraint (DeformableBodyId body_A_id, const RigidBody< T > &body_B, const math::RigidTransform< double > &X_BA, const geometry::Shape &shape, const math::RigidTransform< double > &X_BG)
 Defines a fixed constraint between a deformable body A and a rigid body B. More...
 
systems::DiscreteStateIndex GetDiscreteStateIndex (DeformableBodyId id) const
 Returns the discrete state index of the deformable body identified by the given id. More...
 
void AddExternalForce (std::unique_ptr< ForceDensityField< T >> external_force)
 Registers an external force density field that applies external force to all deformable bodies. More...
 
const std::vector< const ForceDensityField< T > * > & GetExternalForces (DeformableBodyId id) const
 Returns the force density fields acting on the deformable body with the given id. More...
 
const fem::FemModel< T > & GetFemModel (DeformableBodyId id) const
 Returns the FemModel for the body with id. More...
 
const VectorX< T > & GetReferencePositions (DeformableBodyId id) const
 Returns the reference positions of the vertices of the deformable body identified by the given id. More...
 
DeformableBodyId GetBodyId (DeformableBodyIndex index) const
 Returns the DeformableBodyId of the body with the given body index. More...
 
DeformableBodyIndex GetBodyIndex (DeformableBodyId id) const
 (Internal) Returns the DeformableBodyIndex of the body with the given id. More...
 
geometry::GeometryId GetGeometryId (DeformableBodyId id) const
 Returns the GeometryId of the geometry associated with the body with the given id. More...
 
DeformableBodyId GetBodyId (geometry::GeometryId geometry_id) const
 Returns the DeformableBodyId associated with the given geometry_id. More...
 
bool HasConstraint (DeformableBodyId id) const
 (Internal use only) Returns the true iff the deformable body with the given id has constraints associated with it. More...
 
const internal::DeformableRigidFixedConstraintSpec & fixed_constraint_spec (MultibodyConstraintId id) const
 (Internal use only) Returns the fixed constraint specification corresponding to the given id. More...
 
const std::vector< MultibodyConstraintId > & fixed_constraint_ids (DeformableBodyId id) const
 (Internal use only) Returns a reference to the all ids of fixed constraints registered with the deformable body with the given id. More...
 
systems::OutputPortIndex configuration_output_port_index () const
 Returns the output port index of the vertex positions port for all registered deformable bodies. More...
 
bool is_empty () const
 Returns true if there's no deformable body or external force registered to this DeformableModel. More...
 
bool is_cloneable_to_double () const final
 
bool is_cloneable_to_autodiff () const final
 Returns true if and only if this DeformableModel is empty. More...
 
bool is_cloneable_to_symbolic () const final
 Returns true if and only if this DeformableModel is empty. More...
 
Does not allow copy, move, or assignment
 DeformableModel (const DeformableModel &)=delete
 
DeformableModeloperator= (const DeformableModel &)=delete
 
 DeformableModel (DeformableModel &&)=delete
 
DeformableModeloperator= (DeformableModel &&)=delete
 
- Public Member Functions inherited from PhysicalModel< T >
 PhysicalModel (MultibodyPlant< T > *owning_plant)
 Constructs a PhysicalModel owned by the given owning_plant. More...
 
 ~PhysicalModel () override
 
template<typename ScalarType >
std::unique_ptr< PhysicalModel< ScalarType > > CloneToScalar (MultibodyPlant< ScalarType > *plant) const
 (Internal only) Creates a clone of this concrete PhysicalModel object with the scalar type ScalarType to be owned by the given plant. More...
 
bool is_cloneable_to_double () const override
 Defaults to false. More...
 
bool is_cloneable_to_autodiff () const override
 Defaults to false. More...
 
bool is_cloneable_to_symbolic () const override
 Defaults to false. More...
 
void DeclareSystemResources ()
 (Internal only) MultibodyPlant calls this from within Finalize() to declare additional system resources. More...
 
void DeclareSceneGraphPorts ()
 (Internal only) Declares zero or more output ports in the owning MultibodyPlant to communicate with a SceneGraph. More...
 
PhysicalModelPointerVariant< T > ToPhysicalModelPointerVariant () const
 Returns (a const pointer to) the specific model variant of this PhysicalModel. More...
 
const MultibodyPlant< T > * plant () const
 
 PhysicalModel (const PhysicalModel &)=delete
 
PhysicalModeloperator= (const PhysicalModel &)=delete
 
 PhysicalModel (PhysicalModel &&)=delete
 
PhysicalModeloperator= (PhysicalModel &&)=delete
 

Friends

template<typename U >
class DeformableModel
 

Additional Inherited Members

- Protected Member Functions inherited from PhysicalModel< T >
MultibodyPlant< T > * mutable_plant ()
 
void ThrowIfSystemResourcesDeclared (const char *function_name) const
 
void ThrowIfSystemResourcesNotDeclared (const char *function_name) const
 
geometry::SceneGraph< T > & mutable_scene_graph ()
 
systems::DiscreteStateIndex DeclareDiscreteState (const VectorX< T > &model_value)
 
systems::LeafOutputPort< T > & DeclareAbstractOutputPort (std::string name, typename systems::LeafOutputPort< T >::AllocCallback alloc_function, typename systems::LeafOutputPort< T >::CalcCallback calc_function, std::set< systems::DependencyTicket > prerequisites_of_calc={ systems::System< T >::all_sources_ticket()})
 
systems::LeafOutputPort< T > & DeclareVectorOutputPort (std::string name, const systems::BasicVector< T > &model_vector, typename systems::LeafOutputPort< T >::CalcVectorCallback vector_calc_function, std::set< systems::DependencyTicket > prerequisites_of_calc={ systems::System< T >::all_sources_ticket()})
 

Constructor & Destructor Documentation

◆ DeformableModel() [1/3]

DeformableModel ( const DeformableModel< T > &  )
delete

◆ DeformableModel() [2/3]

DeformableModel ( DeformableModel< T > &&  )
delete

◆ DeformableModel() [3/3]

DeformableModel ( MultibodyPlant< T > *  plant)
explicit

(Internal only) Constructs a DeformableModel to be owned by the given MultibodyPlant.

This constructor is only intended to be called internally by MultibodyPlant.

Precondition
plant != nullptr.
Finalize() has not been called on plant.

◆ ~DeformableModel()

~DeformableModel ( )
final

Member Function Documentation

◆ AddExternalForce()

void AddExternalForce ( std::unique_ptr< ForceDensityField< T >>  external_force)

Registers an external force density field that applies external force to all deformable bodies.

Exceptions
std::exceptionif this DeformableModel is not of scalar type double.
std::exceptionif Finalize() has been called on the multibody plant owning this deformable model.

◆ AddFixedConstraint()

MultibodyConstraintId AddFixedConstraint ( DeformableBodyId  body_A_id,
const RigidBody< T > &  body_B,
const math::RigidTransform< double > &  X_BA,
const geometry::Shape shape,
const math::RigidTransform< double > &  X_BG 
)

Defines a fixed constraint between a deformable body A and a rigid body B.

Such a fixed constraint is modeled as distance holonomic constraints:

p_PᵢQᵢ(q) = 0 for each constrained vertex Pᵢ

where Pᵢ is the i-th vertex of the deformable body under constraint and Qᵢ is a point rigidly affixed to the rigid body B. To specify the constraint, we put the reference mesh M of the deformable body A in B's body frame with the given pose X_BA and prescribe a shape G with pose X_BG in B's body frame. All vertices Pᵢ in M that are inside (or on the surface of) G are subject to the fixed constraints with Qᵢ being coincident with Pᵢ when M is in pose X_BA. p_PᵢQᵢ(q) denotes the relative position of point Qᵢ with respect to point Pᵢ as a function of the configuration of the model q. Imposing this constraint forces Pᵢ and Qᵢ to be coincident for each vertex i of the deformable body specified to be under constraint.

Parameters
[in]body_A_idThe unique id of the deformable body under the fixed constraint.
[in]body_BThe rigid body under constraint.
[in]X_BAThe pose of deformable body A's reference mesh in B's body frame
[in]shapeThe prescribed geometry shape, attached to rigid body B, used to determine which vertices of the deformable body A is under constraint.
[in]X_BGThe fixed pose of the geometry frame of the given shape in body B's frame.
Returns
the unique id of the newly added constraint.
Exceptions
std::exceptionif no deformable body with the given body_A_id has been registered.
std::exceptionunless body_B is registered with the same multibody plant owning this deformable model.
std::exceptionif shape is not supported by QueryObject::ComputeSignedDistanceToPoint(). Currently, supported shapes include Box, Capsule, Cylinder, Ellipsoid, HalfSpace, and Sphere.
std::exceptionif Finalize() has been called on the multibody plant owning this deformable model.
std::exceptionif this DeformableModel is not of scalar type double.
std::exceptionif no constraint is added (i.e. no vertex of the deformable body is inside the given shape with the given poses).

◆ configuration_output_port_index()

systems::OutputPortIndex configuration_output_port_index ( ) const

Returns the output port index of the vertex positions port for all registered deformable bodies.

Exceptions
std::exceptionif called before DeclareSceneGraphPorts() is called.

◆ fixed_constraint_ids()

const std::vector<MultibodyConstraintId>& fixed_constraint_ids ( DeformableBodyId  id) const

(Internal use only) Returns a reference to the all ids of fixed constraints registered with the deformable body with the given id.

◆ fixed_constraint_spec()

const internal::DeformableRigidFixedConstraintSpec& fixed_constraint_spec ( MultibodyConstraintId  id) const

(Internal use only) Returns the fixed constraint specification corresponding to the given id.

Exceptions
ifid is not a valid identifier for a fixed constraint.

◆ GetBodyId() [1/2]

DeformableBodyId GetBodyId ( DeformableBodyIndex  index) const

Returns the DeformableBodyId of the body with the given body index.

Exceptions
std::exceptionif MultibodyPlant::Finalize() has not been called yet or if index is larger than or equal to the total number of registered deformable bodies.

◆ GetBodyId() [2/2]

DeformableBodyId GetBodyId ( geometry::GeometryId  geometry_id) const

Returns the DeformableBodyId associated with the given geometry_id.

Exceptions
std::exceptionif the given geometry_id does not correspond to a deformable body registered with this model.

◆ GetBodyIndex()

DeformableBodyIndex GetBodyIndex ( DeformableBodyId  id) const

(Internal) Returns the DeformableBodyIndex of the body with the given id.

This function is for internal bookkeeping use only. Most users should use DeformableBodyId instead.

Exceptions
std::exceptionif MultibodyPlant::Finalize() has not been called yet or if no body with the given id has been registered.

◆ GetDiscreteStateIndex()

systems::DiscreteStateIndex GetDiscreteStateIndex ( DeformableBodyId  id) const

Returns the discrete state index of the deformable body identified by the given id.

Exceptions
std::exceptionif MultibodyPlant::Finalize() has not been called yet. or if no deformable body with the given id has been registered in this model.

◆ GetExternalForces()

const std::vector<const ForceDensityField<T>*>& GetExternalForces ( DeformableBodyId  id) const

Returns the force density fields acting on the deformable body with the given id.

Exceptions
std::exceptionif MultibodyPlant::Finalize() has not been called yet. or if no deformable body with the given id has been registered in this model.

◆ GetFemModel()

const fem::FemModel<T>& GetFemModel ( DeformableBodyId  id) const

Returns the FemModel for the body with id.

Exceptions
exceptionif no deformable body with id is registered with this DeformableModel.

◆ GetGeometryId()

geometry::GeometryId GetGeometryId ( DeformableBodyId  id) const

Returns the GeometryId of the geometry associated with the body with the given id.

Exceptions
std::exceptionif no body with the given id has been registered.

◆ GetReferencePositions()

const VectorX<T>& GetReferencePositions ( DeformableBodyId  id) const

Returns the reference positions of the vertices of the deformable body identified by the given id.

The reference positions are represented as a VectorX with 3N values where N is the number of vertices. The x-, y-, and z-positions (measured and expressed in the world frame) of the j-th vertex are 3j, 3j + 1, and 3j + 2 in the VectorX.

Exceptions
std::exceptionif no deformable body with the given id has been registered in this model.

◆ HasConstraint()

bool HasConstraint ( DeformableBodyId  id) const

(Internal use only) Returns the true iff the deformable body with the given id has constraints associated with it.

◆ is_cloneable_to_autodiff()

bool is_cloneable_to_autodiff ( ) const
final

Returns true if and only if this DeformableModel is empty.

◆ is_cloneable_to_double()

bool is_cloneable_to_double ( ) const
final

◆ is_cloneable_to_symbolic()

bool is_cloneable_to_symbolic ( ) const
final

Returns true if and only if this DeformableModel is empty.

◆ is_empty()

bool is_empty ( ) const

Returns true if there's no deformable body or external force registered to this DeformableModel.

◆ num_bodies()

int num_bodies ( ) const

Returns the number of deformable bodies registered with this DeformableModel.

◆ operator=() [1/2]

DeformableModel& operator= ( DeformableModel< T > &&  )
delete

◆ operator=() [2/2]

DeformableModel& operator= ( const DeformableModel< T > &  )
delete

◆ RegisterDeformableBody()

DeformableBodyId RegisterDeformableBody ( std::unique_ptr< geometry::GeometryInstance geometry_instance,
const fem::DeformableBodyConfig< T > &  config,
double  resolution_hint 
)

Registers a deformable body in this DeformableModel with the given GeometryInstance.

The body is represented in the world frame and simulated with FEM with linear elements and a first order quadrature rule that integrates linear functions exactly. See FemModel for details. Returns a unique identifier for the added geometry.

Parameters
[in]geometry_instanceThe geometry to be registered with the model.
[in]configThe physical properties of deformable body.
[in]resolution_hintThe parameter that guides the level of mesh refinement of the deformable geometry. It has length units (in meters) and roughly corresponds to a typical edge length in the resulting mesh for a primitive shape.
Precondition
resolution_hint > 0.
Exceptions
std::exceptionif this DeformableModel is not of scalar type double.
std::exceptionif Finalize() has been called on the multibody plant owning this deformable model.

◆ SetWallBoundaryCondition()

void SetWallBoundaryCondition ( DeformableBodyId  id,
const Vector3< T > &  p_WQ,
const Vector3< T > &  n_W 
)

Sets wall boundary conditions for the body with the given id.

All vertices of the mesh of the deformable body whose reference positions are inside the prescribed open half space are put under zero displacement boundary conditions. The open half space is defined by a plane with outward normal n_W. A vertex V is considered to be subject to the boundary condition if n̂ ⋅ p_QV < 0 where Q is a point on the plane and n̂ is normalized n_W.

Parameters
[in]idThe body to be put under boundary condition.
[in]p_WQThe position of a point Q on the plane in the world frame.
[in]n_WOutward normal to the half space expressed in the world frame.
Precondition
n_W.norm() > 1e-10.
Warning
Be aware of round-off errors in floating computations when placing a vertex very close to the plane defining the half space.
Exceptions
std::exceptionif Finalize() has been called on the multibody plant owning this deformable model or if no deformable body with the given id has been registered in this model.

Friends And Related Function Documentation

◆ DeformableModel

friend class DeformableModel
friend

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