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

Add 2D membrane elasticity for interpolated flex shell mode

Browse Source 
When elastic2d="stretch" is set on an interpolated flexcomp, treat the bounding box boundary as membrane elements rather than volumetric cells. This computes plane-stress stiffness over the boundary faces and updates the runtime force/derivative kernels accordingly.

Interior vertex tracking (moving vertices that follow the deforming shell) is not yet implemented so all mesh vertices need to be on the bounding box surface or the background grid should have no interior nodes (i.e. cellcount should be 1 on at least one axis).

PiperOrigin-RevId: 907654080
Change-Id: I51b90e2f6a1d1b036f9604e42de20e377dc5d3f9
This commit is contained in:
Alessio Quaglino
2026-04-29 10:16:38 -07:00
committed by Copybara-Service
parent 517c113656
commit 9c6a4f76eb
15 changed files with 1378 additions and 349 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+2 -1
View File
@@ -4328,7 +4328,8 @@ stress-strain relationship. See also :ref:`deformable <CDeformable>` objects and
:at:`elastic2d`: :at-val:`[none, bend, stretch, both], "none"`
Elastic contribution to passive forces of 2D flexes. "none": none, "bend": bending only, "stretch": stretching only,
"both": bending and stretching. Not yet supported by :ref:`dof<body-flexcomp-dof>` **trilinear** and **quadratic**.
"both": bending and stretching. Bending is not yet supported by :ref:`dof<body-flexcomp-dof>` **trilinear** and
**quadratic**.
.. _flex-contact:
doc/changelog.rst
+2
View File
@@ -6,6 +6,8 @@ Upcoming version (not yet released)
-----------------------------------
- Added island support for the :ref:`PGS solver<soAlgorithms>`.
- Added support for :ref:`elastic2d<body-flexcomp-elastic2d>` for trilinear and quadratic flex
:ref:`dofs<body-flexcomp-dof>`.
Python
^^^^^^
model/flex/hollow_vs_solid.xml
+102
View File
@@ -0,0 +1,102 @@
<!-- Copyright 2026 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco>
<include file="scene.xml"/>
<option cone="elliptic" solver="CG" />
<default>
<default class="gripper">
<geom type="box" size="0.02 0.15 0.02" rgba="0.5 0.5 0.5 1"
solref="0.001 1" friction="2 0.1 0.1"/>
<joint type="slide" axis="0 0 1" range="-1 1"/>
<default class="finger">
<geom size="0.06 0.01 0.05"/>
<joint type="slide" range="0 0.1"/>
</default>
</default>
</default>
<worldbody>
<body name="solid" pos=".32 0 .2">
<flexcomp
origin="0 0 0" count="8 2 12" cellcount="6 1 6"
type="grid" name="soft_mesh" dim="3" spacing=".025 .05 .025"
mass="0.43" radius="0.005" dof="trilinear" rgba="0.7 0.9 0.7 1">
<edge equality="strain" solref="0.001 1"/>
<contact selfcollide="none"
internal="false"
friction="2.0 0.5 0.01"
solref="0.001 1"
solimp="0.99 0.999 0.001"/>
</flexcomp>
</body>
<body name="hollow" pos=".32 .45 .2">
<flexcomp
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"/>
<edge equality="strain" solref="0.001 1"/>
<contact selfcollide="none"
internal="false"
friction="2.0 0.5 0.01"
solref="0.001 1"
solimp="0.99 0.999 0.001"/>
</flexcomp>
</body>
<body name="gripper_solid" pos="0.2 0.45 0.2">
<joint name="lift_solid" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_r" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_r" axis="0 1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
<body name="right_finger_r" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_r" axis="0 -1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
</body>
<body name="gripper_hollow" pos="0.2 0 0.2">
<joint name="lift_hollow" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_s" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_s" axis="0 1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
<body name="right_finger_s" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_s" axis="0 -1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
</body>
</worldbody>
<equality>
<joint joint1="right_slide_r" joint2="left_slide_r"/>
<joint joint1="right_slide_s" joint2="left_slide_s"/>
</equality>
<actuator>
<position name="lift_r" joint="lift_solid" kp="400" dampratio="1" ctrlrange="-1 1"/>
<position name="grasp_r" joint="left_slide_r" kp="10" dampratio="1" ctrlrange="0 5"/>
<position name="lift_s" joint="lift_hollow" kp="400" dampratio="1" ctrlrange="-1 1"/>
<position name="grasp_s" joint="left_slide_s" kp="10" dampratio="1" ctrlrange="0 5"/>
</actuator>
</mujoco>
src/engine/engine_core_constraint.c
+166 -96
View File
@@ -706,11 +706,12 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEXSTRAIN: {
// each constraint represents a single cell; cell index in eq_data
// each constraint represents a single element (3D cell or 2D face)
int f = id[0];
int nodenum = m->flex_nodenum[f];
int order = m->flex_interp[f];
order = order < 0 ? -order : order;
int interp = m->flex_interp[f];
int order = interp < 0 ? -interp : interp;
int shell_mode = (interp < 0);
// skip if not interpolated (order == 0 or no nodes)
if (!order || !nodenum) {
@@ -722,60 +723,93 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mjERROR("flex strain constraints only support order 1 and 2, got %d", order);
}
int npc = (order+1)*(order+1)*(order+1);
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 nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + nstart;
// read cell index from eq_data
int ci = (int)data[0];
int cj = (int)data[1];
int ck = (int)data[2];
// nodes per element and element index
int npe;
int elem_idx;
if (shell_mode) {
npe = (order+1) * (order+1);
elem_idx = (int)data[0]; // face element index
} else {
npe = (order+1) * (order+1) * (order+1);
int ci = (int)data[0];
int cj = (int)data[1];
int ck = (int)data[2];
elem_idx = ci * cy * cz + cj * cz + ck;
}
mj_markStack(d);
// get cell node indices
// get element node indices
int gindices[125]; // max npc = 125 for quadratic
mju_flexGatherCellState(order, cy, cz, ci, cj, ck,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
if (shell_mode) {
mju_flexGatherFaceState(order, cx, cy, cz, elem_idx,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
} else {
int ci = (int)data[0], cj = (int)data[1], ck = (int)data[2];
mju_flexGatherCellState(order, cy, cz, ci, cj, ck,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
}
// compute positions only for cell nodes (npc << nodenum)
mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
for (int n = 0; n < npc; n++) {
// compute positions only for element nodes (npe << nodenum)
mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
mjtNum* refpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
for (int n = 0; n < npe; n++) {
int gn = gindices[n];
if (m->flex_centered[f] ||
(m->flex_node[3*(gn + nstart)+0] == 0 &&
m->flex_node[3*(gn + nstart)+1] == 0 &&
m->flex_node[3*(gn + nstart)+2] == 0)) {
mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
mju_copy3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]);
} else {
mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart));
mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
mju_mulMatVec3(xpos_e + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart));
mju_addTo3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]);
}
mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart));
mju_copy3(refpos_e + 3*n, m->flex_node0 + 3*(gn + nstart));
}
// compute corotational quaternion from cell-local positions
mjtNum cell_quat[4] = {1, 0, 0, 0};
{
// compute corotational quaternion
mjtNum elem_quat[4] = {1, 0, 0, 0};
if (shell_mode) {
// determine face normal axis from elem_idx
int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy};
int face_normals[6] = {0, 0, 1, 1, 2, 2};
int cumul = 0, normal_axis = 0;
for (int ff = 0; ff < 6; ff++) {
if (elem_idx < cumul + face_sizes[ff]) {
normal_axis = face_normals[ff];
break;
}
cumul += face_sizes[ff];
}
int na0 = (normal_axis + 1) % 3;
int na1 = (normal_axis + 2) % 3;
// compute corotational rotation from 2D deformation gradient at face center
mjtNum p[2] = {.5, .5};
mju_flexInterpRotation2D(order, xpos_e, npe, na0, na1, normal_axis, p, elem_quat);
} else {
mjtNum center[3] = {0.5, 0.5, 0.5};
mjtNum mat[9];
mju_defGradient(mat, center, xpos_c, order);
mju_mat2Rot(cell_quat, mat);
mju_negQuat(cell_quat, cell_quat);
mju_defGradient(mat, center, xpos_e, order);
mju_mat2Rot(elem_quat, mat);
mju_negQuat(elem_quat, elem_quat);
}
// build per-cell sparse chain and node Jacobians
int* cell_chain = mjSTACKALLOC(d, nv, int);
int cell_nnz = 0;
mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain,
&cell_nnz);
// build per-element sparse chain and node Jacobians
int* elem_chain = mjSTACKALLOC(d, nv, int);
int elem_nnz = 0;
mjtNum* elem_node_jac = cell_pos_and_jac(m, d, f, npe, gindices, nv, xpos_e, elem_chain,
&elem_nnz);
mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum);
mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* strain_jac = mjSTACKALLOC(d, elem_nnz, mjtNum);
mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npe, mjtNum);
// for dense mode: allocate and zero a dense Jacobian buffer once
mjtNum* dense_jac = NULL;
@@ -785,58 +819,55 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
}
// read eigenmode data from flex_stiffness
int ndof_cell = 3 * npc;
int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2]
+ cj * m->flex_cellnum[3*f+2] + ck;
const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
+ cell_idx * ndof_cell * ndof_cell;
int neig = (int)k_cell[0];
int ndof_elem = 3 * npe;
const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f]
+ elem_idx * ndof_elem * ndof_elem;
int neig = (int)k_elem[0];
// compute displacement in corotational frame
mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum);
for (int n = 0; n < npc; n++) {
// rotate xpos_c to corotational frame
mjtNum* displ_e = mjSTACKALLOC(d, ndof_elem, mjtNum);
for (int n = 0; n < npe; n++) {
// rotate xpos_e to corotational frame
mjtNum xrot[3];
mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat);
displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0];
displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1];
displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2];
mju_rotVecQuat(xrot, xpos_e + 3*n, elem_quat);
displ_e[3*n + 0] = xrot[0] - refpos_e[3*n + 0];
displ_e[3*n + 1] = xrot[1] - refpos_e[3*n + 1];
displ_e[3*n + 2] = xrot[2] - refpos_e[3*n + 2];
}
// compute inverse quaternion for rotating eigenvectors to world frame
mjtNum cell_quat_inv[4];
mju_negQuat(cell_quat_inv, cell_quat);
mjtNum elem_quat_inv[4];
mju_negQuat(elem_quat_inv, elem_quat);
// loop over eigenmodes
for (int eig = 0; eig < neig; eig++) {
const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell;
const mjtNum* eigvec = k_elem + 1 + eig * ndof_elem;
// constraint residual: dot product of scaled eigenvector with displacement
mjtNum residual = 0;
for (int j = 0; j < ndof_cell; j++) {
residual += eigvec[j] * displ_c[j];
for (int j = 0; j < ndof_elem; j++) {
residual += eigvec[j] * displ_e[j];
}
cpos[0] = residual;
// rotate eigenvector to world frame for Jacobian
// dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d]
for (int n = 0; n < npc; n++) {
mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv);
for (int n = 0; n < npe; n++) {
mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, elem_quat_inv);
}
// contract with cell_node_jac to get sparse Jacobian
cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac);
// contract with elem_node_jac to get sparse Jacobian
cell_strain_jacobian(npe, elem_nnz, dSdx_local, elem_node_jac, strain_jac);
if (issparse) {
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
cell_nnz, cell_chain);
elem_nnz, elem_chain);
} else {
for (int k = 0; k < cell_nnz; k++) {
dense_jac[cell_chain[k]] = strain_jac[k];
for (int k = 0; k < elem_nnz; k++) {
dense_jac[elem_chain[k]] = strain_jac[k];
}
mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
for (int k = 0; k < cell_nnz; k++) {
dense_jac[cell_chain[k]] = 0;
for (int k = 0; k < elem_nnz; k++) {
dense_jac[elem_chain[k]] = 0;
}
}
}
@@ -1674,36 +1705,56 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEXSTRAIN: {
// strain constraints: per-cell, use avg inv weight of cell's npc nodes
// strain constraints: use avg inv weight of element's nodes
int flex_id = m->eq_obj1id[id];
int nstart = m->flex_nodeadr[flex_id];
int order = m->flex_interp[flex_id];
order = order < 0 ? -order : order;
int npc = (order+1)*(order+1)*(order+1);
int interp = m->flex_interp[flex_id];
int order = interp < 0 ? -interp : interp;
int is_shell = (interp < 0);
// per-cell constraint count
int nquad = order + 1;
int ngauss = nquad * nquad * nquad;
int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss);
// get cell index from eq_data
int eq_id = d->efc_id[i];
int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0];
int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1];
int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2];
int cx = m->flex_cellnum[3*flex_id+0];
int cy = m->flex_cellnum[3*flex_id+1];
int cz = m->flex_cellnum[3*flex_id+2];
// nodes per element
int npe;
int elem_idx;
if (is_shell) {
npe = (order+1) * (order+1);
elem_idx = (int)m->eq_data[mjNEQDATA*id + 0];
} else {
npe = (order+1) * (order+1) * (order+1);
int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0];
int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1];
int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2];
elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
}
// read neig from flex_stiffness
int ndof_elem = 3 * npe;
const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[flex_id]
+ elem_idx * ndof_elem * ndof_elem;
int nconstraint = (int)k_elem[0];
// get element node indices
int gindices[125];
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
if (is_shell) {
mju_flexGatherFaceState(order, cx, cy, cz, elem_idx,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
} else {
int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0];
int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1];
int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2];
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
}
mjtNum avg_invweight = 0;
for (int n = 0; n < npc; n++) {
for (int n = 0; n < npe; n++) {
int bodyid = m->flex_nodebodyid[nstart + gindices[n]];
avg_invweight += m->body_invweight0[2*bodyid];
}
avg_invweight /= npc;
avg_invweight /= npe;
for (int c = 0; c < nconstraint; c++) {
dA[i++] = avg_invweight;
}
@@ -2296,37 +2347,56 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
break;
case mjEQ_FLEXSTRAIN: {
// per-cell strain constraints: each equality is one cell
// per-element strain constraints: each equality is one cell or face
int f = id[0];
int order = m->flex_interp[f];
order = order < 0 ? -order : order;
int interp = m->flex_interp[f];
int order = interp < 0 ? -interp : interp;
int is_shell = (interp < 0);
if (!order || !m->flex_nodenum[f]) {
break;
}
int npc = (order+1)*(order+1)*(order+1);
// read eigenmode count from flex_stiffness
int ndof_cell = 3 * npc;
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
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 cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
+ cell_idx * ndof_cell * ndof_cell;
size = (int)k_cell[0]; // neig stored as first element
int npe;
int elem_idx;
if (is_shell) {
npe = (order+1) * (order+1);
elem_idx = (int)m->eq_data[mjNEQDATA*i + 0];
} else {
npe = (order+1) * (order+1) * (order+1);
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
}
// read eigenmode count from flex_stiffness
int ndof_elem = 3 * npe;
const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f]
+ elem_idx * ndof_elem * ndof_elem;
size = (int)k_elem[0]; // neig stored as first element
if (nnz) {
// get the npc node body IDs for this cell
// get element node body IDs
int gindices[125];
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
if (is_shell) {
mju_flexGatherFaceState(order, cx, cy, cz, elem_idx,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
} else {
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
}
int nstart = m->flex_nodeadr[f];
for (int n = 0; n < npc; n++) {
for (int n = 0; n < npe; n++) {
cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]];
}
NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only
NV = mj_jacSumCount(m, d, chain, npe, cell_bodies);
NV = size * NV;
}
break;
src/engine/engine_derivative.c
+119 -103
View File
@@ -882,23 +882,29 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
// compute upper bounds across all interpolated flexes
int max_nodenum = 0;
int max_npc = 0;
int max_npe = 0; // max nodes per element (3D cell or 2D face)
for (int f = 0; f < m->nflex; f++) {
if (!m->flex_interp[f]) continue;
if (m->flex_rigid[f]) continue;
int order = m->flex_interp[f];
int shell_mode = order < 0;
order = order < 0 ? -order : order;
int npc = (order+1)*(order+1)*(order+1);
if (npc > max_npc) max_npc = npc;
int npe;
if (shell_mode) {
npe = (order+1)*(order+1);
} else {
npe = (order+1)*(order+1)*(order+1);
}
if (npe > max_npe) max_npe = npe;
if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f];
}
// nothing to do
if (max_npc == 0) {
if (max_npe == 0) {
return;
}
int max_dim_c = 3 * max_npc;
int max_dim_c = 3 * max_npe;
// single unconditional markStack
mj_markStack(d);
@@ -915,8 +921,8 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
// per-flex node positions (upper bound)
mjtNum* xpos = mjSTACKALLOC(d, 3*max_nodenum, mjtNum);
// per-cell arrays (upper bound)
mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npc, mjtNum);
// per-element arrays (upper bound)
mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npe, mjtNum);
mjtNum* K_rot_cell = mjSTACKALLOC(d, max_dim_c*max_dim_c, mjtNum);
// sparse Jacobian for one cell (upper bound)
@@ -967,131 +973,141 @@ 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;
int npc = (order+1)*(order+1)*(order+1);
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* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int dim_c = 3 * npc;
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
int npe;
int nelem_fe;
if (shell_mode) {
npe = (order+1)*(order+1);
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
} else {
npe = (order+1)*(order+1)*(order+1);
nelem_fe = cx * cy * cz;
}
int dim_e = 3 * npe;
// gather raw node positions (unrotated)
mju_flexGatherState(m, d, f, xpos, NULL);
// loop over cells
int cell_idx = 0;
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
// get cell stiffness
mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc;
// loop over finite elements
for (int fe = 0; fe < nelem_fe; fe++) {
// get element stiffness
mjtNum* k_elem = K + fe * 3*npe * 3*npe;
// skip empty cells: stiffness buffer is zero-initialized at compile time
// (user_model.cc), and non-empty cells have strictly positive diagonal
if (k_cell[0] == 0) {
cell_idx++;
continue;
}
// skip empty elements: stiffness buffer is zero-initialized at compile time
// (user_model.cc), and non-empty elements have strictly positive diagonal
if (k_elem[0] == 0) {
continue;
}
// gather cell-local node positions
int gindices[125]; // max npc = 125 for quadratic
mjtNum quat[4];
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL,
xpos_c, NULL, NULL, gindices, quat);
// gather element-local node positions
int gindices[125]; // max npe = 125 for quadratic 3D
mjtNum quat[4];
if (shell_mode) {
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos, NULL, NULL,
xpos_c, NULL, NULL, gindices, quat);
} else {
int ci = fe / (cy * cz);
int cj = (fe / cz) % cy;
int ck = fe % cz;
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL,
xpos_c, NULL, NULL, gindices, quat);
}
// R = R_global2local, RT = R_local2global
mjtNum R[9], RT[9];
mju_quat2Mat(R, quat);
mju_transpose(RT, R, 3, 3);
// R = R_global2local, RT = R_local2global
mjtNum R[9], RT[9];
mju_quat2Mat(R, quat);
mju_transpose(RT, R, 3, 3);
// compute K_rot_cell = RT * K_cell * R (block-wise)
mju_zero(K_rot_cell, dim_c*dim_c);
for (int a = 0; a < npc; a++) {
for (int b = 0; b < npc; b++) {
mjtNum blk[9], tmp[9];
// compute K_rot = RT * K_elem * R (block-wise)
mju_zero(K_rot_cell, dim_e*dim_e);
for (int a = 0; a < npe; a++) {
for (int b = 0; b < npe; b++) {
mjtNum blk[9], tmp[9];
// get K_cell(a,b) 3x3 block
int adr_cell = (3*a)*(3*npc) + 3*b;
for (int r = 0; r < 3; r++) {
for (int c = 0; c < 3; c++) {
blk[3*r+c] = k_cell[adr_cell + r*(3*npc) + c];
}
}
// tmp = K * R
mju_mulMatMat3(tmp, blk, R);
// blk = RT * tmp = RT * K * R
mju_mulMatMat3(blk, RT, tmp);
// store in K_rot_cell at (a, b)
int adr_out = (3*a)*dim_c + 3*b;
for (int r = 0; r < 3; r++) {
for (int c = 0; c < 3; c++) {
K_rot_cell[adr_out + r*dim_c + c] = scale * blk[3*r+c];
}
}
// get K_elem(a,b) 3x3 block
int adr_cell = (3*a)*(3*npe) + 3*b;
for (int r = 0; r < 3; r++) {
for (int c = 0; c < 3; c++) {
blk[3*r+c] = k_elem[adr_cell + r*(3*npe) + c];
}
}
// construct sparse Jacobian for this cell's nodes
int current_adr = 0;
for (int n = 0; n < npc; n++) {
int bid = bodyid[gindices[n]];
int chain_nnz = mj_bodyChain(m, bid, chain_colind);
mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid,
chain_nnz, chain_colind, /*flg_skipcommon=*/0);
// tmp = K * R
mju_mulMatMat3(tmp, blk, R);
// blk = RT * tmp = RT * K * R
mju_mulMatMat3(blk, RT, tmp);
for (int r = 0; r < 3; r++) {
int row_idx = 3*n + r;
J_rownnz[row_idx] = chain_nnz;
J_rowadr[row_idx] = current_adr;
for (int idx = 0; idx < chain_nnz; idx++) {
J_colind[current_adr] = chain_colind[idx];
J_val[current_adr] = blk_jac[r*chain_nnz + idx];
current_adr++;
}
// store in K_rot_cell at (a, b)
int adr_out = (3*a)*dim_e + 3*b;
for (int r = 0; r < 3; r++) {
for (int c = 0; c < 3; c++) {
K_rot_cell[adr_out + r*dim_e + c] = scale * blk[3*r+c];
}
}
}
}
// apply operation with cell's K_rot and J
if (op == mjFLEXOP_VEC) {
addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind,
J_val, K_rot_cell, dim_c);
} else if (op == mjFLEXOP_ADDH) {
// H -= J_cell^T * K_rot_cell * J_cell (banded format)
mju_zero(J_reduced, dim_c*ndof);
// construct sparse Jacobian for this element's nodes
int current_adr = 0;
for (int n = 0; n < npe; n++) {
int bid = bodyid[gindices[n]];
int chain_nnz = mj_bodyChain(m, bid, chain_colind);
mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid,
chain_nnz, chain_colind, /*flg_skipcommon=*/0);
for (int i = 0; i < dim_c; i++) {
int nnz = J_rownnz[i];
int adr = J_rowadr[i];
for (int idx = 0; idx < nnz; idx++) {
int global_col = J_colind[adr + idx];
int local_idx = global2local[global_col];
if (local_idx >= 0) {
J_reduced[i*ndof + local_idx] = J_val[adr + idx];
}
}
}
for (int r = 0; r < 3; r++) {
int row_idx = 3*n + r;
J_rownnz[row_idx] = chain_nnz;
J_rowadr[row_idx] = current_adr;
// KJ = K_rot_cell * J_reduced (dim_c x ndof)
mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof);
for (int idx = 0; idx < chain_nnz; idx++) {
J_colind[current_adr] = chain_colind[idx];
J_val[current_adr] = blk_jac[r*chain_nnz + idx];
current_adr++;
}
}
}
// H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format
for (int i = 0; i < ndof; i++) {
for (int j = mjMAX(0, i-nband+1); j <= i; j++) {
mjtNum val = 0;
for (int dim_idx = 0; dim_idx < dim_c; dim_idx++) {
val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j];
}
res[i*nband + nband-1-(i-j)] -= val;
}
// apply operation with element's K_rot and J
if (op == mjFLEXOP_VEC) {
addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind,
J_val, K_rot_cell, dim_e);
} else if (op == mjFLEXOP_ADDH) {
// H -= J_elem^T * K_rot * J_elem (banded format)
mju_zero(J_reduced, dim_e*ndof);
for (int i = 0; i < dim_e; i++) {
int nnz = J_rownnz[i];
int adr = J_rowadr[i];
for (int idx = 0; idx < nnz; idx++) {
int global_col = J_colind[adr + idx];
int local_idx = global2local[global_col];
if (local_idx >= 0) {
J_reduced[i*ndof + local_idx] = J_val[adr + idx];
}
}
}
cell_idx++;
// KJ = K_rot * J_reduced (dim_e x ndof)
mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_e, dim_e, ndof);
// H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format
for (int i = 0; i < ndof; i++) {
for (int j = mjMAX(0, i-nband+1); j <= i; j++) {
mjtNum val = 0;
for (int dim_idx = 0; dim_idx < dim_e; dim_idx++) {
val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j];
}
res[i*nband + nband-1-(i-j)] -= val;
}
}
}
}
src/engine/engine_passive.c
+70 -67
View File
@@ -236,13 +236,23 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
if (m->flex_interp[f]) {
int order = m->flex_interp[f];
int shell_mode = order < 0;
order = order < 0 ? -order : order;
int npc = (order+1)*(order+1)*(order+1); // nodes per cell
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 ny_g = cy * order + 1;
int nz_g = cz * order + 1;
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
int npe; // nodes per element
int nelem_fe; // total finite elements
if (shell_mode) {
npe = (order+1)*(order+1);
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
} else {
npe = (order+1)*(order+1)*(order+1);
nelem_fe = cx * cy * cz;
}
mj_markStack(d);
@@ -261,77 +271,70 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mju_zero(frc_g, 3*nodenum);
mju_zero(dmp_g, 3*nodenum);
// per-cell arrays
mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* vel_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* xpos0_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* displ_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* frc_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* dmp_c = mjSTACKALLOC(d, 3*npc, mjtNum);
// per-element arrays (sized for npe)
mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum);
mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum);
mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum);
mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum);
mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum);
int* gindices = mjSTACKALLOC(d, npe, int);
// loop over cells
int cell_idx = 0;
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
// get cell stiffness matrix
mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
// loop over finite elements
for (int fe = 0; fe < nelem_fe; fe++) {
// get element stiffness matrix
mjtNum* k_elem = k + fe * 3*npe * 3*npe;
// skip empty cells (zero stiffness)
if (k_cell[0] == 0) {
cell_idx++;
continue;
}
// skip empty elements (zero stiffness)
if (k_elem[0] == 0) {
continue;
}
// gather cell-local node data
mjtNum quat[4];
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0,
xpos_c, vel_c, xpos0_c, NULL, quat);
// gather element-local node data and compute corotational rotation
mjtNum quat[4];
if (shell_mode) {
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0,
xpos_e, vel_e, xpos0_e, gindices, quat);
} else {
int ci = fe / (cy * cz);
int cj = (fe / cz) % cy;
int ck = fe % cz;
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g,
xpos0, xpos_e, vel_e, xpos0_e, gindices,
quat);
}
// rotate to corotational frame
for (int n = 0; n < npc; n++) {
mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat);
mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat);
}
// rotate to corotational frame
for (int n = 0; n < npe; n++) {
mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat);
mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat);
}
// compute displacement
for (int n = 0; n < npc; n++) {
mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1);
}
// compute displacement
for (int n = 0; n < npe; n++) {
mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1);
}
// compute force in corotational frame
if (enbl_spring) {
mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc);
}
if (enbl_damper) {
mju_mulMatVec(dmp_c, k_cell, vel_c, 3*npc, 3*npc);
}
// compute force in corotational frame
if (enbl_spring) {
mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe);
}
if (enbl_damper) {
mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe);
}
// rotate back to global frame and scatter
mju_negQuat(quat, quat);
int local = 0;
for (int li = 0; li <= order; li++) {
for (int lj = 0; lj <= order; lj++) {
for (int lk = 0; lk <= order; lk++) {
int gi = ci*order + li;
int gj = cj*order + lj;
int gk = ck*order + lk;
int gidx = gi*ny_g*nz_g + gj*nz_g + gk;
mjtNum qfrc[3], qdmp[3];
mji_rotVecQuat(qfrc, frc_c+3*local, quat);
mji_rotVecQuat(qdmp, dmp_c+3*local, quat);
if (enbl_spring) {
mji_addTo3(frc_g + 3*gidx, qfrc);
}
if (enbl_damper) {
mji_addTo3(dmp_g + 3*gidx, qdmp);
}
local++;
}
}
}
cell_idx++;
// rotate back to global frame and scatter using node indices
mju_negQuat(quat, quat);
for (int n = 0; n < npe; n++) {
mjtNum qfrc[3], qdmp[3];
mji_rotVecQuat(qfrc, frc_e+3*n, quat);
mji_rotVecQuat(qdmp, dmp_e+3*n, quat);
int gidx = gindices[n];
if (enbl_spring) {
mji_addTo3(frc_g + 3*gidx, qfrc);
}
if (enbl_damper) {
mji_addTo3(dmp_g + 3*gidx, qdmp);
}
}
}
src/engine/engine_util_misc.c
+127
View File
@@ -726,6 +726,133 @@ void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck,
}
// compute corotational rotation from 2D deformation gradient on a flat face
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 3x2 deformation gradient F at parametric point local
mjtNum t1[3] = {0, 0, 0}; // tangent along axis0
mjtNum t2[3] = {0, 0, 0}; // tangent along axis1
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++;
}
}
// normal = t1 x t2
mjtNum normal[3];
mju_cross(normal, t1, t2);
// build 3x3 matrix with columns assigned to canonical axes (row-major)
// axis0 → t1, axis1 → t2, normal_axis → normal
// this ensures identity rotation for axis-aligned grids
mjtNum mat[9] = {0};
mjtNum* vecs[3];
vecs[axis0] = t1;
vecs[axis1] = t2;
vecs[normal_axis] = normal;
for (int col = 0; col < 3; col++) {
mat[0*3 + col] = vecs[col][0];
mat[1*3 + col] = vecs[col][1];
mat[2*3 + col] = vecs[col][2];
}
// extract rotation via polar decomposition
quat[0] = 1;
quat[1] = 0;
quat[2] = 0;
quat[3] = 0;
mju_mat2Rot(quat, mat);
mju_negQuat(quat, quat);
}
// gather face-element-local quantities and optionally compute rotation (shell mode)
//
// face element enumeration for a grid with cell counts (cx, cy, cz):
// face 0: x=0 cy*cz quads (normal=0)
// face 1: x=max cy*cz quads (normal=0)
// face 2: y=0 cx*cz quads (normal=1)
// face 3: y=max cx*cz quads (normal=1)
// face 4: z=0 cx*cy quads (normal=2)
// face 5: z=max cx*cy quads (normal=2)
void mju_flexGatherFaceState(int order, int cx, int cy, int cz,
int face_elem_idx,
const mjtNum* xpos_g, const mjtNum* vel_g,
const mjtNum* xpos0_g,
mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f,
int* nodeindices, mjtNum* quat) {
int ny_g = cy * order + 1;
int nz_g = cz * order + 1;
int npe = (order + 1) * (order + 1);
// face sizes and properties
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_vals[6];
face_fixed_vals[0] = 0;
face_fixed_vals[1] = cx * order;
face_fixed_vals[2] = 0;
face_fixed_vals[3] = cy * order;
face_fixed_vals[4] = 0;
face_fixed_vals[5] = cz * order;
// determine which face and quad within face
int face_id = 0;
int within_face = face_elem_idx;
int cumul = 0;
for (int f = 0; f < 6; f++) {
if (face_elem_idx < cumul + face_sizes[f]) {
face_id = f;
within_face = face_elem_idx - cumul;
break;
}
cumul += face_sizes[f];
}
int normal_axis = face_normal[face_id];
int na0 = (normal_axis + 1) % 3; // slow in-plane axis
int na1 = (normal_axis + 2) % 3; // fast in-plane axis
int c1 = face_count1[face_id];
int g_fixed = face_fixed_vals[face_id];
int q0 = within_face / c1;
int q1 = within_face % c1;
// gather nodes
int local = 0;
for (int l0 = 0; l0 <= order; l0++) {
for (int l1 = 0; l1 <= order; l1++) {
int g[3];
g[normal_axis] = g_fixed;
g[na0] = q0 * order + l0;
g[na1] = q1 * order + l1;
int gidx = g[0] * ny_g * nz_g + g[1] * nz_g + g[2];
if (xpos_f && xpos_g) mju_copy3(xpos_f + 3*local, xpos_g + 3*gidx);
if (vel_f && vel_g) mju_copy3(vel_f + 3*local, vel_g + 3*gidx);
if (xpos0_f && xpos0_g) mju_copy3(xpos0_f + 3*local, xpos0_g + 3*gidx);
if (nodeindices) nodeindices[local] = gidx;
local++;
}
}
if (quat && xpos_f) {
mjtNum p[2] = {.5, .5};
mju_flexInterpRotation2D(order, xpos_f, npe, na0, na1, normal_axis, p, quat);
}
}
//------------------------------ actuator models ---------------------------------------------------
// normalized muscle length-gain curve
src/engine/engine_util_misc.h
+13
View File
@@ -103,6 +103,19 @@ MJAPI void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, in
const mjtNum* xpos0_g, mjtNum* xpos_c, mjtNum* vel_c,
mjtNum* xpos0_c, int* nodeindices, mjtNum* quat);
// gather face-element-local quantities and optionally compute rotation (shell mode)
MJAPI void mju_flexGatherFaceState(int order, int cx, int cy, int cz,
int face_elem_idx,
const mjtNum* xpos_g, const mjtNum* vel_g,
const mjtNum* xpos0_g,
mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f,
int* nodeindices, mjtNum* quat);
// compute corotational rotation from 2D deformation gradient on a flat face
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);
// ----------------------------- Base64 ------------------------------------------------------------
src/user/user_flexcomp.cc
+36 -16
View File
@@ -884,27 +884,47 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
pe->active = true;
mjs_setString(pe->name1, name.c_str());
} else if (equality == 3) {
// create one strain constraint per cell, storing cell index in eq_data
// create one strain constraint per finite element, storing element index
flex->has_strain_eq = true;
int cell_cx = flex->spec.cellcount[0];
int cell_cy = flex->spec.cellcount[1];
int cell_cz = flex->spec.cellcount[2];
for (int ci = 0; ci < cell_cx; ci++) {
for (int cj = 0; cj < cell_cy; cj++) {
for (int ck = 0; ck < cell_cz; ck++) {
// skip empty cells
if (!flex->cell_empty.empty() &&
flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) {
continue;
bool shell = (doftype == mjFCOMPDOF_TRILINEAR ||
doftype == mjFCOMPDOF_QUADRATIC) &&
flex->spec.elastic2d;
if (shell) {
// shell mode: one constraint per boundary face element
int nelem_fe = 2*(cell_cy*cell_cz + cell_cx*cell_cz + cell_cx*cell_cy);
for (int fe = 0; fe < nelem_fe; fe++) {
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->Default()->spec);
pe->type = mjEQ_FLEXSTRAIN;
pe->active = true;
mjs_setString(pe->name1, name.c_str());
pe->data[0] = fe;
pe->data[1] = -1; // sentinel: shell mode
pe->data[2] = -1;
}
} else {
// volume mode: one constraint per 3D cell
for (int ci = 0; ci < cell_cx; ci++) {
for (int cj = 0; cj < cell_cy; cj++) {
for (int ck = 0; ck < cell_cz; ck++) {
// skip empty cells
if (!flex->cell_empty.empty() &&
flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) {
continue;
}
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->Default()->spec);
pe->type = mjEQ_FLEXSTRAIN;
pe->active = true;
mjs_setString(pe->name1, name.c_str());
pe->data[0] = ci;
pe->data[1] = cj;
pe->data[2] = ck;
}
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->Default()->spec);
pe->type = mjEQ_FLEXSTRAIN;
pe->active = true;
mjs_setString(pe->name1, name.c_str());
pe->data[0] = ci;
pe->data[1] = cj;
pe->data[2] = ck;
}
}
}
src/user/user_mesh.cc
+215 -44
View File
@@ -3807,6 +3807,98 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
}
// compute the linear stiffness matrix for a flat 2D quad face element (membrane)
// K: output stiffness matrix, size 3*npe x 3*npe, npe = (order+1)^2
// pos: node positions (3*npe doubles), ordered row-major in 2D parametric domain
// E, nu: Young's modulus and Poisson's ratio
// order: interpolation order (1 or 2)
// thickness: shell thickness
// normal_axis: axis perpendicular to the face (0=x, 1=y, 2=z)
void inline ComputeLinearStiffness2D(std::vector<double>& K,
const double* pos,
double E, double nu, int order,
double thickness, int normal_axis) {
int nbasis = order + 1;
int npe = nbasis * nbasis; // nodes per face element
int ndof = 3 * npe;
// in-plane axes
int axis0 = (normal_axis + 1) % 3; // slow-varying
int axis1 = (normal_axis + 2) % 3; // fast-varying
// compute quadrature points
std::vector<double> points(nbasis);
std::vector<double> weight(nbasis);
quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1);
// compute element transformation (diagonal Jacobian on flat face)
double d0 = (pos + 3*(npe-1))[axis0] - pos[axis0]; // extent along axis0
double d1 = (pos + 3*(npe-1))[axis1] - pos[axis1]; // extent along axis1
if (d0 == 0 || d1 == 0) {
throw mjCError(nullptr, "degenerate 2D element with zero extent");
}
double detJ = d0 * d1;
double invJ0 = 1.0 / d0;
double invJ1 = 1.0 / d1;
// plane-stress Lamé parameter: lambda* = E*nu/(1 - nu^2)
double la = E * nu / (1.0 - nu * nu);
double mu = E / (2.0 * (1.0 + nu));
// basis function gradients (2-component)
std::vector<std::array<double, 2>> F(npe);
// loop over quadrature points (2D)
for (int ps = 0; ps < nbasis; ps++) {
for (int pt = 0; pt < nbasis; pt++) {
double s = points[ps];
double t = points[pt];
double dvol = weight[ps] * weight[pt] * detJ * thickness;
int dof = 0;
// cartesian product of 2D basis functions
for (int b0 = 0; b0 < nbasis; b0++) {
for (int b1 = 0; b1 < nbasis; b1++) {
F[dof][0] = dphi(s, b0, order) * phi(t, b1, order);
F[dof][1] = phi(s, b0, order) * dphi(t, b1, order);
dof++;
}
}
if (dof != npe) {
throw mjCError(nullptr, "incorrect number of 2D basis functions");
}
// tensor contraction (same structure as 3D but with zero normal column)
for (int i = 0; i < npe; i++) {
for (int j = 0; j < npe; j++) {
Matrix du;
Matrix dv;
du.fill({0, 0, 0});
dv.fill({0, 0, 0});
for (int k = 0; k < 3; k++) {
for (int l = 0; l < 3; l++) {
// du[k] has non-zero entries only at in-plane axes
du[k][axis0] = invJ0 * F[i][0];
du[k][axis1] = invJ1 * F[i][1];
// du[k][normal_axis] = 0 (already zero)
dv[l][axis0] = invJ0 * F[j][0];
dv[l][axis1] = invJ1 * F[j][1];
// dv[l][normal_axis] = 0 (already zero)
K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol;
// mu (not 2*mu): same convention as 3D ComputeLinearStiffness
K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
mjuu_zerovec(du[k].data(), 3);
mjuu_zerovec(dv[l].data(), 3);
}
}
}
}
}
}
}
// Eigendecompose cell stiffness matrix and store scaled eigenvectors.
// K_cell is n×n stored (negative convention: K_stored = -K_physical).
// Output layout in `out`:
@@ -3964,7 +4056,8 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
mjCBody* pbody = static_cast<mjCBody*>(m->FindObject(mjOBJ_BODY, vertbody));
if (pbody) {
vertbodyid.push_back(pbody->id);
if (pbody->joints.size() != 3 && dim == 2 && (elastic2d == 1 || elastic2d == 3)) {
if (pbody->joints.size() != 3 && dim == 2 &&
(elastic2d == 1 || elastic2d == 3) && !interpolated) {
// TODO(quaglino): add support for pins
throw mjCError(this, "pins are not supported for bending");
}
@@ -4105,8 +4198,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (thickness <= 0) {
throw mjCError(this, "2d elasticity requires positive thickness");
}
if (interpolated) {
throw mjCError(this, "interpolated flex does not yet support 2d elasticity");
if (interpolated && elastic2d != 2) {
mju_warning("bending passive force is not implemented for interpolated flex");
}
if (dim != 2 && !interpolated) {
throw mjCError(this, "2d elasticity requires 2d flex");
@@ -4141,6 +4234,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (spec.cellcount[0] == 0 || spec.cellcount[1] == 0 || spec.cellcount[2] == 0) {
throw mjCError(this, "cellcount cannot be 0 in any dimension when interpolation order > 0");
}
if (elastic2d && !(spec.cellcount[0] == 1 || spec.cellcount[1] == 1 || spec.cellcount[2] == 1)) {
throw mjCError(this,
"shell trilinear flex requires at least one dimension "
"with cell count equal to one (no interior nodes)");
}
int expected_nodes = (spec.cellcount[0] * spec.order + 1) *
(spec.cellcount[1] * spec.order + 1) *
(spec.cellcount[2] * spec.order + 1);
@@ -4354,7 +4452,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// bending stiffness (2D only)
if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) {
if (dim == 2 && (elastic2d == 1 || elastic2d == 3) && !interpolated) {
bending.assign(nedge*17, 0);
for (unsigned int e = 0; e < nedge; e++) {
@@ -4392,57 +4490,130 @@ void mjCFlex::Compile(const mjVFS* vfs) {
double K_young = has_strain_eq ? 1e1 : young;
double K_poisson = has_strain_eq ? 0.3 : poisson;
int npc = pow(spec.order + 1, 3); // nodes per cell
int ndof_cell = 3 * npc;
int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
int ncells = cx * cy * cz;
int ny_global = cy * spec.order + 1;
int nz_global = cz * spec.order + 1;
// total stiffness = ncells * ndof_cell^2
stiffness.resize(ncells * ndof_cell * ndof_cell, 0);
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
bool shell_mode = elastic2d != 0;
int npe; // nodes per element
int nelem_fe; // total finite elements
// compute stiffness per cell
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
int cell_idx = ci * cy * cz + cj * cz + ck;
if (shell_mode) {
npe = pow(spec.order + 1, 2); // (order+1)^2 for 2D quads
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
} else {
npe = pow(spec.order + 1, 3); // (order+1)^3 for 3D cells
nelem_fe = cx * cy * cz;
}
int ndof_elem = 3 * npe;
// skip stiffness computation for empty cells (no mesh content)
if (!cell_empty.empty() && cell_empty[cell_idx]) {
continue;
// total stiffness = nelem_fe * ndof_elem^2
stiffness.resize(nelem_fe * ndof_elem * ndof_elem, 0);
// face layout for shell mode:
// face 0: x=0 (cy*cz quads, normal=0, in-plane=(1,2))
// face 1: x=max (cy*cz quads, normal=0, in-plane=(1,2))
// face 2: y=0 (cx*cz quads, normal=1, in-plane=(0,2))
// face 3: y=max (cx*cz quads, normal=1, in-plane=(0,2))
// face 4: z=0 (cx*cy quads, normal=2, in-plane=(0,1))
// face 5: z=max (cx*cy quads, normal=2, in-plane=(0,1))
// face_sizes = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}
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};
// cell counts along each in-plane axis for each face
int face_count1[6] = {cz, cz, cx, cx, cy, cy}; // fast axis count
// fixed axis value (in grid node units, 0 or max)
int face_fixed[6] = {0, cx*spec.order, 0, cy*spec.order, 0, cz*spec.order};
// compute stiffness per element
for (int fe = 0; fe < nelem_fe; fe++) {
// gather element node positions
std::vector<double> elem_pos(3 * npe);
int normal_axis = -1;
if (shell_mode) {
// determine which face and quad within face
int face_id = 0, within_face = fe;
int cumul = 0;
for (int f = 0; f < 6; f++) {
if (fe < cumul + face_sizes[f]) {
face_id = f;
within_face = fe - cumul;
break;
}
cumul += face_sizes[f];
}
// gather cell's local node positions
std::vector<double> cell_pos(3 * npc);
int local = 0;
for (int li = 0; li <= spec.order; li++) {
for (int lj = 0; lj <= spec.order; lj++) {
for (int lk = 0; lk <= spec.order; lk++) {
int gi = ci * spec.order + li;
int gj = cj * spec.order + lj;
int gk = ck * spec.order + lk;
int global = gi * ny_global * nz_global + gj * nz_global + gk;
mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3);
local++;
}
}
}
normal_axis = face_normal[face_id];
int na0 = (normal_axis + 1) % 3; // slow in-plane axis
int na1 = (normal_axis + 2) % 3; // fast in-plane axis
int c1 = face_count1[face_id]; // cell count along fast axis
int g_fixed = face_fixed[face_id]; // grid index along normal axis
int q0 = within_face / c1; // quad index along slow in-plane axis
int q1 = within_face % c1; // quad index along fast in-plane axis
// compute per-cell stiffness
std::vector<double> K_cell(ndof_cell * ndof_cell, 0);
ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order);
double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell;
if (has_strain_eq) {
// eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...]
std::fill(out, out + ndof_cell * ndof_cell, 0.0);
EigendecomposeStiffness(K_cell.data(), out, ndof_cell);
} else {
// store raw K for passive forces
std::copy(K_cell.begin(), K_cell.end(), out);
// gather 2D face element nodes
int local = 0;
for (int l0 = 0; l0 <= spec.order; l0++) {
for (int l1 = 0; l1 <= spec.order; l1++) {
// build global node index from 3 axis values
int g[3];
g[normal_axis] = g_fixed;
g[na0] = q0 * spec.order + l0;
g[na1] = q1 * spec.order + l1;
int global = g[0] * ny_global * nz_global + g[1] * nz_global + g[2];
mjuu_copyvec(elem_pos.data() + 3*local,
nodexpos_local.data() + 3*global, 3);
local++;
}
}
} else {
// 3D cell: convert flat index to (ci, cj, ck)
int ci = fe / (cy * cz);
int cj = (fe / cz) % cy;
int ck = fe % cz;
// skip stiffness computation for empty cells (no mesh content)
if (!cell_empty.empty() && cell_empty[fe]) {
continue;
}
// gather cell's local node positions
int local = 0;
for (int li = 0; li <= spec.order; li++) {
for (int lj = 0; lj <= spec.order; lj++) {
for (int lk = 0; lk <= spec.order; lk++) {
int gi = ci * spec.order + li;
int gj = cj * spec.order + lj;
int gk = ck * spec.order + lk;
int global = gi * ny_global * nz_global + gj * nz_global + gk;
mjuu_copyvec(elem_pos.data() + 3*local,
nodexpos_local.data() + 3*global, 3);
local++;
}
}
}
}
// compute per-element stiffness
std::vector<double> K_elem(ndof_elem * ndof_elem, 0);
if (shell_mode) {
ComputeLinearStiffness2D(K_elem, elem_pos.data(), K_young, K_poisson,
spec.order, thickness, normal_axis);
} else {
ComputeLinearStiffness(K_elem, elem_pos.data(), K_young, K_poisson,
spec.order);
}
double* out = stiffness.data() + fe * ndof_elem * ndof_elem;
if (has_strain_eq) {
// eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...]
std::fill(out, out + ndof_elem * ndof_elem, 0.0);
EigendecomposeStiffness(K_elem.data(), out, ndof_elem);
} else {
// store raw K for passive forces
std::copy(K_elem.begin(), K_elem.end(), out);
}
}
}
src/user/user_model.cc
+44 -16
View File
@@ -2191,12 +2191,20 @@ void mjCModel::SetSizes() {
nflexevpair += (int)flexes_[i]->evpair.size()/2;
nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0);
if (flexes_[i]->spec.order != 0) {
int npc = (int)pow(flexes_[i]->spec.order + 1, 3);
int ndof_cell = 3 * npc;
int ncells = flexes_[i]->spec.cellcount[0] *
flexes_[i]->spec.cellcount[1] *
flexes_[i]->spec.cellcount[2];
extra_stiffness_size += ncells * ndof_cell * ndof_cell;
int cx = flexes_[i]->spec.cellcount[0];
int cy = flexes_[i]->spec.cellcount[1];
int cz = flexes_[i]->spec.cellcount[2];
bool shell = (flexes_[i]->elastic2d != 0);
int npe, nelem;
if (shell) {
npe = (int)pow(flexes_[i]->spec.order + 1, 2);
nelem = 2*(cy*cz + cx*cz + cx*cy);
} else {
npe = (int)pow(flexes_[i]->spec.order + 1, 3);
nelem = cx * cy * cz;
}
int ndof_elem = 3 * npe;
extra_stiffness_size += nelem * ndof_elem * ndof_elem;
}
if (flexes_[i]->interpolated || flexes_[i]->rigid) {
continue;
@@ -3476,10 +3484,20 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_stiffnessadr[i] = 21 * elem_adr;
} else {
m->flex_stiffnessadr[i] = current_extra_stiffness_adr;
int npc = (int)pow(pfl->spec.order + 1, 3);
int ndof_cell = 3 * npc;
int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2];
current_extra_stiffness_adr += ncells * ndof_cell * ndof_cell;
int pcx = pfl->spec.cellcount[0];
int pcy = pfl->spec.cellcount[1];
int pcz = pfl->spec.cellcount[2];
bool shell = (pfl->elastic2d != 0);
int npe, nelem;
if (shell) {
npe = (int)pow(pfl->spec.order + 1, 2);
nelem = 2*(pcy*pcz + pcx*pcz + pcx*pcy);
} else {
npe = (int)pow(pfl->spec.order + 1, 3);
nelem = pcx * pcy * pcz;
}
int ndof_elem = 3 * npe;
current_extra_stiffness_adr += nelem * ndof_elem * ndof_elem;
}
if (!pfl->stiffness.empty()) {
@@ -3490,10 +3508,20 @@ void mjCModel::CopyObjects(mjModel* m) {
if (pfl->spec.order == 0) {
stiff_size = 21 * pfl->nelem;
} else {
int npc = (int)pow(pfl->spec.order + 1, 3);
int ndof_cell = 3 * npc;
int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2];
stiff_size = ncells * ndof_cell * ndof_cell;
int scx = pfl->spec.cellcount[0];
int scy = pfl->spec.cellcount[1];
int scz = pfl->spec.cellcount[2];
bool shell = (pfl->elastic2d != 0);
int npe, sncells;
if (shell) {
npe = (int)pow(pfl->spec.order + 1, 2);
sncells = 2*(scy*scz + scx*scz + scx*scy);
} else {
npe = (int)pow(pfl->spec.order + 1, 3);
sncells = scx * scy * scz;
}
int ndof_elem = 3 * npe;
stiff_size = sncells * ndof_elem * ndof_elem;
}
mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], stiff_size);
}
@@ -3629,8 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) {
memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int));
}
// set interpolation type, only two types for now
m->flex_interp[i] = pfl->spec.order;
// set interpolation type: positive = volumetric, negative = shell mode
m->flex_interp[i] = pfl->spec.elastic2d ? -pfl->spec.order : pfl->spec.order;
// set cell count for multi-cell finite cell method
m->flex_cellnum[3*i+0] = pfl->spec.cellcount[0];
test/engine/engine_core_constraint_test.cc
+40
View File
@@ -682,6 +682,46 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
mj_deleteModel(m);
}
TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
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="1 1 1" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="0" poisson="0.1" damping="0.01"
elastic2d="bend" thickness="0.02"/>
<edge equality="strain"/>
<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);
mj_forward(m, d);
// Check number of equalities
EXPECT_EQ(m->neq, 6);
// Check total number of scalar equality constraints
EXPECT_EQ(d->ne, 48); // 6 faces * 8 modes per face
// all constraint residuals should be zero at rest
for (int i = 0; i < d->ne; i++) {
EXPECT_NEAR(d->efc_pos[i], 0, 1e-10)
<< "nonzero constraint residual at " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test quadratic passive forces (no constraints) for stability
TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
static constexpr char xml[] = R"(
test/engine/engine_passive_test.cc
+36
View File
@@ -847,5 +847,41 @@ TEST_F(PassiveTest, PolynomialDampingTendon) {
mj_deleteModel(m);
}
// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
// at rest (initial configuration); any nonzero force indicates a rotation
// mismatch between compile-time reference positions and runtime corotation.
TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
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="1 1 1" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0.01"
elastic2d="stretch" 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);
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;
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/engine/engine_util_misc_test.cc
+385
View File
@@ -1165,5 +1165,390 @@ TEST_F(HistoryTest, CubicInterpolation) {
EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9));
}
// -------------------------------- Face State ---------------------------------
using FaceStateTest = MujocoTest;
// verify mju_flexGatherFaceState returns correct node indices for all 6 faces
// of a 1x1x1 trilinear grid (2x2x2 = 8 nodes, 4 nodes per face)
TEST_F(FaceStateTest, NodeIndicesSingleCell) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int ny_g = cy * order + 1; // 2
int nz_g = cz * order + 1; // 2
// nelem_fe = 2*(1*1 + 1*1 + 1*1) = 6 face elements
// face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max,
// face 4: z=0, face 5: z=max
// create dummy positions for 8 nodes
std::vector<mjtNum> xpos(3 * 8, 0);
for (int i = 0; i < 8; i++) {
xpos[3*i + 0] = (i / 4) * 1.0;
xpos[3*i + 1] = ((i / 2) % 2) * 1.0;
xpos[3*i + 2] = (i % 2) * 1.0;
}
// helper: compute expected global node index from (gx, gy, gz)
auto gidx = [&](int gx, int gy, int gz) {
return gx * ny_g * nz_g + gy * nz_g + gz;
};
// face 0: x=0 (fixed g[0]=0, varying g[1], g[2])
// normal_axis=0, na0=1, na1=2
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0));
EXPECT_EQ(indices[3], gidx(0, 1, 1));
}
// face 1: x=max (fixed g[0]=1, varying g[1], g[2])
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(1, 0, 0));
EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(1, 1, 0));
EXPECT_EQ(indices[3], gidx(1, 1, 1));
}
// face 2: y=0 (fixed g[1]=0)
// normal_axis=1, na0=2(z slow), na1=0(x fast)
// loop order: l0→z, l1→x
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0)
EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1)
EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0)
EXPECT_EQ(indices[3], gidx(1, 0, 1)); // l0=1(z=1), l1=1(x=1)
}
// face 3: y=max (fixed g[1]=1)
// normal_axis=1, na0=2(z slow), na1=0(x fast)
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0)
EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1)
EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0)
EXPECT_EQ(indices[3], gidx(1, 1, 1)); // l0=1(z=1), l1=1(x=1)
}
// face 4: z=0 (fixed g[2]=0, varying g[0], g[1])
// normal_axis=2, na0=0, na1=1
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 1, 0));
EXPECT_EQ(indices[2], gidx(1, 0, 0));
EXPECT_EQ(indices[3], gidx(1, 1, 0));
}
// face 5: z=max (fixed g[2]=1)
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 1));
EXPECT_EQ(indices[2], gidx(1, 0, 1));
EXPECT_EQ(indices[3], gidx(1, 1, 1));
}
}
// verify node indices for a multi-cell grid (2x2x2 cells → 3x3x3 = 27 nodes)
TEST_F(FaceStateTest, NodeIndicesMultiCell) {
int order = 1;
int cx = 2, cy = 2, cz = 2;
int ny_g = 3, nz_g = 3; // (2*1+1) = 3
// nelem_fe = 2*(2*2 + 2*2 + 2*2) = 24 face elements
// face 0: x=0, cy*cz = 4 quads (indices 0-3)
// face 1: x=max, 4 quads (indices 4-7)
// face 2: y=0, cx*cz = 4 quads (indices 8-11)
// face 3: y=max, 4 quads (indices 12-15)
// face 4: z=0, cx*cy = 4 quads (indices 16-19)
// face 5: z=max, 4 quads (indices 20-23)
std::vector<mjtNum> xpos(3 * 27, 0);
for (int i = 0; i < 27; i++) {
int gi = i / 9;
int gj = (i / 3) % 3;
int gk = i % 3;
xpos[3*i + 0] = gi * 0.1;
xpos[3*i + 1] = gj * 0.1;
xpos[3*i + 2] = gk * 0.1;
}
auto gidx = [&](int gx, int gy, int gz) {
return gx * ny_g * nz_g + gy * nz_g + gz;
};
// face 0 (x=0), quad 0: (q0=0, q1=0) within cy*cz face
// c1 = face_count1[0] = cz = 2, so quad (0,0) → within_face = 0
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0));
EXPECT_EQ(indices[3], gidx(0, 1, 1));
}
// face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 2));
EXPECT_EQ(indices[2], gidx(0, 2, 1));
EXPECT_EQ(indices[3], gidx(0, 2, 2));
}
// face 1 (x=max), quad 0: fe_idx = 4 (after face 0's 4 quads)
// g[0] = cx*order = 2
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(2, 0, 0));
EXPECT_EQ(indices[1], gidx(2, 0, 1));
EXPECT_EQ(indices[2], gidx(2, 1, 0));
EXPECT_EQ(indices[3], gidx(2, 1, 1));
}
}
// verify node indices for a non-cubic grid (cx != cz)
TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
int order = 1;
int cx = 2, cy = 1, cz = 3;
int ny_g = cy * order + 1; // 2
int nz_g = cz * order + 1; // 4
// create dummy positions for (2*1+1)*(1*1+1)*(3*1+1) = 3*2*4 = 24 nodes
std::vector<mjtNum> xpos(3 * 24, 0);
for (int i = 0; i < 24; i++) {
int gi = i / 8;
int gj = (i / 4) % 2;
int gk = i % 4;
xpos[3*i + 0] = gi * 0.1;
xpos[3*i + 1] = gj * 0.1;
xpos[3*i + 2] = gk * 0.1;
}
auto gidx = [&](int gx, int gy, int gz) {
return gx * ny_g * nz_g + gy * nz_g + gz;
};
// face 2 (y=0): normal_axis=1, na0=2(z slow), na1=0(x fast)
// counts: na0 -> cz = 3, na1 -> cx = 2
// total quads on face 2 = 6
// we test within_face = 2 (third quad)
// correct: c1 = cx = 2. q0 = 2/2 = 1, q1 = 2%2 = 0
//
// face element index calculation:
// face 0: cy*cz = 1*3 = 3 quads (indices 0-2)
// face 1: cy*cz = 1*3 = 3 quads (indices 3-5)
// face 2: cx*cz = 2*3 = 6 quads. Quad 2 is index 2 within this face.
// Total flat index = 3 + 3 + 2 = 8
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 0, 2));
EXPECT_EQ(indices[3], gidx(1, 0, 2));
}
}
// verify data gathering: positions, velocities, and reference positions
TEST_F(FaceStateTest, DataGathering) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int npe = 4;
int nnodes = 8;
// create positions and velocities for 8 nodes
std::vector<mjtNum> xpos(3 * nnodes);
std::vector<mjtNum> vel(3 * nnodes);
std::vector<mjtNum> xpos0(3 * nnodes);
for (int i = 0; i < nnodes; i++) {
for (int d = 0; d < 3; d++) {
xpos[3*i + d] = 10 * i + d;
vel[3*i + d] = 100 * i + d;
xpos0[3*i + d] = 1000 * i + d;
}
}
// gather face 4 (z=0): nodes at (0,0,0), (0,1,0), (1,0,0), (1,1,0)
// = global indices 0, 2, 4, 6
std::vector<mjtNum> xpos_f(3 * npe);
std::vector<mjtNum> vel_f(3 * npe);
std::vector<mjtNum> xpos0_f(3 * npe);
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), vel.data(),
xpos0.data(), xpos_f.data(), vel_f.data(),
xpos0_f.data(), indices, NULL);
for (int n = 0; n < npe; n++) {
int gi = indices[n];
for (int d = 0; d < 3; d++) {
EXPECT_EQ(xpos_f[3*n + d], xpos[3*gi + d]);
EXPECT_EQ(vel_f[3*n + d], vel[3*gi + d]);
EXPECT_EQ(xpos0_f[3*n + d], xpos0[3*gi + d]);
}
}
}
// verify that flexInterpRotation2D produces identity for axis-aligned faces
// (tested via mju_flexGatherFaceState with quat output)
TEST_F(FaceStateTest, IdentityRotationAxisAligned) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int npe = 4;
// create an axis-aligned unit cube: 8 nodes at {0,1}^3
std::vector<mjtNum> xpos(3 * 8);
int idx = 0;
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
xpos[3*idx + 0] = i;
xpos[3*idx + 1] = j;
xpos[3*idx + 2] = k;
idx++;
}
}
}
std::vector<mjtNum> xpos_f(3 * npe);
mjtNum quat[4];
// test all 6 faces: each should give identity rotation (quat = [1,0,0,0])
int nelem_fe = 6;
for (int fe = 0; fe < nelem_fe; fe++) {
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat);
EXPECT_NEAR(mju_abs(quat[0]), 1.0, 1e-10) << "face " << fe;
EXPECT_NEAR(quat[1], 0.0, 1e-10) << "face " << fe;
EXPECT_NEAR(quat[2], 0.0, 1e-10) << "face " << fe;
EXPECT_NEAR(quat[3], 0.0, 1e-10) << "face " << fe;
}
}
// verify that flexInterpRotation2D extracts the correct rotation for a
// globally rotated cube (90° around z-axis)
TEST_F(FaceStateTest, RotatedCubeRotation) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
int npe = 4;
// create an axis-aligned unit cube, then rotate 90° around z
// rotation: (x,y,z) → (-y, x, z)
std::vector<mjtNum> xpos(3 * 8);
int idx = 0;
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k};
mjtNum axis[3] = {0, 0, 1};
mjtNum rot_quat[4];
mju_axisAngle2Quat(rot_quat, axis, mjPI / 2);
mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat);
idx++;
}
}
}
std::vector<mjtNum> xpos_f(3 * npe);
mjtNum quat[4];
// expected rotation: global→local is inverse of the 90° z rotation
// 90° around z: quat = [cos(45°), 0, 0, sin(45°)]
// inverse (global→local): [cos(45°), 0, 0, -sin(45°)]
mjtNum sq2 = mju_sqrt(0.5);
// test face 4 (z=0): normal_axis=2, in-plane axes are (0,1)
// tangent vectors should reflect the 90° z rotation
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat);
EXPECT_NEAR(quat[0], sq2, 1e-5);
EXPECT_NEAR(quat[1], 0.0, 1e-5);
EXPECT_NEAR(quat[2], 0.0, 1e-5);
EXPECT_NEAR(quat[3], -sq2, 1e-5);
// test face 5 (z=max): should give same rotation
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat);
EXPECT_NEAR(quat[0], sq2, 1e-5);
EXPECT_NEAR(quat[1], 0.0, 1e-5);
EXPECT_NEAR(quat[2], 0.0, 1e-5);
EXPECT_NEAR(quat[3], -sq2, 1e-5);
}
// verify that flexInterpRotation2D matches the 3D cell rotation for
// the same globally-rotated cube
TEST_F(FaceStateTest, RotationConsistencyWith3D) {
int order = 1;
int cx = 1, cy = 1, cz = 1;
// create 90° z-rotated unit cube
std::vector<mjtNum> xpos(3 * 8);
int idx = 0;
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k};
mjtNum axis[3] = {0, 0, 1};
mjtNum rot_quat[4];
mju_axisAngle2Quat(rot_quat, axis, mjPI / 6);
mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat);
idx++;
}
}
}
// get 3D cell rotation
int npc = 8;
std::vector<mjtNum> xpos_c(3 * npc);
mjtNum quat_3d[4];
mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL,
xpos_c.data(), NULL, NULL, NULL, quat_3d);
// get 2D face rotation for each face and verify it matches the 3D rotation
int npe = 4;
std::vector<mjtNum> xpos_f(3 * npe);
int nelem_fe = 6;
for (int fe = 0; fe < nelem_fe; fe++) {
mjtNum quat_2d[4];
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL,
xpos_f.data(), NULL, NULL, NULL, quat_2d);
// quaternions may differ by sign; compare unsigned
mjtNum dot = quat_3d[0]*quat_2d[0] + quat_3d[1]*quat_2d[1] +
quat_3d[2]*quat_2d[2] + quat_3d[3]*quat_2d[3];
EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5)
<< "face " << fe << ": 2D rotation differs from 3D cell rotation";
}
}
} // namespace
} // namespace mujoco
test/user/user_mesh_test.cc
+21 -6
View File
@@ -836,11 +836,11 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
HasSubstr("2d elasticity requires positive thickness"));
}
TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) {
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 1" spacing="1 1 1" dim="2" dof="trilinear">
<flexcomp name="f" type="grid" count="3 3 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1" thickness="1" elastic2d="bend"/>
</flexcomp>
@@ -849,10 +849,25 @@ TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(
error.data(),
HasSubstr("interpolated flex does not yet support 2d elasticity"));
EXPECT_THAT(model, testing::NotNull()) << error.data();
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1" thickness="1" elastic2d="both"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data();
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {