[multibody] Allow reversed weld joints

[sherm1]

When building the tree-structured model for a system of rigid bodies and joints, it is possible that the inboard/outboard sense of the underlying mobilizer will have to be reversed from the parent/child ordering of the joint it models. MultibodyPlant currently forbids that; this PR relaxes that restriction for Weld joints, which are the most common case we see where reversal is needed.

Fixes #13040

---

This change is 

[sherm1]

+@jwnimmer-tri for feature review, please
I'm missing a unit test now for the error message for unsupported reverse joints; I'll add that after lunch. But since your time is limited I wanted to get this to you now.

cc @amcastro-tri

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

[jwnimmer-tri]

I can't actually review this, but it seems like about what I was looking for.

In my particular use case, there was actually a somewhat long chain of FixedOffsetFrame instances between the world body and my welded body, and the unit test seems to use the weld between body frames (not offset frames). I imagine that doesn't matter much, but anyway if you want a more representative use case you could demonstrate using a reversed weld between offset frames.

[sherm1]

Will do, thanks. -@jwnimmer-tri

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

[sherm1]

+@mitiguy for feature review, please
Please see if you think the results in the test case are correct.

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

[mitiguy]

I have taken a first pass to "weld" my understanding of notation: mobilized body, parent/child, Jp, Jc, F, M, inboard/outboard.
Also, I suggested revised code and comments for the reversed weld joint in multibody_plant.cc. It seems the original and revised code are both generally applicable to reverse joints (welded or otherwise).
I tested my revision locally on multibody_plant_test.cc and all passed.

Note: I have more work tomorrow to do on the test cases, etc.

Reviewed 2 of 3 files at r5, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/plant/multibody_plant.cc line 3959 at r5 (raw file):

    // Mobilizer reaction force on mobilized body B at mobilized frame's origin
    // Mo, expressed in world frame.

FYI Consider adding W to end of comment, i.e.,
expressed in world frame W.

Also, due to the use of "reaction" in the context of Newton's 3rd law around line 3989 with different sense, consider removing the word "reaction" from this comment (redundant), e.g., with

Code snippet:

// Force from mobilizer on mobilized body B at mobilized frame's origin Mo,
// expressed in world frame W.

---

multibody/plant/multibody_plant.cc line 3981 at r5 (raw file):

    const bool is_reversed = (Jp_index == M_index);

    SpatialForce<T> F_CJc_W;

FYI I think the previous monogram notation F_CJc_W needs a comment, such as:

Code snippet:

// Force from mobilizer on child body C at Jco (origin of joint's child
// frame), expressed in world frame W.

---

multibody/plant/multibody_plant.cc line 3983 at r5 (raw file):

    SpatialForce<T> F_CJc_W;
    if (!is_reversed) {
      // Given we now Mo == Jc and B == C.

BTW There seems to be a typo in the previous comment as "now" was meant to be "know".
However, the comment here is identical to the one below it, and the one below it does not make sense to me. Consider changing this comment to:

Code snippet:

// For this typical case, Mo == Jc and B == C, so F_BMo_W is the force on
// child body C at Jco (origin of joint's child frame), expressed in W.

---

multibody/plant/multibody_plant.cc line 3986 at r5 (raw file):

      F_CJc_W = F_BMo_W;
    } else {
      // Given we now Mo == Jc and B == C.

BTW Comment seems incorrect. Perhaps change to:

Code snippet:

// For this reverse case, Mo == Jp and B == P, so F_BMo_W is the force on
// parent body P at JPo (origin of joint's parent frame), expressed in W.

---

multibody/plant/multibody_plant.cc line 3990 at r5 (raw file):

      // Newton's third law allows to find the reaction on the child body as
      // required.

FYI Grammar issue in previous comment. Consider changing to:

Code snippet:

// Deduce force on the child body from Newton's 3ʳᵈ law (action/reaction).

---

multibody/plant/multibody_plant.cc line 3996 at r5 (raw file):

      // First we need to find the position vector p_JpJc_W.
      const RotationMatrix<T> R_WJp =
          frame_Jp.CalcRotationMatrixInWorld(context);

BTW Expressing back-and-forth in terms of the world frame seems inefficient.
I tested a shorter version of this code -- with relevant parts below which (in my opinion) seems more straight forward and efficient (fewer copies and calculations).

Code snippet:

    // For use below, calculate the rotation matrix between Jc and world W.
    const RotationMatrix<T> R_WJc = frame_Jc.CalcRotationMatrixInWorld(context);
    const RotationMatrix<T> R_JcW = R_WJc.inverse();

    // The quantity of interest is the spatial force from the mobilizer on child
    // body C at Jco (origin of joint's child frame), expressed in frame Jc.
    SpatialForce<T> F_CJc_Jc;
    if (!is_reversed) {
      // For this typical case, Mo == Jc and B == C, so F_BMo_W is the force on
      // child body C at Jco (origin of joint's child frame), expressed in W.
      const SpatialForce<T>& F_CJc_W = F_BMo_W;
      F_CJc_Jc = R_JcW * F_CJc_W;
    } else {
      // For this reverse case, Mo == Jp and B == P, so F_BMo_W is the force on
      // parent body P at JPo (origin of joint's parent frame), expressed in W.
      const SpatialForce<T>& F_PJp_W = F_BMo_W;

      // Deduce force on the child body from Newton's 3ʳᵈ law (action/reaction).
      const SpatialForce<T> F_CJp_W = -F_PJp_W;

      // Express F_CJp_W in the Jc basis.
      const SpatialForce<T> F_CJp_Jc = R_JcW * F_CJp_W;

      // The position vector p_JpJc_Jp helps shift spatial force from Jp to Jc.
      const RigidTransform<T> X_JpJc = frame_Jc.CalcPose(context, frame_Jp);
      const Vector3<T>& p_JpJc_Jp = X_JpJc.translation();

      // Shift the spatial force on child C from Jp to Jc.
      F_CJc_Jc = F_CJp_Jc.Shift(p_JpJc_Jp);
    }

    // Store the result.
    output->at(joint.ordinal()) = F_CJc_Jc;
  }
}

