Move bending stiffness from plugin to the compiler.
PiperOrigin-RevId: 758677644 Change-Id: I7d5c5140ba6948002bfa4ae9d6b4bf0f6de0e39b
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
d8bebdc675
commit
71bdd915f7
@@ -33,54 +33,18 @@ namespace {
|
||||
// local tetrahedron numbering
|
||||
constexpr int kNumVerts = Stencil2D::kNumVerts;
|
||||
|
||||
// cotangent between two edges
|
||||
mjtNum cot(mjtNum* x, int v0, int v1, int v2) {
|
||||
mjtNum normal[3];
|
||||
mjtNum edge1[3];
|
||||
mjtNum edge2[3];
|
||||
|
||||
mju_sub3(edge1, x+3*v1, x+3*v0);
|
||||
mju_sub3(edge2, x+3*v2, x+3*v0);
|
||||
mju_cross(normal, edge1, edge2);
|
||||
|
||||
return mju_dot3(edge1, edge2) / mju_norm3(normal);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("poisson", m, instance) &&
|
||||
CheckAttr("young", m, instance) &&
|
||||
CheckAttr("thickness", m, instance)) {
|
||||
mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
|
||||
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
|
||||
mjtNum thick =
|
||||
strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr);
|
||||
return Shell(m, d, instance, nu, E, thick);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in shell plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
return Shell(m, d, instance);
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
mjtNum thick)
|
||||
: f0(-1), thickness(thick) {
|
||||
Shell::Shell(const mjModel* m, mjData* d, int instance)
|
||||
: f0(-1) {
|
||||
// count plugin bodies
|
||||
nv = 0;
|
||||
for (int i = 1; i < m->nbody; i++) {
|
||||
@@ -104,9 +68,6 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
}
|
||||
}
|
||||
|
||||
// material parameters
|
||||
mjtNum mu = E / (2*(1+nu));
|
||||
|
||||
// loop over all triangles
|
||||
for (int t = 0; t < m->flex_elemnum[f0]; t++) {
|
||||
int* v = m->flex_elem + 3*(t+m->flex_elemadr[f0]);
|
||||
@@ -119,41 +80,9 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
|
||||
// allocate array
|
||||
position.assign(nv*3, 0);
|
||||
bending.assign(m->flex_edgenum[f0]*16, 0);
|
||||
|
||||
// store previous positions
|
||||
mju_copy(position.data(), m->body_pos+3*i0, 3*nv);
|
||||
|
||||
// assemble bending Hessian
|
||||
for (int e = 0; e < m->flex_edgenum[f0]; e++) {
|
||||
int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f0]);
|
||||
int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f0]);
|
||||
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
|
||||
int vadj[3] = {v[1], v[0], v[3]};
|
||||
|
||||
if (v[3]== -1) {
|
||||
// skip boundary edges
|
||||
continue;
|
||||
}
|
||||
|
||||
// cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
|
||||
// Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
|
||||
|
||||
mjtNum a01 = cot(m->body_pos+3*i0, v[0], v[1], v[2]);
|
||||
mjtNum a02 = cot(m->body_pos+3*i0, v[0], v[3], v[1]);
|
||||
mjtNum a03 = cot(m->body_pos+3*i0, v[1], v[2], v[0]);
|
||||
mjtNum a04 = cot(m->body_pos+3*i0, v[1], v[0], v[3]);
|
||||
mjtNum c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
|
||||
mjtNum volume = ComputeVolume(m->body_pos+3*i0, v) +
|
||||
ComputeVolume(m->body_pos+3*i0, vadj);
|
||||
|
||||
for (int v1 = 0; v1 < StencilFlap::kNumVerts; v1++) {
|
||||
for (int v2 = 0; v2 < StencilFlap::kNumVerts; v2++) {
|
||||
bending[16 * e + 4 * v1 + v2] +=
|
||||
1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Shell::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
@@ -166,10 +95,11 @@ void Shell::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
// skip boundary edges
|
||||
continue;
|
||||
}
|
||||
mjtNum* k = m->flex_bending + 16*m->flex_edgeadr[f0];
|
||||
for (int i = 0; i < StencilFlap::kNumVerts; i++) {
|
||||
for (int j = 0; j < StencilFlap::kNumVerts; j++) {
|
||||
for (int x = 0; x < 3; x++) {
|
||||
force[3*i+x] += bending[16*e+4*i+j] * d->xpos[3*(i0+v[j])+x];
|
||||
force[3*i+x] += k[16*e+4*i+j] * d->xpos[3*(i0+v[j])+x];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -195,7 +125,7 @@ void Shell::RegisterPlugin() {
|
||||
plugin.name = "mujoco.elasticity.shell";
|
||||
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
|
||||
|
||||
const char* attributes[] = {"young", "poisson", "thickness", "damping"};
|
||||
const char* attributes[] = {"damping"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
@@ -51,13 +51,9 @@ class Shell {
|
||||
|
||||
// precomputed quantities
|
||||
std::vector<mjtNum> position; // previous-step positions (nv x 3)
|
||||
std::vector<mjtNum> bending; // bending Hessian (ne x 16)
|
||||
|
||||
mjtNum thickness;
|
||||
|
||||
private:
|
||||
Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
mjtNum thick);
|
||||
Shell(const mjModel* m, mjData* d, int instance);
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
|
||||
Reference in New Issue
Block a user