MapQDDotToAcceleration() and MapAccelerationToQDDot() for quaternion floating mobilizer.

[mitiguy]

This is one in a series of PRs to help address issue MapQddotToAcceleration and friends #22630. It creates DoMapAccelerationToQDDot() and DoMapQDDotToAcceleration() for a quaternion floating mobilizer.

FYI: Since mobilizers are Drake internal classes, after the internal mobilizer work is complete, there will be PRs (code and testing) for the public API in MultibodyPlant to address issue MapQddotToAcceleration and friends #22630.

---

This change is 

[mitiguy]

Feature review +assignee:@sherm1

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

[sherm1]

Looks good -- some notation changes needed per f2f. LMK when to take another look.

Reviewed 3 of 3 files at r1, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

---

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

  static Eigen::Matrix<T, 4, 3> CalcLMatrix(const Quaternion<T>& q);

  // Returns the 4x3 Nᵣ(q) matrix that relates q̇_FM = Nᵣ(q) * w_FM_F, where

BTW Nᵣ was unfamiliar notation to me. Later I see that you mean "the rotational part of the full N matrix". Consider noting that here.

---

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

      const Quaternion<T>& q_FM) {
    return CalcLMatrix(
        {0.5 * q_FM.w(), 0.5 * q_FM.x(), 0.5 * q_FM.y(), 0.5 * q_FM.z()});

BTW since you defined the L matrix above as not including the scalar factor, it seems odd to call CalcLMatrix() to return the scaled result. Consider calling this something like CalcScaledLMatrix(0.5, q_FM).

---

multibody/tree/quaternion_floating_mobilizer.cc line 489 at r1 (raw file):

  // generalized positions q̈_FM = q̈ᵣ = [q̈w, q̈x, q̈y, q̈z]ᵀ are related to the
  // 1st-derivatives of the generalized velocities ẇ_FM_F = v̇ᵣ = [ẇx, ẇy, ẇz]ᵀ
  // as q̈_FM = Ṅᵣ(q,q̇)⋅v + Nᵣ(q)⋅v̇, where Nᵣ(q) is the 3x4 matrix below and

Dimensions backwards?

Suggestion:

4x3

---

multibody/tree/quaternion_floating_mobilizer.cc line 490 at r1 (raw file):

  // 1st-derivatives of the generalized velocities ẇ_FM_F = v̇ᵣ = [ẇx, ẇy, ẇz]ᵀ
  // as q̈_FM = Ṅᵣ(q,q̇)⋅v + Nᵣ(q)⋅v̇, where Nᵣ(q) is the 3x4 matrix below and
  // Ṅᵣ(q,q̇)⋅v = -0.25 ω² q_FM, where ω² = |w_FM_F|² and w_FM_F = [wx, wy, wz]ᵀ.

nit: you started to use vᵣ and qᵣ above but then dropped the "r".

You could also consider a simpler notation where you call the (rotation, translation) generalized speeds (v, w) and the generalized coordinates (q, p) to avoid the subscripts. I think the "r" notation is OK too but should be used consistently, but:

• Dragging around the little subscripts doesn't necessarily help readability.
• you can't get the subscripts into code so your documentation and code won't match.

---

multibody/tree/quaternion_floating_mobilizer.cc line 495 at r1 (raw file):

  // | q̈x | = 0.5 |  qw    qz   -qy | | ẇy | - 0.25 ω² | qx |
  // | q̈y |       |  qw   qw    qx  | ⌊ ẇz ⌋           | qy |
  // ⌊ q̈z ⌋       ⌊  qy   -qx    qw ⌋                  ⌊ qz ⌋

BTW if this matrix is "L" it would be better to use that notation. As it is I have to carefully examine 12 entries to determine whether this is the same matrix! That's difficult for the reader and also error-prone for writers (later someone less careful than you may try to copy this style).

---

multibody/tree/quaternion_floating_mobilizer.cc line 523 at r1 (raw file):

  // For the rotational part of this mobilizer, the 1st-derivatives of the
  // generalized velocities ẇ_FM_F = v̇ᵣ = [ẇx, ẇy, ẇz]ᵀ are related to the
  // 2nd-derivatives of the generalized positions q̈̇_FM = q̇ᵣ = [q̈w, q̈x, q̈y, q̈z]ᵀ

BTW this symbol didn't look like a double dot: q̈̇_FM, while the same symbol elsewhere did: q̈_FM. I think there's a typo.

[tyler-yankee]

@drake-jenkins-bot retest this please

Attention contributor: in order for Jenkins to pass, you will need to rebase (or merge) your PR up to the latest master branch, and re-push. If you need help with that, let us know. You can push that change separately, or you can combine it with a push of review fixes.

[mitiguy]

@sherm1 This is ready for another look.

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

---

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

Previously, sherm1 (Michael Sherman) wrote…

BTW Nᵣ was unfamiliar notation to me. Later I see that you mean "the rotational part of the full N matrix". Consider noting that here.

Done. Removed mention of Nᵣ(q).

---

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

Previously, sherm1 (Michael Sherman) wrote…

BTW since you defined the L matrix above as not including the scalar factor, it seems odd to call CalcLMatrix() to return the scaled result. Consider calling this something like CalcScaledLMatrix(0.5, q_FM).

Done.

---

multibody/tree/quaternion_floating_mobilizer.cc line 489 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Dimensions backwards?

Done.

---

multibody/tree/quaternion_floating_mobilizer.cc line 490 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: you started to use vᵣ and qᵣ above but then dropped the "r".

You could also consider a simpler notation where you call the (rotation, translation) generalized speeds (v, w) and the generalized coordinates (q, p) to avoid the subscripts. I think the "r" notation is OK too but should be used consistently, but:

• Dragging around the little subscripts doesn't necessarily help readability.
• you can't get the subscripts into code so your documentation and code won't match.

Done. Cleaned up.

---

multibody/tree/quaternion_floating_mobilizer.cc line 495 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW if this matrix is "L" it would be better to use that notation. As it is I have to carefully examine 12 entries to determine whether this is the same matrix! That's difficult for the reader and also error-prone for writers (later someone less careful than you may try to copy this style).

Done.

---

multibody/tree/quaternion_floating_mobilizer.cc line 523 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this symbol didn't look like a double dot: q̈̇_FM, while the same symbol elsewhere did: q̈_FM. I think there's a typo.

Done.

[sherm1]

Better, thanks. I'm still bothered by the "L matrix" stuff, and I think I know why now. Sometimes you treat it as a function L(q) and other times you talk about it as though it was a particular matrix L. I see now that it really is a matrix-valued function that takes a quaternion as input and rearranges the elements into a 4x3. It produces a bunch of different matrices depending on what's in the quaternion: 2q, q/2, or qdot. So it's wrong to have a function like CalcLMatrixTimesTwo() -- there is no particular L matrix for it to double. Rather, that is the result you get when you compute the function L(2q). In another place you write L̇(q̇) but that isn't right if L is a function -- this is just the result of L(q̇). You could say that L(q̇) = d/dt L(q) but those are two different "L"s.

I'm not sure how best to fix this. Maybe rename CalcLMatrix() to something like CalcQuaternionCoefficientMatrix() ? Then you could give unambiguous names to the resulting matrices, for example N=CalcQuaternionCoefficientMatrix(q_FM/2)
N⁺=CalcQuaternionCoefficientMatrix(2 * q_FM).transpose(), etc.

Let's discuss if it doesn't seem obvious how to straighten this out.

Reviewed 1 of 2 files at r2, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

---

multibody/tree/quaternion_floating_mobilizer.cc line 302 at r2 (raw file):
BTW The only "quaternion product" I've heard of would be the product of two quaternions (like a composition of rotations). This one is a 3-vector times a quaternion. Is that really a thing? The first thing I got when a googled it was:

The quaternion product, also known as the [Hamilton product], is a specific way to multiply two quaternions, resulting in another quaternion.

---

multibody/tree/quaternion_floating_mobilizer.cc line 313 at r2 (raw file):

  // That is:
  //           |         -qv_Fᵀ    |   ⌈ -qx   -qy   -qz ⌉
  // L(q_FM) = | qs * Id - [qv_F]x | = |  qw    qz   -qy |

minor: qs is undefined in all the expressions above. Replace with qw.

[mitiguy]

I understand what are saying. I do not like passing in q_FM/2 or 2*q_FM directly into the function. In all situations, we are computing either 0.5 * L or 2 * L and it happens that it can be more efficiently calculated by using mathematical trickery (which is purely under-the-hood, it is not important for the calling function know this). As for passing q̇ to L(q̇), yes, I think there is a different issue here. Perhaps the argument to CalcLMatrix() should be a Vector4?. More to think about.

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

---

multibody/tree/quaternion_floating_mobilizer.cc line 302 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW The only "quaternion product" I've heard of would be the product of two quaternions (like a composition of rotations). This one is a 3-vector times a quaternion. Is that really a thing? The first thing I got when a googled it was:

The quaternion product, also known as the [Hamilton product], is a specific way to multiply two quaternions, resulting in another quaternion.

I was debating removing this section completely.
Thoughts ?

---

multibody/tree/quaternion_floating_mobilizer.cc line 313 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: qs is undefined in all the expressions above. Replace with qw.

OK -- but perhaps unnecessary if I remove the previous documentation.
If you prefer to keep the previous documentation, I'll define qs, etc.

[sherm1]

FWIW I don't think it's bad to pass quaternions into the function that returns the coefficient matrix (currently misnamed CalcLMatrix). q/2 and 2*q are both quaternions (just not unit quaternions). And according to the internet (always reliable:) the time derivative of a quaternion is also a quaternion. So IMO if you rename that function to something clunky but accurate, you can just pass it a quaternion of any sort and get back the 4x3 coefficient matrix.

Reviewed 1 of 2 files at r2.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

---

multibody/tree/quaternion_floating_mobilizer.cc line 302 at r2 (raw file):

Previously, mitiguy (Mitiguy) wrote…

I was debating removing this section completely.
Thoughts ?

Makes sense to me that while you're making sense of this code you should remove the comments that aren't helpful! (Replacing them with some useful comments)

[mitiguy]

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

---

multibody/tree/quaternion_floating_mobilizer.cc line 302 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Makes sense to me that while you're making sense of this code you should remove the comments that aren't helpful! (Replacing them with some useful comments)

Done.

---

multibody/tree/quaternion_floating_mobilizer.cc line 411 at r3 (raw file):

  // Since N⁺ᵣ(qᵣ) = 2 * Q(q_FM)ᵀ, where Q(q_FM) is linear in the elements of
  // q_FM = [qw, qx, qy, qz]ᵀq_FM, hence Ṅ⁺ᵣ(qᵣ,q̇ᵣ) = 2 * Q(q̇_FM)ᵀ.

Self-blocking: Rewrite this comment.
a. Remove extraneous q_FM.
b. use Q_FM.
c. Reference documentation for CalcQMatrix().

Suggestion:

  // In view of the documentation in CalcQMatrix(), since
  // N⁺ᵣ(qᵣ) = 2 * (Q_FM)ᵀ, where Q_FM is linear in the elements of
  // q_FM = [qw, qx, qy, qz]ᵀ, hence Ṅ⁺ᵣ(qᵣ,q̇ᵣ) = 2 * (Q̇_FM)ᵀ.

---

multibody/tree/quaternion_floating_mobilizer.cc line 483 at r3 (raw file):

    EigenPtr<VectorX<T>> vdot) const {
  // This function maps qddot to vdot by calculating v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
  // It first calculates the rotational part, then the translational part.

Self-blocking. Removing the previous line (unnecessary).
// It first calculates the rotational part, then the translational part.

---

multibody/tree/quaternion_floating_mobilizer.cc line 501 at r3 (raw file):

  // Since QuaternionRateToAngularVelocityMatrix() calculates N⁺ᵣ(qᵣ), we use
  // this function to calculate v̇ᵣ = N⁺ᵣ(q)⋅q̈_FM.

Self-blocking.
Change this comment.

Suggestion:

  // To mimic DoMapQDotToVelocity(), use QuaternionRateToAngularVelocityMatrix() 
  // to calculate N⁺ᵣ(qᵣ) and use it to calculate v̇ᵣ = N⁺ᵣ(q)⋅q̈_FM.

[sherm1]

I found some more stuff to worry about here, Paul. PTAL

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

---

multibody/tree/quaternion_floating_mobilizer.h line 353 at r3 (raw file):

  // q̈_FM = 0.5 * Q_FM * ẇ_FM_F - 0.25 ω² q_FM    Note: ω² = |w_FM_F|²
  // w_FM_F = 2 * (Q_FM)ᵀ * q̇_FM
  // ẇ_FM_F = 2 * (Q_FM)ᵀ * q̈_FM

BTW consider mentioning here (or somewhere) that we also use the notation
Nᵣ ≜ Q_FM / 2 and Nᵣ⁺ ≜ 2 Q_FMᵀ

---

multibody/tree/quaternion_floating_mobilizer.h line 367 at r3 (raw file):

  // quaternion or a quaternion associated with a rotation matrix.
  // @see QuaternionFloatingMobilizer::CalcQMatrix().
  static Eigen::Matrix<T, 4, 3> CalcQMatrixOverTwo(const Quaternion<T>& q) {

minor: this function and the TwoTimes one shouldn't exist. So there should be a comment saying why they do (i.e. Eigen's scalar multiply and divide fail when T=Expression). That will serve as fair warning to someone who wants to get rid of these, as well as possibly guide someone to figure out how to fix the problem.

BTW I still think the better fix for this would be to define a little private inline helper function in this class:

// Provide missing scalar multiply for Quaternions.
static Quaternion<T> scale(double s, Quaternion<T> q) const {
  return {s*q.w(), s*q.x(), s*q.y(), s*q.z()};
}

and then do everything with CalcQMatrix(scale(0.5, q_FM)) and CalcQMatrix(scale(2, q_FM)).transpose(). If you don't need scaling you would still be able to use CalcQMatrix(qdot) or whatever.

Then you could get rid of both of the trivial but ugly helper functions and their comments.

---

multibody/tree/quaternion_floating_mobilizer.h line 382 at r3 (raw file):

  // Helper to compute the kinematic map N⁺(q) from quaternion time derivative
  // to angular velocity for which w_FM_F = N⁺(q)⋅q̇_FM.

nit: I'm not sure about the notation here. The argument should be q_FM if it is going to work with q_FM's time derivative, right? And what about the frame for the resulting matrix? Is it N⁺_F? What are the limitations of this function? Could it be used with q_MF? Or does its implementation mean that only q_FM can work? That needs to be documented. Also I believe the implementation normalizes the input quaternion, yet the documentation doesn't mention that important fact! Does that mean that to use it as described one must compute qdot of the normalized q rather than the qdot we get, which is qdot of the unnormalized q? Looking at the code, I think the routine is designed to produce a scaled N⁺ that is usable with the unnormalized qdots. This should be made clear in the documentation.

BTW also I don't understand the awkward name here. Why isn't this just CalcNPlus() or CaclNPlus_F() or something like that? It's not at all obvious from the name of the function that it is actually computing something we have another (much simpler) name for.

Suggestion:

w_FM_F = N⁺(q_FM)⋅q̇_FM.

---

multibody/tree/quaternion_floating_mobilizer.cc line 325 at r3 (raw file):

  // Upper-left block (rotational part of the N matrix) is 0.5 * Matrix(q_FM).
  // See QuaternionFloatingMobilizer::CalcQMatrix() for details.
  N->template block<4, 3>(0, 0) = CalcQMatrixOverTwo(get_quaternion(context));

BTW looks like we work directly with the unnormalized quaternion here so the returned N will generate an unnormalized qdot from the angular velocity, right? Seems like that should be clarified somewhere to contrast with the N+ stuff?

---

multibody/tree/quaternion_floating_mobilizer.cc line 340 at r3 (raw file):

  // See QuaternionFloatingMobilizer::CalcQMatrix() for details.
  Nplus->template block<3, 4>(0, 0) =
      QuaternionRateToAngularVelocityMatrix(get_quaternion(context));

BTW this I believe expects an unnormalized derivative, does something tricky with it so it produces a physical angular velocity. Clarify?

[mitiguy]

@sherm1 This is ready for another look.
Ideally, this is converging.

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

---

multibody/tree/quaternion_floating_mobilizer.h line 353 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider mentioning here (or somewhere) that we also use the notation
Nᵣ ≜ Q_FM / 2 and Nᵣ⁺ ≜ 2 Q_FMᵀ

Done.

---

multibody/tree/quaternion_floating_mobilizer.h line 367 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this function and the TwoTimes one shouldn't exist. So there should be a comment saying why they do (i.e. Eigen's scalar multiply and divide fail when T=Expression). That will serve as fair warning to someone who wants to get rid of these, as well as possibly guide someone to figure out how to fix the problem.

