mju_boxQP only reads from the lower triangle of the input Hessian.

- Before this change, an asymmetric Hessian would lead to solver failure. After this change the Hessian is symmetric by definition (upper triangle is ignored).
- API documentation was updated to reflect this contract.

PiperOrigin-RevId: 477135411
Change-Id: I60d011d36853afebac26c359771c217baf101360

doc/APIreference.rst

@@ -6388,6 +6388,7 @@ notes:
   ``R`` must have allocatd size ``n*(n+7)``, but only ``nfree*nfree`` values are used in output.
   ``index`` (if given) must have allocated size ``n``, but only ``nfree`` values are used in output.
   The convenience function :ref:`mju_boxQPmalloc` allocates the required data structures.
+  Only the lower triangles of H and R and are read from and written to, respectively.
 
 .. _mju_boxQPmalloc:
 

include/mujoco/mujoco.h

@@ -1032,6 +1032,7 @@ MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjt
 //     the initial value of res is used to warmstart the solver
 //     R must have allocatd size n*(n+7), but only nfree*nfree values are used in output
 //     index (if given) must have allocated size n, but only nfree values are used in output
+//     only the lower triangles of H and R and are read from and written to, respectively
 //     the convenience function mju_boxQPmalloc allocates the required data structures
 MJAPI int mju_boxQP(mjtNum* res, mjtNum* R, int* index, const mjtNum* H, const mjtNum* g, int n,
                     const mjtNum* lower, const mjtNum* upper);

introspect/functions.py

@@ -6524,7 +6524,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  ),
              ),
          ),
-         doc="minimize 0.5*x'*H*x + x'*g  s.t. lower <= x <= upper, return rank or -1 if failed   inputs:     n           - problem dimension     H           - SPD matrix                n*n     g           - bias vector               n     lower       - lower bounds              n     upper       - upper bounds              n     res         - solution warmstart        n   return value:     nfree <= n  - rank of unconstrained subspace, -1 if failure   outputs (required):     res         - solution                  n     R           - subspace Cholesky factor  nfree*nfree    allocated: n*(n+7)   outputs (optional):     index       - set of free dimensions    nfree          allocated: n   notes:     the initial value of res is used to warmstart the solver     R must have allocatd size n*(n+7), but only nfree*nfree values are used in output     index (if given) must have allocated size n, but only nfree values are used in output     the convenience function mju_boxQPmalloc allocates the required data structures",  # pylint: disable=line-too-long
+         doc="minimize 0.5*x'*H*x + x'*g  s.t. lower <= x <= upper, return rank or -1 if failed   inputs:     n           - problem dimension     H           - SPD matrix                n*n     g           - bias vector               n     lower       - lower bounds              n     upper       - upper bounds              n     res         - solution warmstart        n   return value:     nfree <= n  - rank of unconstrained subspace, -1 if failure   outputs (required):     res         - solution                  n     R           - subspace Cholesky factor  nfree*nfree    allocated: n*(n+7)   outputs (optional):     index       - set of free dimensions    nfree          allocated: n   notes:     the initial value of res is used to warmstart the solver     R must have allocatd size n*(n+7), but only nfree*nfree values are used in output     index (if given) must have allocated size n, but only nfree values are used in output     only the lower triangles of H and R and are read from and written to, respectively     the convenience function mju_boxQPmalloc allocates the required data structures",  # pylint: disable=line-too-long
      )),
     ('mju_boxQPmalloc',
      FunctionDecl(

src/engine/engine_util_solve.c

@@ -779,6 +779,7 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
 //     the initial value of res is used to warmstart the solver
 //     R must have allocatd size n*(n+7), but only nfree*nfree values are used in output
 //     index (if given) must have allocated size n, but only nfree values are used in output
+//     only lower triangles of H and R and read from and written to, respectively
 int mju_boxQP(mjtNum* res, mjtNum* R, int* index,        // outputs
               const mjtNum* H, const mjtNum* g, int n,   // QP definition
               const mjtNum* lower, const mjtNum* upper)  // bounds
@@ -834,6 +835,17 @@ enum mjtStatusBoxQP {
 };
 
 
+// multiply symmetric matrix with vector on both sides: return vec'*mat*vec
+//   assumes symmetry of mat, ignores upper triangle
+static mjtNum mulVecMatVecSym(const mjtNum* vec, const mjtNum* mat, int n) {
+  mjtNum res = 0;
+  for (int i=0; i<n; i++) {
+    res += vec[i] * mat[n*i+i] * vec[i];           // diagonal
+    res += 2 * vec[i] * mju_dot(mat+n*i, vec, i);  // off-diagonal
+  }
+  return res;
+}
+
 
 // minimize 0.5*x'*H*x + x'*g  s.t. lower <= x <=upper, explicit options
 //  additional arguments to mju_boxQP (see mju_boxQP documentation):
@@ -934,7 +946,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
     }
 
     // compute objective: value = 0.5*res'*H*res + res'*g
-    value = 0.5 * mju_mulVecMatVec(res, H, res, n) + mju_dot(res, g, n);
+    value = 0.5 * mulVecMatVecSym(res, H, n) + mju_dot(res, g, n);
 
     // save last value
     oldvalue = value;
@@ -994,7 +1006,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
     // R = compress_free(H)
     if (factorize) {
       for (int i=0; i<nfree; i++) {
-        for (int j=0; j<nfree; j++) {
+        for (int j=0; j<i+1; j++) {
           R[i*nfree+j] = H[index[i]*n+index[j]];
         }
       }
@@ -1055,7 +1067,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
       }
 
       // new objective value
-      value = 0.5 * mju_mulVecMatVec(candidate, H, candidate, n) + mju_dot(candidate, g, n);
+      value = 0.5 * mulVecMatVecSym(candidate, H, n) + mju_dot(candidate, g, n);
 
       // increment and break if step is too small
       nstep++;

test/engine/engine_util_solve_test.cc

@@ -194,6 +194,30 @@ TEST_F(BoxQPTest, UnboundedQP) {
   EXPECT_EQ(nfree, -1);
 }
 
+// small bounded QP with asymmetric Hessian (upper triangle ignored)
+TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
+  // small arrays, allocate on stack
+  static const int n = 2;
+  mjtNum H[n*n] = {
+    1, -400,
+    0, 1
+  };
+  mjtNum g[n] = {1, 3};
+  mjtNum res[n] = {0, 0};
+  mjtNum lower[n] = {-2, -2};
+  mjtNum upper[n] = {0, 0};
+  int index[n];
+  mjtNum R[n*(n+7)];
+
+  // solve box-QP
+  int nfree = mju_boxQP(res, R, index, H, g, n, lower, upper);
+
+  EXPECT_EQ(nfree, 1);
+
+  EXPECT_THAT(res[0], DoubleEq(-g[0]/H[0]));
+  EXPECT_THAT(res[1], DoubleEq(lower[1]));
+}
+
 // test mju_boxQP on a single random bounded QP
 TEST_F(BoxQPTest, BoundedQP) {
   int n = 50;                     // problem size