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

@@ -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

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