BTW I still think the better fix for this would be to define a little private inline helper function in this class:

// Provide missing scalar multiply for Quaternions.
static Quaternion<T> scale(double s, Quaternion<T> q) const {
  return {s*q.w(), s*q.x(), s*q.y(), s*q.z()};
}

and then do everything with CalcQMatrix(scale(0.5, q_FM)) and CalcQMatrix(scale(2, q_FM)).transpose(). If you don't need scaling you would still be able to use CalcQMatrix(qdot) or whatever.

Then you could get rid of both of the trivial but ugly helper functions and their comments.

Done. Added @note to provide fair warning.
I like the scale function that you propose (or something named similarly). I think it belongs as a public function in quaternion.h -- with testing, etc. However, not in this PR.

For future consideration, something like the following in quaternion.h ?

// Pre-multiply a Quaternion by a scalar.
// @note: One reason this function exists is that multiplying or dividing an
// Eigen Quaternion by a scalar fails when type <T> is expression.
static Quaternion<T> scale(T& s, Quaternion<T> q) const { 
   return {s*q.w(), s*q.x(), s*q.y(), s*q.z()}; 
}

---

multibody/tree/quaternion_floating_mobilizer.h line 382 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: I'm not sure about the notation here. The argument should be q_FM if it is going to work with q_FM's time derivative, right? And what about the frame for the resulting matrix? Is it N⁺_F? What are the limitations of this function? Could it be used with q_MF? Or does its implementation mean that only q_FM can work? That needs to be documented. Also I believe the implementation normalizes the input quaternion, yet the documentation doesn't mention that important fact! Does that mean that to use it as described one must compute qdot of the normalized q rather than the qdot we get, which is qdot of the unnormalized q? Looking at the code, I think the routine is designed to produce a scaled N⁺ that is usable with the unnormalized qdots. This should be made clear in the documentation.