---

multibody/plant/multibody_plant.cc line 3998 at r5 (raw file):

          frame_Jp.CalcRotationMatrixInWorld(context);
      const RigidTransform<T> X_JpJc = frame_Jc.CalcPose(context, frame_Jp);
      const Vector3<T> p_JpJc_Jp = X_JpJc.translation();

FYI For efficiency, consider using a reference here.

Code snippet:

 const Vector3<T>& p_JpJc_Jp = X_JpJc.translation();

---

multibody/plant/test/multibody_plant_test.cc line 1715 at r5 (raw file):

// mobilizer that specifies World as the inboard body. We'll also add an
// identical rigid body with the frames in the normal (forward) order and check
// that everything makes sense.

FYI In the previous comment, I think it helps to point out the "user" ideas of parent/child as contrasted with the internal Drake developer need for inboard/outboard.

Code snippet:

// Weld world to a body, with body as the parent and World as the child.
// This is fine for a Drake user, but for Drake internal developers,
// this body (parent)-to-World (child) weld is implemented with a reversed
// Weld mobilizer that specifies inboard-to-outboard order as World-to-body. 
// To verify results, We'll also add an identical rigid body with the frames
// in the normal (forward) order and check that everything makes sense

[sherm1]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/plant/multibody_plant.cc line 3959 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Consider adding W to end of comment, i.e.,
expressed in world frame W.

Also, due to the use of "reaction" in the context of Newton's 3rd law around line 3989 with different sense, consider removing the word "reaction" from this comment (redundant), e.g., with

Done, thanks. I reworked most of the comments, PTAL. The use of "reaction" here is correct since this is really the mobilizer reaction force. I removed it in the 3rd law comment below.

---

multibody/plant/multibody_plant.cc line 3981 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI I think the previous monogram notation F_CJc_W needs a comment, such as:

Done

---

multibody/plant/multibody_plant.cc line 3983 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

BTW There seems to be a typo in the previous comment as "now" was meant to be "know".
However, the comment here is identical to the one below it, and the one below it does not make sense to me. Consider changing this comment to:

Done. Reworded. Short comments seemed like enough here, we're just relabeling the frames.

---

multibody/plant/multibody_plant.cc line 3986 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

BTW Comment seems incorrect. Perhaps change to:

Done. Yes, that was wrong. PTAL now.

---

multibody/plant/multibody_plant.cc line 3990 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Grammar issue in previous comment. Consider changing to:

Done

---

multibody/plant/multibody_plant.cc line 3996 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

BTW Expressing back-and-forth in terms of the world frame seems inefficient.
I tested a shorter version of this code -- with relevant parts below which (in my opinion) seems more straight forward and efficient (fewer copies and calculations).

I like this idea, but I think the shift vector is in the wrong frame here:

F_CJc_Jc = F_CJp_Jc.Shift(p_JpJc_Jp);

Shouldn't it be in Jc?

This revealed a problem with the unit test -- the frames are rotated around 1,0,0 but the shift vector is also 1,0,0 so although Jp and Jc are different the shift vector looks the same in both! I'll rework that.

(Unit test is fixed now and the Jp frame makes it fail)

---

multibody/plant/multibody_plant.cc line 3998 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI For efficiency, consider using a reference here.

Done. Reworked to use X_JcJp rather than X_JpJc, PTAL.

---

multibody/plant/test/multibody_plant_test.cc line 1715 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI In the previous comment, I think it helps to point out the "user" ideas of parent/child as contrasted with the internal Drake developer need for inboard/outboard.

Done

[mitiguy]

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

---

multibody/plant/multibody_plant.cc line 3996 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I like this idea, but I think the shift vector is in the wrong frame here:

F_CJc_Jc = F_CJp_Jc.Shift(p_JpJc_Jp);

Shouldn't it be in Jc?

This revealed a problem with the unit test -- the frames are rotated around 1,0,0 but the shift vector is also 1,0,0 so although Jp and Jc are different the shift vector looks the same in both! I'll rework that.

(Unit test is fixed now and the Jp frame makes it fail)

Done. Good catch!

---

multibody/plant/multibody_plant.cc line 3981 at r6 (raw file):

    // We'll need this in both cases below since we're required to report
    // the reaction force in the joint's child frame Jc.

FYI Consider changing "in" to "expressed in" to this comment, e.g. as:

Code snippet:

// We'll need this in both cases below since we're required to report
// the reaction force expressed in the joint's child frame Jc.

---

multibody/plant/multibody_plant.cc line 3986 at r6 (raw file):

    // The quantity of interest, F_CJc_Jc, is the joint's reaction force on the
    // child body C at the joint's child frame Jc, expressed in Jc.

FYI Consider adding the word "joint" before child. Although possibly redundant, I think it helpful as it provides context as parent/child language is used from the user/joint perspective instead of the inboard/outboard language from the mobilizer's perspective.

Code snippet:

// joint's child body C at the joint's child frame Jc, expressed in Jc.

---

