Create Transform class (rotation matrix and position vector) to repla…

…ce Isometry3 .

multibody/multibody_tree/math/BUILD.bazel

@@ -75,6 +75,18 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "transform",
+    srcs = ["transform.cc"],
+    hdrs = ["transform.h"],
+    deps = [
+        ":rotation_matrix",
+        "//drake/common",
+        "//drake/common:default_scalars",
+        "//drake/math:geometric_transform",
+    ],
+)
+
 drake_cc_googletest(
     name = "spatial_algebra_test",
     deps = [
@@ -90,4 +102,11 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "transform_test",
+    deps = [
+        ":transform",
+    ],
+)
+
 add_lint_tests()

multibody/multibody_tree/math/rotation_matrix.h

@@ -58,19 +58,27 @@ class RotationMatrix {
     SetUnchecked(R);
   }
 
-  /// @returns a 3x3 identity %RotationMatrix.
-  static RotationMatrix<T> MakeIdentity() { return RotationMatrix(); }
+  /// Returns the 3x3 identity %RotationMatrix.
+  /// @returns the 3x3 identity %RotationMatrix.
+  // @internal This method's name was chosen to mimic Eigen's Identity().
+  static const RotationMatrix<T>& Identity() {
+    static const never_destroyed<RotationMatrix<T>> kIdentity;
+    return kIdentity.access();
+  }
 
+  /// Calculates R_BA = R_AB⁻¹, the inverse (transpose) of this %RotationMatrix.
   /// @retval R_BA = R_AB⁻¹, the inverse (transpose) of this %RotationMatrix.
   /// @note For a valid rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ.
+  // @internal This method's name was chosen to mimic Eigen's inverse().
   RotationMatrix<T> inverse() const {
     return RotationMatrix<T>(R_AB_.transpose());
   }
 
+  /// Returns the Matrix3 underlying a %RotationMatrix.
   /// @returns the Matrix3 underlying a %RotationMatrix.
   const Matrix3<T>& matrix() const { return R_AB_; }
 
-  /// Operator to multiply `this` rotation matrix `R_AB` by `other` rotation
+  /// In-place multiply of `this` rotation matrix `R_AB` by `other` rotation
   /// matrix `R_BC`.  On return, `this` is set to equal `R_AB * R_BC`.
   /// @param[in] other %RotationMatrix that post-multiplies `this`.
   /// @returns `this` rotation matrix which has been multiplied by `other`.
@@ -81,7 +89,7 @@ class RotationMatrix {
     return *this;
   }
 
-  /// Operator to multiply `this` rotation matrix `R_AB` by `other` rotation
+  /// Calculates `this` rotation matrix `R_AB` multiplied by `other` rotation
   /// matrix `R_BC`, returning the composition `R_AB * R_BC`.
   /// @param[in] other %RotationMatrix that post-multiplies `this`.
   /// @returns rotation matrix that results from `this` multiplied by `other`.
@@ -91,7 +99,7 @@ class RotationMatrix {
     return RotationMatrix<T>(matrix() * other.matrix(), true);
   }
 
-  /// Operator to multiply `this` rotation matrix R by an arbitrary Vector3.
+  /// Calculates `this` rotation matrix R multiplied by an arbitrary Vector3.
   /// @param[in] v 3x1 vector that post-multiplies `this`.
   /// @returns 3x1 vector that results from `R * v`.
   Vector3<T> operator*(const Vector3<T>& v) const {
@@ -154,6 +162,18 @@ class RotationMatrix {
   /// @returns `true` if `this` is a valid rotation matrix.
   bool IsValid() const { return IsValid(matrix()); }
 
+  /// Returns `true` if `this` is exactly equal to the identity matrix.
+  /// @returns `true` if `this` is exactly equal to the identity matrix.
+  bool IsExactlyIdentity() const { return matrix() == Matrix3<T>::Identity(); }
+
+  /// Returns true if `this` is within tolerance of the identity matrix.
+  /// @returns `true` if `this` is equal to the identity matrix to within the
+  /// threshold of get_internal_tolerance_for_orthonormality().
+  bool IsIdentityToInternalTolerance() const {
+    return IsNearlyEqualTo(matrix(), Matrix3<T>::Identity(),
+                           get_internal_tolerance_for_orthonormality());
+  }
+
   /// Compares each element of `this` to the corresponding element of `other`
   /// to check if they are the same to within a specified `tolerance`.
   /// @param[in] other %RotationMatrix to compare to `this`.
@@ -178,6 +198,7 @@ class RotationMatrix {
   /// @returns proper orthonormal matrix R that is close to M.
   /// @throws exception std::logic_error if M fails IsValid(M).
   // @internal This function is not generated for symbolic Expression.
+  // @internal This function's name is referenced in Doxygen documentation.
   template <typename S = T>
   static typename std::enable_if<is_numeric<S>::value, RotationMatrix<S>>::type
   ProjectToRotationMatrix(const Matrix3<S>& M) {
@@ -186,6 +207,7 @@ class RotationMatrix {
     return RotationMatrix<S>(M_orthonormalized, true);
   }
 
+  /// Returns an internal tolerance that checks rotation matrix orthonormality.
   /// @returns internal tolerance (small multiplier of double-precision epsilon)
   /// used to check whether or not a rotation matrix is orthonormal.
   /// @note The tolerance is chosen by developers to ensure a reasonably

multibody/multibody_tree/math/test/rotation_matrix_test.cc

@@ -64,7 +64,7 @@ GTEST_TEST(RotationMatrix, SetRotationMatrix) {
 
 // Test setting a RotationMatrix to an identity matrix.
 GTEST_TEST(RotationMatrix, MakeIdentityMatrix) {
-  RotationMatrix<double> R = RotationMatrix<double>::MakeIdentity();
+  const RotationMatrix<double>& R = RotationMatrix<double>::Identity();
   Matrix3d zero_matrix = Matrix3<double>::Identity() - R.matrix();
   EXPECT_TRUE((zero_matrix.array() == 0).all());
 }
@@ -78,9 +78,9 @@ GTEST_TEST(RotationMatrix, Inverse) {
       0, cos_theta, sin_theta,
       0, -sin_theta, cos_theta;
   RotationMatrix<double> R(m);
-  RotationMatrix<double> I = R * R.inverse();
-  RotationMatrix<double> identity_matrix(Matrix3<double>::Identity());
-  EXPECT_TRUE(I.IsNearlyEqualTo(identity_matrix, 8 * kEpsilon));
+  RotationMatrix<double> RRinv = R * R.inverse();
+  const RotationMatrix<double>& I = RotationMatrix<double>::Identity();
+  EXPECT_TRUE(RRinv.IsNearlyEqualTo(I, 8 * kEpsilon));
 }
 
 
@@ -171,6 +171,21 @@ GTEST_TEST(RotationMatrix, IsValid) {
   EXPECT_FALSE(RotationMatrix<double>::IsValid(m, 5 * kEpsilon));
 }
 
+// Tests whether or not a RotationMatrix is an identity matrix.
+GTEST_TEST(RotationMatrix, IsExactlyIdentity) {
+  const double cos_theta = std::cos(0.5);
+  const double sin_theta = std::sin(0.5);
+  Matrix3d m;
+  m << 1, 0, 0,
+      0, cos_theta, sin_theta,
+      0, -sin_theta, cos_theta;
+
+  const RotationMatrix<double> R1(m);
+  const RotationMatrix<double> R2;
+  EXPECT_FALSE(R1.IsExactlyIdentity());
+  EXPECT_TRUE(R2.IsExactlyIdentity());
+}
+
 // Test ProjectMatrixToRotationMatrix.
 GTEST_TEST(RotationMatrix, ProjectToRotationMatrix) {
   Matrix3d m;