[multibody] Allow Rpy floating joints for free bodies

[sherm1]

What's here

Removes MbP's assumption that floating bodies use quaternion floating joints, allowing rpy floating joints instead. The setting can be global or model instance-specific. Weld joints may also be selected (the new topology code already supports these three types). The following MultibodyPlant method is added to convey the desired base-body joint type pre-finalize:

MultibodyPlant::SetBaseBodyJointType(BaseBodyJointType, optional<ModelInstance>)

where BaseBodyJointType is an enum selecting quaternion/rpy floating, or weld (fixed) joint.

The existing SetFreeBodyRandomRotationDistribution() methods remain restricted to free bodies that use a QuaternionFloatingJoint because the rotation is expressed as a quaternion. I added

MultibodyPlant::SetFreeBodyRandomAnglesDistribution(body, RollPitchYaw angles)

that works with free bodies that use an RpyFloatingJoint. SetFreeBodyRandomTranslationDistribution() now works with either floating joint.

Rather than creating a new "floating" abstraction layer, I have chosen instead to permit more operations on the Joint (or internally, Mobilizer) base class that can deal with pose and velocity (already nice abstractions!) without having to know the underlying joint type. This PR adds several public methods to the base Joint class that permit setting/getting joint pose without knowing what kind of joint is there. There were a few existing Joint:: methods which are now extended to provide a complete set of default & context-taking methods. "Advanced" methods for quaternion/translation pairs are provided for consistency with the existing promise that pre-finalize setting of floating bodies results in the bit-identical pose post-finalize (only when using quaternion floating joints). Italics indicates a pre-existing method:

Joint:: Setter	Joint:: Getter
set_default_positions(q)	q = default_positions()
SetDefaultPose(X_FM)	X_FM = GetDefaultPose()
SetDefaultPosePair(q_FM, p_FM)	q_FM, p_FM = GetDefaultPosePair()
SetPositions(&context, q)	q = GetPositions(context )
SetVelocities(&context, v)	v = GetVelocities(context)
SetPose(&context, X_FM)	X_FM = GetPose(context)
SetSpatialVelocity(&context, V_FM)	V_FM = GetSpatialVelocity(context)
SetPosePair(&context, q_FM, p_FM)	q_FM, p_FM = GetPosePair(context)

APIs SetPose(), SetPosePair(), and SetSpatialVelocity() are currently implemented only for floating joints, which can represent any value of their arguments. (There is good reason to implement them for other joints also, but that is only a TODO here.)
The Get methods work for all joint types.

Multibody code that was previously tied to QuaternionFloatingJoints is rewritten here in terms of the generic setters and getters so that it is floating-joint agnostic.

What's not here

Access to the new base joint controls from parsing, configuration, or Python; only the C++ API is included.
Subsequent PRs should use the new API to provide higher-level access to this control.

Unit testing

Tests are in multibody_plant_test.cc unless otherwise stated:

• Base body joint type selection API is tested for global & model instance settings for quaternion, rpy, and weld.
• The new random angles API gets a test that parallels the existing random rotation API test.
• The new Joint:: APIs are all tested for floating and one non-floating case.
• A test checks for one non-floating joint type that a reasonable "not implemented" error is issued for the unimplemented APIs.
• The mobilizer abstraction is extended a little to support the new PosePair and SpatialVelocity APIs. Only the floating joints support those. Low-level mobilizer tests are added for the implemented cases (in quaternion/rpy_floating_joint_test.cc).

I'm not testing the new API x all joint types because (except for the floating cases) the implementation is the same for all of them and just uses pre-existing, well tested code.

Addresses #20943

---

This change is 

[sherm1]

+@amcastro-tri for feature review please (per f2f)
Please be sure to read the PR description for guidance.

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

[amcastro-tri]

Excellent! a few comments before I give the final LGTM for platform, but looking really good! thanks for the docs @sherm1

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

---

multibody/plant/multibody_plant.h line 231 at r1 (raw file):

};

/// The kind of joint to be used to attach an unattached RigidBody to World.

This sentence sounds a bit loose. While we are talking about adding a joint (a graph element) there is the underlying tree structure underneath (base body). That should be captured in the documentation. Also, I don't think we document anywhere what "attached/unattached" mean? avoid using a new term?

Finally, this should be connected with the building of the tree topology at Finalize() to give users a full picture.

A shortcut, simplify to "The type of joint to be added to base bodies at Finalize(). See SetBaseBodyJointType() for details", and document in detail in SetBaseBodyJointType().

Code quote:

The kind of joint to be used to attach an unattached RigidBody to World.

---

multibody/plant/multibody_plant.h line 1680 at r1 (raw file):

  /// Sets the type of joint to be used by Finalize() to attach any otherwise
  /// unattached bodies to World. (Bodies attached to World are called _base

I'd try to be more precise, I don't think we ever defined "attached/unattached".

Suggestion:

  /// Sets the type of joint to be used by Finalize() connect a base body 
  /// to World. Based bodies are determined at Finalize(), when we build an 
  /// underlying forest structure to model the multibody system for efficient computations.

---

multibody/plant/multibody_plant.h line 1694 at r1 (raw file):

  /// † The 3-angle orientation representation used by RpyFloatingJoint can be
  ///   easier to work with than a quaternion (especially for optimization) but
  ///   has a singular orientation which must be avoided (2nd angle near 90°).

minor,

Suggestion:

