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
doc/changelog.rst
+1
View File
@@ -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<PyViewerPassive>`.
- Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices.
Version 3.0.1 (November 15, 2023)
---------------------------------
model/plugin/elasticity/trampoline_flex.xml
+1 -7
View File
@@ -44,6 +44,7 @@
radius=".001" mass="10" name="plate" dim="2">
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<edge equality="false" damping="10"/>
<pin id="0 15 240 255"/>
<plugin plugin="mujoco.elasticity.membrane">
<config key="poisson" value="0"/>
<config key="thickness" value="1e-2"/>
@@ -52,11 +53,4 @@
</plugin>
</flexcomp>
</worldbody>
<equality>
<connect body1="plate_0" anchor="0 0 0"/>
<connect body1="plate_15" anchor="0 0 0"/>
<connect body1="plate_240" anchor="0 0 0"/>
<connect body1="plate_255" anchor="0 0 0"/>
</equality>
</mujoco>
plugin/elasticity/elasticity.h
+10 -2
View File
@@ -90,6 +90,8 @@ inline void ComputeForce(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) {
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];
}
}
}
plugin/elasticity/membrane.cc
+20 -10
View File
@@ -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<Stencil2D>(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<Stencil2D>(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<Stencil2D>(qfrc, elements, metric, elongation, m, bodyid, xpos);
// update stored lengths
if (kD > 0) {
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) {