Adds parameters for FixedOffsetFrame

[joemasterjohn]

Towards #13289.

Implements MultibodyElement Parameterization (started in #13860) for FixedOffsetFrame.

Introduces API for mutating FixedOffsetFrame given a context:

• FixedOffsetFrame::SetPoseInBodyFrame

Adds python binding for:

• Frame::CalcPoseInBodyFrame()
• Frame::CalcRotationMatrixInBodyFrame()
• FixedOffsetFrame::SetPoseInBodyFrame()

Relevant issues that this may partially/completely solve:
#14084
#13520
#13517

---

This change is 

[joemasterjohn]

+@EricCousineau-TRI for feature review, please.

Reviewable status: LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI)

[EricCousineau-TRI]

with a request to collapse to just setting SE(3), not SO(3) + R3.

Reviewed 2 of 2 files at r1.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers (waiting on @joemasterjohn)

---

multibody/tree/fixed_offset_frame.h, line 117 at r1 (raw file):

    p_PoFo_P_parameter.SetFromVector(X_PF.translation());
  }

nit I guess it makes sense to just compute rotation, but I'm not sure if I understand a useful reason for only setting rotation.

Is there anyone who asked for this?
If not, can this be removed?

---

multibody/tree/fixed_offset_frame.h, line 190 at r1 (raw file):

  const math::RigidTransform<double> X_PF_;

  // System parameter indices for `this` frame's RigidTransform stored in a

nit Why not store this as 1 parameter, a 3x4 matrix, in X_PF_parameter_index_?

I doubt there's any meaningful gain for (ticket-based) caching between rotation / translation?

[joemasterjohn]

Reviewable status: needs at least two assigned reviewers (waiting on @EricCousineau-TRI)

---

multibody/tree/fixed_offset_frame.h, line 117 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit I guess it makes sense to just compute rotation, but I'm not sure if I understand a useful reason for only setting rotation.

Is there anyone who asked for this?
If not, can this be removed?

Done. No explicit ask, I was just mirroring GetPose/GetRotation. I'll remove it.

---

multibody/tree/fixed_offset_frame.h, line 190 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Why not store this as 1 parameter, a 3x4 matrix, in X_PF_parameter_index_?

I doubt there's any meaningful gain for (ticket-based) caching between rotation / translation?

Done. No real reason. I think it might avoid an extra copy (from GetAsMatrix34()) when flattening the RigidTransform into a BasicVector. But I think it's minuscule.

[EricCousineau-TRI]

Reviewed 2 of 2 files at r2.
Reviewable status: needs at least two assigned reviewers

---

multibody/tree/fixed_offset_frame.h, line 117 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Done. No explicit ask, I was just mirroring GetPose/GetRotation. I'll remove it.

OK Thanks!

---

multibody/tree/fixed_offset_frame.h, line 190 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Done. No real reason. I think it might avoid an extra copy (from GetAsMatrix34()) when flattening the RigidTransform into a BasicVector. But I think it's minuscule.

OK Awesome, thanks!

[EricCousineau-TRI]

+@sherm1 for platform review on Tuesday, please!

Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)

a discussion (no related file):
BTW Might I be able to request some good ol' Python bindings too?

I can handle it if it's outta scope for ya.

---

[joemasterjohn]

Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)

a discussion (no related file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW Might I be able to request some good ol' Python bindings too?

I can handle it if it's outta scope for ya.

No problem. Added them in the latest revision. I think I added everything you need, but let me know if not.

---

[EricCousineau-TRI]

Reviewed 2 of 2 files at r3.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)

