Remove legacy factorI/solveLD and clean up benchmarks/tests
Deletes mj_factorI_legacy and mj_solveLD_legacy from the engine and headers. Updates factorI, solveLD, and inertia benchmarks to remove legacy targets and only benchmark CSR. Rewrites engine_core_smooth_test to verify CSR solver against mj_mulM instead of legacy solver. PiperOrigin-RevId: 942507341 Change-Id: I4281decb7018cfa3e46cb446efd5fe1179f6ae1f
This commit is contained in:
committed by
Copybara-Service
parent
10d6c01dce
commit
7c6f519879
@@ -1876,69 +1876,6 @@ void mj_makeM(const mjModel* m, mjData* d) {
|
||||
}
|
||||
|
||||
|
||||
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
|
||||
// (legacy implementation)
|
||||
void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD,
|
||||
mjtNum* qLDiagInv) {
|
||||
int cnt;
|
||||
int Madr_kk, Madr_ki;
|
||||
mjtNum tmp;
|
||||
|
||||
// local copies of key variables
|
||||
int* dof_Madr = m->dof_Madr;
|
||||
int* dof_parentid = m->dof_parentid;
|
||||
int nv = m->nv;
|
||||
|
||||
// copy M into LD
|
||||
mju_copy(qLD, M, m->nM);
|
||||
|
||||
// dense backward loop over dofs (regular only, simple diagonal already copied)
|
||||
for (int k=nv-1; k >= 0; k--) {
|
||||
// get address of M(k,k)
|
||||
Madr_kk = dof_Madr[k];
|
||||
|
||||
// check for small/negative numbers on diagonal
|
||||
if (qLD[Madr_kk] < mjMINVAL) {
|
||||
mj_warning(d, mjWARN_INERTIA, k);
|
||||
qLD[Madr_kk] = mjMINVAL;
|
||||
}
|
||||
|
||||
// skip the rest if simple
|
||||
if (m->dof_simplenum[k]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// sparse backward loop over ancestors of k (excluding k)
|
||||
Madr_ki = Madr_kk + 1;
|
||||
int i = dof_parentid[k];
|
||||
while (i >= 0) {
|
||||
tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k)
|
||||
|
||||
// get number of ancestors of i (including i)
|
||||
if (i < nv-1) {
|
||||
cnt = dof_Madr[i+1] - dof_Madr[i];
|
||||
} else {
|
||||
cnt = m->nM - dof_Madr[i+1];
|
||||
}
|
||||
|
||||
// M(i,j) -= M(k,j) * tmp
|
||||
mju_addToScl(qLD+dof_Madr[i], qLD+Madr_ki, -tmp, cnt);
|
||||
|
||||
qLD[Madr_ki] = tmp; // M(k,i) = tmp
|
||||
|
||||
// advance to i's parent
|
||||
i = dof_parentid[i];
|
||||
Madr_ki++;
|
||||
}
|
||||
}
|
||||
|
||||
// compute 1/diag(D)
|
||||
for (int i=0; i < nv; i++) {
|
||||
qLDiagInv[i] = 1.0 / qLD[dof_Madr[i]];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
|
||||
void mj_factorM(const mjModel* m, mjData* d) {
|
||||
TM_START;
|
||||
@@ -1997,118 +1934,6 @@ void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
|
||||
}
|
||||
|
||||
|
||||
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
|
||||
// (legacy implementation)
|
||||
void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n,
|
||||
const mjtNum* qLD, const mjtNum* qLDiagInv) {
|
||||
// local copies of key variables
|
||||
int* dof_Madr = m->dof_Madr;
|
||||
int* dof_parentid = m->dof_parentid;
|
||||
int nv = m->nv;
|
||||
|
||||
// single vector
|
||||
if (n == 1) {
|
||||
// x <- inv(L') * x; skip simple, exploit sparsity of input vector
|
||||
for (int i=nv-1; i >= 0; i--) {
|
||||
if (!m->dof_simplenum[i] && x[i]) {
|
||||
// init
|
||||
int Madr_ij = dof_Madr[i]+1;
|
||||
int j = dof_parentid[i];
|
||||
|
||||
// traverse ancestors backwards
|
||||
// read directly from x[i] since i cannot be a parent of itself
|
||||
while (j >= 0) {
|
||||
x[j] -= qLD[Madr_ij++]*x[i]; // x(j) -= L(i,j) * x(i)
|
||||
|
||||
// advance to parent
|
||||
j = dof_parentid[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// x <- inv(D) * x
|
||||
for (int i=0; i < nv; i++) {
|
||||
x[i] *= qLDiagInv[i]; // x(i) /= L(i,i)
|
||||
}
|
||||
|
||||
// x <- inv(L) * x; skip simple
|
||||
for (int i=0; i < nv; i++) {
|
||||
if (!m->dof_simplenum[i]) {
|
||||
// init
|
||||
int Madr_ij = dof_Madr[i]+1;
|
||||
int j = dof_parentid[i];
|
||||
|
||||
// traverse ancestors backwards
|
||||
// write directly in x[i] since i cannot be a parent of itself
|
||||
while (j >= 0) {
|
||||
x[i] -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j)
|
||||
|
||||
// advance to parent
|
||||
j = dof_parentid[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// multiple vectors
|
||||
else {
|
||||
int offset;
|
||||
mjtNum tmp;
|
||||
|
||||
// x <- inv(L') * x; skip simple
|
||||
for (int i=nv-1; i >= 0; i--) {
|
||||
if (!m->dof_simplenum[i]) {
|
||||
// init
|
||||
int Madr_ij = dof_Madr[i]+1;
|
||||
int j = dof_parentid[i];
|
||||
|
||||
// traverse ancestors backwards
|
||||
while (j >= 0) {
|
||||
// process all vectors, exploit sparsity
|
||||
for (offset=0; offset < n*nv; offset+=nv)
|
||||
if ((tmp = x[i+offset])) {
|
||||
x[j+offset] -= qLD[Madr_ij]*tmp; // x(j) -= L(i,j) * x(i)
|
||||
}
|
||||
|
||||
// advance to parent
|
||||
Madr_ij++;
|
||||
j = dof_parentid[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// x <- inv(D) * x
|
||||
for (int i=0; i < nv; i++) {
|
||||
for (offset=0; offset < n*nv; offset+=nv) {
|
||||
x[i+offset] *= qLDiagInv[i]; // x(i) /= L(i,i)
|
||||
}
|
||||
}
|
||||
|
||||
// x <- inv(L) * x; skip simple
|
||||
for (int i=0; i < nv; i++) {
|
||||
if (!m->dof_simplenum[i]) {
|
||||
// init
|
||||
int Madr_ij = dof_Madr[i]+1;
|
||||
int j = dof_parentid[i];
|
||||
|
||||
// traverse ancestors backwards
|
||||
tmp = x[i+offset];
|
||||
while (j >= 0) {
|
||||
// process all vectors
|
||||
for (offset=0; offset < n*nv; offset+=nv) {
|
||||
x[i+offset] -= qLD[Madr_ij]*x[j+offset]; // x(i) -= L(i,j) * x(j)
|
||||
}
|
||||
|
||||
// advance to parent
|
||||
Madr_ij++;
|
||||
j = dof_parentid[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// in-place sparse backsubstitution: x = inv(L'*D*L)*x (with dof skipping)
|
||||
void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n,
|
||||
const int* rownnz, const int* rowadr, const int* colind, const int* index) {
|
||||
|
||||
@@ -64,10 +64,6 @@ MJAPI void mj_tendonArmature(const mjModel* m, mjData* d);
|
||||
// make inertia matrix
|
||||
MJAPI void mj_makeM(const mjModel* m, mjData* d);
|
||||
|
||||
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd (legacy implementation)
|
||||
MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M,
|
||||
mjtNum* qLD, mjtNum* qLDiagInv);
|
||||
|
||||
// sparse L'*D*L factorizaton of inertia-like matrix (only dofs in index, if given)
|
||||
MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
|
||||
const int* rownnz, const int* rowadr, const int* colind, const int* index);
|
||||
@@ -75,10 +71,6 @@ MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
|
||||
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
|
||||
MJAPI void mj_factorM(const mjModel* m, mjData* d);
|
||||
|
||||
// sparse backsubstitution: x = inv(L'*D*L)*x (legacy implementation)
|
||||
MJAPI void mj_solveLD_legacy(const mjModel* m, mjtNum* x, int n,
|
||||
const mjtNum* qLD, const mjtNum* qLDiagInv);
|
||||
|
||||
// in-place sparse backsubstitution (only dofs in index, if given): x = inv(L'*D*L)*x
|
||||
// handle n vectors at once
|
||||
MJAPI void mj_solveLD(mjtNum* x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n,
|
||||
|
||||
@@ -30,7 +30,7 @@ static const int kNumBenchmarkSteps = 50;
|
||||
|
||||
// ----------------------------- benchmark ------------------------------------
|
||||
|
||||
static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
|
||||
static void BM_factorI(benchmark::State& state, bool coil) {
|
||||
static mjModel* m;
|
||||
if (coil) {
|
||||
m = LoadModelFromPath("plugin/elasticity/coil.xml");
|
||||
@@ -46,21 +46,14 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
|
||||
|
||||
// M: mass matrix in CSR format
|
||||
mjtNum* M = mj_stackAllocNum(d, m->nC);
|
||||
mju_gather(M, d->qM, m->mapM2M, m->nC);
|
||||
|
||||
// LDlegacy: legacy LD matrix (size nM)
|
||||
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
|
||||
mju_copy(M, d->M, m->nC);
|
||||
|
||||
// benchmark
|
||||
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
|
||||
for (int i=0; i < kNumBenchmarkSteps; i++) {
|
||||
if (legacy) {
|
||||
mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv);
|
||||
} else {
|
||||
mju_copy(d->qLD, M, m->nC);
|
||||
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
}
|
||||
mju_copy(d->qLD, M, m->nC);
|
||||
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -71,31 +64,17 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
|
||||
state.SetItemsProcessed(state.iterations());
|
||||
}
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL
|
||||
BM_factorI_COIL_LEGACY(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_factorI(state, /*legacy=*/true, /*coil=*/true);
|
||||
}
|
||||
BENCHMARK(BM_factorI_COIL_LEGACY);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL
|
||||
BM_factorI_COIL_CSR(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_factorI(state, /*legacy=*/false, /*coil=*/true);
|
||||
BM_factorI(state, /*coil=*/true);
|
||||
}
|
||||
BENCHMARK(BM_factorI_COIL_CSR);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL
|
||||
BM_factorI_H100_LEGACY(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_factorI(state, /*legacy=*/true, /*coil=*/false);
|
||||
}
|
||||
BENCHMARK(BM_factorI_H100_LEGACY);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL
|
||||
BM_factorI_H100_CSR(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_factorI(state, /*legacy=*/false, /*coil=*/false);
|
||||
BM_factorI(state, /*coil=*/false);
|
||||
}
|
||||
BENCHMARK(BM_factorI_H100_CSR);
|
||||
|
||||
|
||||
@@ -31,12 +31,7 @@ static const int kNumBenchmarkSteps = 50;
|
||||
|
||||
// ----------------------------- benchmark ------------------------------------
|
||||
|
||||
enum class SolveType {
|
||||
kLegacy = 0,
|
||||
kCsr,
|
||||
};
|
||||
|
||||
static void BM_solve(benchmark::State& state, SolveType type) {
|
||||
static void BM_solve(benchmark::State& state) {
|
||||
static mjModel* m;
|
||||
m = LoadModelFromPath("../test/benchmark/testdata/inertia.xml");
|
||||
|
||||
@@ -48,10 +43,7 @@ static void BM_solve(benchmark::State& state, SolveType type) {
|
||||
|
||||
// M: mass matrix in CSR format
|
||||
mjtNum* M = mj_stackAllocNum(d, m->nC);
|
||||
mju_gather(M, d->qM, m->mapM2M, m->nC);
|
||||
|
||||
// LDlegacy: legacy LD matrix (size nM)
|
||||
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
|
||||
mju_copy(M, d->M, m->nC);
|
||||
|
||||
// arbitrary input vector
|
||||
mjtNum *res = mj_stackAllocNum(d, m->nv);
|
||||
@@ -64,19 +56,11 @@ static void BM_solve(benchmark::State& state, SolveType type) {
|
||||
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
|
||||
for (int i=0; i < kNumBenchmarkSteps; i++) {
|
||||
mju_copy(res, vec, m->nv);
|
||||
switch (type) {
|
||||
case SolveType::kLegacy:
|
||||
mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv);
|
||||
mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
|
||||
mj_solveM(m, d, res, vec, 1);
|
||||
break;
|
||||
case SolveType::kCsr:
|
||||
mju_copy(d->qLD, M, m->nC);
|
||||
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
}
|
||||
mju_copy(d->qLD, M, m->nC);
|
||||
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -87,15 +71,9 @@ static void BM_solve(benchmark::State& state, SolveType type) {
|
||||
state.SetItemsProcessed(state.iterations());
|
||||
}
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_LEGACY(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_solve(state, SolveType::kLegacy);
|
||||
}
|
||||
BENCHMARK(BM_solve_LEGACY);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_CSR(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_solve(state, SolveType::kCsr);
|
||||
BM_solve(state);
|
||||
}
|
||||
BENCHMARK(BM_solve_CSR);
|
||||
|
||||
|
||||
@@ -30,7 +30,7 @@ static const int kNumBenchmarkSteps = 50;
|
||||
|
||||
// ----------------------------- benchmark ------------------------------------
|
||||
|
||||
static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
|
||||
static void BM_solveLD(benchmark::State& state, bool coil) {
|
||||
static mjModel* m;
|
||||
if (coil) {
|
||||
m = LoadModelFromPath("plugin/elasticity/coil.xml");
|
||||
@@ -51,21 +51,12 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
|
||||
vec[i] = 0.2 + 0.3*i;
|
||||
}
|
||||
|
||||
// scatter into legacy matrix
|
||||
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
|
||||
mju_zero(LDlegacy, m->nM);
|
||||
mju_scatter(LDlegacy, d->qLD, m->mapM2M, m->nC);
|
||||
|
||||
// benchmark
|
||||
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
|
||||
for (int i=0; i < kNumBenchmarkSteps; i++) {
|
||||
mju_copy(res, vec, m->nv);
|
||||
if (featherstone) {
|
||||
mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
|
||||
} else {
|
||||
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
}
|
||||
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
|
||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -76,27 +67,15 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
|
||||
state.SetItemsProcessed(state.iterations());
|
||||
}
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_COIL_FS(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_solveLD(state, /*featherstone=*/true, /*coil=*/true);
|
||||
}
|
||||
BENCHMARK(BM_solveLD_COIL_FS);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_COIL_CSR(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_solveLD(state, /*featherstone=*/false, /*coil=*/true);
|
||||
BM_solveLD(state, /*coil=*/true);
|
||||
}
|
||||
BENCHMARK(BM_solveLD_COIL_CSR);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_H100_FS(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_solveLD(state, /*featherstone=*/true, /*coil=*/false);
|
||||
}
|
||||
BENCHMARK(BM_solveLD_H100_FS);
|
||||
|
||||
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_H100_CSR(benchmark::State& state) {
|
||||
MujocoErrorTestGuard guard;
|
||||
BM_solveLD(state, /*featherstone=*/false, /*coil=*/false);
|
||||
BM_solveLD(state, /*coil=*/false);
|
||||
}
|
||||
BENCHMARK(BM_solveLD_H100_CSR);
|
||||
|
||||
|
||||
@@ -663,22 +663,7 @@ TEST_F(CoreSmoothTest, FactorI) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// Convert legacy-format symmetric matrix to dense (local helper for tests).
|
||||
static void legacyToDense(const mjModel* m, mjtNum* dst, const mjtNum* M) {
|
||||
int adr = 0, nv = m->nv;
|
||||
mju_zero(dst, nv * nv);
|
||||
for (int i = 0; i < nv; i++) {
|
||||
int j = i;
|
||||
while (j >= 0) {
|
||||
dst[i * nv + j] = M[adr];
|
||||
dst[j * nv + i] = M[adr];
|
||||
j = m->dof_parentid[j];
|
||||
adr++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(CoreSmoothTest, SolveLDs) {
|
||||
TEST_F(CoreSmoothTest, SolveLD) {
|
||||
const std::string xml_path = GetTestDataFilePath(kInertiaPath);
|
||||
char error[1024];
|
||||
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
@@ -688,41 +673,24 @@ TEST_F(CoreSmoothTest, SolveLDs) {
|
||||
mj_forward(m, d);
|
||||
|
||||
int nv = m->nv;
|
||||
int nM = m->nM;
|
||||
int nC = m->nC;
|
||||
|
||||
// copy M into LD: Legacy format
|
||||
vector<mjtNum> LDlegacy(nM, 0);
|
||||
mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, nC);
|
||||
// arbitrary RHS vector y
|
||||
vector<mjtNum> y(nv);
|
||||
for (int i = 0; i < nv; i++) y[i] = 20 + 30 * i;
|
||||
for (int i = 0; i < nv; i += 2) y[i] = 0;
|
||||
|
||||
// compare LD and LDs densified matrices
|
||||
vector<mjtNum> LDdense(nv * nv);
|
||||
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
|
||||
m->M_colind);
|
||||
vector<mjtNum> LDdense2(nv * nv);
|
||||
legacyToDense(m, LDdense2.data(), LDlegacy.data());
|
||||
|
||||
// expect lower triangles to match exactly
|
||||
for (int i = 0; i < nv; i++) {
|
||||
for (int j = 0; j < i; j++) {
|
||||
EXPECT_NEAR(LDdense[i * nv + j], LDdense2[i * nv + j],
|
||||
MjTol(1e-14, 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
// compare legacy and CSR LD vector solve
|
||||
vector<mjtNum> vec(nv);
|
||||
vector<mjtNum> vec2(nv);
|
||||
for (int i = 0; i < nv; i++) vec[i] = vec2[i] = 20 + 30 * i;
|
||||
for (int i = 0; i < nv; i += 2) vec[i] = vec2[i] = 0;
|
||||
|
||||
mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
|
||||
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr,
|
||||
// x = inv(M) * y
|
||||
vector<mjtNum> x = y;
|
||||
mj_solveLD(x.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr,
|
||||
m->M_colind, nullptr);
|
||||
|
||||
// expect vectors to match up to floating point precision
|
||||
// z = M * x
|
||||
vector<mjtNum> z(nv);
|
||||
mj_mulM(m, d, z.data(), x.data());
|
||||
|
||||
// expect z to match y
|
||||
for (int i = 0; i < nv; i++) {
|
||||
EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6));
|
||||
EXPECT_NEAR(z[i], y[i], MjTol(1e-12, 5e-4));
|
||||
}
|
||||
|
||||
mj_deleteData(d);
|
||||
@@ -739,25 +707,25 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
|
||||
mj_forward(m, d);
|
||||
|
||||
int nv = m->nv;
|
||||
|
||||
// copy LD into LDlegacy: Legacy format
|
||||
vector<mjtNum> LDlegacy(m->nM, 0);
|
||||
mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, m->nC);
|
||||
|
||||
// compare n LD and LDs vector solve
|
||||
int n = 3;
|
||||
vector<mjtNum> vec(nv * n);
|
||||
vector<mjtNum> vec2(nv * n);
|
||||
for (int i = 0; i < nv * n; i++) vec[i] = vec2[i] = 2 + 3 * i;
|
||||
for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0;
|
||||
|
||||
mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
|
||||
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
|
||||
// Y: arbitrary RHS vectors (nv x n)
|
||||
vector<mjtNum> Y(nv * n);
|
||||
for (int i = 0; i < nv * n; i++) Y[i] = 2 + 3 * i;
|
||||
for (int i = 0; i < nv * n; i += 3) Y[i] = 0;
|
||||
|
||||
// X = inv(M) * Y
|
||||
vector<mjtNum> X = Y;
|
||||
mj_solveLD(X.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
|
||||
m->M_colind, nullptr);
|
||||
|
||||
// expect vectors to match up to floating point precision
|
||||
for (int i = 0; i < nv * n; i++) {
|
||||
EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6));
|
||||
// verify each vector: Z_i = M * X_i
|
||||
for (int i = 0; i < n; i++) {
|
||||
vector<mjtNum> z(nv);
|
||||
mj_mulM(m, d, z.data(), X.data() + i * nv);
|
||||
for (int j = 0; j < nv; j++) {
|
||||
EXPECT_NEAR(z[j], Y[i * nv + j], MjTol(1e-12, 5e-4));
|
||||
}
|
||||
}
|
||||
|
||||
mj_deleteData(d);
|
||||
@@ -804,54 +772,7 @@ TEST_F(CoreSmoothTest, SolveM2) {
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
TEST_F(CoreSmoothTest, FactorIs) {
|
||||
const std::string xml_path = GetTestDataFilePath(kInertiaPath);
|
||||
char error[1024];
|
||||
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
|
||||
|
||||
mjData* d = mj_makeData(m);
|
||||
mj_forward(m, d);
|
||||
|
||||
int nC = m->nC, nM = m->nM, nv = m->nv;
|
||||
|
||||
// copy qM into into qLDlegacy and factorize
|
||||
vector<mjtNum> qLDlegacy(nM);
|
||||
mj_factorI_legacy(m, d, d->qM, qLDlegacy.data(), d->qLDiagInv);
|
||||
|
||||
// copy qLDlegacy into qLDexpected: CSR format
|
||||
vector<mjtNum> qLDexpected(nC);
|
||||
mju_gather(qLDexpected.data(), qLDlegacy.data(), m->mapM2M, nC);
|
||||
|
||||
// copy qM into qLD: CSR format
|
||||
vector<mjtNum> qLD(nC);
|
||||
mju_gather(qLD.data(), d->qM, m->mapM2M, nC);
|
||||
|
||||
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
|
||||
vector<mjtNum> qLDiagInv(nv, 0);
|
||||
|
||||
mj_factorI(qLD.data(), qLDiagInv.data(), nv, m->M_rownnz, m->M_rowadr,
|
||||
m->M_colind, nullptr);
|
||||
|
||||
// expect outputs to match to floating point precision
|
||||
EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected));
|
||||
EXPECT_THAT(qLDiagInv, Pointwise(MjNear(1e-12, 1e-4), qLDiagInvExpected));
|
||||
|
||||
/* uncomment for debugging
|
||||
vector<mjtNum> LDdense(nv*nv);
|
||||
|
||||
mju_sparse2dense(LDdense.data(), qLDexpected.data(), nv, nv,
|
||||
d->C_rownnz, d->C_rowadr, d->C_colind);
|
||||
PrintMatrix(LDdense.data(), nv, nv, 2);
|
||||
|
||||
mju_sparse2dense(LDdense.data(), qLDs.data(), nv, nv,
|
||||
d->C_rownnz, d->C_rowadr, d->C_colind);
|
||||
PrintMatrix(LDdense.data(), nv, nv, 2);
|
||||
*/
|
||||
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
||||
constexpr char xml[] = R"(
|
||||
|
||||
Reference in New Issue
Block a user