Merge pull request #3276 from dparikh79:fix/3275-boxqp-symmetric-lower-matvec

PiperOrigin-RevId: 917279467
Change-Id: If4117b1e2147679336becb558e0f87621555dfc7

src/engine/engine_util_solve.c

@@ -1407,6 +1407,19 @@ static mjtNum mulVecMatVecSym(const mjtNum* vec, const mjtNum* mat, int n) {
 }
 
 
+// multiply symmetric matrix with vector: res = mat*vec
+//   assumes symmetry of mat, ignores upper triangle, res must not alias vec
+static void mulSymVec(mjtNum* restrict res, const mjtNum* mat, const mjtNum* vec, int n) {
+  for (int i=0; i < n; i++) {
+    // diagonal + strict lower triangle: res[i] = sum_{j<=i} mat[i,j] * vec[j]
+    res[i] = mat[n*i+i]*vec[i] + mju_dot(mat+n*i, vec, i);
+
+    // strict upper mirror contribution: res[k] += mat[i,k] * vec[i] for k < i
+    mju_addToScl(res, mat+n*i, vec[i], i);
+  }
+}
+
+
 // minimize 0.5*x'*H*x + x'*g  s.t. lower <= x <=upper, explicit options
 //  additional arguments to mju_boxQP (see mju_boxQP documentation):
 //   maxiter     maximum number of iterations
@@ -1512,7 +1525,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
     oldvalue = value;
 
     // compute gradient
-    mju_mulMatVec(grad, H, res, n, n);
+    mulSymVec(grad, H, res, n);
     mju_addTo(grad, g, n);
 
     // find clamped dimensions
@@ -1555,7 +1568,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
     for (int i=0; i < n; i++) {
       temp[i] = clamped[i] ? res[i] : 0;
     }
-    mju_mulMatVec(search, H, temp, n, n);
+    mulSymVec(search, H, temp, n);
     mju_addTo(search, g, n);
 
     // search = compress_free(search)

test/engine/engine_util_solve_test.cc

@@ -19,6 +19,7 @@
 #include <cstddef>
 #include <iomanip>
 #include <iostream>
+#include <limits>
 #include <random>
 #include <string>
 #include <vector>
@@ -228,6 +229,79 @@ TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
   EXPECT_MJTNUM_EQ(res[1], lower[1]);
 }
 
+// verify mju_boxQP reads only the lower triangle of H by poisoning the upper
+// triangle with NaN and comparing to a clean symmetric solve
+TEST_F(BoxQPTest, UpperTrianglePoisoned) {
+  int n = 30;
+  const mjtNum nan = std::numeric_limits<mjtNum>::quiet_NaN();
+
+  // allocate on heap
+  mjtNum *H, *g, *lower, *upper;  // inputs
+  mjtNum *res, *R;                // outputs
+  int* index;                     // outputs
+  mju_boxQPmalloc(&res, &R, &index, &H, &g, n, &lower, &upper);
+
+  struct Cleanup {
+    mjtNum *res, *R, *H, *g, *lower, *upper;
+    int* index;
+    ~Cleanup() {
+      mju_free(res);
+      mju_free(R);
+      mju_free(index);
+      mju_free(H);
+      mju_free(g);
+      mju_free(lower);
+      mju_free(upper);
+    }
+  } cleanup{res, R, H, g, lower, upper, index};
+
+  // generate a symmetric SPD Hessian and bounded QP problem
+  randomBoxQP(n, H, g, lower, upper, /*seed=*/1);
+
+  int maxiter = 100;
+  mjtNum mingrad = MjTol(1E-16, 1E-5);
+  mjtNum backtrack = 0.5;
+  mjtNum minstep = MjTol(1E-22, 1E-10);
+  mjtNum armijo = 0.1;
+
+  // solve with symmetric H to get the reference result
+  mju_zero(res, n);
+  int nfree_ref = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
+                                  maxiter, mingrad, backtrack, minstep,
+                                  armijo, nullptr, 0);
+  ASSERT_GT(nfree_ref, -1);
+
+  // save reference
+  std::vector<mjtNum> res_ref(res, res + n);
+  std::vector<int> index_ref(index, index + n);
+  std::vector<mjtNum> R_ref(R, R + nfree_ref * nfree_ref);
+
+  // poison the strict upper triangle of H with NaN
+  for (int i=0; i < n; i++) {
+    for (int j=i+1; j < n; j++) {
+      H[n*i+j] = nan;
+    }
+  }
+
+  // solve again; result must match because only lower triangle should be read
+  mju_zero(res, n);
+  int nfree_poisoned = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
+                                       maxiter, mingrad, backtrack, minstep,
+                                       armijo, nullptr, 0);
+
+  EXPECT_EQ(nfree_poisoned, nfree_ref);
+  for (int i=0; i < n; i++) {
+    EXPECT_EQ(res[i], res_ref[i]) << "mismatch at index " << i;
+  }
+  for (int i=0; i < nfree_ref; i++) {
+    EXPECT_EQ(index[i], index_ref[i]) << "index mismatch at " << i;
+    for (int j=0; j <= i; j++) {
+      int k = i * nfree_ref + j;
+      EXPECT_EQ(R[k], R_ref[k]) << "R mismatch at row " << i << ", col " << j;
+    }
+  }
+}
+
 // test mju_boxQP on a single random bounded QP
 TEST_F(BoxQPTest, BoundedQP) {
   int n = 50;                     // problem size