BTW also I don't understand the awkward name here. Why isn't this just CalcNPlus() or CaclNPlus_F() or something like that? It's not at all obvious from the name of the function that it is actually computing something we have another (much simpler) name for.

Done. Notation and documentation updated.
Note: Theoretically, |qdot| != 1, so "unnormalized qdots" may be misleading.
I think I understand your idea, so I documented it accordingly.
A much better more explicit name is not clear to me yet. I added a TODO().

---

multibody/tree/quaternion_floating_mobilizer.cc line 325 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW looks like we work directly with the unnormalized quaternion here so the returned N will generate an unnormalized qdot from the angular velocity, right? Seems like that should be clarified somewhere to contrast with the N+ stuff?

Done. Comments added to grouped functions in quaternion_floating_mobilizer.h

---

multibody/tree/quaternion_floating_mobilizer.cc line 340 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this I believe expects an unnormalized derivative, does something tricky with it so it produces a physical angular velocity. Clarify?

Done. Comments added here.
Comments also added to grouped functions in quaternion_floating_mobilizer.h

---

multibody/tree/quaternion_floating_mobilizer.cc line 411 at r3 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Self-blocking: Rewrite this comment.
a. Remove extraneous q_FM.
b. use Q_FM.
c. Reference documentation for CalcQMatrix().