a discussion (no related file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

No problem. Added them in the latest revision. I think I added everything you need, but let me know if not.

OK Awesome, thanks!

---

---

bindings/pydrake/multibody/test/plant_test.py, line 1462 at r3 (raw file):

        numpy_compare.assert_float_equal(
            frame.CalcPoseInBodyFrame(context).GetAsMatrix34(),
            np.array([[1., 0., 0., 1.],

BTW This may be clearer if you just check numpy_compare.assert_float_equal(actual, X_PF.GetAsMatrix34()).

[joemasterjohn]

Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)

---

bindings/pydrake/multibody/test/plant_test.py, line 1462 at r3 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW This may be clearer if you just check numpy_compare.assert_float_equal(actual, X_PF.GetAsMatrix34()).

Done.

[EricCousineau-TRI]

Reviewed 2 of 2 files at r4.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)

[sherm1]

Platform with a few minor comments.

Reviewed 2 of 2 files at r2, 1 of 2 files at r4, 1 of 1 files at r5.
Reviewable status: 7 unresolved discussions (waiting on @joemasterjohn)

---

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

      .SetSpatialInertiaInBodyFrame(context_autodiff.get(), M2_L2o);

  const RevoluteJoint<AutoDiffXd>& elbow_joint_ad =

minor: please add a comment explaining that you added this test to make sure fixed offset frame parameter differentiation works, and how this tests that.

---

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

GTEST_TEST(MultibodyPlantTests, FixedOffsetFrameParameters) {
  // Add a plant with a few rigid bodies.
  MultibodyPlant<double> plant(0.0);

minor: it looks like the only occupant of this MBPlant is World? (Fine, but the comment isn't right then.)

---

multibody/tree/fixed_offset_frame.h, line 86 at r5 (raw file):

    const systems::BasicVector<T>& X_PF_parameter =
        context.get_numeric_parameter(X_PF_parameter_index_);
    const Eigen::Matrix<T, 3, 4> X_PF =

minor: this is performing a copy in order to go from a Map to a Matrix. That shouldn't be necessary.

---

multibody/tree/fixed_offset_frame.h, line 100 at r5 (raw file):

    const Eigen::Matrix<T, 3, 4> X_PF =
        Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
            X_PF_parameter.get_value().data());

BTW consider collecting the above lines into an inline private method like get_X_PF_as_Matrix34(context) rather than duplicating them here. (And see if you can avoid the extra copy.)

---

multibody/tree/fixed_offset_frame.h, line 110 at r5 (raw file):

        context->get_mutable_numeric_parameter(X_PF_parameter_index_);
    const VectorX<T> X_PF_vector =
        Eigen::Map<const VectorX<T>>(X_PF.GetAsMatrix34().data(), 12, 1);

minor: this is copying 12 elements from the Map to the VectorX. That should not be necessary.

---

multibody/tree/fixed_offset_frame.h, line 151 at r5 (raw file):

  // Implementation for MultibodyElement::DoDeclareParameters().
  // FixedOffsetFrame declares a single parameter for its RigidTransform.
  void DoDeclareParameters(

minor: this method should be private if possible so it doesn't show up in Doxygen (maybe final also?). If you want to leave it in protected it needs a doxygen comment directed to someone who is overriding it in a further derived class.

---

multibody/tree/fixed_offset_frame.h, line 156 at r5 (raw file):

    Frame<T>::DoDeclareParameters(tree_system);
    const VectorX<T> X_PF = Eigen::Map<const VectorX<T>>(
        X_PF_.template cast<T>().GetAsMatrix34().data(), 12, 1);

BTW this copy should not be necessary either, although there isn't a performance issue here since this is just happening during construction. BasicVector's constructor takes an Eigen::Ref and that should be happy to accept the Map directly.

[joemasterjohn]

Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: please add a comment explaining that you added this test to make sure fixed offset frame parameter differentiation works, and how this tests that.

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: it looks like the only occupant of this MBPlant is World? (Fine, but the comment isn't right then.)

Done. Copy/paste error.

---

multibody/tree/fixed_offset_frame.h, line 86 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this is performing a copy in order to go from a Map to a Matrix. That shouldn't be necessary.

Done.

---

multibody/tree/fixed_offset_frame.h, line 100 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider collecting the above lines into an inline private method like get_X_PF_as_Matrix34(context) rather than duplicating them here. (And see if you can avoid the extra copy.)

Done. I can avoid explicitly creating a Matrix34 altogether by just grabbing the 3x3 block from the Map itself.

---

multibody/tree/fixed_offset_frame.h, line 110 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this is copying 12 elements from the Map to the VectorX. That should not be necessary.

Done.

---

multibody/tree/fixed_offset_frame.h, line 151 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this method should be private if possible so it doesn't show up in Doxygen (maybe final also?). If you want to leave it in protected it needs a doxygen comment directed to someone who is overriding it in a further derived class.

Done. The pattern I used forDoDeclareParameters requires a derived class to call DoDeclareParameters of it's parent class, which is why it's usually declared protected. But since FixedOffsetFrame is final itself, I think it suffices for it to be private (and final too).

---

multibody/tree/fixed_offset_frame.h, line 156 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this copy should not be necessary either, although there isn't a performance issue here since this is just happening during construction. BasicVector's constructor takes an Eigen::Ref and that should be happy to accept the Map directly.

Done. Removed the unnecessary copy.

[sherm1]

Per f2f please verify that this works for weld joints (the primary use case I believe) and drop some TODOs and/or an issue if there are problems there.

Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)

[sherm1]

Thanks, looks great. Just a few remaining nits ...

Reviewed 2 of 2 files at r6.
Reviewable status: 3 unresolved discussions (waiting on @joemasterjohn)

---

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

Previously, joemasterjohn (Joe Masterjohn) wrote…

Done.

Nice, thanks.

---

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

Previously, joemasterjohn (Joe Masterjohn) wrote…

Done. Copy/paste error.

Still there?

---

multibody/plant/test/multibody_plant_test.cc, line 3267 at r6 (raw file):

  // Set up our parameters as independent variables.
  // We will use mass and length of the two links of the acrobot as our
  // variables to differentiate. We must update multibody components that

nit: to differentiate against (or with respect to)? Not sure the best wording -- we differentiate functions with respect to variables so "variables to differentiate" seems wrong.

---

multibody/plant/test/multibody_plant_test.cc, line 3280 at r6 (raw file):

  const AutoDiffXd Gc2_ad = (1.0 / 12.0) * l2_ad * l2_ad;

  // Differentiable parameters for the acrobots mass/inertia parameters.

nit: acrobot's

---

multibody/plant/test/multibody_plant_test.cc, line 3413 at r6 (raw file):

  plant.Finalize();

  // Create a default context.override

nit: ".override" looks spurious?

[joemasterjohn]

Ok, I've added to the test to show that updating the parameters on a FixedOffsetFrame that is the frame_on_parent of a WeldJoint correctly affects the pose of the child body.

I guess it should be noted that to achieve the effect of changing the offset between two welded bodies, one of the frames of the joint needs to be a FixedOffsetFrame. So if for instance someone welded a body's origin to the world origin using the optional parameters to MultibodyPlant::AddJoint:

plant.AddJoint<WeldJoint>("weld", plant.world_body(), {}, body, {});

Both the frame_on_parent and frame_on_child of the weld joint will be BodyFrame`s of their respective bodies, and thus won't have parameters to change.

This might seem obvious, but might not be to a user. Especially true for one using the parser since the urdf parser makes frame_on_child of a weld joint to be the child's BodyFrame (a URDF convention I think), while the SDF parser creates two FixedOffsetFrames. Compare:

detail_sdf_parser.cc:

    case sdf::JointType::FIXED: {
      plant->AddJoint<WeldJoint>(
          joint_spec.Name(),
          parent_body, X_PJ,
          child_body, X_CJ,
          RigidTransformd::Identity() /* X_JpJc */);
      break;
    }

detail_urdf_parser.cc

  } else if (type.compare("fixed") == 0) {
    throw_on_custom_joint(false);
    plant->AddJoint<WeldJoint>(name, parent_body, X_PJ,
                               child_body, std::nullopt,
                               RigidTransformd::Identity());
  }

Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform) (waiting on @sherm1)

---

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

Previously, sherm1 (Michael Sherman) wrote…

Still there?

Done.

---

multibody/plant/test/multibody_plant_test.cc, line 3267 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: to differentiate against (or with respect to)? Not sure the best wording -- we differentiate functions with respect to variables so "variables to differentiate" seems wrong.

Done. Bad English on my part.

---

multibody/plant/test/multibody_plant_test.cc, line 3280 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: acrobot's

Done.

---

multibody/plant/test/multibody_plant_test.cc, line 3413 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: ".override" looks spurious?

Done.

[sherm1]

Interesting -- thanks for investigating. I suspect it won't be immediately obvious to users how to move the weld frames around! But at least it can be done now. We might find that we need to add some sugar to make this easier at some point -- maybe like Joint::SetParentFramePose(&context, X_PJp) and Joint::SetChildFramePose(&context, X_CJc) ?

Reviewed 1 of 1 files at r7.
Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform)