Support deformable free body pose

[xuchenhan-tri]

This change is 

[xuchenhan-tri]

+@amcastro-tri for feature review please

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

[amcastro-tri]

Thanks Xuchen. See coments below, I am confused with the terminology/definitions below. That is, there seems to be a geometry frame G and a deformable frame D. To me they could be the same? otherwise there might be a piece of documentation missing.

Reviewed 9 of 9 files at r1.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/tree/deformable_body.h line 217 at r1 (raw file):

   @param[in] X_WD The default pose of the simulated geometry in the
   world frame W. */
  void set_default_pose(const math::RigidTransform<double>& X_WD) {

minor, worth documenting this default pose is identity if never set.

---

multibody/tree/deformable_body.h line 319 at r1 (raw file):

  /* The default pose of the deformable geometry (in its reference
   configuration) in the world frame. */
  math::RigidTransform<double> X_WD_;

still going throug the rest, but not clear to me why we need two separate transforms, X_WG and X_WD. Is there a reason to allow G != D?

---

multibody/tree/deformable_body.cc line 329 at r1 (raw file):

  for (int i = 0; i < num_dofs / 3; ++i) {
    model_state.template segment<3>(3 * i) =
        X_GD * reference_positions_.template segment<3>(3 * i);

so this says that the state is stored in the geometry frame G? is that correct?

---

multibody/parsing/model_directives.h line 77 at r1 (raw file):

  ///
  /// For deformable bodies, the body pose defines the pose of the simulated
  /// geometry in its reference configuration.

consider this change if correct,

Suggestion:

  /// For deformable bodies, the body pose defines the world pose of the rerence configuration.
  /// I.e., the undeformed geometry of the deformable body will have this pose in the world frame.

[xuchenhan-tri]

FWIW, the D stands for "default", not "deformable".

Yes, they could be the same, and they are the same if set_default_pose is never set, but the point of this PR is to allow them to be different if someone wants a different default pose than specified at registration time.

Reviewable status: 4 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/parsing/model_directives.h line 77 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

consider this change if correct,

Done.

---

multibody/tree/deformable_body.h line 217 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor, worth documenting this default pose is identity if never set.

I already have a note related to this in the getter.

---

multibody/tree/deformable_body.h line 319 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

still going throug the rest, but not clear to me why we need two separate transforms, X_WG and X_WD. Is there a reason to allow G != D?

The goal of this PR is to support D != G, i.e., so that the default pose is different from the pose of the geometry at registration time. This allows the dmd workflow that registers the deformable geometry and then later chooses a potentially different initial pose of the body.

---

multibody/tree/deformable_body.cc line 329 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

so this says that the state is stored in the geometry frame G? is that correct?

No. reference_positions_ stores the reference geometry's vertex positions measured and expressed in the world frame as registered with a pose X_WG. X_GD*reference_positions_ converts it back to the geometry frame G and then pose it in the world with a new pose X_WD.

[amcastro-tri]

Right, see below. I am still getting a different result.

Reviewed 1 of 1 files at r2.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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 319 at r1 (raw file):

Previously, xuchenhan-tri wrote…

The goal of this PR is to support D != G, i.e., so that the default pose is different from the pose of the geometry at registration time. This allows the dmd workflow that registers the deformable geometry and then later chooses a potentially different initial pose of the body.

I see, makes sense. Thanks.

---

multibody/tree/deformable_body.cc line 329 at r1 (raw file):

Previously, xuchenhan-tri wrote…

No. reference_positions_ stores the reference geometry's vertex positions measured and expressed in the world frame as registered with a pose X_WG. X_GD*reference_positions_ converts it back to the geometry frame G and then pose it in the world with a new pose X_WD.

oh, I see, right. However, I believe the pose X_GD is not correct. Lets see where I got it wrong.

I am going to introduce some notation to avoid getting confused (hopefully). I will introduce the "reference frame" R, and I define it such that the pose of R at registration in the world is X_WG. I believe, if I'm correct, that what you want, is that the "new pose" of R is X_WD. That is, X_WG is the pose of the reference frame R at registration and I want it now to have a new default pose X_WD.

The invariant here is that the positions p_RQ of points Q of the deformable body in R are constant, in the reference confuguration (before deformation) regardless of R having intial poase X_WG or pose X_WD. Is this correct? if so, I can move to the next part.

I'll assume I am correct for now and move on to the next part (ignore if I was wrong before).
Per comment above, reference postions were registered to be in the world frame W, I'll call those r_WQ ("r" for registered, for the lack of a better name). Now I want the new positions with R at X_WD, I'll call those p_WQ.

So, from registration r_WX = X_WG * p_RQ, or p_RQ = X_WG⁻¹ * r_WX.
Since p_RQ is the invaraint, when I move the reference frame R to X_WD, the new positions are p_WQ = X_WD * p_RQ = X_WD * X_WG⁻¹ * r_WX.

So, the code I was expecting to see is:

 const math::RigidTransform<double> X_WdWg = X_WD_ * X_WG_.inverse(); 
for (int i = 0; i < num_dofs / 3; ++i) {
    model_state.template segment<3>(3 * i) =
        X_WdWg * reference_positions_.template segment<3>(3 * i);
}

Where did I go wrong? probably worth documenting the right procedure regardless.

[xuchenhan-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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.cc line 329 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

oh, I see, right. However, I believe the pose X_GD is not correct. Lets see where I got it wrong.

I am going to introduce some notation to avoid getting confused (hopefully). I will introduce the "reference frame" R, and I define it such that the pose of R at registration in the world is X_WG. I believe, if I'm correct, that what you want, is that the "new pose" of R is X_WD. That is, X_WG is the pose of the reference frame R at registration and I want it now to have a new default pose X_WD.

The invariant here is that the positions p_RQ of points Q of the deformable body in R are constant, in the reference confuguration (before deformation) regardless of R having intial poase X_WG or pose X_WD. Is this correct? if so, I can move to the next part.

I'll assume I am correct for now and move on to the next part (ignore if I was wrong before).
Per comment above, reference postions were registered to be in the world frame W, I'll call those r_WQ ("r" for registered, for the lack of a better name). Now I want the new positions with R at X_WD, I'll call those p_WQ.

So, from registration r_WX = X_WG * p_RQ, or p_RQ = X_WG⁻¹ * r_WX.
Since p_RQ is the invaraint, when I move the reference frame R to X_WD, the new positions are p_WQ = X_WD * p_RQ = X_WD * X_WG⁻¹ * r_WX.

So, the code I was expecting to see is:

 const math::RigidTransform<double> X_WdWg = X_WD_ * X_WG_.inverse(); 
for (int i = 0; i < num_dofs / 3; ++i) {
    model_state.template segment<3>(3 * i) =
        X_WdWg * reference_positions_.template segment<3>(3 * i);
}

Where did I go wrong? probably worth documenting the right procedure regardless.

Working.

I think you are right; the transform that needs to be applied should be X_WD* X_WG.inverse(); I'll add a unit test to confirm this.

[xuchenhan-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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.cc line 329 at r1 (raw file):

Previously, xuchenhan-tri wrote…

Working.

I think you are right; the transform that needs to be applied should be X_WD* X_WG.inverse(); I'll add a unit test to confirm this.

Done.

I think this is fixed. PTAL.

I also fixed the bug where calling DeformableBody::set_default_pose() after Finalize() has no effect (revealed by the new unit test).

[amcastro-tri]

Thanks @xuchenhan-tri.

Reviewed 8 of 8 files at r3.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, 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.cc line 329 at r1 (raw file):

Previously, xuchenhan-tri wrote…

Done.

I think this is fixed. PTAL.

I also fixed the bug where calling DeformableBody::set_default_pose() after Finalize() has no effect (revealed by the new unit test).

nice!

---

multibody/tree/deformable_body.h line 316 at r3 (raw file):

  /* Returns the default positions of the vertices of the deformable body. This
   provides the positions of the registered mesh posed in the default pose,
   measured and expressed in the world frame. */

consider,

Suggestion:

  /* Returns the vertex positons of the deformable body in the reference configuration. This
   provides the positions of the registered mesh posed in the default pose,
   measured and expressed in the world frame. */

---

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

                  -Gz  */
  VectorXd p_GV_G(7 * 3);
  p_GV_G.segment<3>(0) = Vector3d(0, 0, 0);    // v0

ditto, consider shorter notation p_GV instead.

---

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

  EXPECT_FALSE(X_WD.IsNearlyEqualTo(X_WG, 0.1));
  /* Create a new context and verify the default state is updated. */
  auto plant_context_new = plant_->CreateDefaultContext();

do I need to create a new context? can I do plant_->SetDefaultState(*plant_context_, &plant_context->get_mutable_state())?

---

multibody/tree/deformable_body.cc line 350 at r3 (raw file):

  const int num_dofs = fem_model_->num_dofs();
  VectorX<T> p_WVd_W = VectorX<T>::Zero(num_dofs);
  /* `reference_positions_` stores the list of p_WV_W for all vertices V

nit, consider the shorter monogram p_EV instead of the longer version p_EV_E when measured and expressed-in frames are the same.

Code quote:

 p_WV_W f

[amcastro-tri]

+@ggould-tri for platform review please

[xuchenhan-tri]

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

---

multibody/tree/deformable_body.h line 316 at r3 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

consider,

I prefer the old phrasing. The proposed phrasing could easily be confused with what's returned by reference_positions().

---

multibody/tree/deformable_body.cc line 350 at r3 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider the shorter monogram p_EV instead of the longer version p_EV_E when measured and expressed-in frames are the same.

It's unfortunate that we don't have an agreed-upon convention for whether the expressed-in frame should be spelled out when it's unnecessary. I'm being pulled in the other direction in another PR (#23063) under review in the same files.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto, consider shorter notation p_GV instead.

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

do I need to create a new context? can I do plant_->SetDefaultState(*plant_context_, &plant_context->get_mutable_state())?

Done

[ggould-tri]

Reviewed 3 of 9 files at r1, 1 of 1 files at r2, 5 of 8 files at r3, 3 of 3 files at r4, all commit messages.
Reviewable status: 6 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @xuchenhan-tri)

---

multibody/plant/deformable_model.cc line 4 at r4 (raw file):

#include <algorithm>
#include <memory_resource>

minor: This is a fairly alarming thing to be importing, and it doesn't look like it's used. Can you comment why it is needed?

---

multibody/parsing/model_directives.h line 79 at r4 (raw file):

  /// reference configuration, i.e. the undeformed geometry of the deformable
  /// body will have this pose in the world frame.

typo? Extra whitespace.

---

multibody/parsing/detail_dmd_parser.cc line 89 at r4 (raw file):

      return;
    }
    // Set the default pose for the deformable body

typo missing punctuation.

Suggestion:

    // Set the default pose for the deformable body.

[xuchenhan-tri]

Reviewable status: 2 unresolved discussions

---

multibody/parsing/detail_dmd_parser.cc line 89 at r4 (raw file):

Previously, ggould-tri wrote…

typo missing punctuation.

Done

---

multibody/parsing/model_directives.h line 79 at r4 (raw file):

Previously, ggould-tri wrote…

typo? Extra whitespace.

Done

---

multibody/plant/deformable_model.cc line 4 at r4 (raw file):

Previously, ggould-tri wrote…

minor: This is a fairly alarming thing to be importing, and it doesn't look like it's used. Can you comment why it is needed?

It's a bad copypaste... Thanks for catching.

[amcastro-tri]

Reviewed 3 of 3 files at r4, 2 of 3 files at r5, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),amcastro-tri

---

multibody/tree/deformable_body.cc line 350 at r3 (raw file):

Previously, xuchenhan-tri wrote…

It's unfortunate that we don't have an agreed-upon convention for whether the expressed-in frame should be spelled out when it's unnecessary. I'm being pulled in the other direction in another PR (#23063) under review in the same files.

There are many of those agreed-upon conventions that are not documented. However, a quick scan of the multibody code will show you that most often we remove the trailing underscore. I tend to err on the side of conventions used in the code base already for consistency and avoid introducing new conventions (unless, of course, there is a very good reason to change things). I believe the requests on the other PR are not well founded IMO

[xuchenhan-tri]

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

---

multibody/tree/deformable_body.cc line 350 at r3 (raw file):
If you search for [A-Z]*_([A-Z])[A-Z]*_\1\s in the code base you'd see a ton of examples where the expressed in frame is not omitted.

According to https://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html:

Monogram notation includes some defaults that are commonly used to simplify symbols when the meaning is clear. In particular if the reference symbol has an obvious frame, that basis is the default for the expressed-in frame. For example, w_AB can be used instead of w_AB_A (you can think of that as "no need to repeat the '_A' twice").

It doesn't forbid adding the extra expressed-in frame when it's not necessary and some contributors prefer being extra clear at the cost of being redundant.