(pitch angle

---

multibody/plant/multibody_plant.h line 1698 at r1 (raw file):

  /// @note Reminder: if you aren't satisfied with the automatic choice of
  /// base body or the particular selection of joints here, you can always
  /// specify an explicit joint to World.

nit,

Suggestion:

  /// base body or the particular selection of joints here, you can always
  /// add an explicit joint to World with AddJoint().

---

multibody/plant/multibody_plant.h line 3270 at r1 (raw file):

  ///   modeled using an RpyFloatingJoint.
  /// @see SetBaseBodyJointType() for control over the type of automatically-
  ///   added joints.

nit, consider adding a not that this disctribution is not uniform over the solid angles sphere, and reference to SetFreeBodyRandomRotationDistributionToUniform(). People usually tend to assume that "random rotations" is synonim of "uniform rotation".

---

multibody/plant/multibody_plant.h line 3307 at r1 (raw file):

  ///   modeled using a QuaternionFloatingJoint.
  /// @see SetBaseBodyJointType() for control over the type of automatically-
  ///   added joints.

nit, consider adding a not that this disctribution is not uniform over the solid angles sphere, and reference to SetFreeBodyRandomRotationDistributionToUniform(). People usually tend to assume that "random rotations" is synonim of "uniform rotation".

---

multibody/plant/multibody_plant.cc line 1178 at r1 (raw file):

  // Pose of frame F in its parent body frame P.
  const RigidTransform<T>& X_PF = frame_F.EvalPoseInBodyFrame(*context);

why this change? is there a bug here?
According to the docs, this frame F is anchored, meaning that its parent is world and its pose in world is fixed. Are you changing to Eval because we might have parameterized these?

---

multibody/plant/multibody_plant.cc line 1284 at r1 (raw file):

  internal::LinkJointGraph& graph = mutable_tree().mutable_graph();
  if (model_instance.has_value()) {
    graph.SetForestBuildingOptions(*model_instance, option_map.at(joint_type));

this is great! I had forgotten about the option to do it per model instance.

---

multibody/tree/joint.h line 449 at r1 (raw file):

  //  very useful in practice.

  /// @name       Methods to set and get pose and velocity

minor, consider a separate group for default values.

---

multibody/tree/joint.h line 493 at r1 (raw file):

  /// this joint to `positions`.
  /// @note The values in `positions` are NOT constrained to be within
  ///   position_lower_limits() and position_upper_limits().

Since this is a new API, shouldn't we throw if the positions are not within limits?
IMO that's what should happen as well with default positions, though I can see how you'd avoid doing that here in order not to introduce a behavioral change.

Code quote:

  /// @note The values in `positions` are NOT constrained to be within
  ///   position_lower_limits() and position_upper_limits().

---

multibody/tree/joint.h line 497 at r1 (raw file):

  ///   num_positions().
  /// @throws std::exception if the containing MultibodyPlant has not yet been
  ///   finalized.

nit, There is a check for has_mobilizer() which might not render to a nice message that easily correlates to this documentaiton.
Consider throwing with a nicer message. ditto below

Code quote:

  /// @throws std::exception if the containing MultibodyPlant has not yet been
  ///   finalized.

---

multibody/tree/joint.h line 501 at r1 (raw file):

  void SetPositions(systems::Context<T>* context,
                    const Eigen::Ref<const VectorX<T>>& positions) const {
    DRAKE_DEMAND(context != nullptr);

DRAKE_THROW_UNLESS?

Code quote:

 DRAKE_DEMAND(context != nullptr);

---

multibody/tree/joint.h line 518 at r1 (raw file):

    const Eigen::VectorBlock<const VectorX<T>> all_q =
        this->get_parent_tree().get_positions(context);
    DRAKE_THROW_UNLESS(get_implementation().has_mobilizer());

nit, consider moving the THROW_UNLESS to the very begining before doing anything else.

---

multibody/tree/joint.h line 525 at r1 (raw file):

  /// this joint to `velocities`.
  /// @note The values in `velocities` are NOT constrained to be within
  ///   velocity_lower_limits() and velocity_upper_limits().

should they be constrained? or at least throw?

I guess for velocities it is less critical than for positions. Velocity limits are not used for simulation (yes for control/planing) while positions are.

Code quote:

  /// @note The values in `velocities` are NOT constrained to be within
  ///   velocity_lower_limits() and velocity_upper_limits().

---

multibody/tree/joint.h line 583 at r1 (raw file):

  /// Sets in the given `context` this joint's generalized positions q such
  /// that the pose of the child frame M in the parent frame F best matches the

I'm afraid this will mix joint notation with mobilizer's notation. For mobilizers we've been using F (fixed) and M (mobilized).
For joints we've been using parent (Jp) and child (Jc). Consider using X_JpJc?

We could use P and C, but in mobilizers we use P for the parent body frame.
The new kernel will only have P, F, M (with B no longer used).

Code quote:

rame M in the parent frame F b

---

multibody/tree/joint.h line 623 at r1 (raw file):

  /// that the spatial velocity of the child frame M in the parent frame F best
  /// matches the given spatial velocity. The velocity is provided as a spatial
  /// velocity V_FM, but a joint may represent velocity differently. Drake's

ditto, consider V_JpJc?

---

multibody/tree/joint.h line 665 at r1 (raw file):

  /// (Advanced) This is the same as SetDefaultPose() except it takes the
  /// pose as a (quaternion, translation vector) pair. A QuaternionFloatingJoint
  /// will store this pose bit-identically; any other joint will approximate it.

minor, probably worth mentioning "... will approximate it within floating point precision", otherwise it sounds like some sort of mathematical approximation is introduced.

---

multibody/tree/joint.h line 671 at r1 (raw file):

  ///   implement this function.
  /// @see SetDefaultPose()
  void SetDefaultPosePair(const Quaternion<double>& q_FM,

btw, I know you are attempting to avoid deprecation, but I find the use of "Pair" in these functions names unfortunate. (The C++ overload communicates the data effectively, no need to put it in the name of the function).

---

multibody/tree/mobilizer.h line 359 at r1 (raw file):

  bool has_six_dofs() const { return num_velocities() == 6; }

  // Returns `true` if `this` uses a quaternion parametrization of rotations.

nit,

Suggestion:

parameterization

---

multibody/tree/mobilizer_impl.h line 182 at r1 (raw file):

  // represent this perfectly to guarantee consistent pose representation
  // pre- and post-finalize for floating base bodies.
  virtual Vector<T, kNq> DoPoseToState(const Eigen::Quaternion<T> orientation,

nit, consider

Suggestion:

DoPoseToPositions()

---

multibody/tree/mobilizer_impl.h line 187 at r1 (raw file):

    // This is the mobilizer's implementation of Joint::SetPose() so the
    // error message should refer to that.
    throw std::logic_error("SetPose(): not implemented for this joint type.");

this is a bit weird. You could consider making bool SetPosePair() to return true/false indicating if successful or not, and let the specific Joint class to throw. That'd even allow you to state the joint type and name in the message. After all, this methods are not needed to be optimized for kernel computations.

Ditto for DoVelocityToState()

---

multibody/tree/mobilizer_impl.h line 193 at r1 (raw file):

  // to this spatial velocity. 6 dof mobilizers are required to represent it
  // as close to bit-exactly as possible.
  virtual Vector<T, kNv> DoVelocityToState(

nit, consider

Suggestion:

DoSpatialVelocityToVelocities

---

multibody/tree/quaternion_floating_mobilizer.h line 271 at r1 (raw file):

  // required to preserve bit-identical state.
  std::pair<Eigen::Quaternion<T>, Vector3<T>> GetPosePair(
      const systems::Context<T>& context) const final {

minor, move this one also to the cc?

[sherm1]

Thanks, Alejandro. All but one comment addressed. Also I realize I need to move the implementation of the new Joint:: APIs from the header to joint.cc but didn't want to do that until you've had a chance to review the changes. PTAL

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

---

multibody/plant/multibody_plant.h line 231 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This sentence sounds a bit loose. While we are talking about adding a joint (a graph element) there is the underlying tree structure underneath (base body). That should be captured in the documentation. Also, I don't think we document anywhere what "attached/unattached" mean? avoid using a new term?

Finally, this should be connected with the building of the tree topology at Finalize() to give users a full picture.

A shortcut, simplify to "The type of joint to be added to base bodies at Finalize(). See SetBaseBodyJointType() for details", and document in detail in SetBaseBodyJointType().

Done.

---

multibody/plant/multibody_plant.h line 1680 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I'd try to be more precise, I don't think we ever defined "attached/unattached".

Done.

---

multibody/plant/multibody_plant.h line 1694 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor,

Done

---

multibody/plant/multibody_plant.h line 1698 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit,

Done

---

multibody/plant/multibody_plant.h line 3270 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider adding a not that this disctribution is not uniform over the solid angles sphere, and reference to SetFreeBodyRandomRotationDistributionToUniform(). People usually tend to assume that "random rotations" is synonim of "uniform rotation".

Done

---

multibody/plant/multibody_plant.h line 3307 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider adding a not that this disctribution is not uniform over the solid angles sphere, and reference to SetFreeBodyRandomRotationDistributionToUniform(). People usually tend to assume that "random rotations" is synonim of "uniform rotation".

Done

---

multibody/plant/multibody_plant.cc line 1178 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

why this change? is there a bug here?
According to the docs, this frame F is anchored, meaning that its parent is world and its pose in world is fixed. Are you changing to Eval because we might have parameterized these?

Yes. This method was added in PR #21853 to cache pre-calculated parameterized frames. I replaced all the MbT algorithm calls then with the cached method but missed this one in MbP. The previous method ignored parameterizations which I believe was a bug. This is a drive-by fix, doesn't have anything to do with the rest of the PR.

---

multibody/tree/joint.h line 449 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor, consider a separate group for default values.

My thought is that it is better to group these together so that users see the contrast between the default and the context-taking methods. For some reason a lot of people seem to be confused by that concept. IMO separating the groups makes it more likely that a user might look at only one group and not realize the other exists.

---

multibody/tree/joint.h line 493 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Since this is a new API, shouldn't we throw if the positions are not within limits?
IMO that's what should happen as well with default positions, though I can see how you'd avoid doing that here in order not to introduce a behavioral change.

Right, it probably would have made more sense to constrain these values on input. But then the new API would be inconsistent with the "default" API which would be ugly. We would need to change the behavior of the old methods as well, and would also have to decide whether just to trim out-of-range values (possibly hiding user misunderstandings) or issue error messages (possibly producing annoying behavior). That seems like too much for this PR, but we could do a breaking change PR later to change all the behaviors.

My guess is that this is not actually going to be a problem for most users, who are likely to provide reasonable initial values for their joints.

---

multibody/tree/joint.h line 497 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, There is a check for has_mobilizer() which might not render to a nice message that easily correlates to this documentaiton.
Consider throwing with a nicer message. ditto below

Done. I switched all of these to use ThrowIfNotFinalized(). The has_mobilizer check is now a DRAKE_DEMAND because a finalized model should always have a mobilizer (internal bug if not).

---

multibody/tree/joint.h line 501 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

DRAKE_THROW_UNLESS?

Done (for all of these)

---

multibody/tree/joint.h line 518 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider moving the THROW_UNLESS to the very begining before doing anything else.

Done

---

multibody/tree/joint.h line 525 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

should they be constrained? or at least throw?

I guess for velocities it is less critical than for positions. Velocity limits are not used for simulation (yes for control/planing) while positions are.

Same comment as above: yes, but no.

---

multibody/tree/joint.h line 583 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I'm afraid this will mix joint notation with mobilizer's notation. For mobilizers we've been using F (fixed) and M (mobilized).
For joints we've been using parent (Jp) and child (Jc). Consider using X_JpJc?

We could use P and C, but in mobilizers we use P for the parent body frame.
The new kernel will only have P, F, M (with B no longer used).

Unfortunately we've used F and M in the documentation for Drake almost everywhere. See for example, the diagram for Joint or the transform in the WeldJoint constructor.

So this notation won't confuse any Drake users -- the confusion comes for us internally when we define the mobilizer frames. It hasn't been a problem since to date we've always matched the joint and mobilizer frames (i.e., Jp=F, Jc=M). However, Paul and I are dealing with the terminology problem explicitly in the new "reverse weld joints" PR #22122. There we are leaving the user documentation as-is (F and M), but switching internally to Jp and Jc before we get anywhere near the mobilizer. That way the code can be very explicit about whether Jp==F or M, etc.

TL;DR: we are stuck with F and M in user documentation and have to be extra-careful internally with our notation where reversed mobilizers can crop up.

---

multibody/tree/joint.h line 623 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto, consider V_JpJc?

Same issue

---

multibody/tree/joint.h line 665 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor, probably worth mentioning "... will approximate it within floating point precision", otherwise it sounds like some sort of mathematical approximation is introduced.

Done. In future if we allow this for any joint there would be a kind of mathematical approximation (e.g. provide a pose for a prismatic joint would just be a projection onto the sliding axis). PTAL at the rewording. (For now those will get a "not implemented" message instead.)

---

multibody/tree/joint.h line 671 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

btw, I know you are attempting to avoid deprecation, but I find the use of "Pair" in these functions names unfortunate. (The C++ overload communicates the data effectively, no need to put it in the name of the function).

I agree it isn't necessary here, but as an API design feature I think the name is better like this because it is clearly parallel to its mate GetPosePair() which actually does have to have "pair" in the function name.

---

multibody/tree/mobilizer.h line 359 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit,

Done

---

multibody/tree/mobilizer_impl.h line 182 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider

Done

---

multibody/tree/mobilizer_impl.h line 187 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

this is a bit weird. You could consider making bool SetPosePair() to return true/false indicating if successful or not, and let the specific Joint class to throw. That'd even allow you to state the joint type and name in the message. After all, this methods are not needed to be optimized for kernel computations.

Ditto for DoVelocityToState()

Agreed. Still pondering this.

---

multibody/tree/mobilizer_impl.h line 193 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider

Done, much better.

---

multibody/tree/quaternion_floating_mobilizer.h line 271 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor, move this one also to the cc?

Done

[amcastro-tri]

Thank you @sherm1, . Feel free to move things to cc files where approprite. Feature done.

+@SeanCurtis-TRI for platform review please

Reviewed 9 of 9 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

---

multibody/plant/multibody_plant.cc line 1178 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yes. This method was added in PR #21853 to cache pre-calculated parameterized frames. I replaced all the MbT algorithm calls then with the cached method but missed this one in MbP. The previous method ignored parameterizations which I believe was a bug. This is a drive-by fix, doesn't have anything to do with the rest of the PR.

okay. good!

---

multibody/tree/joint.h line 493 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Right, it probably would have made more sense to constrain these values on input. But then the new API would be inconsistent with the "default" API which would be ugly. We would need to change the behavior of the old methods as well, and would also have to decide whether just to trim out-of-range values (possibly hiding user misunderstandings) or issue error messages (possibly producing annoying behavior). That seems like too much for this PR, but we could do a breaking change PR later to change all the behaviors.

My guess is that this is not actually going to be a problem for most users, who are likely to provide reasonable initial values for their joints.

fair, I just wanted to get you thoughts on is.

---

multibody/tree/joint.h line 583 at r1 (raw file):

we are stuck with F and M in user documentation

Probably not. I agree we've introduced this notation publicly in the past (the examples above), but I am nto sure they are that many and probably easier to fix than what we think?

have to be extra-careful internally with our notation

I agree. For now this is a good solution.

---

multibody/tree/mobilizer_impl.h line 187 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Agreed. Still pondering this.

okay

[sherm1]

I moved the implementations now. Sean, I'm glad you're reviewing this because it has some overlap with your earlier free-body stuff. Still one feature review comment to address.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform)

[amcastro-tri]

Reviewed 5 of 5 files at r3, all commit messages.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform)

[sherm1]

All feature review comments addressed now.

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)

---

multibody/tree/mobilizer_impl.h line 187 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

okay

Done, PTAL. This gives a much nicer error message, thanks!

[SeanCurtis-TRI]

Ok, first pass done. I really wish I'd waited until you were done with feature review comments.

Reviewed 11 of 21 files at r1, 8 of 9 files at r2, 4 of 5 files at r3, 9 of 9 files at r4, all commit messages.
Reviewable status: 24 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)

a discussion (no related file):
Question about rotation distribution.

What's the motivation to having two different methods (one taking angles and one taking quaternions)? That feels purely like sugar. For example, I could certainly define a rotation distribution based on quaternions, sample a rotation from that quaternion, and then convert it to Rpy. Similarly, I should think that I could articulate the distribution in angles, sample an Rpy and then convert it to quaternion. So, it's not like specifying the rotation distribution should depend on the underlying implementation.

I see the APIs themselves are quite literal, so without any further effort, the existing APIs constraint you. The QuaternionFloatingMobilizer only takes a Quaternion<Expression> and, therefore, expects the values of the quaternions to be literal state values. So, my question is probably bigger than this API. Why are the APIs so strict in requiring a rotation's distribution declaration to have the same unerlying representation as the implementation? Does conversion from Rpy -> Quaternion not support Expression? Do we worry about the size of expression due to conversions?

One regrettable implication is that we have no ability to create a uniform distribution of orientations for Rpy-floating joints (even though we can for Quaternion-floating joints).

Am I missing something?

---

---

multibody/plant/multibody_plant.h line 231 at r2 (raw file):

};

/// The kind of joint to be used to connect free bodies to world at Finalize().

nit: Phraseology consistency. The type says "base body", the documentation says "free body".

It seems like we've finally defined "base body" in this file (on the documentation for SetBaseBodyJointType, but it's used in a couple of other places.

There is a doxygen section called "Working with free bodies". It seems the definition of "base body" should probably go there. With this enum's documentation referencing that.

Code quote:

free

---

multibody/plant/multibody_plant.h line 1679 at r2 (raw file):

  /// Sets the type of joint to be used by Finalize() to connect any otherwise
  /// unconnected bodies to World. Bodies connected by a joint to World are

I'd advocate that this method should be moved to the doxygen block called "Working with free bodies" and the definition of "base bodies" belongs in that block's overview documentation (as the phrase "base body" is used there). The group name (and its anchor) would also make referencing that defnition easier.

---

multibody/plant/multibody_plant.h line 1699 at r2 (raw file):

  ///   has a singular orientation which must be avoided (pitch angle near 90°).
  ///
  /// @note Reminder: if you aren't satisfied with the automatic choice of

nit: The phrase "the automatic choice of base body" threw me off for a second.

It's very common for us to have models that themselves are trees and the "automatic choice" doesn't appear to be a choice, but rather a simple property of the model. How common do you think it would be for a model to have a surprising base body (i.e., my model is a chain loop or some such thing)?

In contrast, the "I don't like how my obvious base body got connected to the world" seems like a far more common case.

I'd suggest separating these two concepts and the case of a "surprising automatically-chosen base body' should receive a bit more context so a Drake user, less steeped in the vagaries of spanning trees, has an easier time figuring out whether they should be worried about this or not.

---

multibody/plant/multibody_plant.h line 1707 at r2 (raw file):

  ///   which `joint_type` is to be applied.
  /// @throws std::exception if called after Finalize().
  void SetBaseBodyJointType(

BTW No getter for this value?

---

multibody/plant/multibody_plant.h line 3110 at r2 (raw file):

  /// it is a "floating base" body. Free bodies that are added to the plant
  /// without specifying a joint become floating base bodies after
  /// finalization. It is possible (and sometimes recommended) to explicitly

Another reason to move the SetBaseBodyJointType() down here, because that function makes this assertion a lie.

See? They all belong together.

Code quote:

  /// it is a "floating base" body. Free bodies that are added to the plant
  /// without specifying a joint become floating base bodies after
  /// finalization. It is possible (and sometimes recommended) to explicitly

---

multibody/plant/multibody_plant.h line 3266 at r2 (raw file):
BTW This statement is imprecise.

The method, SetFreeBodyRandomRotationDistributionToUniform() actually calls this method. So, the uniformity of this distribution very much depends on the expressions defining the quaternion. So, it can be uniform, but it depends.

To be honest, I'm not sure what expressions people would otherwise be using. So, I don't know how likely people are to be shoving bad distributions in.

So, perhaps rephrasing the note that:

The uniformity of this distribution depends on the quaternion expression rotation. For a uniform distribution over the sphere reachable by the quaternion, use SetFreeBodyRandomRotationDistributionToUniform().

---

multibody/plant/multibody_plant.h line 3307 at r2 (raw file):

  ///   @ref mbp_working_with_free_bodies "above for details".
  /// @note This distribution is not uniform over the sphere reachable by
  ///   the three angles. See SetFreeBodyRandomRotationDistributionToUniform()

nit: This is copy pasta. The reference to SetFreeBodyRandomRotationDistributionToUniform() is unhelpful because that method cannot be applied to an Rpy joint.

---

multibody/plant/multibody_plant.cc line 1273 at r2 (raw file):

    BaseBodyJointType joint_type,
    std::optional<ModelInstanceIndex> model_instance) {
  static const std::map<BaseBodyJointType, internal::ForestBuildingOptions>

nit: This requires a never_destroyed.

---

multibody/plant/multibody_plant.cc line 1273 at r2 (raw file):

    BaseBodyJointType joint_type,
    std::optional<ModelInstanceIndex> model_instance) {
  static const std::map<BaseBodyJointType, internal::ForestBuildingOptions>

BTW Using a map instead of a switch means that instead of having this bark at you during compile time (if the enumeration ever expands), it'll only bark at runtime.

Is the map really better than a switch?

It's five lines longer, but it's not clear that's awful.

template <typename T>
void MultibodyPlant<T>::SetBaseBodyJointType(
    BaseBodyJointType joint_type,
    std::optional<ModelInstanceIndex> model_instance) {
  std::optional<internal::ForestBuildingOptions> options;
  switch (joint_type) {
    case BaseBodyJointType::kQuaternionFloatingJoint:
      options = internal::ForestBuildingOptions::kDefault;
      break;
    case BaseBodyJointType::kRpyFloatingJoint:
      options = internal::ForestBuildingOptions::kUseRpyFloatingJoints;
      break;
    case BaseBodyJointType::kWeldJoint:
      options = internal::ForestBuildingOptions::kUseFixedBase;
      break;
  }
  DRAKE_DEMAND(options.has_value());
  DRAKE_THROW_UNLESS(!is_finalized());
  internal::LinkJointGraph& graph = mutable_tree().mutable_graph();
  if (model_instance.has_value()) {
    graph.SetForestBuildingOptions(*model_instance, *options);
  } else {
    graph.SetGlobalForestBuildingOptions(*options);
  }
}

or

namespace {
  internal::ForestBuildingOptions BaseBodyJointToForestOptions(
      BaseBodyJointType joint_type) {
    switch (joint_type) {
      case BaseBodyJointType::kQuaternionFloatingJoint:
        return internal::ForestBuildingOptions::kDefault;
      case BaseBodyJointType::kRpyFloatingJoint:
        return internal::ForestBuildingOptions::kUseRpyFloatingJoints;
      case BaseBodyJointType::kWeldJoint:
        return internal::ForestBuildingOptions::kUseFixedBase;
    }
    DRAKE_UNREACHABLE();
  }
}  // namespace

template <typename T>
void MultibodyPlant<T>::SetBaseBodyJointType(
    BaseBodyJointType joint_type,
    std::optional<ModelInstanceIndex> model_instance) {
  const internal::ForestBuildingOptions > options =
      BaseBodyJointToForestOptions(joint_type);
  DRAKE_THROW_UNLESS(!is_finalized());
  internal::LinkJointGraph& graph = mutable_tree().mutable_graph();
  if (model_instance.has_value()) {
    graph.SetForestBuildingOptions(*model_instance, options);
  } else {
    graph.SetGlobalForestBuildingOptions(options);
  }
}

---

multibody/plant/multibody_plant.cc line 1284 at r2 (raw file):

  internal::LinkJointGraph& graph = mutable_tree().mutable_graph();
  if (model_instance.has_value()) {
    graph.SetForestBuildingOptions(*model_instance, option_map.at(joint_type));

BTW Too bad LinkJointGraph doesn't have the same optional model instance semantics.

---

multibody/tree/joint.h line 457 at r3 (raw file):

  /// here.) There are several ways to set these values:
  ///  - If you understand a joint's state representation q and v, you can
  ///    directly set them using methods here. The specific choice of

btw: "methods here" and "documentation there"

Perhaps it would be "methods below" and "documentation for the joint type".

---

multibody/tree/joint.h line 586 at r3 (raw file):

  ///   joint used to generate the returned transform.
  math::RigidTransform<T> GetPose(const systems::Context<T>& context) const {
    auto pose_pair = GetPosePair(context);

BTW As someone who isn't a great fan of pairs, had you considered:

  const auto& [q_FM, p_FM] = GetPosePair(context);
  return math::RigidTransform(q_FM, p_FM);

---

multibody/tree/joint.h line 598 at r3 (raw file):

  /// descriptions for specifics.
  /// @note Currently this is implemented only for floating (6 dof) joints
  ///   which can represent any pose.

nit copypasta

Suggestion:

spatial velocity

---

multibody/tree/joint.cc line 87 at r4 (raw file):

    throw std::logic_error(
        fmt::format("{}(): {} joint does not implement this function "
                    "(joint {})",

BTW you might consider putting the name in quotes (here and on SetPosePair()).

---

multibody/tree/mobilizer.h line 354 at r2 (raw file):

  const RigidBody<T>& outboard_body() const { return outboard_frame().body(); }

  // Returns `true` if `this` mobilizer grants 6-dofs to the outboard frame.

BTW This predates this PR, but I thought I'd mention that dofs is an overloaded word in this file.

On line 133, we're using it to describe the number of qs associated with a joint (we refer to "4 dofs for a quaternion plus 3 dofs for a position").

Generally, it is used in the more informal sense of meaning no constraints on pose -- the conceptual sense, divorced from parameterization. The docs of num_positions() draws a strong distinction between having 6 dofs and 7 positions.

---

multibody/tree/mobilizer.h line 414 at r3 (raw file):

  virtual std::pair<Eigen::Quaternion<T>, Vector3<T>> GetPosePair(
      const systems::Context<T>& context) const {
    const math::RigidTransform<T> X_FM = CalcAcrossMobilizerTransform(context);

nit: "Virtual methods should never have their implementations in the header file."
- Book of Jeremy, Chapter 2, verse 3

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

                   const Vector3<T>& p_FM,
                   systems::State<T>* state) const final {
    get_mutable_positions(&*state) = DoPoseToPositions(q_FM, p_FM);

nit: "Yea, verily, it includes functions declared as "final" or "override" as well.
- Book of Jeremy, Chapter 2, verse 4

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

                   const Vector3<T>& p_FM,
                   systems::State<T>* state) const final {
    get_mutable_positions(&*state) = DoPoseToPositions(q_FM, p_FM);

nit By tradition, the NVI partner to this method would be called DoSetPosePair(). Is there a good reason to against the grain?

(Same for SetSpatialVelocities.)

---

multibody/tree/mobilizer_impl.h line 190 at r4 (raw file):

      const Vector3<T>& translation) const {
    unused(orientation, translation);
    return {};

nit: More functions whose (admittedly trivial implementations) still belong in the .cc file (this and the next).

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

  void ThrowDefaultMassInertiaError() const;

  // Helper method for throwing an exception within public methods that should

BTW I couldn't glean why these methods moved. What have I missed?

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

  void ThrowDefaultMassInertiaError() const;

  // Helper method for throwing an exception within public methods that should

BTW This probably doesn't matter with these files. But declaration order is not aligned with the definition order in the .cc file. Admittedly, it doesn't look like that kind of order existed before. Just thought I'd mention it.

---

multibody/tree/multibody_tree.h line 2743 at r2 (raw file):

  // @throws std::exception if `body` is not a free body.
  // @throws std::exception if called pre-finalize.
  // @pre `body` is not World

BTW Probably pedantic, but, we wouldn't call the world body a free body? It has no parent, so it cannot have a 6-dof joint connecting it to what doesn't exist. So, if it's not a free body, haven't we already promised to throw? Do we need it to be a prerequisite as well?

---

multibody/tree/multibody_tree.cc line 1241 at r3 (raw file):

        "Use SetFreeBodyRandomAnglesDistribution() instead.",
        __func__, QuaternionFloatingJoint<T>::kTypeName, body.name(),
        RpyFloatingJoint<T>::kTypeName));

nit: The hard-coding the assertion that you got an RpyFloatingJoint seems to be a potential lie. MultibodyPlant has not confirmed that body is a free body. It only documents you'll throw if it's not a free body. This code certainly satisfies the contract in that it throws. But the message may not be helpful because this code could be reached with a body with an arbitrary inboard joint type.

I think you need to change the error logic here if you want to give better error messages. And you probably need a test as well.

(Same for the angles variant below.)

Code quote:

pyFloatingJoint<T>::kTypeName

[sherm1]

Thanks for the great review and sorry for the last-minute changes! Working ...

Reviewable status: 24 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[sherm1]

All review comments addressed, PTAL. Ready for round 2.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform)

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Question about rotation distribution.

What's the motivation to having two different methods (one taking angles and one taking quaternions)? That feels purely like sugar. For example, I could certainly define a rotation distribution based on quaternions, sample a rotation from that quaternion, and then convert it to Rpy. Similarly, I should think that I could articulate the distribution in angles, sample an Rpy and then convert it to quaternion. So, it's not like specifying the rotation distribution should depend on the underlying implementation.

I see the APIs themselves are quite literal, so without any further effort, the existing APIs constraint you. The QuaternionFloatingMobilizer only takes a Quaternion<Expression> and, therefore, expects the values of the quaternions to be literal state values. So, my question is probably bigger than this API. Why are the APIs so strict in requiring a rotation's distribution declaration to have the same unerlying representation as the implementation? Does conversion from Rpy -> Quaternion not support Expression? Do we worry about the size of expression due to conversions?

One regrettable implication is that we have no ability to create a uniform distribution of orientations for Rpy-floating joints (even though we can for Quaternion-floating joints).

Am I missing something?

The goal here is for free bodies to work with either Rpy or Quaternion floating joints. When the distribution variables match the state variables it is clear how the sampling will work. I have no confidence that symbolically converting a quaternion to rpy (which is a real mess) would result in a usable sampling. If someone more knowledgeable thinks that will work, they are free to remove the restriction from the quaternion-taking function (in another PR). Meanwhile the straightforward angles->rpy distribution will work properly.

If someone wants to create a uniform sampling over joint angles, that would be great if it is possible. That's beyond my capability and I don't know that there is a use case currently.

---

---

multibody/plant/multibody_plant.h line 231 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: Phraseology consistency. The type says "base body", the documentation says "free body".

It seems like we've finally defined "base body" in this file (on the documentation for SetBaseBodyJointType, but it's used in a couple of other places.

There is a doxygen section called "Working with free bodies". It seems the definition of "base body" should probably go there. With this enum's documentation referencing that.

Done, PTAL. I modified the "Working with free bodies" section and referenced it.

---

multibody/plant/multibody_plant.h line 1679 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'd advocate that this method should be moved to the doxygen block called "Working with free bodies" and the definition of "base bodies" belongs in that block's overview documentation (as the phrase "base body" is used there). The group name (and its anchor) would also make referencing that defnition easier.

Done, partly. I updated the Working with free bodies section (including a reference to this function) but I'm not sure it makes sense to move this. When "weld" is chosen, the result is still a base body but not a free or floating base body. WDYT?

---

multibody/plant/multibody_plant.h line 1699 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: The phrase "the automatic choice of base body" threw me off for a second.

It's very common for us to have models that themselves are trees and the "automatic choice" doesn't appear to be a choice, but rather a simple property of the model. How common do you think it would be for a model to have a surprising base body (i.e., my model is a chain loop or some such thing)?

In contrast, the "I don't like how my obvious base body got connected to the world" seems like a far more common case.

I'd suggest separating these two concepts and the case of a "surprising automatically-chosen base body' should receive a bit more context so a Drake user, less steeped in the vagaries of spanning trees, has an easier time figuring out whether they should be worried about this or not.

Done, good point. I limited the discussion here to choice of joint. How base bodies are chosen when ambiguous is a much bigger discussion that doesn't belong here.

---

multibody/plant/multibody_plant.h line 1707 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW No getter for this value?

I wasn't sure it would be useful for anything, but since you brought it up ... Done.

---

multibody/plant/multibody_plant.h line 3110 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Another reason to move the SetBaseBodyJointType() down here, because that function makes this assertion a lie.

See? They all belong together.

Done, PTAL. I updated the documentation rather than move the function.

---

multibody/plant/multibody_plant.h line 3266 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This statement is imprecise.

The method, SetFreeBodyRandomRotationDistributionToUniform() actually calls this method. So, the uniformity of this distribution very much depends on the expressions defining the quaternion. So, it can be uniform, but it depends.

To be honest, I'm not sure what expressions people would otherwise be using. So, I don't know how likely people are to be shoving bad distributions in.

So, perhaps rephrasing the note that:

The uniformity of this distribution depends on the quaternion expression rotation. For a uniform distribution over the sphere reachable by the quaternion, use SetFreeBodyRandomRotationDistributionToUniform().

Done

---

multibody/plant/multibody_plant.h line 3307 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This is copy pasta. The reference to SetFreeBodyRandomRotationDistributionToUniform() is unhelpful because that method cannot be applied to an Rpy joint.

It's not copy pasta (see "angles"). For now a user who wants uniform sampling must use a QuaternionFloatingJoint and then call the indicated function. That
constitutes an "alternative" so I think this is right as is (and even vaguely useful).

---

multibody/plant/multibody_plant.cc line 1273 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This requires a never_destroyed.

Fixed by using your code instead.

---

multibody/plant/multibody_plant.cc line 1273 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Using a map instead of a switch means that instead of having this bark at you during compile time (if the enumeration ever expands), it'll only bark at runtime.

Is the map really better than a switch?

It's five lines longer, but it's not clear that's awful.

template <typename T>
void MultibodyPlant<T>::SetBaseBodyJointType(
    BaseBodyJointType joint_type,
    std::optional<ModelInstanceIndex> model_instance) {
  std::optional<internal::ForestBuildingOptions> options;
  switch (joint_type) {
    case BaseBodyJointType::kQuaternionFloatingJoint:
      options = internal::ForestBuildingOptions::kDefault;
      break;
    case BaseBodyJointType::kRpyFloatingJoint:
      options = internal::ForestBuildingOptions::kUseRpyFloatingJoints;
      break;
    case BaseBodyJointType::kWeldJoint:
      options = internal::ForestBuildingOptions::kUseFixedBase;
      break;
  }
  DRAKE_DEMAND(options.has_value());
  DRAKE_THROW_UNLESS(!is_finalized());
  internal::LinkJointGraph& graph = mutable_tree().mutable_graph();
  if (model_instance.has_value()) {
    graph.SetForestBuildingOptions(*model_instance, *options);
  } else {
    graph.SetGlobalForestBuildingOptions(*options);
  }
}

or

namespace {
  internal::ForestBuildingOptions BaseBodyJointToForestOptions(
      BaseBodyJointType joint_type) {
    switch (joint_type) {
      case BaseBodyJointType::kQuaternionFloatingJoint:
        return internal::ForestBuildingOptions::kDefault;
      case BaseBodyJointType::kRpyFloatingJoint:
        return internal::ForestBuildingOptions::kUseRpyFloatingJoints;
      case BaseBodyJointType::kWeldJoint:
        return internal::ForestBuildingOptions::kUseFixedBase;
    }
    DRAKE_UNREACHABLE();
  }
}  // namespace

template <typename T>
void MultibodyPlant<T>::SetBaseBodyJointType(
    BaseBodyJointType joint_type,
    std::optional<ModelInstanceIndex> model_instance) {
  const internal::ForestBuildingOptions > options =
      BaseBodyJointToForestOptions(joint_type);
  DRAKE_THROW_UNLESS(!is_finalized());
  internal::LinkJointGraph& graph = mutable_tree().mutable_graph();
  if (model_instance.has_value()) {
    graph.SetForestBuildingOptions(*model_instance, options);
  } else {
    graph.SetGlobalForestBuildingOptions(options);
  }
}

Done -- much better, thanks!

---

multibody/plant/multibody_plant.cc line 1284 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Too bad LinkJointGraph doesn't have the same optional model instance semantics.

LinkJointGraph has a much fancier notion of ForestBuildingOptions than I wanted to present to MbP users. There is a lot more besides joint choice, orthogonal options are or-ed together, etc. IMO better to insulate the MbP user from the technical forest-building stuff for now.

A subsequent PR should allow joint choice to be done from MbP config or parsers. Having a one-trick API should make that easier.

---

multibody/tree/joint.h line 457 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

btw: "methods here" and "documentation there"

Perhaps it would be "methods below" and "documentation for the joint type".

Done

---

multibody/tree/joint.h line 586 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW As someone who isn't a great fan of pairs, had you considered:

  const auto& [q_FM, p_FM] = GetPosePair(context);
  return math::RigidTransform(q_FM, p_FM);

Done. Way nicer, thanks!

---

multibody/tree/joint.h line 598 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit copypasta

Done

---

multibody/tree/joint.cc line 87 at r4 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW you might consider putting the name in quotes (here and on SetPosePair()).

Done

---

multibody/tree/mobilizer.h line 354 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This predates this PR, but I thought I'd mention that dofs is an overloaded word in this file.

On line 133, we're using it to describe the number of qs associated with a joint (we refer to "4 dofs for a quaternion plus 3 dofs for a position").

Generally, it is used in the more informal sense of meaning no constraints on pose -- the conceptual sense, divorced from parameterization. The docs of num_positions() draws a strong distinction between having 6 dofs and 7 positions.

Thanks, the line 133 discussion was simply an incorrect use of the term. Those are generalized coordinates, not dofs. Fixed. There are likely other misuses lurking around in internal code. I'll fix them opportunistically as they come up but I don't plan to do an audit.

---

multibody/tree/mobilizer.h line 414 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: "Virtual methods should never have their implementations in the header file."
- Book of Jeremy, Chapter 2, verse 3

Blasphemy!
Done

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: "Yea, verily, it includes functions declared as "final" or "override" as well.
- Book of Jeremy, Chapter 2, verse 4

Done

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit By tradition, the NVI partner to this method would be called DoSetPosePair(). Is there a good reason to against the grain?

(Same for SetSpatialVelocities.)

The low-level method isn't a direct implementation of the higher one, so I gave it a different name. Concrete mobilizers need only know how to convert poses to their generalized coordinates; the higher level function knows how to make use of that to modify the context.

I anticipate that the low-level conversions will have other uses besides helping out with the Joint API.

---

multibody/tree/mobilizer_impl.h line 190 at r4 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: More functions whose (admittedly trivial implementations) still belong in the .cc file (this and the next).

Done

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I couldn't glean why these methods moved. What have I missed?

They were previously not callable from Joint where I now have use for them.

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This probably doesn't matter with these files. But declaration order is not aligned with the definition order in the .cc file. Admittedly, it doesn't look like that kind of order existed before. Just thought I'd mention it.

Yeah. A big cleanup would be nice.

---

multibody/tree/multibody_tree.h line 2743 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Probably pedantic, but, we wouldn't call the world body a free body? It has no parent, so it cannot have a 6-dof joint connecting it to what doesn't exist. So, if it's not a free body, haven't we already promised to throw? Do we need it to be a prerequisite as well?

True, but if World is provided it aborts (since that's a misuse of the API) rather than throws a user-friendly message if a plausible but not-free body is provided. Better to distinguish the cases IMO

---

multibody/tree/multibody_tree.cc line 1241 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: The hard-coding the assertion that you got an RpyFloatingJoint seems to be a potential lie. MultibodyPlant has not confirmed that body is a free body. It only documents you'll throw if it's not a free body. This code certainly satisfies the contract in that it throws. But the message may not be helpful because this code could be reached with a body with an arbitrary inboard joint type.

I think you need to change the error logic here if you want to give better error messages. And you probably need a test as well.

(Same for the angles variant below.)

I might be misunderstanding your comment here. The call to GetFreeBodyMobilizerOrThrow() 10 lines up makes sure this is a free body. So I don't see what change is needed?

[SeanCurtis-TRI]

With a couple of last thoughts.

Reviewed 9 of 9 files at r6, all commit messages.
Reviewable status: 1 unresolved discussion

---

a discussion (no related file):

Previously, sherm1 (Michael Sherman) wrote…

The goal here is for free bodies to work with either Rpy or Quaternion floating joints. When the distribution variables match the state variables it is clear how the sampling will work. I have no confidence that symbolically converting a quaternion to rpy (which is a real mess) would result in a usable sampling. If someone more knowledgeable thinks that will work, they are free to remove the restriction from the quaternion-taking function (in another PR). Meanwhile the straightforward angles->rpy distribution will work properly.

If someone wants to create a uniform sampling over joint angles, that would be great if it is possible. That's beyond my capability and I don't know that there is a use case currently.

Fair enough.

---

multibody/plant/multibody_plant.h line 3307 at r2 (raw file):
I can see what you're saying. There's a key detail that can be inferred, but it still feels like a hidden trap. We could be more explicit:

note: this distribution is not uniform over the sphere reachable by the three angles. For a uniform alternative, switch the joint to a QuaternionFloatingJoint and use SetFreeBodyRandomRotationDistributionToUniform().

---

multibody/plant/multibody_plant.cc line 1284 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

LinkJointGraph has a much fancier notion of ForestBuildingOptions than I wanted to present to MbP users. There is a lot more besides joint choice, orthogonal options are or-ed together, etc. IMO better to insulate the MbP user from the technical forest-building stuff for now.

A subsequent PR should allow joint choice to be done from MbP config or parsers. Having a one-trick API should make that easier.

I was noting the fact that SetForestBuildingOptions is overloaded. Once with the options and once with options and model instance. If a single API simply had an optional model instance, it's functionality would remain unchanged, but this call would become more compact.

Ultimately, testing on the null-ness of the model instance would have to happen somewhere. In this case, you've opted to do it here rather than one layer lower. So, meh.

---

multibody/tree/mobilizer.h line 354 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Thanks, the line 133 discussion was simply an incorrect use of the term. Those are generalized coordinates, not dofs. Fixed. There are likely other misuses lurking around in internal code. I'll fix them opportunistically as they come up but I don't plan to do an audit.

And yet, in planning, we take your generalized collision state and describe the robot dofs based on the number of qs. :) The world is CRAZY!

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

The low-level method isn't a direct implementation of the higher one, so I gave it a different name. Concrete mobilizers need only know how to convert poses to their generalized coordinates; the higher level function knows how to make use of that to modify the context.

I anticipate that the low-level conversions will have other uses besides helping out with the Joint API.

I'm not sure what you mean "direct implementation". This function SetPosePair() calls a DoFoo() to do the derived-specific work and finishes up with doing some generic base-class work. That seems like the poster boy for classic NVI.

When I'd written my original comment, the only argument against "poster boy"-status that I thought of was the fact that typically the non-virtual and virtual helper methods are defined on the same class. This is unique in that the virtual methods are defined in a different class.

I didn't feel that was enough to justify a new name. As I see it, you've got a virtualDoPoseToPositions() which has one purpose: to be called in a non-virtual SetPosePair(), for me, that's enough to justify DoSetPosePair(). It clearly encodes the relationship between the two functions so observing the DoFoo, you know there's a Foo() out there that uniquely invokes it.

But I'm certainly not going to block on this.

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

They were previously not callable from Joint where I now have use for them.

Ah..."now have a use for them" suggests not using as of this PR, but you will be doing so later?

---

multibody/tree/multibody_tree.h line 2743 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

True, but if World is provided it aborts (since that's a misuse of the API) rather than throws a user-friendly message if a plausible but not-free body is provided. Better to distinguish the cases IMO

Fair enough.

---

multibody/tree/multibody_tree.cc line 1241 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I might be misunderstanding your comment here. The call to GetFreeBodyMobilizerOrThrow() 10 lines up makes sure this is a free body. So I don't see what change is needed?

Nope. I'd glossed over the GetFreeBodyMobilizerOrThrow(). So, the scenario I hypothesized can't actually happen.

It'll just have to change if we ever have any other kind of floating mobilizer, but that's probably unlikely. But there's no graceful alternative. We want to print a joint name when all we have are mobilizers. So, this is as good as it gets.

---

multibody/plant/multibody_plant.h line 1724 at r6 (raw file):

  /// that _was_ used if there were any base bodies in need of a joint.
  ///
  /// @note The default if SetBaseBodyJointType() was never called is to return

BTW I'm not sure that this note provides much. The body of the document makes it clear that the default is used if the setter is never called. Is there not some other documentation that defines what the default value is? Should this be redundantly declaring it (as opposed to citing the canonical definition)? And if it is uniquely defining it, is this the right place for defining that? Because it's implemented elsewhere? Admittedly, the implementation may be buried in internal land so no documentation there would be visible out here. But burying this on the getter seems an odd place to put this information.

(Also "never called" is a bit imprecise. The default applies if the setter was never called in a way that would affect the domain specified by model_instance. One can call the setter and get the default value for a different getter invocation.)

[sherm1]

Thanks, Sean!

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

---

multibody/plant/multibody_plant.h line 3307 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I can see what you're saying. There's a key detail that can be inferred, but it still feels like a hidden trap. We could be more explicit:

note: this distribution is not uniform over the sphere reachable by the three angles. For a uniform alternative, switch the joint to a QuaternionFloatingJoint and use SetFreeBodyRandomRotationDistributionToUniform().

Done

---

multibody/plant/multibody_plant.h line 1724 at r6 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I'm not sure that this note provides much. The body of the document makes it clear that the default is used if the setter is never called. Is there not some other documentation that defines what the default value is? Should this be redundantly declaring it (as opposed to citing the canonical definition)? And if it is uniquely defining it, is this the right place for defining that? Because it's implemented elsewhere? Admittedly, the implementation may be buried in internal land so no documentation there would be visible out here. But burying this on the getter seems an odd place to put this information.

(Also "never called" is a bit imprecise. The default applies if the setter was never called in a way that would affect the domain specified by model_instance. One can call the setter and get the default value for a different getter invocation.)

Done, good point. I removed the note -- the reference to the Set method is sufficient.

---

multibody/plant/multibody_plant.cc line 1284 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I was noting the fact that SetForestBuildingOptions is overloaded. Once with the options and once with options and model instance. If a single API simply had an optional model instance, it's functionality would remain unchanged, but this call would become more compact.

Ultimately, testing on the null-ness of the model instance would have to happen somewhere. In this case, you've opted to do it here rather than one layer lower. So, meh.

It's not actually overloaded -- the APIs in LinkJointGraph have different names. Likely it would be better to use the optional model instance there like I did here but I'm not currently in the market for revising the topology stuff! After more of it gets a workout in MbP I may take a cleanup pass. It's still internal:: so easily hacked.

---

multibody/tree/mobilizer.h line 354 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

And yet, in planning, we take your generalized collision state and describe the robot dofs based on the number of qs. :) The world is CRAZY!

It is fair to equate the number of v's (not q's) to the number of dofs -- those have to be the same number. However v's are generalized speeds, not dofs!

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm not sure what you mean "direct implementation". This function SetPosePair() calls a DoFoo() to do the derived-specific work and finishes up with doing some generic base-class work. That seems like the poster boy for classic NVI.

When I'd written my original comment, the only argument against "poster boy"-status that I thought of was the fact that typically the non-virtual and virtual helper methods are defined on the same class. This is unique in that the virtual methods are defined in a different class.

I didn't feel that was enough to justify a new name. As I see it, you've got a virtualDoPoseToPositions() which has one purpose: to be called in a non-virtual SetPosePair(), for me, that's enough to justify DoSetPosePair(). It clearly encodes the relationship between the two functions so observing the DoFoo, you know there's a Foo() out there that uniquely invokes it.

But I'm certainly not going to block on this.

I get your point. Here is my thinking, as explanation not rebuttal: If they had the same name I would feel obligated for them to have related signatures. But the higher-level method speaks System Framework. Alejandro and I are on a vendetta to make the mobilizer interface as Framework-free as possible, so that we're just dealing in floating point numbers. In this case there is a low-level, framework-free logical function (convert pose to generalized coordinates, etc) that can be used in the implementation of the higher-level, framework-friendly API. But the low level one is not the same function as the high level one, so I like the names to be distinct, with each describing what the function actually does.

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Ah..."now have a use for them" suggests not using as of this PR, but you will be doing so later?

I'm using ThrowIfNotFinalized() in this PR (in joint.cc), so I needed to move it here. It seemed weird to move that one and not ThrowIfFinalized() !

[SeanCurtis-TRI]

Reviewed 1 of 1 files at r7, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees amcastro-tri,SeanCurtis-TRI(platform)

---

multibody/plant/multibody_plant.cc line 1284 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

It's not actually overloaded -- the APIs in LinkJointGraph have different names. Likely it would be better to use the optional model instance there like I did here but I'm not currently in the market for revising the topology stuff! After more of it gets a workout in MbP I may take a cleanup pass. It's still internal:: so easily hacked.

My sloppy eye in play again.

---

multibody/tree/mobilizer.h line 354 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

It is fair to equate the number of v's (not q's) to the number of dofs -- those have to be the same number. However v's are generalized speeds, not dofs!

Whereas planners just look at the state space and say, those are the dofs and they define the dimension of c-space.

---

multibody/tree/mobilizer_impl.h line 102 at r3 (raw file):

feel obligated for them to have related signatures.

My long term goal will now be to help you get over that sense of obligation. :) The NVI function never needs to have the same API. It just needs an API that allows the non-virtual function to defer those parts that are strictly necessary to the derived classes. :) Maybe I'll get t-shirts printed. Or, if I make a polo shirt, I'll get Paul to wear it around you. :)

Nevertheless, we're good.

---

multibody/tree/multibody_tree.h line 2526 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I'm using ThrowIfNotFinalized() in this PR (in joint.cc), so I needed to move it here. It seemed weird to move that one and not ThrowIfFinalized() !

Ha! I searched for one of them, but not the other. I had a 50-50 shot and I just picked the wrong one. It all makes fine sense.