From 3d45a33190641bfef58724a21e6bba06613cc608 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sun, 19 Apr 2026 07:20:49 -0700 Subject: [PATCH 01/29] Refactor flex strain constraints to be per-cell. Each mjEQ_FLEXSTRAIN equality now represents a single cell within a flex. This allows for more efficient sparse Jacobian computation by only considering the degrees of freedom of the nodes within each specific cell. This change gives a speedup of about 10x on a 3x3x3 model. PiperOrigin-RevId: 902164069 Change-Id: I78eedf1d5cf39b8989fe9863c22d164922fc0efb --- doc/XMLreference.rst | 5 +- doc/XMLschema.rst | 3 + doc/changelog.rst | 3 + src/engine/engine_core_constraint.c | 489 ++++++++++++++-------------- src/user/user_flexcomp.cc | 35 +- src/xml/xml_native_reader.cc | 6 +- src/xml/xml_native_writer.cc | 4 + 7 files changed, 289 insertions(+), 256 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 9a8eba6c..7e5cfe5a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4888,8 +4888,11 @@ constraint type is only supported for dimension 3 flexes with trilinear or quadr :at:`flex`: :at-val:`string, required` Name of the flex whose strain is being constrained. +.. _equality-flexstrain-cell: - +:at:`cell`: :at-val:`int(3), optional` + 3D grid index (i, j, k) identifying the cell in the flex object. The grid size is specified in the :ref:`cellcount + ` attribute. .. _tendon: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 60d828be..f394f2ac 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2062,6 +2062,9 @@ .. grid-item:: :ref:`flex` + .. grid-item:: + :ref:`cell` + .. grid-item:: :ref:`active` diff --git a/doc/changelog.rst b/doc/changelog.rst index b073a3f2..218bec89 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,6 +10,9 @@ General - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. +- Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the + number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new + attribute ":ref:`cell ` Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 41ca6a39..d8c81bda 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -88,75 +88,79 @@ static mjtNum mat3_det(const mjtNum* mat) { } -// compute node positions and Jacobians for flex strain constraints -// xpos: output array of size 3*nodenum (global node positions) -// node_jac: output array of size 3*nodenum*nv (dense Jacobians) -// combined_chain: output array of DOF indices used by any node (sparse mode) -// combined_nnz: output number of entries in combined_chain -static void node_pos_and_jac(const mjModel* m, mjData* d, int f, int nv, int issparse, mjtNum* xpos, - mjtNum* node_jac, int* combined_chain, int* combined_nnz) { - int nodenum = m->flex_nodenum[f]; - int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - int nstart = m->flex_nodeadr[f]; +// compute cell node Jacobians and combined chain for flex strain constraints +// npc: number of nodes per cell +// gindices: global indices of cell nodes in flex +// cell_node_jac: output array of size 3*npc*cell_nnz (allocated on stack) +// mj_{mark/free}Stack in calling function +static mjtNum* cell_pos_and_jac(const mjModel* m, mjData* d, int flex_id, int npc, const int* gindices, + int nv, const mjtNum* xpos_c, int* cell_chain, int* cell_nnz) { + int* nstart = m->flex_nodeadr + flex_id; + int* bodyid = m->flex_nodebodyid + *nstart; - for (int n = 0; n < nodenum; n++) { - if (m->flex_centered[f]) { - mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]); - } else { - mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart)); - mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]); + // build per-cell sparse chain: union of bodyChain for npc nodes + *cell_nnz = 0; + int* dof_used = mjSTACKALLOC(d, nv, int); + int* temp_chain = mjSTACKALLOC(d, nv, int); + mju_zeroInt(dof_used, nv); + for (int n = 0; n < npc; n++) { + int temp_nnz = mj_bodyChain(m, bodyid[gindices[n]], temp_chain); + for (int k = 0; k < temp_nnz; k++) { + dof_used[temp_chain[k]] = 1; + } + } + for (int q = 0; q < nv; q++) { + if (dof_used[q]) { + cell_chain[(*cell_nnz)++] = q; } } + // build per-cell node Jacobians: 3*npc x cell_nnz + mjtNum* cell_node_jac = mjSTACKALLOC(d, 3*npc*(*cell_nnz), mjtNum); + mju_zero(cell_node_jac, 3*npc*(*cell_nnz)); int* chain_col = mjSTACKALLOC(d, nv, int); mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); - mju_zero(node_jac, 3*nodenum*nv); - - for (int n = 0; n < nodenum; n++) { - int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col); - mju_zero(blk_jac, 3*nv); - mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col, 0); - + for (int n = 0; n < npc; n++) { + int body = bodyid[gindices[n]]; + int chain_n = mj_bodyChain(m, body, chain_col); + mju_zero(blk_jac, 3*chain_n); + mj_jacSparse(m, d, blk_jac, NULL, xpos_c + 3*n, + body, chain_n, chain_col, 0); + // map node's sparse chain into cell_chain indexing for (int r = 0; r < 3; r++) { - for (int k = 0; k < chain_nnz; k++) { - node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k]; + for (int k = 0; k < chain_n; k++) { + // find chain_col[k] in cell_chain via linear scan (chain is short) + for (int cc = 0; cc < *cell_nnz; cc++) { + if (cell_chain[cc] == chain_col[k]) { + cell_node_jac[(3*n + r)*(*cell_nnz) + cc] = blk_jac[r*chain_n + k]; + break; + } + } } } } - *combined_nnz = 0; - if (issparse) { - int* dof_used = mjSTACKALLOC(d, nv, int); - mju_zeroInt(dof_used, nv); - for (int n = 0; n < nodenum; n++) { - int temp_chain[200]; - int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain); - for (int k = 0; k < temp_nnz; k++) { - dof_used[temp_chain[k]] = 1; - } - } - - for (int q = 0; q < nv; q++) { - if (dof_used[q]) { - combined_chain[(*combined_nnz)++] = q; - } - } - } + return cell_node_jac; } -// compute strain Jacobian from strain derivative w.r.t. node positions -// dSdx: input array of size 3*nodenum (dStrain/dNodePosition) -// node_jac: input array of size 3*nodenum*nv (dense Jacobians) -// strain_jac: output array of size nv (dStrain/dq) -static void strain_jacobian(int nodenum, int nv, const mjtNum* dSdx, const mjtNum* node_jac, - mjtNum* strain_jac) { - mju_zero(strain_jac, nv); - for (int n = 0; n < nodenum; n++) { + +// compute strain Jacobian from strain derivative w.r.t. cell-local node positions +// dSdx_local: input array of size 3*npc (dStrain/dNodePosition for cell nodes) +// cell_node_jac: input array of size 3*npc*cell_nnz (sparse Jacobians) +// strain_jac: output array of size cell_nnz (dStrain/dq) +static void cell_strain_jacobian(int npc, int cell_nnz, + const mjtNum* dSdx_local, + const mjtNum* cell_node_jac, + mjtNum* strain_jac) { + mju_zero(strain_jac, cell_nnz); + for (int n = 0; n < npc; n++) { for (int c = 0; c < 3; c++) { + mjtNum w = dSdx_local[3*n + c]; + if (w == 0) continue; int row = 3*n + c; - for (int q = 0; q < nv; q++) { - strain_jac[q] += dSdx[row] * node_jac[row*nv + q]; + for (int k = 0; k < cell_nnz; k++) { + strain_jac[k] += w * cell_node_jac[row*cell_nnz + k]; } } } @@ -872,6 +876,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { + // each constraint represents a single cell; cell index in eq_data int f = id[0]; int nodenum = m->flex_nodenum[f]; int order = m->flex_interp[f]; @@ -881,27 +886,64 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; } + // only order 1 (trilinear) and 2 (quadratic) are supported + if (order > 2) { + 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]; - // allocate stack for node positions and Jacobians mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* node_jac = mjSTACKALLOC(d, 3*nodenum*nv, mjtNum); - int* combined_chain = mjSTACKALLOC(d, nv, int); - mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum); - int combined_nnz = 0; - node_pos_and_jac(m, d, f, nv, issparse, xpos, node_jac, combined_chain, &combined_nnz); + // get cell 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); + + // 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++) { + int gn = gindices[n]; + if (m->flex_centered[f]) { + mju_copy3(xpos_c + 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_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); + } + + // 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); + + + mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum); + mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); + + // for dense mode: allocate and zero a dense Jacobian buffer once + mjtNum* dense_jac = NULL; + if (!issparse) { + dense_jac = mjSTACKALLOC(d, nv, mjtNum); + mju_zero(dense_jac, nv); + } // Gauss-Legendre quadrature points in [0,1]^3 - // order=1: 2x2x2=8 points, order=2: 3x3x3=27 points int nquad = order + 1; int ngauss = nquad * nquad * nquad; - // 1D Gauss points mjtNum gp1d[3]; if (nquad == 2) { gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); @@ -912,8 +954,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); } - // build 3D Gauss points array (max 27 points) - mjtNum gauss[27][3]; + mjtNum (*gauss)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*ngauss, mjtNum); for (int gi = 0; gi < nquad; gi++) { for (int gj = 0; gj < nquad; gj++) { for (int gk = 0; gk < nquad; gk++) { @@ -925,163 +966,113 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } } - // reference positions for all nodes - int nstart = m->flex_nodeadr[f]; - mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - for (int n = 0; n < nodenum; n++) { - mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart)); + // B-bar: center-point volumetric constraints (trilinear) + if (order == 1) { + mjtNum center[3] = {0.5, 0.5, 0.5}; + mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; + + mju_defGradient(Fcur_c, center, xpos_c, order); + mju_defGradient(Fref_c, center, refpos_c, order); + mat3_inverse(Fref_c, Fref_inv_c); + mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); + + mjtNum C_c[9], E_c[9]; + mju_mulMatTMat3(C_c, F_c, F_c); + mju_scl(E_c, C_c, 0.5, 9); + E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; + + mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; + mjtNum J_c = mat3_det(F_c); + + mjtNum grad_c[8][3]; + shape_gradients(order, center, grad_c); + + for (int inv = 0; inv < 2; inv++) { + cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; + volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_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; + } + } + } } - // per-cell arrays - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum); - int gindices[125]; // max npc = 125 for quadratic + // Gauss integration + for (int g = 0; g < ngauss; g++) { + mjtNum* p = gauss[g]; - // loop over cells - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - // gather cell-local node positions - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, refpos, xpos_c, NULL, - refpos_c, gindices, NULL); + mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; + mju_defGradient(Fcur, p, xpos_c, order); + mju_defGradient(Fref, p, refpos_c, order); + mat3_inverse(Fref, Fref_inv); + mju_mulMatMat3(F, Fcur, Fref_inv); - // B-bar: center-point volumetric constraints (trilinear) - if (order == 1) { - mjtNum center[3] = {0.5, 0.5, 0.5}; - mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; + mjtNum C[9], E[9]; + mju_mulMatTMat3(C, F, F); + for (int j = 0; j < 9; j++) { + E[j] = 0.5 * C[j]; + } + E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - mju_defGradient(Fcur_c, center, xpos_c, order); - mju_defGradient(Fref_c, center, refpos_c, order); - mat3_inverse(Fref_c, Fref_inv_c); - mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); + mjtNum I1 = E[0] + E[4] + E[8]; + mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] + + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] + + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; + mjtNum I2 = 0.5 * (I1*I1 - trE2); + mjtNum I3 = mat3_det(E); - mjtNum C_c[9], E_c[9]; - mju_mulMatTMat3(C_c, F_c, F_c); - mju_scl(E_c, C_c, 0.5, 9); - E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; + mjtNum (*grad)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*npc, mjtNum); + shape_gradients(order, p, grad); - mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; - mjtNum J_c = mat3_det(F_c); + for (int s = 0; s < 6; s++) { + if (order == 1 && (s == 0 || s == 1 || s == 2)) { + continue; + } - mjtNum grad_c[8][3]; - shape_gradients(order, center, grad_c); + mjtNum dSdE[9]; + mju_zero(dSdE, 9); - for (int inv = 0; inv < 2; inv++) { - cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; + if (s == 0) { + cpos[0] = I1; + dSdE[0] = dSdE[4] = dSdE[8] = 1.0; + } else if (s == 1) { + cpos[0] = I2; + dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; + dSdE[8] = I1-E[8]; + dSdE[1] = -E[1]; dSdE[3] = -E[3]; + dSdE[2] = -E[2]; dSdE[6] = -E[6]; + dSdE[5] = -E[5]; dSdE[7] = -E[7]; + } else if (s == 2) { + cpos[0] = I3; + mat3_cofactor(E, dSdE); + } else { + int offdiag_idx[3] = {1, 2, 5}; + int ij = offdiag_idx[s - 3]; + cpos[0] = E[ij]; + dSdE[ij] = 1.0; + } - // compute local dSdx - volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); - - // scatter to global dSdx - mju_zero(dSdx, 3*nodenum); - for (int n = 0; n < npc; n++) { - mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); - } - - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mj_markStack(d); - mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sj[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - mj_freeStack(d); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } + invariant_dSdx(npc, grad, F, Fref_inv, dSdE, dSdx_local); + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = strain_jac[k]; } - - // Gauss integration per cell - for (int g = 0; g < ngauss; g++) { - mjtNum* p = gauss[g]; - - // F = Fcur * Fref_inv - mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; - mju_defGradient(Fcur, p, xpos_c, order); - mju_defGradient(Fref, p, refpos_c, order); - mat3_inverse(Fref, Fref_inv); - mju_mulMatMat3(F, Fcur, Fref_inv); - - // Green-Lagrange strain E = 0.5*(C - I) - mjtNum C[9], E[9]; - mju_mulMatTMat3(C, F, F); - for (int j = 0; j < 9; j++) { - E[j] = 0.5 * C[j]; - } - E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - - // 3 invariants of E - mjtNum I1 = E[0] + E[4] + E[8]; - mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] - + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] - + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; - mjtNum I2 = 0.5 * (I1*I1 - trE2); - mjtNum I3 = mat3_det(E); - - // shape function gradients at Gauss point - mjtNum grad[27][3]; - shape_gradients(order, p, grad); - - for (int s = 0; s < 6; s++) { - // skip I1,I2,I3 for trilinear (B-bar handles vol) - if (order == 1 && (s == 0 || s == 1 || s == 2)) { - continue; - } - - mjtNum dSdE[9]; - mju_zero(dSdE, 9); - - if (s == 0) { - cpos[0] = I1; - dSdE[0] = dSdE[4] = dSdE[8] = 1.0; - } else if (s == 1) { - cpos[0] = I2; - dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; - dSdE[8] = I1-E[8]; - dSdE[1] = -E[1]; dSdE[3] = -E[3]; - dSdE[2] = -E[2]; dSdE[6] = -E[6]; - dSdE[5] = -E[5]; dSdE[7] = -E[7]; - } else if (s == 2) { - cpos[0] = I3; - mat3_cofactor(E, dSdE); - } else { - int offdiag_idx[3] = {1, 2, 5}; - int ij = offdiag_idx[s - 3]; - cpos[0] = E[ij]; - dSdE[ij] = 1.0; - } - - // compute local dS/dx for cell nodes - invariant_dSdx(npc, grad, F, Fref_inv, dSdE, - dSdx_local); - - // scatter to global dSdx - mju_zero(dSdx, 3*nodenum); - for (int n = 0; n < npc; n++) { - mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); - } - - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mj_markStack(d); - mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sj[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - mj_freeStack(d); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } + 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; } } } @@ -1920,26 +1911,35 @@ void mj_diagApprox(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: use average node inv weight + // strain constraints: per-cell, use avg inv weight of cell's npc nodes int flex_id = m->eq_obj1id[id]; - int nodenum = m->flex_nodenum[flex_id]; int nstart = m->flex_nodeadr[flex_id]; int order = m->flex_interp[flex_id]; + int npc = (order+1)*(order+1)*(order+1); - // compute constraint count per cell, then multiply by ncells + // per-cell constraint count int nquad = order + 1; int ngauss = nquad * nquad * nquad; - int ncells = m->flex_cellnum[3*flex_id+0] - * m->flex_cellnum[3*flex_id+1] - * m->flex_cellnum[3*flex_id+2]; - int nconstraint = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); + 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 cy = m->flex_cellnum[3*flex_id+1]; + int cz = m->flex_cellnum[3*flex_id+2]; + + int gindices[125]; + 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 < nodenum; n++) { - int bodyid = m->flex_nodebodyid[nstart + n]; + for (int n = 0; n < npc; n++) { + int bodyid = m->flex_nodebodyid[nstart + gindices[n]]; avg_invweight += m->body_invweight0[2*bodyid]; } - avg_invweight /= nodenum; + avg_invweight /= npc; for (int c = 0; c < nconstraint; c++) { dA[i++] = avg_invweight; } @@ -2529,34 +2529,35 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: - // Q1: B-bar, 2 center (I1, J-1) + 3*8 shear = 26 - // Q2: full 3x3x3 Gauss, 6*27 = 162 - // skip if not interpolated (order == 0 or no nodes) - int order = m->flex_interp[id[0]]; - int nodenum = m->flex_nodenum[id[0]]; - if (!order || !nodenum) { + // per-cell strain constraints: each equality is one cell + int f = id[0]; + int order = m->flex_interp[f]; + if (!order || !m->flex_nodenum[f]) { break; } - int nquad = order + 1; // 2 for order=1, 3 for order=2 - int ngauss = nquad * nquad * nquad; // 8 or 27 - int ncells = m->flex_cellnum[3*id[0]+0] - * m->flex_cellnum[3*id[0]+1] - * m->flex_cellnum[3*id[0]+2]; - size = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); + int npc = (order+1)*(order+1)*(order+1); + int nquad = order + 1; + int ngauss = nquad * nquad * nquad; + size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // per cell if (nnz) { - // Count unique DOFs across all node bodies (matching instantiation) - int nstart = m->flex_nodeadr[id[0]]; - int* nodebodies = mjSTACKALLOC(d, nodenum, int); - for (int n = 0; n < nodenum; n++) { - nodebodies[n] = m->flex_nodebodyid[nstart + n]; + // get cell index from eq_data + 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 cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + + // get the npc node body IDs for this cell + int gindices[125]; + 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]; + int* cell_bodies = mjSTACKALLOC(d, npc, int); + for (int n = 0; n < npc; n++) { + cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } - - // mj_jacSumCount deduplicates shared DOFs - NV = mj_jacSumCount(m, d, chain, nodenum, nodebodies); - - // each constraint row shares this combined NV + NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only NV = size * NV; } break; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 4035a176..a86a2f33 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -680,20 +680,35 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf mjs_setDouble(pf->vert, point.data(), point.size()); } - // create edge equality constraint + // create equality constraints if (equality) { - mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); - mjs_setDefault(pe->element, &model->Default()->spec); // equality 1=edge(mjEQ_FLEX), 2=vert(mjEQ_FLEXVERT), 3=strain(mjEQ_FLEXSTRAIN) - if (equality == 1) { - pe->type = mjEQ_FLEX; - } else if (equality == 2) { - pe->type = mjEQ_FLEXVERT; + if (equality == 1 || equality == 2) { + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { - pe->type = mjEQ_FLEXSTRAIN; + // create one strain constraint per cell, storing cell index in eq_data + 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++) { + 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; + } + } + } } - pe->active = true; - mjs_setString(pe->name1, name.c_str()); } return true; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d38bde20..360c46be 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -371,7 +371,7 @@ std::vector MJCF[nMJCF] = { "active", "solref", "solimp"}, {"flexvert", "*", "name", "class", "flex", "active", "solref", "solimp"}, - {"flexstrain", "*", "name", "class", "flex", + {"flexstrain", "*", "name", "class", "flex", "cell", "active", "solref", "solimp"}, {">"}, @@ -2245,8 +2245,12 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { case mjEQ_FLEX: case mjEQ_FLEXVERT: + ReadAttrTxt(elem, "flex", name1, true); + break; + case mjEQ_FLEXSTRAIN: ReadAttrTxt(elem, "flex", name1, true); + ReadAttr(elem, "cell", 3, equality->data, text); break; case mjEQ_DISTANCE: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 007a76a3..ebe95bab 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -728,8 +728,12 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe case mjEQ_FLEX: case mjEQ_FLEXVERT: + WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); + break; + case mjEQ_FLEXSTRAIN: WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); + WriteAttr(elem, "cell", 3, equality->data); break; default: From 6bf31cf68a0f5454f6fa329dc05be793e158c980 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 01:12:03 -0700 Subject: [PATCH 02/29] Update Windows build notes. PiperOrigin-RevId: 902480947 Change-Id: I69f099b3d7eabd6513a62e74e5ef697a5062e7b5 --- doc/programming/index.rst | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/doc/programming/index.rst b/doc/programming/index.rst index 48ef13f2..7b34cdcc 100644 --- a/doc/programming/index.rst +++ b/doc/programming/index.rst @@ -116,9 +116,10 @@ target directory. **Notes:** -- When building on Windows, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or later is - installed (see :issue:`862` for more details). -- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release`` +- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release``. +- When building on Windows with MSVC, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or + later is installed (see :issue:`862` for more details). +- We've found that performance on Windows is best when building with Clang, rather than MSVC. .. tip:: As a reference, a working build configuration can be found in MuJoCo's From f3f12bfad6a4cf974ac0cfbe5c706a4bcf048eb3 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 01:23:20 -0700 Subject: [PATCH 03/29] Manage dynamic meshes (skins, flexes) in model_objects. Simplifies Renderable as it no longer has the option of owning any Meshes. Instead, the ModelObjects owns the Meshes for flex and skin geom (just like it owns all the other mjModel Meshes). PiperOrigin-RevId: 902485912 Change-Id: I8b9f9d394de6c46f7e25aa4b3885a374aad05294 --- .../filament/filament/model_objects.cc | 18 +++++++--------- .../filament/filament/model_objects.h | 16 +++++++------- .../filament/filament/renderable.cc | 21 +++---------------- .../filament/filament/renderable.h | 8 ++----- .../filament/filament/scene_bridge.cc | 4 ++++ .../filament/filament/scene_geom_util.cc | 14 +++++-------- 6 files changed, 30 insertions(+), 51 deletions(-) diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 1f88dce0..15121743 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -630,20 +630,11 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { height_fields_[id] = std::make_unique(engine_, data); } -MeshPtr ModelObjects::CreateFlexMesh(const mjvScene* scene, - const mjvGeom& geom) { +void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) { MeshData data; DefaultMeshData(&data); UpdateSkinFlexMeshData(&data, model_, scene, geom); - return std::make_unique(engine_, data); -} - -MeshPtr ModelObjects::CreateSkinMesh(const mjvScene* scene, - const mjvGeom& geom) { - MeshData data; - DefaultMeshData(&data); - UpdateSkinFlexMeshData(&data, model_, scene, geom); - return std::make_unique(engine_, data); + dynamic_meshes_[geom.objid] = std::make_unique(engine_, data); } const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { @@ -672,6 +663,11 @@ const Mesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { return shapes_[shape].get(); } +const Mesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const { + auto it = dynamic_meshes_.find(geom_id); + return it != dynamic_meshes_.end() ? it->second.get() : nullptr; +} + const Texture* ModelObjects::GetTexture(int tex_id) const { auto it = textures_.find(tex_id); return it != textures_.end() ? it->second.get() : nullptr; diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index bc6a005b..d85693e3 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ +#include #include #include #include @@ -55,6 +56,8 @@ class ModelObjects { void UploadHeightField(const mjModel* model, int id); + void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom); + // Returns the filament engine used by the ModelObjects to create filament // objects. filament::Engine* GetEngine() const { return engine_; } @@ -63,12 +66,10 @@ class ModelObjects { const Mesh* GetShapeBuffer(ShapeType shape) const; const Mesh* GetMeshBuffer(int data_id) const; const Mesh* GetHeightFieldBuffer(int hfield_id) const; + const Mesh* GetFlexSkinGeomMesh(int geom_id) const; const Texture* GetTexture(int tex_id) const; const Texture* GetTexture(int mat_id, int role) const; - MeshPtr CreateFlexMesh(const mjvScene* scene, const mjvGeom& geom); - MeshPtr CreateSkinMesh(const mjvScene* scene, const mjvGeom& geom); - filament::Skybox* CreateSkybox(); filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity); @@ -86,10 +87,11 @@ class ModelObjects { filament::Engine* engine_ = nullptr; std::vector skyboxes_; std::vector indirect_lights_; - std::array shapes_; - std::unordered_map meshes_; - std::unordered_map convex_hulls_; - std::unordered_map height_fields_; + std::array, kNumShapes> shapes_; + std::unordered_map> meshes_; + std::unordered_map> convex_hulls_; + std::unordered_map> height_fields_; + std::unordered_map> dynamic_meshes_; std::unordered_map> textures_; float specular_multiplier_ = 0.2f; float shininess_multiplier_ = 0.1f; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 6a3da110..4fc9aaf3 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -66,25 +66,12 @@ void Renderable::RemoveLastEntity() { void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(index, mesh, nullptr, elem_offset, elem_count); - UpdateEntity(index, mesh_info); -} - -void Renderable::UpdateMesh(int index, MeshPtr mesh, int elem_offset, - int elem_count) { - MeshInfo& mesh_info = - SetMesh(index, mesh.get(), std::move(mesh), elem_offset, elem_count); + MeshInfo& mesh_info = SetMesh(index, mesh, elem_offset, elem_count); UpdateEntity(index, mesh_info); } void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(-1, mesh, nullptr, elem_offset, elem_count); - AppendEntity(mesh_info); -} - -void Renderable::AppendMesh(MeshPtr mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = - SetMesh(-1, mesh.get(), std::move(mesh), elem_offset, elem_count); + MeshInfo& mesh_info = SetMesh(-1, mesh, elem_offset, elem_count); AppendEntity(mesh_info); } @@ -154,8 +141,7 @@ void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) { } Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, - MeshPtr owned_mesh, int elem_offset, - int elem_count) { + int elem_offset, int elem_count) { if (index == -1) { index = meshes_.size(); meshes_.emplace_back(); @@ -165,7 +151,6 @@ Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, } MeshInfo* mesh_info = &meshes_[index]; - mesh_info->owned_mesh = std::move(owned_mesh); mesh_info->mesh = mesh; mesh_info->elem_offset = elem_offset; mesh_info->elem_count = elem_count; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index dbe03498..016824b8 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -60,15 +60,12 @@ class Renderable { // can be used to specify a submesh to append. If elem_count is 0, assumes // the entire mesh should be appended. void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - void AppendMesh(MeshPtr mesh, int elem_offset = 0, int elem_count = 0); // Replaces the mesh at the index with a new mesh. The elem_offset and // elem_count parameters can be used to specify a submesh to append. If // elem_count is 0, assumes the entire mesh should be appended. void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - void UpdateMesh(int index, MeshPtr mesh, int elem_offset = 0, - int elem_count = 0); // Returns the number of meshes that define the renderable. int GetNumMeshes() const { return meshes_.size(); } @@ -124,7 +121,6 @@ class Renderable { private: struct MeshInfo { - MeshPtr owned_mesh; const Mesh* mesh = nullptr; int elem_offset = 0; int elem_count = 0; @@ -132,8 +128,8 @@ class Renderable { // Sets the mesh information for the mesh at the given index. If index is -1, // a new mesh will be appended to the renderable. - MeshInfo& SetMesh(int index, const Mesh* mesh, MeshPtr owned_mesh, - int elem_offset, int elem_count); + MeshInfo& SetMesh(int index, const Mesh* mesh, int elem_offset, + int elem_count); // Appends a new filament::Entity to the renderable, configured to use the // given mesh. diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index e8198b00..3438b8c9 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -381,6 +381,10 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } + if (geom->type == mjGEOM_FLEX || geom->type == mjGEOM_SKIN) { + model_objects_->CreateSkinFlexMesh(scene, *geom); + } + std::unique_ptr renderable = CreateGeomRenderable( *geom, scene, object_mgr_, model_objects_.get(), headpos); diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index e60bacf4..1b20fd18 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -96,13 +96,9 @@ static void AddMesh(Renderable& renderable, ModelObjects* model_objs, renderable.AppendMesh(mesh); } -static void AddGeom(Renderable& renderable, ModelObjects* model_objs, - const mjvScene* scene, const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - renderable.AppendMesh(model_objs->CreateFlexMesh(scene, geom)); - } else if (geom.type == mjGEOM_SKIN) { - renderable.AppendMesh(model_objs->CreateSkinMesh(scene, geom)); - } +static void AddSkinFlexMesh(Renderable& renderable, ModelObjects* model_objs, + int objid) { + renderable.AppendMesh(model_objs->GetFlexSkinGeomMesh(objid)); } static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, @@ -183,10 +179,10 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, AddShape(renderable, model_objects, ModelObjects::kTriangle); break; case mjGEOM_FLEX: - AddGeom(renderable, model_objects, scene, geom); + AddSkinFlexMesh(renderable, model_objects, geom.objid); break; case mjGEOM_SKIN: - AddGeom(renderable, model_objects, scene, geom); + AddSkinFlexMesh(renderable, model_objects, geom.objid); break; case mjGEOM_NONE: case mjGEOM_LABEL: From cf3f6ccf1f5633afb7b0689a680c1b36a1085bbc Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Mon, 20 Apr 2026 01:26:54 -0700 Subject: [PATCH 04/29] Make file search deterministic in FindFileInPath. Collect all entries from the recursive directory iterator, sort them, and then check for the filename. This ensures that if multiple files with the same name exist in different subdirectories, the one found is always the same, regardless of the filesystem's directory iteration order. PiperOrigin-RevId: 902487291 Change-Id: Ia4c45cd2e3cab4a4e3825c267fc47c3134f99f76 --- src/experimental/platform/helpers.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/experimental/platform/helpers.cc b/src/experimental/platform/helpers.cc index add021f9..e10d8e8d 100644 --- a/src/experimental/platform/helpers.cc +++ b/src/experimental/platform/helpers.cc @@ -14,6 +14,7 @@ #include "experimental/platform/helpers.h" +#include #include #include #include @@ -23,6 +24,7 @@ #include #include #include +#include #include "webp/encode.h" #include "webp/types.h" @@ -77,8 +79,13 @@ std::string ResolveFile(const std::string& filename, return resolved; } + std::vector entries; for (const auto& it : std::filesystem::recursive_directory_iterator(path)) { - resolved = CheckPathForFile(it.path(), filename); + entries.push_back(it.path()); + } + std::sort(entries.begin(), entries.end()); + for (const auto& entry : entries) { + resolved = CheckPathForFile(entry, filename); if (!resolved.empty()) { return resolved; } From a04c2b1b4a12771d7834183bc153494808fd4574 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 01:46:48 -0700 Subject: [PATCH 05/29] Consolidate stack allocation calls in primal solvers down from 30 to 2 (dense) or 6 (sparse). Preparation for atomic allocation calls in threaded mode. PiperOrigin-RevId: 902496039 Change-Id: I39df011951713505c4743c7475d74856ddbdf4f9 --- src/engine/engine_solver.c | 201 ++++++++++++++++++++++--------------- 1 file changed, 118 insertions(+), 83 deletions(-) diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index b02302fc..cc9e01ce 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -810,6 +810,12 @@ typedef struct { mjtNum* Mgrad; // M\grad or H\grad (nv x 1) mjtNum* search; // linesearch vector (nv x 1) mjtNum* quad; // quadratic polynomials for constraint costs (nefc x 3) + int* oldstate; // previous constraint state (nefc x 1) + + // CG arrays (PrimalAllocate, CG only) + mjtNum* gradold; // previous gradient (nv x 1) + mjtNum* Mgradold; // previous preconditioned gradient (nv x 1) + mjtNum* Mgraddif; // gradient difference (nv x 1) // Newton arrays, known-size (PrimalAllocate) mjtNum* D; // constraint inertia (nefc x 1) @@ -960,55 +966,99 @@ static void PrimalPointers(const mjModel* m, const mjData* d, mjPrimalContext* c // allocate fixed-size arrays in mjPrimalContext // mj_{mark/free}Stack in calling function! static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { - // local sizes + // local sizes and flags int nv = ctx->nv; int nefc = ctx->nefc; + int nJ = ctx->is_sparse ? d->nJ : 0; + int is_sparse = ctx->is_sparse; + int is_elliptic = ctx->is_elliptic; - // common arrays - ctx->Jaref = mjSTACKALLOC(d, nefc, mjtNum); - ctx->Jv = mjSTACKALLOC(d, nefc, mjtNum); - ctx->Ma = mjSTACKALLOC(d, nv, mjtNum); - ctx->Mv = mjSTACKALLOC(d, nv, mjtNum); - ctx->grad = mjSTACKALLOC(d, nv, mjtNum); - ctx->Mgrad = mjSTACKALLOC(d, nv, mjtNum); - ctx->search = mjSTACKALLOC(d, nv, mjtNum); - ctx->quad = mjSTACKALLOC(d, nefc*3, mjtNum); + // compute mjtNum block size + size_t nNum = 5*nefc + 5*nv; // common arrays + if (is_sparse) nNum += nJ; // JT + if (flg_Newton) { + nNum += nefc + nv; // D, cholupd + if (is_elliptic) nNum += 6*nv; // LTJ + if (is_sparse) { + nNum += nv; // buf_val + } else { + nNum += nv*nv; // L (dense) + if (is_elliptic) nNum += nv*nv; // Lcone (dense) + } + } else { + nNum += 3*nv; // CG arrays + } - // sparse only, compute Jacobian transpose - if (ctx->is_sparse) { - ctx->JT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->JT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->JT_rowsuper = mjSTACKALLOC(d, nv, int); - ctx->JT_colind = mjSTACKALLOC(d, d->nJ, int); - ctx->JT = mjSTACKALLOC(d, d->nJ, mjtNum); - int offset = ctx->J_rowadr[0]; + // compute int block size + size_t nInt = nefc; // oldstate + if (is_sparse) { + nInt += 3*nv + nJ; // JT sparse + if (flg_Newton) nInt += 9*nv; // Newton sparse + } + + // allocate mjtNum and int blocks + mjtNum* numblock = mjSTACKALLOC(d, nNum, mjtNum); + int* intblock = mjSTACKALLOC(d, nInt, int); + + // carve mjtNum block + ctx->Jaref = numblock; numblock += nefc; + ctx->Jv = numblock; numblock += nefc; + ctx->Ma = numblock; numblock += nv; + ctx->Mv = numblock; numblock += nv; + ctx->grad = numblock; numblock += nv; + ctx->Mgrad = numblock; numblock += nv; + ctx->search = numblock; numblock += nv; + ctx->quad = numblock; numblock += 3*nefc; + if (is_sparse) { + ctx->JT = numblock; numblock += nJ; + } + if (flg_Newton) { + ctx->D = numblock; numblock += nefc; + ctx->cholupd = numblock; numblock += nv; + if (is_elliptic) { + ctx->LTJ = numblock; numblock += 6*nv; + } + if (is_sparse) { + ctx->buf_val = numblock; numblock += nv; + } else { + ctx->nL = nv*nv; + ctx->L = numblock; numblock += ctx->nL; + ctx->Lcone = is_elliptic ? numblock : NULL; + if (is_elliptic) numblock += ctx->nL; + } + } else { + ctx->gradold = numblock; numblock += nv; + ctx->Mgradold = numblock; numblock += nv; + ctx->Mgraddif = numblock; numblock += nv; + } + + // carve int block + ctx->oldstate = intblock; intblock += nefc; + if (is_sparse) { + ctx->JT_rownnz = intblock; intblock += nv; + ctx->JT_rowadr = intblock; intblock += nv; + ctx->JT_rowsuper = intblock; intblock += nv; + ctx->JT_colind = intblock; intblock += nJ; + } + if (flg_Newton && is_sparse) { + ctx->H_rowadr = intblock; intblock += nv; + ctx->H_rownnz = intblock; intblock += nv; + ctx->HT_rownnz = intblock; intblock += nv; + ctx->HT_rowadr = intblock; intblock += nv; + ctx->L_rownnz = intblock; intblock += nv; + ctx->L_rowadr = intblock; intblock += nv; + ctx->LT_rownnz = intblock; intblock += nv; + ctx->LT_rowadr = intblock; intblock += nv; + ctx->buf_ind = intblock; intblock += nv; + } + + // sparse: compute Jacobian transpose + if (is_sparse) { + int offset = ctx->J_rowadr[0]; mju_transposeSparse(ctx->JT, ctx->J + offset, nefc, nv, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind + offset); } - - // Newton only, known-size arrays - if (flg_Newton) { - ctx->D = mjSTACKALLOC(d, nefc, mjtNum); - ctx->cholupd = mjSTACKALLOC(d, nv, mjtNum); - if (ctx->is_elliptic) { - ctx->LTJ = mjSTACKALLOC(d, 6*nv, mjtNum); - } - - // sparse Newton only - if (ctx->is_sparse) { - ctx->H_rowadr = mjSTACKALLOC(d, nv, int); - ctx->H_rownnz = mjSTACKALLOC(d, nv, int); - ctx->HT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->HT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->L_rownnz = mjSTACKALLOC(d, nv, int); - ctx->L_rowadr = mjSTACKALLOC(d, nv, int); - ctx->LT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->LT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->buf_val = mjSTACKALLOC(d, nv, mjtNum); - ctx->buf_ind = mjSTACKALLOC(d, nv, int); - } - } } @@ -1529,7 +1579,7 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // sparse if (ctx->is_sparse) { - // initialize Hessian rowadr, rownnz; get total nonzeros + // count Hessian nonzeros, initialize rowadr, rownnz ctx->nH = mju_sqrMatTDSparseSymbolic( ctx->H_rownnz, ctx->H_rowadr, NULL, NULL, nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, @@ -1538,17 +1588,19 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // add M nonzeros to Hessian total (unavoidable overcounting since H_colind is still unknown) ctx->nH += ctx->M_rowadr[nv - 1] + ctx->M_rownnz[nv - 1]; - // shift H row addresses to make room for C + // nH is known: allocate H, H_colind, HT_colind + ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); + int* H_intblock = mjSTACKALLOC(d, 2*ctx->nH, int); + ctx->H_colind = H_intblock; + ctx->HT_colind = H_intblock + ctx->nH; + + // shift H row addresses to make room for M int shift = 0; for (int r = 0; r < nv - 1; r++) { shift += ctx->M_rownnz[r]; ctx->H_rowadr[r + 1] += shift; } - // allocate H_colind and H - ctx->H_colind = mjSTACKALLOC(d, ctx->nH, int); - ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); - // compute H = J'*D*J: symbolic phase mju_sqrMatTDSparseSymbolic( ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, NULL, @@ -1562,13 +1614,12 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->JT, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, ctx->D, d); - // add mass matrix: H = J'*D*J + C + // add mass matrix: H = J'*D*J + M mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, ctx->buf_val, ctx->buf_ind); - // compute H' (upper triangle, required for symbolic Cholesky) - ctx->HT_colind = mjSTACKALLOC(d, ctx->nH, int); + // compute H' sparse structure (upper triangle, required for symbolic Cholesky) mju_transposeSparse(NULL, NULL, nv, nv, ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, NULL, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind); @@ -1578,16 +1629,16 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, nv, d); - // allocate L_colind, L, Lcone - ctx->L_colind = mjSTACKALLOC(d, ctx->nL, int); - ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); - if (ctx->is_elliptic) { - ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); - } - - // allocate LT (CSC representation of L) - ctx->LT_colind = mjSTACKALLOC(d, ctx->nL, int); - ctx->LT_map = mjSTACKALLOC(d, ctx->nL, int); + // nL is known: allocate blocks and carve L_colind, LT_colind, LT_map, L, Lcone + size_t nL_int = 2*ctx->nL + ctx->nL; // L_colind + LT_colind + LT_map + size_t nL_num = ctx->is_elliptic ? 2*ctx->nL : ctx->nL; // L + Lcone + int* L_intblock = mjSTACKALLOC(d, nL_int, int); + mjtNum* L_numblock = mjSTACKALLOC(d, nL_num, mjtNum); + ctx->L_colind = L_intblock; + ctx->LT_colind = L_intblock + ctx->nL; + ctx->LT_map = L_intblock + 2*ctx->nL; + ctx->L = L_numblock; + ctx->Lcone = ctx->is_elliptic ? L_numblock + ctx->nL : NULL; // symbolic Cholesky: populate L_colind and LT structures mju_cholFactorSymbolic(ctx->L_colind, ctx->L_rownnz, ctx->L_rowadr, @@ -1598,13 +1649,6 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // dense else { - // allocate L, Lcone - ctx->nL = nv*nv; - ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); - if (ctx->is_elliptic) { - ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); - } - // compute H = M + J'*D*J mju_sqrMatTD_impl(ctx->L, ctx->J, ctx->D, nefc, nv, /*flg_upper=*/ 0); mju_addToSymSparse(ctx->L, ctx->M, ctx->nv, @@ -1691,13 +1735,12 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) // elliptic case: Hcone = H + cone_contributions static void HessianCone(mjData* d, mjPrimalContext* ctx) { int nv = ctx->nv, nefc = ctx->nefc; + mjtNum* LTJ = ctx->LTJ; mjtNum local[36]; // start with Hcone = H mju_copy(ctx->Lcone, ctx->L, ctx->nL); - mjtNum* LTJ = ctx->LTJ; - // add contributions for (int i=0; i < nefc; i++) { if (ctx->efc_state[i] == mjCNSTRSTATE_CONE) { @@ -1818,7 +1861,6 @@ static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldst static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, int flg_Newton) { int iter = 0; mjtNum alpha, beta; - mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL; mjPrimalContext ctx; mj_markStack(d); @@ -1829,14 +1871,7 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // local copies int nv = ctx.nv; int nefc = ctx.nefc; - - // allocate local storage - if (!flg_Newton) { - gradold = mjSTACKALLOC(d, nv, mjtNum); - Mgradold = mjSTACKALLOC(d, nv, mjtNum); - Mgraddif = mjSTACKALLOC(d, nv, mjtNum); - } - int* oldstate = mjSTACKALLOC(d, nefc, int); + int* oldstate = ctx.oldstate; // compute Ma = M * qacc mju_mulSymVecSparse(ctx.Ma, ctx.M, ctx.qacc, nv, @@ -1895,8 +1930,8 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // save old if (!flg_Newton) { - mju_copy(gradold, ctx.grad, nv); - mju_copy(Mgradold, ctx.Mgrad, nv); + mju_copy(ctx.gradold, ctx.grad, nv); + mju_copy(ctx.Mgradold, ctx.Mgrad, nv); } mju_copyInt(oldstate, ctx.efc_state, nefc); mjtNum oldcost = ctx.cost; @@ -1933,9 +1968,9 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i mju_scl(ctx.search, ctx.Mgrad, -1, nv); } else { // Polak-Ribiere - mju_sub(Mgraddif, ctx.Mgrad, Mgradold, nv); - beta = mju_dot(ctx.grad, Mgraddif, nv) / - mju_max(mjMINVAL, mju_dot(gradold, Mgradold, nv)); + mju_sub(ctx.Mgraddif, ctx.Mgrad, ctx.Mgradold, nv); + beta = mju_dot(ctx.grad, ctx.Mgraddif, nv) / + mju_max(mjMINVAL, mju_dot(ctx.gradold, ctx.Mgradold, nv)); // reset if negative if (beta < 0) { From bf9be2c3127f89f85205419ea593c7b1240ff5a9 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 02:01:02 -0700 Subject: [PATCH 06/29] Remove MeshPtr type alias. PiperOrigin-RevId: 902502036 Change-Id: I5bd0c185f204518f0cd0d9189741c17c24b51eb6 --- .../filament/filament/builtins.cc | 23 ++++++++++--------- src/experimental/filament/filament/builtins.h | 22 ++++++++++-------- .../filament/filament/imgui_bridge.h | 2 +- src/experimental/filament/filament/mesh.h | 2 -- 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 5ee489b3..86a6a5c5 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -65,7 +65,8 @@ class BuiltinBuilder : MeshData { virtual ~BuiltinBuilder() = default; template - static MeshPtr Create(filament::Engine* engine, Args&&... args) { + static std::unique_ptr Create(filament::Engine* engine, + Args&&... args) { auto builder = new T(std::forward(args)...); MeshData* mesh_data = builder->PrepareMeshData(); mesh_data->release_callback = +[](void* user_data) { @@ -621,43 +622,43 @@ class DomeBuilder : public BuiltinBuilder { } }; -MeshPtr CreateLine(filament::Engine* engine) { +std::unique_ptr CreateLine(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreatePlane(filament::Engine* engine, int nquad) { +std::unique_ptr CreatePlane(filament::Engine* engine, int nquad) { return BuiltinBuilder::Create(engine, nquad); } -MeshPtr CreateTriangle(filament::Engine* engine) { +std::unique_ptr CreateTriangle(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreateBox(filament::Engine* engine, int nquad) { +std::unique_ptr CreateBox(filament::Engine* engine, int nquad) { return BuiltinBuilder::Create(engine, nquad); } -MeshPtr CreateLineBox(filament::Engine* engine) { +std::unique_ptr CreateLineBox(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateDisk(filament::Engine* engine, int nslice) { +std::unique_ptr CreateDisk(filament::Engine* engine, int nslice) { return BuiltinBuilder::Create(engine, nslice); } -MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index c698bca8..5fd5c8a5 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -15,22 +15,24 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ +#include + #include #include "experimental/filament/filament/mesh.h" // Generates buffers for built-in shapes. namespace mujoco { -MeshPtr CreateLine(filament::Engine* engine); -MeshPtr CreatePlane(filament::Engine* engine, int nquad); -MeshPtr CreateTriangle(filament::Engine* engine); -MeshPtr CreateBox(filament::Engine* engine, int nquad); -MeshPtr CreateLineBox(filament::Engine* engine); -MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateDisk(filament::Engine* engine, int nslice); -MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateLine(filament::Engine* engine); +std::unique_ptr CreatePlane(filament::Engine* engine, int nquad); +std::unique_ptr CreateTriangle(filament::Engine* engine); +std::unique_ptr CreateBox(filament::Engine* engine, int nquad); +std::unique_ptr CreateLineBox(filament::Engine* engine); +std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateDisk(filament::Engine* engine, int nslice); +std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice); } // namespace mujoco diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index 03ee4f47..4e36b333 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -62,7 +62,7 @@ class ImguiBridge { ObjectManager* object_mgr_ = nullptr; SceneView* scene_view_ = nullptr; std::vector> renderables_; - std::vector meshes_; + std::vector> meshes_; std::unordered_map> textures_; }; diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index cc30b6d3..b75ad6de 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -185,8 +185,6 @@ class Mesh { int num_attributes_ = 0; }; -using MeshPtr = std::unique_ptr; - } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ From 3230cf99f90261e896f26ee683731c91e9bea19c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 02:02:01 -0700 Subject: [PATCH 07/29] Change flex constraints to eigenmodes of the stiffness matrix. This provides a reduction from 26 to 18 constraints for trilinear and from 162 to 75 for quadratic. The assembly of the constraints becomes trivial. In total the speedup for a trilinear 3x3x3 grid is about 3x. PiperOrigin-RevId: 902502398 Change-Id: I764772c7adef78da5a644f64701f842d36e4b543 --- src/engine/engine_core_constraint.c | 373 ++++----------------- src/engine/engine_derivative.c | 5 + src/engine/engine_passive.c | 7 +- src/user/user_flexcomp.cc | 1 + src/user/user_mesh.cc | 62 +++- src/user/user_model.cc | 2 + src/user/user_objects.h | 1 + src/user/user_util.cc | 99 +++++- src/user/user_util.h | 16 +- test/engine/engine_core_constraint_test.cc | 143 ++++++++ test/user/CMakeLists.txt | 2 + test/user/user_util_test.cc | 139 ++++++++ 12 files changed, 523 insertions(+), 327 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d8c81bda..5eab4da6 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -47,47 +47,6 @@ //-------------------------- utility functions ----------------------------------------------------- -// compute 3x3 matrix inverse, storing result in out -// assumes matrix is invertible (det != 0) -static void mat3_inverse(const mjtNum* mat, mjtNum* out) { - mjtNum det = mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) - - mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) + - mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]); - - out[0] = (mat[4]*mat[8] - mat[5]*mat[7]) / det; - out[1] = -(mat[1]*mat[8] - mat[2]*mat[7]) / det; - out[2] = (mat[1]*mat[5] - mat[2]*mat[4]) / det; - out[3] = -(mat[3]*mat[8] - mat[5]*mat[6]) / det; - out[4] = (mat[0]*mat[8] - mat[2]*mat[6]) / det; - out[5] = -(mat[0]*mat[5] - mat[2]*mat[3]) / det; - out[6] = (mat[3]*mat[7] - mat[4]*mat[6]) / det; - out[7] = -(mat[0]*mat[7] - mat[1]*mat[6]) / det; - out[8] = (mat[0]*mat[4] - mat[1]*mat[3]) / det; -} - - -// compute 3x3 matrix cofactor, storing result in out -static void mat3_cofactor(const mjtNum* mat, mjtNum* out) { - out[0] = mat[4]*mat[8] - mat[5]*mat[7]; - out[1] = -(mat[3]*mat[8] - mat[5]*mat[6]); - out[2] = mat[3]*mat[7] - mat[4]*mat[6]; - out[3] = -(mat[1]*mat[8] - mat[2]*mat[7]); - out[4] = mat[0]*mat[8] - mat[2]*mat[6]; - out[5] = -(mat[0]*mat[7] - mat[1]*mat[6]); - out[6] = mat[1]*mat[5] - mat[2]*mat[4]; - out[7] = -(mat[0]*mat[5] - mat[2]*mat[3]); - out[8] = mat[0]*mat[4] - mat[1]*mat[3]; -} - - -// compute 3x3 matrix determinant -static mjtNum mat3_det(const mjtNum* mat) { - return mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) - - mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) + - mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]); -} - - // compute cell node Jacobians and combined chain for flex strain constraints // npc: number of nodes per cell // gindices: global indices of cell nodes in flex @@ -167,133 +126,6 @@ static void cell_strain_jacobian(int npc, int cell_nnz, } -// basis functions for flex strain constraints -static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) { - if (order == 1) { - *phi = (i == 0 ? 1 - p : p); - *dphi = (i == 0 ? -1 : 1); - } else { - if (i == 0) { - *phi = 2 * p * p - 3 * p + 1; - *dphi = 4 * p - 3; - } else if (i == 1) { - *phi = 4 * (p - p * p); - *dphi = 4 * (1 - 2 * p); - } else { - *phi = 2 * p * p - p; - *dphi = 4 * p - 1; - } - } -} - - -// compute shape function gradients at a parametric point -// grad: output array of size nodenum x 3 (gradient w.r.t. parametric coords) -static void shape_gradients( - int order, const mjtNum* p, mjtNum grad[][3]) { - int npoint = (order + 1) * (order + 1) * (order + 1); - int stride = order + 1; - - for (int n = 0; n < npoint; n++) { - int ix = n / (stride * stride); - int iy = (n / stride) % stride; - int iz = n % stride; - - mjtNum phi_x, phi_y, phi_z, dphi_x, dphi_y, dphi_z; - basis(order, ix, p[0], &phi_x, &dphi_x); - basis(order, iy, p[1], &phi_y, &dphi_y); - basis(order, iz, p[2], &phi_z, &dphi_z); - - grad[n][0] = dphi_x * phi_y * phi_z; - grad[n][1] = phi_x * dphi_y * phi_z; - grad[n][2] = phi_x * phi_y * dphi_z; - } -} - - -// compute dStrain/dNodePosition for volumetric invariants (I1 or J-1) -// dSdx: output array of size 3*nodenum -static void volumetric_dSdx(int invariant_type, int nodenum, mjtNum grad[][3], - const mjtNum* F, const mjtNum* Fref_inv, mjtNum* dSdx) { - mju_zero(dSdx, 3*nodenum); - - if (invariant_type == 0) { - mjtNum dSdE[9] = {1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0}; - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dS = 0; - for (int ij = 0; ij < 9; ij++) { - int ii = ij / 3; - int jj = ij % 3; - - mjtNum dF_ci = 0; - for (int k = 0; k < 3; k++) { - dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; - } - mjtNum dF_cj = 0; - for (int k = 0; k < 3; k++) { - dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; - } - - mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; - dS += dSdE[ij] * 0.5 * dC_ij; - } - dSdx[3*n + c] = dS; - } - } - } else { - mjtNum cofF[9]; - mat3_cofactor(F, cofF); - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dJ = 0; - for (int b = 0; b < 3; b++) { - mjtNum dF_cb = 0; - for (int k = 0; k < 3; k++) { - dF_cb += grad[n][k] * Fref_inv[k*3 + b]; - } - dJ += cofF[c*3 + b] * dF_cb; - } - dSdx[3*n + c] = dJ; - } - } - } -} - - -// compute dStrain/dNodePosition for general strain invariants -// dSdx: output array of size 3*nodenum -static void invariant_dSdx(int nodenum, mjtNum grad[][3], const mjtNum* F, - const mjtNum* Fref_inv, const mjtNum* dSdE, mjtNum* dSdx) { - mju_zero(dSdx, 3*nodenum); - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dS = 0; - for (int ij = 0; ij < 9; ij++) { - int ii = ij / 3; - int jj = ij % 3; - - mjtNum dF_ci = 0; - for (int k = 0; k < 3; k++) { - dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; - } - mjtNum dF_cj = 0; - for (int k = 0; k < 3; k++) { - dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; - } - - mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; - dS += dSdE[ij] * 0.5 * dC_ij; - } - dSdx[3*n + c] = dS; - } - } -} - - // allocate efc arrays on arena, return 1 on success, 0 on failure static int arenaAllocEfc(const mjModel* m, mjData* d) { #undef MJ_M @@ -923,6 +755,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); } + // compute corotational quaternion from cell-local positions + mjtNum cell_quat[4] = {1, 0, 0, 0}; + { + 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); + } + // build per-cell sparse chain and node Jacobians int* cell_chain = mjSTACKALLOC(d, nv, int); int cell_nnz = 0; @@ -940,140 +782,59 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_zero(dense_jac, nv); } - // Gauss-Legendre quadrature points in [0,1]^3 - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; + // 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]; - mjtNum gp1d[3]; - if (nquad == 2) { - gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); - gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0); - } else { - gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6); - gp1d[1] = 0.5; - gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); + // 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 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]; } - mjtNum (*gauss)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*ngauss, mjtNum); - for (int gi = 0; gi < nquad; gi++) { - for (int gj = 0; gj < nquad; gj++) { - for (int gk = 0; gk < nquad; gk++) { - int idx = gi*nquad*nquad + gj*nquad + gk; - gauss[idx][0] = gp1d[gi]; - gauss[idx][1] = gp1d[gj]; - gauss[idx][2] = gp1d[gk]; - } + // compute inverse quaternion for rotating eigenvectors to world frame + mjtNum cell_quat_inv[4]; + mju_negQuat(cell_quat_inv, cell_quat); + + // loop over eigenmodes + for (int eig = 0; eig < neig; eig++) { + const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell; + + // 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]; } - } + cpos[0] = residual; - // B-bar: center-point volumetric constraints (trilinear) - if (order == 1) { - mjtNum center[3] = {0.5, 0.5, 0.5}; - mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; - - mju_defGradient(Fcur_c, center, xpos_c, order); - mju_defGradient(Fref_c, center, refpos_c, order); - mat3_inverse(Fref_c, Fref_inv_c); - mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); - - mjtNum C_c[9], E_c[9]; - mju_mulMatTMat3(C_c, F_c, F_c); - mju_scl(E_c, C_c, 0.5, 9); - E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; - - mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; - mjtNum J_c = mat3_det(F_c); - - mjtNum grad_c[8][3]; - shape_gradients(order, center, grad_c); - - for (int inv = 0; inv < 2; inv++) { - cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; - volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); - cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - if (issparse) { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - cell_nnz, cell_chain); - } else { - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_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; - } - } + // 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); } - } - // Gauss integration - for (int g = 0; g < ngauss; g++) { - mjtNum* p = gauss[g]; + // contract with cell_node_jac to get sparse Jacobian + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; - mju_defGradient(Fcur, p, xpos_c, order); - mju_defGradient(Fref, p, refpos_c, order); - mat3_inverse(Fref, Fref_inv); - mju_mulMatMat3(F, Fcur, Fref_inv); - - mjtNum C[9], E[9]; - mju_mulMatTMat3(C, F, F); - for (int j = 0; j < 9; j++) { - E[j] = 0.5 * C[j]; - } - E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - - mjtNum I1 = E[0] + E[4] + E[8]; - mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] - + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] - + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; - mjtNum I2 = 0.5 * (I1*I1 - trE2); - mjtNum I3 = mat3_det(E); - - mjtNum (*grad)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*npc, mjtNum); - shape_gradients(order, p, grad); - - for (int s = 0; s < 6; s++) { - if (order == 1 && (s == 0 || s == 1 || s == 2)) { - continue; + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = strain_jac[k]; } - - mjtNum dSdE[9]; - mju_zero(dSdE, 9); - - if (s == 0) { - cpos[0] = I1; - dSdE[0] = dSdE[4] = dSdE[8] = 1.0; - } else if (s == 1) { - cpos[0] = I2; - dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; - dSdE[8] = I1-E[8]; - dSdE[1] = -E[1]; dSdE[3] = -E[3]; - dSdE[2] = -E[2]; dSdE[6] = -E[6]; - dSdE[5] = -E[5]; dSdE[7] = -E[7]; - } else if (s == 2) { - cpos[0] = I3; - mat3_cofactor(E, dSdE); - } else { - int offdiag_idx[3] = {1, 2, 5}; - int ij = offdiag_idx[s - 3]; - cpos[0] = E[ij]; - dSdE[ij] = 1.0; - } - - invariant_dSdx(npc, grad, F, Fref_inv, dSdE, dSdx_local); - cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - if (issparse) { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - cell_nnz, cell_chain); - } else { - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_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; - } + 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; } } } @@ -2404,6 +2165,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { chain2 = mjSTACKALLOC(d, nv, int); } + // pre-allocate buffer for cell body IDs (max npc = 125 for order=2) + int* cell_bodies = nnz ? mjSTACKALLOC(d, 125, int) : NULL; + // find active equality constraints for (int i=0; i < neq; i++) { // skip inactive @@ -2536,24 +2300,25 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; } int npc = (order+1)*(order+1)*(order+1); - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; - size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // per cell + + // 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 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 if (nnz) { - // get cell index from eq_data - 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 cy = m->flex_cellnum[3*f+1]; - int cz = m->flex_cellnum[3*f+2]; - // get the npc node body IDs for this cell int gindices[125]; 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]; - int* cell_bodies = mjSTACKALLOC(d, npc, int); for (int n = 0; n < npc; n++) { cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 8a7eabc7..f5db7108 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -906,6 +906,11 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, continue; } + // skip if strain constraints present (stiffness handled by constraint solver) + if (m->flex_edgeequality[f] == 3) { + continue; + } + // compute scale mjtNum damping = m->flex_damping[f]; mjtNum scale = s1 + s2 * damping; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index e6d3b6fa..bf59900b 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -229,6 +229,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) { continue; } + // skip interpolated flex with strain constraints (stiffness in constraint solver) + if (m->flex_edgeequality[f] == 3) { + continue; + } + if (m->flex_interp[f]) { int order = m->flex_interp[f]; int npc = (order+1)*(order+1)*(order+1); // nodes per cell @@ -270,14 +275,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) { for (int ck = 0; ck < cz; ck++) { // gather cell-local node data mjtNum quat[4]; - mjtNum p[3] = {.5, .5, .5}; mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, xpos_c, vel_c, xpos0_c, NULL, quat); // rotate to corotational frame for (int n = 0; n < npc; n++) { mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat); - mji_addTo3(xpos_c+3*n, p); mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat); } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index a86a2f33..d4c567d3 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -691,6 +691,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { // create one strain constraint per cell, storing cell index in eq_data + 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]; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 244a48f2..3e448fb5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3818,6 +3818,48 @@ void inline ComputeLinearStiffness(std::vector& K, } } + +// Eigendecompose cell stiffness matrix and store scaled eigenvectors. +// K_cell is n×n stored (negative convention: K_stored = -K_physical). +// Output layout in `out`: +// [0]: neig (as double) +// [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major +// Returns number of retained eigenmodes. +static int EigendecomposeStiffness(const double* K_cell_data, + double* out, int ndof) { + // copy K_cell for in-place decomposition + std::vector mat(K_cell_data, K_cell_data + ndof * ndof); + std::vector eigval(ndof); + std::vector eigvec(ndof * ndof); + + mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof); + + // K_stored = -K_physical, so physical eigenvalue = -eigval[i] + // retain modes where physical eigenvalue > threshold + double max_eigval = 0; + for (int i = 0; i < ndof; i++) { + max_eigval = std::max(max_eigval, std::abs(eigval[i])); + } + double threshold = max_eigval * 1e-8; + + int neig = 0; + for (int i = 0; i < ndof; i++) { + double lambda_phys = -eigval[i]; // negate to get physical eigenvalue + if (lambda_phys > threshold) { + // store sqrt(λ) * eigenvector (column i of eigvec matrix) + double scale = std::sqrt(lambda_phys); + for (int j = 0; j < ndof; j++) { + out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i]; + } + neig++; + } + } + + out[0] = static_cast(neig); + return neig; +} + + //------------------ class mjCFlex implementation -------------------------------------------------- // constructor @@ -4344,7 +4386,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { stiffness_cached = LoadCachedStiffness(); } - if (!stiffness_cached && young > 0 && interpolated) { + if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) { + // use young=1 for strain constraints (eigenvectors are geometry-only) + 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]; @@ -4379,11 +4425,17 @@ void mjCFlex::Compile(const mjVFS* vfs) { // compute per-cell stiffness std::vector K_cell(ndof_cell * ndof_cell, 0); - ComputeLinearStiffness(K_cell, cell_pos.data(), young, poisson, spec.order); + ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order); + double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell; - // copy into global stiffness array - mjuu_copyvec(stiffness.data() + cell_idx * ndof_cell * ndof_cell, - K_cell.data(), 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); + } } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 85245020..19672803 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3657,6 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) { int b1 = pfl->vertbodyid[pfl->edge[k].first]; int b2 = pfl->vertbodyid[pfl->edge[k].second]; m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid); + } else { + m->flexedge_rigid[edge_adr+k] = 0; } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 8ca60124..1f80949d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -983,6 +983,7 @@ class mjCFlex_ : public mjCBase { std::vector edgeidx_; // element edge ids std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix + bool has_strain_eq = false; // true if strain constraints reference this flex // variable-size data std::vector vertbody_; // vertex body names diff --git a/src/user/user_util.cc b/src/user/user_util.cc index d9ad6cb6..b7bbc00e 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -754,6 +754,81 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m return iter; } + +// Jacobi eigenvalue decomposition of symmetric n×n matrix. +// On output, eigenvalues are in eigval and eigenvectors are columns of eigvec. +// Both arrays must be pre-allocated: eigval[n], eigvec[n*n]. +// The input matrix mat is destroyed. +int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) { + // initialize eigvec to identity + std::fill(eigvec, eigvec + n*n, 0.0); + for (int i = 0; i < n; i++) { + eigvec[i*n + i] = 1.0; + } + + const int max_sweeps = 200; + const double tol = 1e-12; + + int sweep; + for (sweep = 0; sweep < max_sweeps; sweep++) { + // check convergence: sum of squared off-diagonal elements + double off_diag = 0; + for (int i = 0; i < n; i++) { + for (int j = i+1; j < n; j++) { + off_diag += mat[i*n + j] * mat[i*n + j]; + } + } + if (off_diag < tol * tol) break; + + // sweep over all off-diagonal pairs + for (int p = 0; p < n; p++) { + for (int q = p+1; q < n; q++) { + double apq = mat[p*n + q]; + if (std::abs(apq) < tol * 1e-3) continue; + + // compute rotation angle + double app = mat[p*n + p]; + double aqq = mat[q*n + q]; + double tau = (aqq - app) / (2.0 * apq); + double t = (tau >= 0 ? 1.0 : -1.0) / + (std::abs(tau) + std::sqrt(1.0 + tau*tau)); + double c = 1.0 / std::sqrt(1.0 + t*t); + double s = t * c; + + // update matrix (Jacobi rotation) + mat[p*n + p] -= t * apq; + mat[q*n + q] += t * apq; + mat[p*n + q] = 0; + mat[q*n + p] = 0; + + for (int r = 0; r < n; r++) { + if (r == p || r == q) continue; + double mrp = mat[r*n + p]; + double mrq = mat[r*n + q]; + mat[r*n + p] = mat[p*n + r] = c*mrp - s*mrq; + mat[r*n + q] = mat[q*n + r] = s*mrp + c*mrq; + } + + // accumulate eigenvectors + for (int r = 0; r < n; r++) { + double vrp = eigvec[r*n + p]; + double vrq = eigvec[r*n + q]; + eigvec[r*n + p] = c*vrp - s*vrq; + eigvec[r*n + q] = s*vrp + c*vrq; + } + } + } + } + + // extract eigenvalues from diagonal + for (int i = 0; i < n; i++) { + eigval[i] = mat[i*n + i]; + } + + return sweep; +} + + // transform vector by pose void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]) { @@ -1189,10 +1264,10 @@ template std::string VectorToString(const std::vector& v) { return s; } -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); namespace { @@ -1258,7 +1333,7 @@ template std::vector StringToVector(char* cs) { return v; } -template<> std::vector StringToVector(const std::string& s) { +template<> MJAPI std::vector StringToVector(const std::string& s) { std::vector v; std::stringstream ss(s); std::string word; @@ -1268,17 +1343,17 @@ template<> std::vector StringToVector(const std::string& s) { return v; } -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template std::vector StringToVector(const std::string& s) { return StringToVector(const_cast(s.c_str())); } -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/src/user/user_util.h b/src/user/user_util.h index 2eeb3a0f..6d17fb43 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -26,6 +26,8 @@ #include #include +#include + const double mjEPS = 1E-14; // minimum value in various calculations const double mjMINMASS = 1E-6; // minimum mass allowed @@ -157,6 +159,12 @@ double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], // eigenvalue decomposition of symmetric 3x3 matrix int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]); +// Jacobi eigenvalue decomposition of symmetric n×n matrix +// eigval[n]: output eigenvalues, eigvec[n*n]: output eigenvectors (columns) +// mat[n*n]: input matrix (destroyed on output) +// returns number of sweeps used +MJAPI int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n); + // transform vector by pose void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]); @@ -166,7 +174,7 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful namespace mujoco::user { // utility class for handling file paths -class FilePath { +class MJAPI FilePath { public: FilePath() = default; explicit FilePath(const std::string& str) : path_(PathReduce(str)) {} @@ -251,11 +259,11 @@ struct Cleanup { std::vector FileToMemory(const char* filename); // convert vector to string separating elements by whitespace -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector -template std::vector StringToVector(char *cs); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(char *cs); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index d795a404..93e3b947 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -622,6 +622,149 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { mj_deleteModel(m); } +// Test flex strain constraint with quadratic interpolation +TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_resetData(m, d); + mj_forward(m, d); + + // Check constraints generated + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Check that initial strain is ~0 + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + if (mju_abs(d->efc_pos[i]) > max_pos) { + max_pos = mju_abs(d->efc_pos[i]); + } + } + EXPECT_LT(max_pos, 1e-6) << "Initial strain should be ~0"; + + // Check Jacobian for NaN + int nv = m->nv; + bool has_bad_jacobian = false; + for (int i = 0; i < d->ne; i++) { + for (int j = 0; j < nv; j++) { + if (mju_isBad(d->efc_J[i*nv + j])) { + has_bad_jacobian = true; + } + } + } + EXPECT_FALSE(has_bad_jacobian) << "Jacobian has NaN"; + + // Run simulation for a few steps + for (int i = 0; i < 100; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation unstable at step " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test quadratic passive forces (no constraints) for stability +TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + // Run for 500 steps — should stay stable + for (int i = 0; i < 500; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Passive quadratic unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test quadratic with anisotropic cells (like what mesh bounding box creates) +TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_forward(m, d); + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Run for 200 steps with gravity + contact + for (int i = 0; i < 200; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Anisotropic quadratic unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i + << ", qvel[" << j << "]=" << d->qvel[j]; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { constexpr char xml[] = R"( diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 912a26c0..c2d3b69d 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -40,3 +40,5 @@ mujoco_test(user_composite_test) mujoco_test(user_resource_test) mujoco_test(user_vfs_test) + +mujoco_test(user_util_test) diff --git a/test/user/user_util_test.cc b/test/user/user_util_test.cc index 9873c74b..67c91e2b 100644 --- a/test/user/user_util_test.cc +++ b/test/user/user_util_test.cc @@ -17,6 +17,8 @@ #include "src/user/user_util.h" #include +#include +#include #include #include @@ -180,5 +182,142 @@ TEST_F(UserUtilTest, VectorToStringEmpty) { EXPECT_EQ(VectorToString(v), ""); } +// utility: modified Gram-Schmidt to orthogonalize columns of Q (n x n) +static void gramSchmidt(double* Q, int n) { + for (int j = 0; j < n; j++) { + // subtract projections onto previous columns + for (int k = 0; k < j; k++) { + double dot = 0; + for (int i = 0; i < n; i++) { + dot += Q[i * n + j] * Q[i * n + k]; + } + for (int i = 0; i < n; i++) { + Q[i * n + j] -= dot * Q[i * n + k]; + } + } + // normalize + double norm = 0; + for (int i = 0; i < n; i++) { + norm += Q[i * n + j] * Q[i * n + j]; + } + norm = std::sqrt(norm); + for (int i = 0; i < n; i++) { + Q[i * n + j] /= norm; + } + } +} + +// utility: compose SPD matrix A = Q * diag(eigvals) * Q^T +static void composeMatrix(double* A, const double* Q, + const double* eigvals, int n) { + for (int i = 0; i < n; i++) { + for (int j = 0; j <= i; j++) { + double sum = 0; + for (int k = 0; k < n; k++) { + sum += Q[i * n + k] * eigvals[k] * Q[j * n + k]; + } + A[i * n + j] = sum; + A[j * n + i] = sum; + } + } +} + +TEST_F(UserUtilTest, EigendecomposeConvergence) { + // seeded RNG for reproducibility + std::mt19937_64 rng; + rng.seed(42); + std::normal_distribution dist(0, 1); + + // sweep over matrix sizes used by flex stiffness + // order=1: 8 nodes * 3 dof = 24 + // order=2: 27 nodes * 3 dof = 81 + for (int n : {24, 81}) { + int total_sweeps = 0; + int max_sweeps = 0; + int count = 0; + + // generate random orthogonal matrix Q via Gram-Schmidt + std::vector Q(n * n); + for (int i = 0; i < n * n; i++) { + Q[i] = dist(rng); + } + gramSchmidt(Q.data(), n); + + // sweep eigenvalue spectra of varying difficulty + // well-separated, clustered, wide condition number + for (double condition : {1e1, 1e3, 1e6}) { + for (double cluster : {0.0, 0.5, 0.9}) { + // construct eigenvalues + std::vector eigvals(n); + for (int i = 0; i < n; i++) { + // base: logarithmically spaced from 1 to condition + double t = (double)i / (n - 1); + double base = std::exp(t * std::log(condition)); + + // cluster: push eigenvalues toward geometric mean + double mean = std::sqrt(condition); + eigvals[i] = (1 - cluster) * base + cluster * mean; + } + + // compose A = Q * diag(eigvals) * Q^T + std::vector A(n * n); + composeMatrix(A.data(), Q.data(), eigvals.data(), n); + + // save copy for verification + std::vector A_copy(A); + + // decompose + std::vector found_eigval(n); + std::vector found_eigvec(n * n); + int sweeps = mjuu_eigendecompose( + A.data(), found_eigval.data(), + found_eigvec.data(), n); + + total_sweeps += sweeps; + if (sweeps > max_sweeps) max_sweeps = sweeps; + count++; + + // verify convergence + EXPECT_LT(sweeps, 200) + << "n=" << n + << " condition=" << condition + << " cluster=" << cluster; + + // verify A*v = lambda*v for each eigenpair + for (int i = 0; i < n; i++) { + for (int r = 0; r < n; r++) { + double Av = 0; + for (int c = 0; c < n; c++) { + Av += A_copy[r * n + c] * found_eigvec[c * n + i]; + } + double lv = found_eigval[i] * found_eigvec[r * n + i]; + EXPECT_NEAR(Av, lv, + 1e-6 * std::abs(found_eigval[i])) + << "n=" << n << " condition=" << condition + << " cluster=" << cluster + << " eigpair=" << i << " row=" << r; + } + } + + // verify all eigenvalues are positive + for (int i = 0; i < n; i++) { + EXPECT_GT(found_eigval[i], 0) + << "n=" << n << " eigenvalue " << i; + } + } + } + + double mean_sweeps = (double)total_sweeps / count; + + // assert reasonable average convergence + EXPECT_LE(mean_sweeps, 20.0) + << "n=" << n << ": mean sweeps too high"; + + // assert max sweeps within budget + EXPECT_LT(max_sweeps, 200) + << "n=" << n << ": max sweeps exceeded 200"; + } +} + } // namespace } // namespace mujoco From e6d77650f727f026da5922b3ca1857b4b9ad5b40 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 03:09:31 -0700 Subject: [PATCH 08/29] Refactor `mju_combineSparse` to eliminate temporary buffers. Combine sparse vectors in-place by first counting total `nnz` and then working backwards from the end. This removes the need for temporary buffers in `mju_combineSparse` and its callers and speeds up the function by ~10%. PiperOrigin-RevId: 902530210 Change-Id: I4f48c327103552ab968d3915399c6067367bec9f --- src/engine/engine_core_constraint.c | 9 +-- src/engine/engine_solver.c | 19 ++---- src/engine/engine_support.c | 12 +--- src/engine/engine_util_solve.c | 9 +-- src/engine/engine_util_sparse.c | 11 ++- src/engine/engine_util_sparse.h | 67 ++++++++++--------- test/benchmark/chol_benchmark_test.cc | 8 +-- .../engine_util_sparse_benchmark_test.cc | 8 +-- 8 files changed, 57 insertions(+), 86 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 5eab4da6..7849836e 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -496,12 +496,12 @@ static void mj_equalityAnchors(const mjModel* m, const mjData* d, int eq_id, // equality constraints void mj_instantiateEquality(const mjModel* m, mjData* d) { int issparse = mj_isSparse(m), nv = m->nv; - int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL; + int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL; int flex_edgeadr, flex_edgenum; int flex_vertadr, flex_vertnum; mjtNum cpos[6], pos[2][3], ref[2], dif, deriv; mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3]; - mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL; + mjtNum *jac[2], *jacdif, *data; // disabled or no equality constraints: return if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) { @@ -520,8 +520,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { if (issparse) { chain = mjSTACKALLOC(d, nv, int); chain2 = mjSTACKALLOC(d, nv, int); - buf_ind = mjSTACKALLOC(d, nv, int); - sparse_buf = mjSTACKALLOC(d, nv, mjtNum); } // find active equality constraints @@ -689,8 +687,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // compute Jacobian: sparse or dense if (issparse) { - NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, - chain2, sparse_buf, buf_ind); + NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, chain2); } else { mju_addToScl(jac[0], jac[1], -deriv, nv); } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index cc9e01ce..1644f841 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -829,8 +829,6 @@ typedef struct { int* L_rowadr; // Hessian factor row addresses (nv x 1) int* LT_rownnz; // Hessian factor transpose row nonzeros (nv x 1) int* LT_rowadr; // Hessian factor transpose row addresses (nv x 1) - int* buf_ind; // index buffer for sparse addition (nv x 1) - mjtNum* buf_val; // value buffer for sparse addition (nv x 1) // Newton arrays, computed-size (MakeHessian) int nH; // number of nonzeros in Hessian H @@ -979,9 +977,7 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { if (flg_Newton) { nNum += nefc + nv; // D, cholupd if (is_elliptic) nNum += 6*nv; // LTJ - if (is_sparse) { - nNum += nv; // buf_val - } else { + if (!is_sparse) { nNum += nv*nv; // L (dense) if (is_elliptic) nNum += nv*nv; // Lcone (dense) } @@ -993,7 +989,7 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { size_t nInt = nefc; // oldstate if (is_sparse) { nInt += 3*nv + nJ; // JT sparse - if (flg_Newton) nInt += 9*nv; // Newton sparse + if (flg_Newton) nInt += 8*nv; // Newton sparse } // allocate mjtNum and int blocks @@ -1018,9 +1014,7 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { if (is_elliptic) { ctx->LTJ = numblock; numblock += 6*nv; } - if (is_sparse) { - ctx->buf_val = numblock; numblock += nv; - } else { + if (!is_sparse) { ctx->nL = nv*nv; ctx->L = numblock; numblock += ctx->nL; ctx->Lcone = is_elliptic ? numblock : NULL; @@ -1049,7 +1043,6 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { ctx->L_rowadr = intblock; intblock += nv; ctx->LT_rownnz = intblock; intblock += nv; ctx->LT_rowadr = intblock; intblock += nv; - ctx->buf_ind = intblock; intblock += nv; } // sparse: compute Jacobian transpose @@ -1616,8 +1609,7 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // add mass matrix: H = J'*D*J + M mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, - ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, - ctx->buf_val, ctx->buf_ind); + ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); // compute H' sparse structure (upper triangle, required for symbolic Cholesky) mju_transposeSparse(NULL, NULL, nv, nv, ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, NULL, @@ -1691,8 +1683,7 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) // add mass matrix: H = J'*D*J + C mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, - ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, - ctx->buf_val, ctx->buf_ind); + ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); } // numeric sparse factorization: L = chol(H) using pre-computed sparsity pattern diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 27537d39..a8725bf7 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -420,17 +420,11 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind) { int nv = m->nv; + // sparse if (rownnz && rowadr && colind) { - mj_markStack(d); - mjtNum* buf_val = mjSTACKALLOC(d, nv, mjtNum); - int* buf_ind = mjSTACKALLOC(d, nv, int); - - mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, - d->M, m->M_rownnz, m->M_rowadr, m->M_colind, - buf_val, buf_ind); - - mj_freeStack(d); + mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, d->M, + m->M_rownnz, m->M_rowadr, m->M_colind); } // dense diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 259f7c59..1f8b839b 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -144,10 +144,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, int* rownnz, const int* rowadr, int* colind, mjData* d) { int rank = n; - - mj_markStack(d); - mjtNum* buf = mjSTACKALLOC(d, n, mjtNum); - int* buf_ind = mjSTACKALLOC(d, n, int); + (void) d; // backpass over rows for (int r=n-1; r >= 0; r--) { @@ -175,15 +172,13 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, // mat(c,0:c) = mat(c,0:c) - mat(r,c) * mat(r,0:c) int nnz_c = mju_combineSparse(mat + rowadr[c], mat+rowadr[r], 1, -mat[adr+i], - rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r], - buf, buf_ind); + rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r]); // assign new nnz to row c rownnz[c] = nnz_c; } } - mj_freeStack(d); return rank; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 05960567..cfed023b 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -198,15 +198,14 @@ void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int } -// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers +// add sparse matrix M to sparse destination matrix void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr, const mjtNum* M, const int* M_rownnz, const int* M_rowadr, - const int* M_colind, - mjtNum* buf_val, int* buf_ind) { + const int* M_colind) { for (int i=0; i < nr; i++) { rownnz[i] = mju_combineSparse(dst + rowadr[i], M + M_rowadr[i], 1, 1, rownnz[i], M_rownnz[i], colind + rowadr[i], - M_colind + M_rowadr[i], buf_val, buf_ind); + M_colind + M_rowadr[i]); } } @@ -256,8 +255,8 @@ void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, for (int k=diag-1; k >= 0; k--) { int j = ind[k]; mjtNum val = row[k]; - res[i] += val * vec[j]; // strict lower - res[j] += val * vec[i]; // strict upper + res[i] += val * vec[j]; // strict lower + res[j] += val * vec[i]; // strict upper } } } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 1350ccff..68bda070 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -62,11 +62,10 @@ MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec MJAPI void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); -// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers +// add sparse matrix M to sparse destination matrix MJAPI void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr, const mjtNum* M, const int* M_rownnz, const int* M_rowadr, - const int* M_colind, - mjtNum* buf_val, int* buf_ind); + const int* M_colind); // add symmetric matrix (only lower triangle represented) to dense matrix MJAPI void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n, @@ -294,8 +293,7 @@ void mju_addToSclScl(mjtNum* res, const mjtNum* vec, mjtNum scl1, mjtNum scl2, i // combine two sparse vectors: dst = a*dst + b*src, return nnz of result static inline int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind, - mjtNum* buf, int* buf_ind) { + int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) { // check for identical pattern if (dst_nnz == src_nnz) { if (mju_compare(dst_ind, src_ind, dst_nnz)) { @@ -305,49 +303,54 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, } } - // copy dst into buf - if (dst_nnz) { - memcpy(buf, dst, dst_nnz * sizeof(mjtNum)); - memcpy(buf_ind, dst_ind, dst_nnz * sizeof(int)); - } + // compute total nnz of result + int nnz = mju_combineSparseCount(dst_nnz, src_nnz, dst_ind, src_ind); - // prepare to merge buf and src into dst - int bi = 0, si = 0, nnz = 0; - int buf_nnz = dst_nnz; + // set up read/write pointers at end of arrays + int bi = dst_nnz - 1, si = src_nnz - 1, w = nnz - 1; - // merge vectors - while (bi < buf_nnz && si < src_nnz) { - int badr = buf_ind[bi]; + // merge backwards + while (bi >= 0 && si >= 0) { + int badr = dst_ind[bi]; int sadr = src_ind[si]; if (badr == sadr) { - dst[nnz] = a*buf[bi++] + b*src[si++]; - dst_ind[nnz++] = badr; + dst[w] = a*dst[bi] + b*src[si]; + dst_ind[w] = badr; + bi--; + si--; } - // buf only - else if (badr < sadr) { - dst[nnz] = a*buf[bi++]; - dst_ind[nnz++] = badr; + // dst only + else if (badr > sadr) { + dst[w] = a*dst[bi]; + dst_ind[w] = badr; + bi--; } // src only else { - dst[nnz] = b*src[si++]; - dst_ind[nnz++] = sadr; + dst[w] = b*src[si]; + dst_ind[w] = sadr; + si--; } + w--; } - // the rest of src only - while (si < src_nnz) { - dst[nnz] = b*src[si]; - dst_ind[nnz++] = src_ind[si++]; + // remaining src elements + while (si >= 0) { + dst[w] = b*src[si]; + dst_ind[w] = src_ind[si]; + si--; + w--; } - // the rest of buf only - while (bi < buf_nnz) { - dst[nnz] = a*buf[bi]; - dst_ind[nnz++] = buf_ind[bi++]; + // remaining dst elements: already in place, scale by a + if (a != 1) { + while (bi >= 0) { + dst[bi] *= a; + bi--; + } } return nnz; diff --git a/test/benchmark/chol_benchmark_test.cc b/test/benchmark/chol_benchmark_test.cc index 004e0c2a..ba8d94bf 100644 --- a/test/benchmark/chol_benchmark_test.cc +++ b/test/benchmark/chol_benchmark_test.cc @@ -22,7 +22,6 @@ #include #include #include -#include "src/engine/engine_memory.h" #include "src/engine/engine_support.h" #include "src/engine/engine_util_solve.h" #include "src/engine/engine_util_sparse.h" @@ -352,10 +351,6 @@ constexpr int kNumUpdateVectors = 25; int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old( mjtNum* mat, mjtNum* x, int n, int flg_plus, const int* rownnz, const int* rowadr, const int* colind, int x_nnz, int* x_ind, mjData* d) { - mj_markStack(d); - int* buf_ind = mjSTACKALLOC(d, n, int); - mjtNum* sparse_buf = mjSTACKALLOC(d, n, mjtNum); - int rank = n, i = x_nnz - 1; while (i >= 0) { int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]]; @@ -372,10 +367,9 @@ int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old( mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c), nnz - 1, i, colind + adr, x_ind); int new_x_nnz = mju_combineSparse(x, mat + adr, c, -s, i, nnz - 1, x_ind, - colind + adr, sparse_buf, buf_ind); + colind + adr); i = i - 1 + (new_x_nnz - i); } - mj_freeStack(d); return rank; } diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index a82be98e..3a28a291 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -99,8 +99,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_baseline(mjtNum* dst, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, - const int* src_ind, - mjtNum* buf, int* buf_ind) { + const int* src_ind) { // check for identical pattern if (compare_baseline(dst_ind, src_ind, dst_nnz)) { // combine mjtNum data directly @@ -116,8 +115,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_new(mjtNum* dst, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, - const int* src_ind, - mjtNum* buf, int* buf_ind) { + const int* src_ind) { // check for identical pattern if (compare_memcmp(dst_ind, src_ind, dst_nnz)) { // combine mjtNum data directly @@ -372,7 +370,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { // in order to trigger all if's in combineSparse func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i], rownnz[c], rownnz[c], - colind+rowadr[c], colind+rowadr[c], NULL, NULL); + colind+rowadr[c], colind+rowadr[c]); } } } From eb31027a7ca2f5587abc14d09b032ccc68d4dea7 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 03:27:10 -0700 Subject: [PATCH 09/29] Simplify RenderTarget creation. Remove the RenderTargetTextureType enum and, instead, create RenderTargets directly from pixel formats. PiperOrigin-RevId: 902536708 Change-Id: I2683aa9db23c011212087d93df0260ad27621170 --- .../filament/filament/filament_context.cc | 15 +++-- .../filament/filament/render_target.cc | 45 +++++++++++---- .../filament/filament/render_target.h | 15 +++-- .../filament/filament/scene_view.cc | 9 ++- src/experimental/filament/filament/texture.cc | 55 ++++++------------- src/experimental/filament/filament/texture.h | 32 ++++------- 6 files changed, 87 insertions(+), 84 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 4da0d63b..09f26cfa 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -191,14 +191,17 @@ void FilamentContext::SetFrameBuffer(int framebuffer) { } void FilamentContext::PrepareRenderTargets(int width, int height) { - color_target_ = std::make_unique( - engine_, RenderTargetTextureType::kColor, - RenderTargetTextureType::kDepth); + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + + config.color_format = mjPIXEL_FORMAT_RGB8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + color_target_ = std::make_unique(engine_, config); color_target_->Prepare(width, height); - depth_target_ = std::make_unique( - engine_, RenderTargetTextureType::kDepthColor, - RenderTargetTextureType::kDepth); + config.color_format = mjPIXEL_FORMAT_R32F; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + depth_target_ = std::make_unique(engine_, config); depth_target_->Prepare(width, height); } diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index eb2d3490..b01fb94c 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -30,10 +30,14 @@ namespace mujoco { +void DefaultRenderTargetConfig(RenderTargetConfig* config) { + config->color_format = mjPIXEL_FORMAT_RGBA8; + config->depth_format = mjPIXEL_FORMAT_DEPTH32F; +} + RenderTarget::RenderTarget(filament::Engine* engine, - RenderTargetTextureType color, - RenderTargetTextureType depth) - : engine_(engine), color_type_(color), depth_type_(depth) {} + const RenderTargetConfig& config) + : engine_(engine), config_(config) {} RenderTarget::~RenderTarget() noexcept { Destroy(); @@ -47,10 +51,29 @@ void RenderTarget::Prepare(int width, int height) { width_ = width; height_ = height; - color_texture_ = - std::make_unique(engine_, color_type_, width, height); - depth_texture_ = - std::make_unique(engine_, depth_type_, width, height); + TextureConfig color_config; + DefaultTextureConfig(&color_config); + Texture::InternalFlags color_flags; + color_config.width = width; + color_config.height = height; + color_config.target = mjTEXTURE_2D; + color_config.format = config_.color_format; + color_config.color_space = mjCOLORSPACE_LINEAR; + color_config.format = mjPIXEL_FORMAT_RGB8; + color_flags.color_attachment = true; + color_texture_ = std::make_unique(engine_, color_config, color_flags); + + TextureConfig depth_config; + DefaultTextureConfig(&depth_config); + Texture::InternalFlags depth_flags; + depth_config.width = width; + depth_config.height = height; + depth_config.target = mjTEXTURE_2D; + depth_config.format = config_.depth_format; + depth_config.color_space = mjCOLORSPACE_LINEAR; + depth_config.format = mjPIXEL_FORMAT_DEPTH32F; + depth_flags.depth_attachment = true; + depth_texture_ = std::make_unique(engine_, depth_config, depth_flags); filament::RenderTarget::Builder builder; builder.texture(filament::RenderTarget::AttachmentPoint::COLOR, @@ -65,19 +88,19 @@ void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, filament::backend::PixelDataFormat format; filament::backend::PixelDataType type; size_t expected_num_bytes = 0; - switch (color_type_) { - case RenderTargetTextureType::kColor: + switch (config_.color_format) { + case mjPIXEL_FORMAT_RGB8: format = filament::backend::PixelDataFormat::RGB; type = filament::backend::PixelDataType::UBYTE; expected_num_bytes = width_ * height_ * 3; break; - case RenderTargetTextureType::kDepthColor: + case mjPIXEL_FORMAT_R32F: format = filament::backend::PixelDataFormat::R; type = filament::backend::PixelDataType::FLOAT; expected_num_bytes = width_ * height_ * sizeof(float); break; default: - mju_error("Unsupported pixel format: %d", color_type_); + mju_error("Unsupported pixel format: %d", config_.color_format); return; } if (num_bytes != expected_num_bytes) { diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index ee033de9..22a02143 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -25,13 +25,21 @@ namespace mujoco { +// Defines the basic properties of a render target. +struct RenderTargetConfig { + mjtPixelFormat color_format; + mjtPixelFormat depth_format; +}; + +// Initializes the RenderTargetConfig to default values. +void DefaultRenderTargetConfig(RenderTargetConfig* config); + // Manages a filament RenderTarget and the textures which are bound to it. class RenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(filament::Engine* engine, RenderTargetTextureType color, - RenderTargetTextureType depth); + RenderTarget(filament::Engine* engine, const RenderTargetConfig& config); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -58,11 +66,10 @@ class RenderTarget { void Destroy(); filament::Engine* engine_ = nullptr; + RenderTargetConfig config_; filament::RenderTarget* render_target_ = nullptr; std::unique_ptr color_texture_ = nullptr; std::unique_ptr depth_texture_ = nullptr; - RenderTargetTextureType color_type_; - RenderTargetTextureType depth_type_; int width_ = 0; int height_ = 0; }; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index d6832f6f..18d950d7 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -325,9 +325,12 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { // Ensure we have the same number of render targets as we do reflective // renderables. while (reflect_targets_.size() < reflectives_.size()) { - reflect_targets_.push_back(std::make_unique( - engine_, RenderTargetTextureType::kReflectionColor, - RenderTargetTextureType::kDepth)); + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + + config.color_format = mjPIXEL_FORMAT_RGBA8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + reflect_targets_.push_back(std::make_unique(engine_, config)); } // Prepare a render target for the reflective renderable. diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index 1ce62774..1277ec44 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -99,6 +99,10 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( return filament::Texture::InternalFormat::RGB8; case mjPIXEL_FORMAT_RGBA8: return filament::Texture::InternalFormat::RGBA8; + case mjPIXEL_FORMAT_R32F: + return filament::Texture::InternalFormat::R32F; + case mjPIXEL_FORMAT_DEPTH32F: + return filament::Texture::InternalFormat::DEPTH32F; default: mju_error("Unsupported format: %d", (int)config.format); return filament::Texture::InternalFormat::UNUSED; @@ -114,7 +118,8 @@ void DefaultTextureConfig(TextureConfig* config) { std::memset(config, 0, sizeof(TextureConfig)); } -Texture::Texture(filament::Engine* engine, const TextureConfig& config) +Texture::Texture(filament::Engine* engine, const TextureConfig& config, + InternalFlags flags) : engine_(engine), config_(config) { if (IsCompressed(config_)) { // We defer creation of compressed textures until Upload() is called. In @@ -139,45 +144,19 @@ Texture::Texture(filament::Engine* engine, const TextureConfig& config) builder.sampler(filament::Texture::Sampler::SAMPLER_2D); } - if (config_.color_space != mjCOLORSPACE_SRGB) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); + filament::Texture::Usage usage = filament::Texture::Usage::DEFAULT; + if (flags.color_attachment) { + usage |= filament::Texture::Usage::COLOR_ATTACHMENT; + usage |= filament::Texture::Usage::BLIT_SRC; + } else if (flags.depth_attachment) { + usage |= filament::Texture::Usage::DEPTH_ATTACHMENT; + usage |= filament::Texture::Usage::BLIT_SRC; + } else if (config_.color_space != mjCOLORSPACE_SRGB) { + usage |= filament::Texture::Usage::GEN_MIPMAPPABLE; } - texture_ = builder.build(*engine_); -} + builder.usage(usage); -Texture::Texture(filament::Engine* engine, RenderTargetTextureType type, - int width, int height) : engine_(engine) { - filament::Texture::Builder builder; - builder.width(width); - builder.height(height); - switch (type) { - case RenderTargetTextureType::kColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::RGB8); - break; - case RenderTargetTextureType::kDepth: - builder.usage(filament::Texture::Usage::DEPTH_ATTACHMENT | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::DEPTH32F); - break; - case RenderTargetTextureType::kDepthColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::R32F); - break; - case RenderTargetTextureType::kReflectionColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::RGBA8); - break; - default: - mju_error("Unknown type: %d", static_cast(type)); - } - texture_ = builder.build(*engine); + texture_ = builder.build(*engine_); } Texture::~Texture() { diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index 44b0ea31..b1493470 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -25,29 +25,13 @@ // Functions for creating filament textures. namespace mujoco { -// The types of textures we can create. For internal use only. -enum class TextureTarget { - // A standard 2D image with a width and a height. - kNormal2d, - // A 2D texture split up into the 6 faces of a cube. - kCube, -}; - -// The different types of textures we can create for a render target. -// For internal use only. -enum class RenderTargetTextureType { - kColor, - kDepth, - kDepthColor, - kReflectionColor, -}; - // Pixel formats for textures. typedef enum mjtPixelFormat_ { mjPIXEL_FORMAT_UNKNOWN = 0, mjPIXEL_FORMAT_R8, mjPIXEL_FORMAT_RGB8, mjPIXEL_FORMAT_RGBA8, + mjPIXEL_FORMAT_R32F, mjPIXEL_FORMAT_DEPTH32F, mjPIXEL_FORMAT_KTX, } mjtPixelFormat; @@ -98,12 +82,16 @@ void DefaultTextureConfig(TextureConfig* config); // Wrapper around a filament::Texture. class Texture { public: - // Creates a texture with the given data. - Texture(filament::Engine* engine, const TextureConfig& config); + // Flags for internal use. + struct InternalFlags { + InternalFlags() : color_attachment(false), depth_attachment(false) {} + bool color_attachment; + bool depth_attachment; + }; - // Creates a texture for use with a render target, for internal use. - Texture(filament::Engine* engine, RenderTargetTextureType type, int width, - int height); + // Creates a texture with the given data. + Texture(filament::Engine* engine, const TextureConfig& config, + InternalFlags flags = InternalFlags()); ~Texture(); From fa7b36d1111f1504cf215cdb1b1c61cc47bd8a6b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 04:16:27 -0700 Subject: [PATCH 10/29] Fix undefined reference errors for StringToVector with GCC and LTO. This CL addresses link failures when building MuJoCo with GCC and Link Time Optimization (LTO) enabled: - Declared the `StringToVector(const std::string&)` specialization in `user_util.h` to prevent the compiler from incorrectly trying to instantiate the general template. - Added an explicit instantiation for `StringToVector(char*)` in `user_util.cc` to provide the definition needed by its `const std::string&` counterpart. PiperOrigin-RevId: 902557141 Change-Id: I27b585cf1deb276d4150a5cbe6cf7e59c323a20a --- src/user/user_util.cc | 1 + src/user/user_util.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/user/user_util.cc b/src/user/user_util.cc index b7bbc00e..b98516fc 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -1346,6 +1346,7 @@ template<> MJAPI std::vector StringToVector(const std::string& s) { template MJAPI std::vector StringToVector(char* cs); template MJAPI std::vector StringToVector(char* cs); template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template std::vector StringToVector(const std::string& s) { diff --git a/src/user/user_util.h b/src/user/user_util.h index 6d17fb43..b2842d85 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -264,6 +264,7 @@ template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector template MJAPI std::vector StringToVector(char *cs); template MJAPI std::vector StringToVector(const std::string& s); +template<> MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user From 508e581ba9232930dc116cf3d3781808d70fcfed Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 04:40:34 -0700 Subject: [PATCH 11/29] Optimize flex by pinning nodes in empty cells. This change introduces an optimization for flexcomp objects defined by a mesh. It identifies grid cells that do not contain any mesh vertices and marks them as empty. Nodes that are exclusively part of empty cells are pinned, preventing them from moving. Stiffness computations are skipped for empty cells, reducing computational cost. The total mass is now distributed only among the non-pinned nodes. PiperOrigin-RevId: 902565735 Change-Id: Id0a9a685536d5e18a3e42124a25ab08ff3a918f2 --- src/engine/engine_core_constraint.c | 5 +- src/engine/engine_core_smooth.c | 30 ++--- src/engine/engine_core_util.c | 42 +++--- src/engine/engine_core_util.h | 2 +- src/engine/engine_derivative.c | 98 +++++++++----- src/engine/engine_passive.c | 30 +++-- src/engine/engine_vis_visualize.c | 15 ++- src/user/user_flexcomp.cc | 115 +++++++++++++++- src/user/user_flexcomp.h | 5 + src/user/user_mesh.cc | 5 + src/user/user_objects.h | 1 + test/user/user_flex_test.cc | 197 ++++++++++++++++++++++++++++ 12 files changed, 455 insertions(+), 90 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7849836e..0f8f4a00 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -743,7 +743,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); for (int n = 0; n < npc; n++) { int gn = gindices[n]; - if (m->flex_centered[f]) { + 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]); } else { mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 8f3412f1..6b052147 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -564,16 +564,13 @@ void mj_flex(const mjModel* m, mjData* d) { // 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs if (m->flex_interp[f] == 0) { - // centered: copy body position - if (m->flex_centered[f]) { - for (int i=vstart; i < vend; i++) { + for (int i=vstart; i < vend; i++) { + if (m->flex_centered[f] || + (m->flex_vert[3*i+0] == 0 && + m->flex_vert[3*i+1] == 0 && + m->flex_vert[3*i+2] == 0)) { mji_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); - } - } - - // non-centered: map from local to global - else { - for (int i=vstart; i < vend; i++) { + } else { mji_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i); mji_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } @@ -585,13 +582,14 @@ void mj_flex(const mjModel* m, mjData* d) { int nodenum = nend - nstart; mj_markStack(d); mjtNum* nodexpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - if (m->flex_centered[f]) { - for (int i=nstart; i < nend; i++) { - mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); - } - } else { - for (int i=nstart; i < nend; i++) { - int j = i - nstart; + for (int i=nstart; i < nend; i++) { + int j = i - nstart; + if (m->flex_centered[f] || + (m->flex_node[3*i+0] == 0 && + m->flex_node[3*i+1] == 0 && + m->flex_node[3*i+2] == 0)) { + mji_copy3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); + } else { mji_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i); mji_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); } diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index bf2b6994..52c5d46f 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -988,28 +988,36 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous utilities ----------------------------------------------- // gather global node positions and velocities -void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel) { +void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel) { int nodenum = m->flex_nodenum[f]; int nstart = m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - // compute positions - if (m->flex_centered[f]) { - for (int i=0; i < nodenum; i++) { - mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); - } + // compute positions and velocities + for (int i=0; i < nodenum; i++) { + int bid = bodyid[i]; + if (m->flex_centered[f] || + (m->flex_node[3*(i+nstart)+0] == 0 && + m->flex_node[3*(i+nstart)+1] == 0 && + m->flex_node[3*(i+nstart)+2] == 0)) { + mju_copy3(xpos + 3*i, d->xpos + 3*bid); + } else { + mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bid, m->flex_node + 3*(i+nstart)); + mju_addTo3(xpos + 3*i, d->xpos + 3*bid); } - } else { - mjtNum screw[6]; - for (int i=0; i < nodenum; i++) { - mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); - mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); - mju_copy3(vel + 3*i, screw + 3); - } + + if (vel) { + mjtNum body_vel[6]; + mj_objectVelocity(m, d, mjOBJ_BODY, bid, body_vel, 0); // returns [omega, v_CoM] in world frame + + // linear velocity at CoM + mju_copy3(vel + 3*i, body_vel + 3); + + // add omega x (xpos - xipos) + mjtNum r[3], cross[3]; + mju_sub3(r, xpos + 3*i, d->xipos + 3*bid); + mju_cross(cross, body_vel, r); + mju_addTo3(vel + 3*i, cross); } } } diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 949aa257..36ec5fd0 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -130,7 +130,7 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous --------------------------------------------------------- // gather global node positions and velocities -MJAPI void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel); +MJAPI void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel); // extract 6D force:torque for one contact, in contact frame MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index f5db7108..875fb70c 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -880,17 +880,62 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, const int* dof_indices, int ndof, int nband) { int nv = m->nv; - // build global2local map for ADDH - int* global2local = NULL; + // compute upper bounds across all interpolated flexes + int max_nodenum = 0; + int max_npc = 0; + 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 npc = (order+1)*(order+1)*(order+1); + if (npc > max_npc) max_npc = npc; + if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; + } + + // nothing to do + if (max_npc == 0) { + return; + } + + int max_dim_c = 3 * max_npc; + + // single unconditional markStack + mj_markStack(d); + + // global2local map for ADDH + int* global2local = mjSTACKALLOC(d, nv, int); if (op == mjFLEXOP_ADDH) { - mj_markStack(d); - global2local = mjSTACKALLOC(d, nv, int); mju_fillInt(global2local, -1, nv); for (int i=0; inflex; f++) { // only process flex_interp @@ -899,10 +944,10 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // get stiffness and damping - mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + mjtNum* K = m->flex_stiffness + m->flex_stiffnessadr[f]; // skip if rigid or no stiffness - if (m->flex_rigid[f] || k[0] == 0) { + if (m->flex_rigid[f] || K[0] == 0) { continue; } @@ -926,27 +971,9 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; - int nodenum = m->flex_nodenum[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - // standard stack allocation - mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - - // per-cell arrays int dim_c = 3 * npc; - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* K_rot_cell = mjSTACKALLOC(d, dim_c*dim_c, mjtNum); - - // sparse Jacobian for one cell - int* J_rownnz = mjSTACKALLOC(d, dim_c, int); - int* J_rowadr = mjSTACKALLOC(d, dim_c, int); - mjtNum* J_val = mjSTACKALLOC(d, dim_c*nv, mjtNum); - int* J_colind = mjSTACKALLOC(d, dim_c*nv, int); - - // temp allocations for chain - int* chain_colind = mjSTACKALLOC(d, nv, int); - mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); // gather raw node positions (unrotated) mju_flexGatherState(m, d, f, xpos, NULL); @@ -956,6 +983,16 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, 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; + + // 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; + } + // gather cell-local node positions int gindices[125]; // max npc = 125 for quadratic mjtNum quat[4]; @@ -967,9 +1004,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, mju_quat2Mat(R, quat); mju_transpose(RT, R, 3, 3); - // get cell stiffness - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; - // 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++) { @@ -1025,9 +1059,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, J_val, K_rot_cell, dim_c); } else if (op == mjFLEXOP_ADDH) { - mj_markStack(d); // H -= J_cell^T * K_rot_cell * J_cell (banded format) - mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_zero(J_reduced, dim_c*ndof); for (int i = 0; i < dim_c; i++) { @@ -1043,7 +1075,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // KJ = K_rot_cell * J_reduced (dim_c x ndof) - mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format @@ -1056,20 +1087,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, res[i*nband + nband-1-(i-j)] -= val; } } - mj_freeStack(d); } cell_idx++; } } } - - mj_freeStack(d); } - if (op == mjFLEXOP_ADDH) { - mj_freeStack(d); // free global2local - } + mj_freeStack(d); } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index bf59900b..8c76a573 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -273,6 +273,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) { 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; + + // skip empty cells (zero stiffness) + if (k_cell[0] == 0) { + cell_idx++; + continue; + } + // gather cell-local node data mjtNum quat[4]; mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, @@ -289,9 +298,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1); } - // get cell stiffness matrix - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; - // compute force in corotational frame if (enbl_spring) { mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc); @@ -332,12 +338,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // apply accumulated forces to bodies for (int i = 0; i < nodenum; i++) { mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]); - if (m->flex_centered[f]) { - if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bodyid[i]], frc_g+3*i); - if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bodyid[i]], dmp_g+3*i); + int bid = bodyid[i]; + int nidx = i + m->flex_nodeadr[f]; + + // fast path: node at body origin (not pinned), 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, bodyid[i], d->qfrc_spring); - if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_damper); + 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); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index c0072dbb..327f0108 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1459,11 +1459,18 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int i=0; i < NX; i++) { for (int j=0; j < NY; j++) { for (int k=0; k < NZ; k++) { - int offset = 3*(i*NY*NZ + j*NZ + k); + int n0 = i*NY*NZ + j*NZ + k; + + // skip if this node is pinned (no joints on its body) + if (m->body_jntnum[bodyid[n0]] == 0) { + continue; + } + + int offset = 3*n0; int offset1 = 3*((i+1)*NY*NZ + j*NZ + k); int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k); int offset3 = 3*(i*NY*NZ + j*NZ + (k+1)); - if (i < NX-1) { + if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1472,7 +1479,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1); releaseGeom(&thisgeom, scn); } - if (j < NY-1) { + if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1481,7 +1488,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2); releaseGeom(&thisgeom, scn); } - if (k < NZ-1) { + if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index d4c567d3..6bd14bb0 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -98,6 +98,71 @@ mjCFlexcomp::mjCFlexcomp(void) { } +// identify empty cells and pin nodes exclusively in empty cells +void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, + int npnt, const double minmax[6], + int nx, int ny, int nz) { + int cx = flex->spec.cellcount[0]; + int cy = flex->spec.cellcount[1]; + int cz = flex->spec.cellcount[2]; + int ncells = cx * cy * cz; + int order = flex->spec.order; + + // determine which cells contain mesh vertices + flex->cell_empty.assign(ncells, true); + for (int i = 0; i < npnt; i++) { + // compute parametric coordinates of mesh vertex in [0, 1]^3 + // for flat meshes (zero extent along an axis), default to 0.5 + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; + double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5; + double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5; + double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5; + + // find containing cell + int ci = std::min((int)(sx * cx), cx - 1); + int cj = std::min((int)(sy * cy), cy - 1); + int ck = std::min((int)(sz * cz), cz - 1); + ci = std::max(ci, 0); + cj = std::max(cj, 0); + ck = std::max(ck, 0); + + flex->cell_empty[ci * cy * cz + cj * cz + ck] = false; + } + + // pin nodes that belong exclusively to empty cells + for (int gi = 0; gi < nx; gi++) { + for (int gj = 0; gj < ny; gj++) { + for (int gk = 0; gk < nz; gk++) { + // find all cells that reference this node + bool all_empty = true; + int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order); + int ci_max = std::min(cx - 1, gi / order); + int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order); + int cj_max = std::min(cy - 1, gj / order); + int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order); + int ck_max = std::min(cz - 1, gk / order); + + for (int ci = ci_min; ci <= ci_max && all_empty; ci++) { + for (int cj = cj_min; cj <= cj_max && all_empty; cj++) { + for (int ck = ck_min; ck <= ck_max && all_empty; ck++) { + if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) { + all_empty = false; + } + } + } + } + + if (all_empty) { + int idx = gi * ny * nz + gj * nz + gk; + pinned[idx] = true; + } + } + } + } +} + // make flexcomp object bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) { @@ -588,15 +653,30 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int nz = flex->spec.cellcount[2] * flex->spec.order + 1; int nnode = nx * ny * nz; + // mark empty cells and pin nodes exclusively in empty cells + MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz); + + // if MarkEmptyCells pinned any nodes, force centered=false + // so that pf->node (local positions) is saved to the model + if (centered) { + for (int i = 0; i < nnode; i++) { + if (pinned[i]) { + centered = false; + break; + } + } + } + std::vector node(3 * nnode, 0); int idx = 0; // Simpson's rule weights for quadratic mass distribution double massP2[3] = {1. / 6., 2. / 3., 1. / 6.}; - // compute per-node mass for trilinear: - // mass / nnode (uniform), or use Simpson for quadratic - double node_mass_uniform = mass / nnode; + + + // collect created bodies for mass normalization + std::vector node_bodies; for (int gi = 0; gi < nx; gi++) { for (int gj = 0; gj < ny; gj++) { @@ -629,7 +709,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // mass distribution if (doftype == mjFCOMPDOF_TRILINEAR) { - pb->mass = node_mass_uniform; + pb->mass = 1.0; } else { // local index within the cell for mass computation int li = gi % flex->spec.order; @@ -639,14 +719,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1; int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1; int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1; - // use Simpson weights scaled by cell count + // use Simpson weights double wi = massP2[li == 0 ? 0 : li]; double wj = massP2[lj == 0 ? 0 : lj]; double wk = massP2[lk == 0 ? 0 : lk]; - pb->mass = mass * wi * wj * wk * ncells_i * ncells_j * ncells_k - / (flex->spec.cellcount[0] * flex->spec.cellcount[1] * flex->spec.cellcount[2]); + pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k; } + node_bodies.push_back(pb); + pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; @@ -671,6 +752,21 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // normalize masses so total equals prescribed mass + double total_mass = 0; + for (mjsBody* pb : node_bodies) { + total_mass += pb->mass; + } + if (total_mass > 0) { + double scale = mass / total_mass; + for (mjsBody* pb : node_bodies) { + pb->mass *= scale; + pb->inertia[0] *= scale; + pb->inertia[1] *= scale; + pb->inertia[2] *= scale; + } + } + if (!centered) { mjs_setDouble(pf->node, node.data(), node.size()); } @@ -698,6 +794,11 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf 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; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 8624be7b..07977a39 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -116,6 +116,11 @@ class mjCFlexcomp { std::string plugin_name; std::string plugin_instance_name; mjsPlugin plugin; + + private: + // identify empty cells and pin nodes exclusively in empty cells + void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt, + const double minmax[6], int nx, int ny, int nz); }; #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3e448fb5..ae54b634 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4407,6 +4407,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { for (int ck = 0; ck < cz; ck++) { int cell_idx = ci * cy * cz + cj * cz + ck; + // skip stiffness computation for empty cells (no mesh content) + if (!cell_empty.empty() && cell_empty[cell_idx]) { + continue; + } + // gather cell's local node positions std::vector cell_pos(3 * npc); int local = 0; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1f80949d..5c81e40c 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -984,6 +984,7 @@ class mjCFlex_ : public mjCBase { std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix bool has_strain_eq = false; // true if strain constraints reference this flex + std::vector cell_empty; // true if cell contains no mesh geometry // variable-size data std::vector vertbody_; // vertex body names diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 5482568d..52e0bb30 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1031,5 +1031,202 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) { mj_deleteModel(m); } +TEST_F(UserFlexTest, EmptyCellNodePinning) { + // A 2x2x2 grid with a box mesh that fills all cells. + // No nodes should be pinned. + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions. + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 27); + + // All cells are occupied by the box, so no node should be pinned. + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + int bid = m->flex_nodebodyid[n]; + if (m->body_jntnum[bid] == 0) { + pinned++; + } + } + EXPECT_EQ(pinned, 0); + + // Verify simulation works + mjData* d = mj_makeData(m); + for (int i = 0; i < 10; i++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellNodePinningMesh) { + // Load bunny_multicell.xml which has a 3x3x3 grid. + // The bunny mesh only occupies some cells, so many nodes should be pinned. + const std::string xml_path = + GetModelPath("flex/bunny_multicell.xml"); + std::array error; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 3x3x3 grid, order=1: (3+1)^3 = 64 node positions + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 64); + + // Count pinned nodes (no joints) + int pinned = 0; + int free_nodes = 0; + for (int n = nadr; n < nadr + nnode; n++) { + int bid = m->flex_nodebodyid[n]; + if (m->body_jntnum[bid] == 0) { + pinned++; + } else { + free_nodes++; + } + } + + // At least some nodes should be pinned since the bunny doesn't fill all cells + EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells"; + EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free"; + EXPECT_EQ(pinned + free_nodes, nnode); + + // Verify the model can simulate + mjData* d = mj_makeData(m); + mj_forward(m, d); + for (int i = 0; i < 10; i++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { + // Regression test for ci_min calculation with order=2. + // A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis). + // We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty. + // + // Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2). + // With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check + // cell 0 (non-empty), incorrectly marking gi=3 as non-pinned. + // Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid. + // Anchor vertex at x=1.0 extends the bounding box to [0,1]^3. + // The 3x1x1 quadratic grid splits at x=0.33, 0.67. + // Cell 0 has vertices, cells 1 and 2 are empty. + // Interior nodes for cells 1,2 should be pinned to the parent body. + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 63); + + // Count pinned nodes: pinned nodes are assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned. + // Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2. + // That's 4 * 3 * 3 = 36 nodes. + EXPECT_EQ(pinned, 36); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, TotalMassTrilinear) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + double total_mass = 0; + for (int i = 1; i < m->nbody; ++i) { + total_mass += m->body_mass[i]; + } + + EXPECT_NEAR(total_mass, 1.5, 1e-5); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, TotalMassQuadratic) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + double total_mass = 0; + for (int i = 1; i < m->nbody; ++i) { + total_mass += m->body_mass[i]; + } + + EXPECT_NEAR(total_mass, 2.0, 1e-5); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco + From 86ad41b9c2ab9de5fd95c60e6da842bf2b804be4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 04:50:38 -0700 Subject: [PATCH 12/29] Use RenderableParams for creating Renderables. PiperOrigin-RevId: 902569516 Change-Id: Id233a5ef7fe86ce5902ce2552d8d0fec6ce860b7 --- .../filament/filament/imgui_bridge.cc | 5 +- .../filament/filament/renderable.cc | 62 ++++++++++--------- .../filament/filament/renderable.h | 31 ++++++---- .../filament/filament/scene_geom_util.cc | 11 ++-- 4 files changed, 63 insertions(+), 46 deletions(-) diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index f5553ea8..5ee492eb 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -268,8 +268,11 @@ void ImguiBridge::Update() { void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { + RenderableParams config; + DefaultRenderableParams(&config); + config.shading_model = ShadingModel::Ux; auto& r = renderables_.emplace_back( - std::make_unique(Renderable::Usage::Ux, object_mgr_)); + std::make_unique(object_mgr_, config)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 4fc9aaf3..3e518938 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -31,8 +30,12 @@ namespace mujoco { -Renderable::Renderable(Usage usage, ObjectManager* object_mgr) - : usage_(usage), object_mgr_(object_mgr) {} +void DefaultRenderableParams(RenderableParams* params) { + params->shading_model = ShadingModel::SceneObject; +} + +Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params) + : object_mgr_(object_mgr), params_(params) {} Renderable::~Renderable() noexcept { while (!entities_.empty()) { @@ -188,18 +191,19 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { void Renderable::UpdateMaterial(const MaterialParams& params, const MaterialTextures& textures) { - params_ = params; - textures_ = textures; + material_params_ = params; + material_textures_ = textures; AssignMaterial(DrawMode::Color, GetColorMaterialType()); - if (usage_ == Usage::SceneObject) { + if (params_.shading_model == ShadingModel::SceneObject) { AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth); AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation); } for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i]) { - UpdateMaterialInstance(instances_[i], params_, textures_, object_mgr_); + UpdateMaterialInstance(instances_[i], material_params_, + material_textures_, object_mgr_); } } SetDrawMode(draw_mode_); @@ -225,16 +229,16 @@ void Renderable::AssignMaterial(DrawMode mode, } const MaterialParams& Renderable::GetMaterialParams() const { - return params_; + return material_params_; } const MaterialTextures& Renderable::GetMaterialTextures() const { - return textures_; + return material_textures_; } void Renderable::SetDrawMode(DrawMode mode) { // Only SceneObjects support non-color draw modes. - if (usage_ != Usage::SceneObject) { + if (params_.shading_model != ShadingModel::SceneObject) { mode = DrawMode::Color; } @@ -333,21 +337,21 @@ void Renderable::SetWireframe(bool wireframe) { ObjectManager::MaterialType Renderable::GetColorMaterialType() const { - if (usage_ == Usage::DecorLines) { + if (params_.shading_model == ShadingModel::DecorLines) { return ObjectManager::kUnlitLine; - } else if (usage_ == Usage::Decor) { - return ObjectManager::kUnlitSegmentation; - } else if (usage_ == Usage::Ux) { + } else if (params_.shading_model == ShadingModel::Decor) { + return ObjectManager::kUnlitDecor; + } else if (params_.shading_model == ShadingModel::Ux) { return ObjectManager::kUnlitUi; - } else if (textures_.orm) { + } else if (material_textures_.orm) { return ObjectManager::kPbrPacked; - } else if (textures_.metallic) { + } else if (material_textures_.metallic) { return ObjectManager::kPbr; - } else if (textures_.roughness) { + } else if (material_textures_.roughness) { return ObjectManager::kPbr; - } else if (params_.metallic >= 0) { + } else if (material_params_.metallic >= 0) { return ObjectManager::kPbr; - } else if (params_.roughness >= 0) { + } else if (material_params_.roughness >= 0) { return ObjectManager::kPbr; } @@ -363,35 +367,35 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { has_texcoords = (it != attribs.end()); } - if (textures_.color == nullptr) { - if (params_.color.a < 1.0f) { + if (material_textures_.color == nullptr) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhongColorFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhongColorReflect; } else { return ObjectManager::kPhongColor; } - } else if (textures_.color->GetFilamentTexture()->getTarget() == + } else if (material_textures_.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhongCubeFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhongCubeReflect; } else { return ObjectManager::kPhongCube; } } else if (has_texcoords) { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhong2dUvFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhong2dUvReflect; } else { return ObjectManager::kPhong2dUv; } } else { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhong2dFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhong2dReflect; } else { return ObjectManager::kPhong2d; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 016824b8..50674c82 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -28,6 +28,21 @@ namespace mujoco { +// The shading model (material) for a Renderable. +enum class ShadingModel { + SceneObject, + Decor, + DecorLines, + Ux, +}; + +// Configuration parameters for a Renderable. +struct RenderableParams { + ShadingModel shading_model; +}; + +void DefaultRenderableParams(RenderableParams* params); + // A collection of meshes and a material that, together, define an object that // can be rendered in a scene. // @@ -38,19 +53,11 @@ namespace mujoco { // assigns the same material instance to all of them. class Renderable { public: - // How the material is to be used for rendering. - enum class Usage { - SceneObject, - Decor, - DecorLines, - Ux, - }; - // Default filament values for priority and layer mask. static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(Usage usage, ObjectManager* object_mgr); + Renderable(ObjectManager* object_mgr, const RenderableParams& params); ~Renderable() noexcept; Renderable(const Renderable&) = delete; @@ -145,11 +152,11 @@ class Renderable { ObjectManager::MaterialType GetColorMaterialType() const; - Usage usage_; ObjectManager* object_mgr_; + RenderableParams params_; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; - MaterialParams params_; - MaterialTextures textures_; + MaterialParams material_params_; + MaterialTextures material_textures_; DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; std::vector entities_; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 1b20fd18..f68f620e 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -459,14 +459,17 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, ModelObjects* model_objs, const float headpos[3]) { - Renderable::Usage usage = Renderable::Usage::SceneObject; + ShadingModel shading_model = ShadingModel::SceneObject; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - usage = Renderable::Usage::DecorLines; + shading_model = ShadingModel::DecorLines; } else if (geom.category == mjCAT_DECOR) { - usage = Renderable::Usage::Decor; + shading_model = ShadingModel::Decor; } - auto renderable = std::make_unique(usage, object_mgr); + RenderableParams config; + DefaultRenderableParams(&config); + config.shading_model = shading_model; + auto renderable = std::make_unique(object_mgr, config); // The order of these calls is important. e.g. We need to create the filament // renderable entities before we can set their transform. From 35f7db8e91a58dfda279f599bb347a8221ac34f2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 06:48:16 -0700 Subject: [PATCH 13/29] Add `StringToVector` overload for `char*`. This overload allows `StringToVector` to accept a `char*` argument when parsing into a `std::vector`, by converting the `char*` to `std::string` before processing. PiperOrigin-RevId: 902611443 Change-Id: I2048918f915c1bac74522f368ea28f23ca9a6885 --- src/user/user_util.cc | 4 ++++ src/user/user_util.h | 1 + 2 files changed, 5 insertions(+) diff --git a/src/user/user_util.cc b/src/user/user_util.cc index b98516fc..6f5bc1a4 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -1333,6 +1333,10 @@ template std::vector StringToVector(char* cs) { return v; } +template<> MJAPI std::vector StringToVector(char* cs) { + return StringToVector(std::string(cs)); +} + template<> MJAPI std::vector StringToVector(const std::string& s) { std::vector v; std::stringstream ss(s); diff --git a/src/user/user_util.h b/src/user/user_util.h index b2842d85..65a21a7b 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -264,6 +264,7 @@ template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector template MJAPI std::vector StringToVector(char *cs); template MJAPI std::vector StringToVector(const std::string& s); +template<> MJAPI std::vector StringToVector(char* cs); template<> MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user From cad734ae7048e300672247253f09f6a14c8655fd Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 07:13:05 -0700 Subject: [PATCH 14/29] Introduce Trs type. Combines a translation, rotation, and size into a single struct. PiperOrigin-RevId: 902620586 Change-Id: I628bfdedebde8c83239215efe0bee4a338eabd06 --- .../filament/filament/math_util.h | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/experimental/filament/filament/math_util.h b/src/experimental/filament/filament/math_util.h index ee3b2741..5438b5e4 100644 --- a/src/experimental/filament/filament/math_util.h +++ b/src/experimental/filament/filament/math_util.h @@ -46,15 +46,31 @@ inline filament::math::float4 ReadFloat4(const T* arr, int index = 0) { // Reads a mat3 from an array buffer in the model/scene. template -inline filament::math::mat3 ReadMat3(const T* arr, int index = 0) { +inline filament::math::mat3f ReadMat3(const T* arr, int index = 0) { // clang-format off const T* ptr = arr + (9 * index); - return filament::math::mat3(ptr[0], ptr[3], ptr[6], - ptr[1], ptr[4], ptr[7], - ptr[2], ptr[5], ptr[8]); + return filament::math::mat3f(ptr[0], ptr[3], ptr[6], + ptr[1], ptr[4], ptr[7], + ptr[2], ptr[5], ptr[8]); // clang-format on } +// A tuple of translation, rotation, and size. +struct Trs { + filament::math::float3 translation{0.0f, 0.0f, 0.0f}; + filament::math::mat3f rotation; + // Note: this is _slightly_ different than scale. For example, for capsules, + // the size determines the length of the tube and the radius of the domes, + // but the shape remains a capsule. + filament::math::float3 size{1.0f, 1.0f, 1.0f}; + + // Converts the TRS to a transform matrix. + filament::math::mat4f ToTransform() const { + return filament::math::mat4f(rotation, translation) * + filament::math::mat4f::scaling(size); + } +}; + // Calculates a reflection matrix for a plane defined by its transform. filament::math::mat4 ToReflectionMatrix(const filament::math::mat4& xform); From 2d12dee025b58c9f535b9adc9311006f9d9f4e4a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 07:13:17 -0700 Subject: [PATCH 15/29] Add dof="2d" option to flexcomp for in-plane deformations. PiperOrigin-RevId: 902620719 Change-Id: Ib06d3f7b9439e1d90a8373a4289ade0c327e72e4 --- doc/XMLreference.rst | 6 +- model/flex/gripper_2d.xml | 429 +++++++++++++++++++++++++++++++++++ src/user/user_flexcomp.cc | 11 + src/user/user_flexcomp.h | 1 + src/xml/xml_native_reader.cc | 3 +- test/user/user_flex_test.cc | 65 ++++++ 6 files changed, 513 insertions(+), 2 deletions(-) create mode 100644 model/flex/gripper_2d.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 7e5cfe5a..be74eede 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3593,7 +3593,7 @@ saving the XML: .. _body-flexcomp-dof: -:at:`dof`: :at-val:`[full, radial, trilinear, quadratic], "full"` +:at:`dof`: :at-val:`[full, radial, trilinear, quadratic, 2d], "full"` The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the different parametrizations with deformable spheres. The three models in the video are respectively `sphere_full `__, @@ -3608,6 +3608,10 @@ saving the XML: requires a free joint at the flex's parent in order for free body motion to be possible. This type of parametrization is appropriate for shapes that are relatively spherical. + **2d** + Two orthogonal translational dofs (X and Y) per vertex. This restricts the motion of the vertices to planes + parallel to the parent body's X-Y plane. + **trilinear** Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire flex, independent of the number of vertices. The positions of the vertices are updated using trilinear diff --git a/model/flex/gripper_2d.xml b/model/flex/gripper_2d.xml new file mode 100644 index 00000000..b6c6c611 --- /dev/null +++ b/model/flex/gripper_2d.xml @@ -0,0 +1,429 @@ + + + + + + diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 6bd14bb0..c62b5d6f 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -613,6 +613,17 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // add two orthogonal sliders (x and y only) + else if (doftype == mjFCOMPDOF_2D) { + for (int j=0; j < 2; j++) { + mjsJoint* jnt = mjs_addJoint(pb, 0); + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[j] = 1; + } + } + // construct body name, add to vertbody char txt[100]; mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 07977a39..782a7915 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -45,6 +45,7 @@ typedef enum _mjtDof { mjFCOMPDOF_RADIAL, mjFCOMPDOF_TRILINEAR, mjFCOMPDOF_QUADRATIC, + mjFCOMPDOF_2D, mjNFCOMPDOFS } mjtDof; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 360c46be..5e17aa1d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = { {"full", mjFCOMPDOF_FULL}, {"radial", mjFCOMPDOF_RADIAL}, {"trilinear", mjFCOMPDOF_TRILINEAR}, - {"quadratic", mjFCOMPDOF_QUADRATIC} + {"quadratic", mjFCOMPDOF_QUADRATIC}, + {"2d", mjFCOMPDOF_2D} }; diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 52e0bb30..f3df253c 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1227,6 +1227,71 @@ TEST_F(UserFlexTest, TotalMassQuadratic) { mj_deleteModel(m); } +TEST_F(UserFlexTest, Dof2d) { + // 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18 + static constexpr char xml_2d[] = R"( + + + + + + + + )"; + + // same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27 + static constexpr char xml_full[] = R"( + + + + + + + + )"; + + std::array error; + + // load 2d model + mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); + ASSERT_THAT(m_2d, NotNull()) << error.data(); + mjData* d_2d = mj_makeData(m_2d); + + // load full model + mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size()); + ASSERT_THAT(m_full, NotNull()) << error.data(); + mjData* d_full = mj_makeData(m_full); + + // verify DOF counts + EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs + EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs + + // same number of vertices and elements + EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert); + EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem); + + // each body has 2 DOFs in 2d mode, 3 in full mode + for (int i = 1; i < m_2d->nbody; i++) { + EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i; + } + for (int i = 1; i < m_full->nbody; i++) { + EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i; + } + + // simulate a few steps to make sure nothing crashes + for (int i = 0; i < 10; i++) { + mj_step(m_2d, d_2d); + mj_step(m_full, d_full); + } + + mj_deleteModel(m_2d); + mj_deleteModel(m_full); + mj_deleteData(d_2d); + mj_deleteData(d_full); +} + } // namespace } // namespace mujoco From a8a5afc8dcbf905b71d9a9c3e900dc1815d5851b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 08:29:16 -0700 Subject: [PATCH 16/29] Cache benchmark data in `engine_util_sparse_benchmark_test` PiperOrigin-RevId: 902651349 Change-Id: I760e3f696b37699366c40c3c93a74e2b1b35678e --- .../engine_util_sparse_benchmark_test.cc | 351 +++++++++++------- 1 file changed, 209 insertions(+), 142 deletions(-) diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index 3a28a291..81609beb 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -14,7 +14,6 @@ // A benchmark for comparing different implementations of mj_solveLD. -#include #include #include @@ -31,14 +30,179 @@ namespace { using CombineFuncPtr = decltype(&mju_combineSparse); using TransposeFuncPtr = decltype(&mju_transposeSparse); -using SqrMatTDFuncPtr = decltype(&mju_sqrMatTDSparse); -// number of steps to roll out before benchmarking -static const int kNumWarmupSteps = 500; +// ================================ Cached Data ================================ -// ----------------------------- old functions -------------------------------- +// ---- MatVecSparse data ---- +struct MatVecData { + int nv; + int nefc; + int nJ; + std::vector efc_J; + std::vector efc_J_rownnz, efc_J_rowadr, efc_J_colind, efc_J_rowsuper; + std::vector vec; +}; +MatVecData& GetMatVecData() { + static MatVecData data = [] { + MatVecData d; + mjModel* m = LoadModelFromPath("flex/flag.xml"); + mjData* dat = mj_makeData(m); + for (int i = 0; i < 500; i++) { + mj_step(m, dat); + } + + d.nv = m->nv; + d.nefc = dat->nefc; + d.nJ = dat->nJ; + d.efc_J.assign(dat->efc_J, dat->efc_J + d.nJ); + d.efc_J_rownnz.assign(dat->efc_J_rownnz, dat->efc_J_rownnz + d.nefc); + d.efc_J_rowadr.assign(dat->efc_J_rowadr, dat->efc_J_rowadr + d.nefc); + d.efc_J_colind.assign(dat->efc_J_colind, dat->efc_J_colind + d.nJ); + d.efc_J_rowsuper.assign(dat->efc_J_rowsuper, dat->efc_J_rowsuper + d.nefc); + + // compute direction: vec = -M^{-1} * (Ma - qfrc_smooth - qfrc_constraint) + mj_markStack(dat); + mjtNum* Ma = mj_stackAllocNum(dat, m->nv); + mjtNum* grad = mj_stackAllocNum(dat, m->nv); + mjtNum* Mgrad = mj_stackAllocNum(dat, m->nv); + mj_mulM(m, dat, Ma, dat->qacc); + for (int i = 0; i < m->nv; i++) { + grad[i] = Ma[i] - dat->qfrc_smooth[i] - dat->qfrc_constraint[i]; + } + mj_solveM(m, dat, Mgrad, grad, 1); + d.vec.resize(m->nv); + mju_scl(d.vec.data(), Mgrad, -1, m->nv); + mj_freeStack(dat); + + mj_deleteData(dat); + mj_deleteModel(m); + return d; + }(); + return data; +} + +// ---- CombineSparse data ---- +struct CombineData { + int nv; + std::vector H; + std::vector rownnz, rowadr, colind; +}; + +CombineData& GetCombineData() { + static CombineData data = [] { + CombineData cd; + mjModel* m = LoadModelFromPath("humanoid/humanoid.xml"); + m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); + + for (int i = 0; i < 500; i++) { + mj_step(m, d); + } + + cd.nv = m->nv; + mj_markStack(d); + mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); + int* rownnz = mj_stackAllocInt(d, m->nv); + int* rowadr = mj_stackAllocInt(d, m->nv); + int* colind = mj_stackAllocInt(d, m->nv*m->nv); + int* diagind = mj_stackAllocInt(d, m->nv); + + mjtNum* D = mj_stackAllocNum(d, d->nefc); + for (int i = 0; i < d->nefc; i++) { + if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { + D[i] = d->efc_D[i]; + } else { + D[i] = 0; + } + } + + int* JT_rownnz = mj_stackAllocInt(d, m->nv); + int* JT_rowadr = mj_stackAllocInt(d, m->nv); + int* JT_rowsuper = mj_stackAllocInt(d, m->nv); + int* JT_colind = mj_stackAllocInt(d, d->nJ); + mjtNum* JT = mj_stackAllocNum(d, d->nJ); + mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, + JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + + // compute H = J'*D*J, uncompressed layout + mju_sqrMatTDUncompressedInit(rowadr, m->nv); + mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv, + rownnz, rowadr, colind, + d->efc_J_rownnz, d->efc_J_rowadr, + d->efc_J_colind, d->efc_J_rowsuper, + JT_rownnz, JT_rowadr, + JT_colind, JT_rowsuper, d, + diagind); + + // compute H = M + J'*D*J + mj_addM(m, d, H, rownnz, rowadr, colind); + + // copy to persistent storage + int nH = rowadr[m->nv-1] + m->nv; // uncompressed: rowadr[r] = r*nv + cd.H.assign(H, H + nH); + cd.rownnz.assign(rownnz, rownnz + m->nv); + cd.rowadr.assign(rowadr, rowadr + m->nv); + cd.colind.assign(colind, colind + nH); + + mj_freeStack(d); + mj_deleteData(d); + mj_deleteModel(m); + return cd; + }(); + return data; +} + +// ---- TransposeSparse data ---- +struct TransposeData { + int nv; + int nefc; + int nJ; + std::vector efc_J; + std::vector efc_J_rownnz, efc_J_rowadr, efc_J_colind; +}; + +enum class Size { H2_100, H100 }; + +template +const char* ModelPath() { + if constexpr (S == Size::H2_100) { + return "../test/benchmark/testdata/2humanoid100_chol.xml"; + } else { + return "../test/benchmark/testdata/100_humanoids_chol.xml"; + } +} + +template +TransposeData& GetTransposeData() { + static TransposeData data = [] { + TransposeData td; + mjModel* m = LoadModelFromPath(ModelPath()); + m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); + + while (d->time < 2) { + mj_step(m, d); + } + + td.nv = m->nv; + td.nefc = d->nefc; + td.nJ = d->nJ; + td.efc_J.assign(d->efc_J, d->efc_J + d->nJ); + td.efc_J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + d->nefc); + td.efc_J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + d->nefc); + td.efc_J_colind.assign(d->efc_J_colind, d->efc_J_colind + d->nJ); + + mj_deleteData(d); + mj_deleteModel(m); + return td; + }(); + return data; +} + +// ================================ old functions ============================== // transpose sparse matrix (uncompressed) void ABSL_ATTRIBUTE_NOINLINE transposeSparse_baseline( @@ -229,61 +393,31 @@ void ABSL_ATTRIBUTE_NOINLINE mulMatVecSparse_8(mjtNum* res, } } -// ----------------------------- benchmark ------------------------------------ +// ----------------------------- benchmark ------------------------------------- static void BM_MatVecSparse(benchmark::State& state, int unroll) { - static mjModel* m = LoadModelFromPath("flex/flag.xml"); - mjData* d = mj_makeData(m); + MatVecData& data = GetMatVecData(); + std::vector res(data.nefc); - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate gradient - mj_markStack(d); - mjtNum *Ma = mj_stackAllocNum(d, m->nv); - mjtNum *vec = mj_stackAllocNum(d, m->nv); - mjtNum *res = mj_stackAllocNum(d, d->nefc); - mjtNum *grad = mj_stackAllocNum(d, m->nv); - mjtNum *Mgrad = mj_stackAllocNum(d, m->nv); - - // compute gradient - mj_mulM(m, d, Ma, d->qacc); - for (int i=0; i < m->nv; i++) { - grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i]; - } - - // compute search direction - mj_solveM(m, d, Mgrad, grad, 1); - mju_scl(vec, Mgrad, -1, m->nv); - - // save state - std::vector qpos = AsVector(d->qpos, m->nq); - std::vector qvel = AsVector(d->qvel, m->nv); - std::vector act = AsVector(d->act, m->na); - std::vector warmstart = AsVector(d->qacc_warmstart, m->nv); - - // time benchmark for (auto s : state) { if (unroll == 4) { - mju_mulMatVecSparse(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mju_mulMatVecSparse(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } else if (unroll == 1) { - mulMatVecSparse_1(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mulMatVecSparse_1(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } else if (unroll == 8) { - mulMatVecSparse_8(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mulMatVecSparse_8(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } } - // finalize - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -309,75 +443,30 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_MatVecSparse_1( BENCHMARK(BM_MatVecSparse_1); static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { - static mjModel* m = LoadModelFromPath("humanoid/humanoid.xml"); - m->opt.jacobian = mjJAC_SPARSE; + CombineData& data = GetCombineData(); - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate - mj_markStack(d); - mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); - int* rownnz = mj_stackAllocInt(d, m->nv); - int* rowadr = mj_stackAllocInt(d, m->nv); - int* colind = mj_stackAllocInt(d, m->nv*m->nv); - int* diagind = mj_stackAllocInt(d, m->nv); - - // compute D corresponding to quad states - mjtNum* D = mj_stackAllocNum(d, d->nefc); - for (int i = 0; i < d->nefc; i++) { - if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { - D[i] = d->efc_D[i]; - } else { - D[i] = 0; - } - } - - int* JT_rownnz = mj_stackAllocInt(d, m->nv); - int* JT_rowadr = mj_stackAllocInt(d, m->nv); - int* JT_rowsuper = mj_stackAllocInt(d, m->nv); - int* JT_colind = mj_stackAllocInt(d, d->nJ); - mjtNum* JT = mj_stackAllocNum(d, d->nJ); - mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, - JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); - - // compute H = J'*D*J, uncompressed layout - mju_sqrMatTDUncompressedInit(rowadr, m->nv); - mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv, - rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper, - JT_rownnz, JT_rowadr, - JT_colind, JT_rowsuper, d, - diagind); - - // compute H = M + J'*D*J - mj_addM(m, d, H, rownnz, rowadr, colind); + // make working copies that get modified each iteration + std::vector H = data.H; + std::vector rownnz = data.rownnz; + std::vector rowadr = data.rowadr; + std::vector colind = data.colind; // time benchmark for (auto s : state) { - for (int r = m->nv-1; r >= 0; r--) { + for (int r = data.nv-1; r >= 0; r--) { for (int i = 0; i < rownnz[r]-1; i++) { int adr = rowadr[r]; int c = colind[adr+i]; // true arguments should be i+1 and colind+rowadr[r] // but instead we repeat rownnz[c] and colind+rowadr[c] // in order to trigger all if's in combineSparse - func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i], + func(H.data()+rowadr[c], H.data()+rowadr[r], 1, -H[adr+i], rownnz[c], rownnz[c], - colind+rowadr[c], colind+rowadr[c]); + colind.data()+rowadr[c], colind.data()+rowadr[c]); } } } - // finalize - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -395,17 +484,6 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_combineSparse_old( } BENCHMARK(BM_combineSparse_old); -enum class Size { H2_100, H100 }; - -template -const char* ModelPath() { - if constexpr (S == Size::H2_100) { - return "../test/benchmark/testdata/2humanoid100_chol.xml"; - } else { - return "../test/benchmark/testdata/100_humanoids_chol.xml"; - } -} - enum class Supernode { None, PostProcess, @@ -415,44 +493,33 @@ enum class Supernode { template static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func, Supernode super) { - static mjModel* m = LoadModelFromPath(ModelPath()); + TransposeData& data = GetTransposeData(); - // force use of sparse matrices - m->opt.jacobian = mjJAC_SPARSE; - - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - while (d->time < 2) { - mj_step(m, d); - } - - mj_markStack(d); - - // need uncompressed layout - mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc); - int* res_rownnz = mj_stackAllocInt(d, m->nv); - int* res_rowadr = mj_stackAllocInt(d, m->nv); - int* res_rowsuper = mj_stackAllocInt(d, m->nv); - int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc); + // allocate output buffers (uncompressed layout) + std::vector res(data.nv * data.nefc); + std::vector res_rownnz(data.nv); + std::vector res_rowadr(data.nv); + std::vector res_rowsuper(data.nv); + std::vector res_colind(data.nv * data.nefc); // time benchmark for (auto s : state) { - int* rowsuper = (super == Supernode::Inline) ? res_rowsuper : nullptr; - func(res, d->efc_J, d->nefc, m->nv, - res_rownnz, res_rowadr, res_colind, rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + int* rowsuper = + (super == Supernode::Inline) ? res_rowsuper.data() : nullptr; + func(res.data(), data.efc_J.data(), data.nefc, data.nv, + res_rownnz.data(), res_rowadr.data(), res_colind.data(), rowsuper, + data.efc_J_rownnz.data(), data.efc_J_rowadr.data(), + data.efc_J_colind.data()); if (super == Supernode::PostProcess) { - mju_superSparse(m->nv, res_rowsuper, - res_rownnz, res_rowadr, res_colind); + mju_superSparse(data.nv, res_rowsuper.data(), + res_rownnz.data(), res_rowadr.data(), res_colind.data()); } } - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } + void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_transposeSparse_2H100_old(benchmark::State& state) { MujocoErrorTestGuard guard; From b2281883dd8f772d8937868853eb650a7df28390 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 08:53:28 -0700 Subject: [PATCH 17/29] Improve slider precision and range clamping, fixes #3206 Add clamping to slider values in both ImGui widgets and mjUI to ensure they remain within the defined min/max ranges, preventing potential floating point inaccuracies from causing values to slightly exceed the bounds. PiperOrigin-RevId: 902660458 Change-Id: Ia153ae7907f07b890f2b5ac2e3a4be93ecae7bd3 --- simulate/simulate.cc | 4 ++-- src/experimental/platform/ux/imgui_widgets.cc | 2 +- src/ui/ui_main.c | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 522404b9..b4e29e35 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1192,7 +1192,7 @@ void MakeJointSection(mj::Simulate* sim) { // set range if (sim->jnt_range_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", + mju::sprintf_arr(defSlider[0].other, "%.17g %.17g", sim->jnt_range_[i]->first, sim->jnt_range_[i]->second); else if (sim->jnt_type_[i]==mjJNT_SLIDE) { mju::strcpy_arr(defSlider[0].other, "-1 1"); @@ -1251,7 +1251,7 @@ void MakeControlSection(mj::Simulate* sim) { // set range if (sim->actuator_ctrlrange_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", + mju::sprintf_arr(defSlider[0].other, "%.17g %.17g", sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second); else { mju::strcpy_arr(defSlider[0].other, "-1 1"); diff --git a/src/experimental/platform/ux/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc index 9ffbbe3e..61d2989a 100644 --- a/src/experimental/platform/ux/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -321,7 +321,7 @@ bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max) { float f = *value; const bool res = ImGui::SliderFloat(name, &f, min, max); if (res) { - *value = f; + *value = mju_clip(f, min, max); } return res; } diff --git a/src/ui/ui_main.c b/src/ui/ui_main.c index ffc2fca2..14f588f1 100644 --- a/src/ui/ui_main.c +++ b/src/ui/ui_main.c @@ -849,8 +849,9 @@ static void setslider(mjuiItem* it, mjUI* ui, rx = mju_round(rx * it->slider.divisions) / mjMAX(1, it->slider.divisions); rx = mjMAX(0, mjMIN(1, rx)); - // compute value - mjtNum val = (mjtNum)(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx); + // compute value, clamp to range + mjtNum val = mju_clip(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx, + it->slider.range[0], it->slider.range[1]); // set slider position if (it->type == mjITEM_SLIDERINT) { From 476e2e909e0b12f7044fdb604dc22a460386991b Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 09:05:43 -0700 Subject: [PATCH 18/29] Allow Transforms to be set directly on Renderable. Also allow multiple Meshes to be assigned at once rather than one at a time. This simplifies both the usage and implementation of Renderable. PiperOrigin-RevId: 902665683 Change-Id: I23ff365a54fd6a3814ed3452b1e094ae9f698a1b --- .../filament/filament/filament_context.cc | 2 - .../filament/filament/imgui_bridge.cc | 17 +- .../filament/filament/imgui_bridge.h | 3 - .../filament/filament/renderable.cc | 213 +++++----- .../filament/filament/renderable.h | 74 ++-- .../filament/filament/scene_geom_util.cc | 378 +++++++++--------- .../filament/filament/scene_view.cc | 7 +- .../filament/filament/scene_view.h | 2 - 8 files changed, 343 insertions(+), 353 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 09f26cfa..427306b8 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -156,7 +156,6 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { request.viewport = viewport; request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain); - request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); renderer_->endFrame(); } @@ -233,7 +232,6 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, request.target = color_target_.get(); request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); - request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); const size_t num_bytes = viewport.width * viewport.height * 3; diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 5ee492eb..97dd852a 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include #include "experimental/filament/filament/material.h" @@ -32,6 +34,9 @@ namespace mujoco { +using filament::math::float3; +using filament::math::mat3f; + ImguiBridge::ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view) : object_mgr_(object_mgr), scene_view_(scene_view) {} @@ -235,11 +240,7 @@ void ImguiBridge::Update() { const int height = size.y * scale.y; auto& renderable = renderables_[renderable_index]; - if (renderable->GetNumMeshes() == 0) { - renderable->AppendMesh(mesh, index_offset, command.ElemCount); - } else { - renderable->UpdateMesh(0, mesh, index_offset, command.ElemCount); - } + renderable->SetMesh(mesh, index_offset, command.ElemCount); MaterialTextures textures; textures.color = textures_[command.GetTexID()].get(); @@ -259,6 +260,8 @@ void ImguiBridge::Update() { properties.scissor[3] = height; } renderable->UpdateMaterial(properties, textures); + renderable->SetTransform( + {float3{0, 0, 0}, mat3f(), float3(scale.x, scale.y, 1.0f)}); index_offset += command.ElemCount; ++renderable_index; @@ -284,10 +287,6 @@ void ImguiBridge::PrepareRenderables(int count) { } } -float ImguiBridge::GetScale() const { - return ImGui::GetIO().DisplayFramebufferScale.x; -} - static ImVec2 ClipSpaceToWindowCoordinates(float x, float y) { const ImVec2& display_size = ImGui::GetIO().DisplaySize; const float pos_x = display_size.x * ((x + 1) * 0.5f); diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index 4e36b333..54b5f494 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -40,9 +40,6 @@ class ImguiBridge { // synced. void Update(); - // Returns the current ImGui scale factor. - float GetScale() const; - // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp); diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 3e518938..2a9659ce 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -16,20 +16,26 @@ #include #include +#include #include #include #include #include +#include +#include #include #include #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { +using filament::math::mat4f; + void DefaultRenderableParams(RenderableParams* params) { params->shading_model = ShadingModel::SceneObject; } @@ -38,68 +44,77 @@ Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params : object_mgr_(object_mgr), params_(params) {} Renderable::~Renderable() noexcept { - while (!entities_.empty()) { - RemoveLastEntity(); + filament::Engine* engine = GetEngine(); + utils::EntityManager& em = utils::EntityManager::get(); + + for (Part& part : parts_) { + if (assigned_scene_) { + assigned_scene_->remove(part.entity); + } + engine->destroy(part.entity); + em.destroy(part.entity); } for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i] != nullptr) { - GetEngine()->destroy(instances_[i]); + engine->destroy(instances_[i]); instances_[i] = nullptr; } } } -void Renderable::RemoveLastEntity() { - if (entities_.empty()) { - return; +void Renderable::SetMesh(const Mesh* mesh, int elem_offset, int elem_count) { + if (mesh == nullptr) { + mju_error("Cannot set mesh to nullptr."); } - - utils::EntityManager& em = utils::EntityManager::get(); - utils::Entity entity = entities_.back(); - - if (assigned_scene_) { - assigned_scene_->remove(entity); - } - - GetEngine()->destroy(entity); - em.destroy(entity); - entities_.pop_back(); - meshes_.pop_back(); -} - -void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset, - int elem_count) { - MeshInfo& mesh_info = SetMesh(index, mesh, elem_offset, elem_count); - UpdateEntity(index, mesh_info); -} - -void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(-1, mesh, elem_offset, elem_count); - AppendEntity(mesh_info); -} - -void Renderable::AppendEntity(const MeshInfo& mesh_info) { - const Mesh* mesh = mesh_info.mesh; filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); } - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); if (index_buffer == nullptr) { mju_error("Invalid (null) index buffer."); } - utils::Entity entity = utils::EntityManager::get().create(); - if (entity.isNull()) { + if (elem_count == 0) { + elem_count = index_buffer->getIndexCount() - elem_offset; + } + + if (parts_.empty()) { + Part& part = parts_.emplace_back(); + part.mesh = mesh; + part.elem_offset = elem_offset; + part.elem_count = elem_count; + InitPartEntity(part); + } else if (parts_.size() == 1) { + Part& part = parts_[0]; + part.mesh = mesh; + part.elem_offset = elem_offset; + part.elem_count = elem_count; + + filament::RenderableManager& rm = GetEngine()->getRenderableManager(); + rm.setGeometryAt(rm.getInstance(part.entity), 0, + part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + part.elem_offset, part.elem_count); + + } else { + mju_error("Cannot set mesh for renderable with multiple parts."); + } +} + +void Renderable::InitPartEntity(Part& part) { + part.entity = utils::EntityManager::get().create(); + if (part.entity.isNull()) { mju_error("Failed to create entity."); } + filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer(); + filament::RenderableManager::Builder builder(1); - builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer, - mesh_info.elem_offset, mesh_info.elem_count); - if (mesh->HasBounds()) { - builder.boundingBox(mesh->GetBounds()); + builder.geometry(0, part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + part.elem_offset, part.elem_count); + if (part.mesh->HasBounds()) { + builder.boundingBox(part.mesh->GetBounds()); } else { builder.culling(false); } @@ -113,56 +128,43 @@ void Renderable::AppendEntity(const MeshInfo& mesh_info) { builder.blendOrder(0, blend_order_); builder.screenSpaceContactShadows(true); - builder.build(*GetEngine(), entity); + builder.build(*GetEngine(), part.entity); if (assigned_scene_) { - assigned_scene_->addEntity(entity); + assigned_scene_->addEntity(part.entity); } - entities_.push_back(entity); } -void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) { - if (index < 0 || index >= entities_.size()) { - mju_error("Invalid index %d for renderable.", index); +void Renderable::SetTransform(const Trs& trs) { + transform_ = trs.ToTransform(); + filament::TransformManager& tm = GetEngine()->getTransformManager(); + for (Part& part : parts_) { + tm.setTransform(tm.getInstance(part.entity), transform_); } - utils::Entity entity = entities_[index]; - - const Mesh* mesh = mesh_info.mesh; - filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); - if (vertex_buffer == nullptr) { - mju_error("Invalid (null) vertex buffer."); - } - - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); - if (index_buffer == nullptr) { - mju_error("Invalid (null) index buffer."); - } - - filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - rm.setGeometryAt(rm.getInstance(entity), 0, mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, mesh_info.elem_offset, - mesh_info.elem_count); } -Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, - int elem_offset, int elem_count) { - if (index == -1) { - index = meshes_.size(); - meshes_.emplace_back(); +const mat4f& Renderable::GetTransform() const { + return transform_; +} + +void Renderable::SetMeshes(std::span meshes, + std::span transforms) { + if (meshes.size() != transforms.size()) { + mju_error("Number of meshes does not match number of transforms."); } - if (index < 0 || index >= static_cast(meshes_.size())) { - mju_error("Invalid index %d for renderable.", index); + if (!parts_.empty()) { + mju_error("Cannot set meshes for renderable with multiple parts."); } - MeshInfo* mesh_info = &meshes_[index]; - mesh_info->mesh = mesh; - mesh_info->elem_offset = elem_offset; - mesh_info->elem_count = elem_count; - if (mesh_info->elem_count == 0) { - const int total = - mesh_info->mesh->GetFilamentIndexBuffer()->getIndexCount(); - mesh_info->elem_count = total - mesh_info->elem_offset; + filament::TransformManager& tm = GetEngine()->getTransformManager(); + for (int i = 0; i < meshes.size(); ++i) { + Part& part = parts_.emplace_back(); + part.mesh = meshes[i]; + part.elem_offset = 0; + part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); + InitPartEntity(part); + + tm.setTransform(tm.getInstance(part.entity), transforms[i]); } - return *mesh_info; } void Renderable::AddToScene(filament::Scene* scene) { @@ -173,8 +175,8 @@ void Renderable::AddToScene(filament::Scene* scene) { // Entities are already added to the scene. return; } - for (utils::Entity& entity : entities_) { - scene->addEntity(entity); + for (Part& part : parts_) { + scene->addEntity(part.entity); } assigned_scene_ = scene; } @@ -183,8 +185,8 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { if (assigned_scene_ != scene) { mju_error("Attempting to remove renderable from wrong scene."); } - for (utils::Entity& entity : entities_) { - scene->remove(entity); + for (Part& part : parts_) { + scene->remove(part.entity); } assigned_scene_ = nullptr; } @@ -245,8 +247,8 @@ void Renderable::SetDrawMode(DrawMode mode) { filament::MaterialInstance* instance = instances_[static_cast(mode)]; if (instance) { filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - filament::RenderableManager::Instance ri = rm.getInstance(entity); + for (Part& part : parts_) { + filament::RenderableManager::Instance ri = rm.getInstance(part.entity); rm.setMaterialInstanceAt(ri, 0, instance); } } @@ -259,8 +261,8 @@ std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) { layer_mask_ = mask; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setLayerMask(rm.getInstance(entity), 0xff, layer_mask_); + for (Part& part : parts_) { + rm.setLayerMask(rm.getInstance(part.entity), 0xff, layer_mask_); } } return prev; @@ -272,8 +274,8 @@ std::uint8_t Renderable::SetPriority(std::uint8_t priority) { priority_ = priority; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setPriority(rm.getInstance(entity), priority_); + for (Part& part : parts_) { + rm.setPriority(rm.getInstance(part.entity), priority_); } } return prev; @@ -285,8 +287,8 @@ std::uint16_t Renderable::SetBlendOrder(std::uint16_t blend_order) { blend_order_ = blend_order; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setBlendOrderAt(rm.getInstance(entity), 0, blend_order_); + for (Part& part : parts_) { + rm.setBlendOrderAt(rm.getInstance(part.entity), 0, blend_order_); } } return prev; @@ -297,8 +299,8 @@ void Renderable::SetCastShadows(bool cast_shadows) { cast_shadows_ = cast_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setCastShadows(rm.getInstance(entity), cast_shadows_); + for (Part& part : parts_) { + rm.setCastShadows(rm.getInstance(part.entity), cast_shadows_); } } } @@ -308,8 +310,8 @@ void Renderable::SetReceiveShadows(bool receive_shadows) { receive_shadows_ = receive_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setReceiveShadows(rm.getInstance(entity), receive_shadows_); + for (Part& part : parts_) { + rm.setReceiveShadows(rm.getInstance(part.entity), receive_shadows_); } } } @@ -322,20 +324,17 @@ void Renderable::SetWireframe(bool wireframe) { wireframe_ = wireframe; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (int i = 0; i < entities_.size(); ++i) { - utils::Entity& entity = entities_[i]; - const Mesh* mesh = meshes_[i].mesh; - filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); - rm.setGeometryAt(rm.getInstance(entity), 0, - wireframe_ ? kWireframeType : mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, meshes_[i].elem_offset, - meshes_[i].elem_count); + for (Part& part : parts_) { + filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer(); + rm.setGeometryAt(rm.getInstance(part.entity), 0, + wireframe_ ? kWireframeType : part.mesh->GetPrimitiveType(), + vertex_buffer, index_buffer, part.elem_offset, + part.elem_count); } } } - ObjectManager::MaterialType Renderable::GetColorMaterialType() const { if (params_.shading_model == ShadingModel::DecorLines) { return ObjectManager::kUnlitLine; @@ -360,8 +359,8 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if // it has them. bool has_texcoords = false; - if (!meshes_.empty()) { - const auto attribs = meshes_[0].mesh->GetVertexAttributes(); + if (!parts_.empty()) { + const auto attribs = parts_[0].mesh->GetVertexAttributes(); auto it = std::find(attribs.begin(), attribs.end(), filament::VertexAttribute::UV0); has_texcoords = (it != attribs.end()); diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 50674c82..380eea3b 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -16,13 +16,16 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLE_H_ #include +#include #include #include #include +#include #include #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" @@ -43,14 +46,20 @@ struct RenderableParams { void DefaultRenderableParams(RenderableParams* params); -// A collection of meshes and a material that, together, define an object that -// can be rendered in a scene. +// A Renderable is effectively two things: a mesh and a material. // -// Meshes can be added to the Renderable either by unique_ptr or raw pointer. -// This determines whether or not the Renderable takes ownership of the mesh. +// The mesh describes the surface geometry of the object and the material +// describes how that surface interacts with light (i.e. the color of each point +// on the surface). // -// Internally, the Renderable creates a filament::Entity for each mesh and -// assigns the same material instance to all of them. +// Defining the mesh is easy; just call SetMesh. +// +// Defining a Material happens in two stages. First, the user specifies the +// ShadingModel to use for Rendering. This describes the overall intent of +// how the Renderable will appear (e.g. lit, unlit, wireframe, etc.). Next, +// the user specifies the MaterialParams and MaterialTextures to use with the +// ShadingModel. Its these properties that ultimately define the actual material +// of the Renderable. class Renderable { public: // Default filament values for priority and layer mask. @@ -63,19 +72,21 @@ class Renderable { Renderable(const Renderable&) = delete; Renderable& operator=(const Renderable&) = delete; - // Appends a mesh to the renderable. The elem_offset and elem_count parameters - // can be used to specify a submesh to append. If elem_count is 0, assumes - // the entire mesh should be appended. - void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); + // Sets the mesh of the renderable. The elem_offset and elem_count parameters + // can be used to specify a submesh within the mesh. If elem_count is 0, + // assumes the entire mesh should be appended. + void SetMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - // Replaces the mesh at the index with a new mesh. The elem_offset and - // elem_count parameters can be used to specify a submesh to append. If - // elem_count is 0, assumes the entire mesh should be appended. - void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0, - int elem_count = 0); + // Sets the transform of the renderable. + void SetTransform(const Trs& trs); - // Returns the number of meshes that define the renderable. - int GetNumMeshes() const { return meshes_.size(); } + // Returns the current transform of the renderable. + const filament::math::mat4f& GetTransform() const; + + // Sets multiple meshes for a renderable. Each mesh is assigned a specific + // transform to allow for assembly of compound shapes. + void SetMeshes(std::span meshes, + std::span transforms); // Sets the layer mask for the managed filament Entities. Layer masks can be // used to show/hide the renderable in different views. Returns the previous @@ -107,7 +118,8 @@ class Renderable { // Removes the renderable from the given filament Scene. void RemoveFromScene(filament::Scene* scene); - // Sets the material instance for all managed entities. + // Further defines the material of the renderable. Only applies to renderables + // with a SceneObject shading model. void SetDrawMode(DrawMode mode); // Updates the parameters for the material. @@ -123,30 +135,15 @@ class Renderable { // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); - // Returns the underlying filament::entity for the given mesh. - utils::Entity operator[](int index) { return entities_[index]; } - private: - struct MeshInfo { + struct Part { + utils::Entity entity; const Mesh* mesh = nullptr; int elem_offset = 0; int elem_count = 0; }; - // Sets the mesh information for the mesh at the given index. If index is -1, - // a new mesh will be appended to the renderable. - MeshInfo& SetMesh(int index, const Mesh* mesh, int elem_offset, - int elem_count); - - // Appends a new filament::Entity to the renderable, configured to use the - // given mesh. - void AppendEntity(const MeshInfo& mesh_info); - - // Updates the filament::Entity at the given index to use the given mesh. - void UpdateEntity(int index, const MeshInfo& mesh_info); - - // Removes the last filament::Entity from the renderable. - void RemoveLastEntity(); + void InitPartEntity(Part& part); void AssignMaterial(DrawMode mode, ObjectManager::MaterialType material_type); @@ -159,8 +156,9 @@ class Renderable { MaterialTextures material_textures_; DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; - std::vector entities_; - std::vector meshes_; + std::vector parts_; + filament::math::mat4f transform_; + std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; std::uint16_t blend_order_ = 0; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index f68f620e..3b15b32d 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -44,30 +45,12 @@ namespace mujoco { using filament::math::float2; using filament::math::float3; using filament::math::float4; -using filament::math::mat4; +using filament::math::mat4f; // An arbitrary scale factor for arrows. static constexpr float kArrowScale = 1.f / 6.f; static constexpr float kArrowHeadSize = 1.75f; -// Some built-in geometries are actually composed of multiple simple shapes. A -// capsule, for example, is a open-ended tube with two dome ends. We use these -// constants to help identify which entity (by index) represents which part of -// the overall shape. -static constexpr int kCapsuleTopDome = 1; -static constexpr int kCapsuleBottomDome = 2; -static constexpr int kCylinderTopDisk = 1; -static constexpr int kCylinderBottomDisk = 2; -static constexpr int kArrow0Cone = 1; -static constexpr int kArrow0ConeDisk = 2; -static constexpr int kArrow0BottomDisk = 3; -static constexpr int kArrow1Cone = 1; -static constexpr int kArrow1BottomDisk = 2; -static constexpr int kArrow2TopCone = 1; -static constexpr int kArrow2BottomCone = 2; -static constexpr int kArrow2TopConeDisk = 3; -static constexpr int kArrow2BottomConeDisk = 4; - // Returns the tile size for infinite plane texture alignment. // This is duplicated from engine_vis_visualize.c (re-center infinite plane) // to ensure UV scaling matches the re-centering increments. @@ -87,102 +70,240 @@ static bool IsBehind(const float* headpos, const float* pos, const float* mat) { 0.0f); } -static void AddMesh(Renderable& renderable, ModelObjects* model_objs, - int data_id) { +static const Mesh* GetMesh(ModelObjects* model_objs, int data_id) { const Mesh* mesh = model_objs->GetMeshBuffer(data_id); if (mesh == nullptr) { mju_error("Unknown mesh %d", data_id); } - renderable.AppendMesh(mesh); + return mesh; } -static void AddSkinFlexMesh(Renderable& renderable, ModelObjects* model_objs, - int objid) { - renderable.AppendMesh(model_objs->GetFlexSkinGeomMesh(objid)); +static const Mesh* GetSkinFlexMesh(ModelObjects* model_objs, int objid) { + return model_objs->GetFlexSkinGeomMesh(objid); } -static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, - int hfield_id) { +static const Mesh* GetHeightField(ModelObjects* model_objs, int hfield_id) { const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); if (mesh == nullptr) { mju_error("Unknown height field %d", hfield_id); } - renderable.AppendMesh(mesh); + return mesh; } -static void AddShape(Renderable& renderable, ModelObjects* model_objs, - ModelObjects::ShapeType shape_type) { +static const Mesh* GetShape(ModelObjects* model_objs, + ModelObjects::ShapeType shape_type) { const Mesh* mesh = model_objs->GetShapeBuffer(shape_type); if (mesh == nullptr) { mju_error("Unknown shape %d", shape_type); } - renderable.AppendMesh(mesh); + return mesh; } static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { + std::vector meshes; + std::vector transforms; + + Trs trs = { + .translation = ReadFloat3(geom.pos), + .rotation = ReadMat3(geom.mat), + .size = ReadFloat3(geom.size), + }; + switch ((mjtGeom)geom.type) { case mjGEOM_MESH: - AddMesh(renderable, model_objects, geom.dataid); + meshes.push_back(GetMesh(model_objects, geom.dataid)); + // Ignore size for meshes. + transforms.push_back(mat4f(trs.rotation, trs.translation)); break; case mjGEOM_HFIELD: - AddHeightField(renderable, model_objects, geom.dataid); + meshes.push_back(GetHeightField(model_objects, geom.dataid)); + // Ignore size for height fields. + transforms.push_back(mat4f(trs.rotation, trs.translation)); break; - case mjGEOM_PLANE: - AddShape(renderable, model_objects, ModelObjects::kPlane); + case mjGEOM_PLANE: { + meshes.push_back(GetShape(model_objects, ModelObjects::kPlane)); + const bool is_infinite = !(trs.size.x > 0 && trs.size.y > 0); + if (is_infinite) { + // Infinite planes are scaled to match the tile size used by + // re-centering in engine_vis_visualize.c. + const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; + trs.size.x = plane_scale; + trs.size.y = plane_scale; + } + // Planes only define an xy size, so set the z-dimension to 1.0f. + trs.size.z = 1.0f; + transforms.push_back(trs.ToTransform()); break; + } case mjGEOM_SPHERE: - AddShape(renderable, model_objects, ModelObjects::kSphere); + meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_ELLIPSOID: - AddShape(renderable, model_objects, ModelObjects::kSphere); + meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_BOX: - AddShape(renderable, model_objects, ModelObjects::kBox); + meshes.push_back(GetShape(model_objects, ModelObjects::kBox)); + transforms.push_back(trs.ToTransform()); break; - case mjGEOM_CAPSULE: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kDome); - AddShape(renderable, model_objects, ModelObjects::kDome); + case mjGEOM_CAPSULE: { + // Capsules are a tube with two domes at the ends. + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); + + transforms.push_back(trs.ToTransform()); + + // We apply an inverse scale to the domes to counteract the capsule's + // overall scale so that the domes remain spherical in shape. + const float xz_size = 0.5f * (trs.size.x + trs.size.y); + + // Move the first dome to the top of the capsule. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + transforms.push_back(top); + + // Move the second dome to the bottom of the capsule and rotate it 180 + // degrees so that it's facing the right way. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + transforms.push_back(bottom); break; - case mjGEOM_CYLINDER: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_CYLINDER: { + // Cylinders are a tube with two disks at the ends. + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + transforms.push_back(trs.ToTransform()); + + // Move the first disk to the top of the cylinder. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(trs.size); + transforms.push_back(top); + + // Move the second disk to the bottom of the cylinder. Rotate the disk + // 180 degrees so that the normals point outwards. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(trs.size); + transforms.push_back(bottom); break; - case mjGEOM_ARROW: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(base * mat4f::scaling(trs.size)); + + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top * mat4f::scaling(trs.size)); + + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top_disk * mat4f::scaling(trs.size)); + + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + transforms.push_back(bottom * mat4f::scaling(trs.size)); + break; - case mjGEOM_ARROW1: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW1: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(base * mat4f::scaling(trs.size)); + + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(top * mat4f::scaling(trs.size)); + + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + transforms.push_back(bottom * mat4f::scaling(trs.size)); break; - case mjGEOM_ARROW2: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW2: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(base * mat4f::scaling(trs.size)); + + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top * mat4f::scaling(trs.size)); + + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(bottom * mat4f::scaling(trs.size)); + + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top_disk * mat4f::scaling(trs.size)); + + mat4f bottom_disk = base; + bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); + transforms.push_back(bottom_disk * mat4f::scaling(trs.size)); + break; + } case mjGEOM_LINE: - AddShape(renderable, model_objects, ModelObjects::kLine); + meshes.push_back(GetShape(model_objects, ModelObjects::kLine)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_LINEBOX: - AddShape(renderable, model_objects, ModelObjects::kLineBox); + meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_TRIANGLE: - AddShape(renderable, model_objects, ModelObjects::kTriangle); + meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_FLEX: - AddSkinFlexMesh(renderable, model_objects, geom.objid); + meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + // Flexes are defined in global space. + transforms.push_back(mat4f()); break; case mjGEOM_SKIN: - AddSkinFlexMesh(renderable, model_objects, geom.objid); + meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + // Skins are defined in global space. + transforms.push_back(mat4f()); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -193,124 +314,8 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, mju_warning("Unsupported geom type: %d", geom.type); break; } -} -static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) { - // Flex and skin geometries are in global space. - if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) { - return; - } - - mat4 transform = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos)); - renderable.SetLayerMask(geom.category); - - float3 size = ReadFloat3(geom.size); - filament::TransformManager& tm = - renderable.GetEngine()->getTransformManager(); - for (int j = 0; j < renderable.GetNumMeshes(); ++j) { - const utils::Entity& entity = renderable[j]; - - // Update object transform. - mat4 entity_transform = transform; - - // Some built-in drawables are composed of multiple entities. For example, - // capsules are a combination of a open tube and two dome end caps. - - if (geom.type == mjGEOM_CYLINDER) { - // Cylinders are a tube with two disks at the ends. The "bottom" disk is - // rotated so that the normals point outwards. - if (j == kCylinderTopDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - } else if (j == kCylinderBottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_CAPSULE) { - // Capsules are a tube with two domes at the ends. We apply an inverse - // scale to the domes to "counteract" the capsule's overall scale so that - // the domes remain spherical in shape. - const float xz_size = 0.5f * (size.x + size.y); - if (j == kCapsuleTopDome) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z}); - } else if (j == kCapsuleBottomDome) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z}); - } - } else if (geom.type == mjGEOM_ARROW) { - // An arrow is a tube with a cone at the end and a disk cap at the other - // end. Because the cone head's base is larger than the tube, an extra - // disk is added to the base of the cone. This disk is rotated such that - // its normal points outwards. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow0Cone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow0ConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow0BottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_ARROW1) { - // An arrow1 is a tube with a cone at the end and a disk cap at the other - // end. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow1Cone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - } else if (j == kArrow1BottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_ARROW2) { - // An arrow2 is a tube with a cone at both ends. Like the standard arrow, - // an extra disk is added to the base of each cone. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow2TopCone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2BottomCone) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2TopConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2BottomConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } - } - if (geom.type == mjGEOM_PLANE) { - const bool is_infinite = !(size.x > 0 && size.y > 0); - if (is_infinite) { - // Infinite planes are scaled to match the tile size used by - // re-centering in engine_vis_visualize.c. - const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; - entity_transform *= - mat4::scaling(float3{plane_scale, plane_scale, 1.0f}); - } else { - // Regular planes are scaled by geom.size. - entity_transform *= mat4::scaling(float3{size.x, size.y, 1.0f}); - } - } else if (geom.type != mjGEOM_MESH && geom.type != mjGEOM_HFIELD) { - entity_transform *= mat4::scaling(size); - } - tm.setTransform(tm.getInstance(entity), entity_transform); - } + renderable.SetMeshes(meshes, transforms); } static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, @@ -334,10 +339,12 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f; } } - renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); + renderable.SetLayerMask(geom.category); if (geom.category == mjCAT_DECOR) { renderable.SetCastShadows(false); renderable.SetReceiveShadows(false); + } else { + renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); } MaterialTextures textures; @@ -471,10 +478,7 @@ std::unique_ptr CreateGeomRenderable( config.shading_model = shading_model; auto renderable = std::make_unique(object_mgr, config); - // The order of these calls is important. e.g. We need to create the filament - // renderable entities before we can set their transform. PrepareGeomMeshes(*renderable, geom, scene, model_objs); - SetGeomTransform(*renderable, geom); UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); return renderable; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 18d950d7..efcbd76f 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -277,13 +277,11 @@ void SceneView::Render(filament::Renderer* renderer, // Render reflection passes. if (request.draw_mode == DrawMode::Color) { - filament::TransformManager& tm = engine_->getTransformManager(); for (size_t i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; // We assume the 0th entity is the reflective entity. - const utils::Entity entity = (*renderable)[0]; - const mat4 transform(tm.getTransform(tm.getInstance(entity))); + mat4 transform(renderable->GetTransform()); SetupReflectionCamera(transform, camera_, reflect_camera_); // Hide reflective surface from its own reflection pass. @@ -305,8 +303,7 @@ void SceneView::Render(filament::Renderer* renderer, if (request.enable_ux) { ux_camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0f, - viewport.width / request.gui_scale, - viewport.height / request.gui_scale, 0.0f, 0.0f, + viewport.width, viewport.height, 0.0f, 0.0f, 1.0f); ux_view_->setRenderTarget(render_target); renderer->render(ux_view_); diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index f50a86fa..a9a2874b 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -69,8 +69,6 @@ class SceneView { RenderTarget* target = nullptr; // Whether or not to render the UX as a separate pass. bool enable_ux = false; - // The scale factor to use for UX rendering. - float gui_scale = 1.0f; }; // Renders the scene. From 188196603d5c1bdfa36f4f6d9ba5b7395324a174 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 09:27:03 -0700 Subject: [PATCH 19/29] Fix flexcomp empty cell detection that was causing missing cells. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The previous implementation only checked whether grid cells contained mesh vertices to determine occupancy. For coarse meshes with large faces, most cells were incorrectly marked empty and pruned—even cells fully inside the object volume. This change improves the algorithm by: - Marking cells that overlap with any mesh element's AABB as non-empty. - For surface meshes, running a flood-fill from the grid boundary through non-overlapping cells to identify truly exterior cells. This preserves empty interior cells, preventing incorrect pruning of the object's core. - For volumetric meshes, defaulting to element-AABB overlap detection directly. Limitations for non-watertight meshes: If the mesh contains holes larger than the grid cell size, the flood-fill will leak into the interior. In this case, all non-element cells (including interior ones) will be marked as empty. PiperOrigin-RevId: 902675331 Change-Id: I5a84303a33d5ca7436213e7ce9aca3806c9f5a0f --- src/user/user_flexcomp.cc | 125 ++++++++++++++++++++++++++++++------ test/user/user_flex_test.cc | 58 +++++++++++++++++ 2 files changed, 164 insertions(+), 19 deletions(-) diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index c62b5d6f..081781ac 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -108,27 +110,112 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, int ncells = cx * cy * cz; int order = flex->spec.order; - // determine which cells contain mesh vertices - flex->cell_empty.assign(ncells, true); - for (int i = 0; i < npnt; i++) { - // compute parametric coordinates of mesh vertex in [0, 1]^3 - // for flat meshes (zero extent along an axis), default to 0.5 - double dx = minmax[3] - minmax[0]; - double dy = minmax[4] - minmax[1]; - double dz = minmax[5] - minmax[2]; - double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5; - double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5; - double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5; + // determine which cells contain mesh elements (not just vertices) + // for each element, compute its AABB and mark all overlapping cells + std::vector has_element(ncells, false); - // find containing cell - int ci = std::min((int)(sx * cx), cx - 1); - int cj = std::min((int)(sy * cy), cy - 1); - int ck = std::min((int)(sz * cz), cz - 1); - ci = std::max(ci, 0); - cj = std::max(cj, 0); - ck = std::max(ck, 0); + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; - flex->cell_empty[ci * cy * cz + cj * cz + ck] = false; + // vertices per element: dim+1 (edges=2, triangles=3, tets=4) + int nvpe = flex->spec.dim + 1; + + if (nvpe > 0 && !element.empty()) { + int nelem = element.size() / nvpe; + for (int e = 0; e < nelem; e++) { + // compute element AABB + double elo[3] = {1e30, 1e30, 1e30}; + double ehi[3] = {-1e30, -1e30, -1e30}; + for (int v = 0; v < nvpe; v++) { + int vid = element[nvpe * e + v]; + for (int j = 0; j < 3; j++) { + elo[j] = std::min(elo[j], points[3 * vid + j]); + ehi[j] = std::max(ehi[j], points[3 * vid + j]); + } + } + + // map element AABB to cell range + auto cellIdx = [](double coord, double lo, double d, int nc) { + if (d <= 0) return 0; + int c = (int)((coord - lo) / d * nc); + return std::max(0, std::min(nc - 1, c)); + }; + + int ci0 = cellIdx(elo[0], minmax[0], dx, cx); + int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); + int cj0 = cellIdx(elo[1], minmax[1], dy, cy); + int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); + int ck0 = cellIdx(elo[2], minmax[2], dz, cz); + int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); + + // mark all overlapping cells as containing elements + for (int ci = ci0; ci <= ci1; ci++) { + for (int cj = cj0; cj <= cj1; cj++) { + for (int ck = ck0; ck <= ck1; ck++) { + has_element[ci * cy * cz + cj * cz + ck] = true; + } + } + } + } + } + + // default: all cells non-empty (only exterior cells will be empty) + flex->cell_empty.assign(ncells, false); + + // for dim=2 (surface mesh): check watertightness and flood-fill + if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) { + // flood-fill from grid boundary to find exterior cells + // cells reachable from the boundary through non-element cells + // are outside the mesh volume; cells NOT reachable are interior + std::vector visited(ncells, false); + std::queue> bfs; + + // seed BFS from boundary cells that have no elements + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + if (ci == 0 || ci == cx - 1 || + cj == 0 || cj == cy - 1 || + ck == 0 || ck == cz - 1) { + int idx = ci * cy * cz + cj * cz + ck; + if (!has_element[idx] && !visited[idx]) { + visited[idx] = true; + flex->cell_empty[idx] = true; + bfs.push({ci, cj, ck}); + } + } + } + } + } + + // BFS: spread through non-element cells + const int dirs[6][3] = { + {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, + {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; + while (!bfs.empty()) { + auto [ci, cj, ck] = bfs.front(); + bfs.pop(); + for (auto& d : dirs) { + int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; + if (ni < 0 || ni >= cx || + nj < 0 || nj >= cy || + nk < 0 || nk >= cz) { + continue; + } + int nidx = ni * cy * cz + nj * cz + nk; + if (!visited[nidx] && !has_element[nidx]) { + visited[nidx] = true; + flex->cell_empty[nidx] = true; + bfs.push({ni, nj, nk}); + } + } + } + } else { + // dim!=2 (e.g., tet mesh): cells without element overlap are empty + for (int c = 0; c < ncells; c++) { + flex->cell_empty[c] = !has_element[c]; + } } // pin nodes that belong exclusively to empty cells diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index f3df253c..c4d10e10 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1179,6 +1179,64 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { mj_deleteModel(m); } +TEST_F(UserFlexTest, EmptyCellDetectsElements) { + // A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3. + // With cellcount="6 6 6" (216 cells), only 8 corner cells contain + // mesh vertices. + // + // Bug: MarkEmptyCells only checked vertices, so 208/216 cells are + // marked empty, causing most interior nodes to be incorrectly pinned. + // Fix: check element AABBs to correctly identify occupied cells. + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + ASSERT_EQ(nnode, 343); + + // Count pinned nodes: those assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // The cube surface fills the entire bounding box. The element-AABB + // marks all boundary cells as surface cells (152/216). The interior + // flood-fill finds no exterior seeds (all boundary cells are surface), + // so the remaining 64 cells are classified as interior (non-empty). + // No cells are empty → 0 nodes pinned. + EXPECT_EQ(pinned, 0); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + TEST_F(UserFlexTest, TotalMassTrilinear) { static constexpr char xml[] = R"( From 8e7787ad09007e0e5fd6311a00eb4d343ecfb2a4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 10:38:58 -0700 Subject: [PATCH 20/29] Use function for applying transforms to multimesh Renderables. PiperOrigin-RevId: 902712241 Change-Id: I5bfdab8bce12ffb1a1f030ae0a32a61e9b8157e1 --- .../filament/filament/renderable.cc | 29 ++- .../filament/filament/renderable.h | 12 +- .../filament/filament/scene_geom_util.cc | 234 ++++++++++-------- 3 files changed, 156 insertions(+), 119 deletions(-) diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 2a9659ce..f1a4e2bc 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -135,11 +135,23 @@ void Renderable::InitPartEntity(Part& part) { } void Renderable::SetTransform(const Trs& trs) { - transform_ = trs.ToTransform(); - filament::TransformManager& tm = GetEngine()->getTransformManager(); - for (Part& part : parts_) { - tm.setTransform(tm.getInstance(part.entity), transform_); + if (parts_.empty()) { + transform_ = trs.ToTransform(); + return; } + + filament::TransformManager& tm = GetEngine()->getTransformManager(); + if (get_transform_fn_) { + for (int i = 0; i < parts_.size(); ++i) { + const mat4f& transform = get_transform_fn_(i, trs); + tm.setTransform(tm.getInstance(parts_[i].entity), transform); + } + } else { + for (Part& part : parts_) { + tm.setTransform(tm.getInstance(part.entity), trs.ToTransform()); + } + } + transform_ = tm.getTransform(tm.getInstance(parts_[0].entity)); } const mat4f& Renderable::GetTransform() const { @@ -147,23 +159,18 @@ const mat4f& Renderable::GetTransform() const { } void Renderable::SetMeshes(std::span meshes, - std::span transforms) { - if (meshes.size() != transforms.size()) { - mju_error("Number of meshes does not match number of transforms."); - } + GetTransformFn get_transform_fn) { if (!parts_.empty()) { mju_error("Cannot set meshes for renderable with multiple parts."); } - filament::TransformManager& tm = GetEngine()->getTransformManager(); + get_transform_fn_ = get_transform_fn; for (int i = 0; i < meshes.size(); ++i) { Part& part = parts_.emplace_back(); part.mesh = meshes[i]; part.elem_offset = 0; part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); InitPartEntity(part); - - tm.setTransform(tm.getInstance(part.entity), transforms[i]); } } diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 380eea3b..9227e634 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLE_H_ #include +#include #include #include @@ -83,10 +84,13 @@ class Renderable { // Returns the current transform of the renderable. const filament::math::mat4f& GetTransform() const; - // Sets multiple meshes for a renderable. Each mesh is assigned a specific - // transform to allow for assembly of compound shapes. + // Sets multiple meshes for a renderable. Users can optionally provide a + // function that will be used to compute the transform for each (sub)mesh + // relative to the transform of the renderable itself. This allows users to + // construct compound (but rigid) objects from multiple meshes. + using GetTransformFn = std::function; void SetMeshes(std::span meshes, - std::span transforms); + GetTransformFn get_transform = nullptr); // Sets the layer mask for the managed filament Entities. Layer masks can be // used to show/hide the renderable in different views. Returns the previous @@ -158,7 +162,7 @@ class Renderable { filament::Scene* assigned_scene_ = nullptr; std::vector parts_; filament::math::mat4f transform_; - + GetTransformFn get_transform_fn_; std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; std::uint16_t blend_order_ = 0; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 3b15b32d..80c653a7 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -103,7 +103,7 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { std::vector meshes; - std::vector transforms; + Renderable::GetTransformFn get_transforms; Trs trs = { .translation = ReadFloat3(geom.pos), @@ -115,12 +115,12 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, case mjGEOM_MESH: meshes.push_back(GetMesh(model_objects, geom.dataid)); // Ignore size for meshes. - transforms.push_back(mat4f(trs.rotation, trs.translation)); + trs.size = float3{1.0f, 1.0f, 1.0f}; break; case mjGEOM_HFIELD: meshes.push_back(GetHeightField(model_objects, geom.dataid)); // Ignore size for height fields. - transforms.push_back(mat4f(trs.rotation, trs.translation)); + trs.size = float3{1.0f, 1.0f, 1.0f}; break; case mjGEOM_PLANE: { meshes.push_back(GetShape(model_objects, ModelObjects::kPlane)); @@ -134,20 +134,16 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, } // Planes only define an xy size, so set the z-dimension to 1.0f. trs.size.z = 1.0f; - transforms.push_back(trs.ToTransform()); break; } case mjGEOM_SPHERE: meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_ELLIPSOID: meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_BOX: meshes.push_back(GetShape(model_objects, ModelObjects::kBox)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_CAPSULE: { // Capsules are a tube with two domes at the ends. @@ -155,25 +151,31 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); - transforms.push_back(trs.ToTransform()); - - // We apply an inverse scale to the domes to counteract the capsule's - // overall scale so that the domes remain spherical in shape. - const float xz_size = 0.5f * (trs.size.x + trs.size.y); - - // Move the first dome to the top of the capsule. - mat4f top = mat4f(trs.rotation, trs.translation); - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); - transforms.push_back(top); - - // Move the second dome to the bottom of the capsule and rotate it 180 - // degrees so that it's facing the right way. - mat4f bottom = mat4f(trs.rotation, trs.translation); - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); - transforms.push_back(bottom); + get_transforms = [](int index, const Trs& trs) { + // We apply an inverse scale to the domes to counteract the capsule's + // overall scale so that the domes remain spherical in shape. + const float xz_size = 0.5f * (trs.size.x + trs.size.y); + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first dome to the top of the capsule. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return top; + } else if (index == 2) { + // Move the second dome to the bottom of the capsule and rotate it 180 + // degrees so that it's facing the right way. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return bottom; + } else { + mju_error("Invalid index for capsule geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_CYLINDER: { @@ -182,21 +184,28 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - transforms.push_back(trs.ToTransform()); - - // Move the first disk to the top of the cylinder. - mat4f top = mat4f(trs.rotation, trs.translation); - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(trs.size); - transforms.push_back(top); - - // Move the second disk to the bottom of the cylinder. Rotate the disk - // 180 degrees so that the normals point outwards. - mat4f bottom = mat4f(trs.rotation, trs.translation); - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(trs.size); - transforms.push_back(bottom); + get_transforms = [](int index, const Trs& trs) { + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first disk to the top of the cylinder. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(trs.size); + return top; + } else if (index == 2) { + // Move the second disk to the bottom of the cylinder. Rotate the disk + // 180 degrees so that the normals point outwards. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(trs.size); + return bottom; + } else { + mju_error("Invalid index for cylinder geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_ARROW: { @@ -205,27 +214,33 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(base * mat4f::scaling(trs.size)); - - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top * mat4f::scaling(trs.size)); - - mat4f top_disk = base; - top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); - top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top_disk * mat4f::scaling(trs.size)); - - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - transforms.push_back(bottom * mat4f::scaling(trs.size)); - + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow geom: %d (expected [0,3])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_ARROW1: { @@ -233,19 +248,26 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + get_transforms = [](int index, const Trs& trs) { mat4f base = mat4f(trs.rotation, trs.translation); base *= mat4f::scaling(float3{1, 1, kArrowScale}); base *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(base * mat4f::scaling(trs.size)); - - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(top * mat4f::scaling(trs.size)); - - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - transforms.push_back(bottom * mat4f::scaling(trs.size)); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow1 geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_ARROW2: { @@ -255,55 +277,58 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(base * mat4f::scaling(trs.size)); - - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top * mat4f::scaling(trs.size)); - - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(bottom * mat4f::scaling(trs.size)); - - mat4f top_disk = base; - top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); - top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top_disk * mat4f::scaling(trs.size)); - - mat4f bottom_disk = base; - bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); - transforms.push_back(bottom_disk * mat4f::scaling(trs.size)); - + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return bottom * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 4) { + mat4f bottom_disk = base; + bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); + return bottom_disk * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow2 geom: %d (expected [0,4])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_LINE: meshes.push_back(GetShape(model_objects, ModelObjects::kLine)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_LINEBOX: meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_TRIANGLE: meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_FLEX: meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); // Flexes are defined in global space. - transforms.push_back(mat4f()); + trs = Trs(); break; case mjGEOM_SKIN: meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); // Skins are defined in global space. - transforms.push_back(mat4f()); + trs = Trs(); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -315,7 +340,8 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, break; } - renderable.SetMeshes(meshes, transforms); + renderable.SetMeshes(meshes, get_transforms); + renderable.SetTransform(trs); } static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, From 6cb6e5a93f62235df6f00a38fe7f023ae99802ae Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 20 Apr 2026 11:59:24 -0700 Subject: [PATCH 21/29] Enable multiccd by default. PiperOrigin-RevId: 902752921 Change-Id: I8e2085ff17db0ac0db1641b8837415c458e5eca4 --- doc/includes/references.h | 8 ++++---- include/mujoco/mjmodel.h | 8 ++++---- mjx/mujoco/mjx/_src/types.py | 2 -- .../mjx/third_party/mujoco_warp/_src/collision_convex.py | 4 ++-- mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py | 2 +- mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py | 4 ++-- plugin/usd_decoder/usd_decoder.cc | 2 +- python/mujoco/bindings_test.py | 2 +- python/mujoco/introspect/enums.py | 8 ++++---- python/mujoco/introspect/enums_test.py | 5 ++--- src/engine/engine_collision_convex.c | 4 ++-- src/engine/engine_support.c | 4 ++-- src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc | 4 ++-- src/xml/xml_native_reader.cc | 2 +- src/xml/xml_native_writer.cc | 6 +++--- test/engine/engine_collision_convex_test.cc | 2 +- test/engine/engine_collision_gjk_test.cc | 2 +- test/engine/engine_solver_test.cc | 1 + .../experimental/usd/mjcPhysics/mjc_physics_scene_test.cc | 2 -- unity/Runtime/Bindings/MjBindings.cs | 8 ++++---- wasm/codegen/generated/bindings.cc | 2 +- wasm/tests/bindings_test.ts | 8 ++++---- 22 files changed, 43 insertions(+), 47 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 0ee5a22b..1e1db78d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -494,8 +494,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected mjDSBL_NATIVECCD = 1<<17, // native convex collision detection mjDSBL_ISLAND = 1<<18, // constraint island discovery + mjDSBL_MULTICCD = 1<<19, // multiple CCD contact points - mjNDISABLE = 19 // number of disable flags + mjNDISABLE = 20 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters @@ -503,10 +504,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_FWDINV = 1<<2, // record solver statistics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: - mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_SLEEP = 1<<5, // sleeping + mjENBL_SLEEP = 1<<4, // sleeping - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; typedef enum mjtJoint_ { // type of degree of freedom mjJNT_FREE = 0, // global position and orientation (quat) (7) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index fba680f9..1c9214ae 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -70,8 +70,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected mjDSBL_NATIVECCD = 1<<17, // native convex collision detection mjDSBL_ISLAND = 1<<18, // constraint island discovery + mjDSBL_MULTICCD = 1<<19, // multiple CCD contact points - mjNDISABLE = 19 // number of disable flags + mjNDISABLE = 20 // number of disable flags } mjtDisableBit; @@ -81,10 +82,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_FWDINV = 1<<2, // record solver statistics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: - mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_SLEEP = 1<<5, // sleeping + mjENBL_SLEEP = 1<<4, // sleeping - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 8bc60bd2..ff08579c 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -92,8 +92,6 @@ class EnableBit(enum.IntFlag): INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE # unsupported: OVERRIDE, ENERGY, FWDINV, ISLAND - # required by the C implementation only, ignored otherwise: MULTICCD - MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD SLEEP = mujoco.mjtEnableBit.mjENBL_SLEEP diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py index 4f6b21c5..88917ea7 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py @@ -36,7 +36,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAX_EPAHORIZON from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data -from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit +from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 @@ -1127,7 +1127,7 @@ def convex_narrowphase(m: Model, d: Data, ctx: CollisionContext, collision_table epa_iterations = 16 if nboxbox == ncollision else m.opt.ccd_iterations # set to true to enable multiccd - use_multiccd = m.opt.enableflags & EnableBit.MULTICCD + use_multiccd = m.opt.disableflags & DisableBit.MULTICCD == 0 # need at least 4 (square sides) if there's a box collision needing multiccd nmaxpolygon = 4 if nboxbox > 0 else 0 diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 4a743681..56bbb18c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -370,7 +370,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: ) # check for unsupported margin + multicontact / box-box CCD combinations - use_multiccd = mjm.opt.enableflags & types.EnableBit.MULTICCD + use_multiccd = (mjm.opt.disableflags & types.DisableBit.MULTICCD) == 0 nativeccd_disabled = mjm.opt.disableflags & types.DisableBit.NATIVECCD BOX = int(mujoco.mjtGeom.mjGEOM_BOX) MESH = int(mujoco.mjtGeom.mjGEOM_MESH) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 7a9dc2a5..bf4f1b6b 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -184,6 +184,7 @@ class DisableBit(enum.IntFlag): EULERDAMP: implicit damping for Euler integration NATIVECCD: native convex collision detection (ignored in MJWarp) ISLAND: constraint islands + MULTICCD: multiple CCD contact points """ CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT @@ -203,6 +204,7 @@ class DisableBit(enum.IntFlag): EULERDAMP = mujoco.mjtDisableBit.mjDSBL_EULERDAMP NATIVECCD = mujoco.mjtDisableBit.mjDSBL_NATIVECCD ISLAND = mujoco.mjtDisableBit.mjDSBL_ISLAND + MULTICCD = mujoco.mjtDisableBit.mjDSBL_MULTICCD # unsupported: MIDPHASE, AUTORESET @@ -212,12 +214,10 @@ class EnableBit(enum.IntFlag): Attributes: ENERGY: energy computation INVDISCRETE: discrete-time inverse dynamics - MULTICCD: multiple contacts with CCD """ ENERGY = mujoco.mjtEnableBit.mjENBL_ENERGY INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE - MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD # unsupported: OVERRIDE, FWDINV, ISLAND diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index f1ab1c05..04fe6976 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -704,7 +704,7 @@ void ParseUsdPhysicsScene(mjSpec* spec, bool multiccd_flag; mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag); - spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0); + spec->option.disableflags |= (!multiccd_flag ? mjDSBL_MULTICCD : 0); // Compiler attributes auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr(); diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 8dd91c60..c4b550d1 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -966,7 +966,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) - self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6) + self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 5) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index d686e861..1ec77cb1 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -46,7 +46,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_AUTORESET', 65536), ('mjDSBL_NATIVECCD', 131072), ('mjDSBL_ISLAND', 262144), - ('mjNDISABLE', 19), + ('mjDSBL_MULTICCD', 524288), + ('mjNDISABLE', 20), ]), )), ('mjtEnableBit', @@ -58,9 +59,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_ENERGY', 2), ('mjENBL_FWDINV', 4), ('mjENBL_INVDISCRETE', 8), - ('mjENBL_MULTICCD', 16), - ('mjENBL_SLEEP', 32), - ('mjNENABLE', 6), + ('mjENBL_SLEEP', 16), + ('mjNENABLE', 5), ]), )), ('mjtJoint', diff --git a/python/mujoco/introspect/enums_test.py b/python/mujoco/introspect/enums_test.py index d023fbcb..e373034b 100644 --- a/python/mujoco/introspect/enums_test.py +++ b/python/mujoco/introspect/enums_test.py @@ -42,9 +42,8 @@ class EnumsTest(absltest.TestCase): ('mjENBL_ENERGY', 1<<1), ('mjENBL_FWDINV', 1<<2), ('mjENBL_INVDISCRETE', 1<<3), - ('mjENBL_MULTICCD', 1<<4), - ('mjENBL_SLEEP', 1<<5), - ('mjNENABLE', 6))) + ('mjENBL_SLEEP', 1<<4), + ('mjNENABLE', 5))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index a847a099..46f4cf09 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -828,7 +828,7 @@ static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* o // reduce mesh collisions to 4 contacts max if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) { if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) { - return mjENABLED(mjENBL_MULTICCD) ? 4 : 1; + return mjDISABLED(mjDSBL_MULTICCD) ? 1 : 4; } } @@ -857,7 +857,7 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN } // look for additional contacts - if (ncon == 1 && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) + if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { // multiCCD parameters diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a8725bf7..2cff8056 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -66,7 +66,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Eulerdamp", "AutoReset", "NativeCCD", - "Island" + "Island", + "MultiCCD" }; @@ -76,7 +77,6 @@ const char* mjENABLESTRING[mjNENABLE] = { "Energy", "Fwdinv", "InvDiscrete", - "MultiCCD", "Sleep" }; diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 2820a3aa..ccc4158c 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -649,7 +649,6 @@ class ModelWriter { }; const std::vector> enable_flags = { - {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD}, {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV}, {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY}, {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE}, @@ -677,7 +676,8 @@ class ModelWriter { {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP}, {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET}, {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}, - {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}}; + {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}, + {MjcPhysicsTokens->mjcFlagMulticcd, mjDSBL_MULTICCD}}; for (const auto &[token, flag] : disable_flags) { create_flag_attr(token, flag, false); } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 5e17aa1d..af291787 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1284,6 +1284,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("autoreset", mjDSBL_AUTORESET) READDSBL("nativeccd", mjDSBL_NATIVECCD) READDSBL("island", mjDSBL_ISLAND) + READDSBL("multiccd", mjDSBL_MULTICCD) #undef READDSBL #define READENBL(NAME, MASK) \ @@ -1295,7 +1296,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READENBL("energy", mjENBL_ENERGY) READENBL("fwdinv", mjENBL_FWDINV) READENBL("invdiscrete", mjENBL_INVDISCRETE) - READENBL("multiccd", mjENBL_MULTICCD) READENBL("sleep", mjENBL_SLEEP) #undef READENBL } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index ebe95bab..0bc4c07a 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1093,7 +1093,7 @@ void mjXWriter::Option(XMLElement* root) { XMLElement* sub = InsertEnd(section, "flag"); #define WRITEDSBL(NAME, MASK) \ - if( model->option.disableflags & MASK ) \ + if (model->option.disableflags & MASK) \ WriteAttrKey(sub, NAME, enable_map, 2, 0); WRITEDSBL("constraint", mjDSBL_CONSTRAINT) WRITEDSBL("equality", mjDSBL_EQUALITY) @@ -1114,16 +1114,16 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("autoreset", mjDSBL_AUTORESET) WRITEDSBL("nativeccd", mjDSBL_NATIVECCD) WRITEDSBL("island", mjDSBL_ISLAND) + WRITEDSBL("multiccd", mjDSBL_MULTICCD) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ - if( model->option.enableflags & MASK ) \ + if (model->option.enableflags & MASK) \ WriteAttrKey(sub, NAME, enable_map, 2, 1); WRITEENBL("override", mjENBL_OVERRIDE) WRITEENBL("energy", mjENBL_ENERGY) WRITEENBL("fwdinv", mjENBL_FWDINV) WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) - WRITEENBL("multiccd", mjENBL_MULTICCD) WRITEENBL("sleep", mjENBL_SLEEP) #undef WRITEENBL } diff --git a/test/engine/engine_collision_convex_test.cc b/test/engine/engine_collision_convex_test.cc index 9615b542..7d4346e5 100644 --- a/test/engine/engine_collision_convex_test.cc +++ b/test/engine/engine_collision_convex_test.cc @@ -68,7 +68,7 @@ TEST_F(MjcConvexTest, CylinderBox) { EXPECT_EQ(data->ncon, 5); // with multiCCD disabled, should find 1 contact - model->opt.enableflags &= ~mjENBL_MULTICCD; + model->opt.disableflags |= mjDSBL_MULTICCD; mj_forward(model, data); EXPECT_EQ(data->ncon, 1); diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 653abbb4..4e4f9f10 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1998,7 +1998,7 @@ TEST_F(MjGjkTest, CylinderBoxMargin) { diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 523dfd33..8aaf1d9d 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -46,6 +46,7 @@ TEST_F(SolverTest, IslandsEquivalent) { model->opt.tolerance = 0; // set tolerance to 0 model->opt.ls_tolerance = 0; // set ls_tolerance to 0 model->opt.ccd_tolerance = 0; // set ccd_tolerance to 0 + model->opt.disableflags |= mjDSBL_MULTICCD; // disable multiccd int nv = model->nv; diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc index 30cb4d70..d7f106d4 100644 --- a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -179,8 +179,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) { EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FwdinvFlag, mjENBL_FWDINV); EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(InvDiscreteFlag, mjENBL_INVDISCRETE); - EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, - mjENBL_MULTICCD); mj_deleteModel(default_model); mj_deleteSpec(empty_spec); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 1673c4d0..d5ea2b37 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -137,16 +137,16 @@ public enum mjtDisableBit : int{ mjDSBL_AUTORESET = 65536, mjDSBL_NATIVECCD = 131072, mjDSBL_ISLAND = 262144, - mjNDISABLE = 19, + mjDSBL_MULTICCD = 524288, + mjNDISABLE = 20, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, mjENBL_ENERGY = 2, mjENBL_FWDINV = 4, mjENBL_INVDISCRETE = 8, - mjENBL_MULTICCD = 16, - mjENBL_SLEEP = 32, - mjNENABLE = 6, + mjENBL_SLEEP = 16, + mjNENABLE = 5, } public enum mjtJoint : int{ mjJNT_FREE = 0, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 185f5bb7..dfe18aea 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -10942,6 +10942,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjDSBL_AUTORESET", mjDSBL_AUTORESET) .value("mjDSBL_NATIVECCD", mjDSBL_NATIVECCD) .value("mjDSBL_ISLAND", mjDSBL_ISLAND) + .value("mjDSBL_MULTICCD", mjDSBL_MULTICCD) .value("mjNDISABLE", mjNDISABLE); enum_("mjtDyn") .value("mjDYN_NONE", mjDYN_NONE) @@ -10956,7 +10957,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjENBL_ENERGY", mjENBL_ENERGY) .value("mjENBL_FWDINV", mjENBL_FWDINV) .value("mjENBL_INVDISCRETE", mjENBL_INVDISCRETE) - .value("mjENBL_MULTICCD", mjENBL_MULTICCD) .value("mjENBL_SLEEP", mjENBL_SLEEP) .value("mjNENABLE", mjNENABLE); enum_("mjtEq") diff --git a/wasm/tests/bindings_test.ts b/wasm/tests/bindings_test.ts index 516cf507..a58fd087 100644 --- a/wasm/tests/bindings_test.ts +++ b/wasm/tests/bindings_test.ts @@ -684,10 +684,10 @@ describe('MuJoCo WASM Bindings', () => { it('should check constants values', () => { expect(mujoco.mjNEQDATA).toBe(11); expect(mujoco.mjDISABLESTRING).toEqual([ - 'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', 'Spring', - 'Damper', 'Gravity', 'Clampctrl', 'Warmstart', 'Filterparent', - 'Actuation', 'Refsafe', 'Sensor', 'Midphase', 'Eulerdamp', 'AutoReset', - 'NativeCCD', 'Island' + 'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', + 'Spring', 'Damper', 'Gravity', 'Clampctrl', 'Warmstart', + 'Filterparent', 'Actuation', 'Refsafe', 'Sensor', 'Midphase', + 'Eulerdamp', 'AutoReset', 'NativeCCD', 'Island', 'MultiCCD', ]); expect(mujoco.mjRNDSTRING).toEqual([ ['Shadow', '1', 'S'], ['Wireframe', '0', 'W'], ['Reflection', '1', 'R'], From bc5883e82f20d2596816944c3c9c64de021927e7 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 20 Apr 2026 12:52:30 -0700 Subject: [PATCH 22/29] Update multiccd documentation. PiperOrigin-RevId: 902779253 Change-Id: I767cd0a230b78efe2a71f83e5f2f134268997dcb --- doc/XMLreference.rst | 2 +- doc/changelog.rst | 8 ++++++++ doc/computation/index.rst | 20 +++++++++++--------- doc/mjwarp/index.rst | 6 ++---- doc/modeling.rst | 9 +++++---- 5 files changed, 27 insertions(+), 18 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index be74eede..f01541e4 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -677,7 +677,7 @@ from its default. .. _option-flag-multiccd: -:at:`multiccd`: :at-val:`[disable, enable], "disable"` +:at:`multiccd`: :at-val:`[disable, enable], "enable"` This flag enables multiple-contact collision detection for geom pairs that use a general-purpose convex-convex collider e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface and the single contact point generated by the convex-convex collider cannot accurately capture the surface contact, leading diff --git a/doc/changelog.rst b/doc/changelog.rst index 218bec89..451b8d0c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -14,6 +14,14 @@ General number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new attribute ":ref:`cell ` + .. admonition:: Breaking API changes + :class: attention + + - The feature :ref:`multiccd` is now enabled by default. This feature has little performance overhead + and gives better contact behavior for stability. + + **Migration:** The flag :ref:`multiccd` must be explicitly disabled. + Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 01b06886..08eea043 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1656,23 +1656,25 @@ Both pipelines are controlled by a tolerance (in units of distance) and maximum Multiple contacts ^^^^^^^^^^^^^^^^^ -Some colliders can return more than one contact per colliding pair to model line or surface contacts, as when two flat +Some colliders can return more than one contact per colliding pair to model edge or surface contacts, as when two flat objects touch. For example the capsule-plane and box-plane colliders can return up to two or four contacts, -respectively. Standard general-purpose convex collision algorithms like MPR and GJK always return a single contact +respectively. Standard general-purpose convex collision algorithms like MPR and GJK/EPA always return a single contact point, which is problematic for surface contact scenarios (e.g., box-stacking). Both of MuJoCo's CCD pipelines can return multiple points per contacting pair ("multiccd"). This behavior is controlled by the :ref:`multiccd` flag, but is implemented in different ways with different trade-offs: -libccd pipeline (legacy) +multi-run pipeline (legacy) Multiple contact points are found by rotating the two geoms by ±1e-3 radians around the tangential axes and re-running the collision routine. If a new contact is detected it is added, allowing for up to 4 additional contact - points. This method is effective, but increases the cost of each collision call by a factor of 5. + points. This method is effective, but increases the cost of each collision call by a factor of 5. This method is + used when the :ref:`nativeccd` flag is disabled, and for geoms collisions involving cylinders + and capsules or with :ref:`positive contact margins`. -native pipeline - Native multiccd discovers multiple contacts using a novel analysis of the contacting surfaces at the solution, - avoiding full re-runs of the collision routine, and is thus effectively "free". Note that native multiccd currently - does not support positive contact margins. If one of the two geoms has a positive margin, native multiccd will fall - back to legacy algorithm. +single-shot pipeline + The single-shot pipeline is used in conjunction with the native CCD pipeline, i.e., when the + :ref:`nativeccd` flag is enabled. As this pipeline is one-shot and most of the geom analysis + is done at compilation time, there is very little performance overhead. Supported geoms are boxes and meshes without + :ref:`positive contact margins`. .. _coDistance: diff --git a/doc/mjwarp/index.rst b/doc/mjwarp/index.rst index bd65306b..f5d10431 100644 --- a/doc/mjwarp/index.rst +++ b/doc/mjwarp/index.rst @@ -1265,8 +1265,6 @@ is available by setting the ``NATIVECCD`` disable flag: The specialized collider generates up to 8 contact points, compared to up to 4 for the convex pipeline, and may improve contact stability for tasks involving box stacking or manipulation. -.. TODO(taylorhowell): update this section once multiccd is on by default. - CCD margin ---------- @@ -1283,8 +1281,8 @@ CCD colliders and will raise a ``NotImplementedError`` when calling :func:`mjw.p - Scenario - Workaround * - box-box, box-mesh, mesh-mesh - - :ref:`MULTICCD ` enabled - - Set margin to ``0`` or do not enable ``MULTICCD`` + - :ref:`MULTICCD ` enabled (on by default) + - Set margin to ``0`` or disable ``MULTICCD`` * - box-box - :ref:`NATIVECCD ` enabled (on by default) - Set margin to ``0`` or disable ``NATIVECCD`` diff --git a/doc/modeling.rst b/doc/modeling.rst index 017e68fc..c5ed084a 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1767,10 +1767,11 @@ better visualize and understand the contact configuration and resulting forces. a. Improve the geometry of the contacting geoms in order to add more contact points, possibly with non-flat geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components. - b. If contacts are between flat surfaces, try enabling the :ref:`multiccd` flag, which allows - the detector to find more contacts than the single contact returned by the convex-convex collider. - c. Try enabling the native collision detection pipeline by setting the :ref:`nativeccd` flag, - which uses a more accurate and efficient convex collision detection algorithm. + b. If contacts are between flat surfaces, make sure that the flag :ref:`multiccd` is not + disabled (enabled by default), as it allows the detector to find more contacts than the single contact + returned by the convex-convex collider. + c. Make sure that the flag :ref:`nativeccd` is not disabled (enabled by default), + as NativeCCD is a more accurate and efficient convex collision detection algorithm. **High-frequency vibration** High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in From 3325971840e92177ca3a38a539f446cf739b36cc Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 21 Apr 2026 04:02:34 -0700 Subject: [PATCH 23/29] Add mj_maxContact API function. PiperOrigin-RevId: 903135055 Change-Id: I5f103c7d51f97e327c923bc567002ace835f5517 --- doc/APIreference/functions.rst | 12 ++ doc/changelog.rst | 3 +- doc/computation/index.rst | 145 +++++++++++++++++--- doc/css/theme_overrides.css | 92 +++++++++++++ doc/includes/references.h | 1 + include/mujoco/mujoco.h | 5 + python/mujoco/functions.cc | 1 + python/mujoco/introspect/functions.py | 26 ++++ src/engine/engine_collision_driver.c | 100 +++++++++++++- src/engine/engine_collision_driver.h | 8 +- test/engine/engine_collision_driver_test.cc | 68 +++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 + wasm/codegen/generated/bindings.cc | 5 + 13 files changed, 448 insertions(+), 21 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index ef49bf39..2e83a385 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1179,6 +1179,18 @@ It is also triggered for :ref:`user sensors` of :ref:`stage`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_maxContact + +Return the maximum number of contacts that can be generated between two geoms. + +If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +indicates that the geoms have a positive margin. + .. _mj_collision: `mj_collision <#mj_collision>`__ diff --git a/doc/changelog.rst b/doc/changelog.rst index 451b8d0c..03b768f7 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,7 +7,8 @@ Upcoming version (not yet released) General ^^^^^^^ - +- Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by + two geoms. - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. - Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 08eea043..07f9adb5 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1718,9 +1718,36 @@ work, but it pays off at runtime and yields both faster and more stable simulati Pair-wise colliders ^^^^^^^^^^^^^^^^^^^ -The table below provides information about the colliders used for different geom pairs. The second row in each cell -lists the maximum number of contacts generated, possibly with ``multiccd`` enabled. For example, ``Mesh`` / ``Mesh`` -will generate up to 1 contact or with ``multiccd`` up to 4 contacts. +The table below provides information about the colliders used for different geom pairs. These values can be computed +dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the +parameters :ref:`nativeccd`, :ref:`multiccd`, and +:ref:`margin`. + +.. raw:: html + +
+
+ + nativeccd +
+
+ + multiccd +
+
+ + with margin +
+
.. list-table:: :header-rows: 1 @@ -1744,7 +1771,7 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | primitive | **1** - | primitive - | **2** + | **4** - | primitive | **4** - | primitive @@ -1787,12 +1814,28 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. | **2** - | CCD | **1** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | primitive | **2** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Ellipsoid @@ -1812,12 +1855,36 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Box @@ -1827,8 +1894,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - | primitive | **8** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Mesh @@ -1837,8 +1912,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | MeshSDF | :ref:`sdf_initpoints ` * - SDF @@ -1851,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | SDF | :ref:`sdf_initpoints ` +.. raw:: html + + + + + .. _Sleeping: Sleeping islands diff --git a/doc/css/theme_overrides.css b/doc/css/theme_overrides.css index 45f94131..a7318056 100644 --- a/doc/css/theme_overrides.css +++ b/doc/css/theme_overrides.css @@ -54,6 +54,98 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) { font-size: 85%; } +.pairwise-toggles { + display: flex; + align-items: center; + gap: 1.5em; + margin-bottom: 0.75em; +} + +.pairwise-toggle-item { + display: flex; + align-items: center; + gap: 0.5em; +} + +.pairwise-switch { + position: relative; + display: inline-block; + width: 36px; + height: 20px; +} + +.pairwise-switch input { + opacity: 0; + width: 0; + height: 0; +} + +.pairwise-slider { + position: absolute; + cursor: pointer; + inset: 0; + background-color: #ccc; + transition: 0.3s; + border-radius: 20px; +} + +.pairwise-slider:before { + content: ""; + position: absolute; + height: 14px; + width: 14px; + left: 3px; + bottom: 3px; + background-color: white; + transition: 0.3s; + border-radius: 50%; +} + +.pairwise-switch input:checked + .pairwise-slider { + background-color: var(--secondary-header-color, #123693); +} + +.pairwise-switch input:checked + .pairwise-slider:before { + transform: translateX(16px); +} + +.multiccd-off, +.multiccd-native, +.multiccd-legacy { + display: none; + margin: 0; +} + +.multiccd-off { + display: inline; +} + +.multiccd-enabled .multiccd-off { + display: none; +} + +.multiccd-enabled.nativeccd-enabled:not(.margin-enabled) .multiccd-native { + display: inline; +} + +.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy, +.multiccd-enabled.nativeccd-enabled.margin-enabled .multiccd-legacy { + display: inline; +} + +.margin-show { + display: none; +} + +.margin-enabled .margin-hide { + display: none; +} + +.margin-enabled .margin-show { + display: inline; +} + + .small-centered td, .small-centered th, .table-pairwise td, .table-pairwise th { text-align: center !important; diff --git a/doc/includes/references.h b/doc/includes/references.h index 1e1db78d..4d55e8ba 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3272,6 +3272,7 @@ void mj_passive(const mjModel* m, mjData* d); void mj_subtreeVel(const mjModel* m, mjData* d); void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); void mj_rnePostConstraint(const mjModel* m, mjData* d); +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); void mj_collision(const mjModel* m, mjData* d); void mj_makeConstraint(const mjModel* m, mjData* d); void mj_island(const mjModel* m, mjData* d); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index fa67e0bf..31954cfb 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -460,6 +460,11 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); // RNE with complete data: compute cacc, cfrc_ext, cfrc_int. MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d); +// Return the maximum number of contacts that can be generated between two geoms. +// If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin. +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // Run collision detection. MJAPI void mj_collision(const mjModel* m, mjData* d); diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index d6e0c724..03a1be1b 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) { m, d, flg_acc, result.data()); }); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index f05bd8a5..ee5795f4 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -2407,6 +2407,32 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='RNE with complete data: compute cacc, cfrc_ext, cfrc_int.', )), + ('mj_maxContact', + FunctionDecl( + name='mj_maxContact', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='g1', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='g2', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='has_margin', + type=ValueType(name='int'), + ), + ), + doc='Return the maximum number of contacts that can be generated between two geoms. If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 indicates that the geoms have a positive margin.', # pylint: disable=line-too-long + )), ('mj_collision', FunctionDecl( name='mj_collision', diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index b77164f6..ca271e5a 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -38,7 +38,7 @@ #include "engine/engine_util_spatial.h" -// table of pair-wise collision functions +// table of pairwise collision functions mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */ /*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_PlaneConvex}, @@ -56,6 +56,104 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { //------------------------------------ utility functions ------------------------------------------ + +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) { + int type1 = m->geom_type[g1]; + int type2 = m->geom_type[g2]; + + if (type1 == mjGEOM_SDF || type2 == mjGEOM_SDF) { + return m->opt.sdf_initpoints; + } + + if (type1 == mjGEOM_HFIELD || type2 == mjGEOM_HFIELD) { + int type = (type1 == mjGEOM_HFIELD) ? type2 : type1; + return (type != mjGEOM_PLANE && type != mjGEOM_HFIELD) ? mjMAXCONPAIR : 0; + } + + // spheres and ellipsoids always generate a single contact + if (type1 == mjGEOM_SPHERE || type1 == mjGEOM_ELLIPSOID || + type2 == mjGEOM_SPHERE || type2 == mjGEOM_ELLIPSOID) { + return 1; + } + + // box-box primitive collider + if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { + return 8; + } + + // capsule-capsule primitive collider + if (type1 == mjGEOM_CAPSULE && type2 == mjGEOM_CAPSULE) { + return 2; + } + + // capsule-box primitive collider + if ((type1 == mjGEOM_CAPSULE && type2 == mjGEOM_BOX) || + (type1 == mjGEOM_BOX && type2 == mjGEOM_CAPSULE)) { + return 4; + } + + // the remaining plane cases + if (type1 == mjGEOM_PLANE || type2 == mjGEOM_PLANE) { + int type = (type1 == mjGEOM_PLANE) ? type2 : type1; + switch (type) { + case mjGEOM_CAPSULE: + return 2; + case mjGEOM_CYLINDER: + case mjGEOM_BOX: + return 4; + case mjGEOM_MESH: + return 3; + default: + return 0; + } + } + + int is_multiccd = !mjDISABLED(mjDSBL_MULTICCD); + if (!is_multiccd) { + return 1; + } + + if (type1 == mjGEOM_CAPSULE || type2 == mjGEOM_CAPSULE || + type1 == mjGEOM_CYLINDER || type2 == mjGEOM_CYLINDER) { + return 5; + } + + if (mjDISABLED(mjDSBL_NATIVECCD)) { + return is_multiccd ? 5 : 1; // mesh-mesh or mesh-box with libccd + } + + // check margin from model + if (has_margin < 0) { + has_margin = 0; + if (mjENABLED(mjENBL_OVERRIDE)) { + has_margin = m->opt.o_margin > 0.0; + } else { + int npair = m->npair; + int ipair = -1; + for (int k=0; k < npair; k++) { + if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) || + (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) { + ipair = k; + break; + } + } + + if (ipair > -1) { + has_margin = m->pair_margin[ipair] > 0.0; + } else { + has_margin = m->geom_margin[g1] > 0.0 || m->geom_margin[g2] > 0.0; + } + } + } + + // 4 contacts for mesh-mesh or mesh-box without margins, 5 with margins + return has_margin ? 5 : 4; +} + + // move arena pointer back to the end of the contact array static inline void resetArena(mjData* d) { d->parena = d->ncon * sizeof(mjContact); diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index 9583cf5f..e231eaaf 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -26,10 +27,15 @@ extern "C" { // collision function pointers and max contact pairs MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // collision detection entry point MJAPI void mj_collision(const mjModel* m, mjData* d); -// applies Separating Axis Theorem for rotated AABBs +// apply the Separating Axis Theorem for rotated AABBs MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], const mjtNum xpos1[3], const mjtNum xmat1[9], const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin, diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 0db2556d..aef1d9fe 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) { mj_deleteModel(m); } +TEST_F(MjCollisionTest, MaxContact) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); + int box = mj_name2id(m, mjOBJ_GEOM, "box"); + int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); + int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); + int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); + int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); + int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); + + EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); + EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); + EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); + EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); + EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); + EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d5ea2b37..cb4c402f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6951,6 +6951,9 @@ public static unsafe extern void mj_rne(mjModel_* m, mjData_* d, int flg_acc, do [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_rnePostConstraint(mjModel_* m, mjData_* d); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_maxContact(mjModel_* m, int g1, int g2, int has_margin); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_collision(mjModel_* m, mjData_* d); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index dfe18aea..ae618fd8 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -8845,6 +8845,10 @@ void mj_makeM_wrapper(const MjModel& m, MjData& d) { mj_makeM(m.get(), d.get()); } +int mj_maxContact_wrapper(const MjModel& m, int g1, int g2, int has_margin) { + return mj_maxContact(m.get(), g1, g2, has_margin); +} + void mj_mulJacTVec_wrapper(const MjModel& m, const MjData& d, const val& res, const NumberArray& vec) { UNPACK_VALUE(mjtNum, res); UNPACK_ARRAY(mjtNum, vec); @@ -13173,6 +13177,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_local2Global", &mj_local2Global_wrapper); function("mj_makeConstraint", &mj_makeConstraint_wrapper); function("mj_makeM", &mj_makeM_wrapper); + function("mj_maxContact", &mj_maxContact_wrapper); function("mj_mulJacTVec", &mj_mulJacTVec_wrapper); function("mj_mulJacVec", &mj_mulJacVec_wrapper); function("mj_mulM", &mj_mulM_wrapper); From da01bd37a2666630629abf14806d2dd177ea9164 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Tue, 21 Apr 2026 06:59:17 -0700 Subject: [PATCH 24/29] Remove modelfiledir_ from compiled assets, use owning spec modelfiledir instead. Previously specs that were attached to some parent spec would resolve its asset paths relative to the modelfiledir of the parent spec. This means path resolution would change depending on the source of the parent spec. Instead, this change makes asset file path resolution relative to the "owning spec" i.e. the spec where the asset was created. This enables workflows such as loading a parent spec via resource provider, then loading a child spec via `from_zip` or in memory providing `spec.assets` and resolution will work as intended. PiperOrigin-RevId: 903207910 Change-Id: Ia58020ab372a3ceadf31e804d145e2ae53d8e5f9 --- doc/changelog.rst | 7 +++ src/user/user_mesh.cc | 20 ++------ src/user/user_objects.cc | 20 ++------ src/user/user_objects.h | 12 ----- test/user/user_api_test.cc | 99 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 114 insertions(+), 44 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 03b768f7..ff3091c8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -23,6 +23,13 @@ General **Migration:** The flag :ref:`multiccd` must be explicitly disabled. +Bug fixes +^^^^^^^^^ + +- Asset paths in attached child specs are now resolved relative to the model file directory of the child spec, rather + than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child + spec. + Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index ae54b634..a7b7dc04 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -300,9 +300,6 @@ void mjCMesh::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } if (!plugin_instance_name.empty()) { plugin_instance_name = m->prefix + plugin_instance_name + m->suffix; } @@ -712,17 +709,14 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } - // remove path from file if necessary if (model->strippath) { file_ = mjuu_strippath(file_); } + mjSpec* owning_spec = model->FindSpec(compiler); FilePath filename = meshdir_ + FilePath(file_); - resource_ = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + resource_ = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); // try loading from cache if (cache != nullptr && LoadCachedMesh(cache, resource_)) { @@ -2957,9 +2951,6 @@ void mjCSkin::NameSpace(const mjCModel* m) { for (auto& name : spec_bodyname_) { name = m->prefix + name + m->suffix; } - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -3046,15 +3037,12 @@ void mjCSkin::Compile(const mjVFS* vfs) { throw mjCError(this, "Unknown skin file type: %s", file_.c_str()); } - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); FilePath filename = meshdir_ + FilePath(file_); - mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); try { LoadSKN(resource); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index f83bd387..75a7e679 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4668,9 +4668,6 @@ void mjCHField::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -4798,15 +4795,12 @@ void mjCHField::Compile(const mjVFS* vfs) { throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str()); } - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); FilePath filename = meshdir_ + FilePath(file_); - mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); struct CachedHField { int nrow, ncol; @@ -4965,9 +4959,6 @@ void mjCTexture::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -5388,7 +5379,8 @@ void mjCTexture::LoadFlip(std::string filename, const mjVFS* vfs, } // try loading from cache - mjResource* resource = LoadResource(modelfiledir_.Str(), filename, vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename, vfs); if (cache && cache->PopulateData(GetCacheId(resource, asset_type), resource, callback)) { mju_closeResource(resource); return; @@ -5640,10 +5632,6 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) { void mjCTexture::Compile(const mjVFS* vfs) { CopyFromSpec(); - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath texturedir_; texturedir_ = FilePath(mjs_getString(compiler->texturedir)); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5c81e40c..4f4da58d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1122,9 +1122,6 @@ class mjCMesh_ : public mjCBase { // octree mjCOctree octree_; // octree of the mesh - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCMesh: public mjCMesh_, private mjsMesh { @@ -1336,9 +1333,6 @@ class mjCSkin_ : public mjCBase { int matid; // material id std::vector bodyid; // body ids - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCSkin: public mjCSkin_, private mjsSkin { @@ -1391,9 +1385,6 @@ class mjCHField_ : public mjCBase { std::string spec_file_; std::string spec_content_type_; std::vector spec_userdata_; - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCHField : public mjCHField_, private mjsHField { @@ -1442,9 +1433,6 @@ class mjCTexture_ : public mjCBase { std::string spec_file_; std::string spec_content_type_; std::vector spec_cubefiles_; - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCTexture : public mjCTexture_, private mjsTexture { diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 3c7af426..57e889bb 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include // NOLINT #include #include @@ -32,6 +33,7 @@ #include "src/cc/array_safety.h" #include #include +#include #include "src/xml/xml_api.h" #include "src/xml/xml_numeric_format.h" #include "test/fixture.h" @@ -203,6 +205,103 @@ TEST_F(MujocoTest, AttachAndChildDeletion) { mj_deleteSpec(parent_spec); } +int open_mock(mjResource* resource) { + static const char parent_xml[] = R"( + + + + + + )"; + resource->data = mju_malloc(sizeof(parent_xml)); + std::strcpy((char*)resource->data, parent_xml); + return 1; +} + +int read_mock(mjResource* resource, const void** buffer) { + *buffer = resource->data; + return std::strlen((const char*)resource->data); +} + +void close_mock(mjResource* resource) { + mju_free(resource->data); + resource->data = nullptr; +} + +TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) { + // This test checks that when we attach a child spec to a parent spec that was + // loaded from a resource provider, the child spec does not inherit the + // resource URI from the parent. This allows the child spec to specify assets + // relative to its model file or in the VFS. + mjpResourceProvider provider = { + .prefix = "fakeprovider", + .open = open_mock, + .read = read_mock, + .close = close_mock, + }; + + mjp_registerResourceProvider(&provider); + + std::array err; + mjSpec* parent_spec = + mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size()); + mjs_setString(parent_spec->modelname, "parent"); + ASSERT_THAT(parent_spec, NotNull()) << err.data(); + + // Create child spec + static constexpr char child_xml[] = R"( + + + + + + + + + + + )"; + + // Setup VFS with asset + mjVFS vfs; + mj_defaultVFS(&vfs); + static constexpr char asset_data[] = R"( + v 0 0 0 + v 1 0 0 + v 0 1 0 + v 0 0 1 + f 1 2 3 + f 1 2 4 + f 2 3 4 + f 3 1 4 + )"; + mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data)); + + mjSpec* child_spec = + mj_parseXMLString(child_xml, &vfs, err.data(), err.size()); + mjs_setString(child_spec->modelname, "child"); + ASSERT_THAT(child_spec, NotNull()) << err.data(); + + // Attach child spec to parent spec's world body + mjsBody* world = mjs_findBody(parent_spec, "world"); + ASSERT_THAT(world, NotNull()); + + mjsElement* attached = + mjs_attach(world->element, child_spec->element, "", ""); + ASSERT_THAT(attached, NotNull()); + + mjModel* model = mj_compile(parent_spec, &vfs); + mj_deleteVFS(&vfs); + + EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec); + + if (model) { + mj_deleteModel(model); + } + mj_deleteSpec(parent_spec); + mj_deleteSpec(child_spec); +} + TEST_F(MujocoTest, ActivatePlugin) { mjSpec* spec = mj_makeSpec(); mjs_activatePlugin(spec, "mujoco.elasticity.cable"); From ba149aa043718f6b2018804e27e4f96674b25fc3 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Apr 2026 08:01:32 -0700 Subject: [PATCH 25/29] Fix flexcomp strain constraints with rotated grids. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The reference node positions and the positions used for computing stiffness eigenvectors were previously stored in world frame. However, the runtime expects these quantities in the unrotated local frame. This caused non-zero constraint residuals and simulation instability when the grid was rotated — either by the parent body's initial orientation, or by the flexcomp's own frame attributes. Rather than tracking each rotation source individually, this change extracts the total grid rotation directly from the cell geometry. All node positions are then un-rotated before computing the stiffness matrix. PiperOrigin-RevId: 903232388 Change-Id: If877af89025ce1e61a76b38c29403d593d892749 --- src/user/user_mesh.cc | 83 +++++++++++- src/user/user_objects.h | 3 +- test/engine/engine_core_constraint_test.cc | 140 ++++++++++++++++++++- 3 files changed, 220 insertions(+), 6 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a7b7dc04..212381c8 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4251,6 +4251,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } + // compute unrotated node positions for stiffness computation + std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos); + // reorder tetrahedra so right-handed face orientation is outside // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) if (dim == 3) { @@ -4410,7 +4413,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { 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.data() + 3*global, 3); + mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3); local++; } } @@ -4453,14 +4456,88 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } - // store node cartesian positions + // store node positions in unrotated (body-local) frame + // this ensures the runtime displacement refpos - R^{-1}*x is zero at rest node0_.assign(3*nnode, 0); for (int i=0; i < nnode; i++) { - mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3); + mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3); } } +// compute unrotated node positions for stiffness computation and node0_ +// +// the runtime corotational code extracts rotation R from the deformation +// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0 +// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get +// zero displacement at rest; additionally, the stiffness eigenvectors must +// be computed from axis-aligned positions to preserve the diagonal Jacobian +// assumption in ComputeLinearStiffness. +std::vector mjCFlex::ComputeUnrotatedNodePositions( + const std::vector& nodexpos) const { + std::vector nodexpos_local(3*nnode); + if (interpolated && nnode > 0) { + int ny_global = spec.cellcount[1] * spec.order + 1; + int nz_global = spec.cellcount[2] * spec.order + 1; + + // find first non-empty cell + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + int ref_ci = 0, ref_cj = 0, ref_ck = 0; + bool found = false; + for (int ci = 0; ci < cx && !found; ci++) { + for (int cj = 0; cj < cy && !found; cj++) { + for (int ck = 0; ck < cz && !found; ck++) { + int cell_idx = ci * cy * cz + cj * cz + ck; + if (cell_empty.empty() || !cell_empty[cell_idx]) { + ref_ci = ci; ref_cj = cj; ref_ck = ck; + found = true; + } + } + } + } + + // corner indices of the reference cell (order=1 corners at local 0,0,0 + // and at offsets along each parametric axis) + int g000 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g010 = (ref_ci * spec.order) * ny_global * nz_global + + ((ref_cj * spec.order) + spec.order) * nz_global + + (ref_ck * spec.order); + int g001 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + ((ref_ck * spec.order) + spec.order); + + // edge vectors (columns of the deformation gradient F = R * S) + // we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1} + double R0[9]; + for (int d = 0; d < 3; d++) { + R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d]; + R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d]; + R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d]; + } + + // normalize to get rotation matrix columns (valid for regular grids) + double li = mjuu_normvec(R0+0, 3); + double lj = mjuu_normvec(R0+3, 3); + double lk = mjuu_normvec(R0+6, 3); + (void)li; (void)lj; (void)lk; + + // apply inverse rotation to each nodexpos to get local-frame positions + for (int i = 0; i < nnode; i++) { + const double* p = nodexpos.data() + 3*i; + double* q = nodexpos_local.data() + 3*i; + mjuu_mulvecmat(q, p, R0); + } + } else { + nodexpos_local = nodexpos; + } + return nodexpos_local; +} + // create flex BVH void mjCFlex::CreateBVH() { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 4f4da58d..bb3bbed5 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1054,7 +1054,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions - + // compute unrotated node positions for stiffness computation + std::vector ComputeUnrotatedNodePositions(const std::vector& nodexpos) const; // stiffness caching std::string ComputeStiffnessCacheKey() const; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 93e3b947..07204df8 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -15,8 +15,6 @@ // Tests for engine/engine_core_constraint.c. #include -#include -#include #include #include @@ -914,5 +912,143 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { } } +// --------------------------- strain constraint rotated parent ---------------- + +struct StrainConstraintTestCase { + std::string test_name; + std::string body_pos; + std::string body_quat; + std::string flex_spacing; + std::string flex_xyaxes; +}; + +class StrainConstraintRotatedTest : public CoreConstraintTest, + public ::testing::WithParamInterface< + StrainConstraintTestCase> { +}; + +TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { + auto param = GetParam(); + std::string xml = R"( + + + )"; + + std::array error; + mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // Check we have strain constraints + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // The critical check: constraint residuals must be ~0 at the initial + // (undeformed) configuration, even though the body is rotated. + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i])); + } + EXPECT_LT(max_pos, 1e-6) + << "Strain constraint residual should be ~0" + << " (max_pos=" << max_pos << ")"; + + // Verify stability + for (int i = 0; i < 200; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i + << ", qvel[" << j << "]=" << d->qvel[j]; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +INSTANTIATE_TEST_SUITE_P( + StrainConstraintRotatedTests, StrainConstraintRotatedTest, + testing::ValuesIn({ + // Test strain constraint with a rotated parent body. + // The flexcomp is placed inside a parent body that has a non-identity + // initial rotation. This reproduces the "grocery scene" bug where the + // stiffness matrix eigenvectors and reference positions were computed + // in world frame instead of the unrotated local frame, causing + // spurious constraint forces. + { + "RotatedParent", + "1 2 3", + "0.707107 0 0.707107 0", + ".1 .1 .1", + "" + }, + // Same test with an anisotropic box (different spacing per axis) and + // arbitrary rotation (combined 45-deg Y + 30-deg X). + { + "RotatedParentAnisotropic", + "0.5 -1 2", + "0.8924 0.2392 0.3696 -0.0990", + ".15 .08 .05", + "" + }, + // Test strain constraint with flexcomp-level xyaxes rotation. + // This is the "grocery scene" pattern where the flexcomp grid itself is + // rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z). + { + "FlexcompXyaxes", + "", + "", + ".1 .02 .1", + "0 1 0 0 0 1" + }, + // Test combining parent body rotation with flexcomp xyaxes rotation. + // The total rotation is the composition of both. + { + "RotatedParentPlusXyaxes", + "1 2 3", + "0.707107 0 0.707107 0", + ".15 .08 .05", + "0 1 0 0 0 1" + } + }), + [](const testing::TestParamInfo< + StrainConstraintRotatedTest::ParamType>& info) { + return info.param.test_name; + } +); + } // namespace } // namespace mujoco From c3fddf5dc150745a43cede9a3646568fed2c14e6 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 21 Apr 2026 10:02:57 -0700 Subject: [PATCH 26/29] Update doc/mjwarp/update_types.py PiperOrigin-RevId: 903289569 Change-Id: I7e05736c54fa72dc0c89152f49cf3d9ee6e5492d --- doc/mjwarp/update_types.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/mjwarp/update_types.py b/doc/mjwarp/update_types.py index a19a0b61..316b2d2e 100644 --- a/doc/mjwarp/update_types.py +++ b/doc/mjwarp/update_types.py @@ -29,7 +29,7 @@ This script updates such instances with valid types @dataclasses.dataclass class Option: ... - timestep: wp.array(dtype=float) + timestep: wp.array[float] ... """ @@ -48,13 +48,13 @@ def replace_array_calls(match): dtype = args[-1] if n_args == 2: - return f'wp.array(dtype={dtype})' + return f'wp.array[{dtype}]' elif n_args == 3: - return f'wp.array2d(dtype={dtype})' + return f'wp.array2d[{dtype}]' elif n_args == 4: - return f'wp.array3d(dtype={dtype})' + return f'wp.array3d[{dtype}]' elif n_args == 5: - return f'wp.array4d(dtype={dtype})' + return f'wp.array4d[{dtype}]' else: return match.group(0) From 4cfebcc32bfa4cf6b7577a4a0fe14c1dbc06d817 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 22 Apr 2026 05:26:33 -0700 Subject: [PATCH 27/29] Add mj_containsFileVFS and mj_containsBufferVFS functions. PiperOrigin-RevId: 903785790 Change-Id: I013b37a177284f8440179c4ae5c6221e0f572b49 --- doc/APIreference/functions.rst | 18 +++++++++++ doc/changelog.rst | 1 + doc/includes/references.h | 2 ++ include/mujoco/mujoco.h | 6 ++++ python/mujoco/introspect/functions.py | 46 +++++++++++++++++++++++++++ src/user/user_vfs.cc | 37 +++++++++++++++++++++ src/user/user_vfs.h | 6 ++++ test/user/user_vfs_test.cc | 32 +++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 6 ++++ wasm/codegen/generators/constants.py | 2 ++ 10 files changed, 156 insertions(+) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 2e83a385..fc513e20 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1519,6 +1519,24 @@ Add file to VFS from buffer; return 0: success, 2: repeated name, -1: failed to Delete file from VFS; return 0: success, -1: not found in VFS. +.. _mj_containsBufferVFS: + +`mj_containsBufferVFS <#mj_containsBufferVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_containsBufferVFS + +Check if buffer exists in VFS; return 1: exists, 0: not found. + +.. _mj_containsFileVFS: + +`mj_containsFileVFS <#mj_containsFileVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_containsFileVFS + +Check if file exists in VFS; return 1: exists, 0: not found. + .. _mj_deleteVFS: `mj_deleteVFS <#mj_deleteVFS>`__ diff --git a/doc/changelog.rst b/doc/changelog.rst index ff3091c8..e3f311b2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,7 @@ General ^^^^^^^ - Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by two geoms. +- Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. - Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the diff --git a/doc/includes/references.h b/doc/includes/references.h index 4d55e8ba..4d767933 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3161,6 +3161,8 @@ int mj_unmountVFS(mjVFS* vfs, const char* filename); int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +int mj_containsBufferVFS(mjVFS* vfs, const char* name); +int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename); void mj_deleteVFS(mjVFS* vfs); size_t mj_getCacheSize(const mjCache* cache); size_t mj_getCacheCapacity(const mjCache* cache); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 31954cfb..2bd0cae8 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -94,6 +94,12 @@ MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int // Delete file from VFS; return 0: success, -1: not found in VFS. MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +// Check if buffer exists in VFS; return 1: exists, 0: not found. +MJAPI int mj_containsBufferVFS(mjVFS* vfs, const char* name); + +// Check if file exists in VFS; return 1: exists, 0: not found. +MJAPI int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename); + // Delete all files from VFS and deallocates VFS internal memory. MJAPI void mj_deleteVFS(mjVFS* vfs); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index ee5795f4..e16f434f 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -162,6 +162,52 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Delete file from VFS; return 0: success, -1: not found in VFS.', )), + ('mj_containsBufferVFS', + FunctionDecl( + name='mj_containsBufferVFS', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='vfs', + type=PointerType( + inner_type=ValueType(name='mjVFS'), + ), + ), + FunctionParameterDecl( + name='name', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Check if buffer exists in VFS; return 1: exists, 0: not found.', + )), + ('mj_containsFileVFS', + FunctionDecl( + name='mj_containsFileVFS', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='vfs', + type=PointerType( + inner_type=ValueType(name='mjVFS'), + ), + ), + FunctionParameterDecl( + name='directory', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + FunctionParameterDecl( + name='filename', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Check if file exists in VFS; return 1: exists, 0: not found.', + )), ('mj_deleteVFS', FunctionDecl( name='mj_deleteVFS', diff --git a/src/user/user_vfs.cc b/src/user/user_vfs.cc index 11d35c71..60c5c945 100644 --- a/src/user/user_vfs.cc +++ b/src/user/user_vfs.cc @@ -236,6 +236,24 @@ VFS::Status VFS::Unmount(const FilePath& path) { return kInvalidResourceProvider; } +bool VFS::ContainsBuffer(const char* name) { + if (name == nullptr) { + return false; + } + std::lock_guard lock(mutex_); + return mounts_.contains(name); +} + +bool VFS::ContainsFile(const char* directory, const char* filename) { + if (filename == nullptr) { + return false; + } + mujoco::user::FilePath path(directory ? directory : "", filename); + std::string key = path.StripPath().Lower().Str(); + std::lock_guard lock(mutex_); + return mounts_.contains(key); +} + int VFS::Read(mjResource* resource, const void** buffer) { if (resource && resource->provider && resource->provider->read) { return resource->provider->read(resource, buffer); @@ -498,3 +516,22 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { } return mujoco::user::VFS::kSuccess; } + +int mj_containsBufferVFS(mjVFS* vfs, const char* name) { + mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs); + if (impl == nullptr) { + mju_error("mjVFS is null."); + return -1; + } + return impl->ContainsBuffer(name); +} + +int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename) { + mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs); + if (impl == nullptr) { + mju_error("mjVFS is null."); + return -1; + } + return impl->ContainsFile(directory, filename); +} + diff --git a/src/user/user_vfs.h b/src/user/user_vfs.h index cfa2694c..6bef85c2 100644 --- a/src/user/user_vfs.h +++ b/src/user/user_vfs.h @@ -90,6 +90,12 @@ class VFS { // Unmounts the ResourceProvider from the given path. Status Unmount(const FilePath& path); + // Returns true if the VFS contains a buffer with the given name. + bool ContainsBuffer(const char* name); + + // Returns true if the VFS contains a file with the given name. + bool ContainsFile(const char* directory, const char* filename); + // Sets a destructor to be called when the VFS has no more open resources. // Assumes that `destructor` will delete `this`. // diff --git a/test/user/user_vfs_test.cc b/test/user/user_vfs_test.cc index db5a22b5..16a62da2 100644 --- a/test/user/user_vfs_test.cc +++ b/test/user/user_vfs_test.cc @@ -224,6 +224,38 @@ TEST_F(UserVfsTest, DeleteFileRepeat) { mj_deleteVFS(&vfs); } +TEST_F(UserVfsTest, ContainsBuffer) { + mjVFS vfs; + mj_defaultVFS(&vfs); + std::string buffer = ""; + const void* ptr = static_cast(buffer.c_str()); + mj_addBufferVFS(&vfs, "model", ptr, buffer.size()); + + EXPECT_TRUE(mj_containsBufferVFS(&vfs, "model")); + EXPECT_FALSE(mj_containsBufferVFS(&vfs, "nonexistent")); + EXPECT_FALSE(mj_containsBufferVFS(&vfs, "Model")); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, ContainsFile) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file.c_str()); + + EXPECT_TRUE(mj_containsFileVFS(&vfs, dir.c_str(), file.c_str())); + EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, (dir + file).c_str())); + EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, "Activation.xml")); + EXPECT_TRUE(mj_containsFileVFS(&vfs, "some/dir/", "activation.xml")); + EXPECT_FALSE(mj_containsFileVFS(&vfs, nullptr, "nonexistent.xml")); + + mj_deleteVFS(&vfs); +} + TEST_F(UserVfsTest, AddBuffer) { mjVFS vfs; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index cb4c402f..e3bd9fe9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6684,6 +6684,12 @@ public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedT [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_containsBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_containsFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string directory, [MarshalAs(UnmanagedType.LPStr)]string filename); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_deleteVFS(void* vfs); diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index 1a8aaeee..111c9997 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -124,6 +124,8 @@ _SKIPPED_ASSET_CACHE_FUNCTIONS: tuple[str, ...] = ( _SKIPPED_VFS_FUNCTIONS: tuple[str, ...] = ( # go/keep-sorted start "mj_addFileVFS", + "mj_containsBufferVFS", + "mj_containsFileVFS", "mj_mountVFS", "mj_unmountVFS", # go/keep-sorted end From 863a084d7f18867c674338ff795ace1db210592d Mon Sep 17 00:00:00 2001 From: Matej Aleksandrov Date: Wed, 22 Apr 2026 06:33:10 -0700 Subject: [PATCH 28/29] Adjust refcount expectations for Python 3.14 PiperOrigin-RevId: 903813225 Change-Id: Ieb529f2a850e697b643e5023554341c0718ac0e1 --- python/mujoco/bindings_test.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index c4b550d1..8df15728 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1216,13 +1216,15 @@ Euler integrator, semi-implicit in velocity. mujoco.set_mjcb_control(lambda m, d: None) mujoco.mj_step(model_instances[-1], data_instances[-1]) mujoco.set_mjcb_control(None) + # Reference counting changed in Python 3.14. + expected_refcount = 2 if sys.version_info < (3, 14) else 1 while data_instances: d = data_instances.pop() - self.assertEqual(sys.getrefcount(d), 2) + self.assertEqual(sys.getrefcount(d), expected_refcount) del d while model_instances: m = model_instances.pop() - self.assertEqual(sys.getrefcount(m), 2) + self.assertEqual(sys.getrefcount(m), expected_refcount) # This test is disabled on PyPy as it uses sys.getrefcount # However PyPy is not officially supported by MuJoCo @@ -1236,7 +1238,9 @@ Euler integrator, semi-implicit in velocity. # passed to getrefcount. self.assertEqual(sys.getrefcount(data.model), 3) del data - self.assertEqual(sys.getrefcount(model), 2) + # Reference counting changed in Python 3.14. + expected_refcount = 2 if sys.version_info < (3, 14) else 1 + self.assertEqual(sys.getrefcount(model), expected_refcount) def test_can_initialize_mjv_structs(self): self.assertIsInstance(mujoco.MjvScene(), mujoco.MjvScene) From a891782553abf2d433a91415b53b97a5456490be Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 22 Apr 2026 08:00:42 -0700 Subject: [PATCH 29/29] Clean-up flex assumptions. Do not allow a mix of `elastic2d != none` with `dof = trilinear` since the latter assumes 3d elasticity. Also, do not assume that `flex_interp > 0` in the engine. This will enable to use, e.g., `flex_interp = -1` to mean a linear surface finite element instead of a 3d finite element which is currently identified with `flex_interp = 1`. PiperOrigin-RevId: 903852035 Change-Id: Ia6290b4a05e9e510ffb7f36d141cd525b40d3110 --- doc/XMLreference.rst | 2 +- model/flex/bunny.xml | 2 +- model/flex/bunny_multicell.xml | 2 +- model/flex/bunny_quadratic.xml | 2 +- model/flex/bunny_with_uv.xml | 2 +- src/engine/engine_core_constraint.c | 4 +++ src/engine/engine_core_smooth.c | 6 ++-- src/engine/engine_derivative.c | 2 ++ src/engine/engine_passive.c | 1 + src/engine/engine_setconst.c | 1 + src/engine/engine_vis_interact.c | 1 + src/engine/engine_vis_visualize.c | 1 + src/user/user_mesh.cc | 16 +++++++-- test/user/user_mesh_test.cc | 52 +++++++++++++++++++++++++++++ 14 files changed, 84 insertions(+), 10 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index f01541e4..1bc3c846 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4328,7 +4328,7 @@ stress-strain relationship. See also :ref:`deformable ` 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. + "both": bending and stretching. Not yet supported by :ref:`dof` **trilinear** and **quadratic**. .. _flex-contact: diff --git a/model/flex/bunny.xml b/model/flex/bunny.xml index ec31684e..0d10cad9 100644 --- a/model/flex/bunny.xml +++ b/model/flex/bunny.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_multicell.xml b/model/flex/bunny_multicell.xml index a77f5376..730cbc5d 100644 --- a/model/flex/bunny_multicell.xml +++ b/model/flex/bunny_multicell.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_quadratic.xml b/model/flex/bunny_quadratic.xml index 5b98759f..59e255aa 100644 --- a/model/flex/bunny_quadratic.xml +++ b/model/flex/bunny_quadratic.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_with_uv.xml b/model/flex/bunny_with_uv.xml index c3cde418..9de1d8c2 100644 --- a/model/flex/bunny_with_uv.xml +++ b/model/flex/bunny_with_uv.xml @@ -37,7 +37,7 @@ - + diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 0f8f4a00..39996c33 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -275,6 +275,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, } int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell // cell lookup: get local coords and node indices @@ -709,6 +710,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { int f = id[0]; int nodenum = m->flex_nodenum[f]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; // skip if not interpolated (order == 0 or no nodes) if (!order || !nodenum) { @@ -1676,6 +1678,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) { 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); // per-cell constraint count @@ -2296,6 +2299,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { // per-cell strain constraints: each equality is one cell int f = id[0]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; if (!order || !m->flex_nodenum[f]) { break; } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 6b052147..164a6527 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -595,7 +595,8 @@ void mj_flex(const mjModel* m, mjData* d) { } } - int order = m->flex_interp[f]; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; @@ -2624,7 +2625,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { case mjEQ_FLEXSTRAIN: { // increment: trilinear uses 2 center (I1,J-1) + 3*ngauss shear, quadratic uses 6*ngauss k = m->eq_obj1id[id]; - int order = m->flex_interp[k]; + int interp_k = m->flex_interp[k]; + int order = interp_k < 0 ? -interp_k : interp_k; int nodenum = m->flex_nodenum[k]; if (order && nodenum) { int nquad = order + 1; diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 875fb70c..60c80073 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -887,6 +887,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, if (!m->flex_interp[f]) continue; if (m->flex_rigid[f]) continue; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); if (npc > max_npc) max_npc = npc; if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; @@ -966,6 +967,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } int order = m->flex_interp[f]; + 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]; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 8c76a573..295ec4a7 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -236,6 +236,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { if (m->flex_interp[f]) { int order = m->flex_interp[f]; + 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]; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 24ef9955..754aa29b 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -714,6 +714,7 @@ static void makeFlexBandwidth(mjModel* m, mjData* d) { for (int f = 0; f < m->nflex; f++) { if (!m->flex_interp[f]) continue; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int nodeadr = m->flex_nodeadr[f]; int nodenum = m->flex_nodenum[f]; int cx = m->flex_cellnum[3*f+0]; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index d1ce7ffe..60533e27 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -864,6 +864,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, if (m->flex_interp[i]) { mjtNum* coord = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid); int order = m->flex_interp[i]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // cell lookup: get local coords and node indices diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 327f0108..0e058bd1 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1452,6 +1452,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int NX = cx * order + 1; int NY = cy * order + 1; int NZ = cz * order + 1; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 212381c8..3389a71e 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4100,6 +4100,19 @@ void mjCFlex::Compile(const mjVFS* vfs) { } nelem = (int)elem_.size()/(dim+1); + // elastic2d checks + if (elastic2d) { + 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 (dim != 2 && !interpolated) { + throw mjCError(this, "2d elasticity requires 2d flex"); + } + } + // set nvert, rigid, centered; check size if (vert_.empty()) { centered = true; @@ -4342,9 +4355,6 @@ void mjCFlex::Compile(const mjVFS* vfs) { // bending stiffness (2D only) if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) { - if (thickness < 0) { - throw mjCError(this, "thickness must be positive for bending stiffness"); - } bending.assign(nedge*17, 0); for (unsigned int e = 0; e < nedge; e++) { diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 98944622..59833aef 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -819,6 +819,58 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), + HasSubstr("2d elasticity requires positive thickness")); +} + +TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array 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")); +} + +TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex")); +} + TEST_F(MjCMeshTest, VolumeNegativeThrowsError) { static constexpr char xml[] = R"(