Drake: Enable cast() method to cast RotationMatrix and RigidTransform from AutoDiff to double.

[mitiguy]

The RigidTransform::cast() method in rigid_transform.h does not allow casting from Autodiff to double.
The RotationMatrix::cast() method in rotation_matrix.h does not allow casting from Autodiff to double.
There is a TODO in the code in rotation_matrix.h that highlights that defect.

Below is a helper function in drake/geometry/utilities.h that shows how Sean Curtis nicely programmed around what I (Mitiguy) perceive as a shortcoming of the RigidTransform::cast() method.

// Don't needlessly copy transforms that are already scalar-valued.
inline const math::RigidTransformd& convert_to_double(
const math::RigidTransformd& X_AB) {
return X_AB;
}

template
math::RigidTransformd convert_to_double(
const math::RigidTransform>& X_AB) {
Matrix3 R_converted;
Vector3 p_converted;
for (int r = 0; r < 3; ++r) {
p_converted(r) = ExtractDoubleOrThrow(X_AB.translation()(r));
for (int c = 0; c < 3; ++c) {
R_converted(r, c) = ExtractDoubleOrThrow(X_AB.rotation().matrix()(r, c));
}
}
return math::RigidTransformd(math::RotationMatrixd(R_converted), p_converted);
}

Related: There are 100+ uses of math::autoDiffToValueMatrix().
For example, below is code in tree_mobilizers_test.cc (line 1216).

// Extract the transformations' values.
Eigen::MatrixXd X_WU_value = math::autoDiffToValueMatrix(X_WU.GetAsMatrix4());
Eigen::MatrixXd X_WL_value = math::autoDiffToValueMatrix(X_WL.GetAsMatrix4());

Note: It may be helpful to see the implementation of the function math::autoDiffToValueMatrix() in drake/math/autodiff.h.

Note: This issue was discussed during Dynamics stand-up meeting on 6/30/2020.
@SeanCurtis-TRI @sherm1 @mitiguy

[jwnimmer-tri]

Demoting from AutoDiff to double is potentially information-losing; see #12650 for some discussions on appropriate function names for such lossy kinds of functions semantics.

I think we should _not_ overload "cast" to mean both kinds of conversion -- we should use a more careful (+ consistent with the rest of Drake) method name for the information-losing direction.

[sherm1]

Good point. Likely the semantics we want here is what we already have with ExtractDoubleOrThrow(), but acting on the whole RotationMatrix or RigidTransform object.

[jwnimmer-tri]

My thought once #12650 settles on semantics was to make the new term an customization point; for example we could have rotation_double = ExtractDoubleOrThrow(rotation_autodiff).

[sherm1]

Yep, nice. That could instead be a member rotation_double = rotation_autodiff.ExtractDoubleOrThrow() but the uniform syntax of an overloaded global function has advantages. (In any case the member function could serve as the implementation of the global function in the manner of, e.g., swap().)

It may still be good to have rotation_double.cast<U>() where U is double, AutoDiff, or Symbolic with no info loss and U=double a ref-returning no-op.

[SeanCurtis-TRI]

I'm wondering if the .cast<> method has any real value in its current state. While the name is suggestive of semantics similar to to C++'s casting features (which do allow for casts that lose information), its implementation is woefully incomplete. I know the cast() method in RotationMatrix and RigidTransform matches that of Eigen; I can't do Eigen::Vector3<AutoDiffXd>().cast<double>() either. So, it can only be used to promote in complexity (double -> AutoDiff and double->Symbolic). Surely, that isn't the same as "casting"?

For my limited purposes, I'd be happy with extending the ExtractDoubleOrThrow() functions to take overloads for RotationMatrix<double|AutoDiffXd> and RigidTransform<double|AutoDiffXd> (as we have for the scalar itself). This assumes that the double-valued inputs have a return value that is just a const reference to the input so that there's no copying expense when T = double. I.e., a signature like const RotationMatrixd& ExtractDoubleOrThrow(const RotationMatrixd&).

[mitiguy]

Thank you @SeanCurtis-TRI. I agree with your "woefully incomplete" sentiments.
It would be helpful to find this functionality somewhere within its associated class (RotationMatrix or RigidTransform).
Hopefully other folks (in addition to you, Sherm, and Jeremy) will weigh in on their preference on cast() or ExtractDoubleOrThrow() or other.

[mitiguy]

After looking at issue #12650 and PR #13003, my current plan employs functions with signatures such as:
const RotationMatrix& ExtractDoubleOrThrow(const RotationMatrix\ const RigidTransform& ExtractDoubleOrThrow(const RigidTransform\ RotationMatrix ExtractDoubleOrThrow(const RotationMatrix\ RigidTransform ExtractDoubleOrThrow(const RigidTransform\

[SeanCurtis-TRI]

I may be misunderstanding your proposal, but it looks like you're proposing overloads that only differ by return value type. C++ is going to get pretty angry about that.

[sherm1]

I presume those would be distinguished by template type with the ref-returning ones being RotationMatrix<double>, etc.?

[jwnimmer-tri]

FYI Paul wrote template <>s, but markdown ate them. You can view the comment source to see them.

Anyway, all of the returns should be by-value. Chain-returning const references from arguments to return values will defeat lifetime extension. Maybe use https://drake.mit.edu/styleguide/cppguide.html#Pass_By_Value to elide the copy, though that might lead to ambiguous overloads.

Since RotationMatrix and RigidTransform also support Symbolic, the overloads here should also handle that case. In fact, there's really no reason we need multiple overloads. Just template the function on T, and delegate to T's ExtractDoubleOrThrow for each element of the underlying matrix. That way, if we extend the set of default scalars, nothing here will have to change.

Off the top of my head:

template <typename T>
RotationMatrix<double> ExtractDoubleOrThrow(const RotationMatrix<T>& arg) {
  return RotationMatrix<double>(arg.matrix().unaryExpr([](const auto& elt) { return ExtractDoubleOrThrow(elt); }));
}

[SeanCurtis-TRI]

How widespread would this usage be? I presume we expect the need to arbitrarily project down to double (regardless of T) is not something that would appear with any great frequency in Drake?

However, it appears with great frequency (from an inner-loop execution measure if not a grep count measure) in geometry. We interface with a lot of black boxes that can't accept AutoDiff. In those case, we always "extract" the double for every entity in geometry. I don't like the idea of copying those in the "normal" case (i.e., T = double). So, I'm going to keep my utilities around.

I did a quick triage and would make the following observations:

• They would benefit from some "Advanced" warning documentation (e.g., If you capture the result by reference, it must have automatic storage duration. Copying is always allowed.). I looked and don't believe any of the usages violate that.
• There are some call sites that aren't capturing the result in a local reference (negating the point of only copying when necessary). Those should be cleaned up.

(P.S. Where do you view the comment source? It's not the page source (by that time the html has long-since digested the markdown.)

[jwnimmer-tri]

Where do you view the comment source?

Use "Edit..."

[jwnimmer-tri]

How widespread would this usage be? I presume we expect the need to arbitrarily project down to double (regardless of T) is not something that would appear with any great frequency in Drake?

This raises a good point. The post that opens this issue doesn't explain why any of this is relevant. Possibly this was intended to help make unit tests more concise?