[math] Add axial rotation and transform functions

[sherm1]

Adds (Advanced) rotation matrix and rigid transform functions for exploiting known special structure of those matrices. These arise for common 1-dof revolute and prismatic joints when implemented with axis-aligned mobilizers.

This is yak shave for using optimal F & M frames for these joints (milestone 3 in this doc). These functions need specialized feature review so get their own PR. You can see these functions in use for revolute mobilizers in WIP PR #22253.

The new functions are templated inlines that are specialized implementations of existing more-general functions so they would add no value in Python and are not intended to be wrapped.

---

This change is 

[sherm1]

+@mitiguy for feature review, please

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

[mitiguy]

@sherm1 just getting started. We'll do more tomorrow.

Reviewed all commit messages.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers

---

math/rigid_transform.h line 295 at r1 (raw file):

  }

  /// (Advanced) Create a transform Xa_AB consisting of only an axial rotation

FYI consider renaming to aX_BC and document how to handle negative axes.

Code snippet:

  /// (Advanced) Create an axial rigid transform aX_AB for an axial rotation 
  /// of `theta` radians about the +x, +y, or +z vector direction.
  /// @param[in] axis 0, 1, or 2, corresponding to +x, +y, or +z directions.
  /// @note An axial rigid transform has zero translation.
  /// @note To create an axial rotation of `theta` radians about the
  /// -x, -y, or -z vector direction, simply use the corresponding
  /// +x, +y, or +z vector direction with negative theta (-theta).

---

math/rigid_transform.h line 307 at r1 (raw file):

      return RigidTransform<T>(RotationMatrix<T>::MakeZRotation(theta));
  }

FYI Possible documentation update.

Code snippet:

  /// (Advanced) Uses an axial rigid transform aX_BC to efficiently re-express a
  /// vector v_C that is expressed in a frame C to be expressed in a frame B.
  /// @param[in] axis 0, 1, or 2, corresponding to +x, +y, or +z directions.
  /// @pre aX_BC.rotation() is _exactly_ a rotation about the given axis and
  ///   Xa_BC.translation() is _exactly_ zero (zero translation).

[sherm1]

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

---

math/rigid_transform.h line 295 at r1 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI consider renaming to aX_BC and document how to handle negative axes.

Done (except per f2f not the negated axes)

---

math/rigid_transform.h line 307 at r1 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Possible documentation update.

Done

[mitiguy]

@sherm1 I have made a few more suggestions on the earlier functions. Let me know if you like them. If so, I can do similarly for the rest of the functions (if it makes it easy to cut/paste for you). If not, I can speed through and make smaller suggestions.

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

---

math/rigid_transform.h line 343 at r5 (raw file):

  /// @pre aX_BC.rotation() is _exactly_ a rotation about the given axis and
  ///   aX_BC.translation() is _exactly_ zero (constant entries are all 0s and
  ///   1s).

FYI I think there is only one constant entry with a 1.

---

math/rigid_transform.h line 322 at r1 (raw file):

        Xa_BC.rotation(), v_C);
  }

FYI Documentation thoughts for you.

Code snippet:

  /// (Advanced) Efficiently resets the 4 relevant elements of an axial rigid 
  ///   transform (without setting or changing the other 8 elements).
  /// @tparam axis 0, 1, or 2, corresponding to +x, +y, or +z directions.
  /// @param[in] theta rotation angle in radians.
  /// @param[in,out] aX_BC on input is an axial rigid transform about the same
  ///   specified `axis`. On output, 4 elements of aX_BC have been reset. 
  /// @see the other same-named function if sin(θ) and cos(θ) are precalculated.
  /// @pre aX_BC.rotation() is _exactly_ a rotation about the given axis and
  ///   aX_BC.translation() is _exactly_ zero (zero translation).

[sherm1]

@mitiguy this is ready for another look. I made doxygen groups for these methods and updated all the comments per your suggestions. To generate the doxygen quickly you can use this command:

bazel run //doc/doxygen_cxx:build -- --serve --quick drake.math

then look at the "Class List" for drake::math.

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

---

math/rigid_transform.h line 322 at r1 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Documentation thoughts for you.

Done

---

math/rigid_transform.h line 343 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI I think there is only one constant entry with a 1.

Done

[mitiguy]

Thanks for the helpful instructions. I used git pr and those instructions, so I was able to view the Doxygen documentation. It formats well. More progress made (comments made). More to do. Will work on it tomorrow (Friday).
Note: CI is failing.

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

---

math/rigid_transform.h line 299 at r6 (raw file):

  /// particular simplifications, we can exploit that knowledge to reduce the
  /// computation required to work with it. The simplifications we understand
  /// here, and the notation we use if X_BC has that simplification, are:

