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/elasticity.h
+27 -12
View File
@@ -86,13 +86,14 @@ void inline GradSquaredLengths(mjtNum gradient[T::kNumEdges][2][3],
}
template <typename T>
inline void ComputeForce(mjtNum* qfrc_passive,
inline void ComputeForce(std::vector<mjtNum>& qfrc_passive,
const std::vector<T>& elements,
const std::vector<mjtNum>& metric,
const std::vector<mjtNum>& 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<mjtNum>& 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];
}
}
}
plugin/elasticity/membrane.cc
+10 -2
View File
@@ -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<Stencil2D>(qfrc, elements, metric, elongation, m, bodyid, xpos);
ComputeForce<Stencil2D>(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) {
plugin/elasticity/membrane.h
+1
View File
@@ -57,6 +57,7 @@ class Membrane {
std::vector<mjtNum> deformed; // deformed lengths (ne x 1)
std::vector<mjtNum> previous; // previous-step lengths (ne x 1)
std::vector<mjtNum> elongation; // edge elongation (ne x 1)
std::vector<mjtNum> force; // force at all vertices (nv x 3)
mjtNum damping;
mjtNum thickness;
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) {
plugin/elasticity/solid.h
+1
View File
@@ -55,6 +55,7 @@ class Solid {
std::vector<mjtNum> deformed; // deformed lengths (ne x 1)
std::vector<mjtNum> previous; // previous-step lengths (ne x 1)
std::vector<mjtNum> elongation; // edge elongation (ne x 1)
std::vector<mjtNum> force; // force at all vertices (nv x 3)
mjtNum damping;