From 7d8d4d398ee2d704edbfc07bd9ab67e458a2c77c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 28 Nov 2023 06:12:35 -0800 Subject: [PATCH] Make elasticity plugins compatible with flex pins. Fixes #1235. 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 --- doc/changelog.rst | 1 + model/plugin/elasticity/trampoline_flex.xml | 8 +----- plugin/elasticity/elasticity.h | 12 +++++++-- plugin/elasticity/membrane.cc | 30 ++++++++++++++------- plugin/elasticity/solid.cc | 30 ++++++++++++++------- 5 files changed, 52 insertions(+), 29 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 57802a76..677cdbcf 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -33,6 +33,7 @@ Bug fixes - Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. Before this fix such actuators could lead to non-conservation of momentum. - Fix bug that prevented using flex with the :ref:`passive viewer`. +- Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. Version 3.0.1 (November 15, 2023) --------------------------------- diff --git a/model/plugin/elasticity/trampoline_flex.xml b/model/plugin/elasticity/trampoline_flex.xml index 874e12fa..106e37cc 100644 --- a/model/plugin/elasticity/trampoline_flex.xml +++ b/model/plugin/elasticity/trampoline_flex.xml @@ -44,6 +44,7 @@ radius=".001" mass="10" name="plate" dim="2"> + @@ -52,11 +53,4 @@ - - - - - - - diff --git a/plugin/elasticity/elasticity.h b/plugin/elasticity/elasticity.h index 52670c75..8436777e 100644 --- a/plugin/elasticity/elasticity.h +++ b/plugin/elasticity/elasticity.h @@ -90,6 +90,8 @@ inline void ComputeForce(mjtNum* qfrc_passive, const std::vector& elements, const std::vector& metric, const std::vector& elongationglob, + const mjModel* m, + const int* vertbodyid, const mjtNum* xpos) { for (int t = 0; t < elements.size(); t++) { const int* v = elements[t].vertices; @@ -126,8 +128,14 @@ inline void ComputeForce(mjtNum* qfrc_passive, // insert into global force for (int i = 0; i < T::kNumVerts; i++) { - for (int x = 0; x < 3; x++) { - qfrc_passive[3*v[i]+x] -= force[3*i+x]; + int body_dofnum = 3; + int body_dofadr = 3*v[i]; + if (vertbodyid) { + body_dofnum = m->body_dofnum[vertbodyid[v[i]]]; + body_dofadr = m->body_dofadr[vertbodyid[v[i]]]; + } + for (int x = 0; x < body_dofnum; x++) { + qfrc_passive[body_dofadr+x] -= force[3*i+x]; } } } diff --git a/plugin/elasticity/membrane.cc b/plugin/elasticity/membrane.cc index d0a93454..51283099 100644 --- a/plugin/elasticity/membrane.cc +++ b/plugin/elasticity/membrane.cc @@ -114,12 +114,17 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, // 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 triangles from the vertices nt = CreateStencils(elements, edges, simplex, edgeidx); @@ -130,13 +135,14 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, 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); } } // triangles area - mjtNum volume = ComputeVolume(m->body_pos+3*i0, v); + mjtNum volume = ComputeVolume(body_pos, v); // material parameters mjtNum mu = E / (2*(1+nu)) * mju_abs(volume) / 4 * thickness; @@ -147,7 +153,7 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, // 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, Stencil2D::edge[Stencil2D::edge[e][0]], Stencil2D::edge[Stencil2D::edge[e][1]], volume); } @@ -164,7 +170,7 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, 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; @@ -191,8 +197,12 @@ void Membrane::Compute(const mjModel* m, mjData* d, int instance) { } // compute gradient of elastic energy and insert into passive force - ComputeForce(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(qfrc, elements, metric, elongation, m, bodyid, xpos); // update stored lengths if (kD > 0) { diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc index cca4ff78..35516a89 100644 --- a/plugin/elasticity/solid.cc +++ b/plugin/elasticity/solid.cc @@ -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(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(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(qfrc, elements, metric, elongation, m, bodyid, xpos); // update stored lengths if (kD > 0) {