Implement bending forces for interpolated flex shells.
This change adds a new passive force computation for flexes with elastic2d="bend" and dof="trilinear". The bending energy is based on the squared difference of normals between adjacent face elements at their shared edge midpoint. The edge data is precomputed during model compilation and stored in flex_bending. PiperOrigin-RevId: 910772638 Change-Id: I3b12c7b7f1ba6ac1875df495d89e8cfec921ca80
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
a692283db3
commit
d933b195ee
@@ -33,7 +33,6 @@
|
||||
<flexcomp type="grid" count="9 19 1" spacing=".05 .05 .05" mass="10"
|
||||
name="flag" radius="0.001">
|
||||
<edge equality="true" damping="0.001"/>
|
||||
<elasticity poisson="0" thickness="1e-2" young="3e6" elastic2d="none"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
origin="0 0 0" count="8 2 12" cellcount="6 1 6"
|
||||
type="grid" name="soft_mesh_2" dim="3" spacing=".025 .05 .025"
|
||||
mass="0.43" radius="0.005" dof="trilinear" rgba="0.9 0.7 0.7 1">
|
||||
<elasticity young="0" poisson="0.3" damping="0.01" elastic2d="bend" thickness="0.03"/>
|
||||
<elasticity young="1e5" poisson="0.3" damping="0" elastic2d="bend" thickness="0.03"/>
|
||||
<edge equality="strain" solref="0.001 1" solimp="0.99 0.999 0.001"/>
|
||||
<contact selfcollide="none"
|
||||
internal="false"
|
||||
|
||||
@@ -975,6 +975,11 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
|
||||
int order = m->flex_interp[f];
|
||||
int shell_mode = order < 0;
|
||||
order = order < 0 ? -order : order;
|
||||
|
||||
// warn that bending derivatives are not yet implemented
|
||||
if (shell_mode) {
|
||||
mj_warning(d, mjWARN_INERTIA, f); // bending implicit derivatives missing
|
||||
}
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
|
||||
+196
-1
@@ -201,6 +201,196 @@ static void mj_flexPassiveInterp(const mjModel* m, mjData* d, int f,
|
||||
}
|
||||
|
||||
|
||||
// 2D shape function gradient: dir=0 returns dphi(s0,l0)*phi(s1,l1),
|
||||
// dir=1 returns phi(s0,l0)*dphi(s1,l1)
|
||||
static inline mjtNum mju_dphi2D(mjtNum s0, int l0, mjtNum s1, int l1,
|
||||
int order, int dir) {
|
||||
if (dir == 0) {
|
||||
return mju_flexDphi(s0, l0, order) * mju_flexPhi(s1, l1, order);
|
||||
} else {
|
||||
return mju_flexPhi(s0, l0, order) * mju_flexDphi(s1, l1, order);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// per-edge data layout in flex_bending for interpolated shell bending
|
||||
#define BEND_EDGE_SIZE 10
|
||||
|
||||
|
||||
// passive bending forces for interpolated flex shell
|
||||
//
|
||||
// Current approach: discrete Crouzeix-Raviart — point evaluation of the
|
||||
// normal jump at each edge midpoint, with energy E = D/(2h) * |Δn - Δn₀|² * l.
|
||||
//
|
||||
// TODO(quaglino): upgrade to a Galerkin formulation by precomputing a bending
|
||||
// stiffness matrix K_bend in the corotated frame using edge normal-jump residuals
|
||||
// as DOFs and Gauss integration over each face. At runtime, rotate the residual
|
||||
// vector into the corotated frame, multiply by K_bend, and rotate back. This
|
||||
// would give implicit derivatives for free (K_bend is constant) and better
|
||||
// accuracy for elements with varying curvature.
|
||||
static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f,
|
||||
int enbl_spring, int enbl_damper) {
|
||||
// read bending edge data
|
||||
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[f];
|
||||
int nedge = (int)bdata[0];
|
||||
if (nedge == 0) return;
|
||||
|
||||
int order = -m->flex_interp[f]; // shell_mode: interp < 0
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int npe = (order+1)*(order+1);
|
||||
int nodenum = m->flex_nodenum[f];
|
||||
|
||||
mj_markStack(d);
|
||||
|
||||
// gather global state
|
||||
mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||
mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||
mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||
mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||
mju_flexGatherState(m, d, f, xpos_g, vel_g);
|
||||
mju_zero(frc_g, 3*nodenum);
|
||||
mju_zero(dmp_g, 3*nodenum);
|
||||
|
||||
// per-face temporaries
|
||||
mjtNum* xpos_A = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* xpos_B = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* vel_A = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* vel_B = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
int* gidx_A = mjSTACKALLOC(d, npe, int);
|
||||
int* gidx_B = mjSTACKALLOC(d, npe, int);
|
||||
|
||||
mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0;
|
||||
if (enbl_damper && kD > 0) {
|
||||
mju_warning("Bending damping is not yet supported for interpolated flex shells.");
|
||||
}
|
||||
|
||||
for (int e = 0; e < nedge; e++) {
|
||||
const mjtNum* edata = bdata + 1 + e * BEND_EDGE_SIZE;
|
||||
int fe_A = (int)edata[0];
|
||||
int fe_B = (int)edata[1];
|
||||
mjtNum local_A[2] = {edata[2], edata[3]};
|
||||
mjtNum local_B[2] = {edata[4], edata[5]};
|
||||
mjtNum stiffness = edata[6];
|
||||
mjtNum dn0[3] = {edata[7], edata[8], edata[9]};
|
||||
|
||||
if (stiffness == 0) continue;
|
||||
|
||||
// gather face A and B positions + velocities
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, fe_A, xpos_g,
|
||||
enbl_damper ? vel_g : NULL, NULL,
|
||||
xpos_A, enbl_damper ? vel_A : NULL, NULL,
|
||||
gidx_A, NULL);
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, fe_B, xpos_g,
|
||||
enbl_damper ? vel_g : NULL, NULL,
|
||||
xpos_B, enbl_damper ? vel_B : NULL, NULL,
|
||||
gidx_B, NULL);
|
||||
|
||||
// compute deformed normals at edge midpoint
|
||||
mjtNum n_A[3], t1_A[3], t2_A[3];
|
||||
mjtNum n_B[3], t1_B[3], t2_B[3];
|
||||
mju_flexFaceNormal2D(n_A, t1_A, t2_A, order, xpos_A, local_A);
|
||||
mju_flexFaceNormal2D(n_B, t1_B, t2_B, order, xpos_B, local_B);
|
||||
|
||||
// normalize normals
|
||||
mjtNum len_A = mju_norm3(n_A);
|
||||
mjtNum len_B = mju_norm3(n_B);
|
||||
if (len_A < mjMINVAL || len_B < mjMINVAL) continue;
|
||||
mjtNum inv_A = 1.0 / len_A;
|
||||
mjtNum inv_B = 1.0 / len_B;
|
||||
mji_scl3(n_A, n_A, inv_A);
|
||||
mji_scl3(n_B, n_B, inv_B);
|
||||
|
||||
// normal jump residual: r = (n_A - n_B) - dn0
|
||||
mjtNum r[3];
|
||||
mji_sub3(r, n_A, n_B);
|
||||
r[0] -= dn0[0]; r[1] -= dn0[1]; r[2] -= dn0[2];
|
||||
|
||||
// --- spring force ---
|
||||
if (enbl_spring) {
|
||||
// w_A = P_A * r = (r - n_A*(n_A.r)) / |c_A|
|
||||
mjtNum dot_A = mju_dot3(n_A, r);
|
||||
mjtNum w_A[3];
|
||||
w_A[0] = (r[0] - n_A[0]*dot_A) * inv_A;
|
||||
w_A[1] = (r[1] - n_A[1]*dot_A) * inv_A;
|
||||
w_A[2] = (r[2] - n_A[2]*dot_A) * inv_A;
|
||||
|
||||
mjtNum dot_B = mju_dot3(n_B, r);
|
||||
mjtNum w_B[3];
|
||||
w_B[0] = (r[0] - n_B[0]*dot_B) * inv_B;
|
||||
w_B[1] = (r[1] - n_B[1]*dot_B) * inv_B;
|
||||
w_B[2] = (r[2] - n_B[2]*dot_B) * inv_B;
|
||||
|
||||
// precompute cross products: wA x t2_A, wA x t1_A
|
||||
mjtNum wAt2[3], wAt1[3], wBt2[3], wBt1[3];
|
||||
mji_cross(wAt2, w_A, t2_A);
|
||||
mji_cross(wAt1, w_A, t1_A);
|
||||
mji_cross(wBt2, w_B, t2_B);
|
||||
mji_cross(wBt1, w_B, t1_B);
|
||||
|
||||
// force on face A nodes: f_k = stiffness * [g0_k * (wA x t2_A) -
|
||||
// g1_k * (wA x t1_A)]
|
||||
int idx = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
mjtNum g0 = mju_dphi2D(local_A[0], l0, local_A[1], l1, order, 0);
|
||||
mjtNum g1 = mju_dphi2D(local_A[0], l0, local_A[1], l1, order, 1);
|
||||
int gi = gidx_A[idx];
|
||||
for (int j = 0; j < 3; j++) {
|
||||
frc_g[3*gi + j] += stiffness * (g0 * wAt2[j] - g1 * wAt1[j]);
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
}
|
||||
|
||||
// force on face B nodes (negative sign: ∂Δn/∂x = -∂n_B/∂x)
|
||||
idx = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
mjtNum g0 = mju_dphi2D(local_B[0], l0, local_B[1], l1, order, 0);
|
||||
mjtNum g1 = mju_dphi2D(local_B[0], l0, local_B[1], l1, order, 1);
|
||||
int gi = gidx_B[idx];
|
||||
for (int j = 0; j < 3; j++) {
|
||||
frc_g[3*gi + j] -= stiffness * (g0 * wBt2[j] - g1 * wBt1[j]);
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --- damping force ---
|
||||
// TODO(quaglino): bending damping is disabled because at corner edges
|
||||
// with nonzero rest normal jump, dΔn/dt = ω × Δn₀ ≠ 0 under rigid
|
||||
// rotation, producing anti-conservative forces. A correct implementation
|
||||
// would subtract the rigid-body velocity component before computing the
|
||||
// damping residual.
|
||||
}
|
||||
|
||||
// apply accumulated forces to bodies
|
||||
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
|
||||
for (int i = 0; i < nodenum; i++) {
|
||||
int bid = bodyid[i];
|
||||
int nidx = i + m->flex_nodeadr[f];
|
||||
|
||||
// fast path: node at body origin, direct DOF write
|
||||
if (m->body_dofnum[bid] > 0 &&
|
||||
(m->flex_centered[f] ||
|
||||
(m->flex_node[3*nidx+0] == 0 &&
|
||||
m->flex_node[3*nidx+1] == 0 &&
|
||||
m->flex_node[3*nidx+2] == 0))) {
|
||||
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
|
||||
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
|
||||
} else {
|
||||
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
|
||||
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
|
||||
}
|
||||
}
|
||||
|
||||
mj_freeStack(d);
|
||||
}
|
||||
|
||||
|
||||
// passive forces for flex bending
|
||||
static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f,
|
||||
int enbl_spring, int enbl_damper) {
|
||||
@@ -466,8 +656,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
}
|
||||
|
||||
if (m->flex_interp[f]) {
|
||||
// interpolated flex
|
||||
// interpolated flex: stretch forces
|
||||
mj_flexPassiveInterp(m, d, f, enbl_spring, enbl_damper);
|
||||
|
||||
// interpolated shell bending forces
|
||||
if (m->flex_interp[f] < 0) {
|
||||
mj_flexPassiveBendInterp(m, d, f, enbl_spring, enbl_damper);
|
||||
}
|
||||
} else {
|
||||
// add bending forces
|
||||
mj_flexPassiveBend(m, d, f, enbl_spring, enbl_damper);
|
||||
|
||||
@@ -533,47 +533,9 @@ void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3],
|
||||
|
||||
// ----------------------------- flex interpolation ------------------------------------------------
|
||||
|
||||
mjtNum static inline phi(mjtNum s, int i, int order) {
|
||||
if (order == 1) {
|
||||
return i == 0 ? 1 - s : s;
|
||||
} else if (order == 2) {
|
||||
switch (i) {
|
||||
case 0:
|
||||
return 2 * s * s - 3 * s + 1;
|
||||
case 1:
|
||||
return 4 * (s - s * s);
|
||||
case 2:
|
||||
return 2 * s * s - s;
|
||||
default:
|
||||
mjERROR("invalid index %d", i);
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
mjERROR("order must be 1 or 2");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum static inline dphi(mjtNum s, int i, int order) {
|
||||
if (order == 1) {
|
||||
return i == 0 ? -1 : 1;
|
||||
} else if (order == 2) {
|
||||
switch (i) {
|
||||
case 0:
|
||||
return 4 * s - 3;
|
||||
case 1:
|
||||
return 4 * (1 - 2 * s);
|
||||
case 2:
|
||||
return 4 * s - 1;
|
||||
default:
|
||||
mjERROR("invalid index %d, must be 0, 1, or 2", i);
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
mjERROR("order must be 1 or 2");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
// use shared shape functions from engine_util_misc.h
|
||||
#define phi mju_flexPhi
|
||||
#define dphi mju_flexDphi
|
||||
|
||||
// evaluate the deformation gradient at p using the nodal dof values
|
||||
void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order) {
|
||||
@@ -853,6 +815,29 @@ void mju_flexGatherFaceState(int order, int cx, int cy, int cz,
|
||||
}
|
||||
|
||||
|
||||
// compute unnormalized surface normal and tangent vectors at a parametric point
|
||||
// on a 2D face element; normal = t1 x t2 (unnormalized)
|
||||
void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3],
|
||||
int order, const mjtNum* xpos_f,
|
||||
const mjtNum local[2]) {
|
||||
mju_zero3(t1);
|
||||
mju_zero3(t2);
|
||||
int idx = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order);
|
||||
mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order);
|
||||
for (int d = 0; d < 3; d++) {
|
||||
t1[d] += xpos_f[3*idx + d] * grad0;
|
||||
t2[d] += xpos_f[3*idx + d] * grad1;
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
}
|
||||
mju_cross(normal, t1, t2);
|
||||
}
|
||||
|
||||
|
||||
//------------------------------ actuator models ---------------------------------------------------
|
||||
|
||||
// normalized muscle length-gain curve
|
||||
|
||||
@@ -116,6 +116,34 @@ MJAPI void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe,
|
||||
int axis0, int axis1, int normal_axis,
|
||||
const mjtNum local[2], mjtNum* quat);
|
||||
|
||||
// compute unnormalized surface normal and tangent vectors at a parametric point
|
||||
// on a 2D face element; normal = t1 x t2 (unnormalized)
|
||||
MJAPI void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3],
|
||||
int order, const mjtNum* xpos_f,
|
||||
const mjtNum local[2]);
|
||||
|
||||
|
||||
// 1D shape function: order 1 (linear) or 2 (quadratic), node index i
|
||||
static inline mjtNum mju_flexPhi(mjtNum s, int i, int order) {
|
||||
if (order == 1) return i == 0 ? 1 - s : s;
|
||||
switch (i) {
|
||||
case 0: return 2*s*s - 3*s + 1;
|
||||
case 1: return 4*(s - s*s);
|
||||
case 2: return 2*s*s - s;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 1D shape function gradient
|
||||
static inline mjtNum mju_flexDphi(mjtNum s, int i, int order) {
|
||||
if (order == 1) return i == 0 ? -1 : 1;
|
||||
switch (i) {
|
||||
case 0: return 4*s - 3;
|
||||
case 1: return 4*(1 - 2*s);
|
||||
case 2: return 4*s - 1;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// ----------------------------- Base64 ------------------------------------------------------------
|
||||
|
||||
|
||||
+250
-2
@@ -4168,6 +4168,7 @@ void mjCFlex::DelTexcoord() {
|
||||
|
||||
|
||||
void mjCFlex::ResolveReferences(const mjCModel* m) {
|
||||
interpolated = !nodebody_.empty();
|
||||
vertbodyid.clear();
|
||||
nodebodyid.clear();
|
||||
for (const auto& vertbody : vertbody_) {
|
||||
@@ -4279,6 +4280,247 @@ void mjCFlex::CacheStiffness() {
|
||||
}
|
||||
|
||||
|
||||
// compute interpolated shell bending edge data
|
||||
// enumerates intra-surface and corner edges, stores per-edge metadata:
|
||||
// [fe_A, fe_B, local_A[2], local_B[2], stiffness, dn0[3]]
|
||||
static void ComputeInterpBending(
|
||||
std::vector<double>& bending,
|
||||
const std::vector<double>& nodexpos_local,
|
||||
int order, const int cellcount[3],
|
||||
double young, double poisson, double thickness) {
|
||||
// bending modulus D = E * t^3 / (12 * (1 - nu^2))
|
||||
double D_bend = young * thickness * thickness * thickness /
|
||||
(12.0 * (1.0 - poisson * poisson));
|
||||
|
||||
int cx = cellcount[0], cy = cellcount[1], cz = cellcount[2];
|
||||
int ny_global = cy * order + 1;
|
||||
int nz_global = cz * order + 1;
|
||||
int npe = (order + 1) * (order + 1); // nodes per 2D face element
|
||||
|
||||
// face layout: 6 surfaces of the box
|
||||
// face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max,
|
||||
// face 4: z=0, face 5: z=max
|
||||
int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy};
|
||||
int face_normal[6] = {0, 0, 1, 1, 2, 2};
|
||||
int face_count1[6] = {cz, cz, cx, cx, cy, cy};
|
||||
int face_fixed[6] = {0, cx*order, 0, cy*order, 0, cz*order};
|
||||
|
||||
// gather node positions for one face element
|
||||
auto gather_face_nodes = [&](int face_id, int within_face,
|
||||
std::vector<double>& fpos) {
|
||||
int nax = face_normal[face_id];
|
||||
int a0 = (nax + 1) % 3;
|
||||
int a1 = (nax + 2) % 3;
|
||||
int c1 = face_count1[face_id];
|
||||
int gf = face_fixed[face_id];
|
||||
int q0 = within_face / c1;
|
||||
int q1 = within_face % c1;
|
||||
fpos.resize(3 * npe);
|
||||
int loc = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
int g[3];
|
||||
g[nax] = gf;
|
||||
g[a0] = q0 * order + l0;
|
||||
g[a1] = q1 * order + l1;
|
||||
int gidx = g[0] * ny_global * nz_global + g[1] * nz_global + g[2];
|
||||
mjuu_copyvec(fpos.data() + 3*loc, &nodexpos_local[3*gidx], 3);
|
||||
loc++;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// compute unnormalized normal and tangents at a parametric point
|
||||
auto compute_normal = [&](const std::vector<double>& fpos,
|
||||
const double local[2],
|
||||
double normal[3], double t1[3], double t2[3]) {
|
||||
mjuu_zerovec(t1, 3);
|
||||
mjuu_zerovec(t2, 3);
|
||||
int idx = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
double g0 = dphi(local[0], l0, order) * phi(local[1], l1, order);
|
||||
double g1 = phi(local[0], l0, order) * dphi(local[1], l1, order);
|
||||
for (int d = 0; d < 3; d++) {
|
||||
t1[d] += fpos[3*idx + d] * g0;
|
||||
t2[d] += fpos[3*idx + d] * g1;
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
}
|
||||
mjuu_crossvec(normal, t1, t2);
|
||||
};
|
||||
|
||||
// face cumulative offsets
|
||||
int face_cumul[6];
|
||||
face_cumul[0] = 0;
|
||||
for (int f = 1; f < 6; f++) {
|
||||
face_cumul[f] = face_cumul[f-1] + face_sizes[f-1];
|
||||
}
|
||||
|
||||
int face_count0[6];
|
||||
for (int f = 0; f < 6; f++) {
|
||||
face_count0[f] = face_sizes[f] / face_count1[f];
|
||||
}
|
||||
|
||||
int cells[3] = {cx, cy, cz};
|
||||
|
||||
// find the neighbor of face element (fid, q0, q1) across the edge in
|
||||
// direction dir (0=a0, 1=a1) at side (+1 or -1).
|
||||
// returns (fid_B, within_B) and fills local_A, local_B with parametric
|
||||
// midpoint coordinates on each side of the shared edge.
|
||||
auto get_neighbor = [&](int fid, int q0, int q1, int dir, int side, double local_A[2],
|
||||
double local_B[2]) -> std::pair<int, int> {
|
||||
int nax = fid / 2, sign_f = fid % 2;
|
||||
int a0 = (nax+1)%3, a1 = (nax+2)%3;
|
||||
int nc1 = face_count1[fid];
|
||||
|
||||
// parametric coordinates on face A at the shared edge
|
||||
local_A[0] = (dir == 0) ? (side > 0 ? 1.0 : 0.0) : 0.5;
|
||||
local_A[1] = (dir == 1) ? (side > 0 ? 1.0 : 0.0) : 0.5;
|
||||
|
||||
// check if neighbor is on the same face (internal)
|
||||
int q_nb = (dir == 0 ? q0 : q1) + side;
|
||||
int q_max = (dir == 0) ? face_count0[fid] : nc1;
|
||||
if (q_nb >= 0 && q_nb < q_max) {
|
||||
// internal neighbor
|
||||
int q0_B = (dir == 0) ? q_nb : q0;
|
||||
int q1_B = (dir == 0) ? q1 : q_nb;
|
||||
local_B[0] = (dir == 0) ? (side > 0 ? 0.0 : 1.0) : 0.5;
|
||||
local_B[1] = (dir == 1) ? (side > 0 ? 0.0 : 1.0) : 0.5;
|
||||
return {fid, q0_B * nc1 + q1_B};
|
||||
}
|
||||
|
||||
// boundary neighbor: cross to adjacent face on the box
|
||||
int ax = (dir == 0) ? a0 : a1; // axis being crossed
|
||||
int fid_B = 2*ax + (side > 0 ? 1 : 0); // neighboring face
|
||||
int nc1_B = face_count1[fid_B];
|
||||
|
||||
// the running coordinate along the shared edge maps to the neighbor face:
|
||||
// dir=0: edge runs along a1, maps to a0_B = (ax+1)%3 = a1 → q0_B
|
||||
// dir=1: edge runs along a0, maps to a1_B = (ax+2)%3 = a0 → q1_B
|
||||
// the boundary position maps to the other axis on face B (= nax of face A):
|
||||
// q_boundary = sign_f ? cells[nax]-1 : 0
|
||||
int q_run = (dir == 0) ? q1 : q0;
|
||||
int q_boundary = sign_f ? (cells[nax]-1) : 0;
|
||||
int q0_B, q1_B;
|
||||
if (dir == 0) {
|
||||
q0_B = q_run;
|
||||
q1_B = q_boundary;
|
||||
local_B[0] = 0.5;
|
||||
local_B[1] = sign_f ? 1.0 : 0.0;
|
||||
} else {
|
||||
q0_B = q_boundary;
|
||||
q1_B = q_run;
|
||||
local_B[0] = sign_f ? 1.0 : 0.0;
|
||||
local_B[1] = 0.5;
|
||||
}
|
||||
return {fid_B, q0_B * nc1_B + q1_B};
|
||||
};
|
||||
|
||||
struct BendEdge {
|
||||
int fe_A, fe_B; // global face element indices (for runtime)
|
||||
int fid_A, fid_B; // face id (0-5)
|
||||
int within_A, within_B; // within-face element index
|
||||
double local_A[2];
|
||||
double local_B[2];
|
||||
};
|
||||
std::vector<BendEdge> edges;
|
||||
|
||||
// enumerate all edges: for each face element, check 4 neighbors
|
||||
// (2 directions × 2 sides). Add each edge once via fe_A < fe_B.
|
||||
for (int f = 0; f < 6; f++) {
|
||||
int nc0 = face_count0[f];
|
||||
int nc1 = face_count1[f];
|
||||
for (int q0 = 0; q0 < nc0; q0++) {
|
||||
for (int q1 = 0; q1 < nc1; q1++) {
|
||||
int within_A = q0 * nc1 + q1;
|
||||
int fe_A = face_cumul[f] + within_A;
|
||||
|
||||
for (int dir = 0; dir < 2; dir++) {
|
||||
for (int side = -1; side <= 1; side += 2) {
|
||||
double lA[2], lB[2];
|
||||
auto [fid_B, within_B] = get_neighbor(f, q0, q1, dir, side, lA, lB);
|
||||
int fe_B = face_cumul[fid_B] + within_B;
|
||||
if (fe_A < fe_B) {
|
||||
BendEdge e;
|
||||
e.fe_A = fe_A; e.fid_A = f; e.within_A = within_A;
|
||||
e.fe_B = fe_B; e.fid_B = fid_B; e.within_B = within_B;
|
||||
mjuu_copyvec(e.local_A, lA, 2);
|
||||
mjuu_copyvec(e.local_B, lB, 2);
|
||||
edges.push_back(e);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// compute per-edge bending data
|
||||
const int BEND_EDGE_SIZE = 10; // should match engine_passive.c
|
||||
bending.resize(1 + edges.size() * BEND_EDGE_SIZE, 0);
|
||||
bending[0] = static_cast<double>(edges.size());
|
||||
|
||||
for (int e = 0; e < (int)edges.size(); e++) {
|
||||
const BendEdge& edge = edges[e];
|
||||
std::vector<double> fpos_A, fpos_B;
|
||||
gather_face_nodes(edge.fid_A, edge.within_A, fpos_A);
|
||||
gather_face_nodes(edge.fid_B, edge.within_B, fpos_B);
|
||||
|
||||
// compute rest normals at edge midpoint
|
||||
double n_A[3], t1_A[3], t2_A[3];
|
||||
double n_B[3], t1_B[3], t2_B[3];
|
||||
compute_normal(fpos_A, edge.local_A, n_A, t1_A, t2_A);
|
||||
compute_normal(fpos_B, edge.local_B, n_B, t1_B, t2_B);
|
||||
|
||||
// normalize
|
||||
double len_A = mjuu_normvec(n_A, 3);
|
||||
double len_B = mjuu_normvec(n_B, 3);
|
||||
if (len_A < 1e-12 || len_B < 1e-12) continue;
|
||||
|
||||
// rest normal jump
|
||||
double dn0[3] = {n_A[0]-n_B[0], n_A[1]-n_B[1], n_A[2]-n_B[2]};
|
||||
|
||||
// stiffness coefficient: D * l_e / h_e
|
||||
// determine which tangent is along vs across the edge for each face:
|
||||
// local[k] == 0.5 means parametric direction k runs along the edge
|
||||
double h_A, l_A, h_B, l_B;
|
||||
if (edge.local_A[0] == 0.5) {
|
||||
// edge runs along ξ on face A: t1 is along edge, t2 is across
|
||||
l_A = mjuu_normvec(t1_A, 3);
|
||||
h_A = mjuu_normvec(t2_A, 3);
|
||||
} else {
|
||||
// edge runs along η on face A: t2 is along edge, t1 is across
|
||||
h_A = mjuu_normvec(t1_A, 3);
|
||||
l_A = mjuu_normvec(t2_A, 3);
|
||||
}
|
||||
if (edge.local_B[0] == 0.5) {
|
||||
l_B = mjuu_normvec(t1_B, 3);
|
||||
h_B = mjuu_normvec(t2_B, 3);
|
||||
} else {
|
||||
h_B = mjuu_normvec(t1_B, 3);
|
||||
l_B = mjuu_normvec(t2_B, 3);
|
||||
}
|
||||
double h_avg = (h_A + h_B) / 2;
|
||||
double l_avg = (l_A + l_B) / 2;
|
||||
double stiffness_coeff = D_bend * l_avg / mjMAX(h_avg, 1e-12);
|
||||
|
||||
// pack into bending array
|
||||
double* edata = bending.data() + 1 + e * BEND_EDGE_SIZE;
|
||||
edata[0] = static_cast<double>(edge.fe_A);
|
||||
edata[1] = static_cast<double>(edge.fe_B);
|
||||
edata[2] = edge.local_A[0];
|
||||
edata[3] = edge.local_A[1];
|
||||
edata[4] = edge.local_B[0];
|
||||
edata[5] = edge.local_B[1];
|
||||
edata[6] = stiffness_coeff;
|
||||
edata[7] = dn0[0];
|
||||
edata[8] = dn0[1];
|
||||
edata[9] = dn0[2];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compiler
|
||||
void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
CopyFromSpec();
|
||||
@@ -4316,8 +4558,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
if (thickness <= 0) {
|
||||
throw mjCError(this, "2d elasticity requires positive thickness");
|
||||
}
|
||||
if (interpolated && elastic2d != 2) {
|
||||
mju_warning("bending passive force is not implemented for interpolated flex");
|
||||
if (poisson < 0.0 || poisson >= 0.5) {
|
||||
throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
|
||||
}
|
||||
if (dim != 2 && !interpolated) {
|
||||
throw mjCError(this, "2d elasticity requires 2d flex");
|
||||
@@ -4737,6 +4979,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
}
|
||||
}
|
||||
|
||||
// compute interpolated shell bending edge data (independent of stiffness cache)
|
||||
if (interpolated && (elastic2d == 1 || elastic2d == 3) && thickness > 0 && young > 0) {
|
||||
ComputeInterpBending(bending, nodexpos_local, spec.order, spec.cellcount,
|
||||
young, poisson, thickness);
|
||||
}
|
||||
|
||||
// create bounding volume hierarchy
|
||||
CreateBVH();
|
||||
|
||||
|
||||
@@ -2883,11 +2883,8 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
|
||||
}
|
||||
|
||||
// check errors
|
||||
if (dflex.elastic2d >= 2 && fcomp.equality) {
|
||||
throw mjXError(elem, "elasticity and edge constraints cannot both be present");
|
||||
}
|
||||
if (fcomp.equality == 3 && dflex.young > 0) {
|
||||
throw mjXError(elem, "strain constraint and elasticity (young) cannot both be present");
|
||||
if (dflex.elastic2d != 1 && fcomp.equality && dflex.young > 0) {
|
||||
throw mjXError(elem, "flex constraints and elasticity (young) cannot both be present");
|
||||
}
|
||||
|
||||
// contact
|
||||
|
||||
@@ -883,5 +883,119 @@ TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
// interpolated shell bending must produce zero spring forces at rest
|
||||
TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option gravity="0 0 0"/>
|
||||
<worldbody>
|
||||
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
|
||||
dim="3" cellcount="2 2 1" radius=".001" rgba="0 .7 .7 1"
|
||||
mass="5" name="softbody" dof="trilinear">
|
||||
<elasticity young="1e4" poisson="0.1" damping="0"
|
||||
elastic2d="bend" thickness="0.02"/>
|
||||
<contact selfcollide="none" internal="false"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024] = {0};
|
||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
||||
mjData* d = mj_makeData(m);
|
||||
|
||||
// verify bending data was compiled
|
||||
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
|
||||
int nedge = (int)bdata[0];
|
||||
EXPECT_GT(nedge, 0) << "no bending edges compiled";
|
||||
|
||||
mj_forward(m, d);
|
||||
|
||||
// all spring forces should be zero at rest
|
||||
for (int i = 0; i < m->nv; i++) {
|
||||
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
|
||||
<< "nonzero spring force at DOF " << i;
|
||||
}
|
||||
|
||||
// verify per-edge bending data
|
||||
int n_flat = 0, n_corner = 0;
|
||||
for (int e = 0; e < nedge; e++) {
|
||||
const mjtNum* edata = bdata + 1 + e * 10;
|
||||
mjtNum stiffness = edata[6];
|
||||
mjtNum dn0[3] = {edata[7], edata[8], edata[9]};
|
||||
mjtNum dn0_norm = mju_norm3(dn0);
|
||||
|
||||
// stiffness must be positive
|
||||
EXPECT_GT(stiffness, 0) << "edge " << e << " has non-positive stiffness";
|
||||
|
||||
if (dn0_norm < 1e-10) {
|
||||
// intra-surface edge: coplanar faces, zero normal jump
|
||||
n_flat++;
|
||||
} else {
|
||||
// corner edge: 90° between perpendicular face normals, |dn0| = sqrt(2)
|
||||
n_corner++;
|
||||
EXPECT_NEAR(dn0_norm, mju_sqrt(2.0), 1e-10)
|
||||
<< "corner edge " << e << " has unexpected |dn0|=" << dn0_norm;
|
||||
}
|
||||
}
|
||||
|
||||
// for a 2x2x1 box: 12 intra-surface + 20 corner = 32 edges
|
||||
EXPECT_GT(n_flat, 0) << "no intra-surface edges found";
|
||||
EXPECT_GT(n_corner, 0) << "no corner edges found";
|
||||
EXPECT_EQ(n_flat + n_corner, nedge);
|
||||
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
// interpolated shell bending must produce zero forces after a rigid rotation
|
||||
TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option gravity="0 0 0"/>
|
||||
<worldbody>
|
||||
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
|
||||
dim="3" cellcount="2 2 1" radius=".001" rgba="0 .7 .7 1"
|
||||
mass="5" name="softbody" dof="trilinear">
|
||||
<elasticity young="1e4" poisson="0.1" damping="0"
|
||||
elastic2d="bend" thickness="0.02"/>
|
||||
<contact selfcollide="none" internal="false"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024] = {0};
|
||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
||||
mjData* d = mj_makeData(m);
|
||||
|
||||
// apply a rigid rotation by setting all body quats to a 30 degree rotation
|
||||
// about z-axis (all flex node bodies get the same rotation)
|
||||
mjtNum angle = 30 * 3.14159265358979 / 180.0;
|
||||
mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2);
|
||||
for (int b = 1; b < m->nbody; b++) {
|
||||
int qadr = m->jnt_qposadr[m->body_jntadr[b]];
|
||||
if (m->body_jntnum[b] > 0 && m->jnt_type[m->body_jntadr[b]] == mjJNT_FREE) {
|
||||
d->qpos[qadr + 3] = ca;
|
||||
d->qpos[qadr + 4] = 0;
|
||||
d->qpos[qadr + 5] = 0;
|
||||
d->qpos[qadr + 6] = sa;
|
||||
}
|
||||
}
|
||||
|
||||
mj_forward(m, d);
|
||||
|
||||
// spring forces should still be zero (or very small) after rigid rotation
|
||||
for (int i = 0; i < m->nv; i++) {
|
||||
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-6)
|
||||
<< "nonzero spring force at DOF " << i << " after rigid rotation";
|
||||
}
|
||||
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
Reference in New Issue
Block a user