diff --git a/doc/changelog.rst b/doc/changelog.rst index aa6533bc..6d917055 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -50,6 +50,19 @@ General `_. - 50% faster ``mju_dotSparse`` using manual loop unroll. See `engine_util_sparse_benchmark_test `_. +- Added new :at:`solid` passive force plugin: + + .. youtube:: AGcTGHbbze4 + :align: right + :height: 150px + + - This is new force field compatible with the :ref:`composite` particles. + - Generates a tetrahedral mesh having particles with mass concentrated at vertices. + - Uses a piecewise-constant strain model equivalent to finite elements but expressed in a coordinate-free + formulation. This implies that all quantities can be precomputed except edge elongation, as in a mass-spring model. + - Only suitable for small strains (large displacements but small deformations). Tetrahedra may invert if subject to + large loads. + - Add API functions ``mj_loadPluginLibrary`` and ``mj_loadAllPluginLibraries``. The first function is identical to ``dlopen`` on a POSIX system, and to ``LoadLibraryA`` on Windows. The second function scans a specified directory for all dynamic libraries file and loads each library found. Dynamic libraries opened by these functions are assumed to diff --git a/model/plugin/floppy.xml b/model/plugin/floppy.xml new file mode 100644 index 00000000..76611538 --- /dev/null +++ b/model/plugin/floppy.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + diff --git a/model/plugin/jelly.xml b/model/plugin/jelly.xml new file mode 100644 index 00000000..279b6f94 --- /dev/null +++ b/model/plugin/jelly.xml @@ -0,0 +1,56 @@ + + + + + + + + + + diff --git a/model/plugin/press.xml b/model/plugin/press.xml new file mode 100644 index 00000000..5b0e9408 --- /dev/null +++ b/model/plugin/press.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + diff --git a/plugin/elasticity/CMakeLists.txt b/plugin/elasticity/CMakeLists.txt index 8dc89c65..ca649524 100644 --- a/plugin/elasticity/CMakeLists.txt +++ b/plugin/elasticity/CMakeLists.txt @@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_INCLUDE set(MUJOCO_ELASTICITY_SRCS cable.cc cable.h + solid.cc + solid.h ) add_library(elasticity SHARED) diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc new file mode 100644 index 00000000..ea7697af --- /dev/null +++ b/plugin/elasticity/solid.cc @@ -0,0 +1,378 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include "solid.h" + + +namespace mujoco::plugin::elasticity { +namespace { + +// local tetrahedron numbering +constexpr int edge[6][2] = {{0, 1}, {1, 2}, {2, 0}, {2, 3}, {0, 3}, {1, 3}}; +constexpr int face[4][3] = {{2, 1, 0}, {0, 1, 3}, {1, 2, 3}, {2, 0, 3}}; +constexpr int e2f[6][2] = {{2, 3}, {1, 3}, {2, 1}, {1, 0}, {0, 2}, {0, 3}}; +constexpr int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7}, {1, 3, 2, 7}, + {1, 2, 6, 7}, {1, 5, 4, 7}, {1, 6, 5, 7}}; + +// Cartesian distance between 3D vectors +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]; +} + +// volume of a tetrahedron +mjtNum ComputeVolume(const mjtNum* x, const int v[4]) { + mjtNum normal[3]; + mjtNum edge1[3]; + mjtNum edge2[3]; + mjtNum edge3[3]; + + mju_sub3(edge1, x+3*v[1], x+3*v[0]); + mju_sub3(edge2, x+3*v[2], x+3*v[0]); + mju_sub3(edge3, x+3*v[3], x+3*v[0]); + mju_cross(normal, edge2, edge1); + + return mju_dot3(normal, edge3) / 6; +} + +// compute local basis +void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[4], + const int faceL[3], const int faceR[3], mjtNum volume) { + mjtNum normalL[3], normalR[3]; + mjtNum edgesL[6], edgesR[6]; + + mju_sub3(edgesL+0, x+3*v[faceL[1]], x+3*v[faceL[0]]); + mju_sub3(edgesL+3, x+3*v[faceL[2]], x+3*v[faceL[0]]); + mju_sub3(edgesR+0, x+3*v[faceR[1]], x+3*v[faceR[0]]); + mju_sub3(edgesR+3, x+3*v[faceR[2]], x+3*v[faceR[0]]); + + mju_cross(normalL, edgesL, edgesL+3); + mju_cross(normalR, edgesR, edgesR+3); + + // we use as basis the symmetrized tensor products of the area normals of the + // two faces not adjacent to the edge; this is the 3D equivalent to the basis + // proposed in Weischedel "A discrete geometric view on shear-deformable shell + // models" in the remark at the end of section 4.1. This is also equivalent to + // linear finite elements but in a coordinate-free formulation. + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + basis[3*i+j] = ( normalL[i]*normalR[j] + + normalR[i]*normalL[j] ) / (36*2*volume*volume); + } + } +} + +// update edge lengths +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); + } +} + +// gradients of edge lengths with respect to vertex positions +void GradSquaredLengths(mjtNum gradient[6][2][3], + const mjtNum* x, + const int v[4], + const int edge[6][2]) { + for (int e = 0; e < 6; e++) { + for (int d = 0; d < 3; d++) { + gradient[e][0][d] = x[3*v[edge[e][0]]+d] - x[3*v[edge[e][1]]+d]; + gradient[e][1][d] = x[3*v[edge[e][1]]+d] - x[3*v[edge[e][0]]+d]; + } + } +} + +// 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 Solid::Create(const mjModel* m, mjData* d, int instance) { + if (CheckAttr("nx", m, instance) && + CheckAttr("ny", m, instance) && + CheckAttr("nz", m, instance) && + CheckAttr("poisson", m, instance) && + CheckAttr("young", m, instance)) { + int nx = strtod(mj_getPluginConfig(m, instance, "nx"), nullptr); + int ny = strtod(mj_getPluginConfig(m, instance, "ny"), nullptr); + int nz = strtod(mj_getPluginConfig(m, instance, "nz"), nullptr); + 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); + return Solid(m, d, instance, nx, ny, nz, nu, E, damp); + } else { + mju_warning("Invalid parameter specification in solid plugin"); + return std::nullopt; + } +} + +// create map from tetrahedra to vertices and edges and from edges to vertices +void Solid::CreateStencils(int nx, int ny, int nz) { + tetrahedra.resize(nt); + + // create a tetrahedral mesh by splitting a grid of hexahedral cells + for (int ix = 0; ix < nx-1; ix++) { + for (int iy = 0; iy < ny-1; iy++) { + for (int iz = 0; iz < nz-1; iz++) { + int t = 6*(nz-1)*(ny-1)*ix + 6*(nz-1)*iy + 6*iz; + int vert[8] = { + nz*ny*(ix+0) + nz*(iy+0) + iz+0, + nz*ny*(ix+1) + nz*(iy+0) + iz+0, + nz*ny*(ix+1) + nz*(iy+1) + iz+0, + nz*ny*(ix+0) + nz*(iy+1) + iz+0, + nz*ny*(ix+0) + nz*(iy+0) + iz+1, + nz*ny*(ix+1) + nz*(iy+0) + iz+1, + nz*ny*(ix+1) + nz*(iy+1) + iz+1, + nz*ny*(ix+0) + nz*(iy+1) + iz+1, + }; + for (int s = 0; s < 6; s++) { + for (int v = 0; v < 4; v++) { + tetrahedra[t+s].vertices[v] = vert[cube2tets[s][v]]; + } + } + } + } + } + + // map from edge vertices to their index in `edges` vector + std::unordered_map, int, PairHash> edge_indices; + + // loop over all tetrahedra + for (int t = 0; t < nt; t++) { + int* v = tetrahedra[t].vertices; + + // compute edges to vertices map for fast computations + for (int e = 0; e < 6; e++) { + auto pair = std::pair( + std::min(v[edge[e][0]], v[edge[e][1]]), + std::max(v[edge[e][0]], v[edge[e][1]]) + ); + + // if edge is already present in the vector only store its index + auto [it, inserted] = edge_indices.insert({pair, ne}); + + if (inserted) { + edges.push_back(pair); + tetrahedra[t].edges[e] = ne++; + } else { + tetrahedra[t].edges[e] = it->second; + } + } + } +} + +// plugin constructor +Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz, + mjtNum nu, mjtNum E, mjtNum damp): damping(damp) { + // count plugin bodies + nv = ne = 0; + for (int i = 1; i < m->nbody; i++) { + if (m->body_plugin[i] == instance) { + if (!nv++) { + i0 = i; + } + } + } + + // allocate arrays + nc = (nx-1)*(ny-1)*(nz-1); // number of cubes + nt = 6*nc; // number of tets + metric.assign(36*nt, 0); // metric induced by the geometry + + // generate tetrahedra from the vertices + CreateStencils(nx, ny, nz); + + // loop over all tetrahedra + for (int t = 0; t < nt; t++) { + int* v = tetrahedra[t].vertices; + for (int i = 0; i < 4; i++) { + if (m->body_plugin[i0+v[i]] != instance) { + mju_error("This body does not have the requested plugin instance"); + } + } + + // tetrahedron volume + mjtNum volume = ComputeVolume(m->body_pos+3*i0, v); + + // local geometric quantities + mjtNum basis[6][9] = {{0}, {0}, {0}, {0}, {0}, {0}}; + mjtNum trT[6] = {0}; + mjtNum trTT[36] = {0}; + + // compute edge basis + for (int e = 0; e < 6; e++) { + ComputeBasis(basis[e], m->body_pos+3*i0, v, + face[e2f[e][0]], face[e2f[e][1]], volume); + } + + // compute first invariant i.e. trace(strain) + for (int e = 0; e < 6; e++) { + for (int i = 0; i < 3; i++) { + trT[e] += basis[e][4*i]; + } + } + + // compute second invariant i.e. trace(strain^2) + for (int ed1 = 0; ed1 < 6; ed1++) { + for (int ed2 = 0; ed2 < 6; ed2++) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + trTT[6*ed1+ed2] += basis[ed1][3*i+j] * basis[ed2][3*j+i]; + } + } + } + } + + // material parameters + mjtNum mu = E / (2*(1+nu)) * volume; + mjtNum la = E*nu / ((1+nu)*(1-2*nu)) * volume; + + // assembly of strain metric tensor + for (int ed1 = 0; ed1 < 6; ed1++) { + for (int ed2 = 0; ed2 < 6; ed2++) { + metric[36*t+6*ed1+ed2] = mu * trTT[6*ed1+ed2] + la * trT[ed2]*trT[ed1]; + } + } + } + + // allocate array + reference.assign(ne, 0); + deformed.assign(ne, 0); + previous.assign(ne, 0); + + // compute edge lengths at equilibrium + UpdateSquaredLengths(reference, edges, m->body_pos+3*i0); + previous = reference; +} + +void Solid::Compute(const mjModel* m, mjData* d, int instance) { + UpdateSquaredLengths(deformed, edges, d->xpos+3*i0); + + // loop over all elements + for (int t = 0; t < nt; t++) { + int* v = tetrahedra[t].vertices; + + // compute length gradient with respect to dofs + mjtNum gradient[6][2][3]; + GradSquaredLengths(gradient, d->xpos+3*i0, v, edge); + + // we add generalized Rayleigh damping as decribed in Section 5.2 of + // Kharevych et al., "Geometric, Variational Integrators for Computer + // Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf + + // compute elongation + mjtNum elongation[6]; + mjtNum kD = damping / m->opt.timestep; + for (int e = 0; e < 6; e++) { + int idx = tetrahedra[t].edges[e]; + elongation[e] = deformed[idx] - reference[idx] + + ( deformed[idx] - previous[idx] ) * kD; + } + + // we now multiply the elongations by the precomputed metric tensor, + // notice that if metric=diag(1/reference) then this would yield a + // mass-spring model + + // compute local force + mjtNum force[12] = {0}; + for (int ed1 = 0; ed1 < 6; ed1++) { + for (int ed2 = 0; ed2 < 6; ed2++) { + for (int i = 0; i < 2; i++) { + for (int x = 0; x < 3; x++) { + force[3*edge[ed2][i]+x] += + elongation[ed1] * gradient[ed2][i][x] * metric[36*t+6*ed1+ed2]; + } + } + } + } + + // insert into global force + for (int i = 0; i < 4; i++) { + for (int x = 0; x < 3; x++) { + d->qfrc_passive[m->body_dofadr[i0]+3*v[i]+x] -= force[3*i+x]; + } + } + } + + // update stored lengths + previous = deformed; +} + + + +mjPLUGIN_DYNAMIC_LIBRARY_INIT { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.elasticity.solid"; + plugin.type |= mjPLUGIN_PASSIVE; + + const char* attributes[] = {"nx", "ny", "nz", "young", "poisson", "damping"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto elasticity_or_null = Solid::Create(m, d, instance); + if (!elasticity_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Solid(std::move(*elasticity_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) { + auto* elasticity = reinterpret_cast(d->plugin_data[instance]); + elasticity->Compute(m, d, instance); + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/solid.h b/plugin/elasticity/solid.h new file mode 100644 index 00000000..fd813e6a --- /dev/null +++ b/plugin/elasticity/solid.h @@ -0,0 +1,71 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_ELASTICITY_SOLID_H_ +#define MUJOCO_PLUGIN_ELASTICITY_SOLID_H_ + +#include +#include + +#include +#include +#include + + +namespace mujoco::plugin::elasticity { + +struct Stencil { + int vertices[4]; + int edges[6]; +}; + +class Solid { + public: + // Returns a new Solid instance or nullopt on failure. + static std::optional Create(const mjModel* m, mjData* d, + int instance); + Solid(Solid&&) = default; + + Solid& operator=(Solid&& other) = default; + + void Compute(const mjModel* m, mjData* d, int instance); + + int i0; // index of first body + int nc; // number of cubes in the grid + int nv; // number of vertices (bodies) in the solid + int nt; // number of volumetric elements (tetrahedra) + int ne; // number of edges in the solid + + // connectivity info for mapping tetrahedra to edges and vertices + std::vector tetrahedra; // 4 vertices and 6 edges (nt x 10) + std::vector > edges; // edge to vertex map (ne x 2) + + // precomputed quantities + std::vector metric; // geom-induced metric (nt x 36) + std::vector reference; // reference lengths (ne x 1) + std::vector deformed; // deformed lengths (ne x 1) + std::vector previous; // previous-step lengths (ne x 1) + + mjtNum damping; + + private: + Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz, + mjtNum nu, mjtNum E, mjtNum damp); + + void CreateStencils(int nx, int ny, int nz); +}; + +} // namespace mujoco::plugin::elasticity + +#endif // MUJOCO_PLUGIN_ELASTICITY_SOLID_H_ diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index aedde5c4..1fc18bd6 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -59,6 +59,7 @@ mjCComposite::mjCComposite(void) { pin.clear(); flatinertia = 0; mj_defaultSolRefImp(solrefsmooth, solimpsmooth); + plugin_instance = nullptr; // cable curve[0] = curve[1] = curve[2] = mjCOMPSHAPE_ZERO; @@ -315,7 +316,10 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int for (int iy=0; iyAddBody(NULL); + mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz); + b->name = txt; // set body position b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]); @@ -345,10 +349,31 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int // add geom mjCGeom* g = b->AddGeom(def); g->def = body->def; + g->type = mjGEOM_SPHERE; + + // add plugin + if (plugin_instance) { + b->is_plugin = true; + b->plugin_name = plugin_name; + b->plugin_instance = plugin_instance; + b->plugin_instance_name = plugin_instance_name; + + // propagate attributes + if (plugin_name == "mujoco.elasticity.solid") { + b->plugin_instance->config_attribs["nx"] = std::to_string(count[0]); + b->plugin_instance->config_attribs["ny"] = std::to_string(count[1]); + b->plugin_instance->config_attribs["nz"] = std::to_string(count[2]); + } + } } } } + // skin + if (skin) { + MakeSkin3(model); + } + return true; } @@ -2014,7 +2039,7 @@ void mjCComposite::MakeSkin3(mjCModel* model) { skin->group = skingroup; // box - if (type==mjCOMPTYPE_BOX) { + if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) { // z-faces MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0"); fmt = "%sB%d_%d_" + string(cnt2); diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index b94c74a9..35b009b2 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -24,6 +24,7 @@ #include #include #include "test/fixture.h" +#include "plugin/elasticity/solid.h" namespace mujoco { namespace { @@ -50,6 +51,62 @@ class PluginTest : public MujocoTest { } }; +// -------------------------------- solid ----------------------------------- +TEST_F(PluginTest, ElasticEnergy) { + static constexpr char cantilever_xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + EXPECT_THAT(mjp_pluginCount(), 2); + auto* solid = reinterpret_cast(d->plugin_data[0]); + + // check that if the entire geometry is rescaled by a factor "scale", then + // trace(strain^2) = 3*scale^2 + + for (mjtNum scale = 1; scale < 4; scale++) { + for (int t = 0; t < solid->nt; t++) { + mjtNum energy = 0; + mjtNum volume = 1./6.; + for (int e1 = 0; e1 < 6; e1++) { + for (int e2 = 0; e2 < 6; e2++) { + int idx1 = solid->tetrahedra[t].edges[e1]; + int idx2 = solid->tetrahedra[t].edges[e2]; + mjtNum elongation1 = scale*solid->reference[idx1]; + mjtNum elongation2 = scale*solid->reference[idx2]; + energy += solid->metric[36*t+6*e2+e1] * elongation1 * elongation2; + } + } + EXPECT_NEAR( + energy/volume, 3*scale*scale, std::numeric_limits::epsilon()); + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + // -------------------------------- cable ----------------------------------- TEST_F(PluginTest, CantileverIntoCircle) {