[WIP] Try reproducing with AutoDiffXd

common/BUILD.bazel

@@ -682,6 +682,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "autodiffxd_ldlt_test",
+    deps = [
+        ":autodiff",
+        "//math:gradient",
+    ],
+)
+
 drake_cc_googletest(
     name = "autodiffxd_heap_test",
     deps = [

common/test/autodiffxd_ldlt_test.cc

@@ -0,0 +1,56 @@
+#include <gtest/gtest.h>
+
+#include "drake/common/autodiff.h"
+#include "drake/math/autodiff_gradient.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace test {
+namespace {
+
+GTEST_TEST(AutoDiffXdLdltTest, derivatives) {
+  using Eigen::VectorXd;
+  using std::cout;
+  using std::endl;
+
+  constexpr int kDerivatives = 5;
+  constexpr int kNumDofs = 2;
+  VectorX<AutoDiffXd> b(kNumDofs);
+  b << 0, -6.00689e-17;
+
+  VectorXd db0(kDerivatives);
+  db0 << 0, 0, 11, -0.5, 0.1;
+  VectorXd db1(kDerivatives);
+  db1 << 0, 0.4905, -0.5, 0.25, 0;
+  b[0].derivatives() = db0;
+  b[1].derivatives() = db1;
+
+  cout << "db: " << endl;
+  cout << math::ExtractGradient(b) << endl;
+  cout << endl;
+
+  Matrix2<AutoDiffXd> A;
+  A << 11, -0.5, -0.5, 0.25;
+
+  VectorXd dA10(kDerivatives);
+  dA10 << 0, -6.12323e-17, 0, 0, 0;
+  VectorXd dA01 = dA10;
+  A(0, 0).derivatives() = VectorXd::Zero(5);
+  A(1, 1).derivatives() = VectorXd::Zero(5);
+  A(0, 1).derivatives() = dA01;
+  A(1, 0).derivatives() = dA10;
+  Vector2<AutoDiffXd> x = A.ldlt().solve(b);
+  Vector2<AutoDiffXd> Ax = A * x;
+  cout << "d(A*x): " << endl;
+  cout << math::ExtractGradient(A* x) << endl;
+  cout << endl;
+
+  cout << "dx: " << endl;
+  cout << math::ExtractGradient(x) << endl;
+  cout << endl;
+  EXPECT_TRUE(false);
+}
+
+}  // namespace
+}  // namespace test
+}  // namespace drake

multibody/plant/BUILD.bazel

@@ -68,6 +68,7 @@ drake_cc_library(
     deps = [
         "//common:default_scalars",
         "//common:extract_double",
+        "//math:gradient",
     ],
 )
 

multibody/plant/tamsi_solver.cc

@@ -7,6 +7,7 @@
 #include <vector>
 
 #include "drake/common/extract_double.h"
+#include "drake/math/autodiff_gradient.h"
 
 namespace drake {
 namespace multibody {
@@ -621,7 +622,19 @@ TamsiSolverResult TamsiSolver<T>::SolveWithGuess(
     auto& v = fixed_size_workspace_.mutable_v();
     // With no friction forces Eq. (3) in the documentation reduces to
     // M vˢ⁺¹ = p*.
-    v = M.ldlt().solve(p_star);
+    v = M.llt().solve(p_star);
+    if constexpr (std::is_same_v<T, AutoDiffXd>) {
+      std::cout << "dp_star = "
+              << math::ExtractGradient(p_star)
+              << std::endl;
+      std::cout << "d(M*v) = "
+              << math::ExtractGradient(M*v)
+              << std::endl;
+      std::cout << "dv = "
+              << math::ExtractGradient(v)
+              << std::endl;
+    }
+
     // "One iteration" with exactly "zero" vt_error.
     statistics_.Update(0.0);
     return TamsiSolverResult::kSuccess;