diff --git a/doc/APIreference.rst b/doc/APIreference.rst index e504cded..bdddec4e 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -6336,6 +6336,62 @@ mju_eig3 Eigenvalue decomposition of symmetric 3x3 matrix. +.. _mju_boxQP: + +mju_boxQP +~~~~~~~~~ + +.. code-block:: C + + int mju_boxQP(mjtNum* res, mjtNum* R, int* index, const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper); + +Minimize :math:`\tfrac{1}{2} x^T H x + x^T g \quad \text{s.t.} \quad l \le x \le u`, 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 :ref:`mju_boxQPmalloc` allocates the required data structures. + +.. _mju_boxQPmalloc: + +mju_boxQPmalloc +~~~~~~~~~~~~~~~ + +.. code-block:: C + + void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, mjtNum** H, mjtNum** g, int n, + mjtNum** lower, mjtNum** upper); + +Allocate heap memory for box-constrained Quadratic Program. +As in :ref:`mju_boxQP`, ``index``, ``lower``, and ``upper`` are optional. +Free all pointers with ``mju_free()``. + .. _Miscellaneous: Miscellaneous diff --git a/doc/changelog.rst b/doc/changelog.rst index 62fb2711..368b90eb 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,6 +16,16 @@ General - Added hamstring tendons which couple the hip and knee at high hip flexion angles. - General cosmetic improvements, including improved use of defaults and better naming scheme. +- Added :ref:`mju_boxQP` and allocation function :ref:`mju_boxQPmalloc` for solving the box-constrained + Quadratic Program: + + .. math:: + + x^* = \text{argmin} \; \tfrac{1}{2} x^T H x + x^T g \quad \text{s.t.} \quad l \le x \le u + + - The algorithm, introduced in `Tassa et al. 2014 `_, + converges after 2-5 Cholesky factorisations, independent of problem size. + Version 2.2.2 (September 7, 2022) --------------------------------- diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 821c1a5f..922092a3 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -991,7 +991,7 @@ MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[ const mjtNum vec[3]); -//--------------------------------- Decompositions ------------------------------------------------- +//--------------------------------- Decompositions / Solvers --------------------------------------- // Cholesky decomposition: mat = L*L'; return rank, decomposition performed in-place into mat. MJAPI int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag); @@ -1005,6 +1005,34 @@ MJAPI int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus); // Eigenvalue decomposition of symmetric 3x3 matrix. MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]); +// 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 +MJAPI int mju_boxQP(mjtNum* res, mjtNum* R, int* index, const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper); + +// allocate heap memory for box-constrained Quadratic Program +// as in mju_boxQP, index, lower, and upper are optional +// free all pointers with mju_free() +MJAPI void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, mjtNum** H, mjtNum** g, int n, + mjtNum** lower, mjtNum** upper); //---------------------- Miscellaneous ------------------------------------------------------------- diff --git a/introspect/functions.py b/introspect/functions.py index 7a3a198c..6214ff7d 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -6416,6 +6416,128 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Eigenvalue decomposition of symmetric 3x3 matrix.', )), + ('mju_boxQP', + FunctionDecl( + name='mju_boxQP', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='res', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='R', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='index', + type=PointerType( + inner_type=ValueType(name='int'), + ), + ), + FunctionParameterDecl( + name='H', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='g', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='n', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='lower', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='upper', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + ), + 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 + )), + ('mju_boxQPmalloc', + FunctionDecl( + name='mju_boxQPmalloc', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='res', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + ), + FunctionParameterDecl( + name='R', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + ), + FunctionParameterDecl( + name='index', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='int'), + ), + ), + ), + FunctionParameterDecl( + name='H', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + ), + FunctionParameterDecl( + name='g', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + ), + FunctionParameterDecl( + name='n', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='lower', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + ), + FunctionParameterDecl( + name='upper', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + ), + ), + doc='allocate heap memory for box-constrained Quadratic Program as in mju_boxQP, index, lower, and upper are optional free all pointers with mju_free()', # pylint: disable=line-too-long + )), ('mju_muscleGain', FunctionDecl( name='mju_muscleGain', diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 65d0a933..2c2f95e4 100644 --- a/python/mujoco/bindings_test.py +++ b/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 diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 897d7afe..14b78b35 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -938,7 +938,41 @@ PYBIND11_MODULE(_functions, pymodule) { mat.data(), x.data(), mat.rows(), flg_plus); }); Def(pymodule); - + DEF_WITH_OMITTED_PY_ARGS(traits::mju_boxQP, "n")( + pymodule, + [](Eigen::Ref res, + Eigen::Ref R, + std::optional>> index, + Eigen::Ref H, + Eigen::Ref g, + std::optional> lower, + std::optional> 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(pymodule); Def(pymodule); diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index fa6dac89..8212c025 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -15,6 +15,7 @@ #include "engine/engine_util_solve.h" #include +#include #include #include @@ -22,6 +23,7 @@ #include "engine/engine_macro.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" #include "engine/engine_util_sparse.h" #include "engine/engine_util_spatial.h" @@ -753,3 +755,349 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, return (la!=0); } + + + +//--------------------------- box-constrained quadratic program ------------------------------------ + +// 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 +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 +{ + // algorithm options + int maxiter = 100; // maximum number of iterations + mjtNum mingrad = 1E-16; // minimum squared norm of (unclamped) gradient + mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize + mjtNum minstep = 1E-22; // minimum stepsize for linesearch + mjtNum armijo = 0.1; // Armijo parameter (fraction of expected linear improvement) + + // logging (disabled) + char* log = NULL; // buffer to write log messages into + int logsz = 0; // size of log buffer + + return mju_boxQPoption(res, R, index, H, g, n, lower, upper, + maxiter, mingrad, backtrack, minstep, armijo, log, logsz); +} + + + +// allocate heap memory for box-constrained Quadratic Program +// as in mju_boxQP, index, lower and upper are optional +// free all pointers with mju_free() +void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, + mjtNum** H, mjtNum** g, int n, + mjtNum** lower, mjtNum** upper) { + // required arrays + *res = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + *R = (mjtNum*) mju_malloc(sizeof(mjtNum)*n*(n+7)); + *H = (mjtNum*) mju_malloc(sizeof(mjtNum)*n*n); + *g = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + + // optional arrays + if (lower) *lower = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + if (upper) *upper = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + if (index) *index = (int*) mju_malloc(sizeof(int)*n); +} + + + +// local enum encoding mju_boxQP solver status (purely for readability) +enum mjtStatusBoxQP { + mjBOXQP_NOT_SPD = -1, // Hessian is not positive definite + mjBOXQP_NO_DESCENT = 0, // no descent direction found + mjBOXQP_MAX_ITER = 1, // maximum main iterations exceeded + mjBOXQP_MAX_LS_ITER = 2, // maximum line-search iterations exceeded + mjBOXQP_TOL_GRAD = 3, // gradient norm smaller than tolerance + mjBOXQP_UNBOUNDED = 4, // no dimensions clamped, returning Newton point + mjBOXQP_ALL_CLAMPED = 5, // all dimensions clamped + + mjNBOXQP = 7 // number of boxQP status values +}; + + + +// 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 +// mingrad minimum squared norm of (unclamped) gradient +// backtrack backtrack factor for decreasing stepsize +// minstep minimum stepsize for linesearch +// armijo Armijo parameter (fraction of expected linear improvement) +// log buffer to write log messages into +// logsz size of log buffer +int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs + const mjtNum* H, const mjtNum* g, int n, // QP definition + const mjtNum* lower, const mjtNum* upper, // bounds + int maxiter, mjtNum mingrad, mjtNum backtrack, // options + mjtNum minstep, mjtNum armijo, // options + char* log, int logsz) // logging +{ + int status = mjBOXQP_NO_DESCENT; // initial status: no descent direction found + int factorize = 1; // always factorize on the first iteration + int nfree = n; // initialise nfree with n + int nfactor = 0; + mjtNum sdotg, improvement=0, value=0, norm2=0; + + // basic checks + if (n<=0) { + mju_error("mju_boxQP: problem size n must be positive"); + } + if (upper && lower) { + for (int i=0; i= upper[i]) { + mju_error("mju_boxQP: upper bounds must be stricly larger than lower bounds"); + } + } + } + + // local scratch vectors, allocate in R + mjtNum* scratch = R + n*n; + mjtNum* grad = scratch + 0*n; + mjtNum* search = scratch + 1*n; + mjtNum* candidate = scratch + 2*n; + mjtNum* temp = scratch + 3*n; + int* clamped = (int*) (scratch + 4*n); + int* oldclamped = (int*) (scratch + 5*n); + + // if index vector not given, use scratch space + if (!index) { + index = (int*) (scratch + 6*n); + } + + static const char status_string[mjNBOXQP][50]= { + "Hessian is not positive definite", + "No descent direction found", + "Maximum main iterations exceeded", + "Maximum line-search iterations exceeded", + "Gradient norm smaller than tolerance", + "No dimensions clamped, returning Newton point", + "All dimensions clamped" + }; + + // no bounds: return Newton point + if (!lower && !upper) { + // try to factorize + mju_copy(R, H, n*n); + int rank = mju_cholFactor(R, n, mjMINVAL); + if (rank == n) { + mju_cholSolve(res, R, g, n); + mju_scl(res, res, -1, n); + nfactor = 1; + status = mjBOXQP_UNBOUNDED; + } else { + status = mjBOXQP_NOT_SPD; + } + + // full index set (no clamping) + for (int i=0; i 0 ) || + ( upper && res[i] == upper[i] && grad[i] < 0 ); + } + + // build index of free dimensions, count them + nfree = 0; + for (int i=0; i= 0) { + break; // SHOULD NOT OCCUR + } + + // ------ projected Armijo line search + mjtNum step = 1; + int nstep = 0; + do { + // candidate = clamp(x + step*search) + mju_scl(candidate, search, step, n); + mju_addTo(candidate, res, n); + for (int i=0; iupper[i]) { + candidate[i] = upper[i]; + } + } + + // new objective value + mju_mulMatVec(temp, H, candidate, n, n); + value = 0.5 * mju_dot(candidate, temp, n) + mju_dot(candidate, g, n); + + // increment and break if step is too small + nstep++; + step = step*backtrack; + if (step= Armijo + improvement = (value - oldvalue) / (step*sdotg); + } while (improvement < armijo); + + + // print iteration info + if (log) { + logptr += snprintf(log+logptr, logsz-logptr, + "iter %-3d: |grad|: %-8.2g reduction: %-8.2g improvement: %-8.4g " + "linesearch: %g^%-2d factorized: %d nfree: %d\n", + iter+1, mju_sqrt(norm2), oldvalue-value, improvement, + backtrack, nstep-1, factorize, nfree); + } + + // accept candidate + mju_copy(res, candidate, n); + } + + // max iterations exceeded + if (iter==maxiter) { + status = mjBOXQP_MAX_ITER; + } + + // print final info + if (log) { + snprintf(log+logptr, logsz-logptr, "BOXQP: %s.\n" + "iterations= %d, factorizations= %d, |grad|= %-12.6g, final value= %-12.6g\n", + status_string[status+1], iter, nfactor, mju_sqrt(norm2), value); + } + + // return nf or -1 if failure + return (status == mjBOXQP_NO_DESCENT || status == mjBOXQP_NOT_SPD) ? -1 : nfree; +} + diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index 3be81b14..adaff910 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -75,6 +75,26 @@ MJAPI int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, const mjt // return 0 if unconstrained, 1 if constrained int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, const mjtNum* d, mjtNum r, int n); +// solve box-constrained Quadratic Program +// min 0.5*x'*H*x + x'*g s.t. lower <= x <=upper +// return rank of unconstrained subspace or -1 on failure +MJAPI int mju_boxQP(mjtNum* res, mjtNum* R, int* index, + const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper); + +// allocate memory for box-constrained Quadratic Program +MJAPI void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, + mjtNum** H, mjtNum** g, int n, + mjtNum** lower, mjtNum** upper); + +// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <=upper, explicit options (see implemetation) +MJAPI int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, + const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper, + int maxiter, mjtNum mingrad, mjtNum backtrack, + mjtNum minstep, mjtNum armijo, + char* log, int logsz); + #ifdef __cplusplus } #endif diff --git a/test/engine/engine_util_solve_test.cc b/test/engine/engine_util_solve_test.cc index ca8a83b8..444ecb05 100644 --- a/test/engine/engine_util_solve_test.cc +++ b/test/engine/engine_util_solve_test.cc @@ -16,13 +16,21 @@ #include "src/engine/engine_util_solve.h" +#include + +#include #include #include +#include "src/engine/engine_util_blas.h" +#include "src/engine/engine_util_misc.h" #include "test/fixture.h" namespace mujoco { namespace { +using ::testing::DoubleEq; +using ::std::string; +using ::std::setw; using QCQP2Test = MujocoTest; TEST_F(QCQP2Test, DegenerateAMatrix) { @@ -62,5 +70,263 @@ TEST_F(QCQP3Test, DegenerateAMatrix) { EXPECT_EQ(res[2], 0); } +// --------------------------- mju_boxQP --------------------------------------- + +using BoxQPTest = MujocoTest; + +// utility: compute QP objective = 0.5*x'*H*x + x'*g +mjtNum objective(const mjtNum* x, const mjtNum* H, const mjtNum* g, int n, + mjtNum* temp) { + mju_mulMatVec(temp, H, x, n, n); + return 0.5 * mju_dot(x, temp, n) + mju_dot(x, g, n); +} + +// utility: test if res is the minimum of a given box-QP problem +bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper) { + static const mjtNum eps = 1e-4; // epsilon used for nudging + bool is_minimum = true; + mjtNum* temp = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + + // get solution value + mjtNum value = objective(res, H, g, n, temp); + mjtNum value_nudge; + + // compare to nudged solution + mju_copy(res_nudge, res, n); + int i; + for (i=0; i < n; i++) { + // nudge down + res_nudge[i] = res[i] - eps; + if (lower) { + res_nudge[i] = mju_max(lower[i], res_nudge[i]); + } + value_nudge = objective(res_nudge, H, g, n, temp); + if (value_nudge - value < 0) { + is_minimum = false; + break; + } + + // nudge up + res_nudge[i] = res[i] + eps; + if (upper) { + res_nudge[i] = mju_min(upper[i], res_nudge[i]); + } + value_nudge = objective(res_nudge, H, g, n, temp); + if (value_nudge - value < 0) { + is_minimum = false; + break; + } + + // reset + res_nudge[i] = res[i]; + } + + mju_free(res_nudge); + mju_free(temp); + return is_minimum; +} + +// utility: define QP with pseudorandom values +void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper, + int seed) { + // make distribution using seed + std::mt19937_64 rng; + rng.seed(seed); + std::normal_distribution dist(0, 1); + + // square root of H + mjtNum* sqrtH = (mjtNum*) mju_malloc(sizeof(mjtNum)*n*n); + + for (int i=0; i < n; i++) { + g[i] = dist(rng); + lower[i] = 5*dist(rng); + upper[i] = 5*dist(rng); + + // fix invalid bounds + if (lower[i] > upper[i]) { + mjtNum tmp = upper[i]; + upper[i] = lower[i]; + lower[i] = tmp; + } + + // sample temp + for (int j=0; j < n; j++) { + sqrtH[n*i+j] = dist(rng); + } + } + + // make SPD matrix H + mju_mulMatTMat(H, sqrtH, sqrtH, n, n, n); + + mju_free(sqrtH); +} + +// test mju_boxQP on a small unbounded QP +TEST_F(BoxQPTest, UnboundedQP) { + // small arrays, allocate on stack + static const int n = 2; + mjtNum H[n*n] = { + 2, 0, + 0, 2 + }; + mjtNum g[n] = {1, 3}; + mjtNum res[n] = {0, 0}; + mjtNum R[n*(n+7)]; + + int nfree = mju_boxQP(res, R, /*index=*/nullptr, H, g, n, + /*lower=*/nullptr, /*upper=*/nullptr); + + // no bounds, expect Newton point + EXPECT_EQ(nfree, 2); + EXPECT_THAT(res[0], DoubleEq(-g[0]/H[0])); + EXPECT_THAT(res[1], DoubleEq(-g[1]/H[3])); + + // check that solution is actual minimum + EXPECT_TRUE(isQPminimum(res, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr)); + + // perturb solution, expected it no longer be the minimum + res[0] += 0.001; + EXPECT_FALSE(isQPminimum(res, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr)); + + // negative-definite Hessian, no solution + H[0] = -1; + nfree = mju_boxQP(res, R, /*index=*/nullptr, H, g, n, + /*lower=*/nullptr, /*upper=*/nullptr); + + EXPECT_EQ(nfree, -1); +} + +// test mju_boxQP on a single random bounded QP +TEST_F(BoxQPTest, BoundedQP) { + int n = 50; // problem size + + // 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); + + randomBoxQP(n, H, g, lower, upper, /*seed=*/1); + + // initialize res + mju_zero(res, n); + + // use default options + int maxiter = 100; // maximum number of iterations + mjtNum mingrad = 1E-16; // minimum squared norm of (unclamped) gradient + mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize + mjtNum minstep = 1E-22; // minimum stepsize for linesearch + mjtNum armijo = 0.1; // Armijo parameter + + // logging + static const int logsz = 10000; + char log[logsz]; + + int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, + maxiter, mingrad, backtrack, + minstep, armijo, log, logsz); + + // EXPECT_TRUE(false) << log; // uncomment to print `log` to error log + + // check solution + EXPECT_GT(nfree, -1); + EXPECT_TRUE(isQPminimum(res, H, g, n, lower, upper)); + + // verify clamping + int j = nfree > 0 ? 0 : -1; + for (int i=0; i < n; i++) { + if (j >= 0 && i == index[j]) { // free dimension + EXPECT_GT(res[i], lower[i]); + EXPECT_LT(res[i], upper[i]); + j++; + } else { // clamped dimension + EXPECT_TRUE(res[i] == lower[i] || res[i] == upper[i]); + } + } + mju_free(res); + mju_free(R); + mju_free(index); + mju_free(H); + mju_free(g); + mju_free(lower); + mju_free(upper); +} + +// test mju_boxQP on a set of random bounded QPs +TEST_F(BoxQPTest, BoundedQPvariations) { + int nmax = 100; + + // allocate maximum size on heap + mjtNum *H, *g, *lower, *upper; // inputs + mjtNum *res, *R; // outputs + int* index; // outputs + mju_boxQPmalloc(&res, &R, &index, &H, &g, nmax, &lower, &upper); + + // logging + static const int logsz = 10000; + char log[logsz]; + + int seed = 1; + for (int n : {3, 30, 100}) { + int count = 0; + int factorizations = 0; + for (mjtNum scaleH : {.01, 1.0, 100.0}) { + for (mjtNum scaleg : {.01, 1.0, 100.0}) { + for (mjtNum scalebounds : {.01, 1.0, 100.0}) { + // make random box-QP + randomBoxQP(n, H, g, lower, upper, seed++); + + mju_scl(H, H, scaleH, n*n); + mju_scl(g, g, scaleg, n); + mju_scl(lower, lower, scalebounds, n); + mju_scl(upper, upper, scalebounds, n); + + // initialize with zeros + mju_zero(res, n); + + // default algorithm options + int maxiter = 100; + mjtNum mingrad = 1E-16; + mjtNum backtrack = 0.5; + mjtNum minstep = 1E-22; + mjtNum armijo = 0.1; + + // solve box-QP with logging + int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, + maxiter, mingrad, backtrack, + minstep, armijo, log, logsz); + + // check solution + EXPECT_GT(nfree, -1) << log; + EXPECT_TRUE(isQPminimum(res, H, g, n, lower, upper)) + << "n " << n << '\n' + << "scaleH " << scaleH << '\n' + << "scaleg " << scaleg << '\n' + << "scalebounds " << scalebounds << '\n'; + + // wrap log with string, count factorizations + string slog(log); + string factorstr = "factorizations="; + std::size_t index = slog.find(factorstr) + factorstr.length(); + factorizations += std::stoi(slog.substr(index, 3)); + count++; + } + } + } + double meanfactor = ((double)factorizations) / count; + std::cerr << "n=" << setw(3) << n + << ": average of " << meanfactor << " factorizations\n"; + } + mju_free(res); + mju_free(R); + mju_free(index); + mju_free(H); + mju_free(g); + mju_free(lower); + mju_free(upper); +} + } // namespace } // namespace mujoco