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
plugin/elasticity/elasticity.cc
-16
View File
@@ -18,7 +18,6 @@
#include <cstdlib>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
@@ -47,19 +46,4 @@ bool CheckAttr(const char* name, const mjModel* m, int instance) {
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
plugin/elasticity/elasticity.h
+15 -8
View File
@@ -31,20 +31,27 @@ struct PairHash
}
};
inline 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];
}
inline 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);
}
}
// 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_
plugin/elasticity/shell.cc
+21 -55
View File
@@ -13,13 +13,10 @@
// limitations under the License.
#include <algorithm>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstdlib>
#include <functional>
#include <sstream>
#include <optional>
#include <string>
#include <unordered_map>
@@ -29,6 +26,7 @@
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "elasticity.h"
#include "shell.h"
@@ -40,22 +38,6 @@ constexpr int kNumEdges = Stencil2D::kNumEdges;
constexpr int kNumVerts = Stencil2D::kNumVerts;
constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
// copied from void mjXUtil
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);
}
}
}
// cotangent between two edges
mjtNum cot(mjtNum* x, int v0, int v1, int v2) {
mjtNum normal[3];
@@ -82,28 +64,12 @@ mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
return mju_norm3(normal) / 2;
}
// 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<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("face", m, instance) &&
CheckAttr("edge", m, instance) &&
CheckAttr("poisson", m, instance) &&
CheckAttr("young", m, instance) &&
CheckAttr("thickness", m, instance)) {
@@ -111,11 +77,10 @@ std::optional<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
mjtNum thick =
strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr);
mjtNum damp =
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
std::vector<int> face;
std::vector<int> face, edge;
String2Vector(mj_getPluginConfig(m, instance, "face"), face);
return Shell(m, d, instance, nu, E, damp, thick, face);
String2Vector(mj_getPluginConfig(m, instance, "edge"), edge);
return Shell(m, d, instance, nu, E, thick, face, edge);
} else {
mju_warning("Invalid parameter specification in shell plugin");
return std::nullopt;
@@ -123,13 +88,14 @@ std::optional<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
}
// create map from triangles to vertices and edges and from edges to vertices
void Shell::CreateStencils(const std::vector<int>& face) {
void Shell::CreateStencils(const std::vector<int>& simplex,
const std::vector<int>& edgeidx) {
// populate stencil
nt = face.size() / kNumVerts;
nt = simplex.size() / kNumVerts;
elements.resize(nt);
for (int t = 0; t < nt; t++) {
for (int v = 0; v < kNumVerts; v++) {
elements[t].vertices[v] = face[kNumVerts*t+v]-1;
elements[t].vertices[v] = simplex[kNumVerts*t+v];
}
}
@@ -162,14 +128,19 @@ void Shell::CreateStencils(const std::vector<int>& face) {
elements[t].edges[e] = it->second;
flaps[it->second].vertices[3] = v[(edge[e][1]+1) % 3];
}
if (!edgeidx.empty()) {
assert(elements[t].edges[e] == edgeidx[kNumEdges*t+e]);
}
}
}
}
// plugin constructor
Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, mjtNum thick, const std::vector<int>& face)
: damping(damp), thickness(thick) {
mjtNum thick, const std::vector<int>& face,
const std::vector<int>& edgeidx)
: thickness(thick) {
// count plugin bodies
nv = ne = 0;
for (int i = 1; i < m->nbody; i++) {
@@ -181,7 +152,7 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
}
// generate triangles from the vertices
CreateStencils(face);
CreateStencils(face, edgeidx);
// material parameters
mjtNum mu = E / (2*(1+nu));
@@ -234,10 +205,6 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
}
void Shell::Compute(const mjModel* m, mjData* d, int instance) {
mjtNum kD = damping / m->opt.timestep;
// compute bending contribution
mjtNum embedding;
for (int e = 0; e < ne; e++) {
int* v = flaps[e].vertices;
mjtNum force[12] = {0};
@@ -248,8 +215,7 @@ void Shell::Compute(const mjModel* m, mjData* d, int instance) {
for (int i = 0; i < StencilFlap::kNumVerts; i++) {
for (int j = 0; j < StencilFlap::kNumVerts; j++) {
for (int x = 0; x < 3; x++) {
embedding = d->xpos[3*(i0+v[j])+x]*(kD+1) - position[3*v[j]+x]*kD;
force[3*i+x] += bending[16*e+4*i+j] * embedding;
force[3*i+x] += bending[16*e+4*i+j] * d->xpos[3*(i0+v[j])+x];
}
}
}
@@ -275,8 +241,8 @@ void Shell::RegisterPlugin() {
plugin.name = "mujoco.elasticity.shell";
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"face", "young", "poisson", "damping",
"thickness"};
const char* attributes[] = {"face", "edge", "young",
"poisson", "thickness"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin/elasticity/shell.h
+4 -3
View File
@@ -64,14 +64,15 @@ class Shell {
std::vector<mjtNum> position; // previous-step positions (nv x 3)
std::vector<mjtNum> bending; // bending Hessian (ne x 16)
mjtNum damping;
mjtNum thickness;
private:
Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, mjtNum thick, const std::vector<int>& face);
mjtNum thick, const std::vector<int>& face,
const std::vector<int>& edgeidx);
void CreateStencils(const std::vector<int>& face);
void CreateStencils(const std::vector<int>& simplex,
const std::vector<int>& edgeidx);
};
} // namespace mujoco::plugin::elasticity
plugin/elasticity/solid.cc
+43 -14
View File
@@ -13,6 +13,7 @@
// limitations under the License.
#include <algorithm>
#include <cassert>
#include <cstdint>
#include <cstdlib>
#include <optional>
@@ -100,15 +101,17 @@ void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
// factory function
std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("face", m, instance) &&
CheckAttr("edge", m, instance) &&
CheckAttr("poisson", m, instance) &&
CheckAttr("young", m, instance)) {
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);
std::vector<int> face;
std::vector<int> face, edge;
String2Vector(mj_getPluginConfig(m, instance, "face"), face);
return Solid(m, d, instance, nu, E, damp, face);
String2Vector(mj_getPluginConfig(m, instance, "edge"), edge);
return Solid(m, d, instance, nu, E, damp, face, edge);
} else {
mju_warning("Invalid parameter specification in solid plugin");
return std::nullopt;
@@ -116,13 +119,14 @@ 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(const std::vector<int>& simplex) {
void Solid::CreateStencils(const std::vector<int>& simplex,
const std::vector<int>& edgeidx) {
// populate stencil
nt = simplex.size() / kNumVerts;
elements.resize(nt);
for (int t = 0; t < nt; t++) {
for (int v = 0; v < kNumVerts; v++) {
elements[t].vertices[v] = simplex[kNumVerts*t+v]-1;
elements[t].vertices[v] = simplex[kNumVerts*t+v];
}
}
@@ -149,14 +153,19 @@ void Solid::CreateStencils(const std::vector<int>& simplex) {
} else {
elements[t].edges[e] = it->second;
}
if (!edgeidx.empty()) {
assert(elements[t].edges[e] == edgeidx[kNumEdges*t+e]);
}
}
}
}
// plugin constructor
Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, const std::vector<int>& simplex)
: damping(damp) {
mjtNum damp, const std::vector<int>& simplex,
const std::vector<int>& edgeidx)
: f0(-1), damping(damp) {
// count plugin bodies
nv = ne = 0;
for (int i = 1; i < m->nbody; i++) {
@@ -167,8 +176,16 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
}
}
// count flexes
for (int i = 0; i < m->nflex; i++) {
if (m->flex_vertbodyid[m->flex_vertadr[i]] == i0) {
f0 = i;
break;
}
}
// generate tetrahedra from the vertices
CreateStencils(simplex);
CreateStencils(simplex, edgeidx);
// allocate arrays
metric.assign(kNumEdges*kNumEdges*nt, 0);
@@ -238,7 +255,10 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
}
void Solid::Compute(const mjModel* m, mjData* d, int instance) {
UpdateSquaredLengths(deformed, edges, d->xpos+3*i0);
// update edges if no flex
if (f0 < 0) {
UpdateSquaredLengths(deformed, edges, d->xpos+3*i0);
}
// loop over all elements
for (int t = 0; t < nt; t++) {
@@ -254,11 +274,18 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) {
// compute elongation
mjtNum elongation[kNumEdges];
mjtNum kD = damping / m->opt.timestep;
for (int e = 0; e < kNumEdges; e++) {
int idx = elements[t].edges[e];
elongation[e] = deformed[idx] - reference[idx] +
( deformed[idx] - previous[idx] ) * kD;
if (f0 < 0) {
int idx = elements[t].edges[e];
mjtNum kD = damping / m->opt.timestep;
elongation[e] = deformed[idx] - reference[idx] +
( deformed[idx] - previous[idx] ) * kD;
} else {
int idx = elements[t].edges[e] + m->flex_edgeadr[f0];
mjtNum deformed = d->flexedge_length[idx]*d->flexedge_length[idx];
mjtNum reference = m->flexedge_length0[idx]*m->flexedge_length0[idx];
elongation[e] = deformed - reference;
}
}
// we now multiply the elongations by the precomputed metric tensor,
@@ -289,7 +316,9 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) {
}
// update stored lengths
previous = deformed;
if (f0 < 0) {
previous = deformed;
}
}
@@ -301,7 +330,7 @@ void Solid::RegisterPlugin() {
plugin.name = "mujoco.elasticity.solid";
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"face", "young", "poisson", "damping"};
const char* attributes[] = {"face", "edge", "young", "poisson", "damping"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin/elasticity/solid.h
+5 -2
View File
@@ -44,6 +44,7 @@ class Solid {
static void RegisterPlugin();
int f0; // index of corresponding flex
int i0; // index of first body
int nc; // number of cubes in the grid
int nv; // number of vertices (bodies) in the solid
@@ -64,9 +65,11 @@ class Solid {
private:
Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, const std::vector<int>& simplex);
mjtNum damp, const std::vector<int>& simplex,
const std::vector<int>& edgeidx);
void CreateStencils(const std::vector<int>& simplex);
void CreateStencils(const std::vector<int>& simplex,
const std::vector<int>& edgeidx);
};
} // namespace mujoco::plugin::elasticity