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

Merge branch 'main' into newton-schemas

Browse Source
This commit is contained in:
Sam Haves
2026-05-13 09:57:59 -04:00
committed by GitHub
parent 792f53c823 69a1087e9e
commit fc2237dad8
430 changed files with 44557 additions and 19852 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/benchmark/chol_benchmark_test.cc
+27 -18
View File
@@ -22,7 +22,6 @@
#include <absl/base/attributes.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_memory.h"
#include "src/engine/engine_support.h"
#include "src/engine/engine_util_solve.h"
#include "src/engine/engine_util_sparse.h"
@@ -68,13 +67,15 @@ struct HessianData {
// D diagonal
std::vector<mjtNum> D;
int nefc;
void Setup(const mjModel* m, mjData* d) {
// initialize simulation state
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
nv = m->nv;
int nefc = d->nefc;
nefc = d->nefc;
// compute D corresponding to quad states
D.resize(nefc);
@@ -205,14 +206,27 @@ mjModel* GetModel() {
return m;
}
template <Size S>
HessianData& GetHessianData() {
static HessianData data;
static bool initialized = false;
if (!initialized) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
data.Setup(m, d);
mj_deleteData(d);
initialized = true;
}
return data;
}
// old implementation benchmark
template <Size S>
static void BM_chol_old(benchmark::State& state) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
HessianData hd;
hd.Setup(m, d);
HessianData& hd = GetHessianData<S>();
std::vector<mjtNum> L_work(hd.nL);
std::vector<int> L_colind_work(hd.nL);
@@ -239,8 +253,7 @@ static void BM_chol_symbolic(benchmark::State& state) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
HessianData hd;
hd.Setup(m, d);
HessianData& hd = GetHessianData<S>();
std::vector<int> L_colind_work(hd.nL);
std::vector<int> LT_rownnz_work(hd.nv);
@@ -266,8 +279,7 @@ static void BM_chol_numeric(benchmark::State& state) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
HessianData hd;
hd.Setup(m, d);
HessianData& hd = GetHessianData<S>();
std::vector<mjtNum> L_work(hd.nL);
std::vector<int> L_colind_work(hd.nL);
@@ -339,10 +351,6 @@ constexpr int kNumUpdateVectors = 25;
int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old(
mjtNum* mat, mjtNum* x, int n, int flg_plus, const int* rownnz,
const int* rowadr, const int* colind, int x_nnz, int* x_ind, mjData* d) {
mj_markStack(d);
int* buf_ind = mjSTACKALLOC(d, n, int);
mjtNum* sparse_buf = mjSTACKALLOC(d, n, mjtNum);
int rank = n, i = x_nnz - 1;
while (i >= 0) {
int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]];
@@ -359,10 +367,9 @@ int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old(
mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c),
nnz - 1, i, colind + adr, x_ind);
int new_x_nnz = mju_combineSparse(x, mat + adr, c, -s, i, nnz - 1, x_ind,
colind + adr, sparse_buf, buf_ind);
colind + adr);
i = i - 1 + (new_x_nnz - i);
}
mj_freeStack(d);
return rank;
}
@@ -371,9 +378,10 @@ template <Size S>
static void BM_update_old(benchmark::State& state) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
HessianData hd;
hd.Setup(m, d);
HessianData& hd = GetHessianData<S>();
int nv = hd.nv;
@@ -433,9 +441,10 @@ template <Size S>
static void BM_update_new(benchmark::State& state) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
HessianData hd;
hd.Setup(m, d);
HessianData& hd = GetHessianData<S>();
int nv = hd.nv;
test/benchmark/engine_util_sparse_benchmark_test.cc
+244 -340
View File
@@ -14,7 +14,6 @@
// A benchmark for comparing different implementations of mj_solveLD.
#include <cstddef>
#include <cstring>
#include <vector>
@@ -31,124 +30,180 @@ namespace {
using CombineFuncPtr = decltype(&mju_combineSparse);
using TransposeFuncPtr = decltype(&mju_transposeSparse);
using SqrMatTDFuncPtr = decltype(&mju_sqrMatTDSparse);
// number of steps to roll out before benchmarking
static const int kNumWarmupSteps = 500;
// ================================ Cached Data ================================
// ----------------------------- old functions --------------------------------
// ---- MatVecSparse data ----
struct MatVecData {
int nv;
int nefc;
int nJ;
std::vector<mjtNum> efc_J;
std::vector<int> efc_J_rownnz, efc_J_rowadr, efc_J_colind, efc_J_rowsuper;
std::vector<mjtNum> vec;
};
void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline(
mjtNum* res, const mjtNum* mat, const mjtNum* matT, const mjtNum* diag,
int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind,
const int* rownnz, const int* rowadr, const int* colind,
const int* rowsuper, const int* rownnzT, const int* rowadrT,
const int* colindT, const int* rowsuperT, mjData* d, int* unused) {
mj_markStack(d);
int* chain = mj_stackAllocInt(d, 2 * nc);
mjtNum* buffer = mj_stackAllocNum(d, nc);
MatVecData& GetMatVecData() {
static MatVecData data = [] {
MatVecData d;
mjModel* m = LoadModelFromPath("flex/flag.xml");
mjData* dat = mj_makeData(m);
for (int r = 0; r < nc; r++) {
res_rowadr[r] = r * nc;
}
for (int r = 0; r < nc; r++) {
if (rowsuperT && r > 0 && rowsuperT[r - 1] > 0) {
res_rownnz[r] = res_rownnz[r - 1];
memcpy(res_colind + res_rowadr[r], res_colind + res_rowadr[r - 1],
res_rownnz[r] * sizeof(int));
if (rownnzT[r]) {
res_colind[res_rowadr[r] + res_rownnz[r]] = r;
res_rownnz[r]++;
}
} else {
int nchain = 0;
int inew = 0, iold = nc;
int lastadded = -1;
for (int i = 0; i < rownnzT[r]; i++) {
int c = colindT[rowadrT[r] + i];
if (rowsuper && lastadded >= 0 &&
(c - lastadded) <= rowsuper[lastadded]) {
continue;
} else {
lastadded = c;
}
int adr = inew;
inew = iold;
iold = adr;
int nnewchain = 0;
adr = 0;
int end = rowadr[c] + rownnz[c];
for (int adr1 = rowadr[c]; adr1 < end; adr1++) {
int col_mat = colind[adr1];
while (adr < nchain && chain[iold + adr] < col_mat &&
chain[iold + adr] <= r) {
chain[inew + nnewchain++] = chain[iold + adr++];
}
if (col_mat > r) {
break;
}
if (adr < nchain && chain[iold + adr] == col_mat) {
adr++;
}
chain[inew + nnewchain++] = col_mat;
}
while (adr < nchain && chain[iold + adr] <= r) {
chain[inew + nnewchain++] = chain[iold + adr++];
}
nchain = nnewchain;
}
res_rownnz[r] = nchain;
if (nchain) {
memcpy(res_colind + res_rowadr[r], chain + inew, nchain * sizeof(int));
}
for (int i = 0; i < 500; i++) {
mj_step(m, dat);
}
}
for (int r = 0; r < nc; r++) {
int adr = res_rowadr[r];
for (int i = 0; i < res_rownnz[r]; i++) {
buffer[res_colind[adr + i]] = 0;
}
for (int i = 0; i < rownnzT[r]; i++) {
int c = colindT[rowadrT[r] + i];
mjtNum matTrc = matT[rowadrT[r] + i];
if (diag) {
matTrc *= diag[c];
}
d.nv = m->nv;
d.nefc = dat->nefc;
d.nJ = dat->nJ;
d.efc_J.assign(dat->efc_J, dat->efc_J + d.nJ);
d.efc_J_rownnz.assign(dat->efc_J_rownnz, dat->efc_J_rownnz + d.nefc);
d.efc_J_rowadr.assign(dat->efc_J_rowadr, dat->efc_J_rowadr + d.nefc);
d.efc_J_colind.assign(dat->efc_J_colind, dat->efc_J_colind + d.nJ);
d.efc_J_rowsuper.assign(dat->efc_J_rowsuper, dat->efc_J_rowsuper + d.nefc);
int end = rowadr[c] + rownnz[c];
for (int adr = rowadr[c]; adr < end; adr++) {
int adr1;
if ((adr1 = colind[adr]) > r) {
break;
}
buffer[adr1] += matTrc * mat[adr];
}
// compute direction: vec = -M^{-1} * (Ma - qfrc_smooth - qfrc_constraint)
mj_markStack(dat);
mjtNum* Ma = mj_stackAllocNum(dat, m->nv);
mjtNum* grad = mj_stackAllocNum(dat, m->nv);
mjtNum* Mgrad = mj_stackAllocNum(dat, m->nv);
mj_mulM(m, dat, Ma, dat->qacc);
for (int i = 0; i < m->nv; i++) {
grad[i] = Ma[i] - dat->qfrc_smooth[i] - dat->qfrc_constraint[i];
}
adr = res_rowadr[r];
for (int i = 0; i < res_rownnz[r]; i++) {
res[adr + i] = buffer[res_colind[adr + i]];
}
}
for (int r = 1; r < nc; r++) {
int end = res_rowadr[r] + res_rownnz[r] - 1;
for (int adr = res_rowadr[r]; adr < end; adr++) {
int adr1 = res_rowadr[res_colind[adr]] + res_rownnz[res_colind[adr]]++;
res[adr1] = res[adr];
res_colind[adr1] = r;
}
}
mj_solveM(m, dat, Mgrad, grad, 1);
d.vec.resize(m->nv);
mju_scl(d.vec.data(), Mgrad, -1, m->nv);
mj_freeStack(dat);
mj_freeStack(d);
mj_deleteData(dat);
mj_deleteModel(m);
return d;
}();
return data;
}
// ---- CombineSparse data ----
struct CombineData {
int nv;
std::vector<mjtNum> H;
std::vector<int> rownnz, rowadr, colind;
};
CombineData& GetCombineData() {
static CombineData data = [] {
CombineData cd;
mjModel* m = LoadModelFromPath("humanoid/humanoid.xml");
m->opt.jacobian = mjJAC_SPARSE;
mjData* d = mj_makeData(m);
for (int i = 0; i < 500; i++) {
mj_step(m, d);
}
cd.nv = m->nv;
mj_markStack(d);
mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv);
int* rownnz = mj_stackAllocInt(d, m->nv);
int* rowadr = mj_stackAllocInt(d, m->nv);
int* colind = mj_stackAllocInt(d, m->nv*m->nv);
int* diagind = mj_stackAllocInt(d, m->nv);
mjtNum* D = mj_stackAllocNum(d, d->nefc);
for (int i = 0; i < d->nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i];
} else {
D[i] = 0;
}
}
int* JT_rownnz = mj_stackAllocInt(d, m->nv);
int* JT_rowadr = mj_stackAllocInt(d, m->nv);
int* JT_rowsuper = mj_stackAllocInt(d, m->nv);
int* JT_colind = mj_stackAllocInt(d, d->nJ);
mjtNum* JT = mj_stackAllocNum(d, d->nJ);
mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv,
JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
// compute H = J'*D*J, uncompressed layout
mju_sqrMatTDUncompressedInit(rowadr, m->nv);
mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv,
rownnz, rowadr, colind,
d->efc_J_rownnz, d->efc_J_rowadr,
d->efc_J_colind, d->efc_J_rowsuper,
JT_rownnz, JT_rowadr,
JT_colind, JT_rowsuper, d,
diagind);
// compute H = M + J'*D*J
mj_addM(m, d, H, rownnz, rowadr, colind);
// copy to persistent storage
int nH = rowadr[m->nv-1] + m->nv; // uncompressed: rowadr[r] = r*nv
cd.H.assign(H, H + nH);
cd.rownnz.assign(rownnz, rownnz + m->nv);
cd.rowadr.assign(rowadr, rowadr + m->nv);
cd.colind.assign(colind, colind + nH);
mj_freeStack(d);
mj_deleteData(d);
mj_deleteModel(m);
return cd;
}();
return data;
}
// ---- TransposeSparse data ----
struct TransposeData {
int nv;
int nefc;
int nJ;
std::vector<mjtNum> efc_J;
std::vector<int> efc_J_rownnz, efc_J_rowadr, efc_J_colind;
};
enum class Size { H2_100, H100 };
template <Size S>
const char* ModelPath() {
if constexpr (S == Size::H2_100) {
return "../test/benchmark/testdata/2humanoid100_chol.xml";
} else {
return "../test/benchmark/testdata/100_humanoids_chol.xml";
}
}
template <Size S>
TransposeData& GetTransposeData() {
static TransposeData data = [] {
TransposeData td;
mjModel* m = LoadModelFromPath(ModelPath<S>());
m->opt.jacobian = mjJAC_SPARSE;
mjData* d = mj_makeData(m);
while (d->time < 2) {
mj_step(m, d);
}
td.nv = m->nv;
td.nefc = d->nefc;
td.nJ = d->nJ;
td.efc_J.assign(d->efc_J, d->efc_J + d->nJ);
td.efc_J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + d->nefc);
td.efc_J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + d->nefc);
td.efc_J_colind.assign(d->efc_J_colind, d->efc_J_colind + d->nJ);
mj_deleteData(d);
mj_deleteModel(m);
return td;
}();
return data;
}
// ================================ old functions ==============================
// transpose sparse matrix (uncompressed)
void ABSL_ATTRIBUTE_NOINLINE transposeSparse_baseline(
mjtNum* res, const mjtNum* mat, int nr, int nc, int* res_rownnz,
@@ -208,8 +263,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_baseline(mjtNum* dst,
mjtNum a, mjtNum b,
int dst_nnz, int src_nnz,
int* dst_ind,
const int* src_ind,
mjtNum* buf, int* buf_ind) {
const int* src_ind) {
// check for identical pattern
if (compare_baseline(dst_ind, src_ind, dst_nnz)) {
// combine mjtNum data directly
@@ -225,8 +279,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_new(mjtNum* dst,
mjtNum a, mjtNum b,
int dst_nnz, int src_nnz,
int* dst_ind,
const int* src_ind,
mjtNum* buf, int* buf_ind) {
const int* src_ind) {
// check for identical pattern
if (compare_memcmp(dst_ind, src_ind, dst_nnz)) {
// combine mjtNum data directly
@@ -340,61 +393,31 @@ void ABSL_ATTRIBUTE_NOINLINE mulMatVecSparse_8(mjtNum* res,
}
}
// ----------------------------- benchmark ------------------------------------
// ----------------------------- benchmark -------------------------------------
static void BM_MatVecSparse(benchmark::State& state, int unroll) {
static mjModel* m = LoadModelFromPath("flex/flag.xml");
mjData* d = mj_makeData(m);
MatVecData& data = GetMatVecData();
std::vector<mjtNum> res(data.nefc);
// warm-up rollout to get a typical state
for (int i=0; i < kNumWarmupSteps; i++) {
mj_step(m, d);
}
// allocate gradient
mj_markStack(d);
mjtNum *Ma = mj_stackAllocNum(d, m->nv);
mjtNum *vec = mj_stackAllocNum(d, m->nv);
mjtNum *res = mj_stackAllocNum(d, d->nefc);
mjtNum *grad = mj_stackAllocNum(d, m->nv);
mjtNum *Mgrad = mj_stackAllocNum(d, m->nv);
// compute gradient
mj_mulM(m, d, Ma, d->qacc);
for (int i=0; i < m->nv; i++) {
grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i];
}
// compute search direction
mj_solveM(m, d, Mgrad, grad, 1);
mju_scl(vec, Mgrad, -1, m->nv);
// save state
std::vector<mjtNum> qpos = AsVector(d->qpos, m->nq);
std::vector<mjtNum> qvel = AsVector(d->qvel, m->nv);
std::vector<mjtNum> act = AsVector(d->act, m->na);
std::vector<mjtNum> warmstart = AsVector(d->qacc_warmstart, m->nv);
// time benchmark
for (auto s : state) {
if (unroll == 4) {
mju_mulMatVecSparse(res, d->efc_J, vec, d->nefc,
d->efc_J_rownnz, d->efc_J_rowadr,
d->efc_J_colind, d->efc_J_rowsuper);
mju_mulMatVecSparse(res.data(), data.efc_J.data(), data.vec.data(),
data.nefc, data.efc_J_rownnz.data(),
data.efc_J_rowadr.data(), data.efc_J_colind.data(),
data.efc_J_rowsuper.data());
} else if (unroll == 1) {
mulMatVecSparse_1(res, d->efc_J, vec, d->nefc,
d->efc_J_rownnz, d->efc_J_rowadr,
d->efc_J_colind, d->efc_J_rowsuper);
mulMatVecSparse_1(res.data(), data.efc_J.data(), data.vec.data(),
data.nefc, data.efc_J_rownnz.data(),
data.efc_J_rowadr.data(), data.efc_J_colind.data(),
data.efc_J_rowsuper.data());
} else if (unroll == 8) {
mulMatVecSparse_8(res, d->efc_J, vec, d->nefc,
d->efc_J_rownnz, d->efc_J_rowadr,
d->efc_J_colind, d->efc_J_rowsuper);
mulMatVecSparse_8(res.data(), data.efc_J.data(), data.vec.data(),
data.nefc, data.efc_J_rownnz.data(),
data.efc_J_rowadr.data(), data.efc_J_colind.data(),
data.efc_J_rowsuper.data());
}
}
// finalize
mj_freeStack(d);
mj_deleteData(d);
state.SetItemsProcessed(state.iterations());
}
@@ -420,75 +443,30 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_MatVecSparse_1(
BENCHMARK(BM_MatVecSparse_1);
static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) {
static mjModel* m = LoadModelFromPath("humanoid/humanoid.xml");
m->opt.jacobian = mjJAC_SPARSE;
CombineData& data = GetCombineData();
mjData* d = mj_makeData(m);
// warm-up rollout to get a typical state
for (int i=0; i < kNumWarmupSteps; i++) {
mj_step(m, d);
}
// allocate
mj_markStack(d);
mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv);
int* rownnz = mj_stackAllocInt(d, m->nv);
int* rowadr = mj_stackAllocInt(d, m->nv);
int* colind = mj_stackAllocInt(d, m->nv*m->nv);
int* diagind = mj_stackAllocInt(d, m->nv);
// compute D corresponding to quad states
mjtNum* D = mj_stackAllocNum(d, d->nefc);
for (int i = 0; i < d->nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i];
} else {
D[i] = 0;
}
}
int* JT_rownnz = mj_stackAllocInt(d, m->nv);
int* JT_rowadr = mj_stackAllocInt(d, m->nv);
int* JT_rowsuper = mj_stackAllocInt(d, m->nv);
int* JT_colind = mj_stackAllocInt(d, d->nJ);
mjtNum* JT = mj_stackAllocNum(d, d->nJ);
mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv,
JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
// compute H = J'*D*J, uncompressed layout
mju_sqrMatTDUncompressedInit(rowadr, m->nv);
mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv,
rownnz, rowadr, colind,
d->efc_J_rownnz, d->efc_J_rowadr,
d->efc_J_colind, d->efc_J_rowsuper,
JT_rownnz, JT_rowadr,
JT_colind, JT_rowsuper, d,
diagind);
// compute H = M + J'*D*J
mj_addM(m, d, H, rownnz, rowadr, colind);
// make working copies that get modified each iteration
std::vector<mjtNum> H = data.H;
std::vector<int> rownnz = data.rownnz;
std::vector<int> rowadr = data.rowadr;
std::vector<int> colind = data.colind;
// time benchmark
for (auto s : state) {
for (int r = m->nv-1; r >= 0; r--) {
for (int r = data.nv-1; r >= 0; r--) {
for (int i = 0; i < rownnz[r]-1; i++) {
int adr = rowadr[r];
int c = colind[adr+i];
// true arguments should be i+1 and colind+rowadr[r]
// but instead we repeat rownnz[c] and colind+rowadr[c]
// in order to trigger all if's in combineSparse
func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i],
func(H.data()+rowadr[c], H.data()+rowadr[r], 1, -H[adr+i],
rownnz[c], rownnz[c],
colind+rowadr[c], colind+rowadr[c], NULL, NULL);
colind.data()+rowadr[c], colind.data()+rowadr[c]);
}
}
}
// finalize
mj_freeStack(d);
mj_deleteData(d);
state.SetItemsProcessed(state.iterations());
}
@@ -512,172 +490,98 @@ enum class Supernode {
Inline
};
template <Size S>
static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func,
Supernode super) {
static mjModel* m = LoadModelFromPath("humanoid/humanoid100.xml");
TransposeData& data = GetTransposeData<S>();
// force use of sparse matrices
m->opt.jacobian = mjJAC_SPARSE;
mjData* d = mj_makeData(m);
// warm-up rollout to get a typical state
while (d->time < 2) {
mj_step(m, d);
}
mj_markStack(d);
// need uncompressed layout
mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc);
int* res_rownnz = mj_stackAllocInt(d, m->nv);
int* res_rowadr = mj_stackAllocInt(d, m->nv);
int* res_rowsuper = mj_stackAllocInt(d, m->nv);
int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc);
// allocate output buffers (uncompressed layout)
std::vector<mjtNum> res(data.nv * data.nefc);
std::vector<int> res_rownnz(data.nv);
std::vector<int> res_rowadr(data.nv);
std::vector<int> res_rowsuper(data.nv);
std::vector<int> res_colind(data.nv * data.nefc);
// time benchmark
for (auto s : state) {
int* rowsuper = (super == Supernode::Inline) ? res_rowsuper : nullptr;
func(res, d->efc_J, d->nefc, m->nv,
res_rownnz, res_rowadr, res_colind, rowsuper,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
int* rowsuper =
(super == Supernode::Inline) ? res_rowsuper.data() : nullptr;
func(res.data(), data.efc_J.data(), data.nefc, data.nv,
res_rownnz.data(), res_rowadr.data(), res_colind.data(), rowsuper,
data.efc_J_rownnz.data(), data.efc_J_rowadr.data(),
data.efc_J_colind.data());
if (super == Supernode::PostProcess) {
mju_superSparse(m->nv, res_rowsuper,
res_rownnz, res_rowadr, res_colind);
mju_superSparse(data.nv, res_rowsuper.data(),
res_rownnz.data(), res_rowadr.data(), res_colind.data());
}
}
mj_freeStack(d);
mj_deleteData(d);
state.SetItemsProcessed(state.iterations());
}
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_transposeSparse_old(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_transposeSparse(state, &transposeSparse_baseline, Supernode::None);
}
BENCHMARK(BM_transposeSparse_old);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_transposeSparse_new(benchmark::State& state) {
BM_transposeSparse_2H100_old(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_transposeSparse(state, &mju_transposeSparse, Supernode::None);
BM_transposeSparse<Size::H2_100>(state, &transposeSparse_baseline,
Supernode::None);
}
BENCHMARK(BM_transposeSparse_new);
BENCHMARK(BM_transposeSparse_2H100_old);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_transposeSparse_superpost(benchmark::State& state) {
BM_transposeSparse_2H100_new(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_transposeSparse(state, &mju_transposeSparse, Supernode::PostProcess);
BM_transposeSparse<Size::H2_100>(state, &mju_transposeSparse,
Supernode::None);
}
BENCHMARK(BM_transposeSparse_superpost);
BENCHMARK(BM_transposeSparse_2H100_new);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_transposeSparse_superinline(benchmark::State& state) {
BM_transposeSparse_2H100_superpost(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_transposeSparse(state, &mju_transposeSparse, Supernode::Inline);
}
BENCHMARK(BM_transposeSparse_superinline);
static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) {
static mjModel* m =
LoadModelFromPath("../test/benchmark/testdata/2humanoid100.xml");
// force use of sparse matrices, Newton solver, no islands
m->opt.jacobian = mjJAC_SPARSE;
m->opt.solver = mjSOL_NEWTON;
m->opt.disableflags |= mjDSBL_ISLAND;
mjData* d = mj_makeData(m);
// warm-up rollout to get a typical state
while (d->time < 2) {
mj_step(m, d);
}
// allocate
mj_markStack(d);
mjtNum* H = mj_stackAllocNum(d, m->nv * m->nv);
int* rownnz = mj_stackAllocInt(d, m->nv);
int* rowadr = mj_stackAllocInt(d, m->nv);
int* colind = mj_stackAllocInt(d, m->nv * m->nv);
int* diagind = mj_stackAllocInt(d, m->nv);
// compute D corresponding to quad states
mjtNum* D = mj_stackAllocNum(d, d->nefc);
for (int i = 0; i < d->nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i];
} else {
D[i] = 0;
}
}
int* JT_rownnz = mj_stackAllocInt(d, m->nv);
int* JT_rowadr = mj_stackAllocInt(d, m->nv);
int* JT_rowsuper = mj_stackAllocInt(d, m->nv);
int* JT_colind = mj_stackAllocInt(d, d->nJ);
mjtNum* JT = mj_stackAllocNum(d, d->nJ);
mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv,
JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
// time benchmark
if (func) {
mju_sqrMatTDSparseCount(rownnz, rowadr, m->nv,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind,
JT_rownnz, JT_rowadr,
JT_colind, nullptr, d, 1);
for (auto s : state) {
// compute H = J'*D*J, compressed layout
func(H, d->efc_J, JT, D, d->nefc, m->nv, rownnz, rowadr, colind,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL,
JT_rownnz, JT_rowadr, JT_colind,
JT_rowsuper, d, diagind);
}
} else {
for (auto s : state) {
// baseline depends on efc_J_rowsuper
mju_superSparse(d->nefc, d->efc_J_rowsuper,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
// compute H = J'*D*J, uncompressed layout
mju_sqrMatTDSparse_baseline(
H, d->efc_J, JT, D, d->nefc, m->nv, rownnz, rowadr, colind,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper,
JT_rownnz, JT_rowadr, JT_colind,
JT_rowsuper, d, /*unused=*/nullptr);
}
}
// finalize
mj_freeStack(d);
mj_deleteData(d);
state.SetItemsProcessed(state.iterations());
BM_transposeSparse<Size::H2_100>(state, &mju_transposeSparse,
Supernode::PostProcess);
}
BENCHMARK(BM_transposeSparse_2H100_superpost);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_sqrMatTDSparse_col(benchmark::State& state) {
BM_transposeSparse_2H100_superinline(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTDSparse(state, &mju_sqrMatTDSparse);
BM_transposeSparse<Size::H2_100>(state, &mju_transposeSparse,
Supernode::Inline);
}
BENCHMARK(BM_sqrMatTDSparse_col);
BENCHMARK(BM_transposeSparse_2H100_superinline);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_sqrMatTDSparse_row(benchmark::State& state) {
BM_transposeSparse_100H_old(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTDSparse(state, &mju_sqrMatTDSparse_row);
BM_transposeSparse<Size::H100>(state, &transposeSparse_baseline,
Supernode::None);
}
BENCHMARK(BM_sqrMatTDSparse_row);
BENCHMARK(BM_transposeSparse_100H_old);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_sqrMatTDSparse_uncompressed(benchmark::State& state) {
BM_transposeSparse_100H_new(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTDSparse(state, nullptr);
BM_transposeSparse<Size::H100>(state, &mju_transposeSparse, Supernode::None);
}
BENCHMARK(BM_sqrMatTDSparse_uncompressed);
BENCHMARK(BM_transposeSparse_100H_new);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_transposeSparse_100H_superpost(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_transposeSparse<Size::H100>(state, &mju_transposeSparse,
Supernode::PostProcess);
}
BENCHMARK(BM_transposeSparse_100H_superpost);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_transposeSparse_100H_superinline(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_transposeSparse<Size::H100>(state, &mju_transposeSparse,
Supernode::Inline);
}
BENCHMARK(BM_transposeSparse_100H_superinline);
} // namespace
} // namespace mujoco
test/benchmark/sqrmat_benchmark_test.cc
+423
View File
@@ -0,0 +1,423 @@
// Copyright 2026 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Benchmarks for sparse matrix operations.
#include <cstring>
#include <vector>
#include "benchmark/benchmark.h"
#include <absl/base/attributes.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_util_sparse.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
// ================================ Test Data ==================================
// Stores pre-computed sparse matrix inputs extracted from MuJoCo simulations.
// Each benchmark computes its own outputs (H, L, etc.) from these inputs.
struct SparseTestData {
// Dimensions
int nv; // number of DoFs
int nefc; // number of constraint rows
int nJ; // nnz in J
// J (Jacobian) - nefc x nv sparse
std::vector<mjtNum> J;
std::vector<int> J_rownnz, J_rowadr, J_colind, J_rowsuper;
// J' (transpose)
std::vector<mjtNum> JT;
std::vector<int> JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper;
// D (diagonal weights for constraints)
std::vector<mjtNum> D;
// M structure (mass matrix, lower triangle)
std::vector<int> M_rownnz, M_rowadr, M_colind;
void Setup(const mjModel* m, mjData* d) {
// initialize simulation state
mj_resetDataKeyframe(m, d, 0);
mj_step(m, d);
mj_forward(m, d);
nv = m->nv;
nefc = d->nefc;
nJ = d->nJ;
// copy J
J.assign(d->efc_J, d->efc_J + nJ);
J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + nefc);
J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + nefc);
J_colind.assign(d->efc_J_colind, d->efc_J_colind + nJ);
J_rowsuper.assign(d->efc_J_rowsuper, d->efc_J_rowsuper + nefc);
// transpose J
JT.assign(nJ, 0);
JT_rownnz.assign(nv, 0);
JT_rowadr.assign(nv, 0);
JT_colind.assign(nJ, 0);
JT_rowsuper.assign(nv, 0);
mju_transposeSparse(JT.data(), J.data(), nefc, nv, JT_rownnz.data(),
JT_rowadr.data(), JT_colind.data(), JT_rowsuper.data(),
J_rownnz.data(), J_rowadr.data(), J_colind.data());
// compute D corresponding to quadratic constraint states
D.resize(nefc);
for (int i = 0; i < nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i];
} else {
D[i] = 0;
}
}
// copy M structure
M_rownnz.assign(m->M_rownnz, m->M_rownnz + nv);
M_rowadr.assign(m->M_rowadr, m->M_rowadr + nv);
int nM = M_rowadr[nv - 1] + M_rownnz[nv - 1];
M_colind.assign(m->M_colind, m->M_colind + nM);
}
};
// ================================ Model Sizes ================================
enum class Size { H2_100, H100 };
template <Size S>
const char* ModelPath() {
if constexpr (S == Size::H2_100) {
return "../test/benchmark/testdata/2humanoid100_chol.xml";
} else {
return "../test/benchmark/testdata/100_humanoids_chol.xml";
}
}
template <Size S>
mjModel* GetModel() {
static mjModel* m = LoadModelFromPath(ModelPath<S>());
m->opt.jacobian = mjJAC_SPARSE;
m->opt.solver = mjSOL_NEWTON;
m->opt.disableflags |= mjDSBL_ISLAND;
return m;
}
template <Size S>
SparseTestData& GetData() {
static SparseTestData data;
static bool initialized = false;
if (!initialized) {
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
data.Setup(m, d);
mj_deleteData(d);
initialized = true;
}
return data;
}
// ========================== Baseline Implementations =========================
// Baseline sqrMatTD (uncompressed layout, from old implementation)
void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline(
mjtNum* res, const mjtNum* mat, const mjtNum* matT, const mjtNum* diag,
int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind,
const int* rownnz, const int* rowadr, const int* colind,
const int* rowsuper, const int* rownnzT, const int* rowadrT,
const int* colindT, const int* rowsuperT, mjData* d) {
mj_markStack(d);
int* chain = mj_stackAllocInt(d, 2 * nc);
mjtNum* buffer = mj_stackAllocNum(d, nc);
for (int r = 0; r < nc; r++) {
res_rowadr[r] = r * nc;
}
for (int r = 0; r < nc; r++) {
if (rowsuperT && r > 0 && rowsuperT[r - 1] > 0) {
res_rownnz[r] = res_rownnz[r - 1];
memcpy(res_colind + res_rowadr[r], res_colind + res_rowadr[r - 1],
res_rownnz[r] * sizeof(int));
if (rownnzT[r]) {
res_colind[res_rowadr[r] + res_rownnz[r]] = r;
res_rownnz[r]++;
}
} else {
int nchain = 0;
int inew = 0, iold = nc;
int lastadded = -1;
for (int i = 0; i < rownnzT[r]; i++) {
int c = colindT[rowadrT[r] + i];
if (rowsuper && lastadded >= 0 &&
(c - lastadded) <= rowsuper[lastadded]) {
continue;
} else {
lastadded = c;
}
int adr = inew;
inew = iold;
iold = adr;
int nnewchain = 0;
adr = 0;
int end = rowadr[c] + rownnz[c];
for (int adr1 = rowadr[c]; adr1 < end; adr1++) {
int col_mat = colind[adr1];
while (adr < nchain && chain[iold + adr] < col_mat &&
chain[iold + adr] <= r) {
chain[inew + nnewchain++] = chain[iold + adr++];
}
if (col_mat > r) {
break;
}
if (adr < nchain && chain[iold + adr] == col_mat) {
adr++;
}
chain[inew + nnewchain++] = col_mat;
}
while (adr < nchain && chain[iold + adr] <= r) {
chain[inew + nnewchain++] = chain[iold + adr++];
}
nchain = nnewchain;
}
res_rownnz[r] = nchain;
if (nchain) {
memcpy(res_colind + res_rowadr[r], chain + inew, nchain * sizeof(int));
}
}
}
for (int r = 0; r < nc; r++) {
int adr = res_rowadr[r];
for (int i = 0; i < res_rownnz[r]; i++) {
buffer[res_colind[adr + i]] = 0;
}
for (int i = 0; i < rownnzT[r]; i++) {
int c = colindT[rowadrT[r] + i];
mjtNum matTrc = matT[rowadrT[r] + i];
if (diag) {
matTrc *= diag[c];
}
int end = rowadr[c] + rownnz[c];
for (int adr2 = rowadr[c]; adr2 < end; adr2++) {
int adr1;
if ((adr1 = colind[adr2]) > r) {
break;
}
buffer[adr1] += matTrc * mat[adr2];
}
}
adr = res_rowadr[r];
for (int i = 0; i < res_rownnz[r]; i++) {
res[adr + i] = buffer[res_colind[adr + i]];
}
}
for (int r = 1; r < nc; r++) {
int end = res_rowadr[r] + res_rownnz[r] - 1;
for (int adr = res_rowadr[r]; adr < end; adr++) {
int adr1 = res_rowadr[res_colind[adr]] + res_rownnz[res_colind[adr]]++;
res[adr1] = res[adr];
res_colind[adr1] = r;
}
}
mj_freeStack(d);
}
// ========================== SqrMatTD Benchmarks ==============================
enum class SqrMatTDVariant {
kBaseline,
kRow,
kCol,
kSplitCol
};
template <Size S>
static void BM_sqrMatTD_impl(benchmark::State& state, SqrMatTDVariant variant) {
SparseTestData& data = GetData<S>();
mjModel* m = GetModel<S>();
mjData* d = mj_makeData(m);
int nv = data.nv;
// nothing to benchmark if no constraints
if (data.nefc == 0) {
for (auto s : state) {}
mj_deleteData(d);
return;
}
// allocate H output (uncompressed for baseline, compressed for others)
int max_nnz = (variant == SqrMatTDVariant::kBaseline) ? nv * nv : 0;
std::vector<mjtNum> H;
std::vector<int> H_rownnz(nv);
std::vector<int> H_rowadr(nv);
std::vector<int> H_colind;
std::vector<int> diagind(nv);
if (variant == SqrMatTDVariant::kBaseline) {
H.resize(max_nnz);
H_colind.resize(max_nnz);
} else if (variant == SqrMatTDVariant::kSplitCol ||
variant == SqrMatTDVariant::kCol) {
// use symbolic to count nnz
int nH = mju_sqrMatTDSparseSymbolic(
H_rownnz.data(), H_rowadr.data(), nullptr, nullptr,
data.nefc, nv, data.J_rownnz.data(), data.J_rowadr.data(),
data.J_colind.data(), data.JT_rownnz.data(), data.JT_rowadr.data(),
data.JT_colind.data(), data.JT_rowsuper.data(), d);
H.resize(nH);
H_colind.resize(nH);
} else {
// row: use Count (lower triangle only)
mju_sqrMatTDSparseCount(
H_rownnz.data(), H_rowadr.data(), nv, data.J_rownnz.data(),
data.J_rowadr.data(), data.J_colind.data(), data.JT_rownnz.data(),
data.JT_rowadr.data(), data.JT_colind.data(), nullptr, d, 0);
int nH = H_rowadr[nv - 1] + H_rownnz[nv - 1];
H.resize(nH);
H_colind.resize(nH);
}
for (auto s : state) {
switch (variant) {
case SqrMatTDVariant::kBaseline:
mju_superSparse(data.nefc, data.J_rowsuper.data(), data.J_rownnz.data(),
data.J_rowadr.data(), data.J_colind.data());
mju_sqrMatTDSparse_baseline(
H.data(), data.J.data(), data.JT.data(), data.D.data(), data.nefc,
nv, H_rownnz.data(), H_rowadr.data(), H_colind.data(),
data.J_rownnz.data(), data.J_rowadr.data(), data.J_colind.data(),
data.J_rowsuper.data(), data.JT_rownnz.data(),
data.JT_rowadr.data(), data.JT_colind.data(),
data.JT_rowsuper.data(), d);
break;
case SqrMatTDVariant::kRow:
mju_sqrMatTDSparseCount(
H_rownnz.data(), H_rowadr.data(), nv, data.J_rownnz.data(),
data.J_rowadr.data(), data.J_colind.data(), data.JT_rownnz.data(),
data.JT_rowadr.data(), data.JT_colind.data(), nullptr, d, 0);
mju_sqrMatTDSparse_row(
H.data(), data.J.data(), data.JT.data(), data.D.data(), data.nefc,
nv, H_rownnz.data(), H_rowadr.data(), H_colind.data(),
data.J_rownnz.data(), data.J_rowadr.data(), data.J_colind.data(),
nullptr, data.JT_rownnz.data(), data.JT_rowadr.data(),
data.JT_colind.data(), data.JT_rowsuper.data(), d, nullptr);
break;
case SqrMatTDVariant::kCol:
mju_sqrMatTDSparseCount(
H_rownnz.data(), H_rowadr.data(), nv, data.J_rownnz.data(),
data.J_rowadr.data(), data.J_colind.data(), data.JT_rownnz.data(),
data.JT_rowadr.data(), data.JT_colind.data(), nullptr, d, 0);
mju_sqrMatTDSparse(
H.data(), data.J.data(), data.JT.data(), data.D.data(), data.nefc,
nv, H_rownnz.data(), H_rowadr.data(), H_colind.data(),
data.J_rownnz.data(), data.J_rowadr.data(), data.J_colind.data(),
nullptr, data.JT_rownnz.data(), data.JT_rowadr.data(),
data.JT_colind.data(), data.JT_rowsuper.data(), d, nullptr);
break;
case SqrMatTDVariant::kSplitCol:
mju_sqrMatTDSparseSymbolic(
H_rownnz.data(), H_rowadr.data(), nullptr, nullptr,
data.nefc, nv, data.J_rownnz.data(), data.J_rowadr.data(),
data.J_colind.data(), data.JT_rownnz.data(), data.JT_rowadr.data(),
data.JT_colind.data(), data.JT_rowsuper.data(), d);
mju_sqrMatTDSparseSymbolic(
H_rownnz.data(), H_rowadr.data(), H_colind.data(), nullptr,
data.nefc, nv, data.J_rownnz.data(), data.J_rowadr.data(),
data.J_colind.data(), data.JT_rownnz.data(), data.JT_rowadr.data(),
data.JT_colind.data(), data.JT_rowsuper.data(), d);
mju_sqrMatTDSparseNumeric(
H.data(), nv, H_rownnz.data(), H_rowadr.data(),
H_colind.data(), nullptr, data.J.data(), data.J_rownnz.data(),
data.J_rowadr.data(), data.J_colind.data(), data.JT.data(),
data.JT_rownnz.data(), data.JT_rowadr.data(), data.JT_colind.data(),
data.JT_rowsuper.data(), data.D.data(), d);
break;
}
}
mj_deleteData(d);
state.SetItemsProcessed(state.iterations());
}
void BM_sqrMatTD_2H100_baseline(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H2_100>(state, SqrMatTDVariant::kBaseline);
}
BENCHMARK(BM_sqrMatTD_2H100_baseline);
void BM_sqrMatTD_2H100_row(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H2_100>(state, SqrMatTDVariant::kRow);
}
BENCHMARK(BM_sqrMatTD_2H100_row);
void BM_sqrMatTD_2H100_col(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H2_100>(state, SqrMatTDVariant::kCol);
}
BENCHMARK(BM_sqrMatTD_2H100_col);
void BM_sqrMatTD_2H100_splitCol(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H2_100>(state, SqrMatTDVariant::kSplitCol);
}
BENCHMARK(BM_sqrMatTD_2H100_splitCol);
void BM_sqrMatTD_100H_baseline(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H100>(state, SqrMatTDVariant::kBaseline);
}
BENCHMARK(BM_sqrMatTD_100H_baseline);
void BM_sqrMatTD_100H_row(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H100>(state, SqrMatTDVariant::kRow);
}
BENCHMARK(BM_sqrMatTD_100H_row);
void BM_sqrMatTD_100H_col(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H100>(state, SqrMatTDVariant::kCol);
}
BENCHMARK(BM_sqrMatTD_100H_col);
void BM_sqrMatTD_100H_splitCol(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_sqrMatTD_impl<Size::H100>(state, SqrMatTDVariant::kSplitCol);
}
BENCHMARK(BM_sqrMatTD_100H_splitCol);
} // namespace
} // namespace mujoco
int main(int argc, char** argv) {
benchmark::Initialize(&argc, argv);
benchmark::RunSpecifiedBenchmarks();
return 0;
}
test/engine/engine_collision_convex_test.cc
+1 -1
View File
@@ -68,7 +68,7 @@ TEST_F(MjcConvexTest, CylinderBox) {
EXPECT_EQ(data->ncon, 5);
// with multiCCD disabled, should find 1 contact
model->opt.enableflags &= ~mjENBL_MULTICCD;
model->opt.disableflags |= mjDSBL_MULTICCD;
mj_forward(model, data);
EXPECT_EQ(data->ncon, 1);
test/engine/engine_collision_driver_test.cc
+68
View File
@@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) {
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, MaxContact) {
constexpr char xml[] = R"(
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<asset>
<mesh name="smallbox"
vertex="-1 -1 -1 1 -1 -1 1 1 -1
1 1 1 1 -1 1 -1 1 -1
-1 1 1 -1 -1 1"/>
</asset>
<worldbody>
<geom name="mesh" type="mesh" mesh="smallbox"/>
<geom name="box" type="box" size="1 1 1"/>
<geom name="plane" type="plane" size="1 1 1"/>
<geom name="sphere" type="sphere" size="1"/>
<geom name="capsule" type="capsule" size="1 1"/>
<geom name="ellipsoid" type="ellipsoid" size="1 1 1"/>
<geom name="cylinder" type="cylinder" size="1 1"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh");
int box = mj_name2id(m, mjOBJ_GEOM, "box");
int plane = mj_name2id(m, mjOBJ_GEOM, "plane");
int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere");
int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule");
int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid");
int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder");
EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4);
EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3);
EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4);
EXPECT_EQ(mj_maxContact(m, box, box, -1), 8);
EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2);
EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4);
EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2);
EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5);
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/engine/engine_collision_gjk_test.cc
+7 -13
View File
@@ -61,14 +61,6 @@ constexpr char kEllipsoidXml[] = R"(
</keyframe>
</mujoco>)";
void* CCDAllocate(void* data, std::size_t nbytes) {
return new std::byte[nbytes];
}
void CCDFree(void* data, void* buffer) {
delete [] (std::byte*)buffer;
}
mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) {
mjCCDConfig config;
@@ -79,6 +71,7 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
config.tolerance = kTolerance,
config.max_contacts = 0; // no geom contacts needed
config.dist_cutoff = cutoff;
config.buffer = nullptr;
mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, m, d, g1, 0);
@@ -129,14 +122,13 @@ int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector<mjtNum>& dir,
mjCCDConfig config;
// set config
auto buffer = std::vector<std::byte>(mjc_ccdSize(kMaxIterations));
config.max_iterations = kMaxIterations;
config.tolerance = kTolerance;
config.max_contacts = max_contacts;
config.dist_cutoff = 0; // no geom distances needed
config.max_contacts = max_contacts;
config.context = nullptr;
config.alloc = CCDAllocate;
config.free = CCDFree;
config.buffer = buffer.data();
mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
if (dist < 0) {
@@ -2006,7 +1998,7 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
<mujoco>
<statistic meansize="0.15"/>
<option>
<flag gravity="disable"/>
<flag gravity="disable" multiccd="disable"/>
</option>
<worldbody>
@@ -2027,8 +2019,10 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
mjData* data = mj_makeData(model);
mj_forward(model, data);
// margin=0.1 means forces generated when dist<0.1
// the contact at dist~0.015 is within margin, so forces are generated
EXPECT_EQ(data->ncon, 1);
EXPECT_LT(data->contact[0].efc_address, 0);
EXPECT_GE(data->contact[0].efc_address, 0);
mj_deleteData(data);
mj_deleteModel(model);
test/engine/engine_core_constraint_test.cc
+434 -130
View File
@@ -15,8 +15,6 @@
// Tests for engine/engine_core_constraint.c.
#include <array>
#include <cstddef>
#include <cstring>
#include <string>
#include <vector>
@@ -35,136 +33,9 @@ namespace {
using ::testing::NotNull;
using ::testing::Pointwise;
using CoreConstraintTest = MujocoTest;
// compute rotation residual following formula in mj_instantiateEquality
void RotationResidual(const mjModel *model, mjData *data,
const mjtNum qpos[7], const mjtNum dqpos[6],
mjtNum res[3]) {
// copy configuration, compute required quantities with mj_step1
mju_copy(data->qpos, qpos, 7);
// perturb configuration if given
if (dqpos) {
mj_integratePos(model, data->qpos, dqpos, 1);
}
// update relevant quantities
mj_step1(model, data);
// compute orientation residual
mjtNum quat1[4], quat2[4], quat3[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(quat2, data->xquat+4*2);
mju_mulQuat(quat3, quat2, quat1);
mju_copy3(res, quat3+1);
}
// validate rotational Jacobian used in welds
TEST_F(CoreConstraintTest, WeldRotJacobian) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "FD Jacobian with eps=1e-6 below float32 precision";
#endif
constexpr char xml[] = R"(
<mujoco>
<option jacobian="dense"/>
<worldbody>
<body>
<joint type="ball"/>
<geom size=".1"/>
</body>
<body pos=".5 0 0">
<joint axis="1 0 0" pos="0 0 .01"/>
<joint axis="0 1 0" pos=".02 0 0"/>
<joint axis="0 0 1" pos="0 .03 0"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
ASSERT_EQ(model->nq, 7);
ASSERT_EQ(model->nv, 6);
static const int nv = 6; // for increased readability
mjData* data = mj_makeData(model);
// arbitrary initial values for the ball and hinge joints
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
// compute required quantities using mj_step1
mj_step1(model, data);
// get orientation error
mjtNum res[3];
RotationResidual(model, data, qpos0, NULL, res);
// compute Jacobian with finite-differencing
mjtNum jacFD[3*nv];
mjtNum dqpos[nv] = {0};
mjtNum dres[3];
const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
// nudge i-th dof
dqpos[i] = eps;
// get nudged residual
RotationResidual(model, data, qpos0, dqpos, dres);
// remove nudge
dqpos[i] = 0.0;
// compute Jacobian column
for (int j=0; j < 3; j++) {
jacFD[nv*j + i] = (dres[j] - res[j]) / eps;
}
}
// reset mjData to qpos0
mju_copy(data->qpos, qpos0, 7);
mj_step1(model, data);
// intermediate quaternions quat1 and quat2
mjtNum quat1[4], negQuat2[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(negQuat2, data->xquat+4*2);
// get analytical Jacobian following formula in mj_instantiateEquality
mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv];
mjtNum point[3] = {0};
// rotational Jacobian difference
mj_jacDifPair(model, data, NULL, 2, 1, point, point,
NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model),
/*flg_skipcommon=*/0);
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
mjtNum axis[3], quat3[4], quat4[4];
for (int j=0; j < nv; j++) {
// axis = [jac1-jac2]_col(j)
axis[0] = jacdif[0*nv+j];
axis[1] = jacdif[1*nv+j];
axis[2] = jacdif[2*nv+j];
// apply formula
mju_mulQuatAxis(quat3, negQuat2, axis);
mju_mulQuat(quat4, quat3, quat1);
// correct Jacobian
jacdif[0*nv+j] = 0.5*quat4[1];
jacdif[1*nv+j] = 0.5*quat4[2];
jacdif[2*nv+j] = 0.5*quat4[3];
}
// test that analytical and finite-differenced Jacobians match
EXPECT_THAT(AsVector(jacFD, 3*nv),
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv)));
mj_deleteData(data);
mj_deleteModel(model);
}
// test formulas for penetration at rest
TEST_F(CoreConstraintTest, RestPenetration) {
constexpr char xml[] = R"(
@@ -749,6 +620,191 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
mj_deleteModel(m);
}
// Test flex strain constraint with quadratic interpolation
TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" jacobian="dense"/>
<worldbody>
<body name="parent">
<joint type="free"/>
<geom type="box" size=".01 .01 .01" mass=".1"/>
<flexcomp name="test" type="box"
spacing=".1 .1 .1" radius="0.001"
pos="0 0 .5" dof="quadratic" mass="1" dim="3">
<contact selfcollide="none"/>
<edge equality="strain"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
mj_resetData(m, d);
mj_forward(m, d);
// Check constraints generated
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
// Check that initial strain is ~0
mjtNum max_pos = 0;
for (int i = 0; i < d->ne; i++) {
if (mju_abs(d->efc_pos[i]) > max_pos) {
max_pos = mju_abs(d->efc_pos[i]);
}
}
EXPECT_LT(max_pos, 1e-6) << "Initial strain should be ~0";
// Check Jacobian for NaN
int nv = m->nv;
bool has_bad_jacobian = false;
for (int i = 0; i < d->ne; i++) {
for (int j = 0; j < nv; j++) {
if (mju_isBad(d->efc_J[i*nv + j])) {
has_bad_jacobian = true;
}
}
}
EXPECT_FALSE(has_bad_jacobian) << "Jacobian has NaN";
// Run simulation for a few steps
for (int i = 0; i < 100; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation unstable at step " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
dim="3" cellcount="1 1 1" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="0" poisson="0.1" damping="0.01"
elastic2d="bend" thickness="0.02"/>
<edge equality="strain"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_forward(m, d);
// Check number of equalities
EXPECT_EQ(m->neq, 6);
// Check total number of scalar equality constraints
// 6 faces * 6 physical modes per face = 36
// (2 spurious rigid-rotation modes from transverse shear are projected out)
EXPECT_EQ(d->ne, 36);
// all constraint residuals should be zero at rest
for (int i = 0; i < d->ne; i++) {
EXPECT_NEAR(d->efc_pos[i], 0, 1e-10)
<< "nonzero constraint residual at " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test quadratic passive forces (no constraints) for stability
TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" solver="CG" tolerance="1e-6"/>
<worldbody>
<geom type="plane" size="10 10 1"/>
<flexcomp name="test" type="grid" count="3 3 3"
spacing=".05 .05 .05" radius="0.001"
pos="0 0 .3" dof="quadratic" mass="1" dim="3">
<contact selfcollide="none"/>
<elasticity young="1e4" damping="0.01"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
// Run for 500 steps — should stay stable
for (int i = 0; i < 500; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Passive quadratic unstable at step " << i;
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i;
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test quadratic with anisotropic cells (like what mesh bounding box creates)
TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" solver="CG" tolerance="1e-6"/>
<size memory="50M"/>
<worldbody>
<geom type="plane" size="10 10 1"/>
<body name="parent">
<joint type="free"/>
<geom type="box" size=".01 .01 .01" mass=".1"/>
<flexcomp name="test" type="grid" count="3 3 3"
spacing=".1 .05 .08" radius="0.001"
pos="0 0 .5" dof="quadratic" mass="1" dim="3">
<contact selfcollide="none" internal="false"/>
<edge equality="strain" damping="0.01"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
mj_forward(m, d);
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
// Run for 200 steps with gravity + contact
for (int i = 0; i < 200; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Anisotropic quadratic unstable at step " << i;
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i
<< ", qvel[" << j << "]=" << d->qvel[j];
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
constexpr char xml[] = R"(
<mujoco>
@@ -787,6 +843,254 @@ TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
mj_deleteModel(model);
}
static const char* const kJdotvConnect2dPath =
"engine/testdata/core_constraint/jdotv_connect_2d.xml";
static const char* const kJdotvConnect3dPath =
"engine/testdata/core_constraint/jdotv_connect_3d.xml";
static const char* const kJdotvWeld3dPath =
"engine/testdata/core_constraint/jdotv_weld_3d.xml";
// validate mj_Jdotv against finite-differenced constraint Jacobian
TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
for (const char* path : {kJdotvConnect2dPath,
kJdotvConnect3dPath,
kJdotvWeld3dPath}) {
const std::string xml_path = GetTestDataFilePath(path);
char err[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
ASSERT_THAT(m, NotNull()) << err << " for " << path;
int nv = m->nv;
mjData* d = mj_makeData(m);
// simulate for 1 second to accumulate velocity
while (d->time < 1.0) {
mj_step(m, d);
}
// forward to populate constraints
mj_forward(m, d);
ASSERT_GT(d->ne, 0) << "no equality constraints for " << path;
int ne = d->ne;
// get dense J_0 (ne x nv)
std::vector<mjtNum> J0(ne * nv);
if (mj_isSparse(m)) {
mju_sparse2dense(J0.data(), d->efc_J, ne, nv,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
} else {
mju_copy(J0.data(), d->efc_J, ne * nv);
}
// compute mj_Jdotv at current state
std::vector<mjtNum> jdv(ne, 0);
mj_Jdotv(m, d, jdv.data());
// save qpos and qvel
std::vector<mjtNum> qpos0(m->nq), qvel0(nv);
mju_copy(qpos0.data(), d->qpos, m->nq);
mju_copy(qvel0.data(), d->qvel, nv);
// integrate qpos forward by h using qvel
const mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(m, d->qpos, d->qvel, h);
mj_forward(m, d);
// get dense J_h (ne x nv)
ASSERT_EQ(d->ne, ne) << "constraint count changed after integration";
std::vector<mjtNum> Jh(ne * nv);
if (mj_isSparse(m)) {
mju_sparse2dense(Jh.data(), d->efc_J, ne, nv,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
} else {
mju_copy(Jh.data(), d->efc_J, ne * nv);
}
// FD: Jdotv_fd[i] = -sum_j (Jh[i,j] - J0[i,j]) / h * qvel[j]
// (negated because mj_Jdotv subtracts)
std::vector<mjtNum> jdv_fd(ne, 0);
for (int i = 0; i < ne; i++) {
for (int j = 0; j < nv; j++) {
jdv_fd[i] -= (Jh[i*nv+j] - J0[i*nv+j]) / h * qvel0[j];
}
}
// compare
EXPECT_THAT(AsVector(jdv.data(), ne),
Pointwise(MjNear(1e-4, 1e-2), AsVector(jdv_fd.data(), ne)))
<< "Jdotv FD mismatch for " << path;
mj_deleteData(d);
mj_deleteModel(m);
}
}
// Test 2: forward-inverse identity preserved with Jdot*v correction
TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
for (const char* path : {kJdotvConnect2dPath,
kJdotvConnect3dPath,
kJdotvWeld3dPath}) {
const std::string xml_path = GetTestDataFilePath(path);
char err[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
ASSERT_THAT(m, NotNull()) << err;
mjData* d = mj_makeData(m);
// give initial velocity
for (int i = 0; i < m->nv; i++) d->qvel[i] = 0.5 * (i + 1);
// forward (with correction ON by default)
mj_forward(m, d);
mj_compareFwdInv(m, d);
mjtNum fwdinv = d->solver_fwdinv[0];
mjtNum epsilon = MjTol(1e-10, 1e-2);
EXPECT_LT(fwdinv, epsilon)
<< "fwdinv broken for " << path
<< " (fwdinv=" << fwdinv << ")";
mj_deleteData(d);
mj_deleteModel(m);
}
}
// --------------------------- strain constraint rotated parent ----------------
struct StrainConstraintTestCase {
std::string test_name;
std::string body_pos;
std::string body_quat;
std::string flex_spacing;
std::string flex_xyaxes;
};
class StrainConstraintRotatedTest : public CoreConstraintTest,
public ::testing::WithParamInterface<
StrainConstraintTestCase> {
};
TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
auto param = GetParam();
std::string xml = R"(
<mujoco>
<option integrator="implicitfast" jacobian="dense" gravity="0 0 0"/>
<worldbody>
<body name="parent" )";
if (!param.body_pos.empty()) {
xml += "pos=\"" + param.body_pos + "\" ";
}
if (!param.body_quat.empty()) {
xml += "quat=\"" + param.body_quat + "\" ";
}
xml += R"(>
<joint type="free"/>
<geom type="box" size=".01 .01 .01" mass=".1"/>
<flexcomp name="test" type="box" )";
if (!param.flex_spacing.empty()) {
xml += "spacing=\"" + param.flex_spacing + "\" ";
}
if (!param.flex_xyaxes.empty()) {
xml += "xyaxes=\"" + param.flex_xyaxes + "\" ";
}
xml += R"(radius="0.001"
pos="0 0 0" dof="trilinear" mass="1" dim="3">
<contact selfcollide="none"/>
<edge equality="strain"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
mj_forward(m, d);
// Check we have strain constraints
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
// The critical check: constraint residuals must be ~0 at the initial
// (undeformed) configuration, even though the body is rotated.
mjtNum max_pos = 0;
for (int i = 0; i < d->ne; i++) {
max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
}
EXPECT_LT(max_pos, 1e-6)
<< "Strain constraint residual should be ~0"
<< " (max_pos=" << max_pos << ")";
// Verify stability
for (int i = 0; i < 200; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation unstable at step " << i;
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i
<< ", qvel[" << j << "]=" << d->qvel[j];
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
INSTANTIATE_TEST_SUITE_P(
StrainConstraintRotatedTests, StrainConstraintRotatedTest,
testing::ValuesIn<StrainConstraintTestCase>({
// Test strain constraint with a rotated parent body.
// The flexcomp is placed inside a parent body that has a non-identity
// initial rotation. This reproduces the "grocery scene" bug where the
// stiffness matrix eigenvectors and reference positions were computed
// in world frame instead of the unrotated local frame, causing
// spurious constraint forces.
{
"RotatedParent",
"1 2 3",
"0.707107 0 0.707107 0",
".1 .1 .1",
""
},
// Same test with an anisotropic box (different spacing per axis) and
// arbitrary rotation (combined 45-deg Y + 30-deg X).
{
"RotatedParentAnisotropic",
"0.5 -1 2",
"0.8924 0.2392 0.3696 -0.0990",
".15 .08 .05",
""
},
// Test strain constraint with flexcomp-level xyaxes rotation.
// This is the "grocery scene" pattern where the flexcomp grid itself is
// rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z).
{
"FlexcompXyaxes",
"",
"",
".1 .02 .1",
"0 1 0 0 0 1"
},
// Test combining parent body rotation with flexcomp xyaxes rotation.
// The total rotation is the composition of both.
{
"RotatedParentPlusXyaxes",
"1 2 3",
"0.707107 0 0.707107 0",
".15 .08 .05",
"0 1 0 0 0 1"
}
}),
[](const testing::TestParamInfo<
StrainConstraintRotatedTest::ParamType>& info) {
return info.param.test_name;
}
);
} // namespace
} // namespace mujoco
test/engine/engine_core_smooth_test.cc
+5 -4
View File
@@ -191,10 +191,8 @@ TEST_F(CoreSmoothTest, TendonJdot) {
mj_forward(m, d);
// get current J and Jdot for the tendon
// get current J for the tendon
vector<mjtNum> ten_J(d->ten_J, d->ten_J + nv);
vector<mjtNum> ten_Jdot(nv, 0);
mj_tendonDot(m, d, 0, ten_Jdot.data());
// compute finite-differenced Jdot
mjtNum h = MjTol(1e-7, 5e-4);
@@ -206,7 +204,10 @@ TEST_F(CoreSmoothTest, TendonJdot) {
mju_subFrom(ten_Jh.data(), ten_J.data(), nv);
mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv);
EXPECT_THAT(ten_Jdot, Pointwise(MjNear(1e-6, 2e-3), ten_Jh));
// test dot product against finite differences
mjtNum dot = mj_tendonDot(m, d, 0, d->qvel);
mjtNum expected_dot = mju_dot(ten_Jh.data(), d->qvel, nv);
EXPECT_NEAR(dot, expected_dot, MjTol(1e-5, 2e-3));
}
mj_deleteData(d);
test/engine/engine_core_util_test.cc
+740
View File
@@ -0,0 +1,740 @@
// Copyright 2026 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Tests for engine/engine_core_util.c.
#include "src/engine/engine_core_util.h"
#include <algorithm>
#include <cstddef>
#include <limits>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::NotNull;
using ::testing::Pointwise;
using FlexGatherStateTest = MujocoTest;
TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="flex0" type="grid" count="3 3 3" spacing=".1 .1 .1"
dim="3" mass="1" radius="0.01" dof="trilinear">
<elasticity young="5e4" poisson="0.2"/>
<contact selfcollide="none"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_forward(model, data);
ASSERT_EQ(model->nflex, 1);
int f = 0;
int nodenum = model->flex_nodenum[f];
int nstart = model->flex_nodeadr[f];
// Simulate a rotated state (90 degrees around Z axis)
ASSERT_TRUE(model->flex_centered[f]);
for (int i = 0; i < nodenum; i++) {
int b = model->flex_nodebodyid[nstart + i];
mjtNum x = data->xpos[3*b + 0];
mjtNum y = data->xpos[3*b + 1];
mjtNum z = data->xpos[3*b + 2];
// Rotate 90 degrees around Z: (x, y, z) -> (-y, x, z)
data->xpos[3*b + 0] = -y;
data->xpos[3*b + 1] = x;
data->xpos[3*b + 2] = z;
}
std::vector<mjtNum> xpos(3 * nodenum);
mju_flexGatherState(model, data, f, xpos.data(), NULL);
// Verify that gathered xpos matches the rotated data->xpos
for (int i = 0; i < nodenum; i++) {
int b = model->flex_nodebodyid[nstart + i];
EXPECT_NEAR(xpos[3*i + 0], data->xpos[3*b + 0], 1e-5);
EXPECT_NEAR(xpos[3*i + 1], data->xpos[3*b + 1], 1e-5);
EXPECT_NEAR(xpos[3*i + 2], data->xpos[3*b + 2], 1e-5);
}
mj_deleteData(data);
mj_deleteModel(model);
}
using AngMomMatTest = MujocoTest;
static constexpr char AngMomTestingModel[] = R"(
<mujoco>
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<body name="link1" pos="0 0 0.5">
<freejoint/>
<geom type="ellipsoid" size="0.15 0.17 0.19" quat="1 .2 .3 .4"/>
<body name="link2" >
<joint type="hinge" axis="1 0 0" />
<geom type="capsule" size="0.05" fromto="0 0 0 0 0.5 0"/>
<body pos="0 0.6 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.05 0.2" quat="0.707 0 0.707 0"/>
<body name="link3">
<joint type="ball" pos="0.2 0 0"/>
<geom type="capsule" pos="0.2 0 0" size="0.03 0.4"/>
</body>
</body>
</body>
</body>
</worldbody>
<keyframe>
<key qvel="0 0 0 .1 .2 .3 .4 .5 .4 .3 .2"/>
</keyframe>
</mujoco>
)";
// compare subtree angular momentum computed in two ways
TEST_F(AngMomMatTest, CompareAngMom) {
char error[1024];
mjModel* model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
mjData* data = mj_makeData(model);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
// get the reference value of angular momentum
mj_subtreeVel(model, data);
mjtNum angmom_ref[3];
mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid);
// compute angular momentum using the angular momentum matrix
mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mj_angmomMat(model, data, angmom_mat, bodyid);
mjtNum angmom_test[3];
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
// compare the two angular momentum values
for (int i = 0; i < 3; i++) {
EXPECT_THAT(angmom_ref[i], MjNear(angmom_test[i], 1e-8, 1e-4));
}
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
// compare subtree angular momentum matrix: analytical and findiff
TEST_F(AngMomMatTest, CompareAngMomMats) {
char error[1024];
mjModel* model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
mjData* data = mj_makeData(model);
mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mjtNum* angmom_mat_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
// compute the angular momentum matrix using the analytical method
mj_angmomMat(model, data, angmom_mat, bodyid);
// compute the angular momentum matrix using finite differences
static constexpr mjtNum eps = MjTol(1e-6, 1e-3);
for (int i = 0; i < nv; i++) {
// reset vel, forward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] += eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mjtNum agmf[3];
mju_copy3(agmf, data->subtree_angmom+3*bodyid);
// reset vel, backward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] -= eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mjtNum agmb[3];
mju_copy3(agmb, data->subtree_angmom+3*bodyid);
// finite-difference the angmom matrix
for (int j = 0; j < 3; j++) {
angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps);
}
}
// compare the two matrices
for (int i = 0; i < 3*nv; i++) {
EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4));
}
mju_free(angmom_mat_fd);
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
using JacobianTest = MujocoTest;
static const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
static constexpr char kJacobianTestingModel[] = R"(
<mujoco>
<worldbody>
<body name="distractor1" pos="0 0 .3">
<freejoint/>
<geom size=".1"/>
</body>
<body name="main">
<freejoint/>
<geom size=".1"/>
<body pos=".1 0 0">
<joint axis="0 1 0"/>
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
</body>
<body pos="0 .1 0">
<joint type="ball"/>
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
<body pos="0 .2 0">
<joint type="slide" axis="1 1 1"/>
<geom size=".05"/>
</body>
</body>
</body>
<body name="distractor2" pos="0 0 -.3">
<freejoint/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
// compare analytic and finite-differenced subtree-com Jacobian
TEST_F(JacobianTest, SubtreeJac) {
char error[1024];
mjModel* model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
mjData* data = mj_makeData(model);
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq);
mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv);
// all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data);
mj_comPos(model, data);
// get subtree CoM Jacobian of free body
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
// save current subtree-com and qpos, clear nudge
mjtNum subtree_com[3];
mju_copy3(subtree_com, data->subtree_com+3*bodyid);
mju_copy(qpos, data->qpos, model->nq);
mju_zero(nudge, nv);
// compare analytic Jacobian to finite-difference approximation
static const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
// reset qpos, nudge i-th dof, update data->qpos, reset nudge
mju_copy(data->qpos, qpos, model->nq);
nudge[i] = 1;
mj_integratePos(model, data->qpos, nudge, eps);
nudge[i] = 0;
// kinematics and comPos to get nudged com
mj_kinematics(model, data);
mj_comPos(model, data);
// compare finite-differenced and analytic Jacobian
for (int j=0; j < 3; j++) {
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps;
EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2));
}
}
mju_free(nudge);
mju_free(qpos);
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
// confirm that applying linear forces via the subtree-com Jacobian only creates
// the expected linear accelerations (no accelerations of internal joints)
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
char error[1024];
mjModel* model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
mjData* data = mj_makeData(model);
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
// all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data);
mj_comPos(model, data);
// get subtree CoM Jacobian of free body
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
// uncomment for debugging
// mju_printMat(jac_subtree, 3, nv);
// call fwdPosition since we'll need the factorised mass matrix in the test
mj_fwdPosition(model, data);
// treating the subtree Jacobian as the projection of 3 axis-aligned unit
// forces into joint space, solve for the resulting accelerations in-place
mj_solveM(model, data, jac_subtree, jac_subtree, 3);
// expect to find accelerations of magnitude 1/subtreemass in the first 3
// coordinates of the free joint and 0s elsewhere, since applying forces to
// the CoM should accelerate the whole mechanism without any internal motion
int body_dofadr = model->body_dofadr[bodyid];
mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid];
for (int r = 0; r < 3; r++) {
for (int c = 0; c < nv; c++) {
mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0;
EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4));
}
}
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char kQuat[] = R"(
<mujoco>
<worldbody>
<body name="query">
<joint type="ball"/>
<geom size="1"/>
<site name="query" pos=".1 .2 .3"/>
</body>
</worldbody>
<keyframe>
<key qvel="2 3 5"/>
</keyframe>
</mujoco>
)";
static constexpr char kFreeBall[] = R"(
<mujoco>
<worldbody>
<body name="distractor1" pos="0 0 .3">
<freejoint/>
<geom size=".1"/>
</body>
<body name="main">
<freejoint/>
<geom size=".1"/>
<body pos=".1 0 0">
<joint axis="0 1 0"/>
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
<body pos=".2 0 0">
<joint type="ball" stiffness="20"/>
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
<body name="query" pos="0 .2 0">
<joint type="slide" axis="1 1 1"/>
<geom size=".05"/>
<site name="query" pos=".1 .2 .3"/>
</body>
</body>
</body>
</body>
<body name="distractor2" pos="0 0 -.3">
<freejoint/>
<geom size=".1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"/>
</keyframe>
</mujoco>
)";
static constexpr char kQuatlessPendulum[] = R"(
<mujoco>
<option integrator="implicit">
<flag constraint="disable"/>
</option>
<worldbody>
<body pos="0.15 0 0">
<joint type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<body pos="0.1 0 0">
<joint type="slide" axis="1 0 0" stiffness="200"/>
<geom type="capsule" size="0.015" fromto="-.1 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint axis="1 0 0"/>
<joint axis="0 1 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".02" fromto="0 0 0 0 .1 0"/>
<body name="query" pos="0 .1 0">
<joint axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 0 .1 0"/>
<site name="query" pos=".1 0 0"/>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>
)";
static constexpr char kTelescope[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="ball"/>
<geom type="capsule" size="0.02" fromto="0 .02 0 .1 .02 0"/>
<body pos=".1 .02 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<body pos=".1 .02 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<body pos=".1 .02 0" name="query">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<site name="query" pos=".1 0 0"/>
</body>
</body>
</body>
</body>
</worldbody>
<keyframe>
<key qvel="1 1 1 1 1 1"/>
</keyframe>
</mujoco>
)";
static constexpr char kHinge[] = R"(
<mujoco>
<worldbody>
<body name="query">
<joint name="link1" axis="0 1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
<site name="query" pos="0 0 -1"/>
</body>
</worldbody>
<keyframe>
<key qpos="1" qvel="1"/>
</keyframe>
</mujoco>
)";
// compare mj_jacDot with finite-differenced mj_jac
TEST_F(JacobianTest, JacDot) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
mjData* data = mj_makeData(model);
// load keyframe if present, step for a bit
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
while (data->time < 0.1) {
mj_step(model, data);
}
// minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data);
mj_comPos(model, data);
mj_comVel(model, data);
// get bodyid
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
EXPECT_GT(bodyid, 0);
// get site position
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
EXPECT_GT(siteid, -1);
mjtNum point[3];
mju_copy3(point, data->site_xpos+3*siteid);
// jac, jac_dot
std::vector<mjtNum> jacp(3*nv);
std::vector<mjtNum> jacr(3*nv);
mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid);
std::vector<mjtNum> jacp_dot(3*nv);
std::vector<mjtNum> jacr_dot(3*nv);
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
// jac_h: jacobian after integrating qpos with a timestep of h
constexpr mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(model, data->qpos, data->qvel, h);
mj_kinematics(model, data);
mj_comPos(model, data);
std::vector<mjtNum> jacp_h(3*nv);
std::vector<mjtNum> jacr_h(3*nv);
mju_copy3(point, data->site_xpos+3*siteid); // get updated site position
mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid);
// jac_dot_h finite-difference approximation
std::vector<mjtNum> jacp_dot_h(3*nv);
mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv);
mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv);
std::vector<mjtNum> jacr_dot_h(3*nv);
mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv);
mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv);
// compare finite-differenced and analytic
mjtNum tol = 1e-5;
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
mj_deleteData(data);
mj_deleteModel(model);
}
}
// compare mj_jacDotSparse with dense mj_jacDot
TEST_F(JacobianTest, JacDotSparse) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
mjData* data = mj_makeData(model);
// load keyframe if present, step for a bit
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
while (data->time < 0.1) {
mj_step(model, data);
}
// minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data);
mj_comPos(model, data);
mj_comVel(model, data);
// get bodyid and site position
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
EXPECT_GT(bodyid, 0);
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
EXPECT_GT(siteid, -1);
mjtNum point[3];
mju_copy3(point, data->site_xpos+3*siteid);
// dense jacDot
std::vector<mjtNum> jacp_dense(3*nv);
std::vector<mjtNum> jacr_dense(3*nv);
mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid);
// compute body chain using public mjModel fields
std::vector<int> chain(nv);
int NV = 0;
int weldbody = model->body_weldid[bodyid];
if (weldbody) {
int da = model->body_dofadr[weldbody] + model->body_dofnum[weldbody] - 1;
while (da >= 0) {
chain[NV++] = da;
da = model->dof_parentid[da];
}
std::reverse(chain.begin(), chain.begin() + NV);
}
EXPECT_GT(NV, 0);
// sparse jacDot
std::vector<mjtNum> jacp_sparse(3*NV);
std::vector<mjtNum> jacr_sparse(3*NV);
mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(),
point, bodyid, NV, chain.data());
// expand sparse to dense and compare
std::vector<mjtNum> jacp_expanded(3*nv, 0);
std::vector<mjtNum> jacr_expanded(3*nv, 0);
for (int ci = 0; ci < NV; ci++) {
int di = chain[ci];
for (int r = 0; r < 3; r++) {
jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV];
jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV];
}
}
// expect bitwise equality
EXPECT_EQ(jacp_expanded, jacp_dense);
EXPECT_EQ(jacr_expanded, jacr_dense);
mj_deleteData(data);
mj_deleteModel(model);
}
}
// validate rotational Jacobian used in welds
TEST_F(JacobianTest, WeldRotJacobian) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "FD Jacobian with eps=1e-6 below float32 precision";
#endif
constexpr char xml[] = R"(
<mujoco>
<option jacobian="dense"/>
<worldbody>
<body>
<joint type="ball"/>
<geom size=".1"/>
</body>
<body pos=".5 0 0">
<joint axis="1 0 0" pos="0 0 .01"/>
<joint axis="0 1 0" pos=".02 0 0"/>
<joint axis="0 0 1" pos="0 .03 0"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
ASSERT_EQ(model->nq, 7);
ASSERT_EQ(model->nv, 6);
static const int nv = 6; // for increased readability
mjData* data = mj_makeData(model);
// arbitrary initial values for the ball and hinge joints
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
// compute required quantities using mj_step1
mj_step1(model, data);
// get orientation error
mjtNum res[3];
// compute rotation residual following formula in mj_instantiateEquality
auto RotationResidual = [](const mjModel *model, mjData *data,
const mjtNum qpos[7], const mjtNum dqpos[6],
mjtNum res[3]) {
// copy configuration, compute required quantities with mj_step1
mju_copy(data->qpos, qpos, 7);
// perturb configuration if given
if (dqpos) {
mj_integratePos(model, data->qpos, dqpos, 1);
}
// update relevant quantities
mj_step1(model, data);
// compute orientation residual
mjtNum quat1[4], quat2[4], quat3[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(quat2, data->xquat+4*2);
mju_mulQuat(quat3, quat2, quat1);
mju_copy3(res, quat3+1);
};
RotationResidual(model, data, qpos0, NULL, res);
// compute Jacobian with finite-differencing
mjtNum jacFD[3*nv];
mjtNum dqpos[nv] = {0};
mjtNum dres[3];
const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
// nudge i-th dof
dqpos[i] = eps;
// get nudged residual
RotationResidual(model, data, qpos0, dqpos, dres);
// remove nudge
dqpos[i] = 0.0;
// compute Jacobian column
for (int j=0; j < 3; j++) {
jacFD[nv*j + i] = (dres[j] - res[j]) / eps;
}
}
// reset mjData to qpos0
mju_copy(data->qpos, qpos0, 7);
mj_step1(model, data);
// intermediate quaternions quat1 and quat2
mjtNum quat1[4], negQuat2[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(negQuat2, data->xquat+4*2);
// get analytical Jacobian following formula in mj_instantiateEquality
mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv];
mjtNum point[3] = {0};
// rotational Jacobian difference
mj_jacDifPair(model, data, NULL, 2, 1, point, point,
NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model),
/*flg_skipcommon=*/0);
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
mjtNum axis[3], quat3[4], quat4[4];
for (int j=0; j < nv; j++) {
// axis = [jac1-jac2]_col(j)
axis[0] = jacdif[0*nv+j];
axis[1] = jacdif[1*nv+j];
axis[2] = jacdif[2*nv+j];
// apply formula
mju_mulQuatAxis(quat3, negQuat2, axis);
mju_mulQuat(quat4, quat3, quat1);
// correct Jacobian
jacdif[0*nv+j] = 0.5*quat4[1];
jacdif[1*nv+j] = 0.5*quat4[2];
jacdif[2*nv+j] = 0.5*quat4[3];
}
// test that analytical and finite-differenced Jacobians match
EXPECT_THAT(AsVector(jacFD, 3*nv),
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv)));
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_derivative_test.cc
+164 -25
View File
@@ -91,6 +91,8 @@ static const char* const kDampedPendulumPath =
"engine/testdata/derivative/damped_pendulum.xml";
static const char* const kLinearPath =
"engine/testdata/derivative/linear.xml";
static const char* const kDCMotorPath =
"engine/testdata/derivative/dcmotor.xml";
static const char* const kModelPath = "testdata/model.xml";
// compare analytic and finite-difference d_smooth/d_qvel
@@ -99,9 +101,12 @@ TEST_F(DerivativeTest, SmoothDvel) {
for (const char* local_path : {kEnergyConservingPendulumPath,
kTumblingThinObjectPath,
kDampedActuatorsPath,
kDamperActuatorsPath}) {
kDamperActuatorsPath,
kDCMotorPath}) {
const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
char error[1024] = "";
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
int nD = model->nD;
mjData* data = mj_makeData(model);
@@ -315,17 +320,19 @@ TEST_F(DerivativeTest, PassiveDvel) {
mj_forward(model, data);
// get analytic derivatives
mju_zero(data->qDeriv, model->nD);
mjd_passive_vel(model, data);
mju_copy(qDerivAnalytic, data->qDeriv, nD);
// clear qDeriv, get finite-difference derivatives
mju_zero(data->qDeriv, nD);
mju_zero(qDerivFD, nD);
mjtNum eps = MjTol(1e-6, 1e-3);
mjtNum eps = MjTol(1e-6, 1e-4);
mjd_passive_velFD(model, data, eps);
// expect FD and analytic derivatives to be similar to tol precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(MjNear(1e-4, 1e-3), AsVector(qDerivAnalytic, nD)));
Pointwise(MjNear(1e-6, 1e-4), AsVector(qDerivAnalytic, nD)));
}
mju_free(qDerivFD);
@@ -758,9 +765,12 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
for (const char* local_path : {kEnergyConservingPendulumPath,
kTumblingThinObjectPath,
kDampedActuatorsPath,
kDamperActuatorsPath}) {
kDamperActuatorsPath,
kDCMotorPath}) {
const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
char error[1024] = "";
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
int nD = model->nD;
mjtNum* qDeriv = (mjtNum*) mju_malloc(sizeof(mjtNum)*nD);
mjData* data = mj_makeData(model);
@@ -1300,6 +1310,123 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
mj_deleteModel(m);
}
// verify stateful DC motor derivative matches analytical formula
TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="2.0" resistance="0.5"
inductance="0 0.001" input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// set nonzero velocity and ctrl
d->qvel[0] = 1.0;
d->ctrl[0] = 0.5;
// forward to compute act_dot, etc.
mj_forward(m, d);
// compute analytical derivatives
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// extract diagonal of qDeriv
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
// expected: K*(dVdw - K)*(1 - exp(-h/te))/R
// with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5
mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0;
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
<< "stateful DC motor derivative should match analytical formula";
mj_deleteData(d);
mj_deleteModel(m);
}
// verify that stateful DC motor derivative converges to stateless as te -> 0
TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
// stateless DC motor with position controller
static constexpr char xml_stateless[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
// stateful DC motor with very small te
static constexpr char xml_stateful[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
inductance="0 1e-8" input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
ASSERT_THAT(m_sl, NotNull()) << error;
mjData* d_sl = mj_makeData(m_sl);
mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
ASSERT_THAT(m_sf, NotNull()) << error;
mjData* d_sf = mj_makeData(m_sf);
// set identical state
d_sl->qvel[0] = d_sf->qvel[0] = 1.0;
d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5;
// forward and compute derivatives
mj_forward(m_sl, d_sl);
mj_forward(m_sf, d_sf);
mjd_smooth_vel(m_sl, d_sl, 1);
mjd_smooth_vel(m_sf, d_sf, 1);
// extract diagonals
mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
<< "stateful derivative should converge to stateless as te -> 0";
mj_deleteData(d_sf);
mj_deleteModel(m_sf);
mj_deleteData(d_sl);
mj_deleteModel(m_sl);
}
// Utility: Rotate flex grid
void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
double angle) {
@@ -1346,6 +1473,26 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
}
}
// Helper: assemble flex stiffness into dense matrix via matrix-vector products.
// Builds K column-by-column using mjd_flexInterp_mul.
// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH
// convention where stiffness is subtracted from the system matrix).
static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense,
int nv, mjtNum h) {
std::vector<mjtNum> e_i(nv, 0);
std::vector<mjtNum> col(nv, 0);
for (int i = 0; i < nv; i++) {
mju_zero(e_i.data(), nv);
mju_zero(col.data(), nv);
e_i[i] = 1.0;
mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h);
// col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K)
for (int j = 0; j < nv; j++) {
H_dense[j * nv + i] = -col[j];
}
}
}
// compare analytic and fin-diff d_qfrc_passive/d_qvel for flex interp
// Combined test for verify mjd_flexInterp_mulK (stiffness) and damping
TEST_F(DerivativeTest, FlexInterpDerivatives) {
@@ -1387,18 +1534,16 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
vec[i] = mju_Halton(i, 2) - 0.5;
}
// use addH to compute K * vec
// addH adds (h^2*K + h*D) to H
// if we set h=1, damping=0, we get K added to H
// use mulKD to compute K * vec
// mulKD adds (h^2*K + h*D)*vec to res
// if we set h=1, damping=0, we get K*vec
mjtNum save_damping = model->flex_damping[0];
model->flex_damping[0] = 0;
std::vector<mjtNum> H(nv * nv, 0);
std::vector<int> dof_indices(nv);
for (int i = 0; i < nv; i++) dof_indices[i] = i;
// assemble K into H
mjd_flexInterp_addH(model, data, H.data(), dof_indices.data(), nv, 1.0);
// assemble K into H column-by-column
mulKD_dense(model, data, H.data(), nv, 1.0);
// restore damping
model->flex_damping[0] = save_damping;
@@ -1485,16 +1630,13 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
// check that we have non-zero damping (FD should find it)
EXPECT_GT(mju_norm(qDerivFD.data(), nD), 1e-3);
// compute expected flex damping using mjd_flexInterp_addH
// compute expected flex damping using mulKD_dense
// D = 4*H(0.5) - H(1)
vector<int> dof_indices(nv);
for (int i = 0; i < nv; i++) dof_indices[i] = i;
vector<mjtNum> H1(nv * nv, 0);
mjd_flexInterp_addH(model, data, H1.data(), dof_indices.data(), nv, 1.0);
mulKD_dense(model, data, H1.data(), nv, 1.0);
vector<mjtNum> H2(nv * nv, 0);
mjd_flexInterp_addH(model, data, H2.data(), dof_indices.data(), nv, 0.5);
mulKD_dense(model, data, H2.data(), nv, 0.5);
vector<mjtNum> D(nv * nv);
for (int i = 0; i < nv * nv; i++) {
@@ -1562,14 +1704,11 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
mj_forward(model, data);
// 1. Compute Analytic Jacobian (Approximate)
// We use mjd_flexInterp_addH to get K_approx
// We use mulKD_dense to get K_approx
std::vector<mjtNum> H_approx(nv * nv, 0);
std::vector<int> dof_indices(nv);
for (int i = 0; i < nv; i++) dof_indices[i] = i;
// h=1, damping=0 => adds K to H
mjd_flexInterp_addH(model, data, H_approx.data(), dof_indices.data(), nv,
1.0);
// h=1, damping=0 => gives K
mulKD_dense(model, data, H_approx.data(), nv, 1.0);
// 2. Compute Finite Difference Jacobian (Ground Truth)
// qfrc_passive = -dV/dq
test/engine/engine_forward_test.cc
+1617 -3
View File
File diff suppressed because it is too large Load Diff
test/engine/engine_inverse_test.cc
+8 -9
View File
@@ -73,9 +73,16 @@ TEST_F(InverseTest, DiscreteInverseMatch) {
mjtNum* qvel_next = (mjtNum*)mju_malloc(nv * sizeof(mjtNum));
mjtNum* qacc_fd = (mjtNum*)mju_malloc(nv * sizeof(mjtNum));
for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) {
for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT}) {
model->opt.integrator = integrator;
for (bool invdiscrete : {false, true}) {
// set/unset mjENBL_INVDISCRETE flag (affects both forward and inverse)
if (invdiscrete) {
model->opt.enableflags |= mjENBL_INVDISCRETE;
} else {
model->opt.enableflags &= ~mjENBL_INVDISCRETE;
}
// simulate
mj_resetData(model, data);
for (int i = 0; i < kSteps; ++i) {
@@ -98,17 +105,9 @@ TEST_F(InverseTest, DiscreteInverseMatch) {
mj_forward(model, data);
mju_copy(data->qacc, qacc_fd, nv);
// set/unset mjENBL_INVDISCRETE flag
if (invdiscrete) {
model->opt.enableflags |= mjENBL_INVDISCRETE;
} else {
model->opt.enableflags &= ~mjENBL_INVDISCRETE;
}
// call built-in testing function
mj_compareFwdInv(model, data);
// depending on mjENBL_INVDISCRETE flag, expect mismatch to be small/large
if (invdiscrete) {
mjtNum epsilon = MjTol(1e-9, 0.05);
EXPECT_LT(data->solver_fwdinv[0], epsilon);
test/engine/engine_island_test.cc
+37
View File
@@ -15,6 +15,7 @@
// Tests for engine/engine_island.c.
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -602,5 +603,41 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) {
mj_deleteModel(model);
}
TEST_F(IslandTest, PGSIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// simulate to get a non-trivial state
while (d->time < 0.5) {
mj_step(m, d);
}
// switch to PGS, disable early termination
m->opt.solver = mjSOL_PGS;
m->opt.tolerance = 0;
// solve with islands
m->opt.disableflags &= ~mjDSBL_ISLAND;
mj_forward(m, d);
ASSERT_GT(d->nisland, 1);
std::vector<mjtNum> qfrc_island(d->qfrc_constraint,
d->qfrc_constraint + m->nv);
// solve without islands
m->opt.disableflags |= mjDSBL_ISLAND;
mj_forward(m, d);
std::vector<mjtNum> qfrc_mono(d->qfrc_constraint,
d->qfrc_constraint + m->nv);
// expect close match (inexact due to randomized constraint visitation order)
EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono));
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/engine/engine_passive_test.cc
+182
View File
@@ -847,5 +847,187 @@ TEST_F(PassiveTest, PolynomialDampingTendon) {
mj_deleteModel(m);
}
// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
// at rest (initial configuration); any nonzero force indicates a rotation
// mismatch between compile-time reference positions and runtime corotation.
TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
dim="3" cellcount="1 1 1" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0.01"
elastic2d="stretch" thickness="0.02"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_forward(m, d);
// all spring forces should be zero at rest
for (int i = 0; i < m->nv; i++) {
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
<< "nonzero spring force at DOF " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
// interpolated shell bending must produce zero spring forces at rest
TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
dim="3" cellcount="2 2 1" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0"
elastic2d="bend" thickness="0.02"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
// verify bending data was compiled
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
int nedge = (int)bdata[0];
EXPECT_GT(nedge, 0) << "no bending edges compiled";
mj_forward(m, d);
// all spring forces should be zero at rest
for (int i = 0; i < m->nv; i++) {
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
<< "nonzero spring force at DOF " << i;
}
// verify per-edge bending data
int n_flat = 0, n_corner = 0;
for (int e = 0; e < nedge; e++) {
const mjtNum* edata = bdata + 1 + e * 10;
mjtNum stiffness = edata[6];
mjtNum dn0[3] = {edata[7], edata[8], edata[9]};
mjtNum dn0_norm = mju_norm3(dn0);
// stiffness must be positive
EXPECT_GT(stiffness, 0) << "edge " << e << " has non-positive stiffness";
if (dn0_norm < 1e-10) {
// intra-surface edge: coplanar faces, zero normal jump
n_flat++;
} else {
// corner edge: 90° between perpendicular face normals, |dn0| = sqrt(2)
n_corner++;
EXPECT_NEAR(dn0_norm, mju_sqrt(2.0), 1e-10)
<< "corner edge " << e << " has unexpected |dn0|=" << dn0_norm;
}
}
// for a 2x2x1 box: 12 intra-surface + 20 corner = 32 edges
EXPECT_GT(n_flat, 0) << "no intra-surface edges found";
EXPECT_GT(n_corner, 0) << "no corner edges found";
EXPECT_EQ(n_flat + n_corner, nedge);
mj_deleteData(d);
mj_deleteModel(m);
}
// interpolated shell bending must produce zero forces after a rigid rotation
TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<flexcomp type="grid" count="8 2 12" spacing=".025 .05 .025" pos="0 0 1"
dim="3" cellcount="6 1 6" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="1e5" poisson="0.3" damping="0"
elastic2d="bend" thickness="0.03"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
// compute geometric center from body positions (skip world body)
mjtNum center[3] = {0, 0, 0};
int nnodes = 0;
for (int b = 1; b < m->nbody; b++) {
center[0] += m->body_pos[3*b + 0];
center[1] += m->body_pos[3*b + 1];
center[2] += m->body_pos[3*b + 2];
nnodes++;
}
ASSERT_GT(nnodes, 0);
center[0] /= nnodes; center[1] /= nnodes; center[2] /= nnodes;
// rotation: 45 degrees about (1,1,1)/sqrt(3)
mjtNum angle = 45 * 3.14159265358979 / 180.0;
mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2);
mjtNum inv_sqrt3 = 1.0 / mju_sqrt(3.0);
mjtNum quat[4] = {ca, sa * inv_sqrt3, sa * inv_sqrt3, sa * inv_sqrt3};
mjtNum neg_quat[4];
mju_negQuat(neg_quat, quat);
// apply rigid rotation via slide joint displacements:
// new_pos = center + R * (body_pos - center)
// qpos = new_pos - body_pos
for (int b = 1; b < m->nbody; b++) {
mjtNum rel[3] = {m->body_pos[3*b+0] - center[0],
m->body_pos[3*b+1] - center[1],
m->body_pos[3*b+2] - center[2]};
mjtNum rotated[3];
mju_rotVecQuat(rotated, rel, neg_quat);
// each body has 3 slide joints (x, y, z)
for (int j = 0; j < m->body_jntnum[b] && j < 3; j++) {
int jid = m->body_jntadr[b] + j;
int qadr = m->jnt_qposadr[jid];
int axis = -1;
for (int a = 0; a < 3; a++) {
if (m->jnt_axis[3*jid + a] != 0) { axis = a; break; }
}
if (axis >= 0) {
d->qpos[qadr] =
(center[axis] + rotated[axis]) - m->body_pos[3 * b + axis];
}
}
}
mj_forward(m, d);
// spring forces should still be zero after rigid rotation
constexpr mjtNum tol = MjTol(1e-6, 1e-3);
for (int i = 0; i < m->nv; i++) {
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
<< "nonzero spring force at DOF " << i << " after rigid rotation";
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/engine/engine_sensor_test.cc
+57
View File
@@ -1727,5 +1727,62 @@ TEST_F(SensorTest, TactileSkipTangents) {
mj_deleteModel(model);
}
// insidesite uses subtree_com for massless flex parent bodies
TEST_F(SensorTest, InsideSiteFlexBody) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body name="parent">
<flexcomp name="soft" type="grid" count="3 3 3"
radius="0.01" dim="3" mass="1">
<elasticity young="5e4" poisson="0.2"/>
</flexcomp>
</body>
<!-- large site centered at origin, should contain the flex -->
<site name="container" type="box" size="2 2 2"/>
</worldbody>
<sensor>
<insidesite name="inside" site="container"
objtype="body" objname="parent"/>
</sensor>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// flex is at origin, site is a large box at origin — should be inside
mj_forward(m, d);
EXPECT_EQ(d->sensordata[0], 1)
<< "flex body should be inside the container site";
// shift all vertex/node bodies far outside the site via qpos
// each body has 3 slide joints (x, y, z); shift z by +10
int parent_id = mj_name2id(m, mjOBJ_BODY, "parent");
for (int b = parent_id + 1; b < m->nbody; b++) {
if (m->body_parentid[b] == parent_id) {
int jadr = m->body_jntadr[b];
if (jadr >= 0 && m->body_jntnum[b] == 3) {
// z-slide is the 3rd joint
d->qpos[m->jnt_qposadr[jadr + 2]] = 10.0;
}
}
}
mj_forward(m, d);
// subtree_com should now be far outside; sensor should read 0
EXPECT_EQ(d->sensordata[0], 0)
<< "flex body should be outside the container site after displacement";
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/engine/engine_sleep_test.cc
+53
View File
@@ -533,6 +533,59 @@ TEST_F(SleepTest, Equality) {
mj_deleteModel(m);
}
// Test that the midpoint integrator doesn't break the sleep qvel=0 invariant.
// A standalone free body (eligible for midpoint) with high viscosity should
// eventually go to sleep, and after sleeping, qvel/qacc must be exactly zero.
TEST_F(SleepTest, MidpointSleepZeroVelocity) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" viscosity="10"
sleep_tolerance="0.01">
<flag sleep="enable" gravity="disable" constraint="disable"
contact="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30"
pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// give initial velocity (both translational and angular)
d->qvel[0] = 0.5;
d->qvel[1] = 0.5;
d->qvel[2] = 0.5;
d->qvel[3] = 1.0;
d->qvel[4] = 2.0;
d->qvel[5] = 3.0;
// step until body goes to sleep
for (int step = 0; step < 1000; step++) {
mj_step(m, d);
if (d->ntree_awake == 0) break;
}
// body should have gone to sleep
ASSERT_EQ(d->ntree_awake, 0) << "body did not go to sleep";
// qvel and qacc must be exactly zero for sleeping body
for (int i = 0; i < 6; i++) {
EXPECT_EQ(d->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep";
EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << i << "] not zero after sleep";
}
mj_deleteData(d);
mj_deleteModel(m);
}
static const char* const kInitIslandFailModel =
"engine/testdata/sleep/init_island_fail.xml";
test/engine/engine_solver_test.cc
+12 -6
View File
@@ -46,6 +46,7 @@ TEST_F(SolverTest, IslandsEquivalent) {
model->opt.tolerance = 0; // set tolerance to 0
model->opt.ls_tolerance = 0; // set ls_tolerance to 0
model->opt.ccd_tolerance = 0; // set ccd_tolerance to 0
model->opt.disableflags |= mjDSBL_MULTICCD; // disable multiccd
int nv = model->nv;
@@ -55,16 +56,21 @@ TEST_F(SolverTest, IslandsEquivalent) {
mjData* data_island = mj_makeData(model);
mjData* data_noisland = mj_makeData(model);
// Below are 3 tolerances associated with 3 different iteration counts,
// they are only moderately tight, 12x higher than x86-64 failure on Linux,
// i.e. in that case the test fails with rtol smaller than {6e-3, 6e-4, 6e-5}.
constexpr int kNumTol = 3;
mjtNum maxiter[kNumTol] = {30, 40, 60};
// Below are 3 tolerances associated with 3 different iteration counts.
// Tolerances are set to be ~12x higher than failure thresholds.
// For float32, failure thresholds are ~6000x larger than for float64.
// Line 99 adds a 500x factor for float32, so we need another ~12x in rtol.
// The point of this test is to show that CG convergence is actually not very
// precise, simply changing whether islands are used changes the solution by
// quite a lot, even at high iteration count and zero {ls_}tolerance.
// Increasing the iteration count higher than 60 does not improve convergence.
constexpr int kNumTol = 3;
mjtNum maxiter[kNumTol] = {30, 40, 60};
mjtNum rtol[kNumTol] = {6e-2, 6e-3, 6e-4};
mjtNum rtol[kNumTol] = {
MjTol(6e-2, 7.2e-1),
MjTol(6e-3, 7.2e-2),
MjTol(6e-4, 7.2e-3)
};
for (int i = 0; i < kNumTol; ++i) {
model->opt.iterations = maxiter[i];
test/engine/engine_support_test.cc
+4 -444
View File
@@ -14,11 +14,9 @@
// Tests for engine/{engine_support.c and engine_core_util.c}
#include "src/engine/engine_core_util.h"
#include "src/engine/engine_support.h"
#include <cstring>
#include <limits>
#include <random>
#include <string>
#include <string_view>
@@ -40,448 +38,9 @@ using ::testing::Ne;
using ::testing::NotNull;
using ::testing::Pointwise;
using AngMomMatTest = MujocoTest;
static constexpr char AngMomTestingModel[] = R"(
<mujoco>
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<body name="link1" pos="0 0 0.5">
<freejoint/>
<geom type="ellipsoid" size="0.15 0.17 0.19" quat="1 .2 .3 .4"/>
<body name="link2" >
<joint type="hinge" axis="1 0 0" />
<geom type="capsule" size="0.05" fromto="0 0 0 0 0.5 0"/>
<body pos="0 0.6 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.05 0.2" quat="0.707 0 0.707 0"/>
<body name="link3">
<joint type="ball" pos="0.2 0 0"/>
<geom type="capsule" pos="0.2 0 0" size="0.03 0.4"/>
</body>
</body>
</body>
</body>
</worldbody>
<keyframe>
<key qvel="0 0 0 .1 .2 .3 .4 .5 .4 .3 .2"/>
</keyframe>
</mujoco>
)";
// compare subtree angular momentum computed in two ways
TEST_F(AngMomMatTest, CompareAngMom) {
char error[1024];
mjModel* model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
mjData* data = mj_makeData(model);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
// get the reference value of angular momentum
mj_subtreeVel(model, data);
mjtNum angmom_ref[3];
mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid);
// compute angular momentum using the angular momentum matrix
mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mj_angmomMat(model, data, angmom_mat, bodyid);
mjtNum angmom_test[3];
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
// compare the two angular momentum values
for (int i = 0; i < 3; i++) {
EXPECT_THAT(angmom_ref[i], MjNear(angmom_test[i], 1e-8, 1e-4));
}
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
// compare subtree angular momentum matrix: analytical and findiff
TEST_F(AngMomMatTest, CompareAngMomMats) {
char error[1024];
mjModel* model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
mjData* data = mj_makeData(model);
mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mjtNum* angmom_mat_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
// compute the angular momentum matrix using the analytical method
mj_angmomMat(model, data, angmom_mat, bodyid);
// compute the angular momentum matrix using finite differences
static constexpr mjtNum eps = MjTol(1e-6, 1e-3);
for (int i = 0; i < nv; i++) {
// reset vel, forward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] += eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mjtNum agmf[3];
mju_copy3(agmf, data->subtree_angmom+3*bodyid);
// reset vel, backward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] -= eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mjtNum agmb[3];
mju_copy3(agmb, data->subtree_angmom+3*bodyid);
// finite-difference the angmom matrix
for (int j = 0; j < 3; j++) {
angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps);
}
}
// compare the two matrices
for (int i = 0; i < 3*nv; i++) {
EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4));
}
mju_free(angmom_mat_fd);
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
using JacobianTest = MujocoTest;
static const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
static constexpr char kJacobianTestingModel[] = R"(
<mujoco>
<worldbody>
<body name="distractor1" pos="0 0 .3">
<freejoint/>
<geom size=".1"/>
</body>
<body name="main">
<freejoint/>
<geom size=".1"/>
<body pos=".1 0 0">
<joint axis="0 1 0"/>
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
</body>
<body pos="0 .1 0">
<joint type="ball"/>
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
<body pos="0 .2 0">
<joint type="slide" axis="1 1 1"/>
<geom size=".05"/>
</body>
</body>
</body>
<body name="distractor2" pos="0 0 -.3">
<freejoint/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
// compare analytic and finite-differenced subtree-com Jacobian
TEST_F(JacobianTest, SubtreeJac) {
char error[1024];
mjModel* model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
mjData* data = mj_makeData(model);
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq);
mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv);
// all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data);
mj_comPos(model, data);
// get subtree CoM Jacobian of free body
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
// save current subtree-com and qpos, clear nudge
mjtNum subtree_com[3];
mju_copy3(subtree_com, data->subtree_com+3*bodyid);
mju_copy(qpos, data->qpos, model->nq);
mju_zero(nudge, nv);
// compare analytic Jacobian to finite-difference approximation
static const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
// reset qpos, nudge i-th dof, update data->qpos, reset nudge
mju_copy(data->qpos, qpos, model->nq);
nudge[i] = 1;
mj_integratePos(model, data->qpos, nudge, eps);
nudge[i] = 0;
// kinematics and comPos to get nudged com
mj_kinematics(model, data);
mj_comPos(model, data);
// compare finite-differenced and analytic Jacobian
for (int j=0; j < 3; j++) {
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps;
EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2));
}
}
mju_free(nudge);
mju_free(qpos);
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
// confirm that applying linear forces via the subtree-com Jacobian only creates
// the expected linear accelerations (no accelerations of internal joints)
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
char error[1024];
mjModel* model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
mjData* data = mj_makeData(model);
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
// all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data);
mj_comPos(model, data);
// get subtree CoM Jacobian of free body
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
// uncomment for debugging
// mju_printMat(jac_subtree, 3, nv);
// call fwdPosition since we'll need the factorised mass matrix in the test
mj_fwdPosition(model, data);
// treating the subtree Jacobian as the projection of 3 axis-aligned unit
// forces into joint space, solve for the resulting accelerations in-place
mj_solveM(model, data, jac_subtree, jac_subtree, 3);
// expect to find accelerations of magnitude 1/subtreemass in the first 3
// coordinates of the free joint and 0s elsewhere, since applying forces to
// the CoM should accelerate the whole mechanism without any internal motion
int body_dofadr = model->body_dofadr[bodyid];
mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid];
for (int r = 0; r < 3; r++) {
for (int c = 0; c < nv; c++) {
mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0;
EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4));
}
}
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char kQuat[] = R"(
<mujoco>
<worldbody>
<body name="query">
<joint type="ball"/>
<geom size="1"/>
<site name="query" pos=".1 .2 .3"/>
</body>
</worldbody>
<keyframe>
<key qvel="2 3 5"/>
</keyframe>
</mujoco>
)";
static constexpr char kFreeBall[] = R"(
<mujoco>
<worldbody>
<body name="distractor1" pos="0 0 .3">
<freejoint/>
<geom size=".1"/>
</body>
<body name="main">
<freejoint/>
<geom size=".1"/>
<body pos=".1 0 0">
<joint axis="0 1 0"/>
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
<body pos=".2 0 0">
<joint type="ball" stiffness="20"/>
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
<body name="query" pos="0 .2 0">
<joint type="slide" axis="1 1 1"/>
<geom size=".05"/>
<site name="query" pos=".1 .2 .3"/>
</body>
</body>
</body>
</body>
<body name="distractor2" pos="0 0 -.3">
<freejoint/>
<geom size=".1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"/>
</keyframe>
</mujoco>
)";
static constexpr char kQuatlessPendulum[] = R"(
<mujoco>
<option integrator="implicit">
<flag constraint="disable"/>
</option>
<worldbody>
<body pos="0.15 0 0">
<joint type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<body pos="0.1 0 0">
<joint type="slide" axis="1 0 0" stiffness="200"/>
<geom type="capsule" size="0.015" fromto="-.1 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint axis="1 0 0"/>
<joint axis="0 1 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".02" fromto="0 0 0 0 .1 0"/>
<body name="query" pos="0 .1 0">
<joint axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 0 .1 0"/>
<site name="query" pos=".1 0 0"/>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>
)";
static constexpr char kTelescope[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="ball"/>
<geom type="capsule" size="0.02" fromto="0 .02 0 .1 .02 0"/>
<body pos=".1 .02 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<body pos=".1 .02 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<body pos=".1 .02 0" name="query">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
<site name="query" pos=".1 0 0"/>
</body>
</body>
</body>
</body>
</worldbody>
<keyframe>
<key qvel="1 1 1 1 1 1"/>
</keyframe>
</mujoco>
)";
static constexpr char kHinge[] = R"(
<mujoco>
<worldbody>
<body name="query">
<joint name="link1" axis="0 1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
<site name="query" pos="0 0 -1"/>
</body>
</worldbody>
<keyframe>
<key qpos="1" qvel="1"/>
</keyframe>
</mujoco>
)";
// compare mj_jacDot with finite-differenced mj_jac
TEST_F(JacobianTest, JacDot) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int nv = model->nv;
mjData* data = mj_makeData(model);
// load keyframe if present, step for a bit
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
while (data->time < 0.1) {
mj_step(model, data);
}
// minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data);
mj_comPos(model, data);
mj_comVel(model, data);
// get bodyid
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
EXPECT_GT(bodyid, 0);
// get site position
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
EXPECT_GT(siteid, -1);
mjtNum point[3];
mju_copy3(point, data->site_xpos+3*siteid);
// jac, jac_dot
vector<mjtNum> jacp(3*nv);
vector<mjtNum> jacr(3*nv);
mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid);
vector<mjtNum> jacp_dot(3*nv);
vector<mjtNum> jacr_dot(3*nv);
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
// jac_h: jacobian after integrating qpos with a timestep of h
constexpr mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(model, data->qpos, data->qvel, h);
mj_kinematics(model, data);
mj_comPos(model, data);
vector<mjtNum> jacp_h(3*nv);
vector<mjtNum> jacr_h(3*nv);
mju_copy3(point, data->site_xpos+3*siteid); // get updated site position
mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid);
// jac_dot_h finite-difference approximation
vector<mjtNum> jacp_dot_h(3*nv);
mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv);
mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv);
vector<mjtNum> jacr_dot_h(3*nv);
mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv);
mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv);
// compare finite-differenced and analytic
mjtNum tol = 1e-5;
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
mj_deleteData(data);
mj_deleteModel(model);
}
}
using Name2idTest = MujocoTest;
@@ -989,7 +548,8 @@ TEST_F(InertiaTest, mulM) {
// dense M matrix
vector<mjtNum> Mdense(nv*nv);
mj_fullM(model, Mdense.data(), data->qM);
mju_sym2dense(Mdense.data(), data->M, nv,
model->M_rownnz, model->M_rowadr, model->M_colind);
// arbitrary RHS vector
vector<mjtNum> vec(nv);
@@ -1052,9 +612,9 @@ TEST_F(InertiaTest, FullM) {
mjData* d = mj_makeData(m);
mj_forward(m, d);
// get dense mass matrix from qM using mj_fullM
// get dense mass matrix from M using mju_sym2dense
vector<mjtNum> M(nv * nv);
mj_fullM(m, M.data(), d->qM);
mju_sym2dense(M.data(), d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
// get dense mass matrix from M using mju_sparse2dense
vector<mjtNum> M_CSR(nv * nv);
test/engine/engine_util_misc_test.cc
+505 -2
View File
@@ -430,13 +430,90 @@ TEST_F(InterpolationTest, mju_interpolate3D) {
expected[0] = quadratic_function_1(sample[0], sample[1], sample[2]);
expected[1] = quadratic_function_2(sample[0], sample[1], sample[2]);
expected[2] = quadratic_function_3(sample[0], sample[1], sample[2]);
mju_interpolate3D(res, sample, coeff, order);
mju_interpolate3D(res, sample, coeff, order, NULL);
EXPECT_NEAR(res[0], expected[0], MjTol(1e-10, 1e-5));
EXPECT_NEAR(res[1], expected[1], MjTol(1e-10, 1e-5));
EXPECT_NEAR(res[2], expected[2], MjTol(1e-10, 1e-5));
}
}
TEST_F(InterpolationTest, mju_cellLookup_SingleCell) {
// single cell (1x1x1): local coords should equal global coords
int cellnum[3] = {1, 1, 1};
mjtNum coord[3] = {0.3, 0.7, 0.5};
mjtNum local[3];
int nodeindices[8];
int npc = mju_cellLookup(coord, cellnum, 1, local, nodeindices);
EXPECT_EQ(npc, 8);
EXPECT_NEAR(local[0], 0.3, MjTol(1e-12, 1e-6));
EXPECT_NEAR(local[1], 0.7, MjTol(1e-12, 1e-6));
EXPECT_NEAR(local[2], 0.5, MjTol(1e-12, 1e-6));
// for trilinear 1x1x1: nodes are 0..7 in lexicographic order
for (int i = 0; i < 8; i++) {
EXPECT_EQ(nodeindices[i], i);
}
}
TEST_F(InterpolationTest, mju_cellLookup_MultiCell) {
// 2x3x4 grid, trilinear: 3x4x5 = 60 nodes
int cellnum[3] = {2, 3, 4};
int order = 1;
int ny_g = 3*1 + 1; // 4
int nz_g = 4*1 + 1; // 5
// point at (0.75, 0.5, 0.125) -> cell (1, 1, 0)
mjtNum coord[3] = {0.75, 0.5, 0.125};
mjtNum local[3];
int nodeindices[8];
int npc = mju_cellLookup(coord, cellnum, order, local, nodeindices);
EXPECT_EQ(npc, 8);
// cell (1,1,0): local = (0.75*2 - 1, 0.5*3 - 1, 0.125*4 - 0)
EXPECT_NEAR(local[0], 0.5, 1e-12);
EXPECT_NEAR(local[1], 0.5, 1e-12);
EXPECT_NEAR(local[2], 0.5, 1e-12);
// expected node indices for cell (1,1,0), trilinear:
// (gi, gj, gk) for li,lj,lk in {0,1}
// gi = 1+li, gj = 1+lj, gk = 0+lk
// gidx = gi*ny_g*nz_g + gj*nz_g + gk
int expected[8];
int ni = 0;
for (int li = 0; li <= 1; li++) {
for (int lj = 0; lj <= 1; lj++) {
for (int lk = 0; lk <= 1; lk++) {
expected[ni++] = (1+li)*ny_g*nz_g + (1+lj)*nz_g + lk;
}
}
}
for (int i = 0; i < 8; i++) {
EXPECT_EQ(nodeindices[i], expected[i]);
}
}
TEST_F(InterpolationTest, mju_cellLookup_Boundary) {
// point exactly at coord=1.0 should clamp to last cell
int cellnum[3] = {3, 3, 3};
mjtNum coord[3] = {1.0, 1.0, 1.0};
mjtNum local[3];
mju_cellLookup(coord, cellnum, 1, local, NULL);
// cell (2,2,2), local = (1*3 - 2, 1*3 - 2, 1*3 - 2) = (1, 1, 1)
EXPECT_NEAR(local[0], 1.0, 1e-12);
EXPECT_NEAR(local[1], 1.0, 1e-12);
EXPECT_NEAR(local[2], 1.0, 1e-12);
// point at coord=0.0 should map to first cell
mjtNum coord0[3] = {0.0, 0.0, 0.0};
mju_cellLookup(coord0, cellnum, 1, local, NULL);
EXPECT_NEAR(local[0], 0.0, 1e-12);
EXPECT_NEAR(local[1], 0.0, 1e-12);
EXPECT_NEAR(local[2], 0.0, 1e-12);
}
TEST_F(InterpolationTest, mju_defGradient) {
int order = 1;
mjtNum mat[9];
@@ -521,7 +598,48 @@ TEST_F(InterpolationTest, mju_defGradient) {
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot7));
}
// --------------------------------- Base64 ------------------------------------
TEST_F(InterpolationTest, mju_flexInterpState_MultiCell) {
int order = 1; // trilinear
int cy = 2;
int cz = 2;
int nodenum = 27; // 3x3x3
std::vector<mjtNum> xpos(3 * nodenum);
mjtNum quat[4];
// Populate xpos directly for a grid centered at origin, rotated 90 deg around
// Z Original grid points: {-0.1, 0.0, 0.1}^3 Rotated: (x, y, z) -> (-y, x, z)
int idx = 0;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
mjtNum x = (i - 1) * 0.1;
mjtNum y = (j - 1) * 0.1;
mjtNum z = (k - 1) * 0.1;
// Apply rotation
xpos[3*idx + 0] = -y;
xpos[3*idx + 1] = x;
xpos[3*idx + 2] = z;
idx++;
}
}
}
int npc = (order+1)*(order+1)*(order+1);
std::vector<mjtNum> xpos_c(3 * npc);
mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL,
xpos_c.data(), NULL, NULL, NULL, quat);
// Expected quaternion for -90 deg around Z (global to local):
// [sqrt(0.5), 0, 0, -sqrt(0.5)]
mjtNum expected_val = mju_sqrt(0.5);
EXPECT_NEAR(quat[0], expected_val, 1e-5);
EXPECT_NEAR(quat[1], 0.0, 1e-5);
EXPECT_NEAR(quat[2], 0.0, 1e-5);
EXPECT_NEAR(quat[3], -expected_val, 1e-5);
}
using Base64Test = MujocoTest;
@@ -1047,5 +1165,390 @@ TEST_F(HistoryTest, CubicInterpolation) {
EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9));
}
// -------------------------------- Face State ---------------------------------
using FaceStateTest = MujocoTest;
// verify mju_flexGatherFaceState returns correct node indices for all 6 faces
// of a 1x1x1 trilinear grid (2x2x2 = 8 nodes, 4 nodes per face)
TEST_F(FaceStateTest, NodeIndicesSingleCell) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int ny_g = cy * order + 1; // 2
int nz_g = cz * order + 1; // 2
// nelem_fe = 2*(1*1 + 1*1 + 1*1) = 6 face elements
// face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max,
// face 4: z=0, face 5: z=max
// create dummy positions for 8 nodes
std::vector<mjtNum> xpos(3 * 8, 0);
for (int i = 0; i < 8; i++) {
xpos[3*i + 0] = (i / 4) * 1.0;
xpos[3*i + 1] = ((i / 2) % 2) * 1.0;
xpos[3*i + 2] = (i % 2) * 1.0;
}
// helper: compute expected global node index from (gx, gy, gz)
auto gidx = [&](int gx, int gy, int gz) {
return gx * ny_g * nz_g + gy * nz_g + gz;
};
// face 0: x=0 (fixed g[0]=0, varying g[1], g[2])
// normal_axis=0, na0=1, na1=2
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0));
EXPECT_EQ(indices[3], gidx(0, 1, 1));
}
// face 1: x=max (fixed g[0]=1, varying g[1], g[2])
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(1, 0, 0));
EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(1, 1, 0));
EXPECT_EQ(indices[3], gidx(1, 1, 1));
}
// face 2: y=0 (fixed g[1]=0)
// normal_axis=1, na0=2(z slow), na1=0(x fast)
// loop order: l0→z, l1→x
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0)
EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1)
EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0)
EXPECT_EQ(indices[3], gidx(1, 0, 1)); // l0=1(z=1), l1=1(x=1)
}
// face 3: y=max (fixed g[1]=1)
// normal_axis=1, na0=2(z slow), na1=0(x fast)
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0)
EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1)
EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0)
EXPECT_EQ(indices[3], gidx(1, 1, 1)); // l0=1(z=1), l1=1(x=1)
}
// face 4: z=0 (fixed g[2]=0, varying g[0], g[1])
// normal_axis=2, na0=0, na1=1
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 1, 0));
EXPECT_EQ(indices[2], gidx(1, 0, 0));
EXPECT_EQ(indices[3], gidx(1, 1, 0));
}
// face 5: z=max (fixed g[2]=1)
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 1));
EXPECT_EQ(indices[2], gidx(1, 0, 1));
EXPECT_EQ(indices[3], gidx(1, 1, 1));
}
}
// verify node indices for a multi-cell grid (2x2x2 cells → 3x3x3 = 27 nodes)
TEST_F(FaceStateTest, NodeIndicesMultiCell) {
int order = 1;
int cx = 2, cy = 2, cz = 2;
int ny_g = 3, nz_g = 3; // (2*1+1) = 3
// nelem_fe = 2*(2*2 + 2*2 + 2*2) = 24 face elements
// face 0: x=0, cy*cz = 4 quads (indices 0-3)
// face 1: x=max, 4 quads (indices 4-7)
// face 2: y=0, cx*cz = 4 quads (indices 8-11)
// face 3: y=max, 4 quads (indices 12-15)
// face 4: z=0, cx*cy = 4 quads (indices 16-19)
// face 5: z=max, 4 quads (indices 20-23)
std::vector<mjtNum> xpos(3 * 27, 0);
for (int i = 0; i < 27; i++) {
int gi = i / 9;
int gj = (i / 3) % 3;
int gk = i % 3;
xpos[3*i + 0] = gi * 0.1;
xpos[3*i + 1] = gj * 0.1;
xpos[3*i + 2] = gk * 0.1;
}
auto gidx = [&](int gx, int gy, int gz) {
return gx * ny_g * nz_g + gy * nz_g + gz;
};
// face 0 (x=0), quad 0: (q0=0, q1=0) within cy*cz face
// c1 = face_count1[0] = cz = 2, so quad (0,0) → within_face = 0
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0));
EXPECT_EQ(indices[3], gidx(0, 1, 1));
}
// face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 2));
EXPECT_EQ(indices[2], gidx(0, 2, 1));
EXPECT_EQ(indices[3], gidx(0, 2, 2));
}
// face 1 (x=max), quad 0: fe_idx = 4 (after face 0's 4 quads)
// g[0] = cx*order = 2
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(2, 0, 0));
EXPECT_EQ(indices[1], gidx(2, 0, 1));
EXPECT_EQ(indices[2], gidx(2, 1, 0));
EXPECT_EQ(indices[3], gidx(2, 1, 1));
}
}
// verify node indices for a non-cubic grid (cx != cz)
TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
int order = 1;
int cx = 2, cy = 1, cz = 3;
int ny_g = cy * order + 1; // 2
int nz_g = cz * order + 1; // 4
// create dummy positions for (2*1+1)*(1*1+1)*(3*1+1) = 3*2*4 = 24 nodes
std::vector<mjtNum> xpos(3 * 24, 0);
for (int i = 0; i < 24; i++) {
int gi = i / 8;
int gj = (i / 4) % 2;
int gk = i % 4;
xpos[3*i + 0] = gi * 0.1;
xpos[3*i + 1] = gj * 0.1;
xpos[3*i + 2] = gk * 0.1;
}
auto gidx = [&](int gx, int gy, int gz) {
return gx * ny_g * nz_g + gy * nz_g + gz;
};
// face 2 (y=0): normal_axis=1, na0=2(z slow), na1=0(x fast)
// counts: na0 -> cz = 3, na1 -> cx = 2
// total quads on face 2 = 6
// we test within_face = 2 (third quad)
// correct: c1 = cx = 2. q0 = 2/2 = 1, q1 = 2%2 = 0
//
// face element index calculation:
// face 0: cy*cz = 1*3 = 3 quads (indices 0-2)
// face 1: cy*cz = 1*3 = 3 quads (indices 3-5)
// face 2: cx*cz = 2*3 = 6 quads. Quad 2 is index 2 within this face.
// Total flat index = 3 + 3 + 2 = 8
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 0, 2));
EXPECT_EQ(indices[3], gidx(1, 0, 2));
}
}
// verify data gathering: positions, velocities, and reference positions
TEST_F(FaceStateTest, DataGathering) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int npe = 4;
int nnodes = 8;
// create positions and velocities for 8 nodes
std::vector<mjtNum> xpos(3 * nnodes);
std::vector<mjtNum> vel(3 * nnodes);
std::vector<mjtNum> xpos0(3 * nnodes);
for (int i = 0; i < nnodes; i++) {
for (int d = 0; d < 3; d++) {
xpos[3*i + d] = 10 * i + d;
vel[3*i + d] = 100 * i + d;
xpos0[3*i + d] = 1000 * i + d;
}
}
// gather face 4 (z=0): nodes at (0,0,0), (0,1,0), (1,0,0), (1,1,0)
// = global indices 0, 2, 4, 6
std::vector<mjtNum> xpos_f(3 * npe);
std::vector<mjtNum> vel_f(3 * npe);
std::vector<mjtNum> xpos0_f(3 * npe);
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), vel.data(),
xpos0.data(), xpos_f.data(), vel_f.data(),
xpos0_f.data(), indices, NULL);
for (int n = 0; n < npe; n++) {
int gi = indices[n];
for (int d = 0; d < 3; d++) {
EXPECT_EQ(xpos_f[3*n + d], xpos[3*gi + d]);
EXPECT_EQ(vel_f[3*n + d], vel[3*gi + d]);
EXPECT_EQ(xpos0_f[3*n + d], xpos0[3*gi + d]);
}
}
}
// verify that flexInterpRotation2D produces identity for axis-aligned faces
// (tested via mju_flexGatherFaceState with quat output)
TEST_F(FaceStateTest, IdentityRotationAxisAligned) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int npe = 4;
// create an axis-aligned unit cube: 8 nodes at {0,1}^3
std::vector<mjtNum> xpos(3 * 8);
int idx = 0;
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
xpos[3*idx + 0] = i;
xpos[3*idx + 1] = j;
xpos[3*idx + 2] = k;
idx++;
}
}
}
std::vector<mjtNum> xpos_f(3 * npe);
mjtNum quat[4];
// test all 6 faces: each should give identity rotation (quat = [1,0,0,0])
int nelem_fe = 6;
for (int fe = 0; fe < nelem_fe; fe++) {
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat);
EXPECT_NEAR(mju_abs(quat[0]), 1.0, 1e-10) << "face " << fe;
EXPECT_NEAR(quat[1], 0.0, 1e-10) << "face " << fe;
EXPECT_NEAR(quat[2], 0.0, 1e-10) << "face " << fe;
EXPECT_NEAR(quat[3], 0.0, 1e-10) << "face " << fe;
}
}
// verify that flexInterpRotation2D extracts the correct rotation for a
// globally rotated cube (90° around z-axis)
TEST_F(FaceStateTest, RotatedCubeRotation) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int npe = 4;
// create an axis-aligned unit cube, then rotate 90° around z
// rotation: (x,y,z) → (-y, x, z)
std::vector<mjtNum> xpos(3 * 8);
int idx = 0;
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k};
mjtNum axis[3] = {0, 0, 1};
mjtNum rot_quat[4];
mju_axisAngle2Quat(rot_quat, axis, mjPI / 2);
mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat);
idx++;
}
}
}
std::vector<mjtNum> xpos_f(3 * npe);
mjtNum quat[4];
// expected rotation: global→local is inverse of the 90° z rotation
// 90° around z: quat = [cos(45°), 0, 0, sin(45°)]
// inverse (global→local): [cos(45°), 0, 0, -sin(45°)]
mjtNum sq2 = mju_sqrt(0.5);
// test face 4 (z=0): normal_axis=2, in-plane axes are (0,1)
// tangent vectors should reflect the 90° z rotation
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat);
EXPECT_NEAR(quat[0], sq2, 1e-5);
EXPECT_NEAR(quat[1], 0.0, 1e-5);
EXPECT_NEAR(quat[2], 0.0, 1e-5);
EXPECT_NEAR(quat[3], -sq2, 1e-5);
// test face 5 (z=max): should give same rotation
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat);
EXPECT_NEAR(quat[0], sq2, 1e-5);
EXPECT_NEAR(quat[1], 0.0, 1e-5);
EXPECT_NEAR(quat[2], 0.0, 1e-5);
EXPECT_NEAR(quat[3], -sq2, 1e-5);
}
// verify that flexInterpRotation2D matches the 3D cell rotation for
// the same globally-rotated cube
TEST_F(FaceStateTest, RotationConsistencyWith3D) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
// create 90° z-rotated unit cube
std::vector<mjtNum> xpos(3 * 8);
int idx = 0;
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k};
mjtNum axis[3] = {0, 0, 1};
mjtNum rot_quat[4];
mju_axisAngle2Quat(rot_quat, axis, mjPI / 6);
mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat);
idx++;
}
}
}
// get 3D cell rotation
int npc = 8;
std::vector<mjtNum> xpos_c(3 * npc);
mjtNum quat_3d[4];
mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL,
xpos_c.data(), NULL, NULL, NULL, quat_3d);
// get 2D face rotation for each face and verify it matches the 3D rotation
int npe = 4;
std::vector<mjtNum> xpos_f(3 * npe);
int nelem_fe = 6;
for (int fe = 0; fe < nelem_fe; fe++) {
mjtNum quat_2d[4];
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat_2d);
// quaternions may differ by sign; compare unsigned
mjtNum dot = quat_3d[0]*quat_2d[0] + quat_3d[1]*quat_2d[1] +
quat_3d[2]*quat_2d[2] + quat_3d[3]*quat_2d[3];
EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5)
<< "face " << fe << ": 2D rotation differs from 3D cell rotation";
}
}
} // namespace
} // namespace mujoco
test/engine/engine_util_solve_test.cc
+142
View File
@@ -1041,5 +1041,147 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
mj_deleteModel(model);
}
// ----------------------------- dense LU --------------------------------------
using DenseLUTest = MujocoTest;
// factor identity, solve recovers b exactly
TEST_F(DenseLUTest, Identity) {
constexpr int n = 4;
mjtNum A[n*n] = {
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1,
};
int pivot[n];
mjtNum b[n] = {1, 2, 3, 4};
mjtNum x[n];
EXPECT_EQ(mju_factorLU(A, n, pivot), 1);
mju_solveLU(x, A, b, pivot, n);
for (int i = 0; i < n; i++) {
EXPECT_MJTNUM_EQ(x[i], b[i]);
}
}
// 3x3 with known solution
TEST_F(DenseLUTest, SmallKnown) {
constexpr int n = 3;
// A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1]
// solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1])
mjtNum A[n*n] = {
2, 1, 1,
4, 3, 3,
8, 7, 9,
};
int pivot[n];
mjtNum b[n] = {1, 1, 1};
mjtNum x[n];
EXPECT_EQ(mju_factorLU(A, n, pivot), 1);
mju_solveLU(x, A, b, pivot, n);
mjtNum eps = MjTol(1e-14, 1e-6);
EXPECT_NEAR(x[0], 1, eps);
EXPECT_NEAR(x[1], -1, eps);
EXPECT_NEAR(x[2], 0, eps);
}
// random SPD matrices: compare LU solve against Cholesky solve
TEST_F(DenseLUTest, RandomSPD) {
std::mt19937_64 rng;
rng.seed(7);
std::normal_distribution<double> dist(0, 1);
for (int n : {4, 8, 16}) {
vector<mjtNum> sqrtH(n * n);
vector<mjtNum> A(n * n);
vector<mjtNum> A_chol(n * n);
vector<mjtNum> b(n);
vector<mjtNum> x_lu(n);
vector<mjtNum> x_chol(n);
vector<int> pivot(n);
// generate random SPD matrix
for (int i = 0; i < n * n; i++) sqrtH[i] = dist(rng);
mju_mulMatTMat(A.data(), sqrtH.data(), sqrtH.data(), n, n, n);
// add diagonal regularizer
for (int i = 0; i < n; i++) A[i*n+i] += n;
// generate random rhs
for (int i = 0; i < n; i++) b[i] = dist(rng);
// solve with Cholesky
mju_copy(A_chol.data(), A.data(), n * n);
int rank = mju_cholFactor(A_chol.data(), n, 0);
EXPECT_EQ(rank, n);
mju_cholSolve(x_chol.data(), A_chol.data(), b.data(), n);
// solve with LU
int ok = mju_factorLU(A.data(), n, pivot.data());
EXPECT_EQ(ok, 1);
mju_solveLU(x_lu.data(), A.data(), b.data(), pivot.data(), n);
// compare
mjtNum eps = MjTol(1e-15, 1e-7);
EXPECT_THAT(AsVector(x_lu.data(), n),
Pointwise(MjNear(eps, eps),
AsVector(x_chol.data(), n)));
}
}
// random non-symmetric matrices: verify A*x == b
TEST_F(DenseLUTest, RandomGeneral) {
std::mt19937_64 rng;
rng.seed(42);
std::normal_distribution<double> dist(0, 1);
for (int n : {3, 5, 10, 20}) {
vector<mjtNum> A(n * n);
vector<mjtNum> A_orig(n * n);
vector<mjtNum> b(n);
vector<mjtNum> x(n);
vector<mjtNum> Ax(n);
vector<int> pivot(n);
// random non-symmetric matrix with diagonal dominance
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
A[i*n+j] = dist(rng);
}
A[i*n+i] += 2 * n;
}
mju_copy(A_orig.data(), A.data(), n * n);
// random rhs
for (int i = 0; i < n; i++) b[i] = dist(rng);
// factor and solve
int ok = mju_factorLU(A.data(), n, pivot.data());
EXPECT_EQ(ok, 1);
mju_solveLU(x.data(), A.data(), b.data(), pivot.data(), n);
// verify: A_orig * x == b
mju_mulMatVec(Ax.data(), A_orig.data(), x.data(), n, n);
mjtNum eps = MjTol(1e-14, 1e-5);
EXPECT_THAT(AsVector(Ax.data(), n),
Pointwise(MjNear(eps, eps), AsVector(b.data(), n)));
}
}
// near-singular matrix returns 0
TEST_F(DenseLUTest, Singular) {
constexpr int n = 3;
// all zeros: maximally singular
mjtNum A[n*n] = {0};
int pivot[n];
EXPECT_EQ(mju_factorLU(A, n, pivot), 0);
}
} // namespace
} // namespace mujoco
test/engine/engine_util_sparse_test.cc
+464 -372
View File
@@ -14,10 +14,11 @@
// Tests for engine/engine_util_sparse.c
#include <array>
#include "src/engine/engine_util_sparse.h"
#include <array>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
@@ -360,6 +361,81 @@ TEST_F(EngineUtilSparseTest, MjuCompressSparse) {
EXPECT_EQ(AsVector(dense, 6), AsVector(dense_expected_minval1, 6));
}
TEST_F(EngineUtilSparseTest, MjuSym2Dense) {
// lower-triangular CSR for a 3x3 symmetric matrix:
// 1 2 0
// 2 3 4
// 0 4 5
// stored as lower triangle:
// row 0: [1] (col 0)
// row 1: [2, 3] (cols 0, 1)
// row 2: [4, 5] (cols 1, 2)
mjtNum mat[] = {1, 2, 3, 4, 5};
int rownnz[] = {1, 2, 2};
int rowadr[] = {0, 1, 3};
int colind[] = {0, 0, 1, 1, 2};
mjtNum dense[9];
mju_sym2dense(dense, mat, 3, rownnz, rowadr, colind);
mjtNum expected[] = {1, 2, 0, 2, 3, 4, 0, 4, 5};
EXPECT_EQ(AsVector(dense, 9), AsVector(expected, 9));
}
TEST_F(EngineUtilSparseTest, MjuSym2DenseWithUpper) {
mjtNum mat[] = {1, 999, 2, 3, 4, 5};
int rownnz[] = {2, 2, 2};
int rowadr[] = {0, 2, 4};
int colind[] = {0, 1, 0, 1, 1, 2};
mjtNum dense[9];
mju_sym2dense(dense, mat, 3, rownnz, rowadr, colind);
mjtNum expected[] = {1, 2, 0,
2, 3, 4,
0, 4, 5};
EXPECT_EQ(AsVector(dense, 9), AsVector(expected, 9));
}
// helper: run split-col approach and return dense result
static void SqrMatTDSplitCol(
std::vector<mjtNum>& dense_result, int nr, int nc,
const mjtNum* mat, const int* rownnz, const int* rowadr, const int* colind,
const mjtNum* matT, const int* rownnzT, const int* rowadrT,
const int* colindT, const int* rowsuperT, const mjtNum* diag,
int* out_diagind, mjData* d) {
// count mode
std::vector<int> H_rownnz(nc, 0);
std::vector<int> H_rowadr(nc, 0);
int nnz = mju_sqrMatTDSparseSymbolic(
H_rownnz.data(), H_rowadr.data(), nullptr,
out_diagind, nr, nc, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, d);
// fill mode
std::vector<int> H_colind(nnz);
mju_sqrMatTDSparseSymbolic(
H_rownnz.data(), H_rowadr.data(), H_colind.data(),
out_diagind, nr, nc, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, d);
// numeric phase
std::vector<mjtNum> H(nnz, 0);
mju_sqrMatTDSparseNumeric(
H.data(), nc, H_rownnz.data(), H_rowadr.data(),
H_colind.data(), out_diagind, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, rowsuperT, diag, d);
// densify
dense_result.assign(nc * nc, 0);
for (int r = 0; r < nc; r++) {
for (int j = 0; j < H_rownnz[r]; j++) {
int c = H_colind[H_rowadr[r] + j];
dense_result[r*nc + c] = H[H_rowadr[r] + j];
}
}
}
TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) {
// 0 0 0
// M = 0 0 0
@@ -378,29 +454,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) {
int rownnzT[] = {3, 3, 3};
int rowadrT[] = {0, 3, 6};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, nullptr,
diagindH, data);
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2));
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
@@ -424,27 +484,12 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseLower) {
int rownnzT[] = {3, 3, 3};
int rowadrT[] = {0, 3, 6};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, nullptr,
nullptr, data);
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 0);
EXPECT_THAT(rownnzH, ElementsAre(1, 2, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 1, 3));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, nullptr);
EXPECT_THAT(matH, ElementsAre(12, 0, 0, 0, 6, 0, 12, 3, 14));
EXPECT_THAT(colindH, ElementsAre(0, 0, 0, 0, 1, 0, 0, 1, 2));
EXPECT_THAT(rownnzH, ElementsAre(1, 2, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(12, 0, 0, 0, 6, 0, 12, 3, 14));
mj_deleteData(data);
mj_deleteModel(model);
@@ -468,31 +513,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) {
int rownnzT[] = {3, 3, 3};
int rowadrT[] = {0, 3, 6};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, nullptr,
diagindH, data);
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2));
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(diagindH, ElementsAre(0, 4, 8));
EXPECT_THAT(dense, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14));
mj_deleteData(data);
mj_deleteModel(model);
@@ -516,31 +543,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) {
int rownnzT[] = {2, 2, 0};
int rowadrT[] = {0, 2, 4};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {2, 3, 4};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0));
EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0));
EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 0, 2, 0, 0));
EXPECT_THAT(rownnzH, ElementsAre(2, 2, 1));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
@@ -564,32 +575,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3b) {
int rownnzT[] = {2, 2, 1};
int rowadrT[] = {0, 2, 4};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {1, 1, 1};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(2, 3, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 2, 5));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(26, 2, 0, 2, 13, 12, 12, 16, 0));
EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 2, 1, 2, 0));
EXPECT_THAT(rownnzH, ElementsAre(2, 3, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(diagindH, ElementsAre(0, 4, 7));
EXPECT_THAT(dense, ElementsAre(26, 2, 0, 2, 13, 12, 0, 12, 16));
mj_deleteData(data);
mj_deleteModel(model);
@@ -613,32 +607,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) {
int rownnzT[] = {2, 0, 2};
int rowadrT[] = {0, 2, 2};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {2, 3, 4};
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, diag,
diagindH, data);
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
EXPECT_THAT(rownnzH, ElementsAre(2, 0, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 2, 2));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(66, 4, 0, 0, 0, 0, 4, 35, 0));
EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0));
EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(66, 0, 4, 0, 0, 0, 4, 0, 35));
mj_deleteData(data);
mj_deleteModel(model);
@@ -662,30 +639,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) {
int rownnzT[] = {2, 1, 1};
int rowadrT[] = {0, 2, 3};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, nullptr,
diagindH, data);
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(5, 6, 4, 6, 9, 0, 4, 16, 0));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0));
EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(5, 6, 4, 6, 9, 0, 4, 0, 16));
mj_deleteData(data);
mj_deleteModel(model);
@@ -709,30 +669,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) {
int rownnzT[] = {1, 1, 2};
int rowadrT[] = {0, 1, 2};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, nullptr,
diagindH, data);
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 2, 3));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(1, 2, 0, 4, 0, 0, 2, 13, 0));
EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0));
EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(diagindH, ElementsAre(0, 3, 7));
EXPECT_THAT(dense, ElementsAre(1, 0, 2, 0, 4, 0, 2, 0, 13));
mj_deleteData(data);
mj_deleteModel(model);
@@ -756,31 +699,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) {
int rownnzT[] = {2, 2};
int rowadrT[] = {0, 2};
mjtNum matH[] = {0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0};
int rownnzH[] = {0, 0};
int rowadrH[] = {0, 0};
int diagindH[] = {0, 0};
mjtNum diag[] = {2, 3, 4};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 2, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
int diagindH[2];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 2, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(2, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 2));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 2);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 2, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(66, 4, 4, 35));
EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 1));
EXPECT_THAT(rownnzH, ElementsAre(2, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 2));
EXPECT_THAT(dense, ElementsAre(66, 4, 4, 35));
mj_deleteData(data);
mj_deleteModel(model);
@@ -803,31 +730,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) {
int rownnzT[] = {2, 1, 1};
int rowadrT[] = {0, 2, 3};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {2, 3};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 2, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 2, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(14, 18, 8, 18, 27, 0, 8, 32, 0));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0));
EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(14, 18, 8, 18, 27, 0, 8, 0, 32));
mj_deleteData(data);
mj_deleteModel(model);
@@ -851,31 +762,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) {
int rownnzT[] = {3, 3, 3};
int rowadrT[] = {0, 3, 6};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {2, 3, 4};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, nullptr, data, 1);
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, nullptr, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
nullptr, data, diagindH);
EXPECT_THAT(matH, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2));
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120));
mj_deleteData(data);
mj_deleteModel(model);
@@ -900,31 +795,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) {
int rowadrT[] = {0, 3, 6};
int rowsuperT[] = {2, 1, 0};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {1, 2, 1};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, data, 1);
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, rowsuperT, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
rowsuperT, data, diagindH);
EXPECT_THAT(matH, ElementsAre(18, 17, 14, 17, 23, 19, 14, 19, 18));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2));
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(18, 17, 14, 17, 23, 19, 14, 19, 18));
mj_deleteData(data);
mj_deleteModel(model);
@@ -949,31 +828,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) {
int rowadrT[] = {0, 1, 3};
int rowsuperT[] = {0, 1, 0};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0};
int rowadrH[] = {0, 0, 0};
int diagindH[] = {0, 0, 0};
mjtNum diag[] = {1, 1, 1};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, data, 1);
int diagindH[3];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, rowsuperT, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 3);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
rowsuperT, data, diagindH);
EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10));
EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2));
EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3));
EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6));
EXPECT_THAT(dense, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10));
mj_deleteData(data);
mj_deleteModel(model);
@@ -998,33 +861,16 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) {
int rowadrT[] = {0, 1, 2, 4};
int rowsuperT[] = {1, 0, 1, 0};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0, 0};
int rowadrH[] = {0, 0, 0, 0};
int diagindH[] = {0, 0, 0, 0};
mjtNum diag[] = {1, 1, 1};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 4, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, data, 1);
int diagindH[4];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 4, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, rowsuperT, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4));
EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 4);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 4, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
rowsuperT, data, diagindH);
EXPECT_THAT(matH,
EXPECT_THAT(dense,
ElementsAre(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 1, 1, 10, 10));
EXPECT_THAT(colindH,
ElementsAre(0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3));
EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4));
EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1049,35 +895,16 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) {
int rowadrT[] = {0, 3, 6, 6, 6};
int rowsuperT[] = {1, 0, 2, 1, 0};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0, 0, 0};
int rowadrH[] = {0, 0, 0, 0, 0};
int diagindH[] = {0, 0, 0, 0, 0};
mjtNum diag[] = {1, 1, 1};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 5, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, data, 1);
int diagindH[5];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 3, 5, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, rowsuperT, diag,
diagindH, data);
EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0, 0, 0));
EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4, 4, 4));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 5);
mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 5, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
rowsuperT, data, diagindH);
EXPECT_THAT(matH, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0));
EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 0,
3, 0, 0, 0, 0, 4, 0, 0, 0, 0));
EXPECT_THAT(rownnzH, ElementsAre(2, 2, 1, 1, 1));
EXPECT_THAT(rowadrH, ElementsAre(0, 5, 10, 15, 20));
EXPECT_THAT(dense, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1100,40 +927,305 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) {
int rowadrT[] = {0, 1, 2, 3, 4, 5, 6};
int rowsuperT[] = {3, 2, 1, 0, 2, 1, 0};
mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int rownnzH[] = {0, 0, 0, 0, 0, 0, 0};
int rowadrH[] = {0, 0, 0, 0, 0, 0, 0};
int diagindH[] = {0, 0, 0, 0, 0, 0, 0};
// test precount
mju_sqrMatTDSparseCount(rownnzH, rowadrH, 7, rownnz, rowadr, colind,
rownnzT, rowadrT, colindT, rowsuperT, data, 1);
EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7));
EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42));
// test computation
mju_sqrMatTDUncompressedInit(rowadrH, 7);
mju_sqrMatTDSparse(matH, mat, matT, nullptr, 1, 7, rownnzH, rowadrH, colindH,
rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT,
rowsuperT, data, diagindH);
int diagindH[7];
std::vector<mjtNum> dense;
SqrMatTDSplitCol(dense, 1, 7, mat, rownnz, rowadr, colind,
matT, rownnzT, rowadrT, colindT, rowsuperT, nullptr,
diagindH, data);
EXPECT_THAT(
matH, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2,
2, 2, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 4, 4, 4, 2, 2, 2,
2, 4, 4, 4, 2, 2, 2, 2, 4, 4, 4));
EXPECT_THAT(colindH,
ElementsAre(0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3,
4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 0,
1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6));
dense, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1,
1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 4, 4,
4, 2, 2, 2, 2, 4, 4, 4, 2, 2, 2, 2, 4, 4, 4));
EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7));
EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) {
// Simple dense 2x2 matrix:
// 1 2
// M = 3 4
//
// M'M (lower triangle) should have 3 elements: (0,0), (1,0), (1,1)
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* data = mj_makeData(model);
// M in CSR: row 0 has cols 0,1; row 1 has cols 0,1
int colind[] = {0, 1, 0, 1};
int rownnz[] = {2, 2};
int rowadr[] = {0, 2};
// compute transpose using mju_transposeSparse
mjtNum mat[] = {1, 2, 3, 4};
mjtNum matT[4];
int colindT[4];
int rownnzT[2];
int rowadrT[2];
mju_transposeSparse(matT, mat, 2, 2, rownnzT, rowadrT, colindT, nullptr,
rownnz, rowadr, colind);
// use old function as ground truth
int rownnzH_expected[] = {0, 0};
int rowadrH_expected[] = {0, 0};
int nnz_expected = mju_sqrMatTDSparseCount(
rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT,
rowadrT, colindT, nullptr, data, /*flg_upper=*/0);
// verify: lower triangle should have 3 elements: (0,0), (1,0), (1,1)
EXPECT_EQ(nnz_expected, 3);
EXPECT_THAT(rownnzH_expected, ElementsAre(1, 2));
EXPECT_THAT(rowadrH_expected, ElementsAre(0, 1));
// test count mode of new function
int rownnzH[] = {0, 0};
int rowadrH[] = {0, 0};
int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr,
2, 2, rownnz, rowadr, colind, rownnzT,
rowadrT, colindT, nullptr, data);
EXPECT_EQ(nnz, nnz_expected);
EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1]));
EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1]));
// test fill mode
std::vector<int> colindH(nnz, -1);
mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), nullptr, 2, 2,
rownnz, rowadr, colind, rownnzT, rowadrT,
colindT, nullptr, data);
// verify: row 0 should have {0}, row 1 should have {0, 1}
EXPECT_THAT(colindH, ElementsAre(0, 0, 1));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicUpper) {
// Test flg_upper=1: count both lower and upper triangle
// Same matrix as previous test
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* data = mj_makeData(model);
int colind[] = {0, 1, 0, 1};
int rownnz[] = {2, 2};
int rowadr[] = {0, 2};
mjtNum mat[] = {1, 2, 3, 4};
mjtNum matT[4];
int colindT[4];
int rownnzT[2];
int rowadrT[2];
mju_transposeSparse(matT, mat, 2, 2, rownnzT, rowadrT, colindT, nullptr,
rownnz, rowadr, colind);
// use old function as ground truth with flg_upper=1
int rownnzH_expected[] = {0, 0};
int rowadrH_expected[] = {0, 0};
int nnz_expected = mju_sqrMatTDSparseCount(
rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT,
rowadrT, colindT, nullptr, data, /*flg_upper=*/1);
// test new function with diagind (upper triangle)
int rownnzH[] = {0, 0};
int rowadrH[] = {0, 0};
int diagindH[] = {0, 0};
int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, diagindH,
2, 2, rownnz, rowadr, colind, rownnzT,
rowadrT, colindT, nullptr, data);
EXPECT_EQ(nnz, nnz_expected);
EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1]));
EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1]));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) {
// Test supernode exploitation with a matrix that has supernodes
// M has two rows with identical sparsity pattern
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* data = mj_makeData(model);
// 3x2 matrix where rows 1 and 2 have same pattern
// 1 0
// M = 2 3
// 4 5
int colind[] = {0, 0, 1, 0, 1};
int rownnz[] = {1, 2, 2};
int rowadr[] = {0, 1, 3};
mjtNum mat[] = {1, 2, 3, 4, 5};
mjtNum matT[5];
int colindT[5];
int rownnzT[2];
int rowadrT[2];
mju_transposeSparse(matT, mat, 3, 2, rownnzT, rowadrT, colindT, nullptr,
rownnz, rowadr, colind);
// compute rowsuperT
int rowsuperT[2];
mju_superSparse(2, rowsuperT, rownnzT, rowadrT, colindT);
// use old function as ground truth
int rownnzH_expected[] = {0, 0};
int rowadrH_expected[] = {0, 0};
int nnz_expected = mju_sqrMatTDSparseCount(
rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT,
rowadrT, colindT, rowsuperT, data, /*flg_upper=*/0);
// test new function with supernodes
int rownnzH[] = {0, 0};
int rowadrH[] = {0, 0};
int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr,
3, 2, rownnz, rowadr, colind, rownnzT,
rowadrT, colindT, rowsuperT, data);
EXPECT_EQ(nnz, nnz_expected);
EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1]));
EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1]));
// test fill mode with supernodes
std::vector<int> colindH(nnz, -1);
mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), nullptr, 3, 2,
rownnz, rowadr, colind, rownnzT, rowadrT,
colindT, rowsuperT, data);
// verify all filled
for (int i = 0; i < nnz; i++) {
EXPECT_GE(colindH[i], 0) << "colindH[" << i << "] not filled";
}
// verify numeric phase with supernodes
std::vector<mjtNum> resH(nnz);
mjtNum diag[] = {1, 1, 1, 1, 1}; // dummy diagonal
mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH, colindH.data(),
nullptr, mat, rownnz, rowadr, colind, matT, rownnzT,
rowadrT, colindT, rowsuperT, diag, data);
// ground truth numeric
std::vector<mjtNum> res_expected(4);
std::vector<int> colindH_expected(4);
int rownnzH_exp[] = {0, 0};
int rowadrH_exp[] = {0, 2};
mju_sqrMatTDSparse(res_expected.data(), mat, matT, diag, 3, 2, rownnzH_exp,
rowadrH_exp, colindH_expected.data(), rownnz, rowadr,
colind, nullptr, rownnzT, rowadrT, colindT, rowsuperT,
data, nullptr);
// compare values (sparse result vs sparse ground truth)
for (int r = 0; r < 2; r++) {
for (int i = 0; i < rownnzH[r]; i++) {
// find matching col in ground truth
int c = colindH[rowadrH[r] + i];
mjtNum val = resH[rowadrH[r] + i];
bool found = false;
for (int j = 0; j < rownnzH_exp[r]; j++) {
if (colindH_expected[rowadrH_exp[r] + j] == c) {
EXPECT_NEAR(val, res_expected[rowadrH_exp[r] + j], 1e-14);
found = true;
break;
}
}
EXPECT_TRUE(found) << "Column " << c
<< " not found in ground truth for row " << r;
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) {
// Test numeric phase using symbolic phase + existing function as ground truth
// 1 2
// M = 3 4
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* data = mj_makeData(model);
int colind[] = {0, 1, 0, 1};
int rownnz[] = {2, 2};
int rowadr[] = {0, 2};
mjtNum mat[] = {1, 2, 3, 4};
// compute transpose
mjtNum matT[4];
int colindT[4];
int rownnzT[2];
int rowadrT[2];
mju_transposeSparse(matT, mat, 2, 2, rownnzT, rowadrT, colindT, nullptr,
rownnz, rowadr, colind);
// compute supernodes
int rowsuperT[2];
mju_superSparse(2, rowsuperT, rownnzT, rowadrT, colindT);
mjtNum diag[] = {2, 3}; // diagonal weighting matrix
// test both diagind cases: lower-only (diagind=NULL) and both triangles
// (diagind!=NULL)
for (int use_diagind = 0; use_diagind <= 1; use_diagind++) {
// compute sparsity pattern using symbolic phase
int rownnzH[] = {0, 0};
int rowadrH[] = {0, 0};
int diagindH[] = {0, 0};
int nnz = mju_sqrMatTDSparseSymbolic(
rownnzH, rowadrH, nullptr, use_diagind ? diagindH : nullptr, 2, 2,
rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data);
std::vector<int> colindH(nnz);
mju_sqrMatTDSparseSymbolic(
rownnzH, rowadrH, colindH.data(), use_diagind ? diagindH : nullptr, 2,
2, rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data);
// compute values using numeric phase
std::vector<mjtNum> resH(nnz);
mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH,
colindH.data(), use_diagind ? diagindH : nullptr,
mat, rownnz, rowadr, colind, matT, rownnzT,
rowadrT, colindT, rowsuperT, diag, data);
// compute ground truth using existing mju_sqrMatTDSparse
// use uncompressed storage to give the old function enough room
std::vector<mjtNum> res_expected(4); // 2x2 uncompressed
std::vector<int> colindH_expected(4);
int rownnzH_exp[] = {0, 0};
int rowadrH_exp[] = {0, 2};
int diagind_exp[] = {0, 0};
mju_sqrMatTDSparse(res_expected.data(), mat, matT, diag, 2, 2, rownnzH_exp,
rowadrH_exp, colindH_expected.data(), rownnz, rowadr,
colind, nullptr, rownnzT, rowadrT, colindT, nullptr,
data, use_diagind ? diagind_exp : nullptr);
// check that rownnz matches (nnz may differ due to compressed vs
// uncompressed storage)
EXPECT_EQ(rownnzH[0], rownnzH_exp[0])
<< "rownnz[0] mismatch for use_diagind=" << use_diagind;
EXPECT_EQ(rownnzH[1], rownnzH_exp[1])
<< "rownnz[1] mismatch for use_diagind=" << use_diagind;
// compare column indices and values for each row
for (int r = 0; r < 2; r++) {
for (int j = 0; j < rownnzH[r]; j++) {
int idx = rowadrH[r] + j;
int idx_exp = rowadrH_exp[r] + j;
EXPECT_EQ(colindH[idx], colindH_expected[idx_exp])
<< "colind mismatch at row " << r << " pos " << j
<< " for use_diagind=" << use_diagind;
EXPECT_NEAR(resH[idx], res_expected[idx_exp], 1e-10)
<< "value mismatch at row " << r << " pos " << j
<< " for use_diagind=" << use_diagind;
}
}
}
mj_deleteData(data);
mj_deleteModel(model);
test/engine/engine_vis_visualize_test.cc
-1
View File
@@ -101,7 +101,6 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) {
mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_);
EXPECT_EQ(scn_.status, 1);
EXPECT_EQ(scn_.ngeom, maxgeoms);
EXPECT_EQ(data->warning[mjWARN_VGEOMFULL].number, 1);
mj_deleteData(data);
FreeSceneObjects();
test/engine/testdata/collision_box/boxbox_bad0.xml
Vendored
+1 -1
View File
@@ -2,7 +2,7 @@
<!-- This model leads to bad contacts from mjc_BoxBox -->
<default>
<geom rgba="1 1 1 1" margin="1e-3" gap="1e-3"/>
<geom rgba="1 1 1 1" gap="1e-3"/>
<site type="sphere" rgba="0 0 0 0" size="0.001"/>
</default>
test/engine/testdata/collision_primitive/sphere_cylinder.xml
Vendored
+1 -1
View File
@@ -16,7 +16,7 @@
<body pos="0 0 .3">
<freejoint/>
<geom type="sphere" size=".05" rgba=".5 .5 .5 .5" margin="0.1" gap="0.1"/>
<geom type="sphere" size=".05" rgba=".5 .5 .5 .5" gap="0.1"/>
</body>
<body pos="-.5 0 .3">
test/engine/testdata/core_constraint/jdotv_connect_2d.xml
Vendored
+29
View File
@@ -0,0 +1,29 @@
<mujoco>
<option>
<flag contact="disable"/>
</option>
<worldbody>
<light pos="0 -1 1"/>
<!-- body 1: pendulum on world hinge, horizontal pointing right -->
<body name="upper" pos="0 0 1">
<joint name="hinge1" type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.03" fromto="0 0 0 0.5 0 0"/>
<site name="tip1" pos="0.5 0 0"/>
</body>
<!-- body 2: planar free body (slide x, slide z, hinge y) -->
<body name="lower" pos="0.5 0 1" euler="0 -90 0">
<joint type="slide" axis="1 0 0"/>
<joint type="slide" axis="0 0 1"/>
<joint type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.04" fromto="0 0 0 0.4 0 0"/>
<site name="tip2" pos="0 0 0"/>
</body>
</worldbody>
<equality>
<connect body1="upper" body2="lower" anchor="0.5 0 0"/>
</equality>
</mujoco>
test/engine/testdata/core_constraint/jdotv_connect_3d.xml
Vendored
+29
View File
@@ -0,0 +1,29 @@
<mujoco>
<option integrator="implicit">
<flag contact="disable"/>
</option>
<worldbody>
<light pos="0 -1 1"/>
<!-- body 1: pendulum on world ball joint, tilted -->
<body name="upper">
<joint type="ball"/>
<geom type="box" size="0.03" fromto="0 0 0 0.5 0 0"/>
<geom type="box" size="0.03" fromto="0.5 0 0 0.5 0.3 0"/>
<site name="tip1" pos="0.5 0.3 0"/>
</body>
<!-- body 2: free body, positioned at tip of upper -->
<body name="lower" pos="0.5 0.3 0">
<freejoint/>
<geom type="box" size="0.04" fromto="0 0 0 0.5 0 0"/>
<geom type="box" size="0.04" fromto="0.5 0 0 0.5 0.3 0"/>
<site name="tip2" pos="0 0 0"/>
</body>
</worldbody>
<equality>
<connect site1="tip1" site2="tip2"/>
</equality>
</mujoco>
test/engine/testdata/core_constraint/jdotv_weld_3d.xml
Vendored
+29
View File
@@ -0,0 +1,29 @@
<mujoco>
<option integrator="implicit">
<flag contact="disable"/>
</option>
<worldbody>
<light pos="0 -1 1"/>
<!-- body 1: pendulum on world ball joint, tilted -->
<body name="upper">
<joint type="ball"/>
<geom type="box" size="0.03" fromto="0 0 0 0.5 0 0"/>
<geom type="box" size="0.03" fromto="0.5 0 0 0.5 0.3 0"/>
<site name="tip1" pos="0.5 0.3 0"/>
</body>
<!-- body 2: free body, positioned at tip of upper -->
<body name="lower" pos="0.5 0.3 0">
<freejoint/>
<geom type="box" size="0.04" fromto="0 0 0 0.5 0 0"/>
<geom type="box" size="0.04" fromto="0.5 0 0 0.5 0.3 0"/>
<site name="tip2" pos="0 0 0"/>
</body>
</worldbody>
<equality>
<weld site1="tip1" site2="tip2" solimp="0 0.96 0.01" torquescale="0.1"/>
</equality>
</mujoco>
test/engine/testdata/derivative/dcmotor.xml
Vendored
+61
View File
@@ -0,0 +1,61 @@
<mujoco>
<worldbody>
<body name="motor1" pos="0 0.1 0">
<joint name="slide1" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor2" pos="0 0.2 0">
<joint name="slide2" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor3" pos="0 0.3 0">
<joint name="slide3" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor4" pos="0 0.4 0">
<joint name="joint4"/>
<geom size=".03"/>
</body>
<body name="motor5" pos="0 0.5 0">
<joint name="slide5" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor6" pos="0 0.6 0">
<joint name="slide6" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor7" pos="0 0.7 0">
<joint name="slide7" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
</worldbody>
<actuator>
<!-- DC motor back-EMF stateless damping -->
<dcmotor name="dc_bias" joint="slide1" motorconst="2.0" resistance="0.5"/>
<!-- DC motor velocity controller damping -->
<dcmotor name="dc_vel" joint="slide2" motorconst="1.0" resistance="1.0" input="velocity" controller="0 5"/>
<!-- DC motor position controller damping -->
<dcmotor name="dc_pos" joint="slide3" motorconst="1.0" resistance="1.0" input="position" controller="10 0 5"/>
<!-- DC motor with LuGre friction (sigma1 micro-damping) -->
<dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0"
damping="0.001" lugre="1e4 100 0.005 0.008 0.1"/>
<!-- Stateful current, voltage mode (back-EMF only through act_dot) -->
<dcmotor name="dc_stateful_v" joint="slide5"
motorconst="2.0" resistance="0.5" inductance="0 0.001"/>
<!-- Stateful current, position mode (controller + back-EMF through act_dot) -->
<dcmotor name="dc_stateful_pos" joint="slide6"
motorconst="1.0" resistance="1.0" inductance="0 0.001"
input="position" controller="10 0 5"/>
<!-- Stateful current, velocity mode -->
<dcmotor name="dc_stateful_vel" joint="slide7"
motorconst="1.0" resistance="1.0" inductance="0 0.001"
input="velocity" controller="5 0"/>
</actuator>
</mujoco>
test/engine/testdata/solver/model.xml
Vendored
+1 -1
View File
@@ -43,7 +43,7 @@
<worldbody>
<light pos="0 0 3"/>
<geom name="floor" type="plane" size="4 4 .1" margin="0.01" gap="0.005"/>
<geom name="floor" type="plane" size="4 4 .1" margin="0.005" gap="0.005"/>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05" rgba="0 0 1 1"/>
<body name="head" pos="0 0 .7" gravcomp="0.5">
<geom type="ellipsoid" size=".2 .2 .4" density="200" material="grid"/>
test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc
+2 -2
View File
@@ -172,6 +172,8 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) {
mjDSBL_EULERDAMP);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(AutoResetFlag,
mjDSBL_AUTORESET);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag,
mjDSBL_MULTICCD);
EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(OverrideFlag,
mjENBL_OVERRIDE);
@@ -179,8 +181,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) {
EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FwdinvFlag, mjENBL_FWDINV);
EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(InvDiscreteFlag,
mjENBL_INVDISCRETE);
EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag,
mjENBL_MULTICCD);
mj_deleteModel(default_model);
mj_deleteSpec(empty_spec);
test/fixture.cc
+10 -3
View File
@@ -251,10 +251,17 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2,
// compare arrays, apart from bvh-related ones (which includes flex_vert0), as
// those are sensitive to numerical differences when meshes are perfectly
// symmetric.
// symmetric. Also skip flex fields derived from node local positions and
// cell geometry that are not fully serialized to XML.
#define X(type, name, nr, nc) \
if (strncmp(#name, "bvh_", 4) && strncmp(#name, "flex_vert0", 4) && \
strncmp(#name, "mesh_poly", 4)) { \
if (strncmp(#name, "bvh_", 4) && \
strncmp(#name, "flex_vert", 9) && \
strncmp(#name, "mesh_poly", 9) && \
strcmp(#name, "flex_centered") && \
strcmp(#name, "flex_size") && \
strcmp(#name, "flexedge_length0") && \
strcmp(#name, "flexedge_invweight0") && \
strncmp(#name, "flex_node", 9)) { \
for (int r = 0; r < m1->nr; r++) { \
for (int c = 0; c < nc; c++) { \
dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \
test/sample/CMakeLists.txt
-1
View File
@@ -16,5 +16,4 @@ if(MUJOCO_BUILD_EXAMPLES)
include(ShellTests)
add_mujoco_shell_test(compile_test compile)
add_mujoco_shell_test(testspeed_test testspeed)
endif()
test/sample/testspeed_test.sh
-91
View File
@@ -1,91 +0,0 @@
#!/bin/bash
# Copyright 2021 DeepMind Technologies Limited
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
MODEL_DIRS=(
"${CMAKE_SOURCE_DIR}/model"
"${CMAKE_SOURCE_DIR}/test"
)
die() { echo "$*" 1>&2 ; exit 1; }
test_model() {
local EXPECTED_STR='Simulation time'
local model="$1"
echo "Testing $model" >&2
local iterations=10
# for particularly slow models, only run 2 steps under ASAN, or skip.
if [[ ${TESTSPEED_ASAN:-0} != 0 ]]; then
if [[ "$model" == */humanoid/100_humanoids.xml ||
"$model" == */composite/particle.xml ||
"$model" == */replicate/bunnies.xml ||
"$model" == */replicate/leaves.xml ||
"$model" == */replicate/particle.xml ||
"$model" == */perf/*
]]; then
# these tests can take several minutes under ASAN
return 0
fi
if [[ "$model" == */benchmark/testdata/humanoid200.xml ||
"$model" == */engine/testdata/collision_convex/stacked_boxes.xml ||
"$model" == */user/testdata/shark_22_ascii_fTetWild.xml ||
"$model" == */user/testdata/shark_22_binary_fTetWild.xml
]]; then
iterations=2
fi
fi
# run testspeed, writing its output to stderr.
# die if testspeed returns a failure code, or if it doesn't have the string
# "Simulation time" in the output.
("$TARGET_BINARY" "$model" "$iterations" || die "testspeed failed") \
| tee >(cat 1>&2) | grep -q "$EXPECTED_STR"
if [ "$?" != 0 ]; then
die "Expected string not found in output ($EXPECTED_STR)."
fi
}
if [ -z "$TARGET_BINARY" ]; then
die "Expecting environment variable TARGET_BINARY."
fi
if [ -z "$MUJOCO_DLL_DIR" ]; then
# Extend PATH to include the directory containing the mujoco DLL.
# This is needed on Windows.
PATH=$PATH:$MUJOCO_DLL_DIR
fi
shopt -s globstar
for model_dir in ${MODEL_DIRS[@]}; do
echo "Looking in $model_dir"
for model in $model_dir/**/*.xml; do
if [[ $(basename $model) == malformed* ]]; then
echo "Skipping $model" >&2
continue
fi
if [[ $(basename $model) == *_fail.xml ]]; then
echo "Skipping $model" >&2
continue
fi
if grep -q "plugin" $model; then
continue
fi
test_model "$model"
done
done
cd $CURRENT_DIR
echo "PASS"
test/testdata/catenary.xml
Vendored
+4
View File
@@ -1,4 +1,8 @@
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<visual>
<global elevation="-20"/>
</visual>
test/testdata/delay.xml
Vendored
+3 -1
View File
@@ -1,5 +1,7 @@
<mujoco>
<option timestep="0.01"/>
<option timestep="0.01">
<flag multiccd="enable"/>
</option>
<worldbody>
<body name="body">
test/testdata/flex.xml
Vendored
+1 -1
View File
@@ -1,6 +1,6 @@
<mujoco model="flex">
<option timestep="0.01" integrator="implicitfast">
<flag island="enable"/>
<flag island="enable" multiccd="enable"/>
</option>
<asset>
test/testdata/frustum.xml
Vendored
+4
View File
@@ -2,6 +2,10 @@
The frustum should match exactly the fron face of the box. -->
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<visual>
<map znear="0.01"/>
<rgba frustum="1. 1. 0 0.2"/>
test/testdata/hammock.xml
Vendored
+3 -1
View File
@@ -22,7 +22,9 @@
Simple hammock, implemented as a 2D grid composite, pinned at the corners.
-->
<option timestep="0.001" solver="CG" iterations="30" tolerance="1e-6"/>
<option timestep="0.001" solver="CG" iterations="30" tolerance="1e-6">
<flag multiccd="enable"/>
</option>
<size memory="20M"/>
test/testdata/model.xml
Vendored
+2 -2
View File
@@ -1,6 +1,6 @@
<mujoco>
<option jacobian="dense" density="1.225" viscosity="1.8e-5" wind="0 0 1">
<flag fwdinv="enable" energy="enable" island="enable"/>
<flag fwdinv="enable" energy="enable" island="enable" multiccd="enable"/>
</option>
<asset>
@@ -43,7 +43,7 @@
<worldbody>
<light pos="0 0 3"/>
<geom name="floor" type="plane" size="4 4 .1" margin="0.01" gap="0.005"/>
<geom name="floor" type="plane" size="4 4 .1" margin="0.005" gap="0.005"/>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05" rgba="0 0 1 1"/>
<body name="head" pos="0 0 .7" gravcomp="0.5">
<geom type="ellipsoid" size=".2 .2 .4" density="200" material="grid"/>
test/testdata/tendon_wrap.xml
Vendored
+1 -1
View File
@@ -1,6 +1,6 @@
<mujoco>
<option>
<flag island="enable"/>
<flag island="enable" multiccd="enable"/>
</option>
<default>
test/user/CMakeLists.txt
+2 -9
View File
@@ -14,9 +14,6 @@
mujoco_test(
user_model_test
PROPERTIES
ENVIRONMENT
"MUJOCO_PLUGIN_DIR=$<TARGET_FILE_DIR:obj_decoder>"
ADDITIONAL_LINK_LIBRARIES absl::str_format
)
@@ -31,16 +28,10 @@ mujoco_test(
mujoco_test(
user_flex_test
PROPERTIES
ENVIRONMENT
"MUJOCO_PLUGIN_DIR=$<TARGET_FILE_DIR:obj_decoder>"
)
mujoco_test(
user_mesh_test
PROPERTIES
ENVIRONMENT
"MUJOCO_PLUGIN_DIR=$<TARGET_FILE_DIR:obj_decoder>"
ADDITIONAL_LINK_LIBRARIES absl::str_format
)
@@ -49,3 +40,5 @@ mujoco_test(user_composite_test)
mujoco_test(user_resource_test)
mujoco_test(user_vfs_test)
mujoco_test(user_util_test)
test/user/testdata/shapes.xml
Vendored
+62
View File
@@ -0,0 +1,62 @@
<mujoco>
<asset>
<texture file="test-pattern.png" type="2d"/>
<texture name="white" type="2d" builtin="flat" rgb1="1 1 1" width="2" height="2"/>
<texture name="grid" type="2d" builtin="checker" rgb1="1 1 1" rgb2="0.9 0.9 0.9" width="64" height="64"/>
<material name="pattern" texture="test-pattern"/>
<material name="floor" reflectance="0.5" texture="grid" texuniform="true"/>
<material name="metal" metallic="1.0" roughness="0.0">
<layer role="rgb" texture="white"/>
<layer role="metallic" texture="white"/>
<layer role="roughness" texture="white"/>
</material>
<mesh name="icosahedron"
vertex="0 1 1.618
0 -1 1.618
0 1 -1.618
0 -1 -1.618
1 1.618 0
-1 1.618 0
1 -1.618 0
-1 -1.618 0
1.618 0 1
1.618 0 -1
-1.618 0 1
-1.618 0 -1"/>
<hfield name="hfield" nrow="3" ncol="3" size="1 1 0.1 0.1"
elevation="1 0 1
0 1 0
1 0 1"/>
</asset>
<worldbody>
<geom name="ground" type="plane" size="12 8 0.1" pos="0 0 -1" material="floor"/>
<geom type="box" rgba="0.3 0.6 0.9 1.0" size="1 1 1" pos="-9 0 0"/>
<geom type="sphere" rgba="0.3 0.6 0.9 1.0" size="1" pos="-6 0 0"/>
<geom type="ellipsoid" rgba="0.3 0.6 0.9 1.0" size="1 2 1" pos="0 0 0"/>
<geom type="capsule" rgba="0.3 0.6 0.9 1.0" size="1 1" pos="-3 0 1"/>
<geom type="cylinder" rgba="0.3 0.6 0.9 1.0" size="1 1" pos="3 0 0"/>
<geom type="hfield" hfield="hfield" rgba="0.3 0.6 0.9 1.0" pos="6 0 -0.5"/>
<geom type="sdf" mesh="icosahedron" rgba="0.3 0.6 0.9 1.0" pos="9 0 0.809"/>
<geom type="box" material="pattern" size="1 1 1" pos="-9 5 0"/>
<geom type="sphere" material="pattern" size="1" pos="-6 5 0"/>
<geom type="ellipsoid" material="pattern" size="1 2 1" pos="0 5 0"/>
<geom type="capsule" material="pattern" size="1 1" pos="-3 5 1"/>
<geom type="cylinder" material="pattern" size="1 1" pos="3 5 0"/>
<geom type="hfield" hfield="hfield" material="pattern" pos="6 5 -0.5"/>
<geom type="sdf" mesh="icosahedron" material="pattern" pos="9 5 0.809"/>
<geom type="box" material="metal" size="1 1 1" pos="-9 -5 0"/>
<geom type="sphere" material="metal" size="1" pos="-6 -5 0"/>
<geom type="ellipsoid" material="metal" size="1 2 1" pos="0 -5 0"/>
<geom type="capsule" material="metal" size="1 1" pos="-3 -5 1"/>
<geom type="cylinder" material="metal" size="1 1" pos="3 -5 0"/>
<geom type="hfield" hfield="hfield" material="metal" pos="6 -5 -0.5"/>
<geom type="sdf" mesh="icosahedron" material="metal" pos="9 -5 0.809"/>
</worldbody>
</mujoco>
test/user/testdata/test-pattern.png
Vendored
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.1 KiB

test/user/user_api_test.cc
+252
View File
@@ -19,6 +19,7 @@
#include <cctype>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <filesystem> // NOLINT
#include <functional>
#include <map>
@@ -32,6 +33,7 @@
#include "src/cc/array_safety.h"
#include <mujoco/mujoco.h>
#include <mujoco/mjspec.h>
#include <mujoco/mjplugin.h>
#include "src/xml/xml_api.h"
#include "src/xml/xml_numeric_format.h"
#include "test/fixture.h"
@@ -173,6 +175,166 @@ TEST_F(MujocoTest, TreeTraversal) {
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, AttachAndChildDeletion) {
mjSpec* child_spec = mj_makeSpec();
mjsBody* child_world = mjs_findBody(child_spec, "world");
mjsBody* child_body = mjs_addBody(child_world, 0);
mjsJoint* freejoint = mjs_addJoint(child_body, 0);
freejoint->type = mjJNT_FREE;
mjs_setName(freejoint->element, "child_freejoint");
mjSpec* parent_spec = mj_makeSpec();
mjsBody* parent_world = mjs_findBody(parent_spec, "world");
mjsBody* parent_body = mjs_addBody(parent_world, 0);
// Attach child spec to parent_body
mjsElement* attached =
mjs_attach(parent_body->element, child_spec->element, "pre_", "");
ASSERT_THAT(attached, NotNull());
// Delete freejoint from child_spec, should fail because it is attached
int result = mjs_delete(child_spec, freejoint->element);
EXPECT_EQ(result, -1);
// The freejoint should still be in parent_spec because deletion failed
mjsElement* found_joint =
mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint");
EXPECT_THAT(found_joint, NotNull());
mj_deleteSpec(child_spec);
mj_deleteSpec(parent_spec);
}
TEST_F(MujocoTest, OriginSpecInvariantToAttachment) {
mjSpec* child_spec = mj_makeSpec();
mjsBody* child_world = mjs_findBody(child_spec, "world");
mjsBody* child_body = mjs_addBody(child_world, 0);
mjsJoint* freejoint = mjs_addJoint(child_body, 0);
freejoint->type = mjJNT_FREE;
mjs_setName(freejoint->element, "child_freejoint");
mjSpec* parent_spec = mj_makeSpec();
mjsBody* parent_world = mjs_findBody(parent_spec, "world");
mjsBody* parent_body = mjs_addBody(parent_world, 0);
mjs_setName(parent_body->element, "parent_body");
// Attach child spec to parent_body
mjsElement* attached =
mjs_attach(parent_body->element, child_spec->element, "pre_", "");
ASSERT_THAT(attached, NotNull());
// The freejoint should still be in parent_spec because deletion failed
mjsElement* child_spec_joint =
mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint");
EXPECT_EQ(mjs_getSpec(child_spec_joint), parent_spec);
EXPECT_EQ(mjs_getOriginSpec(child_spec_joint), child_spec);
mjsElement* parent_spec_body =
mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body");
EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec);
mj_deleteSpec(child_spec);
mj_deleteSpec(parent_spec);
}
int open_mock(mjResource* resource) {
static const char parent_xml[] = R"(
<mujoco>
<worldbody>
<body name="parent_body"/>
</worldbody>
</mujoco>
)";
resource->data = mju_malloc(sizeof(parent_xml));
std::strcpy((char*)resource->data, parent_xml);
return 1;
}
int read_mock(mjResource* resource, const void** buffer) {
*buffer = resource->data;
return std::strlen((const char*)resource->data);
}
void close_mock(mjResource* resource) {
mju_free(resource->data);
resource->data = nullptr;
}
TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) {
// This test checks that when we attach a child spec to a parent spec that was
// loaded from a resource provider, the child spec does not inherit the
// resource URI from the parent. This allows the child spec to specify assets
// relative to its model file or in the VFS.
mjpResourceProvider provider = {
.prefix = "fakeprovider",
.open = open_mock,
.read = read_mock,
.close = close_mock,
};
mjp_registerResourceProvider(&provider);
std::array<char, 1024> err;
mjSpec* parent_spec =
mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size());
mjs_setString(parent_spec->modelname, "parent");
ASSERT_THAT(parent_spec, NotNull()) << err.data();
// Create child spec
static constexpr char child_xml[] = R"(
<mujoco>
<worldbody>
<body name="child_body">
<geom type="mesh" mesh="asset"/>
</body>
</worldbody>
<asset>
<mesh name="asset" file="asset.obj"/>
</asset>
</mujoco>
)";
// Setup VFS with asset
mjVFS vfs;
mj_defaultVFS(&vfs);
static constexpr char asset_data[] = R"(
v 0 0 0
v 1 0 0
v 0 1 0
v 0 0 1
f 1 2 3
f 1 2 4
f 2 3 4
f 3 1 4
)";
mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data));
mjSpec* child_spec =
mj_parseXMLString(child_xml, &vfs, err.data(), err.size());
mjs_setString(child_spec->modelname, "child");
ASSERT_THAT(child_spec, NotNull()) << err.data();
// Attach child spec to parent spec's world body
mjsBody* world = mjs_findBody(parent_spec, "world");
ASSERT_THAT(world, NotNull());
mjsElement* attached =
mjs_attach(world->element, child_spec->element, "", "");
ASSERT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent_spec, &vfs);
mj_deleteVFS(&vfs);
EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec);
if (model) {
mj_deleteModel(model);
}
mj_deleteSpec(parent_spec);
mj_deleteSpec(child_spec);
}
TEST_F(MujocoTest, ActivatePlugin) {
mjSpec* spec = mj_makeSpec();
mjs_activatePlugin(spec, "mujoco.elasticity.cable");
@@ -239,6 +401,96 @@ TEST_F(MujocoTest, DeletePlugin) {
mj_deleteModel(newmodel);
}
TEST_F(MujocoTest, SetToDCMotorNullable) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
nullptr, nullptr, nullptr,
nullptr, nullptr, nullptr,
nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0);
EXPECT_EQ(actuator->gainprm[1], 0.05);
EXPECT_EQ(actuator->gainprm[4], 0);
EXPECT_EQ(actuator->gainprm[5], 0);
EXPECT_EQ(actuator->gainprm[6], 0);
EXPECT_EQ(actuator->dynprm[7], 0);
EXPECT_EQ(actuator->dynprm[8], 0);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorDeriveKe) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double resistance = 2.0;
double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100
const char* err = mjs_setToDCMotor(actuator, nullptr, resistance,
nominal, nullptr, nullptr,
nullptr, nullptr, nullptr,
nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0);
EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorFull) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
double saturation[3] = {1.0, 2.0, 3.0};
double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
nullptr, saturation, nullptr,
nullptr, controller, nullptr,
nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance
EXPECT_EQ(actuator->gainprm[1], 0.05); // K
EXPECT_EQ(actuator->gainprm[4], 10.0); // kp
EXPECT_EQ(actuator->gainprm[5], 20.0); // ki
EXPECT_EQ(actuator->gainprm[6], 30.0); // kd
EXPECT_EQ(actuator->dynprm[7], 40.0); // slewmax
EXPECT_EQ(actuator->dynprm[8], 50.0); // Imax
EXPECT_EQ(actuator->gainprm[7], 60.0); // Vmax
EXPECT_EQ(actuator->dynprm[1], 3.0); // (di/dt)_max
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorLuGre) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
nullptr, nullptr, nullptr,
nullptr, nullptr, nullptr,
lugre, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness
EXPECT_EQ(actuator->dynprm[6], 1.0); // damping
EXPECT_EQ(actuator->biasprm[3], 0.5); // coulomb
EXPECT_EQ(actuator->biasprm[4], 0.7); // static
EXPECT_EQ(actuator->biasprm[5], 10.0); // stribeck
mj_deleteSpec(spec);
}
static constexpr char xml_plugin_1[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
test/user/user_flex_test.cc
+398 -6
View File
@@ -79,6 +79,27 @@ TEST_F(UserFlexTest, CountTooSmall) {
EXPECT_THAT(error.data(), HasSubstr("Count too small"));
}
TEST_F(UserFlexTest, CellnumZeroInterpolated) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="b0"/>
<body name="b1"/>
<body name="b2"/>
<body name="b3"/>
</worldbody>
<deformable>
<flex name="test" cellcount="2 2 0" dof="trilinear"
dim="3" body="b0 b1 b2 b3" element="0 1 2 3"/>
</deformable>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull());
EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0"));
}
TEST_F(UserFlexTest, SpacingGreaterThanGeometry) {
static constexpr char xml[] = R"(
<mujoco>
@@ -371,9 +392,9 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
EXPECT_EQ(m1->nflexvert, m2->nflexvert);
for (int i = 0; i < 3*m1->nflexvert; ++i) {
EXPECT_EQ(m1->flex_vert[i], d2->flexvert_xpos[i]);
EXPECT_EQ(m1->flex_vert0[i], m2->flex_vert0[i]);
EXPECT_EQ(d1->flexvert_xpos[i], d2->flexvert_xpos[i]);
EXPECT_NEAR(m1->flex_vert[i], d2->flexvert_xpos[i], 1e-7);
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-7);
EXPECT_NEAR(d1->flexvert_xpos[i], d2->flexvert_xpos[i], 1e-7);
}
EXPECT_EQ(m1->nM, m2->nM);
@@ -447,7 +468,7 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_NE(m->flex_stiffness[0], 0);
EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0);
EXPECT_EQ(m->nflexnode, 8);
// constants are in the kernel
@@ -456,7 +477,8 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
zeros[i] = 0;
ones[i] = 1;
}
mju_mulMatVec(res, m->flex_stiffness, ones, 3*m->nflexnode, 3*m->nflexnode);
mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones,
3 * m->nflexnode, 3 * m->nflexnode);
EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros));
mj_deleteModel(m);
@@ -496,7 +518,8 @@ TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
// Same number of nodes but different stiffness due to different geometry
EXPECT_EQ(m_small->nflexnode, m_large->nflexnode);
EXPECT_NE(m_small->flex_stiffness[0], m_large->flex_stiffness[0]);
EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]],
m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]);
mj_deleteModel(m_small);
mj_deleteModel(m_large);
@@ -1008,5 +1031,374 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) {
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellNodePinning) {
// A 2x2x2 grid with a box mesh that fills all cells.
// No nodes should be pinned.
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="box" spacing=".1 .1 .1" dim="3"
dof="trilinear" mass="1" cellcount="2 2 2">
<contact selfcollide="none"/>
<elasticity young="1"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
EXPECT_EQ(nnode, 27);
// All cells are occupied by the box, so no node should be pinned.
int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) {
int bid = m->flex_nodebodyid[n];
if (m->body_jntnum[bid] == 0) {
pinned++;
}
}
EXPECT_EQ(pinned, 0);
// Verify simulation works
mjData* d = mj_makeData(m);
for (int i = 0; i < 10; i++) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
// Load bunny_multicell.xml which has a 3x3x3 grid.
// The bunny mesh only occupies some cells, so many nodes should be pinned.
const std::string xml_path =
GetModelPath("flex/bunny_multicell.xml");
std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// 3x3x3 grid, order=1: (3+1)^3 = 64 node positions
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
EXPECT_EQ(nnode, 64);
// Count pinned nodes (no joints)
int pinned = 0;
int free_nodes = 0;
for (int n = nadr; n < nadr + nnode; n++) {
int bid = m->flex_nodebodyid[n];
if (m->body_jntnum[bid] == 0) {
pinned++;
} else {
free_nodes++;
}
}
// At least some nodes should be pinned since the bunny doesn't fill all cells
EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells";
EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free";
EXPECT_EQ(pinned + free_nodes, nnode);
// Verify the model can simulate
mjData* d = mj_makeData(m);
mj_forward(m, d);
for (int i = 0; i < 10; i++) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
// Regression test for ci_min calculation with order=2.
// A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis).
// We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty.
//
// Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2).
// With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check
// cell 0 (non-empty), incorrectly marking gi=3 as non-pinned.
// Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid.
// Anchor vertex at x=1.0 extends the bounding box to [0,1]^3.
// The 3x1x1 quadratic grid splits at x=0.33, 0.67.
// Cell 0 has vertices, cells 1 and 2 are empty.
// Interior nodes for cells 1,2 should be pinned to the parent body.
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<freejoint/>
<inertial mass="0.01" pos="0 0 0"
diaginertia="0.001 0.001 0.001"/>
<flexcomp name="test" type="direct" dim="3"
dof="quadratic" mass="1" cellcount="3 1 1"
point="0.0 0.0 0.0 0.3 0.0 0.0
0.0 1.0 0.0 0.3 1.0 0.0
0.0 0.0 1.0 0.3 0.0 1.0
0.0 1.0 1.0 0.3 1.0 1.0
1.0 0.5 0.5"
element="0 1 3 2 4 5 7 6">
<contact selfcollide="none"/>
<elasticity young="1"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
EXPECT_EQ(nnode, 63);
// Count pinned nodes: pinned nodes are assigned to the parent body.
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
ASSERT_GT(parent_bid, 0);
int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) {
if (m->flex_nodebodyid[n] == parent_bid) {
pinned++;
}
}
// Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned.
// Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2.
// That's 4 * 3 * 3 = 36 nodes.
EXPECT_EQ(pinned, 36);
mj_deleteData(mj_makeData(m));
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellDetectsElements) {
// A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3.
// With cellcount="6 6 6" (216 cells), only 8 corner cells contain
// mesh vertices.
//
// Bug: MarkEmptyCells only checked vertices, so 208/216 cells are
// marked empty, causing most interior nodes to be incorrectly pinned.
// Fix: check element AABBs to correctly identify occupied cells.
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<freejoint/>
<inertial mass="0.01" pos="0.5 0.5 0.5"
diaginertia="0.001 0.001 0.001"/>
<flexcomp name="test" type="direct" dim="2"
dof="trilinear" mass="1" cellcount="6 6 6"
point="0 0 0 1 0 0 1 1 0 0 1 0
0 0 1 1 0 1 1 1 1 0 1 1"
element="0 1 2 0 2 3 4 6 5 4 7 6
0 5 1 0 4 5 2 7 3 2 6 7
0 3 7 0 7 4 1 5 6 1 6 2">
<contact selfcollide="none"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// 6x6x6 trilinear grid: (6+1)^3 = 343 nodes
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
ASSERT_EQ(nnode, 343);
// Count pinned nodes: those assigned to the parent body.
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
ASSERT_GT(parent_bid, 0);
int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) {
if (m->flex_nodebodyid[n] == parent_bid) {
pinned++;
}
}
// The cube surface fills the entire bounding box. The element-AABB
// marks all boundary cells as surface cells (152/216). The interior
// flood-fill finds no exterior seeds (all boundary cells are surface),
// so the remaining 64 cells are classified as interior (non-empty).
// No cells are empty → 0 nodes pinned.
EXPECT_EQ(pinned, 0);
mj_deleteData(mj_makeData(m));
mj_deleteModel(m);
}
TEST_F(UserFlexTest, TotalMassTrilinear) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
dim="3" dof="trilinear" mass="1.5">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
double total_mass = 0;
for (int i = 1; i < m->nbody; ++i) {
total_mass += m->body_mass[i];
}
EXPECT_NEAR(total_mass, 1.5, 1e-5);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, TotalMassQuadratic) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="3 2 2" spacing="1 1 1"
dim="3" dof="quadratic" mass="2.0">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
double total_mass = 0;
for (int i = 1; i < m->nbody; ++i) {
total_mass += m->body_mass[i];
}
EXPECT_NEAR(total_mass, 2.0, 1e-5);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, Dof2d) {
// 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18
static constexpr char xml_2d[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
dim="2" radius=".01" dof="2d">
<edge equality="true"/>
</flexcomp>
</worldbody>
</mujoco>
)";
// same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27
static constexpr char xml_full[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
dim="2" radius=".01">
<edge equality="true"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
// load 2d model
mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
ASSERT_THAT(m_2d, NotNull()) << error.data();
mjData* d_2d = mj_makeData(m_2d);
// load full model
mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size());
ASSERT_THAT(m_full, NotNull()) << error.data();
mjData* d_full = mj_makeData(m_full);
// verify DOF counts
EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs
EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs
// same number of vertices and elements
EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert);
EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem);
// each body has 2 DOFs in 2d mode, 3 in full mode
for (int i = 1; i < m_2d->nbody; i++) {
EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i;
}
for (int i = 1; i < m_full->nbody; i++) {
EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i;
}
// simulate a few steps to make sure nothing crashes
for (int i = 0; i < 10; i++) {
mj_step(m_2d, d_2d);
mj_step(m_full, d_full);
}
mj_deleteModel(m_2d);
mj_deleteModel(m_full);
mj_deleteData(d_2d);
mj_deleteData(d_full);
}
TEST_F(UserFlexTest, Vert0RotationInvariant) {
// unrotated trilinear grid
static constexpr char xml_unrotated[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
dim="3" dof="trilinear">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
// same grid rotated 45 degrees around Z via parent body quaternion
static constexpr char xml_rotated[] = R"(
<mujoco>
<worldbody>
<body name="parent" quat="0.9238795 0 0 0.3826834">
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
dim="3" dof="trilinear">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m1 = LoadModelFromString(xml_unrotated, error.data(), error.size());
ASSERT_THAT(m1, NotNull()) << error.data();
mjModel* m2 = LoadModelFromString(xml_rotated, error.data(), error.size());
ASSERT_THAT(m2, NotNull()) << error.data();
// same number of vertices
ASSERT_EQ(m1->nflexvert, m2->nflexvert);
// vert0 must be identical regardless of rotation
for (int i = 0; i < 3 * m1->nflexvert; ++i) {
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10)
<< "vert0 mismatch at index " << i;
}
mj_deleteModel(m1);
mj_deleteModel(m2);
}
} // namespace
} // namespace mujoco
test/user/user_mesh_test.cc
+129 -1
View File
@@ -713,7 +713,7 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
}
@@ -819,6 +819,73 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 1" spacing="1 1 1" dim="2" dof="2d">
<elasticity young="1" thickness="0" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(),
HasSubstr("2d elasticity requires positive thickness"));
}
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1" thickness="1" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data();
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1" thickness="1" elastic2d="both"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data();
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="2d">
<elasticity young="1" thickness="1" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex"));
}
TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
static constexpr char xml[] = R"(
<mujoco>
@@ -1318,6 +1385,63 @@ TEST_F(MjCMeshTest, QhullCache) {
mj_deleteVFS(&vfs);
}
TEST_F(MjCMeshTest, ColocatedMeshError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 0 0 0 0 0 0 0 0 0"
face="0 1 2 1 3 2"/>
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("colocated"));
}
TEST_F(MjCMeshTest, CollinearMeshError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 2 0 0 3 0 0"
face="0 1 2 1 3 2"/>
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("collinear"));
}
TEST_F(MjCMeshTest, CoplanarMeshError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="flat_quad"
vertex="-5 -5 0 5 -5 0 -5 5 0 5 5 0"
face="0 1 2 1 3 2"/>
</asset>
<worldbody>
<geom type="mesh" mesh="flat_quad"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("coplanar"));
}
TEST_F(MjCMeshTest, LoadSkin) {
const std::string xml_path = GetTestDataFilePath(kCubeSkinPath);
std::array<char, 1024> error;
@@ -1377,6 +1501,8 @@ TEST_F(MjCMeshTest, OctreeIsBalanced) {
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM));
geom->type = mjGEOM_SDF;
mjsMesh* mesh = mjs_asMesh(mjs_firstElement(spec, mjOBJ_MESH));
mesh->octree_maxdepth = 5;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
@@ -1440,6 +1566,8 @@ TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) {
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM));
geom->type = mjGEOM_SDF;
mjsMesh* mesh = mjs_asMesh(mjs_firstElement(spec, mjOBJ_MESH));
mesh->octree_maxdepth = 5;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
test/user/user_util_test.cc
+139
View File
@@ -17,6 +17,8 @@
#include "src/user/user_util.h"
#include <cerrno>
#include <cmath>
#include <random>
#include <string>
#include <vector>
@@ -180,5 +182,142 @@ TEST_F(UserUtilTest, VectorToStringEmpty) {
EXPECT_EQ(VectorToString(v), "");
}
// utility: modified Gram-Schmidt to orthogonalize columns of Q (n x n)
static void gramSchmidt(double* Q, int n) {
for (int j = 0; j < n; j++) {
// subtract projections onto previous columns
for (int k = 0; k < j; k++) {
double dot = 0;
for (int i = 0; i < n; i++) {
dot += Q[i * n + j] * Q[i * n + k];
}
for (int i = 0; i < n; i++) {
Q[i * n + j] -= dot * Q[i * n + k];
}
}
// normalize
double norm = 0;
for (int i = 0; i < n; i++) {
norm += Q[i * n + j] * Q[i * n + j];
}
norm = std::sqrt(norm);
for (int i = 0; i < n; i++) {
Q[i * n + j] /= norm;
}
}
}
// utility: compose SPD matrix A = Q * diag(eigvals) * Q^T
static void composeMatrix(double* A, const double* Q,
const double* eigvals, int n) {
for (int i = 0; i < n; i++) {
for (int j = 0; j <= i; j++) {
double sum = 0;
for (int k = 0; k < n; k++) {
sum += Q[i * n + k] * eigvals[k] * Q[j * n + k];
}
A[i * n + j] = sum;
A[j * n + i] = sum;
}
}
}
TEST_F(UserUtilTest, EigendecomposeConvergence) {
// seeded RNG for reproducibility
std::mt19937_64 rng;
rng.seed(42);
std::normal_distribution<double> dist(0, 1);
// sweep over matrix sizes used by flex stiffness
// order=1: 8 nodes * 3 dof = 24
// order=2: 27 nodes * 3 dof = 81
for (int n : {24, 81}) {
int total_sweeps = 0;
int max_sweeps = 0;
int count = 0;
// generate random orthogonal matrix Q via Gram-Schmidt
std::vector<double> Q(n * n);
for (int i = 0; i < n * n; i++) {
Q[i] = dist(rng);
}
gramSchmidt(Q.data(), n);
// sweep eigenvalue spectra of varying difficulty
// well-separated, clustered, wide condition number
for (double condition : {1e1, 1e3, 1e6}) {
for (double cluster : {0.0, 0.5, 0.9}) {
// construct eigenvalues
std::vector<double> eigvals(n);
for (int i = 0; i < n; i++) {
// base: logarithmically spaced from 1 to condition
double t = (double)i / (n - 1);
double base = std::exp(t * std::log(condition));
// cluster: push eigenvalues toward geometric mean
double mean = std::sqrt(condition);
eigvals[i] = (1 - cluster) * base + cluster * mean;
}
// compose A = Q * diag(eigvals) * Q^T
std::vector<double> A(n * n);
composeMatrix(A.data(), Q.data(), eigvals.data(), n);
// save copy for verification
std::vector<double> A_copy(A);
// decompose
std::vector<double> found_eigval(n);
std::vector<double> found_eigvec(n * n);
int sweeps = mjuu_eigendecompose(
A.data(), found_eigval.data(),
found_eigvec.data(), n);
total_sweeps += sweeps;
if (sweeps > max_sweeps) max_sweeps = sweeps;
count++;
// verify convergence
EXPECT_LT(sweeps, 200)
<< "n=" << n
<< " condition=" << condition
<< " cluster=" << cluster;
// verify A*v = lambda*v for each eigenpair
for (int i = 0; i < n; i++) {
for (int r = 0; r < n; r++) {
double Av = 0;
for (int c = 0; c < n; c++) {
Av += A_copy[r * n + c] * found_eigvec[c * n + i];
}
double lv = found_eigval[i] * found_eigvec[r * n + i];
EXPECT_NEAR(Av, lv,
1e-6 * std::abs(found_eigval[i]))
<< "n=" << n << " condition=" << condition
<< " cluster=" << cluster
<< " eigpair=" << i << " row=" << r;
}
}
// verify all eigenvalues are positive
for (int i = 0; i < n; i++) {
EXPECT_GT(found_eigval[i], 0)
<< "n=" << n << " eigenvalue " << i;
}
}
}
double mean_sweeps = (double)total_sweeps / count;
// assert reasonable average convergence
EXPECT_LE(mean_sweeps, 20.0)
<< "n=" << n << ": mean sweeps too high";
// assert max sweeps within budget
EXPECT_LT(max_sweeps, 200)
<< "n=" << n << ": max sweeps exceeded 200";
}
}
} // namespace
} // namespace mujoco
test/user/user_vfs_test.cc
+32
View File
@@ -224,6 +224,38 @@ TEST_F(UserVfsTest, DeleteFileRepeat) {
mj_deleteVFS(&vfs);
}
TEST_F(UserVfsTest, ContainsBuffer) {
mjVFS vfs;
mj_defaultVFS(&vfs);
std::string buffer = "<mujoco/>";
const void* ptr = static_cast<const void*>(buffer.c_str());
mj_addBufferVFS(&vfs, "model", ptr, buffer.size());
EXPECT_TRUE(mj_containsBufferVFS(&vfs, "model"));
EXPECT_FALSE(mj_containsBufferVFS(&vfs, "nonexistent"));
EXPECT_FALSE(mj_containsBufferVFS(&vfs, "Model"));
mj_deleteVFS(&vfs);
}
TEST_F(UserVfsTest, ContainsFile) {
mjVFS vfs;
mj_defaultVFS(&vfs);
constexpr char path[] = "engine/testdata/actuation/";
const std::string dir = GetTestDataFilePath(path);
std::string file = "activation.xml";
mj_addFileVFS(&vfs, dir.c_str(), file.c_str());
EXPECT_TRUE(mj_containsFileVFS(&vfs, dir.c_str(), file.c_str()));
EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, (dir + file).c_str()));
EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, "Activation.xml"));
EXPECT_TRUE(mj_containsFileVFS(&vfs, "some/dir/", "activation.xml"));
EXPECT_FALSE(mj_containsFileVFS(&vfs, nullptr, "nonexistent.xml"));
mj_deleteVFS(&vfs);
}
TEST_F(UserVfsTest, AddBuffer) {
mjVFS vfs;
test/xml/CMakeLists.txt
-3
View File
@@ -16,9 +16,6 @@ mujoco_test(xml_api_test)
mujoco_test(
xml_native_reader_test
PROPERTIES
ENVIRONMENT
"MUJOCO_PLUGIN_DIR=$<TARGET_FILE_DIR:obj_decoder>"
)
mujoco_test(xml_utils_test)
test/xml/mjz/CMakeLists.txt
+19
View File
@@ -0,0 +1,19 @@
# Copyright 2021 DeepMind Technologies Limited
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
mujoco_test(mjz_api_test)
mujoco_test(
mjz_decoder_test
)
test/experimental/mjz/mjz_decoder_test.cc → test/xml/mjz/mjz_decoder_test.cc
View File
test/xml/xml_native_reader_test.cc
+420
View File
@@ -3003,6 +3003,426 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) {
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorBasicParsing) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0" damping="1 2 3" armature="0.1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_dyntype[0], mjDYN_DCMOTOR);
EXPECT_EQ(model->actuator_gaintype[0], mjGAIN_DCMOTOR);
EXPECT_EQ(model->actuator_biastype[0], mjBIAS_DCMOTOR);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05);
EXPECT_MJTNUM_EQ(model->actuator_damping[0], 1.0);
EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[0], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[1], 3.0);
EXPECT_MJTNUM_EQ(model->actuator_armature[0], 0.1);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorNominalDerivation) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<!-- no damping: Ke = vn/omega0 -->
<dcmotor joint="jnt" nominal="12 0.6 600"/>
<!-- B > 0, R given: quadratic -->
<dcmotor joint="jnt" nominal="12 0 600" resistance="0.4" damping="0.0001"/>
<!-- B > 0, R from nominal: linear -->
<dcmotor joint="jnt" nominal="12 0.6 600" damping="0.0001"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
// actuator 0: B = 0, Ke = vn/omega0
{
double K = 12.0 / 600.0;
double R = K * 12.0 / 0.6;
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0*mjNGAIN + 0], R);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0*mjNGAIN + 1], K);
}
// actuator 1: B > 0, R given, quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0
{
double B = 0.0001, R = 0.4, vn = 12.0, omega0 = 600.0;
double disc = vn*vn - 4*R*B*omega0*omega0;
double Ke = (vn + sqrt(disc)) / (2*omega0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1*mjNGAIN + 0], R);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1*mjNGAIN + 1], Ke);
}
// actuator 2: B > 0, R from nominal, Ke = vn/omega0 - vn*B/tau0
{
double B = 0.0001, vn = 12.0, tau0 = 0.6, omega0 = 600.0;
double Ke = vn / omega0 - vn*B / tau0;
double R = Ke * vn / tau0;
EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 0], R);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 1], Ke);
}
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorSaturation) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
saturation="1.5 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_forcelimited[0], 1);
EXPECT_MJTNUM_EQ(model->actuator_forcerange[0], -1.5);
EXPECT_MJTNUM_EQ(model->actuator_forcerange[1], 1.5);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<dcmotor motorconst="1.0" resistance="1.0" controller="2.0 0.5 0.1 10.0 5.0 12.0"
saturation="0 0 0" inductance="0 0.01" input="velocity"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
// check motorconst and resistance are overridden by instance
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0);
// check controller gains (kp, ki, kd) in gainprm[4:6]
EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 0.5);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[6], 0.1);
// check controller limits (slewmax, Imax) in dynprm[7,8]
EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 10.0);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0);
// check Vmax in gainprm[7]
EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 12.0);
// check input mode in gainprm[8]
EXPECT_MJTNUM_EQ(model->actuator_gainprm[8], 2.0);
// check inductance (te) in dynprm[0]
EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorControllerFull) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
controller="1.0 2.0 3.0 4.0 5.0 6.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 1.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[6], 3.0);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 4.0);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 6.0);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0" damping="0.01"
lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[6], 1);
EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01);
EXPECT_MJTNUM_EQ(model->actuator_biasprm[3], 0.5);
EXPECT_MJTNUM_EQ(model->actuator_biasprm[4], 0.7);
EXPECT_MJTNUM_EQ(model->actuator_biasprm[5], 10);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorLuGreInheritedDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<dcmotor motorconst="0.05" resistance="2.0" damping="0.01" lugre="100 1 0.5 0.7 10"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorActdimStateless) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_actnum[0], 0);
EXPECT_EQ(model->actuator_actadr[0], -1);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorActdimCurrentOnly) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
inductance="0.001 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_actnum[0], 1);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.001 / 2.0);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorActdimThermalOnly) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
thermal="10 5 0 0 0 25"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_actnum[0], 1);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[2], 10);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[3], 5);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[4], 25);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_actnum[0], 1);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorActdimAllThree) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
inductance="0.001 0"
thermal="10 5 0 0 0 25"
lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_actnum[0], 3);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorMissingKError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("motor constant K must be positive"));
}
TEST_F(ActuatorParseTest, DCMotorDefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<dcmotor motorconst="0.03" resistance="1.5"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 1.5);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.03);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorMotorconstGeometricMean) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.03 0.05" resistance="2.0"/>
<dcmotor joint="jnt" motorconst="0.03" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
double K = std::sqrt(0.03 * 0.05);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], K);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[mjNGAIN + 1], 0.03);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
test/xml/xml_native_writer_test.cc
+4 -1
View File
@@ -1404,7 +1404,10 @@ std::vector<std::string> GetWriteReadTestModels() {
absl::StrContains(xml, "hfield_xml") ||
absl::StrContains(xml, "fromto_convex") ||
absl::StrContains(xml, "cube_skin") ||
absl::StrContains(xml, "cube_3x3x3")) {
absl::StrContains(xml, "cube_3x3x3") ||
// exclude files that fail since we do not save pinned flex nodes
absl::StrContains(xml, "gripper_trilinear") ||
absl::StrContains(xml, "strain")) {
continue;
}
models.push_back(xml);