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

Make elasticity plugins compatible with flex pins. Fixes #1235.

Browse Source 
Before change:
poncho       18.20s
poncho_flex  44.60s
floppy       4.70s
floppy_flex  6.43s

After change:
poncho       18.23s
poncho_flex  44.62s
floppy       4.67s
floppy_flex  6.57s

PiperOrigin-RevId: 585952587
Change-Id: I0e7c1513f64c7ab1e6ad71e2168b4467c042b46f
This commit is contained in:
Alessio Quaglino
2023-11-28 06:12:35 -08:00
committed by Copybara-Service
parent 7cb84d45c1
commit 7d8d4d398e
5 changed files with 52 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/elasticity/solid.cc
+20 -10
View File
@@ -119,12 +119,17 @@ 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;
for (int j = 0; j < m->flex_vertnum[i]; j++) {
if (m->flex_vertbodyid[m->flex_vertadr[i]+j] == i0) {
f0 = i;
}
}
}
// vertex positions
mjtNum* body_pos =
f0 < 0 ? m->body_pos + 3*i0 : m->flex_xvert0 + 3*m->flex_vertadr[f0];
// generate tetrahedra from the vertices
nt = CreateStencils<Stencil3D>(elements, edges, simplex, edgeidx);
@@ -135,20 +140,21 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
for (int t = 0; t < nt; t++) {
int* v = elements[t].vertices;
for (int i = 0; i < kNumVerts; i++) {
if (m->body_plugin[i0+v[i]] != instance) {
mju_error("This body does not have the requested plugin instance");
int bi = f0 < 0 ? i0+v[i] : 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(m->body_pos+3*i0, v);
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], m->body_pos+3*i0, v,
ComputeBasis(basis[e], body_pos, v,
face[e2f[e][0]], face[e2f[e][1]], volume);
}
@@ -168,7 +174,7 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
elongation.assign(ne, 0);
// compute edge lengths at equilibrium (m->flexedge_length0 not yet available)
UpdateSquaredLengths(reference, edges, m->body_pos+3*i0);
UpdateSquaredLengths(reference, edges, body_pos);
// save previous lengths
previous = reference;
@@ -195,8 +201,12 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) {
}
// 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);
int flex_vertadr = f0 < 0 ? -1 : m->flex_vertadr[f0];
int* bodyid = f0 < 0 ? nullptr : m->flex_vertbodyid + flex_vertadr;
mjtNum* xpos = f0 < 0 ? d->xpos + 3*i0 : d->flexvert_xpos + 3*flex_vertadr;
mjtNum* qfrc = d->qfrc_passive + (f0 < 0 ? m->body_dofadr[i0] : 0);
ComputeForce<Stencil3D>(qfrc, elements, metric, elongation, m, bodyid, xpos);
// update stored lengths
if (kD > 0) {