From 01d1ac34e083f1dc536af81ae764f3b9c6fa7fa3 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Oct 2025 04:35:44 -0700 Subject: [PATCH] Add quadratic interpolation to flex. PiperOrigin-RevId: 822055781 Change-Id: Ibc252ac560de666b0b28f868e46b88d3ed92f67d --- model/flex/quadratic.xml | 39 +++++++++++++++++++++++++++++++++ src/engine/engine_core_smooth.c | 2 +- src/user/user_flexcomp.cc | 33 +++++++++++++++++----------- src/user/user_flexcomp.h | 1 + src/user/user_mesh.cc | 9 +++++--- src/user/user_model.cc | 2 +- src/user/user_objects.h | 4 ++++ src/xml/xml_native_reader.cc | 6 ++++- 8 files changed, 77 insertions(+), 19 deletions(-) create mode 100644 model/flex/quadratic.xml diff --git a/model/flex/quadratic.xml b/model/flex/quadratic.xml new file mode 100644 index 00000000..02c7b996 --- /dev/null +++ b/model/flex/quadratic.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 513886bb..ac4129b6 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -462,7 +462,7 @@ void mj_flex(const mjModel* m, mjData* d) { // trilinear interpolation else { - mjtNum nodexpos[mjMAXFLEXNODES]; + mjtNum nodexpos[3*mjMAXFLEXNODES]; if (m->flex_centered[f]) { for (int i=nstart; i < nend; i++) { mju_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 6c4f5eb5..ace33c74 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -529,23 +529,28 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { } // create nodal mesh for trilinear interpolation - if (doftype == mjFCOMPDOF_TRILINEAR) { - std::vector node(24, 0); - for (int i=0; i < 2; i++) { - for (int j=0; j < 2; j++) { - for (int k=0; k < 2; k++) { - if (pinned[i*4+j*2+k]) { - node[3*(i*4+j*2+k)+0] = i == 0 ? minmax[0] : minmax[3]; - node[3*(i*4+j*2+k)+1] = j == 0 ? minmax[1] : minmax[4]; - node[3*(i*4+j*2+k)+2] = k == 0 ? minmax[2] : minmax[5]; + if (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) { + int order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2; + flex->SetOrder(order); + std::vector node(3*(order+1)*(order+1)*(order+1), 0); + int idx = 0; + double step = 1.0 / (double)order; + for (int i=0; i <= order; i++) { + for (int j=0; j <= order; j++) { + for (int k=0; k <= order; k++) { + if (pinned[idx]) { + node[3*idx+0] = minmax[0] + i * step * (minmax[3] - minmax[0]); + node[3*idx+1] = minmax[1] + j * step * (minmax[4] - minmax[1]); + node[3*idx+2] = minmax[2] + k * step * (minmax[5] - minmax[2]); mjs_appendString(pf->nodebody, mjs_getName(body->element)->c_str()); + idx++; continue; } mjsBody* pb = mjs_addBody(body, 0); - pb->pos[0] = i == 0 ? minmax[0] : minmax[3]; - pb->pos[1] = j == 0 ? minmax[1] : minmax[4]; - pb->pos[2] = k == 0 ? minmax[2] : minmax[5]; + pb->pos[0] = minmax[0] + i * step * (minmax[3] - minmax[0]); + pb->pos[1] = minmax[1] + j * step * (minmax[4] - minmax[1]); + pb->pos[2] = minmax[2] + k * step * (minmax[5] - minmax[2]); mjuu_zerovec(pb->ipos, 3); pb->mass = mass / 8; pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; @@ -573,6 +578,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k); mjs_setName(pb->element, txt); mjs_appendString(pf->nodebody, mjs_getName(pb->element)->c_str()); + + idx++; } } } @@ -582,7 +589,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { } } - if (!centered || doftype == mjFCOMPDOF_TRILINEAR) { + if (!centered || doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) { mjs_setDouble(pf->vert, point.data(), point.size()); } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 577c06b7..bf278c3f 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -44,6 +44,7 @@ typedef enum _mjtDof { mjFCOMPDOF_FULL = 0, mjFCOMPDOF_RADIAL, mjFCOMPDOF_TRILINEAR, + mjFCOMPDOF_QUADRATIC, mjNFCOMPDOFS } mjtDof; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index d1e01d04..775b39d9 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4109,9 +4109,12 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // set nnode - nnode = (int)nodebody_.size(); - if (nnode && nnode != 8) { - throw mjCError(this, "number of nodes must be 2^dim, it is %d", "", nnode); + nnode = static_cast(nodebody_.size()); + if (nnode && !order_) { + order_ = std::pow(nnode, 1.0 / 3) - 1; + if (nnode != std::pow(order_ + 1, 3)) { + throw mjCError(this, "number of nodes must be %d^3 but it is %d", nullptr, order_, nnode); + } } // check elem vertex ids diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 2f7a4287..ee8210bc 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3377,7 +3377,7 @@ void mjCModel::CopyObjects(mjModel* m) { } // set interpolation type, only two types for now - m->flex_interp[i] = pfl->interpolated; + m->flex_interp[i] = pfl->order_; // convert edge pairs to int array, set edge rigid for (int k=0; k < pfl->nedge; k++) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index b25e42f9..e0ccb65b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1025,6 +1025,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { static constexpr int kNumEdges[3] = {1, 3, 6}; // number of edges per element indexed by dim + void SetOrder(int order) { order_ = order; } // set interpolation order + private: void Compile(const mjVFS* vfs); // compiler void CreateBVH(void); // create flex BVH @@ -1032,6 +1034,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions + + int order_ = 0; // interpolation order }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 0796a43f..cd3c2316 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -859,7 +859,8 @@ const mjMap fcomp_map[mjNFCOMPTYPES] = { const mjMap fdof_map[mjNFCOMPDOFS] = { {"full", mjFCOMPDOF_FULL}, {"radial", mjFCOMPDOF_RADIAL}, - {"trilinear", mjFCOMPDOF_TRILINEAR} + {"trilinear", mjFCOMPDOF_TRILINEAR}, + {"quadratic", mjFCOMPDOF_QUADRATIC} }; @@ -2722,6 +2723,9 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { ReadAttr(elasticity, "damping", 1, &dflex.damping, text); ReadAttr(elasticity, "thickness", 1, &dflex.thickness, text); MapValue(elasticity, "elastic2d", &dflex.elastic2d, elastic2d_map, 4); + if (fcomp.doftype == mjFCOMPDOF_QUADRATIC) { + throw mjXError(elasticity, "elasticity is not yet supported for quadratic flex"); + } } // check errors