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.

[mitiguy]

@sherm1 Sorry for the long delay in restarting this. TRI University program took me offline for a while.
If/when you get a chance, please see the new tests and documentation in quaterion_floating_mobilizer_tests.

Also, there may be trivial items that we resolved in quaternion_floating_mobilizer.cc or quaternion_floating_mobilizer.h, but there is more documentation and work to do there.

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)

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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.

Done. Also included proofs.

---

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

Previously, sherm1 (Michael Sherman) wrote…

nit: N -> N⁺

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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.

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done. Agreed. Mixing notation is not helpful.
The examples now use one or the other.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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.

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done. Complete overhaul here.

[sherm1]

Reviewed tests and already-Done responses, PTAL

@sherm1 reviewed 1 of 3 files at r7, all commit messages.
Reviewable status: 13 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/test/quaternion_floating_mobilizer_test.cc line 380 at r7 (raw file):

  const double is_zero_if_proper_qdot =
      Vector4<double>(q_WB.w(), q_WB.x(), q_WB.y(), q_WB.z()).dot(qdot_WB);
  EXPECT_FALSE(std::abs(is_zero_if_proper_qdot) < 0.1);

BTW this last part isn't really testing anything yet. I was expecting that after you verified that qdot wasn't a proper derivative you would then go on to show that MapQDotToVelocity() would still produce the same angular velocity as you would get with that qdot minus the q-parallel component.

Maybe add something else here? Or should this part be moved elsewhere or removed?

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 449 at r7 (raw file):

  // real number, that angular velocity vr_from_qdot_nonUnit = [0 0 0].
  // Note: The proof of this uses the fact that Nᵣ⁺ * qᵣ = 0.
  qdot_nonUnit.head(4) =

BTW reusing this made the test hard to follow for me. Consider allocating a new qdot_parallel here, and then re-use that in the next test below.

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 467 at r7 (raw file):

  EXPECT_TRUE(CompareMatrices(v_from_qdot_nonUnit, v_from_qdot_unit,
                              16 * kTolerance, MatrixCompareType::relative));
}

BTW awesome tests -- I love it!

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 539 at r7 (raw file):

  mobilizer_->MapQDDotToAcceleration(*context_, qddot, &vdot_redo);
  EXPECT_TRUE(CompareMatrices(vdot_redo, vdot, 16 * kTolerance,
                              MatrixCompareType::relative));

BTW the qdotdot tests here seem good. Probably need to add a parallel/perp treatment like you did for qdot (eventually).

---

multibody/tree/quaternion_floating_mobilizer.h line 314 at r7 (raw file):

  // time-derivative can be calculated as d/dt(q̂ᵣ) = Nᵣ(q̂ᵣ) vᵣ.
  // If the quaternion is context is a non-unit quaternion, then its time-
  // derivative can be calculated d/dt(qᵣ) = Nᵣ(qᵣ) vᵣ = |qᵣ| Nᵣ(q̂ᵣ) vᵣ.

BTW nice!

---

multibody/tree/quaternion_floating_mobilizer.h line 322 at r7 (raw file):

  // This mobilizer's N⁺(q) matrix relates v (6 generalized velocities) to q̇
  // (time derivatives of 7 generalized positions) as v = N⁺(q)⋅q̇, where
  // N⁺(q) = ⎡ Nᵣ(q)⁺  0₃₄ ⎤   0₃₄ is the 4x3 zero matrix.

typo: 4x3 -> 3x4

[mitiguy]

Reviewable status: 5 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.h line 302 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

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.

Improved comment.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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.

Done above.

---

multibody/tree/quaternion_floating_mobilizer.h line 314 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW nice!

Thank you !

---

multibody/tree/quaternion_floating_mobilizer.h line 322 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: 4x3 -> 3x4

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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.

Removed this whole comment as it is redundant with the documentation in quaternion_floating_mobilizer.h

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Yes -- Added a TODO for both the comment and the algorithm. Both need to be fixed.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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.

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

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

Done.

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 380 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this last part isn't really testing anything yet. I was expecting that after you verified that qdot wasn't a proper derivative you would then go on to show that MapQDotToVelocity() would still produce the same angular velocity as you would get with that qdot minus the q-parallel component.

Maybe add something else here? Or should this part be moved elsewhere or removed?

Done.

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 449 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW reusing this made the test hard to follow for me. Consider allocating a new qdot_parallel here, and then re-use that in the next test below.

Done.

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 467 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW awesome tests -- I love it!

Thanks Sherm.

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 539 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW the qdotdot tests here seem good. Probably need to add a parallel/perp treatment like you did for qdot (eventually).

Done. Added TODO.

[sherm1]

Feature review pass done, PTAL.

@sherm1 reviewed 3 of 3 files at r8, all commit messages.
Reviewable status: 24 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 322 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Done.

typo: The subscript on 0₃₃ is wrong, supposed to be 3x4

---

multibody/tree/quaternion_floating_mobilizer.h line 326 at r8 (raw file):
BTW I found this hard to follow. I think the "plausible" phrasing is bad (my fault). Consider something more direct as an intro, like

