From 2d12dee025b58c9f535b9adc9311006f9d9f4e4a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 07:13:17 -0700 Subject: [PATCH] Add dof="2d" option to flexcomp for in-plane deformations. PiperOrigin-RevId: 902620719 Change-Id: Ib06d3f7b9439e1d90a8373a4289ade0c327e72e4 --- doc/XMLreference.rst | 6 +- model/flex/gripper_2d.xml | 429 +++++++++++++++++++++++++++++++++++ src/user/user_flexcomp.cc | 11 + src/user/user_flexcomp.h | 1 + src/xml/xml_native_reader.cc | 3 +- test/user/user_flex_test.cc | 65 ++++++ 6 files changed, 513 insertions(+), 2 deletions(-) create mode 100644 model/flex/gripper_2d.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 7e5cfe5a..be74eede 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3593,7 +3593,7 @@ saving the XML: .. _body-flexcomp-dof: -:at:`dof`: :at-val:`[full, radial, trilinear, quadratic], "full"` +:at:`dof`: :at-val:`[full, radial, trilinear, quadratic, 2d], "full"` The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the different parametrizations with deformable spheres. The three models in the video are respectively `sphere_full `__, @@ -3608,6 +3608,10 @@ saving the XML: requires a free joint at the flex's parent in order for free body motion to be possible. This type of parametrization is appropriate for shapes that are relatively spherical. + **2d** + Two orthogonal translational dofs (X and Y) per vertex. This restricts the motion of the vertices to planes + parallel to the parent body's X-Y plane. + **trilinear** Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire flex, independent of the number of vertices. The positions of the vertices are updated using trilinear diff --git a/model/flex/gripper_2d.xml b/model/flex/gripper_2d.xml new file mode 100644 index 00000000..b6c6c611 --- /dev/null +++ b/model/flex/gripper_2d.xml @@ -0,0 +1,429 @@ + + + + + + diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 6bd14bb0..c62b5d6f 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -613,6 +613,17 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // add two orthogonal sliders (x and y only) + else if (doftype == mjFCOMPDOF_2D) { + for (int j=0; j < 2; j++) { + mjsJoint* jnt = mjs_addJoint(pb, 0); + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[j] = 1; + } + } + // construct body name, add to vertbody char txt[100]; mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 07977a39..782a7915 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -45,6 +45,7 @@ typedef enum _mjtDof { mjFCOMPDOF_RADIAL, mjFCOMPDOF_TRILINEAR, mjFCOMPDOF_QUADRATIC, + mjFCOMPDOF_2D, mjNFCOMPDOFS } mjtDof; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 360c46be..5e17aa1d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = { {"full", mjFCOMPDOF_FULL}, {"radial", mjFCOMPDOF_RADIAL}, {"trilinear", mjFCOMPDOF_TRILINEAR}, - {"quadratic", mjFCOMPDOF_QUADRATIC} + {"quadratic", mjFCOMPDOF_QUADRATIC}, + {"2d", mjFCOMPDOF_2D} }; diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 52e0bb30..f3df253c 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1227,6 +1227,71 @@ TEST_F(UserFlexTest, TotalMassQuadratic) { mj_deleteModel(m); } +TEST_F(UserFlexTest, Dof2d) { + // 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18 + static constexpr char xml_2d[] = R"( + + + + + + + + )"; + + // same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27 + static constexpr char xml_full[] = R"( + + + + + + + + )"; + + std::array error; + + // load 2d model + mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); + ASSERT_THAT(m_2d, NotNull()) << error.data(); + mjData* d_2d = mj_makeData(m_2d); + + // load full model + mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size()); + ASSERT_THAT(m_full, NotNull()) << error.data(); + mjData* d_full = mj_makeData(m_full); + + // verify DOF counts + EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs + EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs + + // same number of vertices and elements + EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert); + EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem); + + // each body has 2 DOFs in 2d mode, 3 in full mode + for (int i = 1; i < m_2d->nbody; i++) { + EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i; + } + for (int i = 1; i < m_full->nbody; i++) { + EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i; + } + + // simulate a few steps to make sure nothing crashes + for (int i = 0; i < 10; i++) { + mj_step(m_2d, d_2d); + mj_step(m_full, d_full); + } + + mj_deleteModel(m_2d); + mj_deleteModel(m_full); + mj_deleteData(d_2d); + mj_deleteData(d_full); +} + } // namespace } // namespace mujoco