Move flex damping to the engine and remove membrane and solid plugins.
PiperOrigin-RevId: 676434954 Change-Id: I24e8dbaa90afcffd613a9cf106ef8b7262195328
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
6c7e1095c5
commit
4998e7b392
@@ -4154,6 +4154,12 @@ these mechanisms to be combined as desired.
|
||||
:el-prefix:`flex/` |-| **elasticity** (?)
|
||||
'''''''''''''''''''''''''''''''''''''''''
|
||||
|
||||
The elasticity model is a `Saint Venant-Kirchhoff
|
||||
<https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ model discretized with
|
||||
piecewise linear finite elements, 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.. See also :ref:`deformable <CDeformable>` objects.
|
||||
|
||||
.. _flex-elasticity-young:
|
||||
|
||||
:at:`young`: :at-val:`real(1), "0"`
|
||||
|
||||
@@ -2,6 +2,17 @@
|
||||
Changelog
|
||||
=========
|
||||
|
||||
Upcoming version (not yet released)
|
||||
-----------------------------------
|
||||
|
||||
General
|
||||
^^^^^^^
|
||||
|
||||
- Removed the :at:`solid` and :at:`membrane` plugins and moved the associated computations into the engine. See `3D
|
||||
example model <https://github.com/google-deepmind/mujoco/blob/main/model/flex/floppy.xml>`__ and `2D example model
|
||||
<https://github.com/google-deepmind/mujoco/blob/main/src/model/trampoline.xml>`__ for examples of flex objects
|
||||
that previously required these plugins.
|
||||
|
||||
Version 3.2.3 (Sep 16, 2024)
|
||||
----------------------------
|
||||
|
||||
|
||||
@@ -1172,6 +1172,7 @@ struct mjModel_ {
|
||||
mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1)
|
||||
mjtNum* flex_radius; // radius around primitive element (nflex x 1)
|
||||
mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21)
|
||||
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
|
||||
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
|
||||
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
|
||||
mjtByte* flex_edgeequality; // is edge equality constraint defined (nflex x 1)
|
||||
|
||||
@@ -274,12 +274,11 @@ There are several first-party plugin directories:
|
||||
`README <https://github.com/google-deepmind/mujoco/blob/main/plugin/actuator/README.md>`__ for details.
|
||||
* **elasticity:** The plugins in the `elasticity/
|
||||
<https://github.com/google-deepmind/mujoco/tree/main/plugin/elasticity>`__ directory are passive forces based on
|
||||
continuum mechanics for 1-dimensional and 3-dimensional bodies. The 1D model is invariant under rotations and captures
|
||||
the large deformation of elastic cables, decoupling twisting and bending strains. The 3D solid is a `Saint
|
||||
Venant-Kirchhoff <https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ model
|
||||
discretized with piecewise linear finite elements, which is suitable for large deformations with small strains. See
|
||||
also :ref:`composite <CComposite>` and :ref:`deformable <CDeformable>` objects. For more information, please see the
|
||||
`README <https://github.com/google-deepmind/mujoco/blob/main/plugin/elasticity/README.md>`__.
|
||||
continuum mechanics for 1-dimensional and 2-dimensional bodies. The 1D model is invariant under rotations and captures
|
||||
the large deformation of elastic cables, decoupling twisting and bending strains. The 2D model is a suitable for
|
||||
computing the bending stiffness of thin elastic plates (i.e. shells having a flat stress-free configuration). In this
|
||||
case, the elastic energy is quadratic and therefore the stiffness matrix is constant. For more information, please see
|
||||
the `README <https://github.com/google-deepmind/mujoco/blob/main/plugin/elasticity/README.md>`__.
|
||||
* **sensor:** The plugins in the `sensor/ <https://github.com/google-deepmind/mujoco/tree/main/plugin/sensor>`__
|
||||
directory implement custom sensors. Currently the sole sensor plugin is the touch grid sensor, see the
|
||||
`README <https://github.com/google-deepmind/mujoco/blob/main/plugin/sensor/README.md>`__ for details.
|
||||
|
||||
@@ -882,6 +882,7 @@ struct mjModel_ {
|
||||
mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1)
|
||||
mjtNum* flex_radius; // radius around primitive element (nflex x 1)
|
||||
mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21)
|
||||
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
|
||||
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
|
||||
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
|
||||
mjtByte* flex_edgeequality; // is edge equality constraint defined (nflex x 1)
|
||||
|
||||
@@ -351,6 +351,7 @@
|
||||
X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \
|
||||
XMJV( mjtNum, flex_radius, nflex, 1 ) \
|
||||
X ( mjtNum, flex_stiffness, nflexelem, 21 ) \
|
||||
X ( mjtNum, flex_damping, nflex, 1 ) \
|
||||
X ( mjtNum, flex_edgestiffness, nflex, 1 ) \
|
||||
X ( mjtNum, flex_edgedamping, nflex, 1 ) \
|
||||
X ( mjtByte, flex_edgeequality, nflex, 1 ) \
|
||||
|
||||
@@ -2475,6 +2475,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
doc='finite element stiffness matrix (nflexelem x 21)', # pylint: disable=line-too-long
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='flex_damping',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjtNum'),
|
||||
),
|
||||
doc="Rayleigh's damping coefficient (nflex x 1)",
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='flex_edgestiffness',
|
||||
type=PointerType(
|
||||
|
||||
@@ -16,10 +16,6 @@
|
||||
<mujoco model="Floppy">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</extension>
|
||||
|
||||
<compiler autolimits="true"/>
|
||||
|
||||
<option solver="CG" tolerance="1e-6" timestep=".001"/>
|
||||
@@ -35,7 +31,6 @@
|
||||
radius=".0" rgba="0 .7 .7 1" name="softbody" dim="3" mass="25">
|
||||
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
|
||||
<elasticity young="5e4" damping="0.002" poisson="0.2"/>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</flexcomp>
|
||||
|
||||
<body>
|
||||
@@ -16,10 +16,6 @@
|
||||
<mujoco model="Jelly">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</extension>
|
||||
|
||||
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
|
||||
|
||||
<size memory="10M"/>
|
||||
@@ -47,7 +43,6 @@
|
||||
<edge damping="1"/>
|
||||
<contact selfcollide="none"/>
|
||||
<elasticity young="5e4"/>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -16,10 +16,6 @@
|
||||
<mujoco model="Press">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</extension>
|
||||
|
||||
<compiler autolimits="true"/>
|
||||
|
||||
<option solver="Newton" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
|
||||
@@ -36,7 +32,6 @@
|
||||
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
|
||||
<edge damping="1"/>
|
||||
<elasticity young="1e4" poisson="0.4"/>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</flexcomp>
|
||||
|
||||
<flexcomp name="B" type="grid" count="4 4 4" spacing=".2 .2 .2" pos="1 0 .5"
|
||||
@@ -44,7 +39,6 @@
|
||||
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
|
||||
<edge damping="1"/>
|
||||
<elasticity young="5e4" poisson="0"/>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</flexcomp>
|
||||
|
||||
<body>
|
||||
@@ -0,0 +1,41 @@
|
||||
<!-- Copyright 2021 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.
|
||||
-->
|
||||
|
||||
<mujoco>
|
||||
<compiler meshdir="asset" texturedir="asset"/>
|
||||
|
||||
<statistic meansize=".05"/>
|
||||
|
||||
<visual>
|
||||
<rgba haze="0.15 0.25 0.35 1"/>
|
||||
<quality shadowsize="4096"/>
|
||||
<map stiffness="700" shadowscale="0.5" fogstart="1" fogend="15" zfar="40" haze="1"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
|
||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
|
||||
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
|
||||
|
||||
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="10 10" texuniform="true"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 2.0" dir="0 0 -1" castshadow="false"/>
|
||||
<light directional="true" diffuse=".8 .8 .8" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
|
||||
|
||||
<geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -16,11 +16,6 @@
|
||||
<mujoco model="Trampoline">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.shell"/>
|
||||
<plugin plugin="mujoco.elasticity.membrane"/>
|
||||
</extension>
|
||||
|
||||
<option timestep="0.001" solver="CG" tolerance="1e-6" integrator="implicitfast"/>
|
||||
|
||||
<size memory="10M"/>
|
||||
@@ -46,7 +41,6 @@
|
||||
<edge equality="false" damping="10"/>
|
||||
<elasticity young="3e5" poisson="0" thickness="1e-2"/>
|
||||
<pin id="0 15 240 255"/>
|
||||
<plugin plugin="mujoco.elasticity.membrane"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -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)
|
||||
|
||||
@@ -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.
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -549,7 +549,8 @@ void mj_flex(const mjModel* m, mjData* d) {
|
||||
// skip Jacobian if no built-in passive force is needed
|
||||
int skipjacobian = !m->flex_edgeequality[f] &&
|
||||
!m->flex_edgedamping[f] &&
|
||||
!m->flex_edgestiffness[f];
|
||||
!m->flex_edgestiffness[f] &&
|
||||
!m->flex_damping[f];
|
||||
|
||||
// process edges of this flex
|
||||
int vbase = m->flex_vertadr[f];
|
||||
|
||||
@@ -127,9 +127,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
const int* elem = m->flex_elem + m->flex_elemdataadr[f];
|
||||
const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[f];
|
||||
mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
|
||||
mjtNum* vel = d->flexedge_velocity + m->flex_edgeadr[f];
|
||||
mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f];
|
||||
mjtNum* ref = m->flexedge_length0 + m->flex_edgeadr[f];
|
||||
mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f];
|
||||
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
|
||||
mjtNum kD = m->flex_damping[f] / m->opt.timestep;
|
||||
|
||||
mj_markStack(d);
|
||||
mjtNum* qfrc = mj_stackAllocNum(d, 3*m->flex_vertnum[f]);
|
||||
@@ -143,11 +145,17 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
mjtNum gradient[6][2][3];
|
||||
GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge);
|
||||
|
||||
// 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
|
||||
|
||||
// extract elongation of edges belonging to this element
|
||||
mjtNum elongation[6];
|
||||
for (int e = 0; e < nedge; e++) {
|
||||
int idx = edgeelem[t * nedge + e];
|
||||
elongation[e] = deformed[idx]*deformed[idx] - ref[idx]*ref[idx];
|
||||
mjtNum previous = deformed[idx] - vel[idx] * m->opt.timestep;
|
||||
elongation[e] = deformed[idx]*deformed[idx] - reference[idx]*reference[idx] +
|
||||
(deformed[idx]*deformed[idx] - previous*previous) * kD;
|
||||
}
|
||||
|
||||
// unpack triangular representation
|
||||
|
||||
@@ -2510,6 +2510,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
} else {
|
||||
mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem);
|
||||
}
|
||||
m->flex_damping[i] = (mjtNum)pfl->damping;
|
||||
|
||||
// set fields: mesh-like
|
||||
m->flex_dim[i] = pfl->dim;
|
||||
|
||||
@@ -37,7 +37,7 @@ using ::testing::DoubleNear;
|
||||
using ::testing::HasSubstr;
|
||||
using ::testing::NotNull;
|
||||
|
||||
constexpr int kNumTruePlugins = 12;
|
||||
constexpr int kNumTruePlugins = 10;
|
||||
constexpr int kNumFakePlugins = 30;
|
||||
constexpr int kNumTestPlugins = 3;
|
||||
|
||||
|
||||
@@ -22,9 +22,7 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "test/fixture.h"
|
||||
#include "plugin/elasticity/membrane.h"
|
||||
#include "plugin/elasticity/shell.h"
|
||||
#include "plugin/elasticity/solid.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
@@ -35,17 +33,12 @@ using ElasticityTest = PluginTest;
|
||||
TEST_F(ElasticityTest, FlexCompatibility) {
|
||||
static constexpr char flex_xml[] = R"(
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</extension>
|
||||
|
||||
<worldbody>
|
||||
<body name="parent">
|
||||
<flexcomp name="soft" type="grid" count="3 3 3"
|
||||
radius="0.01" dim="3"mass="1">
|
||||
<pin id="2"/>
|
||||
<elasticity young="5e4" poisson="0.2"/>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
@@ -119,15 +112,10 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
|
||||
TEST_F(PluginTest, ElasticEnergyMembrane) {
|
||||
static constexpr char cantilever_xml[] = R"(
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.membrane"/>
|
||||
</extension>
|
||||
|
||||
<worldbody>
|
||||
<flexcomp type="grid" count="8 8 1" spacing="1 1 1"
|
||||
radius=".025" name="test" dim="2">
|
||||
<elasticity young="2" poisson="0" thickness="1"/>
|
||||
<plugin plugin="mujoco.elasticity.membrane"/>
|
||||
<edge equality="false"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
@@ -200,15 +188,10 @@ TEST_F(ElasticityTest, InvalidThickness) {
|
||||
TEST_F(ElasticityTest, ElasticEnergySolid) {
|
||||
static constexpr char cantilever_xml[] = R"(
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
</extension>
|
||||
|
||||
<worldbody>
|
||||
<flexcomp type="grid" count="8 8 8" spacing="1 1 1"
|
||||
radius=".025" name="test" dim="3">
|
||||
<elasticity young="2" poisson="0"/>
|
||||
<plugin plugin="mujoco.elasticity.solid"/>
|
||||
<edge equality="false"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
|
||||
@@ -5425,6 +5425,7 @@ public unsafe struct mjModel_ {
|
||||
public double* flexedge_invweight0;
|
||||
public double* flex_radius;
|
||||
public double* flex_stiffness;
|
||||
public double* flex_damping;
|
||||
public double* flex_edgestiffness;
|
||||
public double* flex_edgedamping;
|
||||
public byte* flex_edgeequality;
|
||||
|
||||
Reference in New Issue
Block a user