[deformable] Add DeformableBody class

[xuchenhan-tri]

Closes #22999 and #23043.

This PR doesn't come with python bindings for the new class due to its size. The bindings will come in a follow-up PR.

This change is 

[xuchenhan-tri]

-(status: do not review) +@sherm1 for feature review please, thanks!

This closes a couple of issues related to bookkeeping for deformable bodies (see pr description) and is a follow-up to #23033

Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers

[sherm1]

Checkpoint

Reviewed 1 of 8 files at r1, 6 of 29 files at r2, 23 of 26 files at r3, all commit messages.
Reviewable status: 14 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers

---

multibody/plant/deformable_model.cc line 60 at r3 (raw file):

      throw std::logic_error(fmt::format(
          "Model instance '{}' already contains a deformable body "
          "named '{}'. Body names must be unique within a given model.",

BTW consider prepending the error message with the API call that caused it, e.g.
"RegisterDeformableBody(): Model instance ...". That can make it easier for users to figure out where they went wrong.

---

multibody/plant/deformable_model.cc line 233 at r3 (raw file):

  if (indices.empty()) {
    throw std::runtime_error(fmt::format(
        "No deformable body with the given name {} has been registered.",

nit: prepend with GetBodyByName() as below

---

multibody/plant/deformable_model.cc line 252 at r3 (raw file):

  if (indices.empty()) {
    throw std::runtime_error(
        fmt::format("No deformable body with the given name {} in instance {}.",

nit: prepend with GetBodyByName() as above

---

multibody/plant/deformable_model.cc line 367 at r3 (raw file):

void DeformableModel<T>::DoDeclareSystemResources() {
  if constexpr (!std::is_same_v<T, double>) {
    /* A none double DeformableModel is always empty. */

nit: non-double

---

multibody/plant/deformable_model.cc line 382 at r3 (raw file):

      if (!this->plant().is_discrete()) {
        throw std::runtime_error(
            "Deformable body simulation is only supported with discrete time "

BTW consider adding API name to these messages

---

multibody/plant/deformable_model.cc line 487 at r3 (raw file):

    const std::string& name, ModelInstanceIndex model_instance) const {
  /* Use the name lookup for its side-effect of throwing on an invalid index. */
  unused(this->plant().GetModelInstanceName(model_instance));

BTW this use of "unused" surprised me -- I've only seen that used on variables to avoid compiler warnings. My inclination here would be just to write
(void)this->plant().GetModelInstanceName(model_instance);. Fine as is though if you prefer it.

---

multibody/tree/multibody_element.h line 154 at r3 (raw file):

  /// Implementation of the NVI DeclareDiscreteState(). MultibodyElement-derived
  /// objects may override to declare their specific parameters.

typo: parameters -> state variables

---

multibody/plant/deformable_model.h line 305 at r3 (raw file):

      return deformable_bodies_.get_element(index);
    } else {
      /* A none double DeformableModel is always empty. */

typo: "none double" -> "non-double" ?

---

multibody/plant/deformable_model.h line 317 at r3 (raw file):

      return deformable_bodies_.get_element(index);
    } else {
      /* A none double DeformableModel is always empty. */

typo: "none double" -> "non-double" ? (more below)

---

multibody/plant/deformable_model.h line 322 at r3 (raw file):

  }

  /** Returns the deformable body with the given `id`.

BTW consider making the doxygen "brief" distinct from the const version, e.g. "Returns a mutable reference to the deformable body ..."

---

multibody/plant/deformable_model.h line 347 at r3 (raw file):

  /** Returns the DeformableBody with the given name.
   @throws std::exception if there's no body with the given name or if more than
   one model instance contains deformable body with the given name. */

typo: contains -> contains a

---

multibody/plant/deformable_model.h line 352 at r3 (raw file):

  /** Returns the DeformableBody with the given name.
   @throws std::exception if there's no body with the given name or if more than
   one model instance contains deformable body with the given name. */

minor: this comment is copy pasta from the previous one, not right here

---

multibody/plant/deformable_model.h line 480 at r3 (raw file):

      const std::string& name) const;

  /* Returns all body indices with this name *and* in the given model instance.

BTW is it possible for there to be more than one result?

---

multibody/plant/deformable_ids.h line 15 at r3 (raw file):

/** (Internal use only) Indexes deformable bodies, only used after Finalize().
 */
using DeformableBodyIndex = TypeSafeIndex<class DeformableBodyTag>;

minor: this file is empty now -- should get rid of it.

[xuchenhan-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

---

multibody/plant/deformable_ids.h line 15 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this file is empty now -- should get rid of it.

Done. Thanks for catching!

---

multibody/plant/deformable_model.h line 305 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: "none double" -> "non-double" ?

Done

---

multibody/plant/deformable_model.h line 317 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: "none double" -> "non-double" ? (more below)

Done

---

multibody/plant/deformable_model.h line 322 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider making the doxygen "brief" distinct from the const version, e.g. "Returns a mutable reference to the deformable body ..."

Done

---

multibody/plant/deformable_model.h line 347 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: contains -> contains a

Done

---

multibody/plant/deformable_model.h line 352 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this comment is copy pasta from the previous one, not right here

Done

---

multibody/plant/deformable_model.h line 480 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW is it possible for there to be more than one result?

No. I modified the signature to make that more clear.

---

multibody/plant/deformable_model.cc line 60 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider prepending the error message with the API call that caused it, e.g.
"RegisterDeformableBody(): Model instance ...". That can make it easier for users to figure out where they went wrong.

Done

---

multibody/plant/deformable_model.cc line 233 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: prepend with GetBodyByName() as below

Done

---

multibody/plant/deformable_model.cc line 252 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: prepend with GetBodyByName() as above

Done

---

multibody/plant/deformable_model.cc line 367 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: non-double

Done

---

multibody/plant/deformable_model.cc line 382 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider adding API name to these messages

Directly adding the API name here makes less sense because the function is internal. This is invoked as part of MultibodyPlant::Finalize(), but I feel like the error message is clear even without explicit description of when exactly the error is thrown.

I also added early throw conditions in RegisterDeformableBody and in AddExternalForce so we technically won't hit this throw condition unless there's an implementation bug.

---

multibody/plant/deformable_model.cc line 487 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this use of "unused" surprised me -- I've only seen that used on variables to avoid compiler warnings. My inclination here would be just to write
(void)this->plant().GetModelInstanceName(model_instance);. Fine as is though if you prefer it.

I copypasted it from somewhere else in Drake and I actually think it's quite intuitive -- we computed something and didn't use it (because we only need the side effect).

The code compiles just fine without the unused decoration, but I think it pairs nicely with the comment immediately above.

---

multibody/tree/multibody_element.h line 154 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: parameters -> state variables

Done

[sherm1]

Checkpoint 2

Reviewed 1 of 26 files at r3, 3 of 19 files at r4.
Reviewable status: 15 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

---

multibody/tree/deformable_body.h line 77 at r5 (raw file):

  /** Returns all the external forces acting on this deformable body. */
  const std::vector<const ForceDensityFieldBase<T>*>& external_forces() const {

BTW couldn't these be ForceDensityField<T>* ?

---

multibody/tree/deformable_body.h line 81 at r5 (raw file):

  }

  systems::DiscreteStateIndex discrete_state_index() const {

nit: doc missing

---

multibody/tree/deformable_body.h line 85 at r5 (raw file):

  }

  systems::AbstractParameterIndex is_enabled_parameter_index() const {

nit: doc missing

---

multibody/tree/deformable_body.h line 106 at r5 (raw file):

   @pre 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. */

BTW consider being more specific, something like "Roundoff error may cause a point very near the defining plane to be mischaracterized as to which side of the plane it is on."

---

multibody/tree/deformable_body.h line 138 at r5 (raw file):

           tree owning this deformable body.
   @throws std::exception if no constraint is added (i.e. no vertex of the
           deformable body is inside the given `shape` with the given poses). */

BTW just checking: this seems harsh, especially since we don't allow catching exceptions. I would have thought this would be an acceptable outcome that would just say these objects aren't colliding so would be a return code rather than an exception. Is this really an error condition?

---

multibody/tree/deformable_body.h line 143 at r5 (raw file):

      const geometry::Shape& shape, const math::RigidTransform<double>& X_BG);

  /** Returns true if and only if this deformable body is under any fixed

BTW "if and only if" seems redundant, like saying "true if and false otherwise". How about just "if"?

---

multibody/tree/deformable_body.h line 147 at r5 (raw file):

  bool has_fixed_constraint() const { return !fixed_constraint_specs_.empty(); }

  /** (Internal use only) Returns a reference to the all fixed constraints

typo: "the all" -> "the"

---

multibody/tree/deformable_body.h line 148 at r5 (raw file):

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

minor: no "id" in evidence

Suggestion:

this deformable body

---

multibody/tree/deformable_body.h line 169 at r5 (raw file):

                    const Eigen::Ref<const Matrix3X<T>>& q) const;

  /** Returns the matrix of vertex positions for this deformable body in the

BTW consider "Returns" -> "Copies out" to emphasize that isn't just a reference.

---

multibody/tree/deformable_body.h line 180 at r5 (raw file):

  Matrix3X<T> GetPositions(const systems::Context<T>& context) const;

  /** @return true if and only if this deformable body is enabled.

BTW "if and only if" -> "if"

---

multibody/tree/deformable_body.h line 258 at r5 (raw file):

  */
  void SetExternalForces(
      const std::vector<std::unique_ptr<ForceDensityFieldBase<T>>>&

BTW ForceDensityField<T> ?

---

multibody/tree/deformable_body.h line 270 at r5 (raw file):

  BuildLinearVolumetricModel(const geometry::VolumeMesh<double>& mesh,
                             const fem::DeformableBodyConfig<T>& config,
                             const Vector3<T>& weights);

BTW could the T's here and below be double?

[xuchenhan-tri]

Reviewable status: 6 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

---

multibody/tree/deformable_body.h line 77 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW couldn't these be ForceDensityField<T>* ?

The callsite (inside fem module that the tree module depends on) need the pointer to base to avoid dependency cycles.

Resolved in f2f.

---

multibody/tree/deformable_body.h line 81 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: doc missing

Done

---

multibody/tree/deformable_body.h line 85 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: doc missing

Done

---

multibody/tree/deformable_body.h line 106 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider being more specific, something like "Roundoff error may cause a point very near the defining plane to be mischaracterized as to which side of the plane it is on."

Done

---

multibody/tree/deformable_body.h line 138 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW just checking: this seems harsh, especially since we don't allow catching exceptions. I would have thought this would be an acceptable outcome that would just say these objects aren't colliding so would be a return code rather than an exception. Is this really an error condition?

I have a TODO in the cc file related to this. However, I don't think it's completely unreasonable to throw -- when someone calls AddFixedConstraint, in many cases, they'd expect some constraint to be added. So when a constraint is not added, it may be the case that the wrong pose is provided. (It's very easy to get the pose wrong; It happened to me many times).

OTOH, I could imagine a use case where people just specify a bunch of constraints without thinking about it and are satisfied as long as the valid ones are registered, and thus the TODO.

---

multibody/tree/deformable_body.h line 143 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW "if and only if" seems redundant, like saying "true if and false otherwise". How about just "if"?

Done

---

multibody/tree/deformable_body.h line 147 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: "the all" -> "the"

Done

---

multibody/tree/deformable_body.h line 148 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: no "id" in evidence

Done

---

multibody/tree/deformable_body.h line 169 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider "Returns" -> "Copies out" to emphasize that isn't just a reference.

Done

---

multibody/tree/deformable_body.h line 180 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW "if and only if" -> "if"

Done

---

multibody/tree/deformable_body.h line 258 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW ForceDensityField<T> ?

I believe this can technically be ForceDensityField, but it'd require casting things around, so I don't really see the benefit.

Resolved in f2f.

---

multibody/tree/deformable_body.h line 270 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW could the T's here and below be double?

Done

[sherm1]

Feature . This is a big step towards productizing deformable bodies!
+@sammy-tri for platform review per (Monday) rotation, please (Xuchen is tomorrow's platform reviewer)

Reviewed 1 of 26 files at r3, 15 of 19 files at r4, 1 of 1 files at r5, 2 of 2 files at r6, all commit messages.
Reviewable status: 12 unresolved discussions, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

---

multibody/tree/deformable_body.cc line 29 at r5 (raw file):

                                                 const Vector3<T>& n_W) const {
  DRAKE_THROW_UNLESS(n_W.norm() > 1e-10);
  const Vector3<T>& nhat_W = n_W.normalized();

nit: not a reference

Suggestion:

Vector3<T>

---

multibody/tree/deformable_body.cc line 34 at r5 (raw file):

  constexpr int kDim = 3;
  auto is_inside_wall = [&p_WQ, &nhat_W](const Vector3<T>& p_WV) {
    T distance_to_wall = (p_WV - p_WQ).dot(nhat_W);

BTW can be const

---

multibody/tree/deformable_body.cc line 54 at r5 (raw file):

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

BTW consider shape_G so the X_BG pose meaning is clear

---

multibody/tree/deformable_body.cc line 57 at r5 (raw file):

  if (&this->get_parent_tree().get_body(body_B.index()) != &body_B) {
    throw std::logic_error(
        fmt::format("The rigid body with name {} is not registered with the "

BTW consider prepending the API name to the error message

---

multibody/tree/deformable_body.cc line 84 at r5 (raw file):

       ++vertex_index) {
    /* The vertex position in the deformable body's geometry frame. */
    const Vector3<double>& p_APi =

minor: looks like this should be p_GPi ?

---

multibody/tree/deformable_body.cc line 101 at r5 (raw file):

  if (spec.vertices.size() == 0) {
    throw std::runtime_error(
        fmt::format("No constraint has been added between deformable body with "

BTW consider prepending API name

---

multibody/tree/deformable_body.cc line 119 at r5 (raw file):

  DRAKE_THROW_UNLESS(q.cols() == num_nodes);
  auto all_finite = [](const Matrix3X<T>& positions) {
    return (positions.array().isFinite().all());

nit: redundant outer parentheses

---

multibody/tree/deformable_body.cc line 152 at r5 (raw file):

  this->GetParentTreeSystem().ValidateContext(*context);
  context->get_mutable_abstract_parameter(is_enabled_parameter_index_)
      .set_value(false);

BTW amazing that works! Might be clearer to put the <bool> template argument there explicitly.

---

multibody/tree/deformable_body.h line 236 at r6 (raw file):

                 const geometry::VolumeMesh<double>& mesh_G,
                 const math::RigidTransform<double>& X_WG,
                 const fem::DeformableBodyConfig<T>& config,

BTW should this T -> double also ?

---

multibody/tree/deformable_body.h line 272 at r6 (raw file):

  typename std::enable_if_t<std::is_same_v<T1, double>, void>
  BuildLinearVolumetricModel(const geometry::VolumeMesh<double>& mesh,
                             const fem::DeformableBodyConfig<T>& config,

BTW should this T -> double also ?

---

multibody/tree/test/deformable_body_test.cc line 28 at r3 (raw file):

using systems::DiagramBuilder;

/** Builds a plant with exactly one deformable body, then finalizes and

nit

Suggestion:

/*

---

multibody/tree/test/deformable_body_test.cc line 32 at r3 (raw file):

 APIs are already tested through the DeformableModel tests (e.g. adding
 constraints, boundary conditions, enabling/disabling a body, etc), and here we
 only test the APIs not fully covered in DeformableModel tests. */

BTW just checking: can you confirm that these are tested:

• fem_model()
• external_forces()
• discrete_state_index()
• is_enabled_parameter_index()

[xuchenhan-tri]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform)

---

multibody/tree/test/deformable_body_test.cc line 28 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit

Done

---

multibody/tree/test/deformable_body_test.cc line 32 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW just checking: can you confirm that these are tested:

• fem_model()
• external_forces()
• discrete_state_index()
• is_enabled_parameter_index()

Done

These are covered in deformable_model_test.cc, but it's nice to cover them locally with more direct tests.

---

multibody/tree/deformable_body.h line 236 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW should this T -> double also ?

My considerations for T vs. double here is "is it conceivable at all that this argument might be autodiff sometime in the future"? To me the answer for config is "yes" (e.g. for parameter optimization).

---

multibody/tree/deformable_body.cc line 29 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: not a reference

Done

---

multibody/tree/deformable_body.cc line 34 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW can be const

Done

---

multibody/tree/deformable_body.cc line 54 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider shape_G so the X_BG pose meaning is clear

Done

---

multibody/tree/deformable_body.cc line 57 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider prepending the API name to the error message

Done

---

multibody/tree/deformable_body.cc line 84 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: looks like this should be p_GPi ?

No, but the naming is definitely unfortunate. I added some explanations; hope it helps.

---

multibody/tree/deformable_body.cc line 101 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider prepending API name

Done

---

multibody/tree/deformable_body.cc line 119 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: redundant outer parentheses

Done

---

multibody/tree/deformable_body.cc line 152 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW amazing that works! Might be clearer to put the <bool> template argument there explicitly.

Done

[sherm1]

Reviewed 6 of 6 files at r7, all commit messages.
Reviewable status: LGTM missing from assignee sammy-tri(platform)

[xuchenhan-tri]

+(priority: high) I have a follow-up PR after this one that I'll need soon.

Reviewable status: LGTM missing from assignee sammy-tri(platform)

[sammy-tri]

Reviewed 3 of 29 files at r2, 4 of 26 files at r3, 3 of 19 files at r4, 1 of 6 files at r7, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform)

---

multibody/plant/constraint_specs.h line 103 at r7 (raw file):

struct DeformableRigidFixedConstraintSpec {
  bool operator==(const DeformableRigidFixedConstraintSpec&) const = default;
  DeformableBodyId body_A;    // Index of the deformable body A.

nit, prexisting: type is DeformableBodyId and comment says index

---

multibody/tree/multibody_tree_indexes.h line 54 at r7 (raw file):

/// Type used to identify a deformable body by id within a multibody plant.
using DeformableBodyId = Identifier<class DeformableBodyTag>;

The 'DeformableBodyId' vs 'DeformableBodyIndex` is not immediately clear to me. I am at the start of reviewing this PR but I think some more explanation would be useful (I left a nit one place where the terms are mixed)

[xuchenhan-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform)

---

multibody/plant/constraint_specs.h line 103 at r7 (raw file):

Previously, sammy-tri (Sam Creasey) wrote…

nit, prexisting: type is DeformableBodyId and comment says index

Done

---

multibody/tree/multibody_tree_indexes.h line 54 at r7 (raw file):

Previously, sammy-tri (Sam Creasey) wrote…

The 'DeformableBodyId' vs 'DeformableBodyIndex` is not immediately clear to me. I am at the start of reviewing this PR but I think some more explanation would be useful (I left a nit one place where the terms are mixed)

This is mostly a historical artifact.

I added a TODO to explain.

[sammy-tri]

Reviewed 1 of 8 files at r1, 2 of 29 files at r2, 2 of 26 files at r3, 2 of 6 files at r7, 2 of 2 files at r8, all commit messages.
Reviewable status: LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

---

multibody/tree/multibody_tree_indexes.h line 54 at r7 (raw file):

Previously, xuchenhan-tri wrote…

This is mostly a historical artifact.

I added a TODO to explain.

Thanks!

[sammy-tri]

Reviewed 12 of 19 files at r4, 3 of 6 files at r7.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

[sammy-tri]

+(status: squashing now)

Reviewable status: complete! all discussions resolved, LGTM from assignees sammy-tri(platform),sherm1(platform)