[fem] Prepare matrix utilities to move out of dev (RobotLocomotion#16508

)

* [fem] Prepare matrix utilities to move out of dev

- Remove unused PermuteBlockSparseMatrix.
- Move CalcConditionNumber to matrix_utilities.h/cc

multibody/fixed_fem/dev/BUILD.bazel

@@ -256,17 +256,6 @@ drake_cc_library(
 )
 
 # === test/ ===
-drake_cc_library(
-    name = "test_utilities",
-    testonly = 1,
-    srcs = ["test/test_utilities.cc"],
-    hdrs = ["test/test_utilities.h"],
-    deps = [
-        "//common:default_scalars",
-        "//common:essential",
-    ],
-)
-
 drake_cc_googletest(
     name = "collision_objects_test",
     data = ["//geometry:test_obj_files"],
@@ -285,7 +274,6 @@ drake_cc_library(
     deps = [
         ":corotated_model",
         ":linear_constitutive_model",
-        ":test_utilities",
         "//common:autodiff",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
@@ -314,7 +302,6 @@ drake_cc_googletest(
     name = "matrix_utilities_test",
     deps = [
         ":matrix_utilities",
-        ":test_utilities",
         "//common:essential",
         "//common/test_utilities:eigen_matrix_compare",
         "//math:geometric_transform",

multibody/fixed_fem/dev/matrix_utilities.cc

@@ -7,6 +7,23 @@ namespace multibody {
 namespace fem {
 namespace internal {
 
+template <typename T>
+double CalcConditionNumberOfInvertibleMatrix(
+    const Eigen::Ref<const MatrixX<T>>& A) {
+  DRAKE_THROW_UNLESS(A.rows() == A.cols());
+  Eigen::JacobiSVD<MatrixX<T>> svd(A);
+  /* Eigen::JacobiSVD::singularValues() returns sigma as positive, monotonically
+   decreasing values.
+   See https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html. */
+  const VectorX<T>& sigma = svd.singularValues();
+  /* Prevents division by zero for singular matrix. */
+  const T& sigma_min = sigma(sigma.size() - 1);
+  DRAKE_DEMAND(sigma_min > 0);
+  const T& sigma_max = sigma(0);
+  const T cond = sigma_max / sigma_min;
+  return ExtractDoubleOrThrow(cond);
+}
+
 template <typename T>
 void PolarDecompose(const Matrix3<T>& F, EigenPtr<Matrix3<T>> R,
                     EigenPtr<Matrix3<T>> S) {
@@ -124,49 +141,10 @@ VectorX<T> PermuteBlockVector(const Eigen::Ref<const VectorX<T>>& v,
   return permuted_v;
 }
 
-template <typename T>
-Eigen::SparseMatrix<T> PermuteBlockSparseMatrix(
-    const Eigen::SparseMatrix<T>& matrix,
-    const std::vector<int>& block_permutation) {
-  DRAKE_ASSERT(matrix.rows() == matrix.cols());
-  DRAKE_ASSERT(static_cast<int>(block_permutation.size()) * 3 == matrix.cols());
-  const int nv = matrix.rows();
-  Eigen::SparseMatrix<T> permuted_matrix(nv, nv);
-  std::vector<Eigen::Triplet<T>> triplets;
-  triplets.reserve(matrix.nonZeros());
-  using InnerIterator = typename Eigen::SparseMatrix<T>::InnerIterator;
-  for (int k = 0; k < matrix.outerSize(); ++k) {
-    for (InnerIterator it(matrix, k); it; ++it) {
-      /* The row/column indices of a particular value within its block. */
-      const int block_row = it.row() % 3;
-      const int block_col = it.col() % 3;
-      /* The block indices B(i, j) in which the value is located. */
-      const int i = it.row() / 3;
-      const int j = it.col() / 3;
-      /* The block indices C(p(i), p(j)) into which the value will be written.
-       */
-      const int p_i = block_permutation[i];
-      const int p_j = block_permutation[j];
-      /* The block C(p_i, p_j) is located at (p_i * 3, p_j * 3) in the
-       output matrix and the value is offset from that origin by its
-       indices within the block. */
-      const int permuted_row = p_i * 3 + block_row;
-      const int permuted_col = p_j * 3 + block_col;
-
-      triplets.emplace_back(permuted_row, permuted_col, it.value());
-    }
-  }
-  /* The permuted matrix should have the exact same number of non-zero entries
-   as the old matrix. */
-  DRAKE_DEMAND(static_cast<int>(triplets.size()) == matrix.nonZeros());
-  permuted_matrix.setFromTriplets(triplets.begin(), triplets.end());
-  return permuted_matrix;
-}
-
 DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
-    (&PolarDecompose<T>, &AddScaledRotationalDerivative<T>,
-     &CalcCofactorMatrix<T>, &AddScaledCofactorMatrixDerivative<T>,
-     &PermuteBlockVector<T>, &PermuteBlockSparseMatrix<T>))
+    (&CalcConditionNumberOfInvertibleMatrix<T>, &PolarDecompose<T>,
+     &AddScaledRotationalDerivative<T>, &CalcCofactorMatrix<T>,
+     &AddScaledCofactorMatrixDerivative<T>, &PermuteBlockVector<T>))
 
 }  // namespace internal
 }  // namespace fem

multibody/fixed_fem/dev/matrix_utilities.h

@@ -31,10 +31,23 @@ the 4th order tensor into 9*9 matrices that are organized as follows:
               -------------------------------------
 Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. */
 
+/* Calculates the condition number for the given matrix A.
+ The condition number is calculated via
+     k(A) = σₘₐₓ / σₘᵢₙ
+ where σₘₐₓ and σₘᵢₙ are the largest and smallest singular values (in magnitude)
+ of A.
+ @note This function is intended to be used for testing and may be slow on large
+ matrices.
+ @pre A is invertible.
+ @tparam_nonsymbolic_scalar */
+template <typename T>
+double CalcConditionNumberOfInvertibleMatrix(
+    const Eigen::Ref<const MatrixX<T>>& A);
+
 /* Calculates the polar decomposition of a 3-by-3 matrix F = RS where R is a
  rotation matrix and S is a symmetric matrix. The decomposition is unique when F
  is non-singular.
- @tparam_nonsymbolic_scalar. */
+ @tparam_nonsymbolic_scalar */
 template <typename T>
 void PolarDecompose(const Matrix3<T>& F, EigenPtr<Matrix3<T>> R,
                     EigenPtr<Matrix3<T>> S);
@@ -75,7 +88,7 @@ void PolarDecompose(const Matrix3<T>& F, EigenPtr<Matrix3<T>> R,
  @param[in] scale        The scalar multiple of the result.
  @param[out] scaled_dRdF The variable to which scale * dR/dF is added.
  @pre tr(S)I − S is invertible.
- @tparam_nonsymbolic_scalar. */
+ @tparam_nonsymbolic_scalar */
 template <typename T>
 void AddScaledRotationalDerivative(
     const Matrix3<T>& R, const Matrix3<T>& S, const T& scale,
@@ -90,7 +103,7 @@ void CalcCofactorMatrix(const Matrix3<T>& M, EigenPtr<Matrix3<T>> cofactor);
  @param[in] M            The input matrix.
  @param[in] scale        The scalar multiple of the result.
  @param[out] scaled_dCdF The variable to which scale * dC/dM is added.
- @tparam_nonsymbolic_scalar. */
+ @tparam_nonsymbolic_scalar */
 template <typename T>
 void AddScaledCofactorMatrixDerivative(
     const Matrix3<T>& M, const T& scale,
@@ -109,37 +122,11 @@ permutation will be:
                               permuted block whose original index is `i`.
 @pre v.size() % 3 == 0.
 @pre block_permutation is a permutation of {0, 1, ..., v.size()/3-1}.
-@tparam_nonsymbolic_scalar. */
+@tparam_nonsymbolic_scalar */
 template <typename T>
 VectorX<T> PermuteBlockVector(const Eigen::Ref<const VectorX<T>>& v,
                               const std::vector<int>& block_permutation);
 
-/* Given a 3N-by-3N Eigen::SparseMatrix with block structure with 3x3 block
- entries Bᵢⱼ where i, j ∈ V = {0, ..., N-1} and a permutation P on V, this
- method builds the permuted matrix with 3x3 block entries C's such that
- Cₚ₍ᵢ₎ₚ₍ⱼ₎ = Bᵢⱼ.
-
- For example, suppose the input `matrix` is given by
-    B00  B01  B02
-    B10  B11  B12
-    B20  B21  B22,
- permutation {1, 2, 0} produces
-    B22  B20  B21
-    B02  B00  B01
-    B12  B10  B11.
-
- @param[in] matrix             The original block sparse matrix to be permuted.
- @param[in] block_permutation  block_permutation[i] gives the index of the
-                               permuted block whose original index is `i`.
- @pre matrix.rows() == matrix.cols().
- @pre matrix.rows() % 3 == 0.
- @pre block_permutation is a permutation of {0, 1, ..., matrix.rows()/3-1}.
- @tparam_nonsymbolic_scalar. */
-template <typename T>
-Eigen::SparseMatrix<T> PermuteBlockSparseMatrix(
-    const Eigen::SparseMatrix<T>& matrix,
-    const std::vector<int>& block_permutation);
-
 }  // namespace internal
 }  // namespace fem
 }  // namespace multibody

multibody/fixed_fem/dev/test/constitutive_model_test_utilities.cc

@@ -8,7 +8,7 @@
 #include "drake/multibody/fem/constitutive_model.h"
 #include "drake/multibody/fixed_fem/dev/corotated_model.h"
 #include "drake/multibody/fixed_fem/dev/linear_constitutive_model.h"
-#include "drake/multibody/fixed_fem/dev/test/test_utilities.h"
+#include "drake/multibody/fixed_fem/dev/matrix_utilities.h"
 
 namespace drake {
 namespace multibody {
@@ -126,7 +126,7 @@ void TestPIsDerivativeOfPsi() {
   for (int i = 0; i < num_locations; ++i) {
     EXPECT_TRUE(CompareMatrices(
         Eigen::Map<const Matrix3d>(energy[i].derivatives().data(), 3, 3), P[i],
-        fem::test::CalcConditionNumber<AutoDiffXd>(P[i]) * kTolerance));
+        CalcConditionNumberOfInvertibleMatrix<AutoDiffXd>(P[i]) * kTolerance));
   }
 }
 
@@ -160,7 +160,8 @@ void TestdPdFIsDerivativeOfP() {
         EXPECT_TRUE(CompareMatrices(
             Eigen::Map<const Matrix3d>(P[q](i, j).derivatives().data(), 3, 3),
             dPijdF,
-            fem::test::CalcConditionNumber<AutoDiffXd>(P[q]) * kTolerance));
+            CalcConditionNumberOfInvertibleMatrix<AutoDiffXd>(P[q]) *
+                kTolerance));
       }
     }
   }

multibody/fixed_fem/dev/test/matrix_utilities_test.cc

@@ -6,7 +6,6 @@
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/math/autodiff_gradient.h"
 #include "drake/math/rotation_matrix.h"
-#include "drake/multibody/fixed_fem/dev/test/test_utilities.h"
 
 namespace drake {
 namespace multibody {
@@ -40,7 +39,17 @@ const double kTol = 1e-14;
  input matrix A. */
 template <typename T>
 double CalcTolerance(const Matrix3<T>& A) {
-  return test::CalcConditionNumber<T>(A) * kTol;
+  return CalcConditionNumberOfInvertibleMatrix<T>(A) * kTol;
+}
+
+GTEST_TEST(MatrixUtilitiesTest, CalcConditionNumber) {
+  const Matrix3<double> A = Vector3<double>(1, 2, 3).asDiagonal();
+  EXPECT_DOUBLE_EQ(CalcConditionNumberOfInvertibleMatrix<double>(A), 3);
+
+  /* The input must be square. */
+  const MatrixX<double> B(1, 2);
+  EXPECT_THROW(CalcConditionNumberOfInvertibleMatrix<double>(B),
+               std::exception);
 }
 
 GTEST_TEST(MatrixUtilitiesTest, PolarDecompose) {
@@ -123,30 +132,6 @@ GTEST_TEST(MatrixUtilitiesTest, PermuteBlockVector) {
   EXPECT_TRUE(CompareMatrices(expected_result, permuted_v));
 }
 
-/* Verify that `PermuteBlockSparseMatrix()` provides the expected answer on a
- 3x3 block matrix. We choose a 3x3 block test case so that the permutation is
- neither the identity nor a self-inverse. */
-GTEST_TEST(MatrixUtilitiesTest, PermuteBlockSparseMatrix) {
-  constexpr int kNumBlocks = 3;
-  constexpr int kSize = kNumBlocks * 3;
-  const MatrixXd A = MakeMatrix(kSize, kSize);
-  const Eigen::SparseMatrix<double> A_sparse = A.sparseView();
-  const std::vector<int> block_permutation = {1, 2, 0};
-  const Eigen::SparseMatrix<double> permuted_A =
-      PermuteBlockSparseMatrix(A_sparse, block_permutation);
-  const MatrixXd permuted_A_dense = permuted_A;
-
-  MatrixXd expected_result(kSize, kSize);
-  for (int i = 0; i < kNumBlocks; ++i) {
-    for (int j = 0; j < kNumBlocks; ++j) {
-      expected_result.block<3, 3>(3 * block_permutation[i],
-                                  3 * block_permutation[j]) =
-          A.block<3, 3>(3 * i, 3 * j);
-    }
-  }
-  EXPECT_TRUE(CompareMatrices(expected_result, permuted_A_dense));
-}
-
 }  // namespace
 }  // namespace internal
 }  // namespace fem