diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a97481a7..e1b78754 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3765,7 +3765,7 @@ Associate this composite with an :ref:`engine plugin`. Either :at:`plu .. _body-flexcomp-euler: -.. _body-flexcomp-selfcollide: +.. _body-flexcomp-group: .. _body-flexcomp-flatskin: @@ -3820,6 +3820,15 @@ Associate this composite with an :ref:`engine plugin`. Either :at:`plu .. _flex-contact-gap: .. _flexcomp-contact-gap: +.. _flex-contact-internal: +.. _flexcomp-contact-internal: + +.. _flex-contact-selfcollide: +.. _flexcomp-contact-selfcollide: + +.. _flex-contact-activelayers: +.. _flexcomp-contact-activelayers: + .. _flexcomp-pin: .. _flexcomp-pin-id: @@ -3830,6 +3839,12 @@ Associate this composite with an :ref:`engine plugin`. Either :at:`plu .. _flexcomp-pin-gridrange: +.. _flexcomp-plugin: + +.. _flexcomp-plugin-plugin: + +.. _flexcomp-plugin-instance: + .. _deformable: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 0458347c..149ebc74 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -485,19 +485,19 @@ | :ref:`flexcomp | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`name` | :ref:`class` | :ref:`type` | :ref:`dim` | | +| | | | :ref:`name` | :ref:`class` | :ref:`type` | :ref:`group` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`flatskin` | :ref:`count` | :ref:`spacing` | :ref:`radius` | | +| | | | :ref:`dim` | :ref:`count` | :ref:`spacing` | :ref:`radius` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`rigid` | :ref:`mass` | :ref:`inertiabox` | :ref:`scale` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`file` | :ref:`point` | :ref:`element` | :ref:`texcoord` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`material` | :ref:`rgba` | :ref:`selfcollide` | :ref:`flatskin` | | +| | | | :ref:`material` | :ref:`rgba` | :ref:`flatskin` | :ref:`pos` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`pos` | :ref:`quat` | :ref:`axisangle` | :ref:`xyaxes` | | +| | | | :ref:`quat` | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`zaxis` | :ref:`euler` | | | | +| | | | :ref:`euler` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flexcomp |br| |_2| |L| | | .. table:: | @@ -517,7 +517,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`friction` | :ref:`solmix` | :ref:`solref` | :ref:`solimp` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`margin` | :ref:`gap` | | | | +| | | | :ref:`margin` | :ref:`gap` | :ref:`internal` | :ref:`selfcollide` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`activelayers` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flexcomp |br| |_2| |L| | | .. table:: | @@ -527,6 +529,20 @@ | | | | :ref:`id` | :ref:`range` | :ref:`grid` | :ref:`gridrange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| flexcomp |br| |_2| |L| | | .. table:: | +| :ref:`plugin | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`plugin` | :ref:`instance` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_3| plugin |br| |_3| |L| | | .. table:: | +| :ref:`config | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`key` | :ref:`value` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | *no attributes* | | :ref:`deformable` | | | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ @@ -536,9 +552,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`group` | :ref:`dim` | :ref:`radius` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`material` | :ref:`rgba` | :ref:`flatskin` | :ref:`selfcollide` | | +| | | | :ref:`material` | :ref:`rgba` | :ref:`flatskin` | :ref:`body` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`body` | :ref:`vertex` | :ref:`element` | :ref:`texcoord` | | +| | | | :ref:`vertex` | :ref:`element` | :ref:`texcoord` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flex |br| |_2| |L| | | .. table:: | @@ -549,7 +565,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`friction` | :ref:`solmix` | :ref:`solref` | :ref:`solimp` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`margin` | :ref:`gap` | | | | +| | | | :ref:`margin` | :ref:`gap` | :ref:`internal` | :ref:`selfcollide` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`activelayers` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flex |br| |_2| |L| | | .. table:: | diff --git a/model/plugin/elasticity/flag.xml b/model/plugin/elasticity/flag.xml index cfe4b6c1..ad093663 100644 --- a/model/plugin/elasticity/flag.xml +++ b/model/plugin/elasticity/flag.xml @@ -39,7 +39,6 @@ - diff --git a/model/plugin/elasticity/flag_flex.xml b/model/plugin/elasticity/flag_flex.xml new file mode 100644 index 00000000..667a8c36 --- /dev/null +++ b/model/plugin/elasticity/flag_flex.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/elasticity/floppy_flex.xml b/model/plugin/elasticity/floppy_flex.xml new file mode 100644 index 00000000..a8645f2f --- /dev/null +++ b/model/plugin/elasticity/floppy_flex.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + diff --git a/model/plugin/elasticity/jelly_flex.xml b/model/plugin/elasticity/jelly_flex.xml new file mode 100644 index 00000000..9a0af65d --- /dev/null +++ b/model/plugin/elasticity/jelly_flex.xml @@ -0,0 +1,56 @@ + + + + + + + + + + diff --git a/model/plugin/elasticity/plate.xml b/model/plugin/elasticity/plate.xml index a3eb8b07..24691e05 100644 --- a/model/plugin/elasticity/plate.xml +++ b/model/plugin/elasticity/plate.xml @@ -44,7 +44,6 @@ - diff --git a/model/plugin/elasticity/plate_flex.xml b/model/plugin/elasticity/plate_flex.xml new file mode 100644 index 00000000..b490b3ae --- /dev/null +++ b/model/plugin/elasticity/plate_flex.xml @@ -0,0 +1,53 @@ + + + + + + + + + + diff --git a/model/plugin/elasticity/poncho.xml b/model/plugin/elasticity/poncho.xml index 859bf95d..4a05599e 100644 --- a/model/plugin/elasticity/poncho.xml +++ b/model/plugin/elasticity/poncho.xml @@ -1407,7 +1407,6 @@ - diff --git a/model/plugin/elasticity/poncho_flex.xml b/model/plugin/elasticity/poncho_flex.xml new file mode 100644 index 00000000..d51df2f9 --- /dev/null +++ b/model/plugin/elasticity/poncho_flex.xml @@ -0,0 +1,1427 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/elasticity/press_flex.xml b/model/plugin/elasticity/press_flex.xml new file mode 100644 index 00000000..c2545652 --- /dev/null +++ b/model/plugin/elasticity/press_flex.xml @@ -0,0 +1,70 @@ + + + + + + + + + + + + diff --git a/plugin/elasticity/elasticity.cc b/plugin/elasticity/elasticity.cc index 35c0e0fc..7b1d0d7e 100644 --- a/plugin/elasticity/elasticity.cc +++ b/plugin/elasticity/elasticity.cc @@ -18,7 +18,6 @@ #include #include #include -#include #include #include @@ -47,19 +46,4 @@ bool CheckAttr(const char* name, const mjModel* m, int instance) { return end == value.data() + value.size(); } -mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) { - mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; - return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2]; -} - -void UpdateSquaredLengths(std::vector& len, - const std::vector >& edges, - const mjtNum* x) { - for (int e = 0; e < len.size(); e++) { - const mjtNum* p0 = x + 3*edges[e].first; - const mjtNum* p1 = x + 3*edges[e].second; - len[e] = SquaredDist3(p0, p1); - } -} - } // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/elasticity.h b/plugin/elasticity/elasticity.h index 54b0fb14..18444fd9 100644 --- a/plugin/elasticity/elasticity.h +++ b/plugin/elasticity/elasticity.h @@ -31,20 +31,27 @@ struct PairHash } }; +inline mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) { + mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; + return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2]; +} + +inline void UpdateSquaredLengths(std::vector& len, + const std::vector >& edges, + const mjtNum* x) { + for (int e = 0; e < len.size(); e++) { + const mjtNum* p0 = x + 3*edges[e].first; + const mjtNum* p1 = x + 3*edges[e].second; + len[e] = SquaredDist3(p0, p1); + } +} + // copied from mjXUtil void String2Vector(const std::string& txt, std::vector& vec); // reads numeric attributes bool CheckAttr(const char* name, const mjModel* m, int instance); -// Cartesian distance between 3D vectors -mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]); - -// updates square lengths of edges -void UpdateSquaredLengths(std::vector& len, - const std::vector >& edges, - const mjtNum* x); - } // namespace mujoco::plugin::elasticity #endif // MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_ diff --git a/plugin/elasticity/shell.cc b/plugin/elasticity/shell.cc index 8c671436..cf272a04 100644 --- a/plugin/elasticity/shell.cc +++ b/plugin/elasticity/shell.cc @@ -13,13 +13,10 @@ // limitations under the License. #include -#include +#include #include -#include #include #include -#include -#include #include #include #include @@ -29,6 +26,7 @@ #include #include #include +#include "elasticity.h" #include "shell.h" @@ -40,22 +38,6 @@ constexpr int kNumEdges = Stencil2D::kNumEdges; constexpr int kNumVerts = Stencil2D::kNumVerts; constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}}; -// copied from void mjXUtil -void String2Vector(const std::string& txt, std::vector& vec) { - std::stringstream strm(txt); - vec.clear(); - - while (!strm.eof()) { - int num; - strm >> num; - if (strm.fail()) { - break; - } else { - vec.push_back(num); - } - } -} - // cotangent between two edges mjtNum cot(mjtNum* x, int v0, int v1, int v2) { mjtNum normal[3]; @@ -82,28 +64,12 @@ mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) { return mju_norm3(normal) / 2; } -// reads numeric attributes -bool CheckAttr(const char* name, const mjModel* m, int instance) { - char* end; - std::string value = mj_getPluginConfig(m, instance, name); - value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end()); - strtod(value.c_str(), &end); - return end == value.data() + value.size(); -} - -struct PairHash -{ - template - std::size_t operator() (const std::pair& pair) const { - return std::hash()(pair.first) ^ std::hash()(pair.second); - } -}; - } // namespace // factory function std::optional Shell::Create(const mjModel* m, mjData* d, int instance) { if (CheckAttr("face", m, instance) && + CheckAttr("edge", m, instance) && CheckAttr("poisson", m, instance) && CheckAttr("young", m, instance) && CheckAttr("thickness", m, instance)) { @@ -111,11 +77,10 @@ std::optional Shell::Create(const mjModel* m, mjData* d, int instance) { mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr); mjtNum thick = strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr); - mjtNum damp = - strtod(mj_getPluginConfig(m, instance, "damping"), nullptr); - std::vector face; + std::vector face, edge; String2Vector(mj_getPluginConfig(m, instance, "face"), face); - return Shell(m, d, instance, nu, E, damp, thick, face); + String2Vector(mj_getPluginConfig(m, instance, "edge"), edge); + return Shell(m, d, instance, nu, E, thick, face, edge); } else { mju_warning("Invalid parameter specification in shell plugin"); return std::nullopt; @@ -123,13 +88,14 @@ std::optional Shell::Create(const mjModel* m, mjData* d, int instance) { } // create map from triangles to vertices and edges and from edges to vertices -void Shell::CreateStencils(const std::vector& face) { +void Shell::CreateStencils(const std::vector& simplex, + const std::vector& edgeidx) { // populate stencil - nt = face.size() / kNumVerts; + nt = simplex.size() / kNumVerts; elements.resize(nt); for (int t = 0; t < nt; t++) { for (int v = 0; v < kNumVerts; v++) { - elements[t].vertices[v] = face[kNumVerts*t+v]-1; + elements[t].vertices[v] = simplex[kNumVerts*t+v]; } } @@ -162,14 +128,19 @@ void Shell::CreateStencils(const std::vector& face) { elements[t].edges[e] = it->second; flaps[it->second].vertices[3] = v[(edge[e][1]+1) % 3]; } + + if (!edgeidx.empty()) { + assert(elements[t].edges[e] == edgeidx[kNumEdges*t+e]); + } } } } // plugin constructor Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, - mjtNum damp, mjtNum thick, const std::vector& face) - : damping(damp), thickness(thick) { + mjtNum thick, const std::vector& face, + const std::vector& edgeidx) + : thickness(thick) { // count plugin bodies nv = ne = 0; for (int i = 1; i < m->nbody; i++) { @@ -181,7 +152,7 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, } // generate triangles from the vertices - CreateStencils(face); + CreateStencils(face, edgeidx); // material parameters mjtNum mu = E / (2*(1+nu)); @@ -234,10 +205,6 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, } void Shell::Compute(const mjModel* m, mjData* d, int instance) { - mjtNum kD = damping / m->opt.timestep; - - // compute bending contribution - mjtNum embedding; for (int e = 0; e < ne; e++) { int* v = flaps[e].vertices; mjtNum force[12] = {0}; @@ -248,8 +215,7 @@ void Shell::Compute(const mjModel* m, mjData* d, int instance) { for (int i = 0; i < StencilFlap::kNumVerts; i++) { for (int j = 0; j < StencilFlap::kNumVerts; j++) { for (int x = 0; x < 3; x++) { - embedding = d->xpos[3*(i0+v[j])+x]*(kD+1) - position[3*v[j]+x]*kD; - force[3*i+x] += bending[16*e+4*i+j] * embedding; + force[3*i+x] += bending[16*e+4*i+j] * d->xpos[3*(i0+v[j])+x]; } } } @@ -275,8 +241,8 @@ void Shell::RegisterPlugin() { plugin.name = "mujoco.elasticity.shell"; plugin.capabilityflags |= mjPLUGIN_PASSIVE; - const char* attributes[] = {"face", "young", "poisson", "damping", - "thickness"}; + const char* attributes[] = {"face", "edge", "young", + "poisson", "thickness"}; plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); plugin.attributes = attributes; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; diff --git a/plugin/elasticity/shell.h b/plugin/elasticity/shell.h index 16dc66a5..88a4ae1d 100644 --- a/plugin/elasticity/shell.h +++ b/plugin/elasticity/shell.h @@ -64,14 +64,15 @@ class Shell { std::vector position; // previous-step positions (nv x 3) std::vector bending; // bending Hessian (ne x 16) - mjtNum damping; mjtNum thickness; private: Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, - mjtNum damp, mjtNum thick, const std::vector& face); + mjtNum thick, const std::vector& face, + const std::vector& edgeidx); - void CreateStencils(const std::vector& face); + void CreateStencils(const std::vector& simplex, + const std::vector& edgeidx); }; } // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc index 84130bff..b54a4282 100644 --- a/plugin/elasticity/solid.cc +++ b/plugin/elasticity/solid.cc @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -100,15 +101,17 @@ void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3], // factory function std::optional Solid::Create(const mjModel* m, mjData* d, int instance) { if (CheckAttr("face", m, instance) && + CheckAttr("edge", m, instance) && CheckAttr("poisson", m, instance) && CheckAttr("young", m, instance)) { mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr); mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr); mjtNum damp = strtod(mj_getPluginConfig(m, instance, "damping"), nullptr); - std::vector face; + std::vector face, edge; String2Vector(mj_getPluginConfig(m, instance, "face"), face); - return Solid(m, d, instance, nu, E, damp, face); + String2Vector(mj_getPluginConfig(m, instance, "edge"), edge); + return Solid(m, d, instance, nu, E, damp, face, edge); } else { mju_warning("Invalid parameter specification in solid plugin"); return std::nullopt; @@ -116,13 +119,14 @@ std::optional Solid::Create(const mjModel* m, mjData* d, int instance) { } // create map from tetrahedra to vertices and edges and from edges to vertices -void Solid::CreateStencils(const std::vector& simplex) { +void Solid::CreateStencils(const std::vector& simplex, + const std::vector& edgeidx) { // populate stencil nt = simplex.size() / kNumVerts; elements.resize(nt); for (int t = 0; t < nt; t++) { for (int v = 0; v < kNumVerts; v++) { - elements[t].vertices[v] = simplex[kNumVerts*t+v]-1; + elements[t].vertices[v] = simplex[kNumVerts*t+v]; } } @@ -149,14 +153,19 @@ void Solid::CreateStencils(const std::vector& simplex) { } else { elements[t].edges[e] = it->second; } + + if (!edgeidx.empty()) { + assert(elements[t].edges[e] == edgeidx[kNumEdges*t+e]); + } } } } // plugin constructor Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, - mjtNum damp, const std::vector& simplex) - : damping(damp) { + mjtNum damp, const std::vector& simplex, + const std::vector& edgeidx) + : f0(-1), damping(damp) { // count plugin bodies nv = ne = 0; for (int i = 1; i < m->nbody; i++) { @@ -167,8 +176,16 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, } } + // count flexes + for (int i = 0; i < m->nflex; i++) { + if (m->flex_vertbodyid[m->flex_vertadr[i]] == i0) { + f0 = i; + break; + } + } + // generate tetrahedra from the vertices - CreateStencils(simplex); + CreateStencils(simplex, edgeidx); // allocate arrays metric.assign(kNumEdges*kNumEdges*nt, 0); @@ -238,7 +255,10 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, } void Solid::Compute(const mjModel* m, mjData* d, int instance) { - UpdateSquaredLengths(deformed, edges, d->xpos+3*i0); + // update edges if no flex + if (f0 < 0) { + UpdateSquaredLengths(deformed, edges, d->xpos+3*i0); + } // loop over all elements for (int t = 0; t < nt; t++) { @@ -254,11 +274,18 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) { // compute elongation mjtNum elongation[kNumEdges]; - mjtNum kD = damping / m->opt.timestep; for (int e = 0; e < kNumEdges; e++) { - int idx = elements[t].edges[e]; - elongation[e] = deformed[idx] - reference[idx] + - ( deformed[idx] - previous[idx] ) * kD; + if (f0 < 0) { + int idx = elements[t].edges[e]; + mjtNum kD = damping / m->opt.timestep; + elongation[e] = deformed[idx] - reference[idx] + + ( deformed[idx] - previous[idx] ) * kD; + } else { + int idx = elements[t].edges[e] + m->flex_edgeadr[f0]; + mjtNum deformed = d->flexedge_length[idx]*d->flexedge_length[idx]; + mjtNum reference = m->flexedge_length0[idx]*m->flexedge_length0[idx]; + elongation[e] = deformed - reference; + } } // we now multiply the elongations by the precomputed metric tensor, @@ -289,7 +316,9 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) { } // update stored lengths - previous = deformed; + if (f0 < 0) { + previous = deformed; + } } @@ -301,7 +330,7 @@ void Solid::RegisterPlugin() { plugin.name = "mujoco.elasticity.solid"; plugin.capabilityflags |= mjPLUGIN_PASSIVE; - const char* attributes[] = {"face", "young", "poisson", "damping"}; + const char* attributes[] = {"face", "edge", "young", "poisson", "damping"}; plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); plugin.attributes = attributes; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; diff --git a/plugin/elasticity/solid.h b/plugin/elasticity/solid.h index 7f4ee95d..fcc2bd12 100644 --- a/plugin/elasticity/solid.h +++ b/plugin/elasticity/solid.h @@ -44,6 +44,7 @@ class Solid { static void RegisterPlugin(); + int f0; // index of corresponding flex int i0; // index of first body int nc; // number of cubes in the grid int nv; // number of vertices (bodies) in the solid @@ -64,9 +65,11 @@ class Solid { private: Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, - mjtNum damp, const std::vector& simplex); + mjtNum damp, const std::vector& simplex, + const std::vector& edgeidx); - void CreateStencils(const std::vector& simplex); + void CreateStencils(const std::vector& simplex, + const std::vector& edgeidx); }; } // namespace mujoco::plugin::elasticity diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 5726e2b7..590f5852 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1814,6 +1814,8 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { // test a geom and an elem for collision, add to contact list void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { + TM_START; + mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f])); int dim = m->flex_dim[f], type = m->geom_type[g]; int num; @@ -1927,6 +1929,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // move arena pointer back to the end of the contact array resetArena(d); + + // add duration without incrementing counter + TM_ADD(mjTIMER_COL_NARROW); } @@ -1934,6 +1939,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // test two elems for collision, add to contact list void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) { TM_START; + mjtNum margin = mj_assignMargin(m, mju_max(m->flex_margin[f1], m->flex_margin[f2])); int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2]; int num; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index f083335e..260c41af 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -537,9 +537,7 @@ void mj_flex(const mjModel* m, mjData* d) { // compute lengths and Jacobians of edges for (int f=0; f < m->nflex; f++) { // skip if edges cannot generate forces - if (m->flex_rigid[f] || - (m->flex_edgeequality[f] == 0 && - m->flex_edgestiffness[f] == 0 && m->flex_edgedamping[f] == 0)) { + if (m->flex_rigid[f]) { continue; } diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index f2fb29a6..29d8b654 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -948,11 +948,11 @@ void mj_energyPos(const mjModel* m, mjData* d) { } } - // add flex-level springs + // add flex-level springs for dim=1 (dim>1 requires plugins) if (!mjDISABLED(mjDSBL_PASSIVE)) { for (int i=0; i < m->nflex; i++) { stiffness = m->flex_edgestiffness[i]; - if (m->flex_rigid[i] || stiffness == 0) { + if (m->flex_rigid[i] || stiffness == 0 || m->flex_dim[i] > 1) { continue; } diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 03de87ca..03c8e9b6 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -370,7 +370,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int }; for (int s = 0; s < 6; s++) { for (int v = 0; v < 4; v++) { - face.push_back(vert[cube2tets[s][v]]+1); + face.push_back(vert[cube2tets[s][v]]); } } } @@ -388,7 +388,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int }; for (int s = 0; s < 2; s++) { for (int v = 0; v < 3; v++) { - face.push_back(vert[quad2tri[s][v]]+1); + face.push_back(vert[quad2tri[s][v]]); } } } @@ -398,6 +398,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int } else { dim = 2; // can only load a surface for now mjXUtil::String2Vector(userface, face); + for (int i=0; iplugin_instance->config_attribs["face"] = userface; + b->plugin_instance->config_attribs["edge"] = ""; // update density if (dim == 2) { @@ -505,8 +508,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int // create edges for (int i=0; iface.push_back(j*nvert+face[3*i]-1); - skin->face.push_back(j*nvert+face[3*i+(j==0 ? 1 : 2)]-1); - skin->face.push_back(j*nvert+face[3*i+(j==0 ? 2 : 1)]-1); + skin->face.push_back(j*nvert+face[3*i]); + skin->face.push_back(j*nvert+face[3*i+(j==0 ? 1 : 2)]); + skin->face.push_back(j*nvert+face[3*i+(j==0 ? 2 : 1)]); } } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 2cea596d..b082146c 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -65,11 +65,12 @@ mjCFlexcomp::mjCFlexcomp(void) { mjuu_setvec(scale, 1, 1, 1); mass = 1; inertiabox = 0.005; - equality = true; + equality = false; mjuu_setvec(pos, 0, 0, 0); mjuu_setvec(quat, 1, 0, 0, 0); rigid = false; centered = false; + plugin_instance = nullptr; } @@ -453,6 +454,14 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz pf->vert[3*i+1] = 0; pf->vert[3*i+2] = 0; } + + // add plugin + if (plugin_instance) { + pb->is_plugin = true; + pb->plugin_name = plugin_name; + pb->plugin_instance = plugin_instance; + pb->plugin_instance_name = plugin_instance_name; + } } } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 4d6e8742..45201210 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -85,6 +85,12 @@ class mjCFlexcomp { std::vector used; // is point used by any element (false: skip) std::vector element; // flex elements std::vector texcoord; // vertex texture coordinates + + // plugin support + bool is_plugin; + std::string plugin_name; + std::string plugin_instance_name; + mjCPlugin* plugin_instance; }; #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 9d5c7092..18d18cba 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -18,9 +18,11 @@ #include #include #include +#include #include #include #include +#include #include #include @@ -59,6 +61,7 @@ #include "user/user_model.h" #include "user/user_objects.h" #include "user/user_util.h" +#include "xml/xml_util.h" #include extern "C" { @@ -2157,6 +2160,24 @@ void mjCSkin::LoadSKN(mjResource* resource) { //------------------ class mjCFlex implementation -------------------------------------------------- +// hash function for std::pair +struct PairHash +{ + template + std::size_t operator() (const std::pair& pair) const { + return std::hash()(pair.first) ^ std::hash()(pair.second); + } +}; + +// simplex connectivity +constexpr int kNumEdges[3] = {1, 3, 6}; +constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1}, + {-1, -1}, {-1, -1}, {-1, -1}}, + {{ 1, 2}, { 2, 0}, { 0, 1}, + {-1, -1}, {-1, -1}, {-1, -1}}, + {{ 0, 1}, { 1, 2}, { 2, 0}, + { 2, 3}, { 0, 3}, { 1, 3}}}; + // constructor mjCFlex::mjCFlex(mjCModel* _model) { // set model @@ -2216,6 +2237,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (vert.size() % 3) { throw mjCError(this, "vert size must be a multiple of 3"); } + if (edgestiffness>0 && dim>1) { + throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins"); + } nelem = (int)elem.size()/(dim+1); // set nvert, rigid, centered; check size @@ -2276,42 +2300,34 @@ void mjCFlex::Compile(const mjVFS* vfs) { throw mjCError(this, "repeated vertex in element"); } } - - // make edges from sorted element - switch (dim) { - case 1: // line - edge.push_back(std::make_pair(el[0], el[1])); - break; - - case 2: // triangle - edge.push_back(std::make_pair(el[0], el[1])); - edge.push_back(std::make_pair(el[1], el[2])); - edge.push_back(std::make_pair(el[0], el[2])); - break; - - case 3: // tetrahedron - edge.push_back(std::make_pair(el[0], el[1])); - edge.push_back(std::make_pair(el[1], el[2])); - edge.push_back(std::make_pair(el[2], el[3])); - edge.push_back(std::make_pair(el[0], el[2])); - edge.push_back(std::make_pair(el[0], el[3])); - edge.push_back(std::make_pair(el[1], el[3])); - break; - } } - // sort edges - std::sort(edge.begin(), edge.end()); + // create edges + std::vector edgeidx(elem.size()*kNumEdges[dim-1]); - // remove repeated edges - std::vector> edge1; - edge1.push_back(edge[0]); - for (int i=1; i<(int)edge.size(); i++) { - if (edge1[edge1.size()-1]!=edge[i]) { - edge1.push_back(edge[i]); + // map from edge vertices to their index in `edges` vector + std::unordered_map, int, PairHash> edge_indices; + + // insert local edges into global vector + for (int f = 0; f < (int)elem.size()/(dim+1); f++) { + int* v = elem.data() + f*(dim+1); + for (int e = 0; e < kNumEdges[dim-1]; e++) { + auto pair = std::pair( + std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]), + std::max(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]) + ); + + // if edge is already present in the vector only store its index + auto [it, inserted] = edge_indices.insert({pair, nedge}); + + if (inserted) { + edge.push_back(pair); + edgeidx[f*kNumEdges[dim-1]+e] = nedge++; + } else { + edgeidx[f*kNumEdges[dim-1]+e] = it->second; + } } } - edge = edge1; // set size nedge = (int)edge.size(); @@ -2378,6 +2394,18 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } + // add plugins + std::string userface, useredge; + mjXUtil::Vector2String(userface, elem); + mjXUtil::Vector2String(useredge, edgeidx); + + for (int i=0; i<(int)vertbodyid.size(); i++) { + if (model->bodies[vertbodyid[i]]->plugin_instance) { + model->bodies[vertbodyid[i]]->plugin_instance->config_attribs["face"] = userface; + model->bodies[vertbodyid[i]]->plugin_instance->config_attribs["edge"] = useredge; + } + } + // create shell fragments and element-vertex collision pairs CreateShellPair(); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 0019576b..4b927369 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -78,7 +78,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) { //---------------------------------- MJCF schema --------------------------------------------------- -static const int nMJCF = 223; +static const int nMJCF = 227; static const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, @@ -293,25 +293,31 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"config", "*", "2", "key", "value"}, {">"}, {">"}, - {"flexcomp", "*", "26", "name", "class", "type", "dim", "flatskin", + {"flexcomp", "*", "25", "name", "class", "type", "group", "dim", "count", "spacing", "radius", "rigid", "mass", "inertiabox", - "scale", "file", "point", "element", "texcoord", "material", "rgba", "selfcollide", + "scale", "file", "point", "element", "texcoord", "material", "rgba", "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler"}, {"<"}, {"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"}, - {"contact", "?", "10", "contype", "conaffinity", "condim", "priority", - "friction", "solmix", "solref", "solimp", "margin", "gap"}, + {"contact", "?", "13", "contype", "conaffinity", "condim", "priority", + "friction", "solmix", "solref", "solimp", "margin", "gap", + "internal", "selfcollide", "activelayers"}, {"pin", "*", "4", "id", "range", "grid", "gridrange"}, + {"plugin", "*", "2", "plugin", "instance"}, + {"<"}, + {"config", "*", "2", "key", "value"}, + {">"}, {">"}, {">"}, {"deformable", "*", "0"}, {"<"}, - {"flex", "*", "12", "name", "group", "dim", "radius", "material", "rgba", "flatskin", - "selfcollide", "body", "vertex", "element", "texcoord"}, + {"flex", "*", "11", "name", "group", "dim", "radius", "material", + "rgba", "flatskin", "body", "vertex", "element", "texcoord"}, {"<"}, - {"contact", "?", "10", "contype", "conaffinity", "condim", "priority", - "friction", "solmix", "solref", "solimp", "margin", "gap"}, + {"contact", "?", "13", "contype", "conaffinity", "condim", "priority", + "friction", "solmix", "solref", "solimp", "margin", "gap", + "internal", "selfcollide", "activelayers"}, {"edge", "?", "2", "stiffness", "damping"}, {">"}, {"skin", "*", "9", "name", "file", "material", "rgba", "inflate", @@ -2366,6 +2372,21 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) { epin = epin->NextSiblingElement("pin"); } + // plugin + XMLElement* eplugin = elem->FirstChildElement("plugin"); + if (eplugin) { + ReadAttrTxt(eplugin, "plugin", fcomp.plugin_name); + ReadAttrTxt(eplugin, "instance", fcomp.plugin_instance_name); + if (fcomp.plugin_instance_name.empty()) { + fcomp.plugin_instance = model->AddPlugin(); + fcomp.plugin_instance->name = "flexcomp_" + fcomp.name; + fcomp.plugin_instance_name = fcomp.plugin_instance->name; + } else { + model->hasImplicitPluginElem = true; + } + ReadPluginConfigs(eplugin, fcomp.plugin_instance); + } + // make flexcomp char error[200]; bool res = fcomp.Make(pbody->model, pbody, error, 200); diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index e30add97..c8174ce4 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -39,14 +39,14 @@ TEST_F(ElasticityTest, ElasticEnergyShell) { - - + - + )"; @@ -93,13 +93,14 @@ TEST_F(ElasticityTest, ElasticEnergySolid) { - - + - + + )"; @@ -110,6 +111,9 @@ TEST_F(ElasticityTest, ElasticEnergySolid) { mjData* d = mj_makeData(m); auto* solid = reinterpret_cast(d->plugin_data[0]); + mj_kinematics(m, d); + mj_flex(m, d); + // check that if the entire geometry is rescaled by a factor "scale", then // trace(strain^2) = 3*scale^2 @@ -119,10 +123,12 @@ TEST_F(ElasticityTest, ElasticEnergySolid) { mjtNum volume = 1./6.; for (int e1 = 0; e1 < 6; e1++) { for (int e2 = 0; e2 < 6; e2++) { - int idx1 = solid->elements[t].edges[e1]; - int idx2 = solid->elements[t].edges[e2]; - mjtNum elongation1 = scale*solid->reference[idx1]; - mjtNum elongation2 = scale*solid->reference[idx2]; + int idx1 = solid->elements[t].edges[e1] + m->flex_edgeadr[0]; + int idx2 = solid->elements[t].edges[e2] + m->flex_edgeadr[0]; + mjtNum elongation1 = + scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1]; + mjtNum elongation2 = + scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2]; energy += solid->metric[36*t+6*e2+e1] * elongation1 * elongation2; } }