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

Move flex damping to the engine and remove membrane and solid plugins.

Browse Source 
PiperOrigin-RevId: 676434954
Change-Id: I24e8dbaa90afcffd613a9cf106ef8b7262195328
This commit is contained in:
Alessio Quaglino
2024-09-19 09:00:13 -07:00
committed by Copybara-Service
parent 6c7e1095c5
commit 4998e7b392
26 changed files with 88 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/elasticity/CMakeLists.txt
-4
View File
@@ -21,13 +21,9 @@ set(MUJOCO_ELASTICITY_SRCS
cable.h
elasticity.cc
elasticity.h
membrane.cc
membrane.h
register.cc
shell.cc
shell.h
solid.cc
solid.h
)
add_library(elasticity SHARED)
plugin/elasticity/README.md
-17
View File
@@ -30,20 +30,3 @@ Parameters:
- `young` [Pa]: Young's modulus.
- `poisson` [Pa]: Poisson's ratio; if 0, then the material only opposed shear deformations; if near 0.5, then the material is nearly incompressible (rubber-like).
- `thickness` [m]: shell thickness, used to scale the bending stiffness.
### Membrane
Implemented in [membrane.cc](membrane.cc).
The membrane plugin discretized an extensible 2D continuum. It is intended to simulate the stretching of membranes subjected to tensile stresses, where the bending is negligible.
Parameters: see [flex parameters](mujoco.readthedocs.io/en/latest/XMLreference.html#flexcomp-elasticity) in the XML Reference docs.
### Solid
Implemented in [solid.cc](solid.cc).
The membrane plugin discretized an extensible 3D continuum. It is Saint Venant–Kirchhoff model intended to simulate the compression or elongation of hyperelastic materials subjected to large displacements (finite rotations) and small strains, since it uses a nonlinear strain-displacement but a linear stress-strain relationship.
Parameters: see [flex parameters](mujoco.readthedocs.io/en/latest/XMLreference.html#flexcomp-elasticity) in the XML Reference docs.
plugin/elasticity/elasticity.h
-101
View File
@@ -58,107 +58,6 @@ struct Stencil3D {
int edges[kNumEdges];
};
// gradients of edge lengths with respect to vertex positions
template <typename T>
void inline GradSquaredLengths(mjtNum gradient[T::kNumEdges][2][3],
const mjtNum* x,
const int v[T::kNumVerts]) {
for (int e = 0; e < T::kNumEdges; e++) {
for (int d = 0; d < 3; d++) {
gradient[e][0][d] = x[3*v[T::edge[e][0]]+d] - x[3*v[T::edge[e][1]]+d];
gradient[e][1][d] = x[3*v[T::edge[e][1]]+d] - x[3*v[T::edge[e][0]]+d];
}
}
}
template <typename T>
inline void ComputeForce(std::vector<mjtNum>& qfrc_passive,
const std::vector<mjtNum>& elongationglob,
const mjModel* m, int flex,
const mjtNum* xpos) {
mju_zero(qfrc_passive.data(), qfrc_passive.size());
mjtNum* k = m->flex_stiffness + 21 * m->flex_elemadr[flex];
int dim = m->flex_dim[flex];
const int* elem = m->flex_elem + m->flex_elemdataadr[flex];
const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[flex];
// compute force element-by-element
for (int t = 0; t < m->flex_elemnum[flex]; t++) {
const int* v = elem + (dim+1) * t;
// compute length gradient with respect to dofs
mjtNum gradient[T::kNumEdges][2][3];
GradSquaredLengths<T>(gradient, xpos, v);
// extract elongation of edges belonging to this element
mjtNum elongation[T::kNumEdges];
for (int e = 0; e < T::kNumEdges; e++) {
int idx = edgeelem[t * T::kNumEdges + e];
elongation[e] = elongationglob[idx];
}
// unpack triangular representation
mjtNum metric[T::kNumEdges*T::kNumEdges];
int id = 0;
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = ed1; ed2 < T::kNumEdges; ed2++) {
metric[T::kNumEdges*ed1 + ed2] = k[21*t + id];
metric[T::kNumEdges*ed2 + ed1] = k[21*t + id++];
}
}
// we now multiply the elongations by the precomputed metric tensor,
// notice that if metric=diag(1/reference) then this would yield a
// mass-spring model
// compute local force
mjtNum force[T::kNumVerts*3] = {0};
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
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] -=
elongation[ed1] * gradient[ed2][i][x] *
metric[T::kNumEdges * ed1 + ed2];
}
}
}
}
// 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];
}
}
}
}
// 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[body_dofadr+x] += force[3*v+x];
}
}
}
}
// copied from mjXUtil
void String2Vector(const std::string& txt, std::vector<int>& vec);
plugin/elasticity/membrane.cc
-158
View File
@@ -1,158 +0,0 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <optional>
#include <utility>
#include <vector>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "elasticity.h"
#include "membrane.h"
namespace mujoco::plugin::elasticity {
// factory function
std::optional<Membrane> Membrane::Create(const mjModel* m, mjData* d,
int instance) {
if (CheckAttr("face", m, instance)) {
mjtNum damp =
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
return Membrane(m, d, instance, damp);
} else {
mju_warning("Invalid parameter specification in shell plugin");
return std::nullopt;
}
}
// plugin constructor
Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum damp)
: f0(-1), damping(damp) {
// count plugin bodies
nv = ne = 0;
for (int i = 1; i < m->nbody; i++) {
if (m->body_plugin[i] == instance) {
if (!nv++) {
i0 = i;
}
}
}
// count flexes
for (int i = 0; i < m->nflex; i++) {
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];
}
}
}
// loop over all triangles
const int* elem = m->flex_elem + m->flex_elemdataadr[f0];
for (int t = 0; t < m->flex_elemnum[f0]; t++) {
const int* v = elem + (m->flex_dim[f0]+1) * t;
for (int i = 0; i < Stencil2D::kNumVerts; i++) {
int bi = 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);
}
}
}
// allocate array
ne = m->flex_edgenum[f0];
elongation.assign(ne, 0);
force.assign(3*nv, 0);
}
void Membrane::Compute(const mjModel* m, mjData* d, int instance) {
mjtNum kD = damping / m->opt.timestep;
// read edge lengths
mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f0];
mjtNum* ref = m->flexedge_length0 + m->flex_edgeadr[f0];
// m->flexedge_length0 is not initialized when the plugin is constructed
if (prev.empty()) {
prev.assign(ne, 0);
memcpy(prev.data(), ref, sizeof(mjtNum) * ne);
}
// we add generalized Rayleigh damping as decribed in Section 5.2 of
// Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
for (int idx = 0; idx < ne; idx++) {
elongation[idx] =
(deformed[idx] * deformed[idx] - prev[idx] * prev[idx]) * kD;
}
// compute gradient of elastic energy and insert into passive force
int flex_vertadr = m->flex_vertadr[f0];
mjtNum* xpos = d->flexvert_xpos + 3*flex_vertadr;
mjtNum* qfrc = d->qfrc_passive;
ComputeForce<Stencil2D>(force, elongation, m, f0, xpos);
// insert into passive force
AddFlexForce(qfrc, force, m, d, xpos, f0);
// update stored lengths
if (kD > 0) {
memcpy(prev.data(), deformed, sizeof(mjtNum) * ne);
}
}
void Membrane::RegisterPlugin() {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.elasticity.membrane";
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"face", "edge", "damping"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
auto elasticity_or_null = Membrane::Create(m, d, instance);
if (!elasticity_or_null.has_value()) {
return -1;
}
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
new Membrane(std::move(*elasticity_or_null)));
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<Membrane*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
auto* elasticity = reinterpret_cast<Membrane*>(d->plugin_data[instance]);
elasticity->Compute(m, d, instance);
};
mjp_registerPlugin(&plugin);
}
} // namespace mujoco::plugin::elasticity
plugin/elasticity/membrane.h
-62
View File
@@ -1,62 +0,0 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_PLUGIN_ELASTICITY_MEMBRANE_H_
#define MUJOCO_PLUGIN_ELASTICITY_MEMBRANE_H_
#include <optional>
#include <utility>
#include <vector>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include "elasticity.h"
namespace mujoco::plugin::elasticity {
class Membrane {
public:
// Returns a new Membrane instance or nullopt on failure.
static std::optional<Membrane> Create(const mjModel* m, mjData* d,
int instance);
Membrane(Membrane&&) = default;
Membrane& operator=(Membrane&& other) = default;
void Compute(const mjModel* m, mjData* d, int instance);
static void RegisterPlugin();
int f0; // index of corresponding flex
int i0; // index of first body
int nc; // number of quads in the grid
int nv; // number of vertices (bodies) in the Membrane
int ne; // number of edges in the Membrane
// precomputed quantities
std::vector<mjtNum> prev; // 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;
private:
Membrane(const mjModel* m, mjData* d, int instance, mjtNum damp);
};
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_PLUGIN_ELASTICITY_MEMBRANE_H_
plugin/elasticity/register.cc
-4
View File
@@ -15,16 +15,12 @@
#include <mujoco/mjplugin.h>
#include "cable.h"
#include "shell.h"
#include "membrane.h"
#include "solid.h"
namespace mujoco::plugin::elasticity {
mjPLUGIN_LIB_INIT {
Cable::RegisterPlugin();
Membrane::RegisterPlugin();
Shell::RegisterPlugin();
Solid::RegisterPlugin();
}
} // namespace mujoco::plugin::elasticity
plugin/elasticity/solid.cc
-163
View File
@@ -1,163 +0,0 @@
// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cassert>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <optional>
#include <utility>
#include <vector>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "elasticity.h"
#include "solid.h"
namespace mujoco::plugin::elasticity {
// factory function
std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("face", m, instance) &&
CheckAttr("edge", m, instance)) {
mjtNum damp =
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
return Solid(m, d, instance, damp);
} else {
mju_warning("Invalid parameter specification in solid plugin");
return std::nullopt;
}
}
// plugin constructor
Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum damp)
: f0(-1), damping(damp) {
// count plugin bodies
nv = ne = 0;
for (int i = 1; i < m->nbody; i++) {
if (m->body_plugin[i] == instance) {
if (!nv++) {
i0 = i;
}
}
}
// count flexes
for (int i = 0; i < m->nflex; i++) {
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];
if (m->flex_dim[i] != 3) { // SHOULD NOT OCCUR
mju_error("mujoco.elasticity.solid requires a 3D mesh");
}
}
}
}
// loop over all tetrahedra
const int* elem = m->flex_elem + m->flex_elemdataadr[f0];
for (int t = 0; t < m->flex_elemnum[f0]; t++) {
const int* v = elem + (m->flex_dim[f0]+1) * t;
for (int i = 0; i < Stencil3D::kNumVerts; i++) {
int bi = 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);
}
}
}
// allocate array
ne = m->flex_edgenum[f0];
elongation.assign(ne, 0);
force.assign(3*nv, 0);
}
void Solid::Compute(const mjModel* m, mjData* d, int instance) {
mjtNum kD = damping / m->opt.timestep;
// read edge lengths
mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f0];
mjtNum* ref = m->flexedge_length0 + m->flex_edgeadr[f0];
// m->flexedge_length0 is not initialized when the plugin is constructed
if (prev.empty()) {
prev.assign(ne, 0);
memcpy(prev.data(), ref, sizeof(mjtNum) * ne);
}
// we add generalized Rayleigh damping as decribed in Section 5.2 of
// Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
for (int idx = 0; idx < ne; idx++) {
elongation[idx] =
(deformed[idx] * deformed[idx] - prev[idx] * prev[idx]) * kD;
}
// compute gradient of elastic energy and insert into passive force
int flex_vertadr = m->flex_vertadr[f0];
mjtNum* xpos = d->flexvert_xpos + 3*flex_vertadr;
mjtNum* qfrc = d->qfrc_passive;
ComputeForce<Stencil3D>(force, elongation, m, f0, xpos);
// insert into passive force
AddFlexForce(qfrc, force, m, d, xpos, f0);
// update stored lengths
if (kD > 0) {
memcpy(prev.data(), deformed, sizeof(mjtNum) * ne);
}
}
void Solid::RegisterPlugin() {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.elasticity.solid";
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"face", "edge", "damping"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
auto elasticity_or_null = Solid::Create(m, d, instance);
if (!elasticity_or_null.has_value()) {
return -1;
}
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
new Solid(std::move(*elasticity_or_null)));
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<Solid*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.compute =
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
auto* elasticity = reinterpret_cast<Solid*>(d->plugin_data[instance]);
elasticity->Compute(m, d, instance);
};
mjp_registerPlugin(&plugin);
}
} // namespace mujoco::plugin::elasticity
plugin/elasticity/solid.h
-60
View File
@@ -1,60 +0,0 @@
// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_PLUGIN_ELASTICITY_SOLID_H_
#define MUJOCO_PLUGIN_ELASTICITY_SOLID_H_
#include <optional>
#include <vector>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include "elasticity.h"
namespace mujoco::plugin::elasticity {
class Solid {
public:
// Returns a new Solid instance or nullopt on failure.
static std::optional<Solid> Create(const mjModel* m, mjData* d, int instance);
Solid(Solid&&) = default;
Solid& operator=(Solid&& other) = default;
void Compute(const mjModel* m, mjData* d, int instance);
static void RegisterPlugin();
int f0; // index of corresponding flex
int i0; // index of first body
int nc; // number of cubes in the grid
int nv; // number of vertices (bodies) in the solid
int ne; // number of edges in the solid
// precomputed quantities
std::vector<mjtNum> prev; // 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;
private:
Solid(const mjModel* m, mjData* d, int instance, mjtNum damp);
};
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_PLUGIN_ELASTICITY_SOLID_H_