MakeParticle and elasticity plugin refactoring.

- Removed the explicit dependency on a plugin name.
- Creation of particles from user-defined vertices (if a plugin is present).
- Add skin support for an irregular mesh
- Fixes #642 and #674.

PiperOrigin-RevId: 561313771
Change-Id: I450f91f0d3b578cc18d97ed7180a0a8944b7dd41
This commit is contained in:
Alessio Quaglino
2023-08-30 05:39:42 -07:00
committed by Copybara-Service
parent f86e8b449f
commit 273c23038c
9 changed files with 355 additions and 142 deletions
+2
View File
@@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_SRCS
cable.cc
cable.h
elasticity.cc
elasticity.h
register.cc
solid.cc
solid.h
)
+45 -6
View File
@@ -12,15 +12,54 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <mujoco/mjplugin.h>
#include "cable.h"
#include "solid.h"
#include "elasticity.h"
#include <algorithm>
#include <cctype>
#include <cstdlib>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
namespace mujoco::plugin::elasticity {
mjPLUGIN_LIB_INIT {
Cable::RegisterPlugin();
Solid::RegisterPlugin();
void String2Vector(const std::string& txt, std::vector<int>& 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<mjtNum>& len,
const std::vector<std::pair<int, int> >& 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
+50
View File
@@ -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 <sstream>
#include <string>
#include <vector>
#include <mujoco/mujoco.h>
namespace mujoco::plugin::elasticity {
struct PairHash
{
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
// copied from mjXUtil
void String2Vector(const std::string& txt, std::vector<int>& 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<mjtNum>& len,
const std::vector<std::pair<int, int> >& edges,
const mjtNum* x);
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
+26
View File
@@ -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 <mujoco/mjplugin.h>
#include "cable.h"
#include "solid.h"
namespace mujoco::plugin::elasticity {
mjPLUGIN_LIB_INIT {
Cable::RegisterPlugin();
Solid::RegisterPlugin();
}
} // namespace mujoco::plugin::elasticity
+23 -79
View File
@@ -13,15 +13,17 @@
// limitations under the License.
#include <algorithm>
#include <cstddef>
#include <cstdio>
#include <sstream>
#include <cstdint>
#include <cstdlib>
#include <optional>
#include <unordered_map>
#include <utility>
#include <vector>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#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<mjtNum>& len,
const std::vector<std::pair<int, int> >& 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 <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
} // namespace
// factory function
std::optional<Solid> 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<int> 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> 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<int>& 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<int>& 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; };
+3 -3
View File
@@ -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<int>& simplex);
void CreateStencils(int nx, int ny, int nz);
void CreateStencils(const std::vector<int>& simplex);
};
} // namespace mujoco::plugin::elasticity
+202 -51
View File
@@ -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; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
// create body
char txt[100];
mjCBody* b = body->AddBody(NULL);
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
b->name = txt;
char txt[100];
std::vector<int> 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; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
uservert.push_back(spacing*(ix - 0.5*count[0]));
uservert.push_back(spacing*(iy - 0.5*count[1]));
uservert.push_back(spacing*(iz - 0.5*count[2]));
// 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 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<mjtNum> 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; j<face.size()/3; j++) {
mjtNum area[3];
mjtNum edge1[3];
mjtNum edge2[3];
for (int i=0; i<3; i++) {
edge1[i] = uservert[3*(face[3*j+1]-1)+i] - uservert[3*(face[3*j]-1)+i];
edge2[i] = uservert[3*(face[3*j+2]-1)+i] - uservert[3*(face[3*j]-1)+i];
}
mjuu_crossvec(area, edge1, edge2);
for (int i=0; i<3; i++) {
volume[face[3*j+i]-1] += sqrt(mjuu_dot3(area, area)) / 2 * t;
}
}
} else {
for (int i=0; i<uservert.size()/3; i++) {
volume[i] = 6 * spacing * spacing / 2 * t;
}
}
// create bodies and geoms
for (int i=0; i<uservert.size()/3; i++) {
// create body
mjCBody* b = body->AddBody(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<int> face;
mjXUtil::String2Vector(userface, face);
int nvert = uservert.size()/3;
for (int j=0; j<2; j++) {
for (int i=0; i<nvert; i++) {
skin->vert.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<int> vertid;
vector<float> 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; i<face.size()/3; i++) {
skin->face.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
+4
View File
@@ -106,9 +106,13 @@ class mjCComposite {
// currently used only for cable
std::string initial; // root boundary type
std::vector<float> 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<std::string> username;
// plugin support
bool is_plugin;
std::string plugin_name;
@@ -41,9 +41,6 @@ TEST_F(PluginTest, ElasticEnergy) {
<composite type="particle" count="8 8 8" spacing="1">
<geom size=".025" group="4"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="nx" value="8"/>
<config key="ny" value="8"/>
<config key="nz" value="8"/>
<config key="poisson" value="0"/>
<config key="young" value="2"/>
</plugin>