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
@@ -47,47 +47,6 @@
|
||||
//-------------------------- utility functions -----------------------------------------------------
|
||||
|
||||
|
||||
// compute 3x3 matrix inverse, storing result in out
|
||||
// assumes matrix is invertible (det != 0)
|
||||
static void mat3_inverse(const mjtNum* mat, mjtNum* out) {
|
||||
mjtNum det = mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) -
|
||||
mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) +
|
||||
mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]);
|
||||
|
||||
out[0] = (mat[4]*mat[8] - mat[5]*mat[7]) / det;
|
||||
out[1] = -(mat[1]*mat[8] - mat[2]*mat[7]) / det;
|
||||
out[2] = (mat[1]*mat[5] - mat[2]*mat[4]) / det;
|
||||
out[3] = -(mat[3]*mat[8] - mat[5]*mat[6]) / det;
|
||||
out[4] = (mat[0]*mat[8] - mat[2]*mat[6]) / det;
|
||||
out[5] = -(mat[0]*mat[5] - mat[2]*mat[3]) / det;
|
||||
out[6] = (mat[3]*mat[7] - mat[4]*mat[6]) / det;
|
||||
out[7] = -(mat[0]*mat[7] - mat[1]*mat[6]) / det;
|
||||
out[8] = (mat[0]*mat[4] - mat[1]*mat[3]) / det;
|
||||
}
|
||||
|
||||
|
||||
// compute 3x3 matrix cofactor, storing result in out
|
||||
static void mat3_cofactor(const mjtNum* mat, mjtNum* out) {
|
||||
out[0] = mat[4]*mat[8] - mat[5]*mat[7];
|
||||
out[1] = -(mat[3]*mat[8] - mat[5]*mat[6]);
|
||||
out[2] = mat[3]*mat[7] - mat[4]*mat[6];
|
||||
out[3] = -(mat[1]*mat[8] - mat[2]*mat[7]);
|
||||
out[4] = mat[0]*mat[8] - mat[2]*mat[6];
|
||||
out[5] = -(mat[0]*mat[7] - mat[1]*mat[6]);
|
||||
out[6] = mat[1]*mat[5] - mat[2]*mat[4];
|
||||
out[7] = -(mat[0]*mat[5] - mat[2]*mat[3]);
|
||||
out[8] = mat[0]*mat[4] - mat[1]*mat[3];
|
||||
}
|
||||
|
||||
|
||||
// compute 3x3 matrix determinant
|
||||
static mjtNum mat3_det(const mjtNum* mat) {
|
||||
return mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) -
|
||||
mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) +
|
||||
mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]);
|
||||
}
|
||||
|
||||
|
||||
// compute cell node Jacobians and combined chain for flex strain constraints
|
||||
// npc: number of nodes per cell
|
||||
// gindices: global indices of cell nodes in flex
|
||||
@@ -167,133 +126,6 @@ static void cell_strain_jacobian(int npc, int cell_nnz,
|
||||
}
|
||||
|
||||
|
||||
// basis functions for flex strain constraints
|
||||
static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) {
|
||||
if (order == 1) {
|
||||
*phi = (i == 0 ? 1 - p : p);
|
||||
*dphi = (i == 0 ? -1 : 1);
|
||||
} else {
|
||||
if (i == 0) {
|
||||
*phi = 2 * p * p - 3 * p + 1;
|
||||
*dphi = 4 * p - 3;
|
||||
} else if (i == 1) {
|
||||
*phi = 4 * (p - p * p);
|
||||
*dphi = 4 * (1 - 2 * p);
|
||||
} else {
|
||||
*phi = 2 * p * p - p;
|
||||
*dphi = 4 * p - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute shape function gradients at a parametric point
|
||||
// grad: output array of size nodenum x 3 (gradient w.r.t. parametric coords)
|
||||
static void shape_gradients(
|
||||
int order, const mjtNum* p, mjtNum grad[][3]) {
|
||||
int npoint = (order + 1) * (order + 1) * (order + 1);
|
||||
int stride = order + 1;
|
||||
|
||||
for (int n = 0; n < npoint; n++) {
|
||||
int ix = n / (stride * stride);
|
||||
int iy = (n / stride) % stride;
|
||||
int iz = n % stride;
|
||||
|
||||
mjtNum phi_x, phi_y, phi_z, dphi_x, dphi_y, dphi_z;
|
||||
basis(order, ix, p[0], &phi_x, &dphi_x);
|
||||
basis(order, iy, p[1], &phi_y, &dphi_y);
|
||||
basis(order, iz, p[2], &phi_z, &dphi_z);
|
||||
|
||||
grad[n][0] = dphi_x * phi_y * phi_z;
|
||||
grad[n][1] = phi_x * dphi_y * phi_z;
|
||||
grad[n][2] = phi_x * phi_y * dphi_z;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute dStrain/dNodePosition for volumetric invariants (I1 or J-1)
|
||||
// dSdx: output array of size 3*nodenum
|
||||
static void volumetric_dSdx(int invariant_type, int nodenum, mjtNum grad[][3],
|
||||
const mjtNum* F, const mjtNum* Fref_inv, mjtNum* dSdx) {
|
||||
mju_zero(dSdx, 3*nodenum);
|
||||
|
||||
if (invariant_type == 0) {
|
||||
mjtNum dSdE[9] = {1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0};
|
||||
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
mjtNum dS = 0;
|
||||
for (int ij = 0; ij < 9; ij++) {
|
||||
int ii = ij / 3;
|
||||
int jj = ij % 3;
|
||||
|
||||
mjtNum dF_ci = 0;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
dF_ci += grad[n][k] * Fref_inv[k*3 + ii];
|
||||
}
|
||||
mjtNum dF_cj = 0;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
dF_cj += grad[n][k] * Fref_inv[k*3 + jj];
|
||||
}
|
||||
|
||||
mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj;
|
||||
dS += dSdE[ij] * 0.5 * dC_ij;
|
||||
}
|
||||
dSdx[3*n + c] = dS;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
mjtNum cofF[9];
|
||||
mat3_cofactor(F, cofF);
|
||||
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
mjtNum dJ = 0;
|
||||
for (int b = 0; b < 3; b++) {
|
||||
mjtNum dF_cb = 0;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
dF_cb += grad[n][k] * Fref_inv[k*3 + b];
|
||||
}
|
||||
dJ += cofF[c*3 + b] * dF_cb;
|
||||
}
|
||||
dSdx[3*n + c] = dJ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute dStrain/dNodePosition for general strain invariants
|
||||
// dSdx: output array of size 3*nodenum
|
||||
static void invariant_dSdx(int nodenum, mjtNum grad[][3], const mjtNum* F,
|
||||
const mjtNum* Fref_inv, const mjtNum* dSdE, mjtNum* dSdx) {
|
||||
mju_zero(dSdx, 3*nodenum);
|
||||
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
mjtNum dS = 0;
|
||||
for (int ij = 0; ij < 9; ij++) {
|
||||
int ii = ij / 3;
|
||||
int jj = ij % 3;
|
||||
|
||||
mjtNum dF_ci = 0;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
dF_ci += grad[n][k] * Fref_inv[k*3 + ii];
|
||||
}
|
||||
mjtNum dF_cj = 0;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
dF_cj += grad[n][k] * Fref_inv[k*3 + jj];
|
||||
}
|
||||
|
||||
mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj;
|
||||
dS += dSdE[ij] * 0.5 * dC_ij;
|
||||
}
|
||||
dSdx[3*n + c] = dS;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// allocate efc arrays on arena, return 1 on success, 0 on failure
|
||||
static int arenaAllocEfc(const mjModel* m, mjData* d) {
|
||||
#undef MJ_M
|
||||
@@ -923,6 +755,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart));
|
||||
}
|
||||
|
||||
// compute corotational quaternion from cell-local positions
|
||||
mjtNum cell_quat[4] = {1, 0, 0, 0};
|
||||
{
|
||||
mjtNum center[3] = {0.5, 0.5, 0.5};
|
||||
mjtNum mat[9];
|
||||
mju_defGradient(mat, center, xpos_c, order);
|
||||
mju_mat2Rot(cell_quat, mat);
|
||||
mju_negQuat(cell_quat, cell_quat);
|
||||
}
|
||||
|
||||
// build per-cell sparse chain and node Jacobians
|
||||
int* cell_chain = mjSTACKALLOC(d, nv, int);
|
||||
int cell_nnz = 0;
|
||||
@@ -940,140 +782,59 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
mju_zero(dense_jac, nv);
|
||||
}
|
||||
|
||||
// Gauss-Legendre quadrature points in [0,1]^3
|
||||
int nquad = order + 1;
|
||||
int ngauss = nquad * nquad * nquad;
|
||||
// read eigenmode data from flex_stiffness
|
||||
int ndof_cell = 3 * npc;
|
||||
int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2]
|
||||
+ cj * m->flex_cellnum[3*f+2] + ck;
|
||||
const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
|
||||
+ cell_idx * ndof_cell * ndof_cell;
|
||||
int neig = (int)k_cell[0];
|
||||
|
||||
mjtNum gp1d[3];
|
||||
if (nquad == 2) {
|
||||
gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0);
|
||||
gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0);
|
||||
} else {
|
||||
gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6);
|
||||
gp1d[1] = 0.5;
|
||||
gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6);
|
||||
// compute displacement in corotational frame
|
||||
mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum);
|
||||
for (int n = 0; n < npc; n++) {
|
||||
// rotate xpos_c to corotational frame
|
||||
mjtNum xrot[3];
|
||||
mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat);
|
||||
displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0];
|
||||
displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1];
|
||||
displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2];
|
||||
}
|
||||
|
||||
mjtNum (*gauss)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*ngauss, mjtNum);
|
||||
for (int gi = 0; gi < nquad; gi++) {
|
||||
for (int gj = 0; gj < nquad; gj++) {
|
||||
for (int gk = 0; gk < nquad; gk++) {
|
||||
int idx = gi*nquad*nquad + gj*nquad + gk;
|
||||
gauss[idx][0] = gp1d[gi];
|
||||
gauss[idx][1] = gp1d[gj];
|
||||
gauss[idx][2] = gp1d[gk];
|
||||
}
|
||||
// compute inverse quaternion for rotating eigenvectors to world frame
|
||||
mjtNum cell_quat_inv[4];
|
||||
mju_negQuat(cell_quat_inv, cell_quat);
|
||||
|
||||
// loop over eigenmodes
|
||||
for (int eig = 0; eig < neig; eig++) {
|
||||
const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell;
|
||||
|
||||
// constraint residual: dot product of scaled eigenvector with displacement
|
||||
mjtNum residual = 0;
|
||||
for (int j = 0; j < ndof_cell; j++) {
|
||||
residual += eigvec[j] * displ_c[j];
|
||||
}
|
||||
}
|
||||
cpos[0] = residual;
|
||||
|
||||
// B-bar: center-point volumetric constraints (trilinear)
|
||||
if (order == 1) {
|
||||
mjtNum center[3] = {0.5, 0.5, 0.5};
|
||||
mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9];
|
||||
|
||||
mju_defGradient(Fcur_c, center, xpos_c, order);
|
||||
mju_defGradient(Fref_c, center, refpos_c, order);
|
||||
mat3_inverse(Fref_c, Fref_inv_c);
|
||||
mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c);
|
||||
|
||||
mjtNum C_c[9], E_c[9];
|
||||
mju_mulMatTMat3(C_c, F_c, F_c);
|
||||
mju_scl(E_c, C_c, 0.5, 9);
|
||||
E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5;
|
||||
|
||||
mjtNum I1_c = E_c[0] + E_c[4] + E_c[8];
|
||||
mjtNum J_c = mat3_det(F_c);
|
||||
|
||||
mjtNum grad_c[8][3];
|
||||
shape_gradients(order, center, grad_c);
|
||||
|
||||
for (int inv = 0; inv < 2; inv++) {
|
||||
cpos[0] = (inv == 0) ? I1_c : J_c - 1.0;
|
||||
volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local);
|
||||
cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac);
|
||||
if (issparse) {
|
||||
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
|
||||
cell_nnz, cell_chain);
|
||||
} else {
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = strain_jac[k];
|
||||
}
|
||||
mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = 0;
|
||||
}
|
||||
}
|
||||
// rotate eigenvector to world frame for Jacobian
|
||||
// dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d]
|
||||
for (int n = 0; n < npc; n++) {
|
||||
mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv);
|
||||
}
|
||||
}
|
||||
|
||||
// Gauss integration
|
||||
for (int g = 0; g < ngauss; g++) {
|
||||
mjtNum* p = gauss[g];
|
||||
// contract with cell_node_jac to get sparse Jacobian
|
||||
cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac);
|
||||
|
||||
mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9];
|
||||
mju_defGradient(Fcur, p, xpos_c, order);
|
||||
mju_defGradient(Fref, p, refpos_c, order);
|
||||
mat3_inverse(Fref, Fref_inv);
|
||||
mju_mulMatMat3(F, Fcur, Fref_inv);
|
||||
|
||||
mjtNum C[9], E[9];
|
||||
mju_mulMatTMat3(C, F, F);
|
||||
for (int j = 0; j < 9; j++) {
|
||||
E[j] = 0.5 * C[j];
|
||||
}
|
||||
E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5;
|
||||
|
||||
mjtNum I1 = E[0] + E[4] + E[8];
|
||||
mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6]
|
||||
+ E[3]*E[1] + E[4]*E[4] + E[5]*E[7]
|
||||
+ E[6]*E[2] + E[7]*E[5] + E[8]*E[8];
|
||||
mjtNum I2 = 0.5 * (I1*I1 - trE2);
|
||||
mjtNum I3 = mat3_det(E);
|
||||
|
||||
mjtNum (*grad)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
shape_gradients(order, p, grad);
|
||||
|
||||
for (int s = 0; s < 6; s++) {
|
||||
if (order == 1 && (s == 0 || s == 1 || s == 2)) {
|
||||
continue;
|
||||
if (issparse) {
|
||||
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
|
||||
cell_nnz, cell_chain);
|
||||
} else {
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = strain_jac[k];
|
||||
}
|
||||
|
||||
mjtNum dSdE[9];
|
||||
mju_zero(dSdE, 9);
|
||||
|
||||
if (s == 0) {
|
||||
cpos[0] = I1;
|
||||
dSdE[0] = dSdE[4] = dSdE[8] = 1.0;
|
||||
} else if (s == 1) {
|
||||
cpos[0] = I2;
|
||||
dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4];
|
||||
dSdE[8] = I1-E[8];
|
||||
dSdE[1] = -E[1]; dSdE[3] = -E[3];
|
||||
dSdE[2] = -E[2]; dSdE[6] = -E[6];
|
||||
dSdE[5] = -E[5]; dSdE[7] = -E[7];
|
||||
} else if (s == 2) {
|
||||
cpos[0] = I3;
|
||||
mat3_cofactor(E, dSdE);
|
||||
} else {
|
||||
int offdiag_idx[3] = {1, 2, 5};
|
||||
int ij = offdiag_idx[s - 3];
|
||||
cpos[0] = E[ij];
|
||||
dSdE[ij] = 1.0;
|
||||
}
|
||||
|
||||
invariant_dSdx(npc, grad, F, Fref_inv, dSdE, dSdx_local);
|
||||
cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac);
|
||||
if (issparse) {
|
||||
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
|
||||
cell_nnz, cell_chain);
|
||||
} else {
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = strain_jac[k];
|
||||
}
|
||||
mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = 0;
|
||||
}
|
||||
mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2404,6 +2165,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
chain2 = mjSTACKALLOC(d, nv, int);
|
||||
}
|
||||
|
||||
// pre-allocate buffer for cell body IDs (max npc = 125 for order=2)
|
||||
int* cell_bodies = nnz ? mjSTACKALLOC(d, 125, int) : NULL;
|
||||
|
||||
// find active equality constraints
|
||||
for (int i=0; i < neq; i++) {
|
||||
// skip inactive
|
||||
@@ -2536,24 +2300,25 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
break;
|
||||
}
|
||||
int npc = (order+1)*(order+1)*(order+1);
|
||||
int nquad = order + 1;
|
||||
int ngauss = nquad * nquad * nquad;
|
||||
size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // per cell
|
||||
|
||||
// read eigenmode count from flex_stiffness
|
||||
int ndof_cell = 3 * npc;
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
|
||||
const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
|
||||
+ cell_idx * ndof_cell * ndof_cell;
|
||||
size = (int)k_cell[0]; // neig stored as first element
|
||||
|
||||
if (nnz) {
|
||||
// get cell index from eq_data
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
|
||||
// get the npc node body IDs for this cell
|
||||
int gindices[125];
|
||||
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
int nstart = m->flex_nodeadr[f];
|
||||
int* cell_bodies = mjSTACKALLOC(d, npc, int);
|
||||
for (int n = 0; n < npc; n++) {
|
||||
cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]];
|
||||
}
|
||||
|
||||
@@ -906,6 +906,11 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
|
||||
continue;
|
||||
}
|
||||
|
||||
// skip if strain constraints present (stiffness handled by constraint solver)
|
||||
if (m->flex_edgeequality[f] == 3) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// compute scale
|
||||
mjtNum damping = m->flex_damping[f];
|
||||
mjtNum scale = s1 + s2 * damping;
|
||||
|
||||
@@ -229,6 +229,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// skip interpolated flex with strain constraints (stiffness in constraint solver)
|
||||
if (m->flex_edgeequality[f] == 3) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (m->flex_interp[f]) {
|
||||
int order = m->flex_interp[f];
|
||||
int npc = (order+1)*(order+1)*(order+1); // nodes per cell
|
||||
@@ -270,14 +275,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
for (int ck = 0; ck < cz; ck++) {
|
||||
// gather cell-local node data
|
||||
mjtNum quat[4];
|
||||
mjtNum p[3] = {.5, .5, .5};
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0,
|
||||
xpos_c, vel_c, xpos0_c, NULL, quat);
|
||||
|
||||
// rotate to corotational frame
|
||||
for (int n = 0; n < npc; n++) {
|
||||
mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat);
|
||||
mji_addTo3(xpos_c+3*n, p);
|
||||
mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat);
|
||||
}
|
||||
|
||||
|
||||
@@ -691,6 +691,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
|
||||
mjs_setString(pe->name1, name.c_str());
|
||||
} else if (equality == 3) {
|
||||
// create one strain constraint per cell, storing cell index in eq_data
|
||||
flex->has_strain_eq = true;
|
||||
int cell_cx = flex->spec.cellcount[0];
|
||||
int cell_cy = flex->spec.cellcount[1];
|
||||
int cell_cz = flex->spec.cellcount[2];
|
||||
|
||||
+57
-5
@@ -3818,6 +3818,48 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Eigendecompose cell stiffness matrix and store scaled eigenvectors.
|
||||
// K_cell is n×n stored (negative convention: K_stored = -K_physical).
|
||||
// Output layout in `out`:
|
||||
// [0]: neig (as double)
|
||||
// [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major
|
||||
// Returns number of retained eigenmodes.
|
||||
static int EigendecomposeStiffness(const double* K_cell_data,
|
||||
double* out, int ndof) {
|
||||
// copy K_cell for in-place decomposition
|
||||
std::vector<double> mat(K_cell_data, K_cell_data + ndof * ndof);
|
||||
std::vector<double> eigval(ndof);
|
||||
std::vector<double> eigvec(ndof * ndof);
|
||||
|
||||
mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof);
|
||||
|
||||
// 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;
|
||||
|
||||
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);
|
||||
for (int j = 0; j < ndof; j++) {
|
||||
out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i];
|
||||
}
|
||||
neig++;
|
||||
}
|
||||
}
|
||||
|
||||
out[0] = static_cast<double>(neig);
|
||||
return neig;
|
||||
}
|
||||
|
||||
|
||||
//------------------ class mjCFlex implementation --------------------------------------------------
|
||||
|
||||
// constructor
|
||||
@@ -4344,7 +4386,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
stiffness_cached = LoadCachedStiffness();
|
||||
}
|
||||
|
||||
if (!stiffness_cached && young > 0 && interpolated) {
|
||||
if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) {
|
||||
// use young=1 for strain constraints (eigenvectors are geometry-only)
|
||||
double K_young = has_strain_eq ? 1e1 : young;
|
||||
double K_poisson = has_strain_eq ? 0.3 : poisson;
|
||||
|
||||
int npc = pow(spec.order + 1, 3); // nodes per cell
|
||||
int ndof_cell = 3 * npc;
|
||||
int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
|
||||
@@ -4379,11 +4425,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
|
||||
// compute per-cell stiffness
|
||||
std::vector<double> K_cell(ndof_cell * ndof_cell, 0);
|
||||
ComputeLinearStiffness(K_cell, cell_pos.data(), young, poisson, spec.order);
|
||||
ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order);
|
||||
double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell;
|
||||
|
||||
// copy into global stiffness array
|
||||
mjuu_copyvec(stiffness.data() + cell_idx * ndof_cell * ndof_cell,
|
||||
K_cell.data(), ndof_cell * ndof_cell);
|
||||
if (has_strain_eq) {
|
||||
// eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...]
|
||||
std::fill(out, out + ndof_cell * ndof_cell, 0.0);
|
||||
EigendecomposeStiffness(K_cell.data(), out, ndof_cell);
|
||||
} else {
|
||||
// store raw K for passive forces
|
||||
std::copy(K_cell.begin(), K_cell.end(), out);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3657,6 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
int b1 = pfl->vertbodyid[pfl->edge[k].first];
|
||||
int b2 = pfl->vertbodyid[pfl->edge[k].second];
|
||||
m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid);
|
||||
} else {
|
||||
m->flexedge_rigid[edge_adr+k] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -983,6 +983,7 @@ class mjCFlex_ : public mjCBase {
|
||||
std::vector<int> edgeidx_; // element edge ids
|
||||
std::vector<double> stiffness; // elasticity stiffness matrix
|
||||
std::vector<double> bending; // bending stiffness matrix
|
||||
bool has_strain_eq = false; // true if strain constraints reference this flex
|
||||
|
||||
// variable-size data
|
||||
std::vector<std::string> vertbody_; // vertex body names
|
||||
|
||||
+87
-12
@@ -754,6 +754,81 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
|
||||
return iter;
|
||||
}
|
||||
|
||||
|
||||
// Jacobi eigenvalue decomposition of symmetric n×n matrix.
|
||||
// On output, eigenvalues are in eigval and eigenvectors are columns of eigvec.
|
||||
// Both arrays must be pre-allocated: eigval[n], eigvec[n*n].
|
||||
// The input matrix mat is destroyed.
|
||||
int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) {
|
||||
// initialize eigvec to identity
|
||||
std::fill(eigvec, eigvec + n*n, 0.0);
|
||||
for (int i = 0; i < n; i++) {
|
||||
eigvec[i*n + i] = 1.0;
|
||||
}
|
||||
|
||||
const int max_sweeps = 200;
|
||||
const double tol = 1e-12;
|
||||
|
||||
int sweep;
|
||||
for (sweep = 0; sweep < max_sweeps; sweep++) {
|
||||
// check convergence: sum of squared off-diagonal elements
|
||||
double off_diag = 0;
|
||||
for (int i = 0; i < n; i++) {
|
||||
for (int j = i+1; j < n; j++) {
|
||||
off_diag += mat[i*n + j] * mat[i*n + j];
|
||||
}
|
||||
}
|
||||
if (off_diag < tol * tol) break;
|
||||
|
||||
// sweep over all off-diagonal pairs
|
||||
for (int p = 0; p < n; p++) {
|
||||
for (int q = p+1; q < n; q++) {
|
||||
double apq = mat[p*n + q];
|
||||
if (std::abs(apq) < tol * 1e-3) continue;
|
||||
|
||||
// compute rotation angle
|
||||
double app = mat[p*n + p];
|
||||
double aqq = mat[q*n + q];
|
||||
double tau = (aqq - app) / (2.0 * apq);
|
||||
double t = (tau >= 0 ? 1.0 : -1.0) /
|
||||
(std::abs(tau) + std::sqrt(1.0 + tau*tau));
|
||||
double c = 1.0 / std::sqrt(1.0 + t*t);
|
||||
double s = t * c;
|
||||
|
||||
// update matrix (Jacobi rotation)
|
||||
mat[p*n + p] -= t * apq;
|
||||
mat[q*n + q] += t * apq;
|
||||
mat[p*n + q] = 0;
|
||||
mat[q*n + p] = 0;
|
||||
|
||||
for (int r = 0; r < n; r++) {
|
||||
if (r == p || r == q) continue;
|
||||
double mrp = mat[r*n + p];
|
||||
double mrq = mat[r*n + q];
|
||||
mat[r*n + p] = mat[p*n + r] = c*mrp - s*mrq;
|
||||
mat[r*n + q] = mat[q*n + r] = s*mrp + c*mrq;
|
||||
}
|
||||
|
||||
// accumulate eigenvectors
|
||||
for (int r = 0; r < n; r++) {
|
||||
double vrp = eigvec[r*n + p];
|
||||
double vrq = eigvec[r*n + q];
|
||||
eigvec[r*n + p] = c*vrp - s*vrq;
|
||||
eigvec[r*n + q] = s*vrp + c*vrq;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// extract eigenvalues from diagonal
|
||||
for (int i = 0; i < n; i++) {
|
||||
eigval[i] = mat[i*n + i];
|
||||
}
|
||||
|
||||
return sweep;
|
||||
}
|
||||
|
||||
|
||||
// transform vector by pose
|
||||
void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4],
|
||||
const double vec[3]) {
|
||||
@@ -1189,10 +1264,10 @@ template<typename T> std::string VectorToString(const std::vector<T>& v) {
|
||||
return s;
|
||||
}
|
||||
|
||||
template std::string VectorToString(const std::vector<int>& v);
|
||||
template std::string VectorToString(const std::vector<float>& v);
|
||||
template std::string VectorToString(const std::vector<double>& v);
|
||||
template std::string VectorToString(const std::vector<std::string>& v);
|
||||
template MJAPI std::string VectorToString(const std::vector<int>& v);
|
||||
template MJAPI std::string VectorToString(const std::vector<float>& v);
|
||||
template MJAPI std::string VectorToString(const std::vector<double>& v);
|
||||
template MJAPI std::string VectorToString(const std::vector<std::string>& v);
|
||||
|
||||
namespace {
|
||||
|
||||
@@ -1258,7 +1333,7 @@ template <typename T> std::vector<T> StringToVector(char* cs) {
|
||||
return v;
|
||||
}
|
||||
|
||||
template<> std::vector<std::string> StringToVector(const std::string& s) {
|
||||
template<> MJAPI std::vector<std::string> StringToVector(const std::string& s) {
|
||||
std::vector<std::string> v;
|
||||
std::stringstream ss(s);
|
||||
std::string word;
|
||||
@@ -1268,17 +1343,17 @@ template<> std::vector<std::string> StringToVector(const std::string& s) {
|
||||
return v;
|
||||
}
|
||||
|
||||
template std::vector<int> StringToVector(char* cs);
|
||||
template std::vector<float> StringToVector(char* cs);
|
||||
template std::vector<double> StringToVector(char* cs);
|
||||
template MJAPI std::vector<int> StringToVector(char* cs);
|
||||
template MJAPI std::vector<float> StringToVector(char* cs);
|
||||
template MJAPI std::vector<double> StringToVector(char* cs);
|
||||
|
||||
|
||||
template <typename T> std::vector<T> StringToVector(const std::string& s) {
|
||||
return StringToVector<T>(const_cast<char*>(s.c_str()));
|
||||
}
|
||||
template std::vector<int> StringToVector(const std::string& s);
|
||||
template std::vector<float> StringToVector(const std::string& s);
|
||||
template std::vector<double> StringToVector(const std::string& s);
|
||||
template std::vector<unsigned char> StringToVector(const std::string& s);
|
||||
template MJAPI std::vector<int> StringToVector(const std::string& s);
|
||||
template MJAPI std::vector<float> StringToVector(const std::string& s);
|
||||
template MJAPI std::vector<double> StringToVector(const std::string& s);
|
||||
template MJAPI std::vector<unsigned char> StringToVector(const std::string& s);
|
||||
|
||||
} // namespace mujoco::user
|
||||
|
||||
+12
-4
@@ -26,6 +26,8 @@
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjexport.h>
|
||||
|
||||
const double mjEPS = 1E-14; // minimum value in various calculations
|
||||
const double mjMINMASS = 1E-6; // minimum mass allowed
|
||||
|
||||
@@ -157,6 +159,12 @@ double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3],
|
||||
// eigenvalue decomposition of symmetric 3x3 matrix
|
||||
int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]);
|
||||
|
||||
// Jacobi eigenvalue decomposition of symmetric n×n matrix
|
||||
// eigval[n]: output eigenvalues, eigvec[n*n]: output eigenvectors (columns)
|
||||
// mat[n*n]: input matrix (destroyed on output)
|
||||
// returns number of sweeps used
|
||||
MJAPI int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n);
|
||||
|
||||
// transform vector by pose
|
||||
void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]);
|
||||
|
||||
@@ -166,7 +174,7 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful
|
||||
namespace mujoco::user {
|
||||
|
||||
// utility class for handling file paths
|
||||
class FilePath {
|
||||
class MJAPI FilePath {
|
||||
public:
|
||||
FilePath() = default;
|
||||
explicit FilePath(const std::string& str) : path_(PathReduce(str)) {}
|
||||
@@ -251,11 +259,11 @@ struct Cleanup {
|
||||
std::vector<uint8_t> FileToMemory(const char* filename);
|
||||
|
||||
// convert vector to string separating elements by whitespace
|
||||
template<typename T> std::string VectorToString(const std::vector<T>& v);
|
||||
template<typename T> MJAPI std::string VectorToString(const std::vector<T>& v);
|
||||
|
||||
// convert string to vector
|
||||
template<typename T> std::vector<T> StringToVector(char *cs);
|
||||
template<typename T> std::vector<T> StringToVector(const std::string& s);
|
||||
template<typename T> MJAPI std::vector<T> StringToVector(char *cs);
|
||||
template<typename T> MJAPI std::vector<T> StringToVector(const std::string& s);
|
||||
|
||||
} // namespace mujoco::user
|
||||
|
||||
|
||||
@@ -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