chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add mju_boxQP solving box-constrained quadratic programs.

Browse Source 
PiperOrigin-RevId: 474256629
Change-Id: I87d70fe6899608122fe0688b017420a3e81afae2
This commit is contained in:
Yuval Tassa
2022-09-14 04:30:58 -07:00
committed by Copybara-Service
parent 04d44e1e0b
commit 8957976674
9 changed files with 898 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+56
View File
@@ -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
doc/changelog.rst
+10
View File
@@ -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 <https://doi.org/10.1109/ICRA.2014.6907001>`_,
converges after 2-5 Cholesky factorisations, independent of problem size.
Version 2.2.2 (September 7, 2022)
---------------------------------
include/mujoco/mujoco.h
+29 -1
View File
@@ -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 -------------------------------------------------------------
introspect/functions.py
+122
View File
@@ -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',
python/mujoco/bindings_test.py
+12
View File
@@ -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
+35 -1
View File
@@ -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);
src/engine/engine_util_solve.c
+348
View File
@@ -15,6 +15,7 @@
#include "engine/engine_util_solve.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <mujoco/mjdata.h>
@@ -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<n; i++) {
if (lower[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<n; i++) {
index[i] = i;
}
}
// have bounds: clamp res
else {
for (int i=0; i<n; i++) {
if (lower) {
res[i] = mju_max(res[i], lower[i]);
}
if (upper) {
res[i] = mju_min(res[i], upper[i]);
}
}
}
// ------ main loop
int iter, logptr = 0;
mjtNum oldvalue;
for (iter=0; iter<maxiter; iter++) {
if (status != mjBOXQP_NO_DESCENT) {
break;
}
// compute objective: value = 0.5*res'*H*res + res'*g
mju_mulMatVec(temp, H, res, n, n); // TODO(b/246267542): do this in one call
value = 0.5 * mju_dot(res, temp, n) + mju_dot(res, g, n);
// save last value
oldvalue = value;
// compute gradient
mju_mulMatVec(grad, H, res, n, n);
mju_addTo(grad, g, n);
// find clamped dimensions
for (int i=0; i<n; i++) {
clamped[i] = ( lower && res[i] == lower[i] && grad[i] > 0 ) ||
( upper && res[i] == upper[i] && grad[i] < 0 );
}
// build index of free dimensions, count them
nfree = 0;
for (int i=0; i<n; i++) {
if (!clamped[i]) {
index[nfree++] = i;
}
}
// all dimensions are clamped: minimum found
if (!nfree) {
status = mjBOXQP_ALL_CLAMPED;
break;
}
// re-factorize if clamped dimensions have changed
if (iter) {
factorize = 0;
for (int i=0; i<n; i++) {
if (clamped[i] != oldclamped[i]) {
factorize = 1;
break;
}
}
}
// save last clamped
for (int i=0; i<n; i++) {
oldclamped[i] = clamped[i];
}
// get search direction: search = g + H_all,clamped * res_clamped
for (int i=0; i<n; i++) {
temp[i] = clamped[i] ? res[i] : 0;
}
mju_mulMatVec(search, H, temp, n, n);
mju_addTo(search, g, n);
// search = compress_free(search)
for (int i=0; i<nfree; i++) {
search[i] = search[index[i]];
}
// R = compress_free(H)
if (factorize) {
for (int i=0; i<nfree; i++) {
for (int j=0; j<nfree; j++) {
R[i*nfree+j] = H[index[i]*n+index[j]];
}
}
}
// re-factorize and increment counter, if required
int rank = factorize ? mju_cholFactor(R, nfree, mjMINVAL) : nfree;
nfactor += factorize;
// abort if factorization failed
if (rank != nfree) {
status = mjBOXQP_NOT_SPD;
break;
}
// temp = H_free,free \ search_free
mju_cholSolve(temp, R, search, nfree);
// search_free = expand_free(-temp) - x_free
mju_zero(search, n);
for (int i=0; i<nfree; i++) {
search[index[i]] = -temp[i] -res[index[i]];
}
// ------ check gradient
// squared norm of free gradient
norm2 = 0;
for (int i=0; i<nfree; i++) {
mjtNum grad_i = grad[index[i]];
norm2 += grad_i*grad_i;
}
// small gradient: minimum found
if (norm2<mingrad) {
status = nfree == n ? mjBOXQP_UNBOUNDED : mjBOXQP_TOL_GRAD;
break;
}
// sanity check: make sure we have a descent direction
if ((sdotg = mju_dot(search, grad, n)) >= 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; i<n; i++) {
if (lower && candidate[i]<lower[i]) {
candidate[i] = lower[i];
} else if (upper && candidate[i]>upper[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<minstep) {
status = mjBOXQP_MAX_LS_ITER;
break;
}
// repeat until relative improvement >= 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;
}
src/engine/engine_util_solve.h
+20
View File
@@ -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
test/engine/engine_util_solve_test.cc
+266
View File
@@ -16,13 +16,21 @@
#include "src/engine/engine_util_solve.h"
#include <random>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
#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<double> 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