Move bending stiffness from plugin to the compiler.

PiperOrigin-RevId: 758677644
Change-Id: I7d5c5140ba6948002bfa4ae9d6b4bf0f6de0e39b
This commit is contained in:
Alessio Quaglino
2025-05-14 07:38:25 -07:00
committed by Copybara-Service
parent d8bebdc675
commit 71bdd915f7
23 changed files with 134 additions and 154 deletions
+7 -1
View File
@@ -3627,9 +3627,10 @@ element is used to adjust the properties of all edges in the flex.
.. _flexcomp-elasticity-poisson: .. _flexcomp-elasticity-poisson:
.. _flexcomp-elasticity-damping: .. _flexcomp-elasticity-damping:
.. _flexcomp-elasticity-thickness: .. _flexcomp-elasticity-thickness:
.. _flexcomp-elasticity-elastic2d:
.. |body/flexcomp/elasticity attrib list| replace:: .. |body/flexcomp/elasticity attrib list| replace::
:at:`young`, :at:`poisson`, :at:`damping`, :at:`thickness` :at:`young`, :at:`poisson`, :at:`damping`, :at:`thickness`, :at:`elastic2d`
|body/flexcomp/elasticity attrib list| |body/flexcomp/elasticity attrib list|
Same as in :ref:`flex/elasticity<flex-elasticity>`. Same as in :ref:`flex/elasticity<flex-elasticity>`.
@@ -4070,6 +4071,11 @@ stress-strain relationship.. See also :ref:`deformable <CDeformable>` objects.
This thickness can be set equal to 2 times the :ref:`radius <deformable-flex-radius>` in order to match the geometry, This thickness can be set equal to 2 times the :ref:`radius <deformable-flex-radius>` in order to match the geometry,
but is exposed separately since the radius might be constrained by considerations related to collision detection. but is exposed separately since the radius might be constrained by considerations related to collision detection.
.. _flex-elasticity-elastic2d:
:at:`elastic2d`: :at-val:`int, "1"`
Elastic contribution to passive forces of 2D flexes. 0: none, 1: bending only, 2: stretching only, 3: bending and
stretching
.. _flex-contact: .. _flex-contact:
+4
View File
@@ -443,6 +443,8 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`young<flexcomp-elasticity-young>` | :ref:`poisson<flexcomp-elasticity-poisson>` | :ref:`damping<flexcomp-elasticity-damping>` | :ref:`thickness<flexcomp-elasticity-thickness>` | | | | | | :ref:`young<flexcomp-elasticity-young>` | :ref:`poisson<flexcomp-elasticity-poisson>` | :ref:`damping<flexcomp-elasticity-damping>` | :ref:`thickness<flexcomp-elasticity-thickness>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`elastic2d<flexcomp-elasticity-elastic2d>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flexcomp |br| |_2| |L| | | .. table:: | | |_2| flexcomp |br| |_2| |L| | | .. table:: |
| :ref:`contact | ? | :class: mjcf-attributes | | :ref:`contact | ? | :class: mjcf-attributes |
@@ -520,6 +522,8 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`young<flex-elasticity-young>` | :ref:`poisson<flex-elasticity-poisson>` | :ref:`damping<flex-elasticity-damping>` | :ref:`thickness<flex-elasticity-thickness>` | | | | | | :ref:`young<flex-elasticity-young>` | :ref:`poisson<flex-elasticity-poisson>` | :ref:`damping<flex-elasticity-damping>` | :ref:`thickness<flex-elasticity-thickness>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`elastic2d<flex-elasticity-elastic2d>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| deformable |br| |_| |L| | | .. table:: | | |_| deformable |br| |_| |L| | | .. table:: |
| :ref:`skin | \* | :class: mjcf-attributes | | :ref:`skin | \* | :class: mjcf-attributes |
+2
View File
@@ -1246,6 +1246,7 @@ struct mjModel_ {
mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1)
mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1)
mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21)
mjtNum* flex_bending; // bending stiffness (nflexedge x 16)
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
mjtNum* flex_edgedamping; // edge damping (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1)
@@ -2078,6 +2079,7 @@ typedef struct mjsFlex_ { // flex specification
double poisson; // Poisson's ratio double poisson; // Poisson's ratio
double damping; // Rayleigh's damping double damping; // Rayleigh's damping
double thickness; // thickness (2D only) double thickness; // thickness (2D only)
int elastic2d; // 2D passive forces; 0: none, 1: bending, 2: stretching, 3: both
// mesh properties // mesh properties
mjStringVec* nodebody; // node body names mjStringVec* nodebody; // node body names
+1
View File
@@ -913,6 +913,7 @@ struct mjModel_ {
mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1)
mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1)
mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21)
mjtNum* flex_bending; // bending stiffness (nflexedge x 16)
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
mjtNum* flex_edgedamping; // edge damping (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1)
+1
View File
@@ -444,6 +444,7 @@ typedef struct mjsFlex_ { // flex specification
double poisson; // Poisson's ratio double poisson; // Poisson's ratio
double damping; // Rayleigh's damping double damping; // Rayleigh's damping
double thickness; // thickness (2D only) double thickness; // thickness (2D only)
int elastic2d; // 2D passive forces; 0: none, 1: bending, 2: stretching, 3: both
// mesh properties // mesh properties
mjStringVec* nodebody; // node body names mjStringVec* nodebody; // node body names
+1
View File
@@ -366,6 +366,7 @@
X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \
XMJV( mjtNum, flex_radius, nflex, 1 ) \ XMJV( mjtNum, flex_radius, nflex, 1 ) \
X ( mjtNum, flex_stiffness, nflexelem, 21 ) \ X ( mjtNum, flex_stiffness, nflexelem, 21 ) \
X ( mjtNum, flex_bending, nflexedge, 16 ) \
X ( mjtNum, flex_damping, nflex, 1 ) \ X ( mjtNum, flex_damping, nflex, 1 ) \
X ( mjtNum, flex_edgestiffness, nflex, 1 ) \ X ( mjtNum, flex_edgestiffness, nflex, 1 ) \
X ( mjtNum, flex_edgedamping, nflex, 1 ) \ X ( mjtNum, flex_edgedamping, nflex, 1 ) \
+1 -1
View File
@@ -39,7 +39,7 @@
radius=".001" mass="10" name="plate" dim="2"> radius=".001" mass="10" name="plate" dim="2">
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/> <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<edge equality="false" damping="10"/> <edge equality="false" damping="10"/>
<elasticity young="3e5" poisson="0" thickness="1e-2"/> <elasticity young="3e5" poisson="0" thickness="1e-2" elastic2d="2"/>
<pin id="0 15 240 255"/> <pin id="0 15 240 255"/>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
+2 -6
View File
@@ -37,12 +37,8 @@
<flexcomp type="grid" count="9 19 1" spacing=".05 .05 .05" mass="10" <flexcomp type="grid" count="9 19 1" spacing=".05 .05 .05" mass="10"
name="flag" radius="0.001"> name="flag" radius="0.001">
<edge equality="true" damping="0.001"/> <edge equality="true" damping="0.001"/>
<plugin plugin="mujoco.elasticity.shell"> <elasticity poisson="0" thickness="1e-2" young="3e6"/>
<config key="poisson" value="0"/> <plugin plugin="mujoco.elasticity.shell"/>
<config key="thickness" value="1e-2"/>
<!--Units are in Pa (SI)-->
<config key="young" value="3e6"/>
</plugin>
</flexcomp> </flexcomp>
</body> </body>
</worldbody> </worldbody>
+1 -6
View File
@@ -39,12 +39,7 @@
radius=".01" mass=".5" name="plate" dim="2"> radius=".01" mass=".5" name="plate" dim="2">
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/> <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<edge equality="true" damping="10"/> <edge equality="true" damping="10"/>
<plugin plugin="mujoco.elasticity.shell"> <elasticity poisson="0" thickness="8e-3" young="3e5"/>
<config key="poisson" value="0"/>
<config key="thickness" value="8e-3"/>
<!--Units are in Pa (SI)-->
<config key="young" value="3e5"/>
</plugin>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
</mujoco> </mujoco>
+2 -6
View File
@@ -42,12 +42,8 @@
radius=".001" mass="100" name="plate"> radius=".001" mass="100" name="plate">
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/> <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001"/>
<edge equality="true" damping="0.001"/> <edge equality="true" damping="0.001"/>
<plugin plugin="mujoco.elasticity.shell"> <elasticity poisson="0" thickness="1e-2" young="3e7"/>
<config key="poisson" value="0"/> <plugin plugin="mujoco.elasticity.shell"/>
<config key="thickness" value="1e-2"/>
<!--Units are in Pa (SI)-->
<config key="young" value="3e7"/>
</plugin>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
</mujoco> </mujoco>
+2 -6
View File
@@ -1418,13 +1418,9 @@
398 399 418 398 399 418
398 376 378"> 398 376 378">
<edge equality="true" damping="0.1"/> <edge equality="true" damping="0.1"/>
<elasticity young="3e5" poisson="0" thickness="8e-3"/>
<contact solref="0.003"/> <contact solref="0.003"/>
<plugin plugin="mujoco.elasticity.shell"> <plugin plugin="mujoco.elasticity.shell"/>
<config key="poisson" value="0"/>
<config key="thickness" value="8e-3"/>
<!--Units are in Pa (SI)-->
<config key="young" value="3e5"/>
</plugin>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
@@ -1418,13 +1418,9 @@
398 399 418 398 399 418
398 376 378"> 398 376 378">
<edge equality="true" damping="0.1"/> <edge equality="true" damping="0.1"/>
<elasticity poisson="0.3" young="3e5" thickness="8e-3"/>
<contact solref="0.003" contype="0" conaffinity="0" vertcollide="true"/> <contact solref="0.003" contype="0" conaffinity="0" vertcollide="true"/>
<plugin plugin="mujoco.elasticity.shell"> <plugin plugin="mujoco.elasticity.shell"/>
<config key="poisson" value="0"/>
<config key="thickness" value="8e-3"/>
<!--Units are in Pa (SI)-->
<config key="young" value="3e5"/>
</plugin>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
+6 -76
View File
@@ -33,54 +33,18 @@ namespace {
// local tetrahedron numbering // local tetrahedron numbering
constexpr int kNumVerts = Stencil2D::kNumVerts; constexpr int kNumVerts = Stencil2D::kNumVerts;
// 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;
}
} // namespace } // namespace
// factory function // factory function
std::optional<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) { std::optional<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("poisson", m, instance) && return Shell(m, d, 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);
return Shell(m, d, instance, nu, E, thick);
} else {
mju_warning("Invalid parameter specification in shell plugin");
return std::nullopt;
}
} }
// plugin constructor // plugin constructor
Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, Shell::Shell(const mjModel* m, mjData* d, int instance)
mjtNum thick) : f0(-1) {
: f0(-1), thickness(thick) {
// count plugin bodies // count plugin bodies
nv = 0; nv = 0;
for (int i = 1; i < m->nbody; i++) { for (int i = 1; i < m->nbody; i++) {
@@ -104,9 +68,6 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
} }
} }
// material parameters
mjtNum mu = E / (2*(1+nu));
// loop over all triangles // loop over all triangles
for (int t = 0; t < m->flex_elemnum[f0]; t++) { for (int t = 0; t < m->flex_elemnum[f0]; t++) {
int* v = m->flex_elem + 3*(t+m->flex_elemadr[f0]); int* v = m->flex_elem + 3*(t+m->flex_elemadr[f0]);
@@ -119,41 +80,9 @@ Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
// allocate array // allocate array
position.assign(nv*3, 0); position.assign(nv*3, 0);
bending.assign(m->flex_edgenum[f0]*16, 0);
// store previous positions // store previous positions
mju_copy(position.data(), m->body_pos+3*i0, 3*nv); mju_copy(position.data(), m->body_pos+3*i0, 3*nv);
// assemble bending Hessian
for (int e = 0; e < m->flex_edgenum[f0]; e++) {
int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f0]);
int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f0]);
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
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) { void Shell::Compute(const mjModel* m, mjData* d, int instance) {
@@ -166,10 +95,11 @@ void Shell::Compute(const mjModel* m, mjData* d, int instance) {
// skip boundary edges // skip boundary edges
continue; continue;
} }
mjtNum* k = m->flex_bending + 16*m->flex_edgeadr[f0];
for (int i = 0; i < StencilFlap::kNumVerts; i++) { for (int i = 0; i < StencilFlap::kNumVerts; i++) {
for (int j = 0; j < StencilFlap::kNumVerts; j++) { for (int j = 0; j < StencilFlap::kNumVerts; j++) {
for (int x = 0; x < 3; x++) { for (int x = 0; x < 3; x++) {
force[3*i+x] += bending[16*e+4*i+j] * d->xpos[3*(i0+v[j])+x]; force[3*i+x] += k[16*e+4*i+j] * d->xpos[3*(i0+v[j])+x];
} }
} }
} }
@@ -195,7 +125,7 @@ void Shell::RegisterPlugin() {
plugin.name = "mujoco.elasticity.shell"; plugin.name = "mujoco.elasticity.shell";
plugin.capabilityflags |= mjPLUGIN_PASSIVE; plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"young", "poisson", "thickness", "damping"}; const char* attributes[] = {"damping"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes; plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
+1 -5
View File
@@ -51,13 +51,9 @@ class Shell {
// precomputed quantities // precomputed quantities
std::vector<mjtNum> position; // previous-step positions (nv x 3) std::vector<mjtNum> position; // previous-step positions (nv x 3)
std::vector<mjtNum> bending; // bending Hessian (ne x 16)
mjtNum thickness;
private: private:
Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, Shell(const mjModel* m, mjData* d, int instance);
mjtNum thick);
}; };
} // namespace mujoco::plugin::elasticity } // namespace mujoco::plugin::elasticity
+13
View File
@@ -2755,6 +2755,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='finite element stiffness matrix', doc='finite element stiffness matrix',
array_extent=('nflexelem', 21), array_extent=('nflexelem', 21),
), ),
StructFieldDecl(
name='flex_bending',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='bending stiffness',
array_extent=('nflexedge', 16),
),
StructFieldDecl( StructFieldDecl(
name='flex_damping', name='flex_damping',
type=PointerType( type=PointerType(
@@ -10867,6 +10875,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='double'), type=ValueType(name='double'),
doc='thickness (2D only)', doc='thickness (2D only)',
), ),
StructFieldDecl(
name='elastic2d',
type=ValueType(name='int'),
doc='2D passive forces; 0: none, 1: bending, 2: stretching, 3: both', # pylint: disable=line-too-long
),
StructFieldDecl( StructFieldDecl(
name='nodebody', name='nodebody',
type=PointerType( type=PointerType(
+1
View File
@@ -238,6 +238,7 @@ void mjs_defaultFlex(mjsFlex* flex) {
flex->rgba[0] = flex->rgba[1] = flex->rgba[2] = 0.5f; flex->rgba[0] = flex->rgba[1] = flex->rgba[2] = 0.5f;
flex->rgba[3] = 1.0f; flex->rgba[3] = 1.0f;
flex->thickness = -1; flex->thickness = -1;
flex->elastic2d = 1;
} }
+70 -6
View File
@@ -3075,6 +3075,56 @@ static void CreateFlapStencil(std::vector<StencilFlap>& flaps,
} }
} }
// cotangent between two edges
double inline cot(double* x, int v0, int v1, int v2) {
double normal[3];
double edge1[3] = {x[3*v1]-x[3*v0], x[3*v1+1]-x[3*v0+1], x[3*v1+2]-x[3*v0+2]};
double edge2[3] = {x[3*v2]-x[3*v0], x[3*v2+1]-x[3*v0+1], x[3*v2+2]-x[3*v0+2]};
mjuu_crossvec(normal, edge1, edge2);
return mjuu_dot3(edge1, edge2) / sqrt(mjuu_dot3(normal, normal));
}
// area of a triangle
double inline ComputeVolume(const double* x, const int v[Stencil2D::kNumVerts]) {
double normal[3];
double edge1[3] = {x[3*v[1]]-x[3*v[0]], x[3*v[1]+1]-x[3*v[0]+1], x[3*v[1]+2]-x[3*v[0]+2]};
double edge2[3] = {x[3*v[2]]-x[3*v[0]], x[3*v[2]+1]-x[3*v[0]+1], x[3*v[2]+2]-x[3*v[0]+2]};
mjuu_crossvec(normal, edge1, edge2);
return sqrt(mjuu_dot3(normal, normal)) / 2;
}
// compute bending stiffness for a single edge
template <typename T>
void inline ComputeBending(double* bending, double* pos, const int v[4], double mu,
double thickness) {
int vadj[3] = {v[1], v[0], v[3]};
if (v[3]== -1) {
// skip boundary edges
return;
}
// cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
// Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
mjtNum a01 = cot(pos, v[0], v[1], v[2]);
mjtNum a02 = cot(pos, v[0], v[3], v[1]);
mjtNum a03 = cot(pos, v[1], v[2], v[0]);
mjtNum a04 = cot(pos, v[1], v[0], v[3]);
mjtNum c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
mjtNum volume = ComputeVolume(pos, v) +
ComputeVolume(pos, vadj);
for (int v1 = 0; v1 < T::kNumVerts; v1++) {
for (int v2 = 0; v2 < T::kNumVerts; v2++) {
bending[4 * v1 + v2] +=
1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12;
}
}
}
//----------------------------- linear elasticity -------------------------------------------------- //----------------------------- linear elasticity --------------------------------------------------
// Gauss Legendre quadrature points in 1 dimension on the interval [a, b] // Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
@@ -3570,11 +3620,18 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// set size // set size
nedge = (int)edge.size(); nedge = (int)edge.size();
// create flap stencil
if (dim == 2) {
CreateFlapStencil(flaps, elem_, edgeidx_);
}
// compute elasticity // compute elasticity
if (young > 0) { if (young > 0) {
if (poisson < 0 || poisson >= 0.5) { if (poisson < 0 || poisson >= 0.5) {
throw mjCError(this, "Poisson ratio must be in [0, 0.5)"); throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
} }
// linear elasticity
stiffness.assign(21*nelem, 0); stiffness.assign(21*nelem, 0);
if (interpolated) { if (interpolated) {
int min_size = ceil(nodexpos.size()*nodexpos.size() / 21); int min_size = ceil(nodexpos.size()*nodexpos.size() / 21);
@@ -3583,11 +3640,13 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson); ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
} }
// geometrically nonlinear elasticity
for (unsigned int t = 0; t < nelem; t++) { for (unsigned int t = 0; t < nelem; t++) {
if (interpolated) { if (interpolated) {
continue; continue;
} }
if (dim == 2) { if (dim == 2 && elastic2d >= 2 && thickness > 0) {
ComputeStiffness<Stencil2D>(stiffness, vertxpos, ComputeStiffness<Stencil2D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young, elem_.data() + (dim + 1) * t, t, young,
poisson, thickness); poisson, thickness);
@@ -3597,6 +3656,16 @@ void mjCFlex::Compile(const mjVFS* vfs) {
poisson); poisson);
} }
} }
// bending stiffness (2D only)
if (dim == 2 && (elastic2d == 1 || elastic2d == 3) && thickness > 0) {
bending.assign(nedge*16, 0);
for (unsigned int e = 0; e < nedge; e++) {
ComputeBending<StencilFlap>(bending.data() + 16 * e, vertxpos.data(), flaps[e].vertices,
young / (2 * (1 + poisson)), thickness);
}
}
} }
// add plugins // add plugins
@@ -3613,11 +3682,6 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
} }
// create flap stencil
if (dim == 2) {
CreateFlapStencil(flaps, elem_, edgeidx_);
}
// create shell fragments and element-vertex collision pairs // create shell fragments and element-vertex collision pairs
CreateShellPair(); CreateShellPair();
+5
View File
@@ -3043,6 +3043,11 @@ void mjCModel::CopyObjects(mjModel* m) {
} else { } else {
mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem); mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem);
} }
if (!pfl->bending.empty()) {
mjuu_copyvec(m->flex_bending + 16 * edge_adr, pfl->bending.data(), pfl->bending.size());
} else {
mjuu_zerovec(m->flex_bending + 16 * edge_adr, 16 * pfl->nedge);
}
m->flex_damping[i] = (mjtNum)pfl->damping; m->flex_damping[i] = (mjtNum)pfl->damping;
// set fields: mesh-like // set fields: mesh-like
+1
View File
@@ -857,6 +857,7 @@ class mjCFlex_ : public mjCBase {
std::vector<double> elemaabb_; // element bounding volume std::vector<double> elemaabb_; // element bounding volume
std::vector<int> edgeidx_; // element edge ids std::vector<int> edgeidx_; // element edge ids
std::vector<double> stiffness; // elasticity stiffness matrix std::vector<double> stiffness; // elasticity stiffness matrix
std::vector<double> bending; // bending stiffness matrix
// variable-size data // variable-size data
std::vector<std::string> vertbody_; // vertex body names std::vector<std::string> vertbody_; // vertex body names
+5 -3
View File
@@ -311,7 +311,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"}, "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"},
{"<"}, {"<"},
{"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"}, {"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
{"elasticity", "?", "4", "young", "poisson", "damping", "thickness"}, {"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
{"contact", "?", "14", "contype", "conaffinity", "condim", "priority", {"contact", "?", "14", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap", "friction", "solmix", "solref", "solimp", "margin", "gap",
"internal", "selfcollide", "activelayers", "vertcollide"}, "internal", "selfcollide", "activelayers", "vertcollide"},
@@ -332,7 +332,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"friction", "solmix", "solref", "solimp", "margin", "gap", "friction", "solmix", "solref", "solimp", "margin", "gap",
"internal", "selfcollide", "activelayers", "vertcollide"}, "internal", "selfcollide", "activelayers", "vertcollide"},
{"edge", "?", "2", "stiffness", "damping"}, {"edge", "?", "2", "stiffness", "damping"},
{"elasticity", "?", "4", "young", "poisson", "damping", "thickness"}, {"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
{">"}, {">"},
{"skin", "*", "9", "name", "file", "material", "rgba", "inflate", {"skin", "*", "9", "name", "file", "material", "rgba", "inflate",
"vertex", "texcoord", "face", "group"}, "vertex", "texcoord", "face", "group"},
@@ -1401,6 +1401,7 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
ReadAttr(elasticity, "poisson", 1, &flex->poisson, text); ReadAttr(elasticity, "poisson", 1, &flex->poisson, text);
ReadAttr(elasticity, "thickness", 1, &flex->thickness, text); ReadAttr(elasticity, "thickness", 1, &flex->thickness, text);
ReadAttr(elasticity, "damping", 1, &flex->damping, text); ReadAttr(elasticity, "damping", 1, &flex->damping, text);
ReadAttr(elasticity, "elastic2d", 1, &flex->elastic2d, text);
} }
// write error info // write error info
@@ -2664,10 +2665,11 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
ReadAttr(elasticity, "poisson", 1, &dflex.poisson, text); ReadAttr(elasticity, "poisson", 1, &dflex.poisson, text);
ReadAttr(elasticity, "damping", 1, &dflex.damping, text); ReadAttr(elasticity, "damping", 1, &dflex.damping, text);
ReadAttr(elasticity, "thickness", 1, &dflex.thickness, text); ReadAttr(elasticity, "thickness", 1, &dflex.thickness, text);
ReadAttr(elasticity, "elastic2d", 1, &dflex.elastic2d, text);
} }
// check errors // check errors
if (elasticity && fcomp.equality) { if (dflex.elastic2d >= 2 && fcomp.equality) {
throw mjXError(elem, "elasticity and edge constraints cannot both be present"); throw mjXError(elem, "elasticity and edge constraints cannot both be present");
} }
+1
View File
@@ -194,6 +194,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson); WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson);
WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness); WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness);
WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping); WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping);
WriteAttr(elastic, "elastic2d", 1, &flex->elastic2d, &defflex.elastic2d);
// edge subelement // edge subelement
XMLElement* edge = InsertEnd(elem, "edge"); XMLElement* edge = InsertEnd(elem, "edge");
+4 -32
View File
@@ -65,11 +65,8 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
<worldbody> <worldbody>
<flexcomp type="grid" count="8 8 1" spacing="1 1 1" <flexcomp type="grid" count="8 8 1" spacing="1 1 1"
radius=".025" name="test" dim="2"> radius=".025" name="test" dim="2">
<plugin plugin="mujoco.elasticity.shell"> <elasticity young="2" poisson="0" thickness="1"/>
<config key="poisson" value="0"/> <plugin plugin="mujoco.elasticity.shell"/>
<config key="young" value="2"/>
<config key="thickness" value="1"/>
</plugin>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
</mujoco> </mujoco>
@@ -97,7 +94,7 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
for (int x = 0; x < 3; x++) { for (int x = 0; x < 3; x++) {
mjtNum elongation1 = scale * shell->position[3*v[i]+x]; mjtNum elongation1 = scale * shell->position[3*v[i]+x];
mjtNum elongation2 = scale * shell->position[3*v[j]+x]; mjtNum elongation2 = scale * shell->position[3*v[j]+x];
energy += shell->bending[16*e+4*i+j] * elongation1 * elongation2; energy += m->flex_bending[16*e+4*i+j] * elongation1 * elongation2;
} }
} }
} }
@@ -117,7 +114,7 @@ TEST_F(PluginTest, ElasticEnergyMembrane) {
<worldbody> <worldbody>
<flexcomp type="grid" count="8 8 1" spacing="1 1 1" <flexcomp type="grid" count="8 8 1" spacing="1 1 1"
radius=".025" name="test" dim="2"> radius=".025" name="test" dim="2">
<elasticity young="2" poisson="0" thickness="1"/> <elasticity young="2" poisson="0" thickness="1" elastic2d="2"/>
<edge equality="false"/> <edge equality="false"/>
</flexcomp> </flexcomp>
</worldbody> </worldbody>
@@ -161,31 +158,6 @@ TEST_F(PluginTest, ElasticEnergyMembrane) {
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(ElasticityTest, InvalidThickness) {
static constexpr char xml[] = R"(
<mujoco>
<extension>
<plugin plugin="mujoco.elasticity.shell"/>
</extension>
<worldbody>
<flexcomp type="grid" count="2 2 1" spacing="1 1 1"
radius=".025" name="test" dim="2">
<plugin plugin="mujoco.elasticity.shell">
<config key="thickness" value="hello"/>
</plugin>
<edge equality="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::IsNull());
EXPECT_THAT(error, ::testing::HasSubstr("Invalid parameter"));
}
// -------------------------------- solid ----------------------------------- // -------------------------------- solid -----------------------------------
TEST_F(ElasticityTest, ElasticEnergySolid) { TEST_F(ElasticityTest, ElasticEnergySolid) {
static constexpr char cantilever_xml[] = R"( static constexpr char cantilever_xml[] = R"(
+1
View File
@@ -5497,6 +5497,7 @@ public unsafe struct mjModel_ {
public double* flexedge_invweight0; public double* flexedge_invweight0;
public double* flex_radius; public double* flex_radius;
public double* flex_stiffness; public double* flex_stiffness;
public double* flex_bending;
public double* flex_damping; public double* flex_damping;
public double* flex_edgestiffness; public double* flex_edgestiffness;
public double* flex_edgedamping; public double* flex_edgedamping;