Done.

---

multibody/tree/quaternion_floating_mobilizer.cc line 483 at r3 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Self-blocking. Removing the previous line (unnecessary).
// It first calculates the rotational part, then the translational part.

Done.

---

multibody/tree/quaternion_floating_mobilizer.cc line 501 at r3 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Self-blocking.
Change this comment.

Done.

[sherm1]

@sherm1 reviewed 1 of 3 files at r4, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 395 at r4 (raw file):

  // time-derivative of the quaternion constraint (q_FM)ᵀ * q_FM = constant, so
  // (q̇_FM)ᵀ * q_FM + (q_FM)ᵀ * q̇_FM = 2 * (q_FM)ᵀ * q̇_FM = 0.  However, since
  // the qdot above is arbitrary, this is not true.

minor: this comment is wrong: this qdot does satisfy the constraint

[sherm1]

Post-vacation review resurrected! PTAL

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

---

multibody/tree/quaternion_floating_mobilizer.h line 297 at r4 (raw file):
minor: I don't think it will be clear to the reader what you mean here. Maybe illustrate, something like:

That is, if q is the unnormalized quaternion taken directly from the context, and w_FM_F an angular velocity of the outboard body in the inboard F frame, we compute N(q) such that qdot = N(q) *w_FM_F with qdot=dq/dt (qdot is the time derivative of the unnormalized q). Note that if w_FM_F is taken from the same context, then the resulting qdot is the time derivative of the q from that context.