FYI: Suggest rewording.

Suggestion:

  /// computation required to form it and use it. The simplifications and
  /// associated notations for the rigid transform X_BC are:

---

math/rigid_transform.h line 321 at r6 (raw file):

  /// efficiency for various operations involving axial %RigidTransform
  /// arguments. Similarly, an axial translation is a translation along one of
  /// the coordinate axes.

FYI: My suggestion to make it more concise and explicit.

Suggestion:

  /// Axial rotations are %RotationMatrix objects that represent a rotation 
  /// about a basis vector x, y, or z and the %RotationMatrix has only four
  /// of its nine elements that depend on the rotation angle (the other five
  /// elements are zero or one). Axial rotation methods are templatized by the
  /// the axis number 0, 1, 2 (denoting +x, +y, +z, respectively) and exploit
  /// the axis number to enhance efficiency for operations involving axial 
  /// %RotationMatrices and axial %RigidTransforms. Similarly, an axial 
  /// translation %RigidTransform is a translation along one of +x, +y, +z.

---

math/rigid_transform.h line 325 at r6 (raw file):

  /// @note There are no Python bindings for these methods since there is
  /// nothing to be gained by using them in Python. Use the general equivalents
  /// instead.

FYI More explicit

Suggestion:

  /// nothing to be gained by using them in Python. Use the general equivalent
  /// %RotationMatrix and %RigidTransform methods instead.

---

math/rigid_transform.h line 334 at r6 (raw file):

  /// and operating with this transform.
  /// @param[in] theta the rotation angle.
  /// @retval aX_BC the axial transform (also known as X_BC(θ)).

FYI the looks θ looks too much like 0.

Suggestion:

 /// @retval aX_BC the axial transform (also known as X_BC(theta)).

[sherm1]

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

---

math/rigid_transform.h line 299 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI: Suggest rewording.

Done

---

math/rigid_transform.h line 325 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI More explicit

Done

---

math/rigid_transform.h line 334 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI the looks θ looks too much like 0.

Done

[mitiguy]

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

---

math/rotation_matrix.h line 329 at r7 (raw file):

  /// @name          Advanced methods for axial rotations
  /// (Advanced) Axial rotations are %RotationMatrix objects for which it is
  /// known that the represented rotation is about one of the coordinate axes

FYI basis vectors (coordinate axes).

Suggestion:

 /// known that the represented rotation is about one of the basis vectors
 /// (coordinate axes) x, y, or z. ...

---

math/rotation_matrix.h line 346 at r7 (raw file):

  /// @note There are no Python bindings for these methods since there is
  /// nothing to be gained by using them in Python. Use the general equivalents
  /// instead.

FYI

Suggestion:

  /// nothing to be gained by using them in Python. Use the general equivalent
  /// %RotationMatrix methods instead.

---

math/rotation_matrix.h line 377 at r7 (raw file):

    v_B(x) = v_C(x);  // No effect along the rotation axis.
    v_B(y) = c * v_C(y) - s * v_C(z);
    v_B(z) = c * v_C(z) + s * v_C(y);

FYI Nice consolidation !

---

math/rotation_matrix.h line 388 at r7 (raw file):

  /// @param[in,out] aR_BC the axial rotation matrix to be updated.
  /// @tparam axis 0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
  /// @see the other signature if you already have sine & cosine θ.

FYI:

Suggestion:

  /// @see the other overloaded function if you already have sin(θ) and cos(θ).

---

math/rotation_matrix.h line 398 at r7 (raw file):

    using std::cos, std::sin;
    DRAKE_ASSERT(aR_BC != nullptr);
    UpdateAxialRotation<axis>(sin(theta), cos(theta), &*aR_BC);

FYI I am confused by why the address of the dereferenced pointer, i.e., &*aR_BC ?
Could it be simply aR_BC ?

Suggestion:

 UpdateAxialRotation<axis>(sin(theta), cos(theta), aR_BC);

---

math/rotation_matrix.h line 430 at r7 (raw file):

    M(y, z) = -sin_theta;  // only 1 flop
  }

Reviewed to here. I'll pick it up tomorrow.

---

math/rigid_transform.h line 361 at r7 (raw file):

  /// @pre aX_BC.rotation() is _exactly_ a rotation about the given axis and
  ///   aX_BC.translation() is _exactly_ zero (constant entries are all 0 or 1,
  ///   even for symbolic expressions).

FYI The placement of the parens next to zero seems almost contradictory.
Perhaps instead something as follows. If not, no worries. If yes, copy/paste elsewhere.

Suggestion:

  /// @pre aX_BC.rotation() is _exactly_ a rotation about the given axis 
  ///   (constant entries are all 0 or 1, even for symbolic expressions) and
  ///   aX_BC.translation() is _exactly_ zero.

---

math/rigid_transform.h line 368 at r7 (raw file):

  static Vector3<T> ApplyAxialRotation(const RigidTransform<T>& aX_BC,
                                       const Vector3<T>& v_C) {
    DRAKE_ASSERT(aX_BC.translation() == Vector3<T>::Zero());

FYI: Consider something like

DRAKE_ASSERT(aX_BC.IsRotationMatrix(axis))

---

math/rigid_transform.h line 380 at r7 (raw file):

  /// @param[in,out] aX_BC the axial transform matrix to be updated.
  /// @tparam axis 0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
  /// @see the other signature if you already have sin(θ) and cos(θ).

FYI Is there a possibility to be more explicit for the @see.
The "other signature" is somewhat ambiguous. Perhaps something like:

 /// @see the other overloaded function if you already have sin(θ) and cos(θ).

---

math/rigid_transform.h line 438 at r7 (raw file):

    DRAKE_ASSERT(X_AC != nullptr);
    DRAKE_ASSERT(aX_BC.translation() == Vector3<T>::Zero());
    // 14 flops rather than 63.

FYI -- does the reduced number of flops (14 rather than 63) belong in the documentation?
I see above that you include these interesting details in the documentation.

---

math/rigid_transform.h line 440 at r7 (raw file):

    // 14 flops rather than 63.
    rotation().template ComposeWithAxialRotation<axis>(aX_BC.rotation(),
                                                       &X_AC->R_AB_);

It seems this signature might lead to problems later if we want to compose an axial transform aX_AB with a general transform X_BC. If needed, would the other one be named something like ComposeAxialTransformWithGeneralTransform()?
If so, perhaps name this ComposeGeneralTransformWithAxialTransform().
This might also leave the naming convention open for ComposeAxialTransforms()?
These questions are also relevant for the RotationMatrix class as well as for
ComposeWithAxialTranslation().

---

math/rigid_transform.h line 453 at r7 (raw file):

  /// @pre The rotation part of atX_BC is exactly identity, and only
  ///   a single entry in the translation vector is non zero (the one
  ///   corresponding to `axis`).

FYI Some parallelism.

Suggestion:

  /// @pre The rotation part of atX_BC is exactly identity and its
  ///   translation part has only one non-zero entry, which 
  ///   corresponds to `axis`.

---

math/rigid_transform.h line 463 at r7 (raw file):

    DRAKE_ASSERT(atX_BC.rotation().IsExactlyIdentity());
    const Eigen::Vector3<T>& p_BC = atX_BC.translation();
    DRAKE_ASSERT(p_BC[(axis + 1) % 3] == 0 && p_BC[(axis + 2) % 3] == 0);

FYI: Consider consolidating these calculations to a single function, something like

DRAKE_ASSERT(atX_BC.IsTranslation(axis))

---

math/rigid_transform.h line 464 at r7 (raw file):

    const Eigen::Vector3<T>& p_BC = atX_BC.translation();
    DRAKE_ASSERT(p_BC[(axis + 1) % 3] == 0 && p_BC[(axis + 2) % 3] == 0);
    X_AC->set_rotation(R_AB_);  // unchanged

FYI Thoughts about ComposeThisWithAxisTranslation() to eliminate:

 X_AC->set_rotation(R_AB_);  // unchanged

---

math/rigid_transform.h line 480 at r7 (raw file):

    DRAKE_ASSERT(X_AC != nullptr);
    DRAKE_ASSERT(rX_BC.translation() == Vector3<T>::Zero());
    // 45 flops rather than 63 (nothing to write home about).

FYI Since the flops are already mentioned above, I would not repeat the comment.
However, 63 flops is 40% more than 45. OK to write home!

---

math/rigid_transform.h line 497 at r7 (raw file):

    DRAKE_ASSERT(tX_BC.rotation().IsExactlyIdentity());
    X_AC->set_rotation(R_AB_);  // unchanged
    // 18 flops rather than 63.

FYI Remove comment as it is already in documentation.

[sherm1]

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

---

math/rigid_transform.h line 361 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI The placement of the parens next to zero seems almost contradictory.
Perhaps instead something as follows. If not, no worries. If yes, copy/paste elsewhere.

Thanks, I see your point but the "even for symbolic expressions" needs to apply to the translation also so it is not easy to reword precisely. I think the current wording gets the message across even though it is awkward so I'll leave it.

---

math/rigid_transform.h line 368 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI: Consider something like

DRAKE_ASSERT(aX_BC.IsRotationMatrix(axis))

Done. This is a great idea -- I did it for several conditions and in lots of places. Also switched to throw (in Debug) rather than assert.

---

math/rigid_transform.h line 380 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Is there a possibility to be more explicit for the @see.
The "other signature" is somewhat ambiguous. Perhaps something like:

 /// @see the other overloaded function if you already have sin(θ) and cos(θ).

Done

---

math/rigid_transform.h line 438 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI -- does the reduced number of flops (14 rather than 63) belong in the documentation?
I see above that you include these interesting details in the documentation.

Done

---

math/rigid_transform.h line 440 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

It seems this signature might lead to problems later if we want to compose an axial transform aX_AB with a general transform X_BC. If needed, would the other one be named something like ComposeAxialTransformWithGeneralTransform()?
If so, perhaps name this ComposeGeneralTransformWithAxialTransform().
This might also leave the naming convention open for ComposeAxialTransforms()?
These questions are also relevant for the RotationMatrix class as well as for
ComposeWithAxialTranslation().

OK per f2f we'll add another function if this use case arises

---

math/rigid_transform.h line 453 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Some parallelism.

Done

---

math/rigid_transform.h line 463 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI: Consider consolidating these calculations to a single function, something like

DRAKE_ASSERT(atX_BC.IsTranslation(axis))

Done

---

math/rigid_transform.h line 464 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Thoughts about ComposeThisWithAxisTranslation() to eliminate:

 X_AC->set_rotation(R_AB_);  // unchanged

OK per f2f will add new functions if an "in place" use case arises -- like the Update function here.

---

math/rigid_transform.h line 480 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Since the flops are already mentioned above, I would not repeat the comment.
However, 63 flops is 40% more than 45. OK to write home!

Done

---

math/rigid_transform.h line 497 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Remove comment as it is already in documentation.

Done

---

math/rotation_matrix.h line 329 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI basis vectors (coordinate axes).

Done

---

math/rotation_matrix.h line 346 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI

Done

---

math/rotation_matrix.h line 377 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Nice consolidation !

Thanks!

---

math/rotation_matrix.h line 388 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI:

Done (keeping "signature" though)

---

math/rotation_matrix.h line 398 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI I am confused by why the address of the dereferenced pointer, i.e., &*aR_BC ?
Could it be simply aR_BC ?

OK per f2f, just there to signal output parameter

---

math/rotation_matrix.h line 430 at r7 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Reviewed to here. I'll pick it up tomorrow.

Done

[sherm1]

+@SeanCurtis-TRI for out-of-order platform review per f2f, please

Reviewable status: 2 unresolved discussions, LGTM missing from assignees mitiguy,SeanCurtis-TRI(platform)

[mitiguy]

Reminder: Need unit tests for error messages.

Reviewed 1 of 3 files at r3, 1 of 3 files at r5, 3 of 3 files at r8, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignees mitiguy,SeanCurtis-TRI(platform)

---

math/rigid_transform.h line 423 at r8 (raw file):

  /// transform aX_BC (known to consist only of an axial rotation about the
  /// indicated `axis` and no translation), efficiently form X_AC. This requires
  /// only 14 floating point operations while composing with a general

FYI "whereas" instead of "while"

Suggestion:

 /// only 14 floating point operations whereas composing with a general

---

math/rotation_matrix.cc line 256 at r8 (raw file):

  if constexpr (scalar_predicate<T>::is_bool) {  // double or AutoDiffScalar
    using std::abs;
    // Just a sanity check; don't be too fussy.

FYI Wordsmithing. Some might regard this as "fussy".

Suggestion:

  // Make a reasonably tolerant check on sin(θ) and cos(θ) values.

---

math/rotation_matrix.cc line 440 at r8 (raw file):

  if constexpr (scalar_predicate<T>::is_bool) {  // double or AutoDiffScalar
    using std::abs;
    // Just a sanity check; don't be too fussy.

FYI Wordsmithing. Some might regard this as "fussy".

Suggestion:

    // Make a reasonably tolerant check on sin(θ) and cos(θ) values.

---

math/test/rotation_matrix_test.cc line 732 at r8 (raw file):

  Rmat::UpdateAxialRotation<0>(2 * theta, &R_BC_update);
  EXPECT_TRUE(
      R_BC_update.IsNearlyEqualTo(Rmat::MakeXRotation(2 * theta), kTol));

FYI Make it easier to check code by using an "updated_theta" here/elsewhere.

Suggestion:

  const double new_theta = 1.23 * theta;
  Rmat::UpdateAxialRotation<0>(new_theta, &R_BC_update);
  EXPECT_TRUE(
      R_BC_update.IsNearlyEqualTo(Rmat::MakeXRotation(new_theta), kTol));

---

math/test/rotation_matrix_test.cc line 746 at r8 (raw file):

  // Sanity check that the alternate signature that takes sine & cosine
  // produces the same result (the angle signature tested above just calls
  // this one).

FYI Wordsmithing

Suggestion:

  // Ensure the overloaded signature with sin(θ) and cos(θ) arguments gives
  // the same result as the function with the angle argument tested above.

---

math/rotation_matrix.h line 462 at r8 (raw file):

  /// test -- the other five entries must be _exactly_ 1 or 0, even for
  /// symbolic expressions. For numeric types T we also check that the
  /// numerical entries are reasonable within a fairly loose tolerance of 16ε:

FYI slight reword?

Suggestion:

  /// symbolic expressions. For numeric types T we check that the numerical
  /// values are within 16ε of their proper values:

---

math/rotation_matrix.h line 463 at r8 (raw file):

  /// symbolic expressions. For numeric types T we also check that the
  /// numerical entries are reasonable within a fairly loose tolerance of 16ε:
  /// there should be two cos(θ) entries, +/- sin(θ) entries, and sin²+cos²==1.

FYI Consider ± instead of +/- (albeit, you probably already thought of this).

Suggestion:

  /// there should be two cos(θ) entries, ±sin(θ) entries, and sin²(θ)+cos²(θ)==1.

---

math/rotation_matrix.h line 898 at r8 (raw file):

  // expensive check considering that the sin & cos arguments were likely a
  // performance tweak to avoid recalculating those functions. Nothing happens
  // here if T is symbolic.

FYI Slightly rewording.

Suggestion:

  // expensive check considering that the sin(θ) and cos(θ) arguments were likely
  // precomputed performance tweak to avoid recalculating those functions. Nothing happens
  // here if T is symbolic.

---

math/rotation_matrix.h line 898 at r8 (raw file):

  // expensive check considering that the sin & cos arguments were likely a
  // performance tweak to avoid recalculating those functions. Nothing happens
  // here if T is symbolic.

FYI -- rewording possible.

Suggestion:

  // relaxed tolerance) and throw an exception if not. Due to this function's 
  // relatively high computational cost, this function is intended to be called only
  // in debug mode [e.g., from within DRAKE_ASSERT_VOID()], as it is likely that the
  // sin(θ) and cos(θ) arguments were precomputed to avoid their recalculation.
  // This function does nothing if T is symbolic.

---

math/test/rigid_transform_test.cc line 838 at r8 (raw file):

  // Make transforms that are axial rotations only (no translations).
  const double theta = 0.234;
  const Xform xX_BC(Xform::MakeAxialRotation<0>(theta));  // about x

FYI Possible notation change here and below.

Suggestion:

const Xform xaX_BC(Xform::MakeAxialRotation<0>(theta));  // about x

---

math/test/rigid_transform_test.cc line 864 at r8 (raw file):

  Xform::UpdateAxialRotation<0>(2 * theta, &X_BC_update);
  EXPECT_TRUE(X_BC_update.IsNearlyEqualTo(
      Xform::MakeAxialRotation<0>(2 * theta), kTol));

FYI Make it easier to check code by using an "updated_theta" here/elsewhere.

Suggestion:

  const double new_theta = 1.23 * theta;
  Xform X_BC_update = axX_BC;
  Xform::UpdateAxialRotation<0>(new_theta, &X_BC_update);
  EXPECT_TRUE(X_BC_update.IsNearlyEqualTo(
      Xform::MakeAxialRotation<0>(new_theta), kTol));   

[SeanCurtis-TRI]

First round done.

Reviewed 1 of 3 files at r3, 1 of 3 files at r5, 3 of 3 files at r8, all commit messages.
Reviewable status: 21 unresolved discussions, LGTM missing from assignees mitiguy,SeanCurtis-TRI(platform)

---

math/rigid_transform.h line 311 at r8 (raw file):
btw:

This sentence isn't quite right. We can afford to verify. We choose not to because that would undercut the intent.

Perhaps an alternate phrasing (which doesn't represent design decisions as constraints):

we will not verify the assumptions in the name of performance.

(Or some such thing.)

Code quote:

  /// objects have the appropriate simplified structure; we can't afford to
  /// verify that in Release builds.

---

math/rigid_transform.h line 423 at r8 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI "whereas" instead of "while"

FYI:

If you use "while" without a comma, you are suggesting "during the time". To use it to contrast, it requires a comma.

https://www.scribbr.com/commas/comma-before-while/

"Whereas" also requires a comma.

---

math/test/rigid_transform_test.cc line 838 at r8 (raw file):

Previously, mitiguy (Mitiguy) wrote…

FYI Possible notation change here and below.

BTW Related to my note elsewhere, I'd advocate: rxX_BC. It communicates that it's axial, but, more importantly, communicates that it's rotational (and not translational) and around which axis.

---

math/test/rotation_matrix_test.cc line 710 at r8 (raw file):

GTEST_TEST(RotationMatrix, SpecializedRotationOperators) {
  using Rmat = RotationMatrixd;          // For less clutter below.
  constexpr double kTol = 4 * kEpsilon;  // Allow for differing implementations.

BTW I didn't do an exact analysis of tolerance on this test; but I suspect the same thoughts apply here.

---

math/test/rotation_matrix_test.cc line 714 at r8 (raw file):

  // Make axial rotations.
  const double theta = 0.234;
  const Rmat Rx_BC(Rmat::MakeXRotation(theta));

nit Ha! I deleted a comment suggesting this notation for axial rotation matrices. I like this more. But you've documented a prefix approach and use a prefix approach with rigid transform, so in the name of consistency, I deleted my preference.

We should tighten the notation to tell a more unified story. I'm flexible about what moves, but we're certainly not there yet.

---

math/rotation_matrix.h line 337 at r8 (raw file):

  /// that knowledge for increased efficiency for various operations involving
  /// axial %RotationMatrix arguments. Note that we depend on the
  /// caller's promise that these are the appropriate rotations; we can't afford

nit: Same note on "can't" verify as in RigidTransform -- although, I do find it amusing that while the sentiment is the same, the phrasing differs.

---

math/rotation_matrix.h line 462 at r8 (raw file):

  /// test -- the other five entries must be _exactly_ 1 or 0, even for
  /// symbolic expressions. For numeric types T we also check that the
  /// numerical entries are reasonable within a fairly loose tolerance of 16ε:

nit: These two sentences seem to contradict each other. "exactly 0 or 1" and "loose tolerance". After all, for "numeric types T" all the entries are "numerical".

Having read the code, I think you meant to limit this discussion to the active "four" matrix elements. (See my note in rigid_transform.h about partitioning the elements of the matrices). Having clear definition between the "active" and "ignored" elements would help clarify this immensely.

Code quote:

  /// test -- the other five entries must be _exactly_ 1 or 0, even for
  /// symbolic expressions. For numeric types T we also check that the
  /// numerical entries are reasonable within a fairly loose tolerance of 16ε:

---

math/test/rigid_transform_test.cc line 0 at r8 (raw file):
Part of the purpose of these new methods is to do minimum work -- ignore fields during calculation, don't update fields that aren't part of the special structure, etc.

Do you want to put in regression tests that would catch if those purposes get undercut?

Same for RotationMatrix.

---

math/test/rigid_transform_test.cc line 828 at r8 (raw file):

// Test special case routines for working with transforms that involve only
// axial rotations or just translations. These should produce the same answers

nit Not just any translation, surely.

Suggestion:

// Test special case routines for working with transforms that involve only
// axial rotations or axial translations. These should produce the same answers

---

math/test/rigid_transform_test.cc line 842 at r8 (raw file):

  const Xform zX_BC(Xform::MakeAxialRotation<2>(theta));  // about z

  // Test MakeAxialRotation().

The use of kTol throughout this test seems odd.

For example, it seems perfectly reasonable to demand MakeAxialRotation() would produce the exact same transform as creating one from the corresponding RotationMatrix operation. Why allow slop? (The same applies to UpdateAxialRotation().)

Also, given the known properties of aX_AB, it also doesn't make much sense to have tolerances in re-expressing vectors.

By applying tolerance in cases such as these, you're weakening the power of the tests to confirm some of the most important properties of these APIs.

(Obviously, all bets are off when you start composing with general transforms and tolerance gains relevance again.)

FTR I found that the only operations that depended on tolerance were ComposeWithAxialRotation() (and they were satisfied with a tolerance of ε / 2). Everything else produced exact values.

I'm open to the possibility that we don't want to crank down tolerance minimally across the board. But some of these should definitely not have a tolerance and other cases should use them more judiciously.

---

math/test/rigid_transform_test.cc line 878 at r8 (raw file):

  // Make general, rotation-only, and translation-only transforms.
  const Xform X_AB(RollPitchYaw(1.0, 2.0, 3.0), Vector3d(1.5, 2.5, 3.5));
  const Xform rX_BC(RollPitchYaw(0.5, 1.5, -2.5), Vector3d::Zero());

BTW Up to this point, you were content to omit translation to produce rX_BC, seems a funny time to start worrying about it.

But if you think worrying about it now is appropriate, why not pass rotation identity for the tX_BC variants?

Suggestion:

const Xform rX_BC(RollPitchYaw(0.5, 1.5, -2.5));

---

math/rigid_transform.h line 300 at r8 (raw file):

  /// amount of computation required to form it and use it. The simplifications
  /// and associated notations for the rigid transform X_BC are:
  ///  - a transform that contains a _general_ rotation and no translation

nit: The idea of having "no translation" is a bit of a fiction. I didn't mind the fiction, until you characterized the transform with "no rotation" as having the "identity rotation". There is, of course, the identity translation.

I think you need to pick a lane. When describing some aspect of the transform as being idempotent, you should go with it being absent or being identity, but not both.

---

math/rigid_transform.h line 303 at r8 (raw file):

  ///    (rX_BC),
  ///  - a transform that contains an _axial_ rotation (see below) and no
  ///    translation (aX_BC),

I had to scrub my first comments about your notation. I thought I was raising concerns that hadn't been addressed. When I got to the unit test, I saw that you'd already tackled them. So, based on that, my concerns pivoted.

1. I don't think the documentation here does a good job of leading me to believe you would've used the notation you used in the test.
   • I didn't infer that in notation of particular instances that a would be swapped with the actual axis. Particularly as all of the implementations here use a.
   • Note: using a in the parameter names in this file make perfect sense, as they are templated on axis and can apply to any of the axes. But a single instance must only ever be a single axis over its lifespan. So, I'd suggest calling out the naming of instances explicitly is worthwhile.
2. I'm not overly pleased with the asymmetric handling of rotation-only vs translation-only.
   • You've got good parallels initially (rX_AB and tX_AB), but then when we get down to a single axis, we get xX_AB and xtX_AB. We have a single outlier there where the r disappears. Let's keep it: xrX_AB and xtX_AB.

---

math/rigid_transform.h line 337 at r8 (raw file):

  /// @retval aX_BC the axial transform (also known as X_BC(theta)).
  /// @tparam axis 0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
  template <int axis>

BTW Had you considered using an enum? I can imagine that:

RigidTransformd::MakeAxialRotation<Axis::kX>(theta);

might be slightly more readable than

RigidTransformd::MakeAxialRotation<1>(theta);

And for all the other implementations as well.

---

math/rigid_transform.h line 355 at r8 (raw file):

  /// 18.
  /// @param[in] aX_BC the axial transform to be applied.
  /// @param[in] v_C a vector expressed in frame C, to be transformed to frame

BTW I'm somewhat surprised that Paul didn't jump on this.

By definition a vector cannot be translated. So, the assertion that this operation just re-expresses because there is no translation is redundant. A vector can't be translated in the first place.

I believe this language is alluding to the possibility of a position vector and the idea that it isn't being remeasured because translation is zero. But that's borrowing trouble. Isn't it enough to say this simply re-expresses v_C as v_B?

Also, "apply" is so generic. Why not RexpressAsAxialRotation() or some such thing? It makes the call site more clear that this only re-expresses.

---

math/rigid_transform.h line 361 at r8 (raw file):

  /// @pre aX_BC.rotation() is _exactly_ a rotation about the given axis and
  ///   aX_BC.translation() is _exactly_ zero (constant entries are all 0 or 1,
  ///   even for symbolic expressions).

nit: I'm not sure what you mean by this parenthetical addition.

I see it as having two problems:

1. The set of entries being discussed under the term "constant" is unclear. I'm assuming it's all entries that are not set by MakeAxialRotation(). (Although, that phrase is also kind of vague.) So, characterizing which values you're talking about is both challenging but, arguably, worthwhile. And, for the record, "constant" doesn't get the job done.
2. However we define the set of values under discussion, saying they "are all 0 or 1" is imprecise. Some of them have to be zero. Some of them have to be 1. Essentially, they have to be the values in the identity matrix.
3. The discussion of symbolic expression misses the mark. I believe your requirement is that any value in the set defined above cannot require an Environment to evaluate. Perhaps the word "constant" is adjacent to this idea, but the rhetoric has separated them.

Perhaps it's worth going back to the original discussion when introducing the axial-only transforms and introducing the a term to distinguish the "active" elements of the matrix (with all other being "inactive"). And the idea that all "inactive" elements must carry the identity value. And likewise indicate the symbolic expression requirements.

In fact, as I type this, I'd recommend the following:

1. In the main group documentation, inject an additional linkable reference tag to discuss requirements for evaluation.
2. Here is where you describe the requirements of actually being the axis-only transform.
3. You also describe the Expression requirements.

In each operator method, do @pre @ref tag_to_requirements "Satisfies operator requirements" (or some such thing. This also solves the problem of copypasta-ing these requirements everywhere.

Code quote:

  ///   aX_BC.translation() is _exactly_ zero (constant entries are all 0 or 1,
  ///   even for symbolic expressions).

---

math/rigid_transform.h line 366 at r8 (raw file):

    requires(0 <= axis && axis <= 2)
#endif
  static Vector3<T> ApplyAxialRotation(const RigidTransform<T>& aX_BC,

Can you elaborate on why there are so many static methods instead of merely non-const member methods? It's not obviously an aliasing thing that I can see.

---

math/rigid_transform.h line 375 at r8 (raw file):

  /// (Advanced) Given a new rotation angle θ, update the axial transform aX_BC
  /// to represent the new rotation angle. We expect that aX_BC was already such
  /// a transform (about the given x, y, or z axis) but by some other angle.

BTW "but by some other angle" doesn't provide much value and is technically a lie. The caller is not obliged to make sure they're actually changing the angle.

They are obliged to make sure that aX_AB comports with the function invocation.

Code quote:

but by some other angle

---

math/rigid_transform.h line 488 at r8 (raw file):

  /// translation-only transform tX_BC to efficiently calculate
  /// X_AC = X_AB * tX_BC. This requires only 18 floating point operations while
  /// composing with a general transform would take 63.

BTW This feels like it's turning into a drinking game. I've read the phrase below so many times, I feel I'm being indoctrinated. :)

Code quote:

with a general transform would take 63.

---

math/rigid_transform.h line 900 at r8 (raw file):

  [[noreturn]] static void ThrowInvalidMultiplyVector4(const Vector4<T>& vec_B);

  // Throws if the translational part is not exactly zero, even for symbolic.

nit: "even for symbolic". Imprecise or, perhaps, merely indirect. For Expression you'll throw if you need an Environment or if you don't and the value isn't zero.

Furthermore, because of the dependencies on these tests testing for zero, they all share the same property. But the other methods don't allude to it. Another reason why putting it in the group documentation and linking to it from here is probably your best path forward.

---

math/rotation_matrix.cc line 250 at r8 (raw file):

void RotationMatrix<T>::IsAxialRotationOrThrow() const {
  constexpr int x = axis, y = (axis + 1) % 3, z = (axis + 2) % 3;
  // Formulate this using != so NaNs will get rejected.

BTW Given the document and the (arguably unnecessary) contortions in phrasing the test, I was expecting to see a NaN in the unit test.

---

math/rotation_matrix.cc line 251 at r8 (raw file):

  constexpr int x = axis, y = (axis + 1) % 3, z = (axis + 2) % 3;
  // Formulate this using != so NaNs will get rejected.
  DRAKE_THROW_UNLESS(!(R_AB_(x, x) != 1 || R_AB_(x, y) != 0 ||

nit: Surely, this whole thing is nicer after applying De Morgan's law.

  DRAKE_THROW_UNLESS(R_AB_(x, x) == 1 && R_AB_(x, y) == 0 && R_AB_(x, z) == 0 &&
                     R_AB_(y, x) == 0 && R_AB_(z, x) == 0);

These tests obviously fail for non-finite values.

[mitiguy]

Reviewable status: 21 unresolved discussions, LGTM missing from assignees mitiguy,SeanCurtis-TRI(platform)

---

math/rigid_transform.h line 337 at r8 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Had you considered using an enum? I can imagine that:

RigidTransformd::MakeAxialRotation<Axis::kX>(theta);

might be slightly more readable than

RigidTransformd::MakeAxialRotation<1>(theta);

And for all the other implementations as well.

FYI If possible, I like the enum idea as it makes the code more readable.

---

math/rigid_transform.h line 356 at r8 (raw file):

  /// @param[in] aX_BC the axial transform to be applied.
  /// @param[in] v_C a vector expressed in frame C, to be transformed to frame
  ///   B (just re-expressed in B since there is no translation).

FYI I think somewhere along the way I did mention this. Perhaps it was lost in translation (pun intended). The rigid transform can add position vectors, so in some sense, perhaps one might regard a position vector as being translated. I wonder if "there is no translation" diminishes or increases confusion.

Suggestion:

  /// @param[in] v_C a vector expressed in frame C, to be
  ///   re-expressed in frame B (there is no translation).