diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index bca349cc..df9972d1 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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` 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`. .. _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 ` 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| diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index e9eca9d5..5f92bdb1 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2021,6 +2021,30 @@ :ref:`solimp` + .. dropdown:: flexvert |m| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`class` + + .. grid-item:: + :ref:`flex` + + .. grid-item:: + :ref:`active` + + .. grid-item:: + :ref:`solref` + + .. grid-item:: + :ref:`solimp` + + .. dropdown:: tendon |m| diff --git a/doc/changelog.rst b/doc/changelog.rst index 76e3c878..877c06f8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -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 constraints that enable cloth simulations with coarser meshes. + This adds a new option ``vert`` to flexcomp edge :ref:`equality` and the new equality type + :ref:`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``. diff --git a/doc/images/changelog/poncho.png b/doc/images/changelog/poncho.png new file mode 100644 index 00000000..40034f80 Binary files /dev/null and b/doc/images/changelog/poncho.png differ diff --git a/doc/includes/references.h b/doc/includes/references.h index 6b9abd51..8080355e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index af81166e..c0be229c 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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; diff --git a/model/flex/poncho.xml b/model/flex/poncho.xml index 8e6e2e80..ffa8aa28 100644 --- a/model/flex/poncho.xml +++ b/model/flex/poncho.xml @@ -1413,7 +1413,7 @@ 402 419 401 398 399 418 398 376 378"> - + diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 2c0924b1..1e8fa84e 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -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', diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index c90f35cc..ab6abd39 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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; kflexvert_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; vflex_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) { diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 37344cac..b8d70f77 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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 } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 749b7593..4a356b8f 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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."; } diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index b2f62c4c..a7c3323a 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -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; } } diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c index f6d5892f..9805e0ab 100644 --- a/src/engine/engine_sleep.c +++ b/src/engine/engine_sleep.c @@ -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: diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 83fa18a7..7f15a9b8 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -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()); } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index bf278c3f..dd3374c8 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -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 diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f201daed..a5833ffd 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 124c8261..a304aaef 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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); } } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index e1485b88..807570a5 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -364,6 +364,8 @@ std::vector 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); diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 570a6542..2ad11e6d 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -102,7 +102,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 244 +#define nMJCF 245 extern std::vector MJCF[nMJCF]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 1840f605..07d8cbce 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index d37a4f15..3ac2944c 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -419,5 +419,75 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) { mj_deleteModel(model); } +// check mjEQ_FLEXVERT +TEST_F(CoreConstraintTest, FlexvertEquality) { + constexpr char xml[] = R"( + + + )"; + 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 qvel(model->nv); + std::vector 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 diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index c08e0ffa..247f453f 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include "test/fixture.h" @@ -882,8 +883,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { @@ -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"( + + + )"; + 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"( + + + )"; + + struct TestCase { + mjtIntegrator integrator; + mjtSolver solver; + mjtNum tolerance; + bool expect_stable; + }; + + std::vector 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 diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 9406207e..d2e8ab36 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index d27a4487..18e54a4c 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -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") .value("mjEVENT_NONE", mjEVENT_NONE)