multibody/plant/multibody_plant.cc line 3990 at r6 (raw file):

    if (!is_reversed) {
      F_CJc_Jc = R_JcW * F_BMo_W;  // The typical case: Mo==Jc and B==C.
    } else {

FYI Here, I think it helps to clarify what F_BMo_W means in the context of the joint, something like:

Code snippet:

// For this reverse case, F_BMo_W is the force on the joint's parent body P 
// at the joint's parent frame Jp, expressed in W.

---

multibody/plant/multibody_plant.cc line 3994 at r6 (raw file):

      // Newton's 3ʳᵈ law (action/reaction) says the force on the child
      // _at Jp_ is equal and opposite to the force on the parent at Jp.

FYI As discussed f2f, this depends on a massless joint.
Here is my attempt to simplify this to a few lines.

Code snippet:

// Using Newton's 3ʳᵈ law (action/reaction) and knowing Drake's joints
// are massless, the force from the joint on the child at Jp is equal
// and opposite to the force from the joint on the parent at Jp.

---

multibody/plant/multibody_plant.cc line 3999 at r6 (raw file):

      // However, the reaction force we want to report on the child is at Jc,
      // not Jp. We need to shift the application point from Jp to Jc.

FYI: Consider notation to help contrast what is needed.

Code snippet:

// F_C𝐉𝐩_Jc is the joint's reaction force on the joint's child body C at
// the joint's parent frame 𝐉𝐩. However, the reaction force to report is
// F_C𝐉𝐜_Jc, the joint's reaction force on the joint's child body C at the
// the joint's child frame 𝐉𝐜. Hence, we need to shift the application
// point of the force from Jp to Jc.

[sherm1]

All comments so far addressed.

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

---

multibody/plant/multibody_plant.cc line 3981 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Consider changing "in" to "expressed in" to this comment, e.g. as:

Done

---

multibody/plant/multibody_plant.cc line 3986 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Consider adding the word "joint" before child. Although possibly redundant, I think it helpful as it provides context as parent/child language is used from the user/joint perspective instead of the inboard/outboard language from the mobilizer's perspective.

Done

---

multibody/plant/multibody_plant.cc line 3990 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Here, I think it helps to clarify what F_BMo_W means in the context of the joint, something like:

Done

---

multibody/plant/multibody_plant.cc line 3994 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI As discussed f2f, this depends on a massless joint.
Here is my attempt to simplify this to a few lines.

Done, nice!

---

multibody/plant/multibody_plant.cc line 3999 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI: Consider notation to help contrast what is needed.

Nice try, but that just made it more confusing to me since some Jp's and some Jc's are bold. Maybe if one symbol was always bold and the other not? I think I'll just let the reader struggle with the plain font.

[mitiguy]

See new simple test case.
Other test cases in the works.

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

---

multibody/plant/test/multibody_plant_test.cc line 1727 at r7 (raw file):

  MultibodyPlant<double> plant(0.0);
  Parser(&plant).AddModelsFromUrl(sdf_url);

FYI Consider adding a simple reverse weld joint test starting here.

Code snippet:

  //---------------------------------------------------------------------------
  // Coincident test case: parent body welded to a coincident child = World.
  // As a Drake user may do, create a weld joint with parent=body, child=World.
  // As a Drake internal developer, the mobilizer has reversed ordering, i.e.,
  // inboard=World and outboard=body. Ensure joint reaction forces are reported
  // on child=World at Jc (joint child frame), expressed in Jc. This first test
  // has no rotation or translation so Jp = body frame and Jc = world frame.
  // The expected joint reaction spatial force should have zero torque and a
  // downward force of m*g (m = 1 kg is the body's mass and g = 9.81 m/s²).
  const RigidBody<double>& parent_body_coincident =
      plant.AddRigidBody("parent_body_coincident", default_model_instance(),
                         SpatialInertia<double>::MakeUnitary());
  const RigidBody<double>& child_world = plant.world_body();
  // Create a reverse weld (parent=body, child=World) in which the all the
  // transforms are identity, i.e., X_PJp, X_CJc, X_JpJc are all identity.
  const RigidTransform<double> X_Identity;
  const Joint<double>& reverse_weld_coincident = plant.AddJoint<WeldJoint>(
      "reverse_weld_coincident", parent_body_coincident, X_Identity,
      child_world, X_Identity, X_Identity);
  //---------------------------------------------------------------------------
  

---

multibody/plant/test/multibody_plant_test.cc line 1771 at r7 (raw file):

  const double g = 9.81;  // Our default gravity.

FYI Ensure the coincident test case results make sense here.

Code snippet:

  // Coincident test case: Joint reaction spatial force should have zero torque
  // and downward force of m*g (m = 1 kg is the body's mass and g = 9.81 m/s²).
  const double kTolerance = 16 * std::numeric_limits<double>::epsilon();
  EXPECT_TRUE(CompareMatrices(
      reaction_forces[reverse_weld_coincident.ordinal()].get_coeffs(),
      SpatialForce<double>(Vector3d(0, 0, 0), Vector3d(0, 0, -g)).get_coeffs(),
          kTolerance));

[mitiguy]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/plant/test/multibody_plant_test.cc line 1726 at r8 (raw file):

      "package://drake/multibody/plant/test/split_pendulum.sdf";
  MultibodyPlant<double> plant(0.0);
  Parser(&plant).AddModelsFromUrl(sdf_url);

BTW Add a simple reverse weld joint test starting here?

Code snippet:

  //---------------------------------------------------------------------------
  // Coincident test case: parent body welded to a coincident child = World.
  // As a Drake user may do, create a weld joint with parent=body, child=World.
  // As a Drake internal developer, the mobilizer has reversed ordering, i.e.,
  // inboard=World and outboard=body. Ensure joint reaction forces are reported
  // on child=World at Jc (joint child frame), expressed in Jc. This first test
  // has no rotation or translation so Jp = body frame and Jc = world frame.
  // The expected joint reaction spatial force should have zero torque and a
  // downward force of m*g (m = 1 kg is the body's mass and g = 9.81 m/s²).
  const RigidBody<double>& parent_body_coincident =
      plant.AddRigidBody("parent_body_coincident", default_model_instance(),
                         SpatialInertia<double>::MakeUnitary());
  const RigidBody<double>& child_world = plant.world_body();
  // Create a reverse weld (parent=body, child=World) in which the all the
  // transforms are identity, i.e., X_PJp, X_CJc, X_JpJc are all identity.
  const RigidTransform<double> X_Identity;
  const Joint<double>& reverse_weld_coincident = plant.AddJoint<WeldJoint>(
      "reverse_weld_coincident", parent_body_coincident, X_Identity,
      child_world, X_Identity, X_Identity);
  //---------------------------------------------------------------------------

---

multibody/plant/test/multibody_plant_test.cc line 1770 at r8 (raw file):

          .Eval<std::vector<SpatialForce<double>>>(*context);

  const double g = 9.81;  // Our default gravity.

BTW Ensure the coincident test case results make sense here.

Code snippet:

  // Coincident test case: Joint reaction spatial force should have zero torque
  // and downward force of m*g (m = 1 kg is the body's mass and g = 9.81 m/s²).
  const double kTolerance = 16 * std::numeric_limits<double>::epsilon();
  EXPECT_TRUE(CompareMatrices(
      reaction_forces[reverse_weld_coincident.ordinal()].get_coeffs(),
      SpatialForce<double>(Vector3d(0, 0, 0), Vector3d(0, 0, -g)).get_coeffs(),
          kTolerance));