Change flex constraints to eigenmodes of the stiffness matrix.
This provides a reduction from 26 to 18 constraints for trilinear and from 162 to 75 for quadratic. The assembly of the constraints becomes trivial. In total the speedup for a trilinear 3x3x3 grid is about 3x. PiperOrigin-RevId: 902502398 Change-Id: I764772c7adef78da5a644f64701f842d36e4b543
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
bf9be2c312
commit
3230cf99f9
@@ -622,6 +622,149 @@ 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 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>
|
||||
|
||||
@@ -40,3 +40,5 @@ mujoco_test(user_composite_test)
|
||||
mujoco_test(user_resource_test)
|
||||
|
||||
mujoco_test(user_vfs_test)
|
||||
|
||||
mujoco_test(user_util_test)
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user