mju_boxQP documents that only the lower triangle of the Hessian H is
read, but the gradient and search-direction updates inside
mju_boxQPoption still called the dense mju_mulMatVec, which reads the
upper triangle as well. This violated the documented contract and
prevented callers from safely leaving the upper triangle uninitialized.
Add a file-local mulMatVecSym helper that computes res = H*vec while
reading only the lower triangle of H (mirroring the convention of the
existing mulVecMatVecSym quadratic-form helper), and use it in place of
mju_mulMatVec in both call sites. Extend the BoxQP test suite with
UpperTrianglePoisoned, which fills the strict upper triangle of H with
NaN and verifies that the solver produces the same result as on the
clean symmetric input.
Reported by @lshdlut.
Fixes#3275
Introduce a fast path in mj_addFlexDerivatives for centered flexes where all nodes are attached to bodies with only 3 translational DOFs and no DOF-contributing ancestors. This allows for a direct scatter of K*vec, avoiding the overhead of sparse Jacobian construction and multiplication.
PiperOrigin-RevId: 916063477
Change-Id: I824728a5bdc3b975b04b46ecf22994922e307a8c
The global node indices stored in face_gidx are now checked to ensure they are within the valid range of node numbers, raising an error if any index is out of bounds.
PiperOrigin-RevId: 916025763
Change-Id: I1d2dcb11603137c337493541066b62b2ed2854c0
In `mj_flexPassive`, precompute and cache face positions, global indices, and corotational quaternions for all faces. This avoids redundant calls to `mju_flexGatherFaceState` for each bending edge, as each face can be part of multiple bending edges. This gives a 3-4x speedup in mj_passive.
PiperOrigin-RevId: 915912918
Change-Id: I3e1c00cf2d989f85398a0750a0faf359249cfe01
Skip flexedge velocity calculation for rigid and interpolated flexes, as they are not used in those cases. The computation is now done per-flex, only for the edges within each non-rigid, non-interpolated flex.
PiperOrigin-RevId: 915907396
Change-Id: I0beb1f98f0115c83da13fed184e89101f57563ab
This gives a 3x speedup in implicitfast.
Also cleanup old code that was used in the dense factorization of the stiffness matrix before we switched to CG.
PiperOrigin-RevId: 915900315
Change-Id: Id6973c4bfd7d371a43ec6db982703969b23a3550
The flex interp stiffness matrix is Negative Semi-Definite (NSD). When forming the RHS for implicit integration, the term involving the velocity and stiffness should be added, not subtracted. A new test is added to ensure energy stability for flex interp stretch stiffness with the implicitfast integrator.
PiperOrigin-RevId: 914845245
Change-Id: Iaaf0914909128e64e195f17cc5f2f344a8a43bc2
Standard flex (flex_interp=0) with thin-plate bending treated bending forces purely explicitly. This caused contact-induced vertex vibrations and non-physical energy injection for flat resting sheets, because the solver treated each vertex as an independent mass during contact and contact normals are orthogonal to stretch constraints.
Fix: extend the existing preconditioned CG solver to include the constant bending stiffness K_bend in the implicit operator via matrix-free mat-vec.
PiperOrigin-RevId: 914774020
Change-Id: I45e0d6749abb6f873566203bccae956514b2576b
with a preconditioned Conjugate Gradient (CG) solver that operates
on the full system matrix.
The previous approach extracted flex DOFs into a reduced banded system,
factored it separately, and overwrote the global solve. This required
precomputed bandwidth (makeFlexBandwidth), parent-joint detection,
coupling corrections, and a FlexInterpContext struct — and only worked
for standalone flex trees without parent joints.
The new CG solver uses the already-factored global system (M - h*qDeriv)
as a preconditioner and adds the flex stiffness contribution via
matrix-free products (mjd_flexInterp_mulKD/mulK). This handles any
kinematic configuration — including flexes attached to articulated
chains or with parent joints — without sparsity pattern restrictions.
Before (`bunny_multicell`):
```
Simulation time : 50.80 s
Steps per second : 197
Realtime factor : 0.20 x
Time per step : 5080.3 µs
CG iters / step : 3.16
Contacts / step : 31.04
Constraints / step : 124.15
Degrees of freedom : 178
Dynamic memory usage : 0.4% of 100M
```
After:
```
Simulation time : 9.52 s
Steps per second : 1051
Realtime factor : 1.05 x
Time per step : 951.7 µs
CG iters / step : 3.21
Contacts / step : 30.90
Constraints / step : 123.61
Degrees of freedom : 178
Dynamic memory usage : 0.3% of 100M
```
PiperOrigin-RevId: 913758038
Change-Id: If5aa617b2d535c86aec9bd71c9e0003a2b38bdd7
The coordinates for flex interpolation are now computed using the absolute values of the vertex weights. The sign of the first vertex weight is then applied to the resulting barycentric weights. This correctly handles cases where the flex is both the first and the second entity in the contact pair.
PiperOrigin-RevId: 913590928
Change-Id: I970b35fba3d209e13b5b33bb5f945e3c6a43d487
The normal jump residual in flex elasticity calculations now rotates the rest-frame normal jump into the current frame using the face's corotational quaternion before subtracting the jump from the current normal difference.
PiperOrigin-RevId: 913242127
Change-Id: Ia62b28ecccd59e79225737d8225ef4131b333c96
- Compute efc_J_rowsuper incrementally in mj_addConstraint instead of post-hoc via mju_superSparse.
- Better exploitation of supernodes in A matrix pipeline: precount and fill skip redundant chain traversals for supernode rows.
- Redundant B_rowsuper computation via mju_superSparse is eliminated (indentical to efc_J_rowsuper).
PiperOrigin-RevId: 910787825
Change-Id: I6eda9996659602b7051ee1090aeedb862603c84e
This change adds a new passive force computation for flexes with elastic2d="bend" and dof="trilinear". The bending energy is based on the squared difference of normals between adjacent face elements at their shared edge midpoint. The edge data is precomputed during model compilation and stored in flex_bending.
PiperOrigin-RevId: 910772638
Change-Id: I3b12c7b7f1ba6ac1875df495d89e8cfec921ca80
Total testspeed runtime for `2humanoids100.xml` reduced by 19.6% (49.5 -> 39.8s).
PiperOrigin-RevId: 910608140
Change-Id: Ided0ae5bdb8e4e8196f84e08354a9ae8cfa5b626
mjSENS_INSIDESITE uses xipos to test containment, but for massless flex
parent bodies xipos equals the static body frame origin and does not
track the actual flex position.
Fix: in the INSIDESITE sensor case, when the object is a massless body
with positive subtree mass (i.e., a flex parent), use subtree_com
instead of xipos. This correctly reflects the mass-weighted centroid of
the flex child bodies without changing the global semantics of xipos.
PiperOrigin-RevId: 908745144
Change-Id: I527f0efb7b419345188411c9da883505156ebeea
The code for computing passive forces for flex elements is moved into new static functions `mj_flexPassiveInterp`, `mj_flexPassiveBend`, and `mj_flexPassiveStretch`. This improves the structure of `mj_springdamper`.
PiperOrigin-RevId: 907671516
Change-Id: I6d1e781bbd370331ae645a954b78092ecb1925b9
When elastic2d="stretch" is set on an interpolated flexcomp, treat the bounding box boundary as membrane elements rather than volumetric cells. This computes plane-stress stiffness over the boundary faces and updates the runtime force/derivative kernels accordingly.
Interior vertex tracking (moving vertices that follow the deforming shell) is not yet implemented so all mesh vertices need to be on the bounding box surface or the background grid should have no interior nodes (i.e. cellcount should be 1 on at least one axis).
PiperOrigin-RevId: 907654080
Change-Id: I51b90e2f6a1d1b036f9604e42de20e377dc5d3f9
Total testspeed runtime for `2humanoids100.xml` reduced by 27.6% (63.4 -> 49.5s) due to early termination on small islands
PiperOrigin-RevId: 906910915
Change-Id: If55ad468c3680ef44eda7000455a77f8003b3122
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
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
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
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
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
The flex interpolation stiffness matrix within the implicit/implicitfast solvers is now built and factorized in a banded format instead of a dense one. This involves:
- Calculating the bandwidth based on the sparsity of the mass/damping matrix and the connectivity within flex cells.
- Allocating and populating a banded matrix `H`.
- Using `mju_cholFactorBand` and `mju_cholSolveBand` for factorization and solving.
This change improves performance for flexes with many DOFs but local coupling.
PiperOrigin-RevId: 901297952
Change-Id: I3efe06353d1903ea65ab30dc49685cede228bb68
This change introduces a `flex_cellcount` field to `mjModel` to specify the number of cells in each dimension for interpolated flexes. The stiffness computation, passive force calculation, and Jacobian derivatives are updated to operate on a per-cell basis, significantly improving performance by localizing computations to the nodes within each cell.
PiperOrigin-RevId: 901216393
Change-Id: Ic23132e609de11e71bb7fef8d1f139daad2ec264
Yuval Tassa