Add transmorgification to RotationMatrix class (#7722)

* Added transmorgification to RotationMatrix class

* Removed unnecessary #include

* Merge branch 'master' into TransformMatrixPartB

* Respond to reviewer comments

* Merge branch 'master' into TransformMatrixPartB

* Respond to Sherm's comments

* Merge branch 'master' into TransformMatrixPartB

* Respond to Alejandro's comments

multibody/multibody_tree/math/rotation_matrix.h

@@ -50,6 +50,25 @@ class RotationMatrix {
 #endif
   }
 
+  /// Creates a %RotationMatrix templatized on a scalar type U from a
+  /// %RotationMatrix templatized on scalar type T.  For example,
+  /// ```
+  /// RotationMatrix<double> source = RotationMatrix<double>::Identity();
+  /// RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
+  /// ```
+  /// @tparam U Scalar type on which the returned %RotationMatrix is templated.
+  /// @note `RotationMatrix<From>::cast<To>()` creates a new
+  /// `RotationMatrix<To>` from a `RotationMatrix<From>` but only if
+  /// type `To` is constructible from type `From`.
+  /// This cast method works in accordance with Eigen's cast method for Eigen's
+  /// %Matrix3 that underlies this %RotationMatrix.  For example, Eigen
+  /// currently allows cast from type double to AutoDiffXd, but not vice-versa.
+  template <typename U>
+  RotationMatrix<U> cast() const {
+    const Matrix3<U> m = R_AB_.template cast<U>();
+    return RotationMatrix<U>(m, true);
+  }
+
   /// Sets `this` %RotationMatrix from a Matrix3.
   /// @param[in] R an allegedly valid rotation matrix.
   /// @throws exception std::logic_error in debug builds if R fails IsValid(R).
@@ -218,6 +237,13 @@ class RotationMatrix {
   }
 
  private:
+  // Make RotationMatrix<U> templatized on any typename U be a friend of a
+  // %RotationMatrix templatized on any other typename T.
+  // This is needed for the method RotationMatrix<T>::cast<U>() to be able to
+  // use the necessary private constructor.
+  template <typename U>
+  friend class RotationMatrix;
+
   // Declares the allowable tolerance (small multiplier of double-precision
   // epsilon) used to check whether or not a rotation matrix is orthonormal.
   static constexpr double kInternalToleranceForOrthonormality_{

multibody/multibody_tree/math/test/rotation_matrix_test.cc

@@ -12,6 +12,50 @@ using Eigen::Vector3d;
 
 constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
 
+// Helper function to create a rotation matrix associated with a BodyXYZ
+// rotation by angles q1 = 0.2 radians, q2 = 0.3 radians, q3 = 0.4 radians.
+// Note: These matrices must remain BodyXYZ matrices with the specified angles
+// q1, q2, q3, as these matrices are used in conjunction with MotionGenesis
+// pre-computed solutions based on these exact matrices.
+Matrix3d MakeRotationMatrixBodyXYZ() {
+  const double q1 = 0.2, q2 = 0.3, q3 = 0.4;
+  const double c1 = std::cos(q1), c2 = std::cos(q2), c3 = std::cos(q3);
+  const double s1 = std::sin(q1), s2 = std::sin(q2), s3 = std::sin(q3);
+  Matrix3d m;
+  m << c2 * c3,
+      s3 * c1 + s1 * s2 * c3,
+      s1 * s3 - s2 * c1 * c3,
+      -s3 * c2,
+      c1 * c3 - s1 * s2 * s3,
+      s1 * c3 + s2 * s3 * c1,
+      s2,
+      -s1 * c2,
+      c1 * c2;
+  return m;
+}
+
+// Helper function to create a rotation matrix associated with a BodyXYX
+// rotation by angles r1 = 0.5 radians, r2 = 0.5 radians, r3 = 0.7 radians.
+// Note: These matrices must remain BodyXYX matrices with the specified angles
+// r1, r2, r3, as these matrices are used in conjunction with MotionGenesis
+// pre-computed solutions based on these exact matrices.
+Matrix3d MakeRotationMatrixBodyXYX() {
+  const double r1 = 0.5, r2 = 0.5, r3 = 0.7;
+  const double c1 = std::cos(r1), c2 = std::cos(r2), c3 = std::cos(r3);
+  const double s1 = std::sin(r1), s2 = std::sin(r2), s3 = std::sin(r3);
+  Matrix3d m;
+  m << c2,
+      s1 * s2,
+      -s2 * c1,
+      s2 * s3,
+      c1 * c3 - s1 * s3 * c2,
+      s1 * c3 + s3 * c1 * c2,
+      s2 * c3,
+      -s3 * c1 - s1 * c2 * c3,
+      c1 * c2 * c3 - s1 * s3;
+  return m;
+}
+
 // Test default constructor - should be identity matrix.
 GTEST_TEST(RotationMatrix, DefaultRotationMatrixIsIdentity) {
   RotationMatrix<double> R;
@@ -86,35 +130,8 @@ GTEST_TEST(RotationMatrix, Inverse) {
 
 // Test rotation matrix multiplication and IsNearlyEqualTo.
 GTEST_TEST(RotationMatrix, OperatorMultiplyAndIsNearlyEqualTo) {
-  // Create a rotation matrix from a BodyXYZ rotation by angles q1, q2, q3.
-  double q1 = 0.2, q2 = 0.3, q3 = 0.4;
-  double c1 = std::cos(q1), c2 = std::cos(q2), c3 = std::cos(q3);
-  double s1 = std::sin(q1), s2 = std::sin(q2), s3 = std::sin(q3);
-  Matrix3d m_BA;
-  m_BA << c2 * c3,
-          s3 * c1 + s1 * s2 * c3,
-          s1 * s3 - s2 * c1 * c3,
-         -s3 * c2,
-          c1 * c3 - s1 * s2 * s3,
-          s1 * c3 + s2 * s3 * c1,
-          s2,
-         -s1 * c2,
-          c1 * c2;
-
-  // Create a rotation matrix from a BodyXYX rotation by angles r1, r2, r3.
-  double r1 = 0.5, r2 = 0.5, r3 = 0.7;
-  c1 = std::cos(r1), c2 = std::cos(r2), c3 = std::cos(r3);
-  s1 = std::sin(r1), s2 = std::sin(r2), s3 = std::sin(r3);
-  Matrix3d m_CB;
-  m_CB << c2,
-          s1 * s2,
-         -s2 * c1,
-          s2 * s3,
-          c1 * c3 - s1 * s3 * c2,
-          s1 * c3 + s3 * c1 * c2,
-          s2 * c3,
-         -s3 * c1 - s1 * c2 * c3,
-          c1 * c2 * c3 - s1 * s3;
+  Matrix3d m_BA = MakeRotationMatrixBodyXYZ();
+  Matrix3d m_CB = MakeRotationMatrixBodyXYX();
 
   RotationMatrix<double> R_BA(m_BA);
   RotationMatrix<double> R_CB(m_CB);
@@ -212,6 +229,29 @@ GTEST_TEST(RotationMatrix, ProjectToRotationMatrix) {
                std::logic_error);
 }
 
+// Test RotationMatrix cast method from double to AutoDiffXd.
+GTEST_TEST(RotationMatrix, CastFromDoubleToAutoDiffXd) {
+  const Matrix3d m = MakeRotationMatrixBodyXYZ();
+  const RotationMatrix<double> R_double(m);
+  const RotationMatrix<AutoDiffXd> R_autodiff = R_double.cast<AutoDiffXd>();
+
+  // To avoid a (perhaps) tautological test, do not just use an Eigen cast() to
+  // the Matrix3 that underlies the RotationMatrix class -- i.e., avoid just
+  // comparing m_autodiff.cast<double>() with m_double.
+  // Instead, check element-by-element equality as follows.
+  const Matrix3<double>& m_double = R_double.matrix();
+  const Matrix3<AutoDiffXd>& m_autodiff = R_autodiff.matrix();
+  for (int i = 0;  i < 3; i++) {
+    for (int j = 0; j < 3; j++) {
+      const double mij_double = m_double(i, j);
+      const AutoDiffXd& mij_autodiff = m_autodiff(i, j);
+      EXPECT_EQ(mij_autodiff.value(), mij_double);
+      EXPECT_EQ(mij_autodiff.derivatives().size(), 0);
+    }
+  }
+}
+
+
 }  // namespace
 }  // namespace math
 }  // namespace multibody

multibody/multibody_tree/rotational_inertia.h

@@ -397,10 +397,10 @@ class RotationalInertia {
   ///
   /// @note `RotationalInertia<From>::cast<To>()` creates a new
   /// `RotationalInertia<To>` from a `RotationalInertia<From>` but only if
-  /// type `To` is constructible from type `From`. As an example of this,
-  /// `RotationalInertia<double>::cast<AutoDiffXd>()` is valid since
-  /// `AutoDiffXd a(1.0)` is valid. However,
-  /// `RotationalInertia<AutoDiffXd>::cast<double>()` is not.
+  /// type `To` is constructible from type `From`.
+  /// This cast method works in accordance with Eigen's cast method for Eigen's
+  /// %Matrix3 that underlies this %RotationalInertia.  For example, Eigen
+  /// currently allows cast from type double to AutoDiffXd, but not vice-versa.
   template <typename Scalar>
   RotationalInertia<Scalar> cast() const {
     return RotationalInertia<Scalar>(I_SP_E_.template cast<Scalar>());

multibody/multibody_tree/spatial_inertia.h

@@ -154,10 +154,10 @@ class SpatialInertia {
   ///
   /// @note `SpatialInertia<From>::cast<To>()` creates a new
   /// `SpatialInertia<To>` from a `SpatialInertia<From>` but only if
-  /// type `To` is constructible from type `From`. As an example of this,
-  /// `SpatialInertia<double>::cast<AutoDiffXd>()` is valid since
-  /// `AutoDiffXd a(1.0)` is valid. However,
-  /// `SpatialInertia<AutoDiffXd>::cast<double>()` is not.
+  /// type `To` is constructible from type `From`.
+  /// This cast method works in accordance with Eigen's cast method for Eigen's
+  /// objects that underlie this %SpatialInertia.  For example, Eigen
+  /// currently allows cast from type double to AutoDiffXd, but not vice-versa.
   template <typename Scalar>
   SpatialInertia<Scalar> cast() const {
     return SpatialInertia<Scalar>(