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/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();