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

Add shell plugin.

Browse Source 
- Add bending passive force
- Add two examples (plate and poncho)
- Add stretching constraints for particles using tendons on mesh edges

PiperOrigin-RevId: 566980584
Change-Id: Id0cd8e1084c05560f1008518a8afbd5311d563e2
This commit is contained in:
Alessio Quaglino
2023-09-20 08:59:26 -07:00
committed by Copybara-Service
parent c54aba15f9
commit dd5a7115cf
12 changed files with 2171 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+1 -1
View File
@@ -3346,7 +3346,7 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui
.. _body-composite-vertex:
:at:`vertex`: :at-val:`real(3*nvert), optional`
Vertex 3D positions in global coordinates (cable only).
Vertex 3D positions in global coordinates (cable and shell).
.. _body-composite-face:
doc/changelog.rst
+34 -23
View File
@@ -31,27 +31,38 @@ New features
3. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within the MuJoCo engine
pipeline.
.. youtube:: ra2bTiZHGlw
:align: right
:width: 240px
4. Added capability to initialize :ref:`composite<body-composite>` particles with arbitrary positions.
5. Added `shell <https://github.com/deepmind/mujoco/blob/main/plugin/elasticity/shell.cc>`_ passive force plugin:
- Collisions use spheres located at mesh vertices.
- Stretching as tendon constraints and bending using a constant precomputed Hessian (cotangent operator).
General
^^^^^^^
.. admonition:: Breaking API changes
:class: attention
4. Removed the macros ``mjMARKSTACK`` and ``mjFREESTACK``.
6. Removed the macros ``mjMARKSTACK`` and ``mjFREESTACK``.
**Migration:** These macros have been replaced by new functions :ref:`mj_markStack` and
:ref:`mj_freeStack`. These functions manage the :ref:`mjData stack<siStack>` in a fully encapsulated way (i.e.,
without introducing a local variable at the call site).
5. Renamed ``mj_stackAlloc`` to :ref:`mj_stackAllocNum`. The new function :ref:`mj_stackAllocByte` allocates an
7. Renamed ``mj_stackAlloc`` to :ref:`mj_stackAllocNum`. The new function :ref:`mj_stackAllocByte` allocates an
arbitrary number of bytes and has an additional argument for specifying the alignment of the returned pointer.
**Migration:** The functionality for allocating ``mjtNum`` arrays is now available via :ref:`mj_stackAllocNum`.
6. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``,
8. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``,
and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s.
7. Changed :ref:`mjData.solver<mjData>`, the array used to collect solver diagnostic information.
9. Changed :ref:`mjData.solver<mjData>`, the array used to collect solver diagnostic information.
This array of :ref:`mjSolverStat` structs is now of length ``mjNISLAND * mjNSOLVER``, interpreted as as a matrix.
Each row of length ``mjNSOLVER`` contains separate solver statistics for each constraint island.
If the solver does not use islands, only row 0 is filled.
@@ -62,46 +73,46 @@ General
- Renamed ``mjData.solver_iter`` to ``solver_niter``. Both this member and ``mjData.solver_nnz`` are now integer
vectors of length ``mjNISLAND``.
8. Removed ``mjOption.collision`` and the associated ``option/collision`` attribute.
10. Removed ``mjOption.collision`` and the associated ``option/collision`` attribute.
**Migration:**
**Migration:**
- For models which have ``<option collision="all"/>``, delete the attribute.
- For models which have ``<option collision="dynamic"/>``, delete all :ref:`pair<contact-pair>` elements.
- For models which have ``<option collision="pair"/>``, disable all dynamic collisions (determined
via contype/conaffinity) by first deleting all :ref:`contype<body-geom-contype>` and
:ref:`conaffinity<body-geom-conaffinity>` attributes in the model and then setting them globally to ``0`` using
|br| ``<default> <geom contype="0" conaffinity="0"/> </default>``.
- For models which have ``<option collision="all"/>``, delete the attribute.
- For models which have ``<option collision="dynamic"/>``, delete all :ref:`pair<contact-pair>` elements.
- For models which have ``<option collision="pair"/>``, disable all dynamic collisions (determined
via contype/conaffinity) by first deleting all :ref:`contype<body-geom-contype>` and
:ref:`conaffinity<body-geom-conaffinity>` attributes in the model and then setting them globally to ``0`` using
|br| ``<default> <geom contype="0" conaffinity="0"/> </default>``.
9. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
the exact formula rather than with Euler integration.
10. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
11. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
the exact formula rather than with Euler integration.
12. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
11. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
13. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
12. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
14. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
13. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
15. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
:ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
14. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
16. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
linesearch stopping criteria in CG and Newton solvers. These can be useful for performance tuning.
15. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store the normalizing transformation applied to
17. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store the normalizing transformation applied to
mesh assets. Fixes `#409 <https://github.com/google-deepmind/mujoco/issues/409>`__ .
16. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
18. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a
target site, projected onto the camera image, in pixel coordinates.
Python bindings
^^^^^^^^^^^^^^^
17. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
19. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera.
Bug fixes
^^^^^^^^^
18. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
20. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
midphase collision trees.
model/plugin/elasticity/mannequin.xml
+171
View File
@@ -0,0 +1,171 @@
<!-- 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="Mannequin">
<!-- Static humanoid model with all joints removed -->
<option timestep="0.005"/>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<statistic center="0 0 0.7"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<!-- geoms -->
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body"/>
<default class="thigh">
<geom size=".06"/>
</default>
<default class="shin">
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
</default>
<default class="foot">
<geom size=".027"/>
<default class="foot1">
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
</default>
<default class="foot2">
<geom fromto="-.07 .01 0 .14 .03 0"/>
</default>
</default>
<default class="arm_upper">
<geom size=".04"/>
</default>
<default class="arm_lower">
<geom size=".031"/>
</default>
<default class="hand">
<geom type="sphere" size=".04"/>
</default>
<!-- joints -->
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="joint_big">
<joint damping="5" stiffness="10"/>
<default class="hip_x">
<joint range="-30 10"/>
</default>
<default class="hip_z">
<joint range="-60 35"/>
</default>
<default class="hip_y">
<joint axis="0 1 0" range="-150 20"/>
</default>
<default class="joint_big_stiff">
<joint stiffness="20"/>
</default>
</default>
<default class="knee">
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
</default>
<default class="ankle">
<joint range="-50 50"/>
<default class="ankle_y">
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
</default>
<default class="ankle_x">
<joint pos="0 0 .04" stiffness="3"/>
</default>
</default>
<default class="shoulder">
<joint range="-85 60"/>
</default>
<default class="elbow">
<joint range="-100 50" stiffness="0"/>
</default>
</default>
</default>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
<body name="torso" pos="0 0 1.282" childclass="body">
<light name="top" pos="0 0 2" mode="trackcom"/>
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="waist_lower" pos="-.01 0 -.26">
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
<body name="pelvis" pos="0 0 -.165">
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="thigh_right" pos="0 -.1 -.04">
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
<body name="shin_right" pos="0 .01 -.4">
<geom name="shin_right" class="shin"/>
<body name="foot_right" pos="0 0 -.39">
<geom name="foot1_right" class="foot1"/>
<geom name="foot2_right" class="foot2"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 .1 -.04">
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
<body name="shin_left" pos="0 -.01 -.4">
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
<body name="foot_left" pos="0 0 -.39">
<geom name="foot1_left" class="foot1"/>
<geom name="foot2_left" class="foot2"/>
</body>
</body>
</body>
</body>
</body>
<body name="upper_arm_right" pos="0 -.17 .06">
<geom name="upper_arm_right" fromto="0 0 0 0 -.22 0" class="arm_upper"/>
<body name="lower_arm_right" pos="0 -.22 0">
<geom name="lower_arm_right" fromto=".01 .01 .01 0 -.21 -.05" class="arm_lower"/>
<body name="hand_right" pos="0 -.22 -.05">
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
</body>
</body>
</body>
<body name="upper_arm_left" pos="0 .17 .06">
<geom name="upper_arm_left" fromto="0 0 0 0 .22 0" class="arm_upper"/>
<body name="lower_arm_left" pos="0 .22 0">
<geom name="lower_arm_left" fromto=".01 -.01 .01 0 .21 -.05" class="arm_lower"/>
<body name="hand_left" pos="0 .22 -.05">
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
</body>
</body>
</body>
</body>
</worldbody>
<contact>
<exclude body1="waist_lower" body2="thigh_right"/>
<exclude body1="waist_lower" body2="thigh_left"/>
</contact>
</mujoco>
model/plugin/elasticity/plate.xml
+54
View File
@@ -0,0 +1,54 @@
<!-- 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="Plate">
<include file="scene.xml"/>
<extension>
<plugin plugin="mujoco.elasticity.shell"/>
</extension>
<option solver="CG" tolerance="1e-6"/>
<size memory="10M"/>
<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"/>
</body>
<composite type="particle" count="16 16 1" spacing="0.05" offset="0 0 1">
<geom size=".02" rgba=".8 .2 .1 1" group="4" density="1000"
condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<skin inflate="0.01" rgba=".4 0 .3 1" subgrid="4"/>
<plugin plugin="mujoco.elasticity.shell">
<config key="poisson" value="0"/>
<config key="damping" value="0.001"/>
<config key="thickness" value="1e-2"/>
<!--Units are in Pa (SI)-->
<config key="young" value="3e7"/>
</plugin>
</composite>
</worldbody>
</mujoco>
model/plugin/elasticity/poncho.xml
+1417
View File
File diff suppressed because it is too large Load Diff
plugin/elasticity/CMakeLists.txt
+2
View File
@@ -22,6 +22,8 @@ set(MUJOCO_ELASTICITY_SRCS
elasticity.cc
elasticity.h
register.cc
shell.cc
shell.h
solid.cc
solid.h
)
plugin/elasticity/register.cc
+2
View File
@@ -14,12 +14,14 @@
#include <mujoco/mjplugin.h>
#include "cable.h"
#include "shell.h"
#include "solid.h"
namespace mujoco::plugin::elasticity {
mjPLUGIN_LIB_INIT {
Cable::RegisterPlugin();
Shell::RegisterPlugin();
Solid::RegisterPlugin();
}
plugin/elasticity/shell.cc
+305
View File
@@ -0,0 +1,305 @@
// 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 <algorithm>
#include <cctype>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstdlib>
#include <functional>
#include <sstream>
#include <optional>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "shell.h"
namespace mujoco::plugin::elasticity {
namespace {
// local tetrahedron numbering
constexpr int kNumEdges = Stencil2D::kNumEdges;
constexpr int kNumVerts = Stencil2D::kNumVerts;
constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
// copied from void mjXUtil
void String2Vector(const std::string& txt, std::vector<int>& vec) {
std::stringstream strm(txt);
vec.clear();
while (!strm.eof()) {
int num;
strm >> num;
if (strm.fail()) {
break;
} else {
vec.push_back(num);
}
}
}
// cotangent between two edges
mjtNum cot(mjtNum* x, int v0, int v1, int v2) {
mjtNum normal[3];
mjtNum edge1[3];
mjtNum edge2[3];
mju_sub3(edge1, x+3*v1, x+3*v0);
mju_sub3(edge2, x+3*v2, x+3*v0);
mju_cross(normal, edge1, edge2);
return mju_dot3(edge1, edge2) / mju_norm3(normal);
}
// area of a triangle
mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
mjtNum normal[3];
mjtNum edge1[3];
mjtNum edge2[3];
mju_sub3(edge1, x+3*v[1], x+3*v[0]);
mju_sub3(edge2, x+3*v[2], x+3*v[0]);
mju_cross(normal, edge1, edge2);
return mju_norm3(normal) / 2;
}
// 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<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("face", m, instance) &&
CheckAttr("poisson", m, instance) &&
CheckAttr("young", m, instance) &&
CheckAttr("thickness", m, instance)) {
mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
mjtNum thick =
strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr);
mjtNum damp =
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
std::vector<int> face;
String2Vector(mj_getPluginConfig(m, instance, "face"), face);
return Shell(m, d, instance, nu, E, damp, thick, face);
} else {
mju_warning("Invalid parameter specification in shell plugin");
return std::nullopt;
}
}
// create map from triangles to vertices and edges and from edges to vertices
void Shell::CreateStencils(const std::vector<int>& face) {
// populate stencil
nt = face.size() / kNumVerts;
elements.resize(nt);
for (int t = 0; t < nt; t++) {
for (int v = 0; v < kNumVerts; v++) {
elements[t].vertices[v] = face[kNumVerts*t+v]-1;
}
}
// map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// loop over all triangles
for (int t = 0; t < nt; t++) {
int* v = elements[t].vertices;
// compute edges to vertices map for fast computations
for (int e = 0; e < kNumEdges; 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) {
StencilFlap flap;
flap.vertices[0] = v[edge[e][0]];
flap.vertices[1] = v[edge[e][1]];
flap.vertices[2] = v[(edge[e][1]+1) % 3];
flap.vertices[3] = -1;
flaps.push_back(flap);
elements[t].edges[e] = ne++;
} else {
elements[t].edges[e] = it->second;
flaps[it->second].vertices[3] = v[(edge[e][1]+1) % 3];
}
}
}
}
// plugin constructor
Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, mjtNum thick, const std::vector<int>& face)
: damping(damp), thickness(thick) {
// count plugin bodies
nv = ne = 0;
for (int i = 1; i < m->nbody; i++) {
if (m->body_plugin[i] == instance) {
if (!nv++) {
i0 = i;
}
}
}
// generate triangles from the vertices
CreateStencils(face);
// material parameters
mjtNum mu = E / (2*(1+nu));
// loop over all triangles
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");
}
}
}
// allocate array
position.assign(nv*3, 0);
bending.assign(ne*16, 0);
// store previous positions
mju_copy(position.data(), m->body_pos+3*i0, 3*nv);
// assemble bending Hessian
for (int e = 0; e < ne; e++) {
int* v = flaps[e].vertices;
int vadj[3] = {v[1], v[0], v[3]};
if (v[3]== -1) {
// skip boundary edges
continue;
}
// cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
// Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
mjtNum a01 = cot(m->body_pos+3*i0, v[0], v[1], v[2]);
mjtNum a02 = cot(m->body_pos+3*i0, v[0], v[3], v[1]);
mjtNum a03 = cot(m->body_pos+3*i0, v[1], v[2], v[0]);
mjtNum a04 = cot(m->body_pos+3*i0, v[1], v[0], v[3]);
mjtNum c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
mjtNum volume = ComputeVolume(m->body_pos+3*i0, v) +
ComputeVolume(m->body_pos+3*i0, vadj);
for (int v1 = 0; v1 < StencilFlap::kNumVerts; v1++) {
for (int v2 = 0; v2 < StencilFlap::kNumVerts; v2++) {
bending[16 * e + 4 * v1 + v2] +=
1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12;
}
}
}
}
void Shell::Compute(const mjModel* m, mjData* d, int instance) {
mjtNum kD = damping / m->opt.timestep;
// compute bending contribution
mjtNum embedding;
for (int e = 0; e < ne; e++) {
int* v = flaps[e].vertices;
mjtNum force[12] = {0};
if (v[3] == -1) {
// skip boundary edges
continue;
}
for (int i = 0; i < StencilFlap::kNumVerts; i++) {
for (int j = 0; j < StencilFlap::kNumVerts; j++) {
for (int x = 0; x < 3; x++) {
embedding = d->xpos[3*(i0+v[j])+x]*(kD+1) - position[3*v[j]+x]*kD;
force[3*i+x] += bending[16*e+4*i+j] * embedding;
}
}
}
// update stored positions
mju_copy(position.data(), d->xpos+3*i0, 3*nv);
// insert into global force
for (int i = 0; i < StencilFlap::kNumVerts; i++) {
for (int x = 0; x < 3; x++) {
d->qfrc_passive[m->body_dofadr[i0]+3*v[i]+x] -= force[3*i+x];
}
}
}
}
void Shell::RegisterPlugin() {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.elasticity.shell";
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"face", "young", "poisson", "damping",
"thickness"};
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 = Shell::Create(m, d, instance);
if (!elasticity_or_null.has_value()) {
return -1;
}
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
new Shell(std::move(*elasticity_or_null)));
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<Shell*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
auto* elasticity = reinterpret_cast<Shell*>(d->plugin_data[instance]);
elasticity->Compute(m, d, instance);
};
mjp_registerPlugin(&plugin);
}
} // namespace mujoco::plugin::elasticity
plugin/elasticity/shell.h
+79
View File
@@ -0,0 +1,79 @@
// 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_SHELL_H_
#define MUJOCO_PLUGIN_ELASTICITY_SHELL_H_
#include <optional>
#include <vector>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
namespace mujoco::plugin::elasticity {
struct Stencil2D {
static constexpr int kNumEdges = 3;
static constexpr int kNumVerts = 3;
int vertices[kNumVerts];
int edges[kNumEdges];
};
struct StencilFlap {
static constexpr int kNumVerts = 4;
int vertices[kNumVerts];
};
class Shell {
public:
// Returns a new Shell instance or nullopt on failure.
static std::optional<Shell> Create(const mjModel* m, mjData* d,
int instance);
Shell(Shell&&) = default;
Shell& operator=(Shell&& other) = default;
void Compute(const mjModel* m, mjData* d, int instance);
static void RegisterPlugin();
int i0; // index of first body
int nc; // number of quads in the grid
int nv; // number of vertices (bodies) in the Shell
int nt; // number of area elements (triangles)
int ne; // number of edges in the Shell
// connectivity info for mapping tetrahedra to edges and vertices
std::vector<Stencil2D> elements; // triangles (nt x 6)
std::vector<StencilFlap> flaps; // adjacent triangles (ne x 4)
// precomputed quantities
std::vector<mjtNum> position; // previous-step positions (nv x 3)
std::vector<mjtNum> bending; // bending Hessian (ne x 16)
mjtNum damping;
mjtNum thickness;
private:
Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, mjtNum thick, const std::vector<int>& face);
void CreateStencils(const std::vector<int>& face);
};
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_PLUGIN_ELASTICITY_SHELL_H_
src/user/user_composite.cc
+46 -2
View File
@@ -14,6 +14,7 @@
#include "user/user_composite.h"
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdio>
@@ -25,9 +26,9 @@
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
@@ -394,7 +395,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
std::vector<mjtNum> volume(uservert.size()/3);
mjtNum t = 1;
if (dim == 2 && plugin_instance) {
// do nothing for now (until new passive forces are supported)
t = stod(plugin_instance->config_attribs["thickness"], nullptr);
}
if (!userface.empty()) {
mjXUtil::String2Vector(userface, face);
@@ -487,6 +488,49 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
}
}
// add isometry constraints
if (dim==2) {
char txt0[100], txt1[100], txt2[100];
std::vector<std::pair<int, int>> edge;
// create edges
for (int i=0; i<face.size()/3; i++) {
for (int j=0; j<3; j++) {
int v0 = face[3*i+(j+0)%3]-1;
int v1 = face[3*i+(j+1)%3]-1;
edge.push_back(v0 < v1 ? std::pair(v0, v1) : std::pair(v1, v0));
}
}
std::sort(edge.begin(), edge.end());
auto last = std::unique(edge.begin(), edge.end());
edge.erase(last, edge.end());
// create constraints
for (int i=0; i<edge.size(); i++) {
int v0 = edge[i].first;
int v1 = edge[i].second;
mju::sprintf_arr(txt0, "%sT%d_%d", prefix.c_str(), v0, v1);
mju::sprintf_arr(txt1, "%sS%d", prefix.c_str(), v0);
mju::sprintf_arr(txt2, "%sS%d", prefix.c_str(), v1);
// create tendon
mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
ten->def = model->defaults[0];
ten->name = txt0;
ten->group = 4;
ten->WrapSite(txt1);
ten->WrapSite(txt2);
// add equality constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TENDON);
eq->def = model->defaults[0];
eq->type = mjEQ_TENDON;
eq->name1 = ten->name;
}
}
if (skin) {
MakeSkin3(model);
}
test/engine/engine_plugin_test.cc
+1 -1
View File
@@ -32,7 +32,7 @@ namespace {
using ::testing::HasSubstr;
using ::testing::NotNull;
constexpr int kNumTruePlugins = 9;
constexpr int kNumTruePlugins = 10;
constexpr int kNumFakePlugins = 30;
constexpr int kNumTestPlugins = 3;
test/plugin/elasticity/elasticity_test.cc
+59 -5
View File
@@ -14,23 +14,77 @@
// Tests for plugin-related functionalities.
#include <array>
#include <cstdint>
#include <cstring>
#include <sstream>
#include <cmath>
#include <limits>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
#include "plugin/elasticity/shell.h"
#include "plugin/elasticity/solid.h"
namespace mujoco {
namespace {
// -------------------------------- shell -----------------------------------
TEST_F(PluginTest, ElasticEnergyShell) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<extension>
<plugin plugin="mujoco.elasticity.shell"/>
</extension>
<worldbody>
<composite type="particle" count="8 8 1" spacing="1">
<geom size=".025" group="4"/>
<plugin plugin="mujoco.elasticity.shell">
<config key="poisson" value="0"/>
<config key="young" value="2"/>
<config key="thickness" value="1"/>
</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);
auto* shell = reinterpret_cast<plugin::elasticity::Shell*>(d->plugin_data[0]);
// check that a plane is in the kernel of the energy
for (mjtNum scale = 1; scale < 4; scale++) {
for (int e = 0; e < shell->ne; e++) {
int* v = shell->flaps[e].vertices;
if (v[3]== -1) {
continue;
}
mjtNum energy = 0;
mjtNum volume = 1./2.;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
for (int x = 0; x < 3; x++) {
mjtNum elongation1 = scale * shell->position[3*v[i]+x];
mjtNum elongation2 = scale * shell->position[3*v[j]+x];
energy += shell->bending[16*e+4*i+j] * elongation1 * elongation2;
}
}
}
EXPECT_NEAR(
4*energy/volume, 0, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- solid -----------------------------------
TEST_F(PluginTest, ElasticEnergy) {
TEST_F(PluginTest, ElasticEnergySolid) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<extension>