---

multibody/tree/quaternion_floating_mobilizer.h line 302 at r4 (raw file):
minor: this needs some clarification also. Maybe something like:

That is, if q is the unnormalized quaternion taken directly from the context, and qdot is a possible dq/dt, we calculate N⁺(q) such that w_FM_F = N⁺(q) * qdot is the angular velocity of the outboard body in the inboard frame F corresponding to that qdot.

I think we could also make some kind of promise here relating N and N⁺, such as "With these definitions it is always the case that w_FM_F = N⁺ (N w_FM_F )."

Can we be more specific about the meaning of a "possible" qdot for the unnormalized q? For a general qdot it won't be the case that NN⁺qdot returns the original qdot. However if the qdot resulted from qdot=Nw, then we do have qdot=NN⁺qdot (I think!).

My thought on the meaning of "possible" here: dot(qdot, q)==0, that is, qdot leaves the length of q unchanged.

---

multibody/tree/quaternion_floating_mobilizer.cc line 319 at r4 (raw file):

  // Returns the matrix that when multiplied by q̇_FM produces
  // w_FM_F (M's angular velocity in F, expressed in F).
  return NrPlus_q_unit * dqnorm_dq;

minor: should clarify somehow that the returned value is what you call Nᵣ⁺(q_FM) below.

