diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d8c81bda..5eab4da6 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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]]; } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 8a7eabc7..f5db7108 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -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; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index e6d3b6fa..bf59900b 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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); } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index a86a2f33..d4c567d3 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -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]; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 244a48f2..3e448fb5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3818,6 +3818,48 @@ void inline ComputeLinearStiffness(std::vector& 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 mat(K_cell_data, K_cell_data + ndof * ndof); + std::vector eigval(ndof); + std::vector 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(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 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); + } } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 85245020..19672803 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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; } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 8ca60124..1f80949d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -983,6 +983,7 @@ class mjCFlex_ : public mjCBase { std::vector edgeidx_; // element edge ids std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix + bool has_strain_eq = false; // true if strain constraints reference this flex // variable-size data std::vector vertbody_; // vertex body names diff --git a/src/user/user_util.cc b/src/user/user_util.cc index d9ad6cb6..b7bbc00e 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -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 std::string VectorToString(const std::vector& v) { return s; } -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); namespace { @@ -1258,7 +1333,7 @@ template std::vector StringToVector(char* cs) { return v; } -template<> std::vector StringToVector(const std::string& s) { +template<> MJAPI std::vector StringToVector(const std::string& s) { std::vector v; std::stringstream ss(s); std::string word; @@ -1268,17 +1343,17 @@ template<> std::vector StringToVector(const std::string& s) { return v; } -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template std::vector StringToVector(const std::string& s) { return StringToVector(const_cast(s.c_str())); } -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/src/user/user_util.h b/src/user/user_util.h index 2eeb3a0f..6d17fb43 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -26,6 +26,8 @@ #include #include +#include + 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 FileToMemory(const char* filename); // convert vector to string separating elements by whitespace -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector -template std::vector StringToVector(char *cs); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(char *cs); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index d795a404..93e3b947 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -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"( + + + )"; + std::array 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"( + + + )"; + std::array 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"( + + + )"; + std::array 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"( diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 912a26c0..c2d3b69d 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -40,3 +40,5 @@ mujoco_test(user_composite_test) mujoco_test(user_resource_test) mujoco_test(user_vfs_test) + +mujoco_test(user_util_test) diff --git a/test/user/user_util_test.cc b/test/user/user_util_test.cc index 9873c74b..67c91e2b 100644 --- a/test/user/user_util_test.cc +++ b/test/user/user_util_test.cc @@ -17,6 +17,8 @@ #include "src/user/user_util.h" #include +#include +#include #include #include @@ -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 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 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 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 A(n * n); + composeMatrix(A.data(), Q.data(), eigvals.data(), n); + + // save copy for verification + std::vector A_copy(A); + + // decompose + std::vector found_eigval(n); + std::vector 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