diff --git a/plugin/elasticity/CMakeLists.txt b/plugin/elasticity/CMakeLists.txt index f35cf2d3..81946e9a 100644 --- a/plugin/elasticity/CMakeLists.txt +++ b/plugin/elasticity/CMakeLists.txt @@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_SRCS cable.cc cable.h elasticity.cc + elasticity.h + register.cc solid.cc solid.h ) diff --git a/plugin/elasticity/elasticity.cc b/plugin/elasticity/elasticity.cc index e349379f..35c0e0fc 100644 --- a/plugin/elasticity/elasticity.cc +++ b/plugin/elasticity/elasticity.cc @@ -12,15 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include "cable.h" -#include "solid.h" +#include "elasticity.h" +#include +#include +#include +#include +#include +#include +#include +#include namespace mujoco::plugin::elasticity { -mjPLUGIN_LIB_INIT { - Cable::RegisterPlugin(); - Solid::RegisterPlugin(); +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); + } + } +} + +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(); +} + +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 new file mode 100644 index 00000000..54b0fb14 --- /dev/null +++ b/plugin/elasticity/elasticity.h @@ -0,0 +1,50 @@ +// 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_ELASTICITY_H_ +#define MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_ + +#include +#include +#include + +#include + +namespace mujoco::plugin::elasticity { + +struct PairHash +{ + template + std::size_t operator() (const std::pair& pair) const { + return std::hash()(pair.first) ^ std::hash()(pair.second); + } +}; + +// 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/register.cc b/plugin/elasticity/register.cc new file mode 100644 index 00000000..e349379f --- /dev/null +++ b/plugin/elasticity/register.cc @@ -0,0 +1,26 @@ +// 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 "cable.h" +#include "solid.h" + +namespace mujoco::plugin::elasticity { + +mjPLUGIN_LIB_INIT { + Cable::RegisterPlugin(); + Solid::RegisterPlugin(); +} + +} // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc index 83777613..84130bff 100644 --- a/plugin/elasticity/solid.cc +++ b/plugin/elasticity/solid.cc @@ -13,15 +13,17 @@ // limitations under the License. #include -#include -#include -#include +#include +#include #include #include +#include +#include #include #include #include +#include "elasticity.h" #include "solid.h" @@ -36,15 +38,6 @@ constexpr int edge[kNumEdges][2] = {{0, 1}, {1, 2}, {2, 0}, constexpr int face[kNumVerts][3] = {{2, 1, 0}, {0, 1, 3}, {1, 2, 3}, {2, 0, 3}}; constexpr int e2f[kNumEdges][2] = {{2, 3}, {1, 3}, {2, 1}, {1, 0}, {0, 2}, {0, 3}}; -constexpr int cube2tets[kNumEdges][kNumVerts] = {{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[kNumVerts]) { @@ -89,17 +82,6 @@ void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[kNumVerts], } } -// 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[kNumEdges][2][3], const mjtNum* x, @@ -113,40 +95,20 @@ void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3], } } -// 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) && + if (CheckAttr("face", 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); + std::vector face; + String2Vector(mj_getPluginConfig(m, instance, "face"), face); + return Solid(m, d, instance, nu, E, damp, face); } else { mju_warning("Invalid parameter specification in solid plugin"); return std::nullopt; @@ -154,30 +116,13 @@ 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(int nx, int ny, int nz) { +void Solid::CreateStencils(const std::vector& simplex) { + // populate stencil + nt = simplex.size() / kNumVerts; elements.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 < kNumVerts; v++) { - elements[t+s].vertices[v] = vert[cube2tets[s][v]]; - } - } - } + for (int t = 0; t < nt; t++) { + for (int v = 0; v < kNumVerts; v++) { + elements[t].vertices[v] = simplex[kNumVerts*t+v]-1; } } @@ -209,8 +154,9 @@ void Solid::CreateStencils(int nx, int ny, int nz) { } // 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) { +Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, + mjtNum damp, const std::vector& simplex) + : damping(damp) { // count plugin bodies nv = ne = 0; for (int i = 1; i < m->nbody; i++) { @@ -221,13 +167,11 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz, } } - // allocate arrays - nc = (nx-1)*(ny-1)*(nz-1); // number of cubes - nt = 6*nc; // number of tets - metric.assign(kNumEdges*kNumEdges*nt, 0); // metric induced by the geometry - // generate tetrahedra from the vertices - CreateStencils(nx, ny, nz); + CreateStencils(simplex); + + // allocate arrays + metric.assign(kNumEdges*kNumEdges*nt, 0); // loop over all tetrahedra for (int t = 0; t < nt; t++) { @@ -357,7 +301,7 @@ void Solid::RegisterPlugin() { plugin.name = "mujoco.elasticity.solid"; plugin.capabilityflags |= mjPLUGIN_PASSIVE; - const char* attributes[] = {"nx", "ny", "nz", "young", "poisson", "damping"}; + const char* attributes[] = {"face", "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 96527a1d..7f4ee95d 100644 --- a/plugin/elasticity/solid.h +++ b/plugin/elasticity/solid.h @@ -63,10 +63,10 @@ class Solid { mjtNum damping; private: - Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz, - mjtNum nu, mjtNum E, mjtNum damp); + Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, + mjtNum damp, const std::vector& simplex); - void CreateStencils(int nx, int ny, int nz); + void CreateStencils(const std::vector& simplex); }; } // namespace mujoco::plugin::elasticity diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 256f9a95..b44b1600 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -34,6 +34,7 @@ #include "user/user_model.h" #include "user/user_objects.h" #include "user/user_util.h" +#include "xml/xml_util.h" namespace { namespace mju = ::mujoco::util; @@ -314,66 +315,178 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s -// make particles bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz) { - // create bodies and geoms - for (int ix=0; ixAddBody(NULL); - mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz); - b->name = txt; + char txt[100]; + std::vector face; - // set body position - b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]); - b->pos[1] = offset[1] + spacing*(iy - 0.5*count[1]); - b->pos[2] = offset[2] + spacing*(iz - 0.5*count[2]); + // populate vertices and names + if (uservert.empty()) { + if (spacing < mju_max(def[0].geom.size[0], + mju_max(def[0].geom.size[1], def[0].geom.size[2]))) + return comperr(error, "Spacing must be larger than geometry size", error_sz); - // add slider joints if none defined - if (!add[mjCOMPKIND_PARTICLE]) { - for (int i=0; i<3; i++) { - mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false); - jnt->def = body->def; - jnt->type = mjJNT_SLIDE; - mjuu_setvec(jnt->pos, 0, 0, 0); - mjuu_setvec(jnt->axis, 0, 0, 0); - jnt->axis[i] = 1; - } - } + for (int ix=0; ixAddJoint(&defjnt, false); - jnt->def = body->def; - } - } - - // add geom - mjCGeom* g = b->AddGeom(def); - g->def = body->def; - - // 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]); - } + mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz); + username.push_back(std::string(txt)); } } } } - // skin + // create faces + if (userface.empty()) { + if (dim == 3) { + 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}}; + for (int ix = 0; ix < count[0]-1; ix++) { + for (int iy = 0; iy < count[1]-1; iy++) { + for (int iz = 0; iz < count[2]-1; iz++) { + int vert[8] = { + count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0, + count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0, + count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0, + count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0, + count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1, + count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1, + count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1, + count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1, + }; + for (int s = 0; s < 6; s++) { + for (int v = 0; v < 4; v++) { + face.push_back(vert[cube2tets[s][v]]+1); + } + } + } + } + } + } else if (dim == 2) { + int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}}; + for (int ix = 0; ix < count[0]-1; ix++) { + for (int iy = 0; iy < count[1]-1; iy++) { + int vert[4] = { + count[2]*count[1]*(ix+0) + count[2]*(iy+0), + count[2]*count[1]*(ix+1) + count[2]*(iy+0), + count[2]*count[1]*(ix+1) + count[2]*(iy+1), + count[2]*count[1]*(ix+0) + count[2]*(iy+1), + }; + for (int s = 0; s < 2; s++) { + for (int v = 0; v < 3; v++) { + face.push_back(vert[quad2tri[s][v]]+1); + } + } + } + } + } + mjXUtil::Vector2String(userface, face); + } else { + dim = 2; // can only load a surface for now + mjXUtil::String2Vector(userface, face); + } + + // compute volume + std::vector volume(uservert.size()/3); + mjtNum t = 1; + if (dim == 2 && plugin_instance) { + // do nothing for now (until new passive forces are supported) + } + if (!userface.empty()) { + mjXUtil::String2Vector(userface, face); + for (int j=0; jAddBody(NULL); + + if (!username.empty()) { + b->name = username[i]; + } else { + mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i); + b->name = txt; + } + + // set body position + b->pos[0] = offset[0] + uservert[3*i]; + b->pos[1] = offset[1] + uservert[3*i+1]; + b->pos[2] = offset[2] + uservert[3*i+2]; + + // add slider joints if none defined + if (!add[mjCOMPKIND_PARTICLE]) { + for (int i=0; i<3; i++) { + mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false); + jnt->def = body->def; + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[i] = 1; + } + } + + // add user-specified joints + else { + for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) { + mjCJoint* jnt = b->AddJoint(&defjnt, false); + jnt->def = body->def; + } + } + + // add geom + mjCGeom* g = b->AddGeom(def); + g->def = body->def; + + // add site + mjCSite* s = b->AddSite(def); + s->def = body->def; + s->type = mjGEOM_SPHERE; + mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i); + s->name = txt; + + // 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; + + if (i==0 && !plugin_instance->config_attribs["face"].empty()) { + return comperr(error, "Face attribute already exists in plugin", error_sz); + } + + b->plugin_instance->config_attribs["face"] = userface; + + // update density + if (dim == 2) { + g->density *= volume[i] / (4./3. * mjPI * pow(g->size[0], 3)); + } + } + } + if (skin) { MakeSkin3(model); } @@ -2040,6 +2153,44 @@ void mjCComposite::MakeSkin3(mjCModel* model) { skin->inflate = skininflate; skin->group = skingroup; + // copy skin from existing mesh + if (type==mjCOMPTYPE_PARTICLE && username.empty()) { + std::vector face; + mjXUtil::String2Vector(userface, face); + int nvert = uservert.size()/3; + + for (int j=0; j<2; j++) { + for (int i=0; ivert.push_back(0); + skin->vert.push_back(0); + skin->vert.push_back(0); + + mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i); + skin->bodyname.push_back(txt); + skin->bindpos.push_back(0); + skin->bindpos.push_back(0); + skin->bindpos.push_back(0); + skin->bindquat.push_back(1); + skin->bindquat.push_back(0); + skin->bindquat.push_back(0); + skin->bindquat.push_back(0); + + vector vertid; + vector vertweight; + vertid.push_back(j*nvert+i); + vertweight.push_back(1); + skin->vertid.push_back(vertid); + skin->vertweight.push_back(vertweight); + } + + 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); + } + } + } + // box if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) { // z-faces diff --git a/src/user/user_composite.h b/src/user/user_composite.h index af6e6807..0d60b97e 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -106,9 +106,13 @@ class mjCComposite { // currently used only for cable std::string initial; // root boundary type std::vector uservert; // user-specified vertex positions + std::string userface; // connectivity mjtNum size[3]; // rope size (meaning depends on the shape) mjtCompShape curve[3]; // geometric shape + // body names used in the skin + std::vector username; + // plugin support bool is_plugin; std::string plugin_name; diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index d6a03d2e..d2e36025 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -41,9 +41,6 @@ TEST_F(PluginTest, ElasticEnergy) { - - -