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

Allow pinning flexes to non-simple bodies. Fixes #1270.

Browse Source 
PiperOrigin-RevId: 598865882
Change-Id: I105ff1910a7d67f5b7bcb46bcef0e5e388ec08ea
This commit is contained in:
Alessio Quaglino
2024-01-16 09:31:15 -08:00
committed by Copybara-Service
parent 3c1907e678
commit 80674149ce
5 changed files with 49 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/elasticity/solid.cc
+10 -2
View File
@@ -122,6 +122,7 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
for (int j = 0; j < m->flex_vertnum[i]; j++) {
if (m->flex_vertbodyid[m->flex_vertadr[i]+j] == i0) {
f0 = i;
nv = m->flex_vertnum[f0];
}
}
}
@@ -172,6 +173,7 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
deformed.assign(ne, 0);
previous.assign(ne, 0);
elongation.assign(ne, 0);
force.assign(3*nv, 0);
// compute edge lengths at equilibrium (m->flexedge_length0 not yet available)
UpdateSquaredLengths(reference, edges, body_pos);
@@ -202,11 +204,17 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) {
// compute gradient of elastic energy and insert into passive force
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);
ComputeForce<Stencil3D>(force, elements, metric, elongation, m, xpos);
// insert into passive force
if (f0 < 0) {
mju_addTo(qfrc, force.data(), force.size());
} else {
AddFlexForce(qfrc, force, m, d, xpos, f0);
}
// update stored lengths
if (kD > 0) {