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

Add vertex equality option.

Browse Source 
PiperOrigin-RevId: 860299454
Change-Id: I7befd8e5f4c8bac6d74684a96a9cc3cee9ae1e29
This commit is contained in:
Alessio Quaglino
2026-01-23 16:29:17 -08:00
committed by Copybara-Service
parent a5de40b400
commit 7da271c687
24 changed files with 391 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+29 -3
View File
@@ -3820,9 +3820,10 @@ element is used to adjust the properties of all edges in the flex.
.. _flexcomp-edge-equality:
:at:`equality`: :at-val:`[true, false], "false"`
When enabled, an equality constraint of :ref:`type flex<equality-flex>` is added to the model, referencing the
automatically-generated flex by name.
: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>`.
.. _flexcomp-edge-solref:
.. _flexcomp-edge-solimp:
@@ -4801,6 +4802,31 @@ as there are edges in the specified flex.
Name of the flex whose edges are being constrained.
.. _equality-flexvert:
:el-prefix:`equality/` |-| **flexvert** |m|
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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.
.. _equality-flexvert-name:
.. _equality-flexvert-class:
.. _equality-flexvert-active:
.. _equality-flexvert-solref:
.. _equality-flexvert-solimp:
:at:`name`, :at:`class`, :at:`active`, :at:`solref`, :at:`solimp`
Same as in :ref:`connect <equality-connect>` element.
.. _equality-flexvert-flex:
:at:`flex`: :at-val:`string, required`
Name of the flex whose vertices are being constrained.
.. _equality-distance:
:el-prefix:`equality/` |-| **distance** |m|
doc/XMLschema.rst
+24
View File
@@ -2021,6 +2021,30 @@
:ref:`solimp<equality-flex-solimp>`
.. dropdown:: flexvert |m|
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`name<equality-flexvert-name>`
.. grid-item::
:ref:`class<equality-flexvert-class>`
.. grid-item::
:ref:`flex<equality-flexvert-flex>`
.. grid-item::
:ref:`active<equality-flexvert-active>`
.. grid-item::
:ref:`solref<equality-flexvert-solref>`
.. grid-item::
:ref:`solimp<equality-flexvert-solimp>`
.. dropdown:: tendon |m|
doc/changelog.rst
+10
View File
@@ -32,6 +32,15 @@ Upcoming version (not yet released)
General
^^^^^^^
.. image:: images/changelog/poncho.png
:width: 45%
:align: right
:target: https://github.com/google-deepmind/mujoco/blob/main/model/flex/poncho.xml
- Added new :ref:`flexvert<equality-flexvert>` equality constraints that enable cloth simulations with coarser meshes.
This adds a new option ``vert`` to flexcomp edge :ref:`equality<flexcomp-edge-equality>` and the new equality type
:ref:`flexvert<equality-flexvert>`.
.. image:: images/XMLreference/rfcamera.png
:width: 45%
:align: right
@@ -65,6 +74,7 @@ General
- Allcating sizes in :ref:`mjModel` now use 64-bit rather than 32-bit integers to accommodate larger
scenes.
MJX
^^^
- Added ``actuator_length``, ``cdof`` and ``cdof_dof`` fields to ``mjx.Data``.
doc/images/changelog/poncho.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 608 KiB

doc/includes/references.h
+1
View File
@@ -603,6 +603,7 @@ typedef enum mjtEq_ { // type of equality constraint
mjEQ_JOINT, // couple the values of two scalar joints with cubic
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_DISTANCE // unsupported, will cause an error if used
} mjtEq;
typedef enum mjtWrap_ { // type of tendon wrap object
include/mujoco/mjmodel.h
+1
View File
@@ -210,6 +210,7 @@ typedef enum mjtEq_ { // type of equality constraint
mjEQ_JOINT, // couple the values of two scalar joints with cubic
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_DISTANCE // unsupported, will cause an error if used
} mjtEq;
model/flex/poncho.xml
+1 -1
View File
@@ -1413,7 +1413,7 @@
402 419 401
398 399 418
398 376 378">
<edge equality="true" damping="0.1"/>
<edge equality="vert" damping="0.1"/>
<elasticity young="3e5" poisson="0" thickness="8e-3" elastic2d="bend"/>
<contact solref="0.003"/>
</flexcomp>
python/mujoco/introspect/enums.py
+2 -1
View File
@@ -221,7 +221,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjEQ_JOINT', 2),
('mjEQ_TENDON', 3),
('mjEQ_FLEX', 4),
('mjEQ_DISTANCE', 5),
('mjEQ_FLEXVERT', 5),
('mjEQ_DISTANCE', 6),
]),
)),
('mjtWrap',
src/engine/engine_core_constraint.c
+62 -4
View File
@@ -376,6 +376,7 @@ 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 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;
@@ -643,6 +644,32 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
}
break;
case mjEQ_FLEXVERT:
// add two constraints per vertex
flex_vertadr = m->flex_vertadr[id[0]];
flex_vertnum = m->flex_vertnum[id[0]];
for (int v=flex_vertadr; v < flex_vertadr+flex_vertnum; v++) {
for (int j=0; j < 2; j++) {
cpos[0] = d->flexvert_length[2*v+j];
int row = 2*v+j;
if (issparse) {
mj_addConstraint(m, d, d->flexvert_J + m->flexvert_J_rowadr[row],
cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
m->flexvert_J_rownnz[row],
m->flexvert_J_colind + m->flexvert_J_rowadr[row]);
} else {
mju_zero(jac[0], nv); // reuse first row of jac[0]
int rowadr = m->flexvert_J_rowadr[row];
int rownnz = m->flexvert_J_rownnz[row];
for (int k=0; k<rownnz; k++) {
jac[0][m->flexvert_J_colind[rowadr+k]] = d->flexvert_J[rowadr+k];
}
mj_addConstraint(m, d, jac[0], cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
}
}
}
break;
default: // SHOULD NOT OCCUR
mjERROR("invalid equality constraint type %d", m->eq_type[i]);
}
@@ -1138,8 +1165,23 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
i--;
break;
case mjEQ_FLEXVERT:
// process all vertices for this flex
f = m->eq_obj1id[id];
int vertadr = m->flex_vertadr[f];
int vertnum = m->flex_vertnum[f];
for (int v=vertadr; v<vertadr+vertnum; v++) {
int bodyid = m->flex_vertbodyid[v];
dA[i++] = m->body_invweight0[2*bodyid];
dA[i++] = m->body_invweight0[2*bodyid];
}
// adjust constraint counter
i--;
break;
default:
mjERROR("unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR
mjERROR("unknown constraint type %d", d->efc_type[i]); // SHOULD NOT OCCUR
}
break;
@@ -1605,7 +1647,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
int nv = m->nv, neq = m->neq;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL;
int issparse = (nnz != NULL);
int flex_edgeadr, flex_edgenum;
int flex_edgeadr, flex_edgenum, flex_vertadr, flex_vertnum;
// disabled or no equality constraints: return
if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
@@ -1728,14 +1770,30 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
}
break;
case mjEQ_FLEXVERT:
flex_vertadr = m->flex_vertadr[id[0]];
flex_vertnum = m->flex_vertnum[id[0]];
size = 2 * flex_vertnum;
if (nnz) {
for (int v=flex_vertadr; v < flex_vertadr+flex_vertnum; v++) {
NV += m->flexvert_J_rownnz[2*v+0];
NV += m->flexvert_J_rownnz[2*v+1];
}
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown constraint type type %d", m->eq_type[i]); // SHOULD NOT OCCUR
mjERROR("unknown constraint type %d", m->eq_type[i]); // SHOULD NOT OCCUR
}
// accumulate counts; flex NV already accumulated
ne += mj_addConstraintCount(m, size, NV);
nnze += (m->eq_type[i] == mjEQ_FLEX) ? NV : size*NV;
if (m->eq_type[i] == mjEQ_FLEX || m->eq_type[i] == mjEQ_FLEXVERT) {
nnze += NV;
} else {
nnze += size*NV;
}
}
if (nnz) {
src/engine/engine_core_smooth.c
+5
View File
@@ -2775,6 +2775,11 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
break;
case mjEQ_FLEXVERT:
k = m->eq_obj1id[id];
i += 2*m->flex_vertnum[k];
break;
default:
mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR
}
src/engine/engine_io.c
+1
View File
@@ -1898,6 +1898,7 @@ const char* mj_validateReferences(const mjModel* m) {
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
if (obj1id >= m->nflex || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
src/engine/engine_island.c
+3 -1
View File
@@ -352,7 +352,9 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg
// row i is still in the same constraint: skip it,
if (efc_type == d->efc_type[i] && efc_id == d->efc_id[i]) {
// unless it is a flex equality, where the tree pattern changes per dof
if (!(efc_type == mjCNSTR_EQUALITY && m->eq_type[efc_id] == mjEQ_FLEX)) {
if (!(efc_type == mjCNSTR_EQUALITY &&
(m->eq_type[efc_id] == mjEQ_FLEX ||
m->eq_type[efc_id] == mjEQ_FLEXVERT))) {
continue;
}
}
src/engine/engine_sleep.c
+2
View File
@@ -399,6 +399,7 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
mjERROR("tendon equality does not yet support sleeping");
continue;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
mjERROR("flex equality does not yet support sleeping");
continue;
default:
@@ -641,6 +642,7 @@ static mjtSleepState mj_equalitySleepState(const mjModel* m, const mjData* d, in
objtype = mjOBJ_TENDON;
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
objtype = mjOBJ_FLEX;
break;
default:
src/user/user_flexcomp.cc
+2 -2
View File
@@ -81,7 +81,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
mjuu_setvec(scale, 1, 1, 1);
mass = 1;
inertiabox = 0.005;
equality = false;
equality = 0;
mjuu_setvec(pos, 0, 0, 0);
mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false;
@@ -602,7 +602,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
if (equality) {
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->Default()->spec);
pe->type = mjEQ_FLEX;
pe->type = equality == 1 ? mjEQ_FLEX : mjEQ_FLEXVERT;
pe->active = true;
mjs_setString(pe->name1, name.c_str());
}
src/user/user_flexcomp.h
+1 -1
View File
@@ -81,7 +81,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
bool equality; // create edge equality constraint
int equality; // create equality constraint, 0:none, 1:edge, 2:vert
std::string file; // mesh/gmsh file name
mjtDof doftype; // dof type, all vertices or trilinear interpolation
src/user/user_model.cc
+9 -4
View File
@@ -3403,11 +3403,16 @@ void mjCModel::CopyObjects(mjModel* m) {
// find equality constraint referencing this flex
m->flex_edgeequality[i] = 0;
for (int k=0; k < (int)equalities_.size(); k++) {
if (equalities_[k]->type == mjEQ_FLEX && equalities_[k]->name1_ == pfl->name) {
m->flex_edgeequality[i] = 1;
break;
if (equalities_[k]->name1_ == pfl->name) {
if (equalities_[k]->type == mjEQ_FLEX) {
m->flex_edgeequality[i] = 1;
break;
}
if (equalities_[k]->type == mjEQ_FLEXVERT) {
m->flex_edgeequality[i] = 2;
break;
}
}
// TODO: support flex_edgeequality = 2
}
// copy bvh data (flex aabb computed dynamically in mjData)
src/user/user_objects.cc
+2 -2
View File
@@ -5995,7 +5995,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) {
} else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT) {
object_type = mjOBJ_FLEX;
} else {
throw mjCError(this, "invalid type in equality constraint");
@@ -6052,7 +6052,7 @@ void mjCEquality::Compile(void) {
ResolveReferences(model);
// make sure flex is not rigid
if (type == mjEQ_FLEX && model->Flexes()[obj1id]->rigid) {
if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT) && model->Flexes()[obj1id]->rigid) {
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
}
}
src/xml/xml_native_reader.cc
+14 -4
View File
@@ -364,6 +364,8 @@ std::vector<const char*> MJCF[nMJCF] = {
"active", "solref", "solimp"},
{"flex", "*", "name", "class", "flex",
"active", "solref", "solimp"},
{"flexvert", "*", "name", "class", "flex",
"active", "solref", "solimp"},
{">"},
{"tendon", "*"},
@@ -667,13 +669,14 @@ const mjMap solver_map[solver_sz] = {
// constraint type
const int equality_sz = 6;
const int equality_sz = 7;
const mjMap equality_map[equality_sz] = {
{"connect", mjEQ_CONNECT},
{"weld", mjEQ_WELD},
{"joint", mjEQ_JOINT},
{"tendon", mjEQ_TENDON},
{"flex", mjEQ_FLEX},
{"flexvert", mjEQ_FLEXVERT},
{"distance", mjEQ_DISTANCE}
};
@@ -917,6 +920,14 @@ const mjMap elastic2d_map[5] = {
};
// flex equality type
const mjMap flexeq_map[3] = {
{"false", 0},
{"true", 1},
{"vert", 2},
};
//---------------------------------- class mjXReader implementation --------------------------------
@@ -2186,6 +2197,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
ReadAttrTxt(elem, "flex", name1, true);
break;
@@ -2744,9 +2756,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
// edge
XMLElement* edge = FirstChildElement(elem, "edge");
if (edge) {
if (MapValue(edge, "equality", &n, bool_map, 2)) {
fcomp.equality = (n == 1);
}
MapValue(edge, "equality", &fcomp.equality, flexeq_map, 3);
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);
src/xml/xml_native_reader.h
+1 -1
View File
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 244
#define nMJCF 245
extern std::vector<const char*> MJCF[nMJCF];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+1
View File
@@ -710,6 +710,7 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
break;
test/engine/engine_core_constraint_test.cc
+70
View File
@@ -419,5 +419,75 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
mj_deleteModel(model);
}
// check mjEQ_FLEXVERT
TEST_F(CoreConstraintTest, FlexvertEquality) {
constexpr char xml[] = R"(
<mujoco>
<option jacobian="dense"/>
<worldbody>
<flexcomp name="flex" type="grid" count="3 3 1" spacing=".05 .15 .25" dim="2">
<edge equality="vert"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
mjData* data = mj_makeData(model);
ASSERT_EQ(model->neq, 1);
ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT);
ASSERT_EQ(model->nflex, 1);
ASSERT_EQ(model->flex_vertnum[0], 9);
ASSERT_EQ(model->flex_edgenum[0], 16);
// step1 to populate flexvert_length
mj_step1(model, data);
EXPECT_EQ(data->ne, 2*model->flex_vertnum[0]);
EXPECT_EQ(data->nefc, 18);
for (int i = 0; i < 18; ++i) {
EXPECT_EQ(data->efc_type[i], mjCNSTR_EQUALITY);
EXPECT_NEAR(data->efc_pos[i], 0, 1e-9);
}
// check that efc_J has rigid-body motions in kernel
std::vector<mjtNum> qvel(model->nv);
std::vector<mjtNum> Jqvel(data->nefc);
// pure translations
for (int i = 0; i < 3; ++i) {
mju_zero(qvel.data(), model->nv);
for (int j = 0; j < model->flex_vertnum[0]; ++j) {
qvel[3*j+i] = 1.0;
}
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, 1e-9);
}
}
// pure rotations
for (int i = 0; i < 3; ++i) {
mju_zero(qvel.data(), model->nv);
for (int j = 0; j < model->flex_vertnum[0]; ++j) {
mjtNum* p = data->flexvert_xpos + 3 * j;
mjtNum axisvel[3] = {0};
axisvel[i] = 1.0;
mjtNum linvel[3];
mju_cross(linvel, axisvel, p);
qvel[3 * j + 0] = linvel[0];
qvel[3 * j + 1] = linvel[1];
qvel[3 * j + 2] = linvel[2];
}
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, 1e-9);
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_core_smooth_test.cc
+147 -5
View File
@@ -27,6 +27,7 @@
#include <gtest/gtest.h>
#include <absl/types/span.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjspec.h>
#include <mujoco/mjxmacro.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
@@ -882,8 +883,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
<mujoco>
<option jacobian="sparse"/>
<worldbody>
<flexcomp name="g" type="grid" count="3 3 1" spacing=".5 .5 .5" dim="2" radius=".05">
<edge equality="true"/>
<flexcomp name="g" type="grid" count="3 3 1" spacing="1 1 1" dim="2" radius=".05">
<edge equality="vert"/>
</flexcomp>
</worldbody>
</mujoco>
@@ -900,9 +901,6 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// nJfv = 42 + 60 + 21 = 123
EXPECT_EQ(m->nJfv, 123);
// Set edge equality to 2
m->flex_edgeequality[0] = 2;
// Run kinematics to populate xpos/xmat initially
mj_fwdKinematics(m, d);
@@ -1062,6 +1060,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// Strain E = C - I = 3I.
// Invariant 0: Trace(E) = 3 + 3 = 6
// Invariant 1: Det(C) - 1 = 4 * 4 - 1 = 15
// Note: constraints are now scaled by sqrt(mass)
for (int i=0; i < nvert; i++) {
EXPECT_NEAR(d->flexvert_length[2 * i + 0], 6.0 * scale, 1e-5);
EXPECT_NEAR(d->flexvert_length[2 * i + 1], 15.0 * scale, 1e-5);
@@ -1077,5 +1076,148 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
mj_deleteModel(m);
}
// Test failure case for flexvert_J sparsity with skipped flexes
TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
constexpr char xml[] = R"(
<mujoco>
<option jacobian="sparse"/>
<worldbody>
<body name="body0">
<joint type="free"/>
<geom type="sphere" size="0.1"/>
<flexcomp name="f0" type="grid" count="2 2 1" spacing="0.1 0.1 0.1" radius="0.01" dim="2">
<edge damping="1"/>
</flexcomp>
</body>
<body name="body1" pos="1 0 0">
<joint type="free"/>
<geom type="sphere" size="0.1"/>
<!-- This flex is rigid, so it will be skipped in flexvert_J computation -->
<flexcomp name="f1" type="grid" count="2 2 1" spacing="0.1 0.1 0.1" radius="0.01" dim="2" rigid="true"/>
</body>
<body name="body2" pos="2 0 0">
<joint type="free"/>
<geom type="sphere" size="0.1"/>
<flexcomp name="f2" type="grid" count="2 2 1" spacing="0.1 0.1 0.1" radius="0.01" dim="2">
<edge damping="1"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// Forward dynamics to compute Jacobians
mj_forward(model, data);
// Check sparsity overlap
// Flex 0 starts at row 0
// Flex 1 is skipped
// Flex 2 should start after Flex 0's rows
// If the bug exists, Flex 2's rows might start at 0, overwriting Flex 0
int f0_vert_start = model->flex_vertadr[0];
int f0_vert_num = model->flex_vertnum[0];
int f2_vert_start = model->flex_vertadr[2];
// Check last row of flex 0
int f0_last_row = 2 * (f0_vert_start + f0_vert_num - 1) + 1;
int f0_end_adr = model->flexvert_J_rowadr[f0_last_row] +
model->flexvert_J_rownnz[f0_last_row];
// Check first row of flex 2
int f2_first_row = 2 * (f2_vert_start);
int f2_start_adr = model->flexvert_J_rowadr[f2_first_row];
// Verify that Flex 2 starts AFTER Flex 0 ends
EXPECT_GE(f2_start_adr, f0_end_adr)
<< "Flex 2 Jacobian overwrites Flex 0 Jacobian due to skipped Flex 1";
mj_deleteData(data);
mj_deleteModel(model);
}
// Test stability of flexvert constraint under different integrator/solver
// configurations
TEST_F(CoreSmoothTest, FlexVertStability) {
constexpr char xml[] = R"(
<mujoco>
<option jacobian="sparse"/>
<worldbody>
<geom name="floor" type="plane" size="0 0 .1"/>
<flexcomp name="flex" type="grid" count="10 10 1" spacing="0.05 0.05 0.05" radius="0.01" dim="2" mass="1" pos="0 0 1">
<edge equality="vert" damping="0.1"/>
<contact solref="0.003"/>
<elasticity young="3e5" poisson="0" thickness="8e-3" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
struct TestCase {
mjtIntegrator integrator;
mjtSolver solver;
mjtNum tolerance;
bool expect_stable;
};
std::vector<TestCase> cases = {
// Explicit integration with Newton solver should be stable
{mjINT_RK4, mjSOL_NEWTON, 1e-6, true},
// ImplicitFast with CG solver should now be STABLE with mass weighting
{mjINT_IMPLICITFAST, mjSOL_CG, 1e-6, true},
// ImplicitFast with Newton solver should be stable
{mjINT_IMPLICITFAST, mjSOL_NEWTON, 1e-6, true},
};
for (const auto& test_case : cases) {
char error[1024];
mjSpec* spec = mj_parseXMLString(xml, nullptr, error, sizeof(error));
ASSERT_THAT(spec, NotNull()) << error;
spec->option.integrator = test_case.integrator;
spec->option.solver = test_case.solver;
spec->option.tolerance = test_case.tolerance;
mjModel* model = mj_compile(spec, nullptr);
ASSERT_THAT(model, NotNull())
<< error << " (Case: " << test_case.integrator << ", "
<< test_case.solver << ", " << test_case.tolerance << ")";
mjData* data = mj_makeData(model);
// Run simulation
bool exploded = false;
for (int i = 0; i < 100; ++i) {
mj_step(model, data);
// Check for explosion
for (int j = 0; j < model->nv; ++j) {
if (mju_abs(data->qvel[j]) > 1000.0) {
exploded = true;
break;
}
}
if (exploded) break;
}
if (test_case.expect_stable) {
EXPECT_FALSE(exploded) << "Expected stable simulation for "
<< test_case.integrator << "/" << test_case.solver;
} else {
EXPECT_TRUE(exploded) << "Expected explosion for " << test_case.integrator
<< "/" << test_case.solver
<< ". If this passes, the reproduction is no "
"longer valid (which is good, but unexpected).";
}
mj_deleteData(data);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
}
} // namespace
} // namespace mujoco
unity/Runtime/Bindings/MjBindings.cs
+2 -1
View File
@@ -287,7 +287,8 @@ public enum mjtEq : int{
mjEQ_JOINT = 2,
mjEQ_TENDON = 3,
mjEQ_FLEX = 4,
mjEQ_DISTANCE = 5,
mjEQ_FLEXVERT = 5,
mjEQ_DISTANCE = 6,
}
public enum mjtWrap : int{
mjWRAP_NONE = 0,
wasm/codegen/generated/bindings.cc
+1
View File
@@ -10514,6 +10514,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.value("mjEQ_JOINT", mjEQ_JOINT)
.value("mjEQ_TENDON", mjEQ_TENDON)
.value("mjEQ_FLEX", mjEQ_FLEX)
.value("mjEQ_FLEXVERT", mjEQ_FLEXVERT)
.value("mjEQ_DISTANCE", mjEQ_DISTANCE);
enum_<mjtEvent>("mjtEvent")
.value("mjEVENT_NONE", mjEVENT_NONE)