Since there is no physical significance to the quaternion norm, we don't expect the time derivative to change the length of qᵣ. That is, qdotᵣ must (1) be perpendicular to qᵣ, and (2) be scaled by the same norm as qᵣ. However, we're given an arbitrary qdotᵣ and expect to produce a physically-significant velocity with Nᵣ+ so we have to make sure it is insensitive to length. Consequently Nᵣ+(q) is constructed to remove the scaling from both qᵣ and qdotᵣ, and to project out any qᵣ-parallel component of qdotᵣ.

Then you could just go on to show that if qdot is perpendicular you get a pseudoinverse, and if not etc.

Intro should probably mention that although we use pseudoinverse notation N+ there are some cases where it will differ from a pseudoinverse as explained below.

---

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

Previously, mitiguy (Mitiguy) wrote…

Yes -- Added a TODO for both the comment and the algorithm. Both need to be fixed.

Per f2f this code has been changed in this PR and is now incorrect. It might have been right before. But we should defer changing it until we can make it right and test it.

---

multibody/tree/quaternion_floating_mobilizer.h line 31 at r8 (raw file):

// The translational part of this mobilizer is characterized by generalized
// positions p_FM_F = qₜ = [x, y, z]ᵀ (the position from frame F's origin Fo
// to frame M's origin Mo, expresed in frame F) and generalized velocities

typo (not your fault!)

Suggestion:

expressed

---

multibody/tree/quaternion_floating_mobilizer.h line 275 at r8 (raw file):

  bool is_velocity_equal_to_qdot() const final { return false; }

  // This mobilizer can't use the default implementaion because it is

typo (not your fault!)

Suggestion:

implementation

---

multibody/tree/quaternion_floating_mobilizer.h line 302 at r8 (raw file):

  // N(q) = ⎡ Nᵣ(q)  0₄₃ ⎤   0₄₃ is the 4x3 zero matrix.
  //        ⎣ 0₃₃    I₃₃ ⎦   I₃₃ is the 3x3 identity matrix.
  // Nᵣ(q) = 0.5 * QuaternionFloatingMobilizer::CalcQMatrix() is a 4x3 matrix.

BTW consider showing q-dependence

Suggestion:

CalcQMatrix(q)

---

multibody/tree/quaternion_floating_mobilizer.h line 332 at r8 (raw file):

  // Contextual definition of the 3x4 matrix Nᵣ⁺(qᵣ): denoting q̂ = qᵣ / |qᵣ|,
  // w_FM_F = Nᵣ⁺(q̂ᵣ) * d/dt(q̂_FM) = Nᵣ⁺(qᵣ) * d/dt(qᵣ). Hence, using
  // Nᵣ⁺(qᵣ) = QuaternionRateToAngularVelocityMatrix() accounts for a non-unit

BTW consider showing the q dependence (as is, not normalized)

Suggestion:

QuaternionRateToAngularVelocityMatrix(qᵣ)

---

multibody/tree/quaternion_floating_mobilizer.h line 334 at r8 (raw file):

  // Nᵣ⁺(qᵣ) = QuaternionRateToAngularVelocityMatrix() accounts for a non-unit
  // qᵣ and its corresponding time-derivative q̇ᵣ.
  // Now, considering this mobilizer entirety (both rotation and translation),

typo

Suggestion:

in its entirety

---

multibody/tree/quaternion_floating_mobilizer.h line 381 at r8 (raw file):

  // the quaternion q = [qw, qx, qy, qz] as shown below.
  // @param[in] q a generic quaternion which is not necessarily a unit
  // quaternion or a quaternion associated with a rotation matrix.

nit: I'm not sure what this means: "or a quaternion associated with a rotation matrix". Every quaternion generates a rotation matrix after normalization. Maybe just delete that phrase?

---

multibody/tree/quaternion_floating_mobilizer.h line 395 at r8 (raw file):

  // with w_FM_F (frame M's angular velocity in frame F, expressed in F).
  // Another use of Q_FM and Q̂_FM are for rotational parts of this mobilizer's
  // N and Nplus matrices, e.g., as Nᵣ ≜ 0.5 Q_FM and Nᵣ⁺ = 2 (Q̂_FM)ᵀ.

minor: this is not the correct formula for Nᵣ⁺. Probably you don't want to show how we define Nᵣ⁺ here since it is quite complicated. Even Nhat+ still has to include the projection matrix. Maybe just say Q is used in computation of N and Nplus without giving specific definitions here?

---

multibody/tree/quaternion_floating_mobilizer.h line 400 at r8 (raw file):

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

BTW nice, writing it out with Q's and Qhats here, and with explicit d/dt s makes it right. It would be wrong if N+ and qdot was used instead. More reason not to define N+ here!

---

multibody/tree/quaternion_floating_mobilizer.h line 432 at r8 (raw file):

  }

  // Helper to compute this mobilizer's rotational kinematic map Nᵣ⁺(q̂_FM) from

minor: I don't understand what you mean by this notation: Nᵣ⁺(q̂_FM)
The function here takes an arbitrary q and computes N+(q) which includes proper treatment of the norm. It is therefore different than N+(q̂) because q̂ has a different norm than q.

