chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update Solid and Shell plugins to use FlexComp.

Browse Source 
PiperOrigin-RevId: 573755821
Change-Id: I3b8d7fb27f0e940038fa9fd2b26ecc746df4f7ac
This commit is contained in:
Alessio Quaglino
2023-10-16 03:02:14 -07:00
committed by Copybara-Service
parent 93d1c3c92e
commit 110ade1435
26 changed files with 1988 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+6
View File
@@ -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;
src/engine/engine_core_smooth.c
+1 -3
View File
@@ -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;
}
src/engine/engine_sensor.c
+2 -2
View File
@@ -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;
}
src/user/user_composite.cc
+13 -10
View File
@@ -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; i<face.size(); face[i++]--) {};
mjXUtil::Vector2String(userface, face);
}
// compute volume
@@ -414,13 +416,13 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
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];
edge1[i] = uservert[3*face[3*j+1]+i] - uservert[3*face[3*j]+i];
edge2[i] = uservert[3*face[3*j+2]+i] - uservert[3*face[3*j]+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;
volume[face[3*j+i]] += sqrt(mjuu_dot3(area, area)) / 2 * t;
}
}
} else {
@@ -489,6 +491,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
}
b->plugin_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; i<face.size()/3; i++) {
for (int j=0; j<3; j++) {
int v0 = face[3*i+(j+0)%3]-1;
int v1 = face[3*i+(j+1)%3]-1;
int v0 = face[3*i+(j+0)%3];
int v1 = face[3*i+(j+1)%3];
edge.push_back(v0 < v1 ? std::pair(v0, v1) : std::pair(v1, v0));
}
}
@@ -1271,9 +1274,9 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
}
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);
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)]);
}
}
src/user/user_flexcomp.cc
+10 -1
View File
@@ -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;
}
}
}
src/user/user_flexcomp.h
+6
View File
@@ -85,6 +85,12 @@ class mjCFlexcomp {
std::vector<bool> used; // is point used by any element (false: skip)
std::vector<int> element; // flex elements
std::vector<float> 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_
src/user/user_mesh.cc
+59 -31
View File
@@ -18,9 +18,11 @@
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <functional>
#include <memory>
#include <optional>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
@@ -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 <tiny_obj_loader.h>
extern "C" {
@@ -2157,6 +2160,24 @@ void mjCSkin::LoadSKN(mjResource* resource) {
//------------------ class mjCFlex implementation --------------------------------------------------
// hash function for std::pair
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);
}
};
// 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<int> edgeidx(elem.size()*kNumEdges[dim-1]);
// remove repeated edges
std::vector<std::pair<int,int>> 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<std::pair<int, int>, 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();
src/xml/xml_native_reader.cc
+30 -9
View File
@@ -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);