Change flex strain constraints for trilinear to a more stable formulation.
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
committed by
Copybara-Service
Copybara-Service
parent
a5098a068a
commit
ac2730ab80
@@ -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
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/strain.xml>`__ 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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
+360
-221
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user