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

Move elasticity computation to mjCFlex.

Browse Source 
PiperOrigin-RevId: 675949380
Change-Id: Ia48f4fd6ae1ede206ccacd1205914e7e4b3c7aec
This commit is contained in:
Alessio Quaglino
2024-09-18 05:22:05 -07:00
committed by Copybara-Service
parent 0657e3e871
commit ddbc083810
7 changed files with 250 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/elasticity/elasticity.h
+8 -54
View File
@@ -36,7 +36,10 @@ struct PairHash
struct Stencil2D {
static constexpr int kNumEdges = 3;
static constexpr int kNumVerts = 3;
static constexpr int kNumFaces = 2;
static constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
static constexpr int face[kNumVerts][2] = {{1, 2}, {2, 0}, {0, 1}};
static constexpr int edge2face[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
int vertices[kNumVerts];
int edges[kNumEdges];
};
@@ -44,8 +47,13 @@ struct Stencil2D {
struct Stencil3D {
static constexpr int kNumEdges = 6;
static constexpr int kNumVerts = 4;
static constexpr int kNumFaces = 3;
static constexpr int edge[kNumEdges][2] = {{0, 1}, {1, 2}, {2, 0},
{2, 3}, {0, 3}, {1, 3}};
static constexpr int face[kNumVerts][3] = {{2, 1, 0}, {0, 1, 3},
{1, 2, 3}, {2, 0, 3}};
static constexpr int edge2face[kNumEdges][2] = {{2, 3}, {1, 3}, {2, 1},
{1, 0}, {0, 2}, {0, 3}};
int vertices[kNumVerts];
int edges[kNumEdges];
};
@@ -151,60 +159,6 @@ inline void AddFlexForce(mjtNum* qfrc,
}
}
// compute metric tensor of edge lengths inner product
template <typename T>
void inline MetricTensor(mjtNum* metric, int idx, mjtNum mu,
mjtNum la, const mjtNum basis[T::kNumEdges][9]) {
mjtNum trE[T::kNumEdges] = {0};
mjtNum trEE[T::kNumEdges*T::kNumEdges] = {0};
mjtNum k[T::kNumEdges*T::kNumEdges];
// compute first invariant i.e. trace(strain)
for (int e = 0; e < T::kNumEdges; e++) {
for (int i = 0; i < 3; i++) {
trE[e] += basis[e][4*i];
}
}
// compute second invariant i.e. trace(strain^2)
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = 0; ed2 < T::kNumEdges; ed2++) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
trEE[T::kNumEdges*ed1+ed2] += basis[ed1][3*i+j] * basis[ed2][3*j+i];
}
}
}
}
// assembly of strain metric tensor
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = 0; ed2 < T::kNumEdges; ed2++) {
k[T::kNumEdges*ed1 + ed2] = mu * trEE[T::kNumEdges * ed1 + ed2] +
la * trE[ed2] * trE[ed1];
}
}
// copy to triangular representation
int id = 0;
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = ed1; ed2 < T::kNumEdges; ed2++) {
metric[21*idx + id++] = k[T::kNumEdges*ed1 + ed2];
}
}
if (id != T::kNumEdges*(T::kNumEdges+1)/2) {
mju_error("incorrect stiffness matrix size");
}
}
// convert from Flex connectivity to stencils
template <typename T>
int CreateStencils(std::vector<T>& elements,
std::vector<std::pair<int, int>>& edges,
const std::vector<int>& simplex,
const std::vector<int>& edgeidx);
// copied from mjXUtil
void String2Vector(const std::string& txt, std::vector<int>& vec);
plugin/elasticity/membrane.cc
+1 -74
View File
@@ -27,54 +27,6 @@
namespace mujoco::plugin::elasticity {
namespace {
// local tetrahedron numbering
constexpr int kNumEdges = Stencil2D::kNumEdges;
constexpr int kNumVerts = Stencil2D::kNumVerts;
// area of a triangle
mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
mjtNum normal[3];
mjtNum edge1[3];
mjtNum edge2[3];
mju_sub3(edge1, x+3*v[1], x+3*v[0]);
mju_sub3(edge2, x+3*v[2], x+3*v[0]);
mju_cross(normal, edge1, edge2);
return mju_norm3(normal) / 2;
}
// compute local basis
void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[kNumVerts],
const int faceL[2], const int faceR[2], mjtNum area) {
mjtNum basisL[3], basisR[3];
mjtNum edgesL[3], edgesR[3];
mjtNum normal[3];
mju_sub3(edgesL, x+3*v[faceL[0]], x+3*v[faceL[1]]);
mju_sub3(edgesR, x+3*v[faceR[1]], x+3*v[faceR[0]]);
mju_cross(normal, edgesR, edgesL);
mju_normalize3(normal);
mju_cross(basisL, normal, edgesL);
mju_cross(basisR, edgesR, normal);
// we use as basis the symmetrized tensor products of the edge normals of the
// other two edges; this is shown in Weischedel "A discrete geometric view on
// shear-deformable shell models" in the remark at the end of section 4.1;
// equivalent to linear finite elements but in a coordinate-free formulation.
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
basis[3*i+j] = ( basisL[i]*basisR[j] +
basisR[i]*basisL[j] ) / (8*area*area);
}
}
}
} // namespace
// factory function
std::optional<Membrane> Membrane::Create(const mjModel* m, mjData* d,
@@ -118,41 +70,16 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu,
}
}
// vertex positions
mjtNum* body_pos = m->flex_xvert0 + 3*m->flex_vertadr[f0];
// loop over all triangles
const int* elem = m->flex_elem + m->flex_elemdataadr[f0];
for (int t = 0; t < m->flex_elemnum[f0]; t++) {
const int* v = elem + (m->flex_dim[f0]+1) * t;
for (int i = 0; i < kNumVerts; i++) {
for (int i = 0; i < Stencil2D::kNumVerts; i++) {
int bi = m->flex_vertbodyid[m->flex_vertadr[f0]+v[i]];
if (bi && m->body_plugin[bi] != instance) {
mju_error("Body %d does not have plugin instance %d", bi, instance);
}
}
// triangles area
mjtNum volume = ComputeVolume(body_pos, v);
// material parameters
mjtNum mu = E / (2*(1+nu)) * mju_abs(volume) / 4 * thickness;
mjtNum la = E*nu / ((1+nu)*(1-2*nu)) * mju_abs(volume) / 4 * thickness;
// local geometric quantities
mjtNum basis[kNumEdges][9] = {{0}, {0}, {0}};
// compute edge basis
for (int e = 0; e < kNumEdges; e++) {
ComputeBasis(basis[e], body_pos, v,
Stencil2D::edge[Stencil2D::edge[e][0]],
Stencil2D::edge[Stencil2D::edge[e][1]], volume);
}
// compute metric tensor
// TODO: do not write in a const mjModel
MetricTensor<Stencil2D>(m->flex_stiffness + 21 * m->flex_elemadr[f0], t, mu,
la, basis);
}
// allocate array
plugin/elasticity/solid.cc
+1 -79
View File
@@ -12,7 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <cassert>
#include <cstdint>
#include <cstdlib>
@@ -29,59 +28,6 @@
namespace mujoco::plugin::elasticity {
namespace {
// local tetrahedron numbering
constexpr int kNumEdges = Stencil3D::kNumEdges;
constexpr int kNumVerts = Stencil3D::kNumVerts;
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}};
// volume of a tetrahedron
mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
mjtNum normal[3];
mjtNum edge1[3];
mjtNum edge2[3];
mjtNum edge3[3];
mju_sub3(edge1, x+3*v[1], x+3*v[0]);
mju_sub3(edge2, x+3*v[2], x+3*v[0]);
mju_sub3(edge3, x+3*v[3], x+3*v[0]);
mju_cross(normal, edge2, edge1);
return mju_dot3(normal, edge3) / 6;
}
// compute local basis
void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[kNumVerts],
const int faceL[3], const int faceR[3], mjtNum volume) {
mjtNum normalL[3], normalR[3];
mjtNum edgesL[6], edgesR[6];
mju_sub3(edgesL+0, x+3*v[faceL[1]], x+3*v[faceL[0]]);
mju_sub3(edgesL+3, x+3*v[faceL[2]], x+3*v[faceL[0]]);
mju_sub3(edgesR+0, x+3*v[faceR[1]], x+3*v[faceR[0]]);
mju_sub3(edgesR+3, x+3*v[faceR[2]], x+3*v[faceR[0]]);
mju_cross(normalL, edgesL, edgesL+3);
mju_cross(normalR, edgesR, edgesR+3);
// we use as basis the symmetrized tensor products of the area normals of the
// two faces not adjacent to the edge; this is the 3D equivalent to the basis
// proposed in Weischedel "A discrete geometric view on shear-deformable shell
// models" in the remark at the end of section 4.1. This is also equivalent to
// linear finite elements but in a coordinate-free formulation.
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
basis[3*i+j] = ( normalL[i]*normalR[j] +
normalR[i]*normalL[j] ) / (36*2*volume*volume);
}
}
}
} // namespace
// factory function
std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
@@ -127,40 +73,16 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
}
}
// vertex positions
mjtNum* body_pos = m->flex_xvert0 + 3*m->flex_vertadr[f0];
// loop over all tetrahedra
const int* elem = m->flex_elem + m->flex_elemdataadr[f0];
for (int t = 0; t < m->flex_elemnum[f0]; t++) {
const int* v = elem + (m->flex_dim[f0]+1) * t;
for (int i = 0; i < kNumVerts; i++) {
for (int i = 0; i < Stencil3D::kNumVerts; i++) {
int bi = m->flex_vertbodyid[m->flex_vertadr[f0]+v[i]];
if (bi && m->body_plugin[bi] != instance) {
mju_error("Body %d does not have plugin instance %d", bi, instance);
}
}
// tetrahedron volume
mjtNum volume = ComputeVolume(body_pos, v);
// local geometric quantities
mjtNum basis[kNumEdges][9] = {{0}, {0}, {0}, {0}, {0}, {0}};
// compute edge basis
for (int e = 0; e < kNumEdges; e++) {
ComputeBasis(basis[e], body_pos, v,
face[e2f[e][0]], face[e2f[e][1]], volume);
}
// material parameters
mjtNum mu = E / (2*(1+nu)) * volume;
mjtNum la = E*nu / ((1+nu)*(1-2*nu)) * volume;
// compute metric tensor
// TODO: do not write in a const mjModel
MetricTensor<Stencil3D>(m->flex_stiffness + 21 * m->flex_elemadr[f0], t, mu,
la, basis);
}
// allocate array