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

Deformable solid plugin for particles.

Browse Source 
Using coordinate-free formula equivalent to linear finite elements.

PiperOrigin-RevId: 491317615
Change-Id: I096b9a56075bbb630f64bfffac8a8d66457e5e2d
This commit is contained in:
Alessio Quaglino
2022-11-28 05:31:41 -08:00
committed by Copybara-Service
parent 062cb53a4a
commit d0b1a97338
9 changed files with 730 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+13
View File
@@ -50,6 +50,19 @@ General
<https://github.com/deepmind/mujoco/tree/main/test/benchmark/engine_core_smooth_benchmark_test.cc>`_.
- 50% faster ``mju_dotSparse`` using manual loop unroll. See `engine_util_sparse_benchmark_test
<https://github.com/deepmind/mujoco/tree/main/test/benchmark/engine_util_sparse_benchmark_test.cc>`_.
- Added new :at:`solid` passive force plugin:
.. youtube:: AGcTGHbbze4
:align: right
:height: 150px
- This is new force field compatible with the :ref:`composite<body-composite>` particles.
- Generates a tetrahedral mesh having particles with mass concentrated at vertices.
- Uses a piecewise-constant strain model equivalent to finite elements but expressed in a coordinate-free
formulation. This implies that all quantities can be precomputed except edge elongation, as in a mass-spring model.
- Only suitable for small strains (large displacements but small deformations). Tetrahedra may invert if subject to
large loads.
- Add API functions ``mj_loadPluginLibrary`` and ``mj_loadAllPluginLibraries``. The first function is identical to
``dlopen`` on a POSIX system, and to ``LoadLibraryA`` on Windows. The second function scans a specified directory for
all dynamic libraries file and loads each library found. Dynamic libraries opened by these functions are assumed to
model/plugin/floppy.xml
+55
View File
@@ -0,0 +1,55 @@
<!-- 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 model="Floppy">
<include file="scene.xml"/>
<extension>
<required plugin="mujoco.elasticity.solid"/>
</extension>
<compiler autolimits="true"/>
<option solver="Newton" tolerance="1e-6" timestep=".001"/>
<size nconmax="6000" njmax="6000" nstack="50000000"/>
<visual>
<map stiffness="100"/>
</visual>
<worldbody>
<composite type="particle" count="24 4 4" spacing="0.1" offset=".1 0 1.5">
<geom size=".025" rgba=".8 .2 .1 1" group="4"
condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<skin inflate="0" rgba="0 .7 .7 .6"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="poisson" value="0.2"/>
<config key="damping" value="0.002"/>
<!--Units are in Pa (SI)-->
<config key="young" value="5e4"/>
</plugin>
</composite>
<body>
<joint name="hinge" pos="0 0 .5" axis="0 1 0" damping="50"/>
<geom type="cylinder" size=".4" fromto="0 -.5 .5 0 .5 .5" density="300"/>
</body>
</worldbody>
<actuator>
<motor name="cylinder" joint="hinge" gear="1 0 0 0 0 0" ctrlrange="-100 100"/>
</actuator>
</mujoco>
model/plugin/jelly.xml
+56
View File
@@ -0,0 +1,56 @@
<!-- 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 model="Jelly">
<include file="scene.xml"/>
<extension>
<required plugin="mujoco.elasticity.solid"/>
</extension>
<option solver="CG" tolerance="1e-6" timestep=".001"/>
<size nconmax="6000" njmax="6000" nstack="50000000"/>
<visual>
<map stiffness="100"/>
</visual>
<default>
<default class="wall">
<geom type="plane" size=".5 .5 .05"/>
</default>
</default>
<worldbody>
<body mocap="true" pos="-.1 .05 0" zaxis=".5 0 1">
<geom type="capsule" size=".1 .1" group="1" condim="1"/>
</body>
<geom name="+x" class="wall" zaxis="1 0 0" pos="-.5 0 -.25"/>
<geom name="-x" class="wall" zaxis="-1 0 0" pos=".5 0 -.25"/>
<geom name="+y" class="wall" zaxis="0 1 0" pos="0 -.5 -.25"/>
<geom name="-y" class="wall" zaxis="0 -1 0" pos="0 .5 -.25"/>
<composite type="particle" count="8 8 8" spacing="0.07" offset="0 0 1">
<geom size=".025" rgba=".8 .2 .1 1" group="4"/>
<skin inflate="0" rgba="0 .7 .7 .6"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="poisson" value="0"/>
<config key="damping" value="0.001"/>
<!--Units are in Pa (SI)-->
<config key="young" value="5e4"/>
</plugin>
</composite>
</worldbody>
</mujoco>
model/plugin/press.xml
+72
View File
@@ -0,0 +1,72 @@
<!-- 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 model="Press">
<include file="scene.xml"/>
<extension>
<required plugin="mujoco.elasticity.solid"/>
</extension>
<compiler autolimits="true"/>
<option solver="Newton" tolerance="1e-6" timestep=".001"/>
<size nconmax="6000" njmax="6000" nstack="50000000"/>
<visual>
<map stiffness="100"/>
</visual>
<worldbody>
<composite prefix="A" type="particle" count="4 4 4" spacing="0.2" offset="0 0 .5">
<geom size=".025" rgba=".8 .2 .1 1" group="4"
condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<skin inflate="0" rgba="0 .7 .7 .6"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="poisson" value="0.4"/>
<config key="damping" value="0.001"/>
<!--Units are in Pa (SI)-->
<config key="young" value="1e4"/>
</plugin>
</composite>
<composite prefix="B" type="particle" count="4 4 4" spacing="0.2" offset="1 0 .5">
<geom size=".025" rgba=".8 .2 .1 1" group="4"
condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<skin inflate="0" rgba="0 .7 .7 .6"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="poisson" value="0"/>
<config key="damping" value="0.001"/>
<!--Units are in Pa (SI)-->
<config key="young" value="5e4"/>
</plugin>
</composite>
<body>
<joint name="soft" type="slide" axis="0 0 1" damping="500"/>
<geom type="box" size=".35 .35 0.05" pos=".1 .1 .8" density="300"/>
</body>
<body>
<joint name="stiff" type="slide" axis="0 0 1" damping="500"/>
<geom type="box" size=".35 .35 0.05" pos="1.1 .1 .8" density="300"/>
</body>
</worldbody>
<actuator>
<motor name="soft" joint="soft" gear="-1 0 0 0 0 0" ctrlrange="-150 300"/>
<motor name="stiff" joint="stiff" gear="-1 0 0 0 0 0" ctrlrange="-150 1600"/>
</actuator>
</mujoco>
plugin/elasticity/CMakeLists.txt
+2
View File
@@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_INCLUDE
set(MUJOCO_ELASTICITY_SRCS
cable.cc
cable.h
solid.cc
solid.h
)
add_library(elasticity SHARED)
plugin/elasticity/solid.cc
+378
View File
@@ -0,0 +1,378 @@
// 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 <algorithm>
#include <cstddef>
#include <cstdio>
#include <sstream>
#include <optional>
#include <unordered_map>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "solid.h"
namespace mujoco::plugin::elasticity {
namespace {
// local tetrahedron numbering
constexpr int edge[6][2] = {{0, 1}, {1, 2}, {2, 0}, {2, 3}, {0, 3}, {1, 3}};
constexpr int face[4][3] = {{2, 1, 0}, {0, 1, 3}, {1, 2, 3}, {2, 0, 3}};
constexpr int e2f[6][2] = {{2, 3}, {1, 3}, {2, 1}, {1, 0}, {0, 2}, {0, 3}};
constexpr int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7}, {1, 3, 2, 7},
{1, 2, 6, 7}, {1, 5, 4, 7}, {1, 6, 5, 7}};
// Cartesian distance between 3D vectors
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// volume of a tetrahedron
mjtNum ComputeVolume(const mjtNum* x, const int v[4]) {
mjtNum normal[3];
mjtNum edge1[3];
mjtNum edge2[3];
mjtNum edge3[3];
mju_sub3(edge1, x+3*v[1], x+3*v[0]);
mju_sub3(edge2, x+3*v[2], x+3*v[0]);
mju_sub3(edge3, x+3*v[3], x+3*v[0]);
mju_cross(normal, edge2, edge1);
return mju_dot3(normal, edge3) / 6;
}
// compute local basis
void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[4],
const int faceL[3], const int faceR[3], mjtNum volume) {
mjtNum normalL[3], normalR[3];
mjtNum edgesL[6], edgesR[6];
mju_sub3(edgesL+0, x+3*v[faceL[1]], x+3*v[faceL[0]]);
mju_sub3(edgesL+3, x+3*v[faceL[2]], x+3*v[faceL[0]]);
mju_sub3(edgesR+0, x+3*v[faceR[1]], x+3*v[faceR[0]]);
mju_sub3(edgesR+3, x+3*v[faceR[2]], x+3*v[faceR[0]]);
mju_cross(normalL, edgesL, edgesL+3);
mju_cross(normalR, edgesR, edgesR+3);
// we use as basis the symmetrized tensor products of the area normals of the
// two faces not adjacent to the edge; this is the 3D equivalent to the basis
// proposed in Weischedel "A discrete geometric view on shear-deformable shell
// models" in the remark at the end of section 4.1. This is also equivalent to
// linear finite elements but in a coordinate-free formulation.
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
basis[3*i+j] = ( normalL[i]*normalR[j] +
normalR[i]*normalL[j] ) / (36*2*volume*volume);
}
}
}
// update edge lengths
void UpdateSquaredLengths(std::vector<mjtNum>& len,
const std::vector<std::pair<int, int> >& edges,
const mjtNum* x) {
for (int e = 0; e < len.size(); e++) {
const mjtNum* p0 = x + 3*edges[e].first;
const mjtNum* p1 = x + 3*edges[e].second;
len[e] = SquaredDist3(p0, p1);
}
}
// gradients of edge lengths with respect to vertex positions
void GradSquaredLengths(mjtNum gradient[6][2][3],
const mjtNum* x,
const int v[4],
const int edge[6][2]) {
for (int e = 0; e < 6; e++) {
for (int d = 0; d < 3; d++) {
gradient[e][0][d] = x[3*v[edge[e][0]]+d] - x[3*v[edge[e][1]]+d];
gradient[e][1][d] = x[3*v[edge[e][1]]+d] - x[3*v[edge[e][0]]+d];
}
}
}
// reads numeric attributes
bool CheckAttr(const char* name, const mjModel* m, int instance) {
char* end;
std::string value = mj_getPluginConfig(m, instance, name);
value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
strtod(value.c_str(), &end);
return end == value.data() + value.size();
}
struct PairHash
{
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
} // namespace
// factory function
std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("nx", m, instance) &&
CheckAttr("ny", m, instance) &&
CheckAttr("nz", m, instance) &&
CheckAttr("poisson", m, instance) &&
CheckAttr("young", m, instance)) {
int nx = strtod(mj_getPluginConfig(m, instance, "nx"), nullptr);
int ny = strtod(mj_getPluginConfig(m, instance, "ny"), nullptr);
int nz = strtod(mj_getPluginConfig(m, instance, "nz"), nullptr);
mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
mjtNum damp =
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
return Solid(m, d, instance, nx, ny, nz, nu, E, damp);
} else {
mju_warning("Invalid parameter specification in solid plugin");
return std::nullopt;
}
}
// create map from tetrahedra to vertices and edges and from edges to vertices
void Solid::CreateStencils(int nx, int ny, int nz) {
tetrahedra.resize(nt);
// create a tetrahedral mesh by splitting a grid of hexahedral cells
for (int ix = 0; ix < nx-1; ix++) {
for (int iy = 0; iy < ny-1; iy++) {
for (int iz = 0; iz < nz-1; iz++) {
int t = 6*(nz-1)*(ny-1)*ix + 6*(nz-1)*iy + 6*iz;
int vert[8] = {
nz*ny*(ix+0) + nz*(iy+0) + iz+0,
nz*ny*(ix+1) + nz*(iy+0) + iz+0,
nz*ny*(ix+1) + nz*(iy+1) + iz+0,
nz*ny*(ix+0) + nz*(iy+1) + iz+0,
nz*ny*(ix+0) + nz*(iy+0) + iz+1,
nz*ny*(ix+1) + nz*(iy+0) + iz+1,
nz*ny*(ix+1) + nz*(iy+1) + iz+1,
nz*ny*(ix+0) + nz*(iy+1) + iz+1,
};
for (int s = 0; s < 6; s++) {
for (int v = 0; v < 4; v++) {
tetrahedra[t+s].vertices[v] = vert[cube2tets[s][v]];
}
}
}
}
}
// map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// loop over all tetrahedra
for (int t = 0; t < nt; t++) {
int* v = tetrahedra[t].vertices;
// compute edges to vertices map for fast computations
for (int e = 0; e < 6; e++) {
auto pair = std::pair(
std::min(v[edge[e][0]], v[edge[e][1]]),
std::max(v[edge[e][0]], v[edge[e][1]])
);
// if edge is already present in the vector only store its index
auto [it, inserted] = edge_indices.insert({pair, ne});
if (inserted) {
edges.push_back(pair);
tetrahedra[t].edges[e] = ne++;
} else {
tetrahedra[t].edges[e] = it->second;
}
}
}
}
// plugin constructor
Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
mjtNum nu, mjtNum E, mjtNum damp): 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;
}
}
}
// allocate arrays
nc = (nx-1)*(ny-1)*(nz-1); // number of cubes
nt = 6*nc; // number of tets
metric.assign(36*nt, 0); // metric induced by the geometry
// generate tetrahedra from the vertices
CreateStencils(nx, ny, nz);
// loop over all tetrahedra
for (int t = 0; t < nt; t++) {
int* v = tetrahedra[t].vertices;
for (int i = 0; i < 4; i++) {
if (m->body_plugin[i0+v[i]] != instance) {
mju_error("This body does not have the requested plugin instance");
}
}
// tetrahedron volume
mjtNum volume = ComputeVolume(m->body_pos+3*i0, v);
// local geometric quantities
mjtNum basis[6][9] = {{0}, {0}, {0}, {0}, {0}, {0}};
mjtNum trT[6] = {0};
mjtNum trTT[36] = {0};
// compute edge basis
for (int e = 0; e < 6; e++) {
ComputeBasis(basis[e], m->body_pos+3*i0, v,
face[e2f[e][0]], face[e2f[e][1]], volume);
}
// compute first invariant i.e. trace(strain)
for (int e = 0; e < 6; e++) {
for (int i = 0; i < 3; i++) {
trT[e] += basis[e][4*i];
}
}
// compute second invariant i.e. trace(strain^2)
for (int ed1 = 0; ed1 < 6; ed1++) {
for (int ed2 = 0; ed2 < 6; ed2++) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
trTT[6*ed1+ed2] += basis[ed1][3*i+j] * basis[ed2][3*j+i];
}
}
}
}
// material parameters
mjtNum mu = E / (2*(1+nu)) * volume;
mjtNum la = E*nu / ((1+nu)*(1-2*nu)) * volume;
// assembly of strain metric tensor
for (int ed1 = 0; ed1 < 6; ed1++) {
for (int ed2 = 0; ed2 < 6; ed2++) {
metric[36*t+6*ed1+ed2] = mu * trTT[6*ed1+ed2] + la * trT[ed2]*trT[ed1];
}
}
}
// allocate array
reference.assign(ne, 0);
deformed.assign(ne, 0);
previous.assign(ne, 0);
// compute edge lengths at equilibrium
UpdateSquaredLengths(reference, edges, m->body_pos+3*i0);
previous = reference;
}
void Solid::Compute(const mjModel* m, mjData* d, int instance) {
UpdateSquaredLengths(deformed, edges, d->xpos+3*i0);
// loop over all elements
for (int t = 0; t < nt; t++) {
int* v = tetrahedra[t].vertices;
// compute length gradient with respect to dofs
mjtNum gradient[6][2][3];
GradSquaredLengths(gradient, d->xpos+3*i0, v, edge);
// 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
// compute elongation
mjtNum elongation[6];
mjtNum kD = damping / m->opt.timestep;
for (int e = 0; e < 6; e++) {
int idx = tetrahedra[t].edges[e];
elongation[e] = deformed[idx] - reference[idx] +
( deformed[idx] - previous[idx] ) * kD;
}
// 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[12] = {0};
for (int ed1 = 0; ed1 < 6; ed1++) {
for (int ed2 = 0; ed2 < 6; ed2++) {
for (int i = 0; i < 2; i++) {
for (int x = 0; x < 3; x++) {
force[3*edge[ed2][i]+x] +=
elongation[ed1] * gradient[ed2][i][x] * metric[36*t+6*ed1+ed2];
}
}
}
}
// insert into global force
for (int i = 0; i < 4; i++) {
for (int x = 0; x < 3; x++) {
d->qfrc_passive[m->body_dofadr[i0]+3*v[i]+x] -= force[3*i+x];
}
}
}
// update stored lengths
previous = deformed;
}
mjPLUGIN_DYNAMIC_LIBRARY_INIT {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.elasticity.solid";
plugin.type |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"nx", "ny", "nz", "young", "poisson", "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 type) {
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
+71
View File
@@ -0,0 +1,71 @@
// 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>
namespace mujoco::plugin::elasticity {
struct Stencil {
int vertices[4];
int edges[6];
};
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);
int i0; // index of first body
int nc; // number of cubes in the grid
int nv; // number of vertices (bodies) in the solid
int nt; // number of volumetric elements (tetrahedra)
int ne; // number of edges in the solid
// connectivity info for mapping tetrahedra to edges and vertices
std::vector<Stencil> tetrahedra; // 4 vertices and 6 edges (nt x 10)
std::vector<std::pair<int, int> > edges; // edge to vertex map (ne x 2)
// precomputed quantities
std::vector<mjtNum> metric; // geom-induced metric (nt x 36)
std::vector<mjtNum> reference; // reference lengths (ne x 1)
std::vector<mjtNum> deformed; // deformed lengths (ne x 1)
std::vector<mjtNum> previous; // previous-step lengths (ne x 1)
mjtNum damping;
private:
Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
mjtNum nu, mjtNum E, mjtNum damp);
void CreateStencils(int nx, int ny, int nz);
};
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_PLUGIN_ELASTICITY_SOLID_H_
src/user/user_composite.cc
+26 -1
View File
@@ -59,6 +59,7 @@ mjCComposite::mjCComposite(void) {
pin.clear();
flatinertia = 0;
mj_defaultSolRefImp(solrefsmooth, solimpsmooth);
plugin_instance = nullptr;
// cable
curve[0] = curve[1] = curve[2] = mjCOMPSHAPE_ZERO;
@@ -315,7 +316,10 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
// create body
char txt[100];
mjCBody* b = body->AddBody(NULL);
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
b->name = txt;
// set body position
b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]);
@@ -345,10 +349,31 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
// add geom
mjCGeom* g = b->AddGeom(def);
g->def = body->def;
g->type = mjGEOM_SPHERE;
// add plugin
if (plugin_instance) {
b->is_plugin = true;
b->plugin_name = plugin_name;
b->plugin_instance = plugin_instance;
b->plugin_instance_name = plugin_instance_name;
// propagate attributes
if (plugin_name == "mujoco.elasticity.solid") {
b->plugin_instance->config_attribs["nx"] = std::to_string(count[0]);
b->plugin_instance->config_attribs["ny"] = std::to_string(count[1]);
b->plugin_instance->config_attribs["nz"] = std::to_string(count[2]);
}
}
}
}
}
// skin
if (skin) {
MakeSkin3(model);
}
return true;
}
@@ -2014,7 +2039,7 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
skin->group = skingroup;
// box
if (type==mjCOMPTYPE_BOX) {
if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
// z-faces
MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
fmt = "%sB%d_%d_" + string(cnt2);
test/plugin/elasticity/elasticity_test.cc
+57
View File
@@ -24,6 +24,7 @@
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
#include "plugin/elasticity/solid.h"
namespace mujoco {
namespace {
@@ -50,6 +51,62 @@ class PluginTest : public MujocoTest {
}
};
// -------------------------------- solid -----------------------------------
TEST_F(PluginTest, ElasticEnergy) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<extension>
<required plugin="mujoco.elasticity.solid"/>
</extension>
<worldbody>
<composite type="particle" count="8 8 8" spacing="1">
<geom size=".025" group="4"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="nx" value="8"/>
<config key="ny" value="8"/>
<config key="nz" value="8"/>
<config key="poisson" value="0"/>
<config key="young" value="2"/>
</plugin>
</composite>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
EXPECT_THAT(mjp_pluginCount(), 2);
auto* solid = reinterpret_cast<plugin::elasticity::Solid*>(d->plugin_data[0]);
// check that if the entire geometry is rescaled by a factor "scale", then
// trace(strain^2) = 3*scale^2
for (mjtNum scale = 1; scale < 4; scale++) {
for (int t = 0; t < solid->nt; t++) {
mjtNum energy = 0;
mjtNum volume = 1./6.;
for (int e1 = 0; e1 < 6; e1++) {
for (int e2 = 0; e2 < 6; e2++) {
int idx1 = solid->tetrahedra[t].edges[e1];
int idx2 = solid->tetrahedra[t].edges[e2];
mjtNum elongation1 = scale*solid->reference[idx1];
mjtNum elongation2 = scale*solid->reference[idx2];
energy += solid->metric[36*t+6*e2+e1] * elongation1 * elongation2;
}
}
EXPECT_NEAR(
energy/volume, 3*scale*scale, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- cable -----------------------------------
TEST_F(PluginTest, CantileverIntoCircle) {