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

Move 2d and 3d plugin force computation to elasticity.h.

Browse Source 
Change physical properties of `floppy_flex.xml` to resemble `floppy.xml`.

Make `Membrane` backward compatible with `Composite`.

PiperOrigin-RevId: 577190384
Change-Id: I5bc56ec62faa134156e839bec45d685c22eba5c4
This commit is contained in:
Alessio Quaglino
2023-10-27 07:17:14 -07:00
committed by Copybara-Service
parent f8c5ad2ba8
commit 40d927c9fa
7 changed files with 131 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/elasticity/solid.cc
+21 -55
View File
@@ -165,75 +165,41 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
reference.assign(ne, 0);
deformed.assign(ne, 0);
previous.assign(ne, 0);
elongation.assign(ne, 0);
// compute edge lengths at equilibrium
// compute edge lengths at equilibrium (m->flexedge_length0 not yet available)
UpdateSquaredLengths(reference, edges, m->body_pos+3*i0);
// save previous lengths
previous = reference;
}
void Solid::Compute(const mjModel* m, mjData* d, int instance) {
// update edges if no flex
mjtNum kD = damping / m->opt.timestep;
// update edge lengths
if (f0 < 0) {
UpdateSquaredLengths(deformed, edges, d->xpos+3*i0);
} else {
UpdateSquaredLengthsFlex(deformed,
d->flexedge_length + m->flex_edgeadr[f0]);
}
// loop over all elements
for (int t = 0; t < nt; t++) {
int* v = elements[t].vertices;
// we add generalized Rayleigh damping as decribed in Section 5.2 of
// Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
// compute length gradient with respect to dofs
mjtNum gradient[kNumEdges][2][3];
GradSquaredLengths<Stencil3D>(gradient, d->xpos+3*i0, v);
// we add generalized Rayleigh damping as decribed in Section 5.2 of
// Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
// compute elongation
mjtNum elongation[kNumEdges];
for (int e = 0; e < kNumEdges; e++) {
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,
// notice that if metric=diag(1/reference) then this would yield a
// mass-spring model
// compute local force
mjtNum force[kNumVerts*3] = {0};
int offset = kNumEdges*kNumEdges;
for (int ed1 = 0; ed1 < kNumEdges; ed1++) {
for (int ed2 = 0; ed2 < kNumEdges; ed2++) {
for (int i = 0; i < 2; i++) {
for (int x = 0; x < 3; x++) {
force[3 * Stencil3D::edge[ed2][i] + x] +=
elongation[ed1] * gradient[ed2][i][x] *
metric[offset * t + kNumEdges * ed1 + ed2];
}
}
}
}
// insert into global force
for (int i = 0; i < kNumVerts; i++) {
for (int x = 0; x < 3; x++) {
d->qfrc_passive[m->body_dofadr[i0]+3*v[i]+x] -= force[3*i+x];
}
}
for (int idx = 0; idx < ne; idx++) {
elongation[idx] = deformed[idx] - reference[idx] +
( deformed[idx] - previous[idx] ) * kD;
}
// compute gradient of elastic energy and insert into passive force
ComputeForce<Stencil3D>(d->qfrc_passive + m->body_dofadr[i0], elements,
metric, elongation, d->xpos + 3 * i0);
// update stored lengths
if (f0 < 0) {
if (kD > 0) {
previous = deformed;
}
}