diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 61864af4..9e689ec0 100644 --- a/doc/APIreference.rst +++ b/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: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 059cd2de..39afef05 100644 --- a/include/mujoco/mujoco.h +++ b/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); diff --git a/introspect/functions.py b/introspect/functions.py index e304f040..65d92a61 100644 --- a/introspect/functions.py +++ b/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( diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 1d463f95..50f152d1 100644 --- a/src/engine/engine_util_solve.c +++ b/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