---

multibody/tree/quaternion_floating_mobilizer.cc line 342 at r4 (raw file):

  // See QuaternionFloatingMobilizer::CalcQMatrix() for details.
  // Note: Contextual definition of Nᵣ⁺ -- denoting q̂_FM = q_FM / |q_FM|,
  // w_FM_F = Nᵣ⁺ * d/dt(q̂_FM), where w_FM_F is frame M's angular velocity

minor: something seems fuzzy here. The code below (in QuaternionRateToAngularVelocityMatrix) goes through hell to produce an Nᵣ⁺ such that it can work with the unnormalized qdot, that is w_FM_F = Nᵣ⁺ d/dt(q_FM) (no hat on q_FM). It has a built-in projection matrix that cleans up the bad qdot so that the result is physical.

---

multibody/tree/quaternion_floating_mobilizer.cc line 417 at r4 (raw file):

  // In view of the documentation in CalcQMatrix(), since
  // N⁺ᵣ(q_FM) = 2 * (Q_FM)ᵀ, where Q_FM is linear in the elements of

This isn't right. For unnormalized q, Nᵣ⁺(q) has that normalization time derivative stuff in it besides the Q matrix term.

---

multibody/tree/quaternion_floating_mobilizer.cc line 418 at r4 (raw file):

  // In view of the documentation in CalcQMatrix(), since
  // N⁺ᵣ(q_FM) = 2 * (Q_FM)ᵀ, where Q_FM is linear in the elements of
  // q_FM = [qw, qx, qy, qz]ᵀ, hence Ṅ⁺ᵣ(q̇_FM) = 2 * (Q̇_FM)ᵀ.

nit: elsewhere you wrote Nᵣ⁺ rather than N⁺ᵣ -- should stick to one.

---

