Move elasticity computation to mjCFlex.
PiperOrigin-RevId: 675949380 Change-Id: Ia48f4fd6ae1ede206ccacd1205914e7e4b3c7aec
This commit is contained in:
committed by
Copybara-Service
parent
0657e3e871
commit
ddbc083810
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
+226
-2
@@ -54,7 +54,6 @@
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include "engine/engine_crossplatform.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_sort.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
@@ -2374,7 +2373,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
|
||||
|
||||
|
||||
|
||||
//------------------ class mjCFlex implementation --------------------------------------------------
|
||||
//--------------------- elasticity implementation --------------------------------------------------
|
||||
|
||||
// hash function for std::pair
|
||||
struct PairHash
|
||||
@@ -2393,6 +2392,215 @@ constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1},
|
||||
{{ 0, 1}, { 1, 2}, { 2, 0},
|
||||
{ 2, 3}, { 0, 3}, { 1, 3}}};
|
||||
|
||||
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];
|
||||
};
|
||||
|
||||
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];
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
inline double ComputeVolume(const double* x, const int v[T::kNumVerts]);
|
||||
|
||||
template <>
|
||||
inline double ComputeVolume<Stencil2D>(const double* x,
|
||||
const int v[Stencil2D::kNumVerts]) {
|
||||
double normal[3];
|
||||
const double* x0 = x + 3*v[0];
|
||||
const double* x1 = x + 3*v[1];
|
||||
const double* x2 = x + 3*v[2];
|
||||
double edge1[3] = {x1[0]-x0[0], x1[1]-x0[1], x1[2]-x0[2]};
|
||||
double edge2[3] = {x2[0]-x0[0], x2[1]-x0[1], x2[2]-x0[2]};
|
||||
mjuu_crossvec(normal, edge1, edge2);
|
||||
return mjuu_normvec(normal, 3) / 2;
|
||||
}
|
||||
|
||||
template<>
|
||||
inline double ComputeVolume<Stencil3D>(const double* x,
|
||||
const int v[Stencil3D::kNumVerts]) {
|
||||
double normal[3];
|
||||
const double* x0 = x + 3*v[0];
|
||||
const double* x1 = x + 3*v[1];
|
||||
const double* x2 = x + 3*v[2];
|
||||
const double* x3 = x + 3*v[3];
|
||||
double edge1[3] = {x1[0]-x0[0], x1[1]-x0[1], x1[2]-x0[2]};
|
||||
double edge2[3] = {x2[0]-x0[0], x2[1]-x0[1], x2[2]-x0[2]};
|
||||
double edge3[3] = {x3[0]-x0[0], x3[1]-x0[1], x3[2]-x0[2]};
|
||||
mjuu_crossvec(normal, edge1, edge2);
|
||||
return mjuu_dot3(normal, edge3) / 6;
|
||||
}
|
||||
|
||||
// compute metric tensor of edge lengths inner product
|
||||
template <typename T>
|
||||
void inline MetricTensor(double* metric, int idx, double mu,
|
||||
double la, const double basis[T::kNumEdges][9]) {
|
||||
double trE[T::kNumEdges] = {0};
|
||||
double trEE[T::kNumEdges*T::kNumEdges] = {0};
|
||||
double 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");
|
||||
}
|
||||
}
|
||||
|
||||
// compute local basis
|
||||
template <typename T>
|
||||
void inline ComputeBasis(double basis[9], const double* x,
|
||||
const int v[T::kNumVerts],
|
||||
const int faceL[T::kNumFaces],
|
||||
const int faceR[T::kNumFaces], double volume);
|
||||
|
||||
template <>
|
||||
void inline ComputeBasis<Stencil2D>(double basis[9], const double* x,
|
||||
const int v[Stencil2D::kNumVerts],
|
||||
const int faceL[Stencil2D::kNumFaces],
|
||||
const int faceR[Stencil2D::kNumFaces],
|
||||
double volume) {
|
||||
double basisL[3], basisR[3];
|
||||
double normal[3];
|
||||
|
||||
const double* xL0 = x + 3*v[faceL[0]];
|
||||
const double* xL1 = x + 3*v[faceL[1]];
|
||||
const double* xR0 = x + 3*v[faceR[0]];
|
||||
const double* xR1 = x + 3*v[faceR[1]];
|
||||
double edgesL[3] = {xL0[0]-xL1[0], xL0[1]-xL1[1], xL0[2]-xL1[2]};
|
||||
double edgesR[3] = {xR1[0]-xR0[0], xR1[1]-xR0[1], xR1[2]-xR0[2]};
|
||||
|
||||
mjuu_crossvec(normal, edgesR, edgesL);
|
||||
mjuu_normvec(normal, 3);
|
||||
mjuu_crossvec(basisL, normal, edgesL);
|
||||
mjuu_crossvec(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*volume*volume);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// compute local basis
|
||||
template <>
|
||||
void inline ComputeBasis<Stencil3D>(double basis[9], const double* x,
|
||||
const int v[Stencil3D::kNumVerts],
|
||||
const int faceL[Stencil3D::kNumFaces],
|
||||
const int faceR[Stencil3D::kNumFaces],
|
||||
double volume) {
|
||||
const double* xL0 = x + 3*v[faceL[0]];
|
||||
const double* xL1 = x + 3*v[faceL[1]];
|
||||
const double* xL2 = x + 3*v[faceL[2]];
|
||||
const double* xR0 = x + 3*v[faceR[0]];
|
||||
const double* xR1 = x + 3*v[faceR[1]];
|
||||
const double* xR2 = x + 3*v[faceR[2]];
|
||||
double edgesL[6] = {xL1[0] - xL0[0], xL1[1] - xL0[1], xL1[2] - xL0[2],
|
||||
xL2[0] - xL0[0], xL2[1] - xL0[1], xL2[2] - xL0[2]};
|
||||
double edgesR[6] = {xR1[0] - xR0[0], xR1[1] - xR0[1], xR1[2] - xR0[2],
|
||||
xR2[0] - xR0[0], xR2[1] - xR0[1], xR2[2] - xR0[2]};
|
||||
|
||||
double normalL[3], normalR[3];
|
||||
mjuu_crossvec(normalL, edgesL, edgesL+3);
|
||||
mjuu_crossvec(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// compute stiffness for a single element
|
||||
template <typename T>
|
||||
void inline ComputeStiffness(std::vector<double>& stiffness,
|
||||
const std::vector<double>& body_pos,
|
||||
const int* v, int t, double E,
|
||||
double nu, double thickness = 4) {
|
||||
// triangles area
|
||||
double volume = ComputeVolume<T>(body_pos.data(), v);
|
||||
|
||||
// material parameters
|
||||
double mu = E / (2*(1+nu)) * std::abs(volume) / 4 * thickness;
|
||||
double la = E*nu / ((1+nu)*(1-2*nu)) * std::abs(volume) / 4 * thickness;
|
||||
|
||||
// local geometric quantities
|
||||
double basis[T::kNumEdges][9] = {{0}};
|
||||
|
||||
// compute edge basis
|
||||
for (int e = 0; e < T::kNumEdges; e++) {
|
||||
ComputeBasis<T>(basis[e], body_pos.data(), v,
|
||||
T::face[T::edge2face[e][0]],
|
||||
T::face[T::edge2face[e][1]], volume);
|
||||
}
|
||||
|
||||
// compute metric tensor
|
||||
MetricTensor<T>(stiffness.data(), t, mu, la, basis);
|
||||
}
|
||||
|
||||
//------------------ class mjCFlex implementation --------------------------------------------------
|
||||
|
||||
// constructor
|
||||
mjCFlex::mjCFlex(mjCModel* _model) {
|
||||
mjs_defaultFlex(&spec);
|
||||
@@ -2665,6 +2873,22 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
// set size
|
||||
nedge = (int)edge.size();
|
||||
|
||||
// compute elasticity
|
||||
if (young > 0) {
|
||||
stiffness.assign(21*nelem, 0);
|
||||
for (unsigned int t = 0; t < nelem; t++) {
|
||||
if (dim==2) {
|
||||
ComputeStiffness<Stencil2D>(stiffness, vertxpos,
|
||||
elem_.data() + (dim + 1) * t, t, young,
|
||||
poisson, thickness);
|
||||
} else if (dim==3) {
|
||||
ComputeStiffness<Stencil3D>(stiffness, vertxpos,
|
||||
elem_.data() + (dim + 1) * t, t, young,
|
||||
poisson);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add plugins
|
||||
std::string userface, useredge;
|
||||
userface = VectorToString(elem_);
|
||||
|
||||
@@ -2518,8 +2518,11 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4);
|
||||
|
||||
// elasticity
|
||||
// TODO: these are now written by plugins, they will be moved to mjCFlex
|
||||
mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem);
|
||||
if (!pfl->stiffness.empty()) {
|
||||
mjuu_copyvec(m->flex_stiffness + 21 * elem_adr, pfl->stiffness.data(), pfl->stiffness.size());
|
||||
} else {
|
||||
mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem);
|
||||
}
|
||||
|
||||
// set fields: mesh-like
|
||||
m->flex_dim[i] = pfl->dim;
|
||||
@@ -2588,7 +2591,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
|
||||
}
|
||||
else {
|
||||
memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum));
|
||||
mjuu_copyvec(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert);
|
||||
}
|
||||
|
||||
// copy or set vertbodyid
|
||||
|
||||
@@ -695,6 +695,7 @@ class mjCFlex_ : public mjCBase {
|
||||
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
|
||||
std::vector<double> elemaabb_; // element bounding volume
|
||||
std::vector<int> edgeidx_; // element edge ids
|
||||
std::vector<double> stiffness; // elasticity stiffness matrix
|
||||
|
||||
// variable-size data
|
||||
std::vector<std::string> vertbody_; // vertex body names
|
||||
|
||||
@@ -180,6 +180,13 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
|
||||
elem->DeleteChild(cont);
|
||||
}
|
||||
|
||||
// elasticity subelement
|
||||
XMLElement* elastic = InsertEnd(elem, "elasticity");
|
||||
WriteAttr(elastic, "young", 1, &flex->young, &defflex.young);
|
||||
WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson);
|
||||
WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness);
|
||||
WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping);
|
||||
|
||||
// edge subelement
|
||||
XMLElement* edge = InsertEnd(elem, "edge");
|
||||
WriteAttr(edge, "stiffness", 1, &flex->edgestiffness, &defflex.edgestiffness);
|
||||
|
||||
Reference in New Issue
Block a user