Introduces a new "strain" equality mode (flex_edgeequality = 3) for flexcomps with trilinear interpolation. This mode enforces constraints on the three invariants of the Green-Lagrange strain tensor at 8 Gauss quadrature points within each flex element, resulting in 24 equality constraints per flexcomp.
Also fixes a bug when attaching a flex with constraints. PiperOrigin-RevId: 881349765 Change-Id: I8e6df8239488bfdc87d7e5b473568975e38c737e
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
fe6c81ed93
commit
713b5524bc
+42
-9
@@ -3614,7 +3614,9 @@ saving the XML:
|
||||
for the entire flex, independent of the number of vertices. The positions of the vertices are updated using
|
||||
quadratic interpolation over the bounding box. While this option requires more degrees of freedom than trilinear
|
||||
flexes, it enables curved deformation modes, while the only modes achievable for trilinear flexes are
|
||||
strech/compression and shear.
|
||||
strech/compression and shear. To understand the difference between the two parametrizations, see `a trilinear cube
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/trilinear.xml>`__ and `a quadratic cube
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/quadratic.xml>`__.
|
||||
|
||||
Note that a higher interpolation order generally requires a smaller time step for stability, although usually not as
|
||||
large as with the "full" option and a fine mesh.
|
||||
@@ -3820,10 +3822,12 @@ element is used to adjust the properties of all edges in the flex.
|
||||
|
||||
.. _flexcomp-edge-equality:
|
||||
|
||||
:at:`equality`: :at-val:`[false, true, vert], "false"`
|
||||
The type of equality constraint applied to this edge. If **false**, no equality constraint is applied. If **true**,
|
||||
then edge constraints are enforced. If **vert**, an averaged constraint is used, see
|
||||
:ref:`flexvert<equality-flexvert>`.
|
||||
:at:`equality`: :at-val:`[false, true, vert, strain], "false"`
|
||||
The type of equality constraint applied to this edge. If :at-val:`false`, no equality constraint is applied. If
|
||||
:at-val:`true`, then edge constraints are enforced. If :at-val:`vert`, an averaged constraint is used, see
|
||||
:ref:`flexvert<equality-flexvert>`. if :at-val:`strain`, then a constraint is added to enforce that the invariants of
|
||||
the strain tensor do not change; this is only equality constraint type supported for trilinear and quadratic
|
||||
:ref:`dofs<body-flexcomp-dof>` elements and :ref:`here<equality-flexstrain>`.
|
||||
|
||||
.. _flexcomp-edge-solref:
|
||||
.. _flexcomp-edge-solimp:
|
||||
@@ -4254,7 +4258,8 @@ The elasticity model is a `Saint Venant-Kirchhoff
|
||||
<https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ model discretized with
|
||||
piecewise linear finite elements, intended to simulate the compression or elongation of hyperelastic materials subjected
|
||||
to large displacements (finite rotations) and small strains, since it uses a nonlinear strain-displacement but a linear
|
||||
stress-strain relationship.. See also :ref:`deformable <CDeformable>` objects.
|
||||
stress-strain relationship. See also :ref:`deformable <CDeformable>` objects and `this model
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/floppy.xml>`__.
|
||||
|
||||
.. _flex-elasticity-young:
|
||||
|
||||
@@ -4787,7 +4792,8 @@ This element constrains the length of one tendon to be a quartic polynomial of a
|
||||
This element constrains the lengths of all edges of a specified flex to their respective lengths in the initial model
|
||||
configuration. In this way the edges are used to maintain the shape of the deformable entity. Note that all other
|
||||
equality constraint types add a fixed number of scalar constraints, while this element adds as many scalar constraints
|
||||
as there are edges in the specified flex.
|
||||
as there are edges in the specified flex. See `this model
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/plate.xml>`__ for an example.
|
||||
|
||||
.. _equality-flex-name:
|
||||
.. _equality-flex-class:
|
||||
@@ -4811,8 +4817,9 @@ as there are edges in the specified flex.
|
||||
|
||||
This element constrains the trace and the derminant of the strain tensor to that of the identity matrix as in Chen, Kry,
|
||||
and Vouga, "Locking-free Simulation of Isometric Thin Plates", 2019. The strain tensor is computed per triangle and
|
||||
averaged over all triangles adjacent to a vertex. This reduces the number of constraints from 2T to 2V, freeing
|
||||
V degrees of freedom to avoid locking. It is only supported for dimension 2, i.e., cloth-like flexes.
|
||||
averaged over all triangles adjacent to a vertex. This reduces the number of constraints from 2T to 2V, freeing V
|
||||
degrees of freedom to avoid locking. It is only supported for dimension 2, i.e., cloth-like flexes. See `this model
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/poncho.xml>`__ for an example.
|
||||
|
||||
.. _equality-flexvert-name:
|
||||
.. _equality-flexvert-class:
|
||||
@@ -4829,6 +4836,32 @@ V degrees of freedom to avoid locking. It is only supported for dimension 2, i.e
|
||||
Name of the flex whose vertices are being constrained.
|
||||
|
||||
|
||||
.. _equality-flexstrain:
|
||||
|
||||
:el-prefix:`equality/` |-| **flexstrain** |*|
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
This element constrains the strain invariants of a trilinear or quadratic flex to their initial values. Specifically, it
|
||||
enforces that the trace and determinant of the deformation gradient remain constant, preserving volume and preventing
|
||||
excessive stretching. This constraint type is only supported for dimension 3 trilinear flexes (i.e., volumetric
|
||||
deformable bodies using trilinear interpolation). See `this model
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/strain.xml>`__ for an example.
|
||||
|
||||
.. _equality-flexstrain-name:
|
||||
.. _equality-flexstrain-class:
|
||||
.. _equality-flexstrain-active:
|
||||
.. _equality-flexstrain-solref:
|
||||
.. _equality-flexstrain-solimp:
|
||||
|
||||
:at:`name`, :at:`class`, :at:`active`, :at:`solref`, :at:`solimp`
|
||||
Same as in :ref:`connect <equality-connect>` element.
|
||||
|
||||
.. _equality-flexstrain-flex:
|
||||
|
||||
:at:`flex`: :at-val:`string, required`
|
||||
Name of the flex whose strain invariants are being constrained.
|
||||
|
||||
|
||||
.. _equality-distance:
|
||||
|
||||
:el-prefix:`equality/` |-| **distance** |*|
|
||||
|
||||
@@ -2045,6 +2045,30 @@
|
||||
:ref:`solimp<equality-flexvert-solimp>`
|
||||
|
||||
|
||||
.. dropdown:: :ref:`flexstrain<equality-flexstrain>` |*|
|
||||
|
||||
.. grid:: 2 3 4 4
|
||||
:gutter: 0
|
||||
|
||||
.. grid-item::
|
||||
:ref:`name<equality-flexstrain-name>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`class<equality-flexstrain-class>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`flex<equality-flexstrain-flex>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`active<equality-flexstrain-active>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`solref<equality-flexstrain-solref>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`solimp<equality-flexstrain-solimp>`
|
||||
|
||||
|
||||
.. dropdown:: :ref:`tendon<tendon>` |*|
|
||||
|
||||
|
||||
|
||||
@@ -15,6 +15,8 @@ General
|
||||
``ten_J_colind`` have been moved from :ref:`mjData` to :ref:`mjModel` and are no longer computed at run time by
|
||||
``mj_tendon`` but at compile time.
|
||||
|
||||
- Added a new ``strain`` :ref:`equality constraint<flexcomp-edge-equality>` type for trilinear and quadratic
|
||||
:ref:`dofs<body-flexcomp-dof>`.
|
||||
- Flexes now support collisions with SDF geoms.
|
||||
- Improved memory requirements for ``ten_J`` and ``ten_J_colind`` by reducing the upper bound for the number
|
||||
of non-zeros ``nJten``.
|
||||
|
||||
@@ -609,6 +609,7 @@ typedef enum mjtEq_ { // type of equality constraint
|
||||
mjEQ_TENDON, // couple the lengths of two tendons with cubic
|
||||
mjEQ_FLEX, // fix all edge lengths of a flex
|
||||
mjEQ_FLEXVERT, // fix all vertex lengths of a flex
|
||||
mjEQ_FLEXSTRAIN, // fix strain invariants of a trilinear flex
|
||||
mjEQ_DISTANCE // unsupported, will cause an error if used
|
||||
} mjtEq;
|
||||
typedef enum mjtWrap_ { // type of tendon wrap object
|
||||
@@ -1350,7 +1351,7 @@ struct mjModel_ {
|
||||
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
|
||||
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
|
||||
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
|
||||
int* flex_edgeequality; // 0: none, 1: edges, 2: vertices (nflex x 1)
|
||||
int* flex_edgeequality; // 0:none, 1:edges, 2:vertices, 3:strain (nflex x 1)
|
||||
mjtByte* flex_rigid; // are all vertices in the same body (nflex x 1)
|
||||
mjtByte* flexedge_rigid; // are both edge vertices in same body (nflexedge x 1)
|
||||
mjtByte* flex_centered; // are all vertex coordinates (0,0,0) (nflex x 1)
|
||||
|
||||
@@ -211,6 +211,7 @@ typedef enum mjtEq_ { // type of equality constraint
|
||||
mjEQ_TENDON, // couple the lengths of two tendons with cubic
|
||||
mjEQ_FLEX, // fix all edge lengths of a flex
|
||||
mjEQ_FLEXVERT, // fix all vertex lengths of a flex
|
||||
mjEQ_FLEXSTRAIN, // fix strain invariants of a trilinear flex
|
||||
mjEQ_DISTANCE // unsupported, will cause an error if used
|
||||
} mjtEq;
|
||||
|
||||
@@ -1011,7 +1012,7 @@ struct mjModel_ {
|
||||
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
|
||||
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
|
||||
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
|
||||
int* flex_edgeequality; // 0: none, 1: edges, 2: vertices (nflex x 1)
|
||||
int* flex_edgeequality; // 0:none, 1:edges, 2:vertices, 3:strain (nflex x 1)
|
||||
mjtByte* flex_rigid; // are all vertices in the same body (nflex x 1)
|
||||
mjtByte* flexedge_rigid; // are both edge vertices in same body (nflexedge x 1)
|
||||
mjtByte* flex_centered; // are all vertex coordinates (0,0,0) (nflex x 1)
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
<!-- Copyright 2026 DeepMind Technologies Limited
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<mujoco model="Gripper">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<option cone="elliptic" impratio="10" integrator="implicitfast"/>
|
||||
|
||||
<worldbody>
|
||||
<body>
|
||||
<freejoint/>
|
||||
<geom type="box" size=".46 .5 .4" pos="0 0 1.5" rgba=".5 .5 0 1" condim="6"/>
|
||||
</body>
|
||||
<body name="column_00" pos=".7 .4 .5">
|
||||
<geom type="box" size=".1 .1 .5" rgba=".2 .2 .2 1"/>
|
||||
<flexcomp type="box" pos="-.24 0 .1" dim="3" spacing=".03 .01 .01"
|
||||
radius=".001" rgba="0 .7 .7 1" mass=".5" name="left" dof="trilinear">
|
||||
<edge equality="strain"/>
|
||||
<contact selfcollide="none" internal="false" friction="3"/>
|
||||
<pin id="4 5 6 7"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
<body name="column_01" pos=".7 -.4 .5">
|
||||
<geom type="box" size=".1 .1 .5" rgba=".2 .2 .2 1"/>
|
||||
<flexcomp type="box" pos="-.24 0 .1" dim="3" spacing=".03 .01 .01"
|
||||
radius=".001" rgba="0 .7 .7 1" mass=".5" name="right" dof="trilinear">
|
||||
<edge equality="strain"/>
|
||||
<contact selfcollide="none" internal="false" friction="3"/>
|
||||
<pin id="4 5 6 7"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
<body name="column_02" pos=" -.7 .4 .5">
|
||||
<geom type="box" size=".1 .1 .5" rgba=".2 .2 .2 1"/>
|
||||
<flexcomp type="box" pos=".24 0 .1" dim="3" spacing=".03 .01 .01"
|
||||
radius=".001" rgba="0 .7 .7 1" mass=".5" name="bottom" dof="trilinear">
|
||||
<edge equality="strain"/>
|
||||
<contact selfcollide="none" internal="false" friction="3"/>
|
||||
<pin id="0 1 2 3"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
<body name="column_03" pos="-.7 -.4 .5">
|
||||
<geom type="box" size=".1 .1 .5" rgba=".2 .2 .2 1"/>
|
||||
<flexcomp type="box" pos=".24 0 .1" dim="3" spacing=".03 .01 .01"
|
||||
radius=".001" rgba="0 .7 .7 1" mass=".5" name="top" dof="trilinear">
|
||||
<edge equality="strain"/>
|
||||
<contact selfcollide="none" internal="false" friction="3"/>
|
||||
<pin id="0 1 2 3"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
</mujoco>
|
||||
@@ -222,7 +222,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
||||
('mjEQ_TENDON', 3),
|
||||
('mjEQ_FLEX', 4),
|
||||
('mjEQ_FLEXVERT', 5),
|
||||
('mjEQ_DISTANCE', 6),
|
||||
('mjEQ_FLEXSTRAIN', 6),
|
||||
('mjEQ_DISTANCE', 7),
|
||||
]),
|
||||
)),
|
||||
('mjtWrap',
|
||||
|
||||
@@ -2980,7 +2980,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='0: none, 1: edges, 2: vertices',
|
||||
doc='0:none, 1:edges, 2:vertices, 3:strain',
|
||||
array_extent=('nflex',),
|
||||
),
|
||||
StructFieldDecl(
|
||||
|
||||
@@ -47,6 +47,26 @@
|
||||
//-------------------------- utility functions -----------------------------------------------------
|
||||
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// allocate efc arrays on arena, return 1 on success, 0 on failure
|
||||
static int arenaAllocEfc(const mjModel* m, mjData* d) {
|
||||
#undef MJ_M
|
||||
@@ -612,36 +632,336 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
size = 1;
|
||||
break;
|
||||
|
||||
case mjEQ_FLEX:
|
||||
flex_edgeadr = m->flex_edgeadr[id[0]];
|
||||
flex_edgenum = m->flex_edgenum[id[0]];
|
||||
// add one constraint per non-rigid edge
|
||||
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
|
||||
// skip rigid
|
||||
if (m->flexedge_rigid[e]) {
|
||||
continue;
|
||||
}
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// strain constraint mode: add 24 constraints (3 invariants at 8 Gauss points)
|
||||
int f = id[0];
|
||||
int nodenum = m->flex_nodenum[f];
|
||||
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
|
||||
int order = m->flex_interp[f];
|
||||
|
||||
// position error
|
||||
cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e];
|
||||
// skip if not interpolated (order == 0 or no nodes)
|
||||
if (!order || !nodenum) {
|
||||
break;
|
||||
}
|
||||
|
||||
// add constraint: sparse or dense
|
||||
if (issparse) {
|
||||
mj_addConstraint(m, d, d->flexedge_J+m->flexedge_J_rowadr[e], cpos, 0, 0,
|
||||
1, mjCNSTR_EQUALITY, i,
|
||||
m->flexedge_J_rownnz[e],
|
||||
m->flexedge_J_colind+m->flexedge_J_rowadr[e]);
|
||||
// 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* chain_col = mjSTACKALLOC(d, nv, int);
|
||||
mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum);
|
||||
|
||||
// compute global node positions from body states
|
||||
// Green-Lagrange strain E = ½(F'F - I) is rotationally invariant,
|
||||
// so we don't need corotational decomposition - use global positions directly
|
||||
int nstart = m->flex_nodeadr[f];
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
if (m->flex_centered[f]) {
|
||||
mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]);
|
||||
} else {
|
||||
mju_zero(jac[0], nv); // reuse first row of jac[0]
|
||||
int rowadr = m->flexedge_J_rowadr[e];
|
||||
int rownnz = m->flexedge_J_rownnz[e];
|
||||
for (int k=0; k<rownnz; k++) {
|
||||
jac[0][m->flexedge_J_colind[rowadr+k]] = d->flexedge_J[rowadr+k];
|
||||
}
|
||||
mj_addConstraint(m, d, jac[0], cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
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]);
|
||||
}
|
||||
}
|
||||
|
||||
// compute node Jacobians in global frame (3*nodenum rows x nv cols)
|
||||
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);
|
||||
|
||||
// expand sparse Jacobian to dense row format
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// compute combined chain for all nodes (for sparse mode)
|
||||
int* combined_chain = mjSTACKALLOC(d, nv, int);
|
||||
int combined_nnz = 0;
|
||||
if (issparse) {
|
||||
// mark which DOFs are used by any node body
|
||||
int* dof_used = mjSTACKALLOC(d, nv, int);
|
||||
mju_zeroInt(dof_used, nv);
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
int temp_chain[200]; // max DOFs per body
|
||||
int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain);
|
||||
for (int k = 0; k < temp_nnz; k++) {
|
||||
dof_used[temp_chain[k]] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// build combined chain from marked DOFs
|
||||
for (int q = 0; q < nv; q++) {
|
||||
if (dof_used[q]) {
|
||||
combined_chain[combined_nnz++] = q;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Gauss-Legendre quadrature points in [0,1]^3
|
||||
// order=1: 2x2x2=8 points, order=2: 3x3x3=27 points
|
||||
int nquad = order + 1; // quadrature order per axis
|
||||
int ngauss = nquad * nquad * nquad; // total Gauss points
|
||||
|
||||
// 1D Gauss-Legendre points and weights on [0,1]
|
||||
mjtNum gp1d[3]; // max 3 points for order=2
|
||||
if (nquad == 2) {
|
||||
gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); // ~ 0.211
|
||||
gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0); // ~ 0.789
|
||||
} else { // nquad == 3
|
||||
gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6); // ~ 0.113
|
||||
gp1d[1] = 0.5; // 0.5
|
||||
gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); // ~ 0.887
|
||||
}
|
||||
|
||||
// build 3D Gauss points array (max 27 points)
|
||||
mjtNum gauss[27][3];
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// loop over Gauss points
|
||||
// get reference positions from m->flex_node0 (Cartesian positions at qpos0)
|
||||
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));
|
||||
}
|
||||
|
||||
for (int g = 0; g < ngauss; g++) {
|
||||
mjtNum* p = gauss[g];
|
||||
|
||||
// compute current position gradient dxcur/dxi
|
||||
mjtNum Fcur[9];
|
||||
mju_defGradient(Fcur, p, xpos, order);
|
||||
|
||||
// compute reference position gradient dxref/dxi
|
||||
mjtNum Fref[9];
|
||||
mju_defGradient(Fref, p, refpos, order);
|
||||
|
||||
// compute true deformation gradient F = Fcur * inv(Fref)
|
||||
// F maps from reference to current configuration
|
||||
mjtNum Fref_inv[9];
|
||||
mju_transpose(Fref_inv, Fref, 3, 3); // start with transpose for cofactor
|
||||
mjtNum det = Fref[0]*(Fref[4]*Fref[8] - Fref[5]*Fref[7]) -
|
||||
Fref[1]*(Fref[3]*Fref[8] - Fref[5]*Fref[6]) +
|
||||
Fref[2]*(Fref[3]*Fref[7] - Fref[4]*Fref[6]);
|
||||
|
||||
// compute adjugate (cofactor transposed)
|
||||
Fref_inv[0] = (Fref[4]*Fref[8] - Fref[5]*Fref[7]) / det;
|
||||
Fref_inv[1] = -(Fref[1]*Fref[8] - Fref[2]*Fref[7]) / det;
|
||||
Fref_inv[2] = (Fref[1]*Fref[5] - Fref[2]*Fref[4]) / det;
|
||||
Fref_inv[3] = -(Fref[3]*Fref[8] - Fref[5]*Fref[6]) / det;
|
||||
Fref_inv[4] = (Fref[0]*Fref[8] - Fref[2]*Fref[6]) / det;
|
||||
Fref_inv[5] = -(Fref[0]*Fref[5] - Fref[2]*Fref[3]) / det;
|
||||
Fref_inv[6] = (Fref[3]*Fref[7] - Fref[4]*Fref[6]) / det;
|
||||
Fref_inv[7] = -(Fref[0]*Fref[7] - Fref[1]*Fref[6]) / det;
|
||||
Fref_inv[8] = (Fref[0]*Fref[4] - Fref[1]*Fref[3]) / det;
|
||||
|
||||
// F = Fcur * Fref_inv
|
||||
mjtNum F[9];
|
||||
mju_mulMatMat(F, Fcur, Fref_inv, 3, 3, 3);
|
||||
|
||||
// compute C = F'*F (right Cauchy-Green tensor)
|
||||
mjtNum C[9];
|
||||
mju_mulMatTMat3(C, F, F);
|
||||
|
||||
// compute Green-Lagrange strain E = 0.5*(C - I)
|
||||
mjtNum E[9];
|
||||
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;
|
||||
|
||||
// compute 3 invariants of E
|
||||
// I1 = tr(E) = E[0] + E[4] + E[8]
|
||||
mjtNum I1 = E[0] + E[4] + E[8];
|
||||
|
||||
// I2 = 0.5*(tr(E)^2 - tr(E^2))
|
||||
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);
|
||||
|
||||
// I3 = det(E)
|
||||
mjtNum I3 = E[0]*(E[4]*E[8] - E[5]*E[7]) -
|
||||
E[1]*(E[3]*E[8] - E[5]*E[6]) +
|
||||
E[2]*(E[3]*E[7] - E[4]*E[6]);
|
||||
|
||||
// compute shape function gradients at this Gauss point
|
||||
// grad[n][k] = d(basis_n)/d(p_k), for n in [0, nodenum), k in [0,3)
|
||||
// indexing matches mju_defGradient: idx = i*(order+1)^2 + j*(order+1) + k
|
||||
mjtNum grad[27][3]; // max 27 nodes for order=2
|
||||
int npoint = (order + 1) * (order + 1) * (order + 1);
|
||||
for (int n = 0; n < npoint; n++) {
|
||||
int stride = order + 1;
|
||||
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 Jacobian for each invariant
|
||||
mjtNum invariants[3] = {I1, I2, I3};
|
||||
|
||||
for (int inv = 0; inv < 3; inv++) {
|
||||
cpos[0] = invariants[inv];
|
||||
|
||||
// compute dI/dE (3x3 symmetric, stored as 9)
|
||||
mjtNum dIdE[9];
|
||||
if (inv == 0) {
|
||||
// dI1/dE = I (identity)
|
||||
mju_zero(dIdE, 9);
|
||||
dIdE[0] = dIdE[4] = dIdE[8] = 1.0;
|
||||
} else if (inv == 1) {
|
||||
// dI2/dE = tr(E)*I - E
|
||||
mju_zero(dIdE, 9);
|
||||
dIdE[0] = I1 - E[0];
|
||||
dIdE[4] = I1 - E[4];
|
||||
dIdE[8] = I1 - E[8];
|
||||
dIdE[1] = -E[1]; dIdE[3] = -E[3];
|
||||
dIdE[2] = -E[2]; dIdE[6] = -E[6];
|
||||
dIdE[5] = -E[5]; dIdE[7] = -E[7];
|
||||
} else {
|
||||
// dI3/dE = cofactor(E) = det(E) * E^{-T} for invertible E
|
||||
// For small strain, use adjugate directly
|
||||
dIdE[0] = E[4]*E[8] - E[5]*E[7];
|
||||
dIdE[1] = -(E[1]*E[8] - E[2]*E[7]);
|
||||
dIdE[2] = E[1]*E[5] - E[2]*E[4];
|
||||
dIdE[3] = -(E[3]*E[8] - E[5]*E[6]);
|
||||
dIdE[4] = E[0]*E[8] - E[2]*E[6];
|
||||
dIdE[5] = -(E[0]*E[5] - E[2]*E[3]);
|
||||
dIdE[6] = E[3]*E[7] - E[4]*E[6];
|
||||
dIdE[7] = -(E[0]*E[7] - E[1]*E[6]);
|
||||
dIdE[8] = E[0]*E[4] - E[1]*E[3];
|
||||
}
|
||||
|
||||
// dI/dx_n = sum over i,j: dI/dE_ij * dE_ij/dx_n
|
||||
// where dE_ij/dx_n = 0.5 * d(F'F)_ij/dx_n
|
||||
// d(F'F)_ij/dx_n = sum_k (dF_ki/dx_n * F_kj + F_ki * dF_kj/dx_n)
|
||||
//
|
||||
// With F = Fcur * Fref_inv:
|
||||
// dF_ab/d(x_n)_c = sum_k (dFcur_ak/d(x_n)_c) * Fref_inv[kb]
|
||||
// = sum_k delta_{ac} * grad[n][k] * Fref_inv[kb]
|
||||
// So only row a=c contributes
|
||||
|
||||
// compute dI/dx for all nodes (3*nodenum values)
|
||||
mjtNum* dIdx = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||
mju_zero(dIdx, 3*nodenum);
|
||||
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
for (int c = 0; c < 3; c++) { // component of x_n
|
||||
mjtNum dI = 0;
|
||||
for (int ij = 0; ij < 9; ij++) {
|
||||
int ii = ij / 3; // row of E/C
|
||||
int jj = ij % 3; // col of E/C
|
||||
|
||||
// dF_ab/d(x_n)_c = sum_k grad[n][k] * Fref_inv[kb] (only for a=c)
|
||||
// dC_ij/d(x_n)_c = dF_ci * F_cj + F_ci * dF_cj
|
||||
// = (sum_k grad[n][k]*Fref_inv[ki]) * F_cj
|
||||
// + F_ci * (sum_k grad[n][k]*Fref_inv[kj])
|
||||
|
||||
// compute dF_ci = sum_k grad[n][k] * Fref_inv[ki]
|
||||
mjtNum dF_ci = 0;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
dF_ci += grad[n][k] * Fref_inv[k*3 + ii];
|
||||
}
|
||||
// compute dF_cj = sum_k grad[n][k] * Fref_inv[kj]
|
||||
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;
|
||||
|
||||
// dE_ij = 0.5 * dC_ij
|
||||
dI += dIdE[ij] * 0.5 * dC_ij;
|
||||
}
|
||||
dIdx[3*n + c] = dI;
|
||||
}
|
||||
}
|
||||
|
||||
// strain_jac[q] = sum_n sum_c dIdx[3*n+c] * node_jac[(3*n+c)*nv + q]
|
||||
mju_zero(strain_jac, nv);
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
int row = 3*n + c;
|
||||
for (int q = 0; q < nv; q++) {
|
||||
strain_jac[q] += dIdx[row] * node_jac[row*nv + q];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add constraint: sparse or dense
|
||||
if (issparse) {
|
||||
// convert dense strain_jac to sparse format
|
||||
mjtNum* sparse_jac = mjSTACKALLOC(d, combined_nnz, mjtNum);
|
||||
for (int k = 0; k < combined_nnz; k++) {
|
||||
sparse_jac[k] = strain_jac[combined_chain[k]];
|
||||
}
|
||||
mj_addConstraint(m, d, sparse_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
|
||||
combined_nnz, combined_chain);
|
||||
} else {
|
||||
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mj_freeStack(d);
|
||||
break;
|
||||
}
|
||||
|
||||
case mjEQ_FLEX:
|
||||
// edge constraint mode: add one constraint per non-rigid edge
|
||||
flex_edgeadr = m->flex_edgeadr[id[0]];
|
||||
flex_edgenum = m->flex_edgenum[id[0]];
|
||||
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
|
||||
// skip rigid
|
||||
if (m->flexedge_rigid[e]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// position error
|
||||
cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e];
|
||||
|
||||
// add constraint: sparse or dense
|
||||
if (issparse) {
|
||||
mj_addConstraint(m, d, d->flexedge_J+m->flexedge_J_rowadr[e], cpos, 0, 0,
|
||||
1, mjCNSTR_EQUALITY, i,
|
||||
m->flexedge_J_rownnz[e],
|
||||
m->flexedge_J_colind+m->flexedge_J_rowadr[e]);
|
||||
} else {
|
||||
mju_zero(jac[0], nv); // reuse first row of jac[0]
|
||||
int rowadr = m->flexedge_J_rowadr[e];
|
||||
int rownnz = m->flexedge_J_rownnz[e];
|
||||
for (int k=0; k<rownnz; k++) {
|
||||
jac[0][m->flexedge_J_colind[rowadr+k]] = d->flexedge_J[rowadr+k];
|
||||
}
|
||||
mj_addConstraint(m, d, jac[0], cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXVERT:
|
||||
// add two constraints per vertex
|
||||
@@ -1186,6 +1506,26 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
|
||||
i--;
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// strain constraints: 24 constraints, use average node inv weight
|
||||
int flex_id = m->eq_obj1id[id];
|
||||
int nodenum = m->flex_nodenum[flex_id];
|
||||
int nstart = m->flex_nodeadr[flex_id];
|
||||
mjtNum avg_invweight = 0;
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
int bodyid = m->flex_nodebodyid[nstart + n];
|
||||
avg_invweight += m->body_invweight0[2*bodyid];
|
||||
}
|
||||
avg_invweight /= nodenum;
|
||||
for (int c = 0; c < 24; c++) {
|
||||
dA[i++] = avg_invweight;
|
||||
}
|
||||
|
||||
// adjust constraint counter
|
||||
i--;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
mjERROR("unknown constraint type %d", d->efc_type[i]); // SHOULD NOT OCCUR
|
||||
}
|
||||
@@ -1788,6 +2128,31 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
}
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// strain constraints: 3 invariants × ngauss Gauss points
|
||||
// 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) {
|
||||
break;
|
||||
}
|
||||
int nquad = order + 1; // 2 for order=1, 3 for order=2
|
||||
int ngauss = nquad * nquad * nquad; // 8 or 27
|
||||
size = 3 * ngauss; // 24 or 81
|
||||
|
||||
if (nnz) {
|
||||
// NV is sum of all node Jacobians
|
||||
int nstart = m->flex_nodeadr[id[0]];
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
int bodyid = m->flex_nodebodyid[nstart + n];
|
||||
NV += mj_bodyChain(m, bodyid, chain);
|
||||
}
|
||||
// each constraint row shares this NV
|
||||
NV = size * NV;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
// might occur in case of the now-removed distance equality constraint
|
||||
mjERROR("unknown constraint type %d", m->eq_type[i]); // SHOULD NOT OCCUR
|
||||
@@ -1795,7 +2160,8 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
|
||||
// accumulate counts; flex NV already accumulated
|
||||
ne += mj_addConstraintCount(m, size, NV);
|
||||
if (m->eq_type[i] == mjEQ_FLEX || m->eq_type[i] == mjEQ_FLEXVERT) {
|
||||
if (m->eq_type[i] == mjEQ_FLEX || m->eq_type[i] == mjEQ_FLEXVERT ||
|
||||
m->eq_type[i] == mjEQ_FLEXSTRAIN) {
|
||||
nnze += NV;
|
||||
} else {
|
||||
nnze += size*NV;
|
||||
|
||||
@@ -2573,6 +2573,19 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
|
||||
i += 2*m->flex_vertnum[k];
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// increment with 3 invariants × ngauss Gauss points
|
||||
k = m->eq_obj1id[id];
|
||||
int order = m->flex_interp[k];
|
||||
int nodenum = m->flex_nodenum[k];
|
||||
if (order && nodenum) {
|
||||
int nquad = order + 1;
|
||||
int ngauss = nquad * nquad * nquad;
|
||||
i += 3 * ngauss;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR
|
||||
}
|
||||
|
||||
@@ -1958,6 +1958,7 @@ const char* mj_validateReferences(const mjModel* m) {
|
||||
|
||||
case mjEQ_FLEX:
|
||||
case mjEQ_FLEXVERT:
|
||||
case mjEQ_FLEXSTRAIN:
|
||||
if (obj1id >= m->nflex || obj1id < 0) {
|
||||
return "Invalid model: eq_obj1id out of bounds.";
|
||||
}
|
||||
|
||||
@@ -644,7 +644,14 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
|
||||
if (equality) {
|
||||
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
|
||||
mjs_setDefault(pe->element, &model->Default()->spec);
|
||||
pe->type = equality == 1 ? mjEQ_FLEX : mjEQ_FLEXVERT;
|
||||
// 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;
|
||||
} else if (equality == 3) {
|
||||
pe->type = mjEQ_FLEXSTRAIN;
|
||||
}
|
||||
pe->active = true;
|
||||
mjs_setString(pe->name1, name.c_str());
|
||||
}
|
||||
|
||||
@@ -83,7 +83,7 @@ class mjCFlexcomp {
|
||||
double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh
|
||||
double mass; // total mass of auto-generated bodies
|
||||
double inertiabox; // size of inertia box for each body
|
||||
int equality; // create equality constraint, 0:none, 1:edge, 2:vert
|
||||
int equality; // create equality constraint, 0:none, 1:edge, 2:vert, 3:strain
|
||||
std::string file; // mesh/gmsh file name
|
||||
mjtDof doftype; // dof type, all vertices or trilinear interpolation
|
||||
|
||||
|
||||
@@ -3876,6 +3876,7 @@ void mjCFlex::PointToLocal() {
|
||||
|
||||
|
||||
void mjCFlex::NameSpace(const mjCModel* m) {
|
||||
mjCBase::NameSpace(m);
|
||||
for (auto& name : spec_vertbody_) {
|
||||
name = m->prefix + name + m->suffix;
|
||||
}
|
||||
|
||||
@@ -3533,6 +3533,10 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
m->flex_edgeequality[i] = 2;
|
||||
break;
|
||||
}
|
||||
if (equalities_[k]->type == mjEQ_FLEXSTRAIN) {
|
||||
m->flex_edgeequality[i] = 3;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -6292,7 +6292,7 @@ void mjCEquality::ResolveReferences(const mjCModel* m) {
|
||||
object_type = mjOBJ_JOINT;
|
||||
} else if (type == mjEQ_TENDON) {
|
||||
object_type = mjOBJ_TENDON;
|
||||
} else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT) {
|
||||
} else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT || type == mjEQ_FLEXSTRAIN) {
|
||||
object_type = mjOBJ_FLEX;
|
||||
} else {
|
||||
throw mjCError(this, "invalid type in equality constraint");
|
||||
@@ -6349,7 +6349,8 @@ void mjCEquality::Compile(void) {
|
||||
ResolveReferences(model);
|
||||
|
||||
// make sure flex is not rigid
|
||||
if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT) && model->Flexes()[obj1id]->rigid) {
|
||||
if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT || type == mjEQ_FLEXSTRAIN) &&
|
||||
model->Flexes()[obj1id]->rigid) {
|
||||
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -366,6 +366,8 @@ std::vector<const char*> MJCF[nMJCF] = {
|
||||
"active", "solref", "solimp"},
|
||||
{"flexvert", "*", "name", "class", "flex",
|
||||
"active", "solref", "solimp"},
|
||||
{"flexstrain", "*", "name", "class", "flex",
|
||||
"active", "solref", "solimp"},
|
||||
{">"},
|
||||
|
||||
{"tendon", "*"},
|
||||
@@ -669,7 +671,7 @@ const mjMap solver_map[solver_sz] = {
|
||||
|
||||
|
||||
// constraint type
|
||||
const int equality_sz = 7;
|
||||
const int equality_sz = 8;
|
||||
const mjMap equality_map[equality_sz] = {
|
||||
{"connect", mjEQ_CONNECT},
|
||||
{"weld", mjEQ_WELD},
|
||||
@@ -677,6 +679,7 @@ const mjMap equality_map[equality_sz] = {
|
||||
{"tendon", mjEQ_TENDON},
|
||||
{"flex", mjEQ_FLEX},
|
||||
{"flexvert", mjEQ_FLEXVERT},
|
||||
{"flexstrain", mjEQ_FLEXSTRAIN},
|
||||
{"distance", mjEQ_DISTANCE}
|
||||
};
|
||||
|
||||
@@ -930,10 +933,11 @@ const mjMap elastic2d_map[5] = {
|
||||
|
||||
|
||||
// flex equality type
|
||||
const mjMap flexeq_map[3] = {
|
||||
const mjMap flexeq_map[4] = {
|
||||
{"false", 0},
|
||||
{"true", 1},
|
||||
{"vert", 2},
|
||||
{"strain", 3},
|
||||
};
|
||||
|
||||
|
||||
@@ -2207,6 +2211,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
|
||||
|
||||
case mjEQ_FLEX:
|
||||
case mjEQ_FLEXVERT:
|
||||
case mjEQ_FLEXSTRAIN:
|
||||
ReadAttrTxt(elem, "flex", name1, true);
|
||||
break;
|
||||
|
||||
@@ -2768,7 +2773,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
|
||||
// edge
|
||||
XMLElement* edge = FirstChildElement(elem, "edge");
|
||||
if (edge) {
|
||||
MapValue(edge, "equality", &fcomp.equality, flexeq_map, 3);
|
||||
MapValue(edge, "equality", &fcomp.equality, flexeq_map, 4);
|
||||
ReadAttr(edge, "solref", mjNREF, fcomp.def.spec.equality->solref, text, false, false);
|
||||
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.spec.equality->solimp, text, false, false);
|
||||
ReadAttr(edge, "stiffness", 1, &dflex.edgestiffness, text);
|
||||
@@ -2789,6 +2794,9 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
|
||||
if (dflex.elastic2d >= 2 && fcomp.equality) {
|
||||
throw mjXError(elem, "elasticity and edge constraints cannot both be present");
|
||||
}
|
||||
if (fcomp.equality == 3 && dflex.young > 0) {
|
||||
throw mjXError(elem, "strain constraint and elasticity (young) cannot both be present");
|
||||
}
|
||||
|
||||
// contact
|
||||
XMLElement* cont = FirstChildElement(elem, "contact");
|
||||
|
||||
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
|
||||
};
|
||||
|
||||
// MJCF schema
|
||||
#define nMJCF 245
|
||||
#define nMJCF 246
|
||||
extern std::vector<const char*> MJCF[nMJCF];
|
||||
|
||||
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
|
||||
|
||||
@@ -711,6 +711,7 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
|
||||
|
||||
case mjEQ_FLEX:
|
||||
case mjEQ_FLEXVERT:
|
||||
case mjEQ_FLEXSTRAIN:
|
||||
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
|
||||
break;
|
||||
|
||||
|
||||
@@ -14,6 +14,7 @@
|
||||
|
||||
// Tests for engine/engine_core_constraint.c.
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
@@ -489,5 +490,258 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// Test flex strain constraint with pinned nodes attached to freejoint parent
|
||||
TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast" jacobian="dense" gravity="0 0 0"/>
|
||||
<worldbody>
|
||||
<geom type="plane" size="10 10 1" pos="0 0 -.1"/>
|
||||
<body>
|
||||
<joint type="free"/>
|
||||
<geom type="box" size="0.13 0.18 0.036"/>
|
||||
<body name="parent">
|
||||
<flexcomp name="test" type="box"
|
||||
spacing=".1 .02 .1" radius="0.001"
|
||||
pos="0 0 .2" dof="trilinear" xyaxes="0 1 0 0 0 1" mass="1" dim="3">
|
||||
<contact selfcollide="none"/>
|
||||
<edge equality="strain"/>
|
||||
<pin id="0 2 4 6"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1024> 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 that we have constraints
|
||||
EXPECT_GT(d->nefc, 0) << "No constraints generated";
|
||||
EXPECT_GT(d->ne, 0) << "Expected some strain constraints";
|
||||
|
||||
// Check qacc and forces at rest with gravity=0
|
||||
EXPECT_NEAR(d->qacc_smooth[6], 0, 1e-6) << "qacc_smooth should be 0 at rest";
|
||||
EXPECT_NEAR(d->qacc[6], 0, 1e-6) << "qacc should be 0 at rest";
|
||||
|
||||
// Check initial constraint values (efc_pos)
|
||||
bool has_bad_constraint = false;
|
||||
for (int i = 0; i < d->nefc; i++) {
|
||||
if (d->efc_type[i] == mjCNSTR_EQUALITY) {
|
||||
if (mju_abs(d->efc_pos[i]) > 1.0) {
|
||||
has_bad_constraint = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
EXPECT_FALSE(has_bad_constraint)
|
||||
<< "Some constraint values are too large at rest";
|
||||
|
||||
// Check Jacobian values - look for NaN or huge values
|
||||
int nv = m->nv;
|
||||
bool has_bad_jacobian = false;
|
||||
for (int i = 0; i < d->nefc; i++) {
|
||||
if (d->efc_type[i] == mjCNSTR_EQUALITY) {
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum val = d->efc_J[i*nv + j];
|
||||
if (mju_isBad(val) || mju_abs(val) > 1e10) {
|
||||
has_bad_jacobian = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
EXPECT_FALSE(has_bad_jacobian) << "Jacobian contains NaN or huge values";
|
||||
|
||||
// Verify Jacobian with finite differences for first few constraints
|
||||
mjtNum eps = 1e-6;
|
||||
std::vector<mjtNum> qpos0(m->nq);
|
||||
mju_copy(qpos0.data(), d->qpos, m->nq);
|
||||
|
||||
// Store original constraint values
|
||||
std::vector<mjtNum> efc_pos0(d->nefc);
|
||||
mju_copy(efc_pos0.data(), d->efc_pos, d->nefc);
|
||||
|
||||
int num_constraints_to_check = mju_min(3, d->ne);
|
||||
bool has_jacobian_mismatch = false;
|
||||
for (int j = 0; j < nv && j < 6; j++) {
|
||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||
mjtNum dqpos[100] = {0};
|
||||
dqpos[j] = eps;
|
||||
mj_integratePos(m, d->qpos, dqpos, 1);
|
||||
mj_forward(m, d);
|
||||
|
||||
for (int i = 0; i < num_constraints_to_check; i++) {
|
||||
mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps;
|
||||
mjtNum analytic = d->efc_J[i*nv + j];
|
||||
// Use relative tolerance with absolute floor to handle near-zero values
|
||||
mjtNum tol = mju_max(1e-8, 0.1 * (mju_abs(fd) + mju_abs(analytic)));
|
||||
if (mju_abs(fd - analytic) > tol) {
|
||||
has_jacobian_mismatch = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
EXPECT_FALSE(has_jacobian_mismatch)
|
||||
<< "Jacobian FD mismatch at initial config";
|
||||
|
||||
// Test rotation invariance: rotate via freejoint quaternion
|
||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||
mjtNum angle = 0.785398; // 45 degrees
|
||||
d->qpos[3] = mju_cos(angle/2); // w
|
||||
d->qpos[4] = 0;
|
||||
d->qpos[5] = 0;
|
||||
d->qpos[6] = mju_sin(angle/2); // z
|
||||
mj_forward(m, d);
|
||||
|
||||
mjtNum max_strain_rotated = 0;
|
||||
for (int i = 0; i < d->ne; i++) {
|
||||
if (mju_abs(d->efc_pos[i]) > max_strain_rotated) {
|
||||
max_strain_rotated = mju_abs(d->efc_pos[i]);
|
||||
}
|
||||
}
|
||||
EXPECT_LT(max_strain_rotated, 1e-6)
|
||||
<< "Strain should remain ~0 after rigid rotation";
|
||||
|
||||
// Check Jacobian in rotated configuration via FD
|
||||
std::vector<mjtNum> qpos_rot(m->nq);
|
||||
mju_copy(qpos_rot.data(), d->qpos, m->nq);
|
||||
std::vector<mjtNum> efc_pos_rot(d->nefc);
|
||||
mju_copy(efc_pos_rot.data(), d->efc_pos, d->nefc);
|
||||
|
||||
bool has_rotated_jacobian_mismatch = false;
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mju_copy(d->qpos, qpos_rot.data(), m->nq);
|
||||
mjtNum dqpos[100] = {0};
|
||||
dqpos[j] = eps;
|
||||
mj_integratePos(m, d->qpos, dqpos, 1);
|
||||
mj_forward(m, d);
|
||||
|
||||
mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps;
|
||||
mjtNum analytic = d->efc_J[0*nv + j];
|
||||
mjtNum tol = 0.1 * (mju_abs(fd) + mju_abs(analytic) + 1e-8);
|
||||
if ((mju_abs(fd) > 1e-8 || mju_abs(analytic) > 1e-8) &&
|
||||
mju_abs(fd - analytic) > tol) {
|
||||
has_rotated_jacobian_mismatch = true;
|
||||
}
|
||||
}
|
||||
EXPECT_FALSE(has_rotated_jacobian_mismatch)
|
||||
<< "Jacobian FD mismatch in rotated config";
|
||||
|
||||
// Reset for simulation
|
||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||
mj_forward(m, d);
|
||||
|
||||
// Run simulation only if checks pass
|
||||
if (!has_bad_constraint && !has_bad_jacobian) {
|
||||
for (int i = 0; i < 2000; i++) {
|
||||
mj_step(m, d);
|
||||
|
||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
||||
<< "Simulation became unstable at step " << i;
|
||||
ASSERT_FALSE(mju_isBad(d->qvel[0]))
|
||||
<< "Velocity became 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 flex strain constraint WITHOUT pinned nodes (simpler case)
|
||||
TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast" jacobian="dense"/>
|
||||
<worldbody>
|
||||
<body name="parent">
|
||||
<joint type="free"/>
|
||||
<geom type="box" size=".01 .01 .01" mass=".1"/>
|
||||
<flexcomp name="test" type="box"
|
||||
spacing=".1 .1 .1" radius="0.001"
|
||||
pos="0 0 .5" dof="trilinear" mass="1" dim="3">
|
||||
<contact selfcollide="none"/>
|
||||
<edge equality="strain"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1024> 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
|
||||
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
||||
|
||||
// Check no contacts
|
||||
EXPECT_EQ(d->ncon, 0);
|
||||
|
||||
// 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";
|
||||
|
||||
// Test rigid rotation: rotate flex and check strain still ~0
|
||||
std::vector<mjtNum> qpos0(m->nq);
|
||||
mju_copy(qpos0.data(), d->qpos, m->nq);
|
||||
// Rotate by 45 degrees around Z axis via quaternion
|
||||
mjtNum angle = 0.785398; // 45 degrees
|
||||
d->qpos[3] = mju_cos(angle/2); // w
|
||||
d->qpos[4] = 0; // x
|
||||
d->qpos[5] = 0; // y
|
||||
d->qpos[6] = mju_sin(angle/2); // z
|
||||
mj_forward(m, d);
|
||||
|
||||
mjtNum max_strain_rotated = 0;
|
||||
for (int i = 0; i < d->ne; i++) {
|
||||
if (mju_abs(d->efc_pos[i]) > max_strain_rotated) {
|
||||
max_strain_rotated = mju_abs(d->efc_pos[i]);
|
||||
}
|
||||
}
|
||||
EXPECT_LT(max_strain_rotated, 1e-6)
|
||||
<< "Strain should remain ~0 after rigid rotation";
|
||||
|
||||
// Run simulation for a few steps to check stability
|
||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||
mj_forward(m, d);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -15,7 +15,7 @@
|
||||
// Tests for user/user_model.cc.
|
||||
|
||||
#include <array>
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
@@ -919,5 +919,72 @@ TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
|
||||
mj_deleteVFS(&vfs);
|
||||
}
|
||||
|
||||
// Test that flex constraints are preserved when attaching a model
|
||||
TEST_F(UserFlexTest, FlexAttachConstraintPreserved) {
|
||||
// Child model with flex and strain constraint
|
||||
static constexpr char flex_xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="flex_parent">
|
||||
<flexcomp name="test" type="box"
|
||||
spacing=".1 .1 .1" radius="0.001"
|
||||
dof="trilinear" mass="1" dim="3">
|
||||
<contact selfcollide="none"/>
|
||||
<edge equality="strain"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
// Parent model that attaches the flex model
|
||||
static constexpr char parent_xml[] = R"(
|
||||
<mujoco>
|
||||
<asset>
|
||||
<model name="flex" file="flex.xml"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<frame pos="0 0 0.3">
|
||||
<attach model="flex" prefix="flex_"/>
|
||||
</frame>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
// Set up VFS with both XML files
|
||||
auto vfs = std::make_unique<mjVFS>();
|
||||
mj_defaultVFS(vfs.get());
|
||||
mj_addBufferVFS(vfs.get(), "flex.xml", flex_xml, sizeof(flex_xml));
|
||||
|
||||
// First verify the standalone flex model has constraints
|
||||
std::array<char, 1024> error;
|
||||
mjModel* m_standalone =
|
||||
LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get());
|
||||
ASSERT_THAT(m_standalone, NotNull()) << error.data();
|
||||
mjData* d_standalone = mj_makeData(m_standalone);
|
||||
mj_forward(m_standalone, d_standalone);
|
||||
int standalone_neq = m_standalone->neq;
|
||||
EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints";
|
||||
mj_deleteData(d_standalone);
|
||||
mj_deleteModel(m_standalone);
|
||||
|
||||
// Now load the parent model which attaches the flex
|
||||
mjModel* m_attached =
|
||||
LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
|
||||
ASSERT_THAT(m_attached, NotNull()) << error.data();
|
||||
mjData* d_attached = mj_makeData(m_attached);
|
||||
mj_forward(m_attached, d_attached);
|
||||
|
||||
// THE BUG: flex constraints disappear when attached
|
||||
EXPECT_GT(m_attached->neq, 0)
|
||||
<< "Attached flex should preserve strain constraints";
|
||||
EXPECT_EQ(m_attached->neq, standalone_neq)
|
||||
<< "Attached flex should have same number of constraints as standalone";
|
||||
|
||||
mj_deleteData(d_attached);
|
||||
mj_deleteModel(m_attached);
|
||||
mj_deleteVFS(vfs.get());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -288,7 +288,8 @@ public enum mjtEq : int{
|
||||
mjEQ_TENDON = 3,
|
||||
mjEQ_FLEX = 4,
|
||||
mjEQ_FLEXVERT = 5,
|
||||
mjEQ_DISTANCE = 6,
|
||||
mjEQ_FLEXSTRAIN = 6,
|
||||
mjEQ_DISTANCE = 7,
|
||||
}
|
||||
public enum mjtWrap : int{
|
||||
mjWRAP_NONE = 0,
|
||||
|
||||
@@ -10868,6 +10868,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
||||
.value("mjEQ_TENDON", mjEQ_TENDON)
|
||||
.value("mjEQ_FLEX", mjEQ_FLEX)
|
||||
.value("mjEQ_FLEXVERT", mjEQ_FLEXVERT)
|
||||
.value("mjEQ_FLEXSTRAIN", mjEQ_FLEXSTRAIN)
|
||||
.value("mjEQ_DISTANCE", mjEQ_DISTANCE);
|
||||
enum_<mjtEvent>("mjtEvent")
|
||||
.value("mjEVENT_NONE", mjEVENT_NONE)
|
||||
|
||||
Reference in New Issue
Block a user