multibody/tree/quaternion_floating_mobilizer.cc line 447 at r4 (raw file):

    const Eigen::Ref<const VectorX<T>>& qdot, EigenPtr<VectorX<T>> v) const {
  const Quaternion<T> q_FM = get_quaternion(context);
  // Angular component, w_FM_F = Nᵣ⁺(q̂_FM)⋅q̇_FM.

minor: This should be w_FM_F =Nᵣ⁺(q_FM)⋅q̇_FM (unnormalized q_FM) -- see the code below where you do use the unnormalized q.

Suggestion:

Nᵣ⁺(q_FM)

---

multibody/tree/quaternion_floating_mobilizer.cc line 509 at r4 (raw file):

  // To mimic DoMapQDotToVelocity(), use QuaternionRateToAngularVelocityMatrix()
  // to calculate N⁺ᵣ(q̂_FM) and use it to calculate v̇ᵣ = N⁺ᵣ(q̂_FM)⋅q̈_FM.

minor: same here -- the comment (normalized) is inconsistent with the code right below which uses (correctly IMO) the unnormalized q_FM.

---

multibody/tree/quaternion_floating_mobilizer.h line 296 at r4 (raw file):

  }

  // This function calculates this mobilizers N matrix using the quaternion in

nit: mobilizers -> mobilizer's (and below)

---

multibody/tree/quaternion_floating_mobilizer.h line 301 at r4 (raw file):

                     EigenPtr<MatrixX<T>> N) const final;

  // This function calculates this mobilizers N matrix using the quaternion in

nit: N -> N⁺

---

multibody/tree/quaternion_floating_mobilizer.h line 307 at r4 (raw file):

  // This function calculates this mobilizers Ṅ matrix using the quaternion in
  // context, _without_ normalization.

BTW isn't it sufficient just to say that Ṅ = d/dt N ? It doesn't seem like we get to vote on normalization or not in this case.

---

multibody/tree/quaternion_floating_mobilizer.h line 311 at r4 (raw file):

                        EigenPtr<MatrixX<T>> Ndot) const final;

  // TODO(Mitiguy) Provide a precise definition of this function.

BTW same here. Can't we just say that Ṅ⁺ is the time derivative of N⁺ ?

---

multibody/tree/quaternion_floating_mobilizer.h line 363 at r4 (raw file):

  // q̈_FM = 0.5 * Q_FM * ẇ_FM_F - 0.25 ω² q_FM    Note: ω² = |w_FM_F|²
  // w_FM_F = 2 * (Q̂_FM)ᵀ * q̇_FM
  // ẇ_FM_F = 2 * (Q̂_FM)ᵀ * q̈_FM

These last two equations seem fuzzy about normalization. They involve the product of the normalized Q_FM

[sherm1]

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

---

multibody/tree/quaternion_floating_mobilizer.h line 304 at r5 (raw file):

  // Nᵣ(q) = 0.5 * QuaternionFloatingMobilizer::CalcQMatrix() is a 4x3 matrix.
  // Note: The time-derivative of the quaternion qᵣ in context can be calculated
  // q̇ᵣ = Nᵣ(q) w_FM_F, where w_FM_F is frame M's angular velocity in frame F,

BTW per f2f consider using vᵣ instead of w_FM_F to avoid talking about the frames.

---

multibody/tree/quaternion_floating_mobilizer.h line 306 at r5 (raw file):

  // q̇ᵣ = Nᵣ(q) w_FM_F, where w_FM_F is frame M's angular velocity in frame F,
  // expressed in F. For a unit quaternion q̂ᵣ, we prove d/dt(q̂ᵣ) satisfies the
  // "orthogonality constraint" i.e., d/dt(q̂ᵣ ⋅ q̂ᵣ = 1)  =>  q̂ᵣ ⋅ d/dt(q̂ᵣ) = 0

BTW per f2f you could make this stronger since it also applies to unnormalized q, e.g. q dot q = constant means qdot is perpendicular.

[sherm1]

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

---

multibody/tree/quaternion_floating_mobilizer.h line 327 at r5 (raw file):

  // Nᵣ⁺(q) is a 3x4 matrix formed by QuaternionRateToAngularVelocityMatrix().
  // Note: Contextual definition of Nᵣ⁺ -- denoting q̂_FM = q_FM / |q_FM|,
  // w_FM_F = Nᵣ⁺ * d/dt(q̂_FM), where w_FM_F is frame M's angular velocity

BTW (dropping r suffix in this comment) I think you want to show that N+ works correctly with unnormalized q. Something like v = N+(q) d/dt(q) = N+(qhat) d/dt(qhat) -- that is, it doesn't matter whether the original quaternion was normalized. As long as the q and qdot match, N+(q)*qdot will give the same v.