diff --git a/model/flex/flag.xml b/model/flex/flag.xml index c7bfa371..d35e1ead 100644 --- a/model/flex/flag.xml +++ b/model/flex/flag.xml @@ -33,7 +33,6 @@ - diff --git a/model/flex/hollow_vs_solid.xml b/model/flex/hollow_vs_solid.xml index 811090b1..73606d55 100644 --- a/model/flex/hollow_vs_solid.xml +++ b/model/flex/hollow_vs_solid.xml @@ -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"> - + 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]; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 7438f4c7..9a18b71c 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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); diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index ef9916a0..723e2625 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -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 diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 374f54c2..ebf2ced7 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -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 ------------------------------------------------------------ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index cf90b1a9..3083fb46 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -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& bending, + const std::vector& 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& 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& 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 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 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(edges.size()); + + for (int e = 0; e < (int)edges.size(); e++) { + const BendEdge& edge = edges[e]; + std::vector 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(edge.fe_A); + edata[1] = static_cast(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(); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index af291787..c31b1bf2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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 diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index f87dc0a5..461b0090 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -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"( + + + )"; + + 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"( + + + )"; + + 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