From 8ecb82df23e4049d09cf4dfb14cea31d1880ae48 Mon Sep 17 00:00:00 2001
From: Russ Tedrake <russt@mit.edu>
Date: Thu, 11 May 2023 10:31:25 -0400
Subject: [PATCH] Improve math::AreAutoDiffVecXdEqual (#19393)

Previously, it was possible that ExtractGradient (and therefore
AreAutoDiffVecXdEqual) would throw, when this method should simply
return false. Now we handle the case where the elements of the vector
could have a different number of derivative values.

This version should also be slightly faster.

N.B. This came up for me because I had two different constraints
setting values in a shared Context, but one was calling
SetPositions(), and the other was calling SetPositionsAndVelocities().
---
 math/autodiff_gradient.cc  | 14 ++++++++------
 math/test/autodiff_test.cc |  4 ++++
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/math/autodiff_gradient.cc b/math/autodiff_gradient.cc
index f3016e6a6227..e8954a6140c4 100644
--- a/math/autodiff_gradient.cc
+++ b/math/autodiff_gradient.cc
@@ -11,13 +11,15 @@ bool AreAutoDiffVecXdEqual(const Eigen::Ref<const VectorX<AutoDiffXd>>& a,
   if (math::ExtractValue(a) != math::ExtractValue(b)) {
     return false;
   }
-  const Eigen::MatrixXd a_gradient = math::ExtractGradient(a);
-  const Eigen::MatrixXd b_gradient = math::ExtractGradient(b);
-  if (a_gradient.rows() != b_gradient.rows() ||
-      a_gradient.cols() != b_gradient.cols()) {
-    return false;
+  for (int i = 0; i < a.size(); ++i) {
+    if (a(i).derivatives().size() != b(i).derivatives().size()) {
+      return false;
+    }
+    if (a(i).derivatives() != b(i).derivatives()) {
+      return false;
+    }
   }
-  return a_gradient == b_gradient;
+  return true;
 }
 
 }  // namespace math
diff --git a/math/test/autodiff_test.cc b/math/test/autodiff_test.cc
index 9967483d06e3..8565ebba00a5 100644
--- a/math/test/autodiff_test.cc
+++ b/math/test/autodiff_test.cc
@@ -472,6 +472,10 @@ GTEST_TEST(AutoDiffEqual, AreAutoDiffVecXdEqualTest) {
   EXPECT_FALSE(AreAutoDiffVecXdEqual(a, b));
   b[0].derivatives() = a[0].derivatives();
   EXPECT_TRUE(AreAutoDiffVecXdEqual(a, b));
+  // Set derivatives of b[0] and b[1] to have different sizes. ExtractGradient
+  // will throw, but AreAutoDiffVecXdEqual should return false.
+  b[1].derivatives() = Eigen::Vector3d(1, 2, 3);
+  EXPECT_FALSE(AreAutoDiffVecXdEqual(a, b));
 }
 
 }  // namespace