diff --git a/plugin/elasticity/elasticity.h b/plugin/elasticity/elasticity.h index 00074a6e..fa70e356 100644 --- a/plugin/elasticity/elasticity.h +++ b/plugin/elasticity/elasticity.h @@ -86,13 +86,14 @@ void inline GradSquaredLengths(mjtNum gradient[T::kNumEdges][2][3], } template -inline void ComputeForce(mjtNum* qfrc_passive, +inline void ComputeForce(std::vector& qfrc_passive, const std::vector& elements, const std::vector& metric, const std::vector& elongationglob, const mjModel* m, - const int* vertbodyid, const mjtNum* xpos) { + mju_zero(qfrc_passive.data(), qfrc_passive.size()); + for (int t = 0; t < elements.size(); t++) { const int* v = elements[t].vertices; @@ -118,7 +119,7 @@ inline void ComputeForce(mjtNum* qfrc_passive, for (int ed2 = 0; ed2 < T::kNumEdges; ed2++) { for (int i = 0; i < 2; i++) { for (int x = 0; x < 3; x++) { - force[3 * T::edge[ed2][i] + x] += + force[3 * T::edge[ed2][i] + x] -= elongation[ed1] * gradient[ed2][i][x] * metric[offset * t + T::kNumEdges * ed1 + ed2]; } @@ -128,17 +129,31 @@ inline void ComputeForce(mjtNum* qfrc_passive, // insert into global force for (int i = 0; i < T::kNumVerts; i++) { - 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]]]; - if (body_dofnum && m->body_simple[vertbodyid[v[i]]] != 2) { - mju_error("Non-simple or non-static bodies are not yet supported"); - } + for (int x = 0; x < 3; x++) { + qfrc_passive[3*v[i]+x] += force[3*i+x]; } + } + } +} + +// add flex force to degrees of freedom +inline void AddFlexForce(mjtNum* qfrc, + const std::vector& force, + const mjModel* m, mjData* d, + const mjtNum* xpos, + int f0) { + int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f0]; + + for (int v = 0; v < m->flex_vertnum[f0]; v++) { + int bid = bodyid[v]; + if (m->body_simple[bid] != 2) { + // this should only occur for pinned flex vertices + mj_applyFT(m, d, force.data() + 3*v, 0, xpos + 3*v, bid, qfrc); + } else { + int body_dofnum = m->body_dofnum[bid]; + int body_dofadr = m->body_dofadr[bid]; for (int x = 0; x < body_dofnum; x++) { - qfrc_passive[body_dofadr+x] -= force[3*i+x]; + qfrc[body_dofadr+x] += force[3*v+x]; } } } diff --git a/plugin/elasticity/membrane.cc b/plugin/elasticity/membrane.cc index 51283099..f2f88009 100644 --- a/plugin/elasticity/membrane.cc +++ b/plugin/elasticity/membrane.cc @@ -117,6 +117,7 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, 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]; } } } @@ -168,6 +169,7 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, 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); @@ -198,11 +200,17 @@ void Membrane::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(qfrc, elements, metric, elongation, m, bodyid, xpos); + ComputeForce(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) { diff --git a/plugin/elasticity/membrane.h b/plugin/elasticity/membrane.h index 4b39d426..2b9a23da 100644 --- a/plugin/elasticity/membrane.h +++ b/plugin/elasticity/membrane.h @@ -57,6 +57,7 @@ class Membrane { std::vector deformed; // deformed lengths (ne x 1) std::vector previous; // previous-step lengths (ne x 1) std::vector elongation; // edge elongation (ne x 1) + std::vector force; // force at all vertices (nv x 3) mjtNum damping; mjtNum thickness; diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc index 35516a89..5660e144 100644 --- a/plugin/elasticity/solid.cc +++ b/plugin/elasticity/solid.cc @@ -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(qfrc, elements, metric, elongation, m, bodyid, xpos); + ComputeForce(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) { diff --git a/plugin/elasticity/solid.h b/plugin/elasticity/solid.h index ae1f2fe5..acbd99a8 100644 --- a/plugin/elasticity/solid.h +++ b/plugin/elasticity/solid.h @@ -55,6 +55,7 @@ class Solid { std::vector deformed; // deformed lengths (ne x 1) std::vector previous; // previous-step lengths (ne x 1) std::vector elongation; // edge elongation (ne x 1) + std::vector force; // force at all vertices (nv x 3) mjtNum damping;