diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a771d3cd..ed720998 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4841,10 +4841,12 @@ degrees of freedom to avoid locking. It is only supported for dimension 2, i.e., :el-prefix:`equality/` |-| **flexstrain** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element constrains the strain invariants of a trilinear or quadratic flex to their initial values. Specifically, it -enforces that the trace and determinant of the deformation gradient remain constant, preserving volume and preventing -excessive stretching. This constraint type is only supported for dimension 3 trilinear flexes (i.e., volumetric -deformable bodies using trilinear interpolation). See `this model +This element constrains the strain of a trilinear or quadratic flex to its initial values. For trilinear elements, +a B-bar formulation is used to prevent volumetric locking: the trace of strain (I₁) and volume ratio (J-1 = det(F)-1) +are constrained at the element center, while the three off-diagonal shear components (E₁₂, E₁₃, E₂₃) are constrained +at each of the 8 Gauss points, giving 26 constraints per element. For quadratic elements, all 6 strain components +(3 invariants + 3 shear) are constrained at each of the 27 Gauss points, giving 162 constraints per element. This +constraint type is only supported for dimension 3 flexes with trilinear or quadratic interpolation. See `this model `__ for an example. .. _equality-flexstrain-name: @@ -4859,7 +4861,7 @@ deformable bodies using trilinear interpolation). See `this model .. _equality-flexstrain-flex: :at:`flex`: :at-val:`string, required` - Name of the flex whose strain invariants are being constrained. + Name of the flex whose strain is being constrained. .. _equality-distance: diff --git a/doc/includes/references.h b/doc/includes/references.h index 8c79e7b8..6d0d4896 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -609,7 +609,7 @@ typedef enum mjtEq_ { // type of equality constraint mjEQ_TENDON, // couple the lengths of two tendons with cubic mjEQ_FLEX, // fix all edge lengths of a flex mjEQ_FLEXVERT, // fix all vertex lengths of a flex - mjEQ_FLEXSTRAIN, // fix strain invariants of a trilinear flex + mjEQ_FLEXSTRAIN, // constrain strain of a trilinear/quadratic flex (B-bar) mjEQ_DISTANCE // unsupported, will cause an error if used } mjtEq; typedef enum mjtWrap_ { // type of tendon wrap object diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 449020a2..6f735c10 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -211,7 +211,7 @@ typedef enum mjtEq_ { // type of equality constraint mjEQ_TENDON, // couple the lengths of two tendons with cubic mjEQ_FLEX, // fix all edge lengths of a flex mjEQ_FLEXVERT, // fix all vertex lengths of a flex - mjEQ_FLEXSTRAIN, // fix strain invariants of a trilinear flex + mjEQ_FLEXSTRAIN, // constrain strain of a trilinear/quadratic flex (B-bar) mjEQ_DISTANCE // unsupported, will cause an error if used } mjtEq; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 0ab2360d..13ee8dda 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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; } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 557bbd4f..63357155 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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; }