Add mju_boxQP solving box-constrained quadratic programs.

PiperOrigin-RevId: 474256629
Change-Id: I87d70fe6899608122fe0688b017420a3e81afae2

python/mujoco/bindings_test.py

@@ -994,6 +994,18 @@ Euler integrator, semi-implicit in velocity.
         bodyexclude=0,
         geomid=geomid)
 
+  def test_mju_box_qp(self):
+    n = 5
+    res = np.zeros(n)
+    r = np.zeros((n, n+7))
+    index = np.zeros(n, np.int32)
+    h = np.eye(n)
+    g = np.ones((n,))
+    lower = -np.ones((n,))
+    upper = np.ones((n,))
+    rank = mujoco.mju_boxQP(res, r, index, h, g, lower, upper)
+    self.assertGreater(rank, -1)
+
   @parameterized.product(flg_html=(False, True), flg_pad=(False, True))
   def test_mj_printSchema(self, flg_html, flg_pad):  # pylint: disable=invalid-name
     # Make sure that mj_printSchema doesn't raise an exception

python/mujoco/functions.cc

@@ -938,7 +938,41 @@ PYBIND11_MODULE(_functions, pymodule) {
             mat.data(), x.data(), mat.rows(), flg_plus);
       });
   Def<traits::mju_eig3>(pymodule);
-
+  DEF_WITH_OMITTED_PY_ARGS(traits::mju_boxQP, "n")(
+      pymodule,
+      [](Eigen::Ref<EigenVectorX> res,
+         Eigen::Ref<EigenArrayXX> R,
+         std::optional<Eigen::Ref<Eigen::Vector<int, Eigen::Dynamic>>> index,
+         Eigen::Ref<const EigenArrayXX> H,
+         Eigen::Ref<const EigenVectorX> g,
+         std::optional<Eigen::Ref<const EigenVectorX>> lower,
+         std::optional<Eigen::Ref<const EigenVectorX>> upper) {
+        int n = res.size();
+        if (R.size() != n*(n+7)) {
+          throw py::type_error("size of R should be n*(n+7)");
+        }
+        if (index.has_value() && (index->size() != n)) {
+          throw py::type_error("size of index should equal n");
+        }
+        if (H.rows() != n || H.cols() != n) {
+          throw py::type_error("H should be of shape (n, n)");
+        }
+        if (g.size() != n) {
+          throw py::type_error("size of g should equal n");
+        }
+        if (lower.has_value() && (lower->size() != n)) {
+          throw py::type_error("size of lower should equal n");
+        }
+        if (upper.has_value() && (upper->size() != n)) {
+          throw py::type_error("size of upper should equal n");
+        }
+        return InterceptMjErrors(::mju_boxQP)(
+            res.data(), R.data(),
+            index.has_value() ? index->data() : nullptr,
+            H.data(), g.data(), n,
+            lower.has_value() ? lower->data() : nullptr,
+            upper.has_value() ? upper->data() : nullptr);
+      });
   // Miscellaneous
   Def<traits::mju_muscleGain>(pymodule);
   Def<traits::mju_muscleBias>(pymodule);