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

Project out rigid body modes from flex strain equality constraints.

Browse Source 
Modify EigendecomposeStiffness to project out rigid body translations and rotations from the stiffness matrix eigenvectors. This prevents "ghost damping" from rigid body modes being incorrectly constrained. Update hollow_vs_solid.xml with solimp parameters for edge constraints. Adjust engine_core_constraint_test.cc to reflect the reduced number of equality constraints due to the projection.

PiperOrigin-RevId: 910631207
Change-Id: Ibaaa59bc030cfc6ae657d8f0d1b2a51002cc8d4e
This commit is contained in:
Alessio Quaglino
2026-05-05 06:00:30 -07:00
committed by Copybara-Service
parent d168eb2d7d
commit b9c1877ecb
4 changed files with 128 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+1 -1
View File
@@ -10,7 +10,7 @@ General
- Added island support for the :ref:`PGS solver<soAlgorithms>`.
- The :ref:`PGS solver<soAlgorithms>` now iterates over constraints in pseudo-random order, improving performance by
~20%.
- Added support for :ref:`elastic2d<body-flexcomp-elastic2d>` for trilinear and quadratic flex
- Added support for :ref:`elastic2d<flex-elasticity-elastic2d>` for trilinear and quadratic flex
:ref:`dofs<body-flexcomp-dof>`.
- :ref:`Midpoint integration<geMidpoint>` is now restricted to the ``implicitfast``
:ref:`integrator<geIntegrators>` and is disabled when fluid forces are active
model/flex/hollow_vs_solid.xml
+2 -2
View File
@@ -24,7 +24,7 @@
solref="0.001 1" friction="2 0.1 0.1"/>
<joint type="slide" axis="0 0 1" range="-1 1"/>
<default class="finger">
<geom size="0.06 0.01 0.05"/>
<geom size="0.01 0.06 0.03"/>
<joint type="slide" range="0 0.1"/>
</default>
</default>
@@ -51,7 +51,7 @@
type="grid" name="soft_mesh_2" dim="3" spacing=".025 .05 .025"
mass="0.43" radius="0.005" dof="trilinear" rgba="0.9 0.7 0.7 1">
<elasticity young="0" poisson="0.3" damping="0.01" elastic2d="bend" thickness="0.03"/>
<edge equality="strain" solref="0.001 1"/>
<edge equality="strain" solref="0.001 1" solimp="0.99 0.999 0.001"/>
<contact selfcollide="none"
internal="false"
friction="2.0 0.5 0.01"
src/user/user_mesh.cc
+122 -4
View File
@@ -3904,9 +3904,16 @@ void inline ComputeLinearStiffness2D(std::vector<double>& K,
// Output layout in `out`:
// [0]: neig (as double)
// [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major
// If pos is non-null (3*npe doubles), rigid body modes are projected out of
// each eigenvector to prevent ghost damping in the constraint solver. The
// constraint Jacobian freezes the corotational frame, so eigenvectors aligned
// with rigid rotation patterns produce spurious velocity-level forces.
// Returns number of retained eigenmodes.
static int EigendecomposeStiffness(const double* K_cell_data,
double* out, int ndof) {
double* out, int ndof,
const double* pos) {
int npe = ndof / 3;
// copy K_cell for in-place decomposition
std::vector<double> mat(K_cell_data, K_cell_data + ndof * ndof);
std::vector<double> eigval(ndof);
@@ -3914,27 +3921,138 @@ static int EigendecomposeStiffness(const double* K_cell_data,
mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof);
// build orthonormal rigid body modes for projection
// 6 modes: 3 translations + 3 rotations about centroid
const int kMaxRigid = 6;
std::vector<double> rigid(pos ? kMaxRigid * ndof : 0, 0);
if (pos) {
// compute centroid
double centroid[3] = {0, 0, 0};
for (int n = 0; n < npe; n++) {
for (int k = 0; k < 3; k++) {
centroid[k] += pos[3*n + k];
}
}
for (int k = 0; k < 3; k++) {
centroid[k] /= npe;
}
// translation modes: uniform displacement along each axis
for (int n = 0; n < npe; n++) {
rigid[0*ndof + 3*n + 0] = 1;
rigid[1*ndof + 3*n + 1] = 1;
rigid[2*ndof + 3*n + 2] = 1;
}
// rotation modes: e_axis × (pos_n - centroid)
for (int n = 0; n < npe; n++) {
double rx = pos[3*n + 0] - centroid[0];
double ry = pos[3*n + 1] - centroid[1];
double rz = pos[3*n + 2] - centroid[2];
// rotation about x: [0, -rz, ry]
rigid[3*ndof + 3*n + 1] = -rz;
rigid[3*ndof + 3*n + 2] = ry;
// rotation about y: [rz, 0, -rx]
rigid[4*ndof + 3*n + 0] = rz;
rigid[4*ndof + 3*n + 2] = -rx;
// rotation about z: [-ry, rx, 0]
rigid[5*ndof + 3*n + 0] = -ry;
rigid[5*ndof + 3*n + 1] = rx;
}
// orthonormalize via modified Gram-Schmidt
for (int i = 0; i < kMaxRigid; i++) {
double* ri = rigid.data() + i * ndof;
for (int j = 0; j < i; j++) {
const double* rj = rigid.data() + j * ndof;
double dot = 0;
for (int k = 0; k < ndof; k++) {
dot += ri[k] * rj[k];
}
for (int k = 0; k < ndof; k++) {
ri[k] -= dot * rj[k];
}
}
double norm2 = 0;
for (int k = 0; k < ndof; k++) {
norm2 += ri[k] * ri[k];
}
if (norm2 > 1e-20) {
double inv_norm = 1.0 / std::sqrt(norm2);
for (int k = 0; k < ndof; k++) {
ri[k] *= inv_norm;
}
} else {
// degenerate mode (e.g., collinear nodes): zero out
std::fill(ri, ri + ndof, 0.0);
}
}
}
// K_stored = -K_physical, so physical eigenvalue = -eigval[i]
// retain modes where physical eigenvalue > threshold
double max_eigval = 0;
for (int i = 0; i < ndof; i++) {
max_eigval = std::max(max_eigval, std::abs(eigval[i]));
}
double threshold = max_eigval * 1e-8;
double threshold = max_eigval * 1e-8;
int neig = 0;
for (int i = 0; i < ndof; i++) {
double lambda_phys = -eigval[i]; // negate to get physical eigenvalue
if (lambda_phys > threshold) {
// store sqrt(λ) * eigenvector (column i of eigvec matrix)
double scale = std::sqrt(lambda_phys);
double* w = out + 1 + neig * ndof;
for (int j = 0; j < ndof; j++) {
out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i];
w[j] = scale * eigvec[j * ndof + i];
}
// project out rigid body components
if (pos) {
for (int r = 0; r < kMaxRigid; r++) {
const double* rr = rigid.data() + r * ndof;
double dot = 0;
for (int j = 0; j < ndof; j++) {
dot += w[j] * rr[j];
}
for (int j = 0; j < ndof; j++) {
w[j] -= dot * rr[j];
}
}
// discard if projected norm is negligible relative to original
double norm2 = 0;
for (int j = 0; j < ndof; j++) {
norm2 += w[j] * w[j];
}
if (norm2 < lambda_phys * 1e-6) {
continue; // mode was mostly rigid body: skip
}
}
neig++;
// guard: eigendecomposed data must fit within ndof*ndof slot
// (guaranteed by rigid-body projection discarding >= 6 modes)
if (1 + neig * ndof > ndof * ndof) {
mju_error("EigendecomposeStiffness: output size %d exceeds buffer %d",
1 + neig * ndof, ndof * ndof);
}
}
}
// check that enough modes were discarded (rigid body + numerical artifacts)
// for 3D elements: expect ndof - neig == 6
if (pos && ndof - neig != kMaxRigid) {
mju_warning("EigendecomposeStiffness: only %d modes discarded, expected "
"at least %d rigid body modes", ndof - neig, kMaxRigid);
}
out[0] = static_cast<double>(neig);
return neig;
}
@@ -4611,7 +4729,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (has_strain_eq) {
// eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...]
std::fill(out, out + ndof_elem * ndof_elem, 0.0);
EigendecomposeStiffness(K_elem.data(), out, ndof_elem);
EigendecomposeStiffness(K_elem.data(), out, ndof_elem, elem_pos.data());
} else {
// store raw K for passive forces
std::copy(K_elem.begin(), K_elem.end(), out);
test/engine/engine_core_constraint_test.cc
+3 -1
View File
@@ -710,7 +710,9 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
EXPECT_EQ(m->neq, 6);
// Check total number of scalar equality constraints
EXPECT_EQ(d->ne, 48); // 6 faces * 8 modes per face
// 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++) {