---

multibody/tree/quaternion_floating_mobilizer.h line 444 at r8 (raw file):

  // @note This function is only documented for use with q_FM, not q_MF.
  // @returns Nᵣ⁺(q_unit), which is the rotational part of the NPlus matrix
  // for w_FM_F (M's angular velocity in F, expressed in F) -- not w_FM_M.

Actually this whole comment is wrong. It doesn't correctly describe what is computed here, which is Nᵣ⁺(q), a matrix that contains two normalization factors and a projection matrix. If you don't feel like describing the details here you could just say that it returns Nᵣ⁺(q) and refer to the comment on DoCalcNplusMatrix() and the .cc implementation for details.

---

multibody/tree/test/quaternion_floating_mobilizer_test.cc line 394 at r8 (raw file):

  Vector6<double> v2;
  mobilizer_->MapQDotToVelocity(*context_, qdot, &v2);
  EXPECT_TRUE(CompareMatrices(v, v2, kTolerance, MatrixCompareType::relative));

BTW nice!

---

multibody/tree/quaternion_floating_mobilizer.cc line 311 at r8 (raw file):

  //        = dqnorm_dq * q̇_FM
  // Note: If the returned matrix is multiplied by s * q̇_FM (where s is any
  // real number and q̇_FM is truly the time-derivative of q_FM), then we can

BTW I don't understand the point of multiplying by "s" here?

---

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

  // real number and q̇_FM is truly the time-derivative of q_FM), then we can
  // prove q_unitᵀ q̇_FM = 0, which means that the q_unit * q_unit.transpose()
  // term in dqnorm_dq is necessary, i.e., q_unit * q_unitᵀ * q̇_FM is zero.

BTW I don't understand what you're getting at with the above note. Can you clarify?

---

multibody/tree/quaternion_floating_mobilizer.cc line 315 at r8 (raw file):

  // term in dqnorm_dq is necessary, i.e., q_unit * q_unitᵀ * q̇_FM is zero.
  /* const Matrix4<T> dqnorm_dq =
      (Matrix4<T>::Identity() - q_unit * q_unit.transpose()) / q_norm; */

nit: weird change of comment style in the middle of a comment

---

multibody/tree/quaternion_floating_mobilizer.cc line 316 at r8 (raw file):

  /* const Matrix4<T> dqnorm_dq =
      (Matrix4<T>::Identity() - q_unit * q_unit.transpose()) / q_norm; */
  // const Matrix4<T> dqnorm_dq = Matrix4<T>::Identity() / q_norm;

minor: comment leftover from debugging?

---

multibody/tree/quaternion_floating_mobilizer.cc line 320 at r8 (raw file):

      (Matrix4<T>::Identity() - q_unit * q_unit.transpose()) / q_norm;

  // From documentation in CalcQMatrix(), Nᵣ⁺(q_unit) = 2 * Q(q_unit)ᵀ.

BTW this comment (referring to the incorrect docs) is wrong, though it does match the wrong name below. We need a different notation here, because Nᵣ⁺(q_unit) is actually
2 * Q(q_unit)ᵀ * dnorm_dq (as you can see in the return statement).

---

multibody/tree/quaternion_floating_mobilizer.cc line 321 at r8 (raw file):

  // From documentation in CalcQMatrix(), Nᵣ⁺(q_unit) = 2 * Q(q_unit)ᵀ.
  const Eigen::Matrix<T, 3, 4> NrPlus_q_unit = CalcTwoTimesQMatrixTranspose(

BTW this needs a different name, like NrPlus_q_unit_no_projection or something like that.

---

multibody/tree/quaternion_floating_mobilizer.cc line 326 at r8 (raw file):

  // Returns the matrix that when multiplied by q̇_FM produces w_FM_F
  // (M's angular velocity in F, expressed in F). Note: When q_FM is a unit
  // quaternion (so q_norm = 1), the returned value is denoted Nᵣ⁺(q̂_FM).

BTW not correct. I think we used Nhat+ for the quantity you're referring to in the google doc we were working on. It can't be N+ because it doesn't yet include the projection matrix dnorm_dq.

---

multibody/tree/quaternion_floating_mobilizer.cc line 400 at r8 (raw file):

  // | ωy | = 2.0 | -qy    qz    qw  -qx | | q̇x |
  // ⌊ ωz ⌋       | -qz   -qy    qx   qw | | q̇y |
  //                                       ⌊ q̇z ⌋

Ouch! That's not N+ since it is missing the normalization and projection.

---

multibody/tree/quaternion_floating_mobilizer.cc line 409 at r8 (raw file):

  // TODO(Mitiguy) The next comment is not fully correct and is misleading.
  //  Fix the normalization to mimic what was done in DoCalcNPlusMatrix().

BTW make this TODO more visible and note that the code is wrong. Should have an issue that notes this bug.

---

multibody/tree/quaternion_floating_mobilizer.cc line 454 at r8 (raw file):

template <typename T>
void QuaternionFloatingMobilizer<T>::DoMapAccelerationToQDDot(

BTW does these two need a TODO?