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

Change flex strain constraints for trilinear to a more stable formulation.

Browse Source 
This change improves trilinear flex elements by using reduced integration for volumetric quantities (strain trace and volume ratio) at the element center, while adding full integration for shear components at 8 Gauss points and removing the second strain invariant from the constraints, which is negligible for small strains. This reduced integration "B-bar" technique is standard in finite element analysis and prevents artificial stiffness that can occur when low-order elements are nearly incompressible. The constraint count per trilinear element changes from 24 to 26 compared to using invariants. For quadratic elements, the full 27 quadrature point are used resulting in 162 constraints.

PiperOrigin-RevId: 884570752
Change-Id: Ib74ece8f4712c2c81fbfd784524fcac52a2c80e5
This commit is contained in:
Alessio Quaglino
2026-03-16 12:04:46 -07:00
committed by Copybara-Service
parent a5098a068a
commit ac2730ab80
5 changed files with 371 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+360 -221
View File
@@ -47,6 +47,122 @@
//-------------------------- 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 node positions and Jacobians for flex strain constraints
// xpos: output array of size 3*nodenum (global node positions)
// node_jac: output array of size 3*nodenum*nv (dense Jacobians)
// combined_chain: output array of DOF indices used by any node (sparse mode)
// combined_nnz: output number of entries in combined_chain
static void node_pos_and_jac(const mjModel* m, mjData* d, int f, int nv, int issparse, mjtNum* xpos,
mjtNum* node_jac, int* combined_chain, int* combined_nnz) {
int nodenum = m->flex_nodenum[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int nstart = m->flex_nodeadr[f];
for (int n = 0; n < nodenum; n++) {
if (m->flex_centered[f]) {
mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]);
} else {
mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart));
mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]);
}
}
int* chain_col = mjSTACKALLOC(d, nv, int);
mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
mju_zero(node_jac, 3*nodenum*nv);
for (int n = 0; n < nodenum; n++) {
int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col);
mju_zero(blk_jac, 3*nv);
mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col, 0);
for (int r = 0; r < 3; r++) {
for (int k = 0; k < chain_nnz; k++) {
node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k];
}
}
}
*combined_nnz = 0;
if (issparse) {
int* dof_used = mjSTACKALLOC(d, nv, int);
mju_zeroInt(dof_used, nv);
for (int n = 0; n < nodenum; n++) {
int temp_chain[200];
int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain);
for (int k = 0; k < temp_nnz; k++) {
dof_used[temp_chain[k]] = 1;
}
}
for (int q = 0; q < nv; q++) {
if (dof_used[q]) {
combined_chain[(*combined_nnz)++] = q;
}
}
}
}
// compute strain Jacobian from strain derivative w.r.t. node positions
// dSdx: input array of size 3*nodenum (dStrain/dNodePosition)
// node_jac: input array of size 3*nodenum*nv (dense Jacobians)
// strain_jac: output array of size nv (dStrain/dq)
static void strain_jacobian(int nodenum, int nv, const mjtNum* dSdx, const mjtNum* node_jac,
mjtNum* strain_jac) {
mju_zero(strain_jac, nv);
for (int n = 0; n < nodenum; n++) {
for (int c = 0; c < 3; c++) {
int row = 3*n + c;
for (int q = 0; q < nv; q++) {
strain_jac[q] += dSdx[row] * node_jac[row*nv + q];
}
}
}
}
// basis functions for flex strain constraints
static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) {
if (order == 1) {
@@ -67,6 +183,113 @@ static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) {
}
// 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, const mjtNum grad[8][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, const 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
@@ -635,10 +858,8 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEXSTRAIN: {
// strain constraint mode: add 24 constraints (3 invariants at 8 Gauss points)
int f = id[0];
int nodenum = m->flex_nodenum[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int order = m->flex_interp[f];
// skip if not interpolated (order == 0 or no nodes)
@@ -650,76 +871,26 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mj_markStack(d);
mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
mjtNum* node_jac = mjSTACKALLOC(d, 3*nodenum*nv, mjtNum);
int* chain_col = mjSTACKALLOC(d, nv, int);
mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum);
// compute global node positions from body states
// Green-Lagrange strain E = ½(F'F - I) is rotationally invariant,
// so we don't need corotational decomposition - use global positions directly
int nstart = m->flex_nodeadr[f];
for (int n = 0; n < nodenum; n++) {
if (m->flex_centered[f]) {
mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]);
} else {
mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart));
mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]);
}
}
// compute node Jacobians in global frame (3*nodenum rows x nv cols)
mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
mju_zero(node_jac, 3*nodenum*nv);
for (int n = 0; n < nodenum; n++) {
int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col);
mju_zero(blk_jac, 3*nv);
mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col,
/*flg_skipcommon=*/0);
// expand sparse Jacobian to dense row format
for (int r = 0; r < 3; r++) {
for (int k = 0; k < chain_nnz; k++) {
node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k];
}
}
}
// compute combined chain for all nodes (for sparse mode)
int* combined_chain = mjSTACKALLOC(d, nv, int);
mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum);
int combined_nnz = 0;
if (issparse) {
// mark which DOFs are used by any node body
int* dof_used = mjSTACKALLOC(d, nv, int);
mju_zeroInt(dof_used, nv);
for (int n = 0; n < nodenum; n++) {
int temp_chain[200]; // max DOFs per body
int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain);
for (int k = 0; k < temp_nnz; k++) {
dof_used[temp_chain[k]] = 1;
}
}
// build combined chain from marked DOFs
for (int q = 0; q < nv; q++) {
if (dof_used[q]) {
combined_chain[combined_nnz++] = q;
}
}
}
node_pos_and_jac(m, d, f, nv, issparse, xpos, node_jac, combined_chain, &combined_nnz);
// Gauss-Legendre quadrature points in [0,1]^3
// order=1: 2x2x2=8 points, order=2: 3x3x3=27 points
int nquad = order + 1; // quadrature order per axis
int ngauss = nquad * nquad * nquad; // total Gauss points
int nquad = order + 1;
int ngauss = nquad * nquad * nquad;
// 1D Gauss-Legendre points and weights on [0,1]
mjtNum gp1d[3]; // max 3 points for order=2
// 1D Gauss points
mjtNum gp1d[3];
if (nquad == 2) {
gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); // ~ 0.211
gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0); // ~ 0.789
} else { // nquad == 3
gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6); // ~ 0.113
gp1d[1] = 0.5; // 0.5
gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); // ~ 0.887
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);
}
// build 3D Gauss points array (max 27 points)
@@ -737,51 +908,81 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// loop over Gauss points
// get reference positions from m->flex_node0 (Cartesian positions at qpos0)
int nstart = m->flex_nodeadr[f];
mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
for (int n = 0; n < nodenum; n++) {
mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart));
}
// B-bar: precompute center-point values for volumetric constraint (trilinear only)
if (order == 1) {
mjtNum center[3] = {0.5, 0.5, 0.5};
mjtNum Fcur_c[9], Fref_c[9], Fref_inv_center[9], F_center[9];
// compute deformation gradient at center
mju_defGradient(Fcur_c, center, xpos, order);
mju_defGradient(Fref_c, center, refpos, order);
mat3_inverse(Fref_c, Fref_inv_center);
mju_mulMatMat3(F_center, Fcur_c, Fref_inv_center);
// compute C and E at center
mjtNum C_c[9], E_c[9];
mju_mulMatTMat3(C_c, F_center, F_center);
mju_scl(E_c, C_c, 0.5, 9);
E_c[0] -= 0.5;
E_c[4] -= 0.5;
E_c[8] -= 0.5;
// J = det(F) at center
mjtNum I1_center = E_c[0] + E_c[4] + E_c[8];
mjtNum J_center = mat3_det(F_center);
// compute shape function gradients at center (8 nodes for trilinear)
mjtNum grad_center[8][3];
shape_gradients(order, center, grad_center);
// add I1 and J-1 constraints at center (reduced integration for volumetric)
mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum);
for (int inv = 0; inv < 2; inv++) {
if (inv == 0) {
// I1 = tr(E), dI1/dE = I
cpos[0] = I1_center;
} else {
// J - 1 = det(F) - 1, dJ/dF = cofactor(F)
cpos[0] = J_center - 1.0;
}
volumetric_dSdx(inv, nodenum, grad_center, F_center, Fref_inv_center, dSdx);
strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac);
if (issparse) {
mjtNum* sparse_jac = mjSTACKALLOC(d, combined_nnz, mjtNum);
for (int k = 0; k < combined_nnz; k++) {
sparse_jac[k] = strain_jac[combined_chain[k]];
}
mj_addConstraint(m, d, sparse_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
combined_nnz, combined_chain);
} else {
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
}
}
}
// add I1 and J-1 constraints at center (reduced integration for volumetric)
mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum);
for (int g = 0; g < ngauss; g++) {
mjtNum* p = gauss[g];
// compute current position gradient dxcur/dxi
mjtNum Fcur[9];
mju_defGradient(Fcur, p, xpos, order);
// compute reference position gradient dxref/dxi
mjtNum Fref[9];
mju_defGradient(Fref, p, refpos, order);
// compute true deformation gradient F = Fcur * inv(Fref)
// F maps from reference to current configuration
mjtNum Fref_inv[9];
mju_transpose(Fref_inv, Fref, 3, 3); // start with transpose for cofactor
mjtNum det = Fref[0]*(Fref[4]*Fref[8] - Fref[5]*Fref[7]) -
Fref[1]*(Fref[3]*Fref[8] - Fref[5]*Fref[6]) +
Fref[2]*(Fref[3]*Fref[7] - Fref[4]*Fref[6]);
// compute adjugate (cofactor transposed)
Fref_inv[0] = (Fref[4]*Fref[8] - Fref[5]*Fref[7]) / det;
Fref_inv[1] = -(Fref[1]*Fref[8] - Fref[2]*Fref[7]) / det;
Fref_inv[2] = (Fref[1]*Fref[5] - Fref[2]*Fref[4]) / det;
Fref_inv[3] = -(Fref[3]*Fref[8] - Fref[5]*Fref[6]) / det;
Fref_inv[4] = (Fref[0]*Fref[8] - Fref[2]*Fref[6]) / det;
Fref_inv[5] = -(Fref[0]*Fref[5] - Fref[2]*Fref[3]) / det;
Fref_inv[6] = (Fref[3]*Fref[7] - Fref[4]*Fref[6]) / det;
Fref_inv[7] = -(Fref[0]*Fref[7] - Fref[1]*Fref[6]) / det;
Fref_inv[8] = (Fref[0]*Fref[4] - Fref[1]*Fref[3]) / det;
// F = Fcur * Fref_inv
mjtNum F[9];
mju_mulMatMat(F, Fcur, Fref_inv, 3, 3, 3);
// compute C = F'*F (right Cauchy-Green tensor)
mjtNum C[9];
mju_mulMatTMat3(C, F, F);
mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9];
mju_defGradient(Fcur, p, xpos, order);
mju_defGradient(Fref, p, refpos, order);
mat3_inverse(Fref, Fref_inv);
mju_mulMatMat3(F, Fcur, Fref_inv);
// compute Green-Lagrange strain E = 0.5*(C - I)
mjtNum E[9];
mjtNum C[9], E[9];
mju_mulMatTMat3(C, F, F);
for (int j = 0; j < 9; j++) {
E[j] = 0.5 * C[j];
}
@@ -790,135 +991,59 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
E[8] -= 0.5;
// compute 3 invariants of E
// I1 = tr(E) = E[0] + E[4] + E[8]
mjtNum I1 = E[0] + E[4] + E[8];
// I2 = 0.5*(tr(E)^2 - tr(E^2))
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);
// I3 = det(E)
mjtNum I3 = E[0]*(E[4]*E[8] - E[5]*E[7]) -
E[1]*(E[3]*E[8] - E[5]*E[6]) +
E[2]*(E[3]*E[7] - E[4]*E[6]);
mjtNum I3 = mat3_det(E);
// compute shape function gradients at this Gauss point
// grad[n][k] = d(basis_n)/d(p_k), for n in [0, nodenum), k in [0,3)
// indexing matches mju_defGradient: idx = i*(order+1)^2 + j*(order+1) + k
mjtNum grad[27][3]; // max 27 nodes for order=2
int npoint = (order + 1) * (order + 1) * (order + 1);
for (int n = 0; n < npoint; n++) {
int stride = order + 1;
int ix = n / (stride * stride);
int iy = (n / stride) % stride;
int iz = n % stride;
mjtNum grad[27][3];
shape_gradients(order, p, grad);
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);
// trilinear: 3 constraints per Gauss point (I1, I2, I3 skipped - only shear)
// quadratic: 6 constraints per Gauss point
for (int s = 0; s < 6; s++) {
// skip I1, I2, I3 for trilinear (I1, J-1 at center; I2 is small for small strain)
if (order == 1 && (s == 0 || s == 1 || s == 2)) {
continue;
}
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;
}
mjtNum dSdE[9];
mju_zero(dSdE, 9);
// compute Jacobian for each invariant
mjtNum invariants[3] = {I1, I2, I3};
for (int inv = 0; inv < 3; inv++) {
cpos[0] = invariants[inv];
// compute dI/dE (3x3 symmetric, stored as 9)
mjtNum dIdE[9];
if (inv == 0) {
// dI1/dE = I (identity)
mju_zero(dIdE, 9);
dIdE[0] = dIdE[4] = dIdE[8] = 1.0;
} else if (inv == 1) {
// dI2/dE = tr(E)*I - E
mju_zero(dIdE, 9);
dIdE[0] = I1 - E[0];
dIdE[4] = I1 - E[4];
dIdE[8] = I1 - E[8];
dIdE[1] = -E[1]; dIdE[3] = -E[3];
dIdE[2] = -E[2]; dIdE[6] = -E[6];
dIdE[5] = -E[5]; dIdE[7] = -E[7];
if (s == 0) {
// I1 = tr(E), dI1/dE = I (only for quadratic)
cpos[0] = I1;
dSdE[0] = dSdE[4] = dSdE[8] = 1.0;
} else if (s == 1) {
// I2 = 0.5*(tr(E)^2 - tr(E^2)), dI2/dE = tr(E)*I - E
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) {
// I3 = det(E), dI3/dE = cofactor(E)
cpos[0] = I3;
mat3_cofactor(E, dSdE);
} else {
// dI3/dE = cofactor(E) = det(E) * E^{-T} for invertible E
// For small strain, use adjugate directly
dIdE[0] = E[4]*E[8] - E[5]*E[7];
dIdE[1] = -(E[1]*E[8] - E[2]*E[7]);
dIdE[2] = E[1]*E[5] - E[2]*E[4];
dIdE[3] = -(E[3]*E[8] - E[5]*E[6]);
dIdE[4] = E[0]*E[8] - E[2]*E[6];
dIdE[5] = -(E[0]*E[5] - E[2]*E[3]);
dIdE[6] = E[3]*E[7] - E[4]*E[6];
dIdE[7] = -(E[0]*E[7] - E[1]*E[6]);
dIdE[8] = E[0]*E[4] - E[1]*E[3];
// off-diagonal entries: s=3->E12, s=4->E13, s=5->E23
int offdiag_idx[3] = {1, 2, 5};
int ij = offdiag_idx[s - 3];
cpos[0] = E[ij];
dSdE[ij] = 1.0;
}
// dI/dx_n = sum over i,j: dI/dE_ij * dE_ij/dx_n
// where dE_ij/dx_n = 0.5 * d(F'F)_ij/dx_n
// d(F'F)_ij/dx_n = sum_k (dF_ki/dx_n * F_kj + F_ki * dF_kj/dx_n)
//
// With F = Fcur * Fref_inv:
// dF_ab/d(x_n)_c = sum_k (dFcur_ak/d(x_n)_c) * Fref_inv[kb]
// = sum_k delta_{ac} * grad[n][k] * Fref_inv[kb]
// So only row a=c contributes
// compute dS/dx for all nodes
invariant_dSdx(nodenum, (const mjtNum(*)[3])grad, F, Fref_inv, dSdE, dSdx);
strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac);
// compute dI/dx for all nodes (3*nodenum values)
mjtNum* dIdx = mjSTACKALLOC(d, 3*nodenum, mjtNum);
mju_zero(dIdx, 3*nodenum);
for (int n = 0; n < nodenum; n++) {
for (int c = 0; c < 3; c++) { // component of x_n
mjtNum dI = 0;
for (int ij = 0; ij < 9; ij++) {
int ii = ij / 3; // row of E/C
int jj = ij % 3; // col of E/C
// dF_ab/d(x_n)_c = sum_k grad[n][k] * Fref_inv[kb] (only for a=c)
// dC_ij/d(x_n)_c = dF_ci * F_cj + F_ci * dF_cj
// = (sum_k grad[n][k]*Fref_inv[ki]) * F_cj
// + F_ci * (sum_k grad[n][k]*Fref_inv[kj])
// compute dF_ci = sum_k grad[n][k] * Fref_inv[ki]
mjtNum dF_ci = 0;
for (int k = 0; k < 3; k++) {
dF_ci += grad[n][k] * Fref_inv[k*3 + ii];
}
// compute dF_cj = sum_k grad[n][k] * Fref_inv[kj]
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;
// dE_ij = 0.5 * dC_ij
dI += dIdE[ij] * 0.5 * dC_ij;
}
dIdx[3*n + c] = dI;
}
}
// strain_jac[q] = sum_n sum_c dIdx[3*n+c] * node_jac[(3*n+c)*nv + q]
mju_zero(strain_jac, nv);
for (int n = 0; n < nodenum; n++) {
for (int c = 0; c < 3; c++) {
int row = 3*n + c;
for (int q = 0; q < nv; q++) {
strain_jac[q] += dIdx[row] * node_jac[row*nv + q];
}
}
}
// add constraint: sparse or dense
// add constraint
if (issparse) {
// convert dense strain_jac to sparse format
mjtNum* sparse_jac = mjSTACKALLOC(d, combined_nnz, mjtNum);
for (int k = 0; k < combined_nnz; k++) {
sparse_jac[k] = strain_jac[combined_chain[k]];
@@ -1562,17 +1687,24 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEXSTRAIN: {
// strain constraints: 24 constraints, use average node inv weight
// strain constraints: use average node inv weight
int flex_id = m->eq_obj1id[id];
int nodenum = m->flex_nodenum[flex_id];
int nstart = m->flex_nodeadr[flex_id];
int order = m->flex_interp[flex_id];
// compute constraint count: trilinear (2 + 3*8 = 26), quadratic (6*27 = 162)
int nquad = order + 1;
int ngauss = nquad * nquad * nquad;
int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss);
mjtNum avg_invweight = 0;
for (int n = 0; n < nodenum; n++) {
int bodyid = m->flex_nodebodyid[nstart + n];
avg_invweight += m->body_invweight0[2*bodyid];
}
avg_invweight /= nodenum;
for (int c = 0; c < 24; c++) {
for (int c = 0; c < nconstraint; c++) {
dA[i++] = avg_invweight;
}
@@ -2161,7 +2293,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
break;
case mjEQ_FLEXSTRAIN: {
// strain constraints: 3 invariants × ngauss Gauss points
// strain constraints:
// Q1: B-bar, 2 center (I1, J-1) + 3*8 shear = 26
// Q2: full 3x3x3 Gauss, 6*27 = 162
// skip if not interpolated (order == 0 or no nodes)
int order = m->flex_interp[id[0]];
int nodenum = m->flex_nodenum[id[0]];
@@ -2170,16 +2304,20 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
}
int nquad = order + 1; // 2 for order=1, 3 for order=2
int ngauss = nquad * nquad * nquad; // 8 or 27
size = 3 * ngauss; // 24 or 81
size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // 26 or 162
if (nnz) {
// NV is sum of all node Jacobians
// Count unique DOFs across all node bodies (matching instantiation)
int nstart = m->flex_nodeadr[id[0]];
int* nodebodies = mjSTACKALLOC(d, nodenum, int);
for (int n = 0; n < nodenum; n++) {
int bodyid = m->flex_nodebodyid[nstart + n];
NV += mj_bodyChain(m, bodyid, chain);
nodebodies[n] = m->flex_nodebodyid[nstart + n];
}
// each constraint row shares this NV
// mj_jacSumCount deduplicates shared DOFs
NV = mj_jacSumCount(m, d, chain, nodenum, nodebodies);
// each constraint row shares this combined NV
NV = size * NV;
}
break;
@@ -2368,7 +2506,8 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
int ne_allocated = mj_ne(m, d, nnz);
int nf_allocated = mj_instantiateFriction(m, d, 1, nnz);
int nl_allocated = mj_instantiateLimit(m, d, 1, nnz);
int nefc_allocated = ne_allocated + nf_allocated + nl_allocated + mj_nc(m, d, nnz);
int nc_allocated = mj_nc(m, d, nnz);
int nefc_allocated = ne_allocated + nf_allocated + nl_allocated + nc_allocated;
if (!mj_isSparse(m)) {
d->nJ = nefc_allocated * m->nv;
}
src/engine/engine_core_smooth.c
+2 -2
View File
@@ -2579,14 +2579,14 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEXSTRAIN: {
// increment with 3 invariants × ngauss Gauss points
// increment: trilinear uses 2 center (I1,J-1) + 3*ngauss shear, quadratic uses 6*ngauss
k = m->eq_obj1id[id];
int order = m->flex_interp[k];
int nodenum = m->flex_nodenum[k];
if (order && nodenum) {
int nquad = order + 1;
int ngauss = nquad * nquad * nquad;
i += 3 * ngauss;
i += (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss);
}
break;
}