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

Bending stiffness for curved shells in flex.

Browse Source 
PiperOrigin-RevId: 796361841
Change-Id: I105ea235fe2a8e1bcbde67276d77fa92eed214a1
This commit is contained in:
Alessio Quaglino
2025-08-18 04:12:34 -07:00
committed by Copybara-Service
parent 972ffd7b90
commit b66175eba6
18 changed files with 488 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+5
View File
@@ -5,6 +5,11 @@ Changelog
Upcoming version (not yet released) Upcoming version (not yet released)
----------------------------------- -----------------------------------
General
^^^^^^^
- Added support for shells with a curved reference configuration. See this `example
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/basket.xml>`__
MJX MJX
^^^ ^^^
- Promote ``ten_length`` to the public MJX API. Add Warp support for ``mjx.tendon``. - Promote ``ten_length`` to the public MJX API. Add Warp support for ``mjx.tendon``.
doc/includes/references.h
+1 -1
View File
@@ -1268,7 +1268,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_bending; // bending stiffness (nflexedge x 17)
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)
include/mujoco/mjmodel.h
+1 -1
View File
@@ -958,7 +958,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_bending; // bending stiffness (nflexedge x 17)
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)
include/mujoco/mjxmacro.h
+1 -1
View File
@@ -372,7 +372,7 @@
X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \
X ( mjtNum, flex_radius, nflex, 1 ) \ X ( 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_bending, nflexedge, 17 ) \
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 ) \
mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py
Vendored
+1 -1
View File
@@ -609,7 +609,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model:
flex_elemedge=wp.array(mjm.flex_elemedge, dtype=int), flex_elemedge=wp.array(mjm.flex_elemedge, dtype=int),
flexedge_length0=wp.array(mjm.flexedge_length0, dtype=float), flexedge_length0=wp.array(mjm.flexedge_length0, dtype=float),
flex_stiffness=wp.array(mjm.flex_stiffness.flatten(), dtype=float), flex_stiffness=wp.array(mjm.flex_stiffness.flatten(), dtype=float),
flex_bending=wp.array(mjm.flex_bending, dtype=wp.mat44f), flex_bending=wp.array(mjm.flex_bending.flatten(), dtype=float),
flex_damping=wp.array(mjm.flex_damping, dtype=float), flex_damping=wp.array(mjm.flex_damping, dtype=float),
mesh_vertadr=wp.array(mjm.mesh_vertadr, dtype=int), mesh_vertadr=wp.array(mjm.mesh_vertadr, dtype=int),
mesh_vertnum=wp.array(mjm.mesh_vertnum, dtype=int), mesh_vertnum=wp.array(mjm.mesh_vertnum, dtype=int),
mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py
Vendored
+2 -2
View File
@@ -454,7 +454,7 @@ def _flex_bending(
flex_vertbodyid: wp.array(dtype=int), flex_vertbodyid: wp.array(dtype=int),
flex_edge: wp.array(dtype=wp.vec2i), flex_edge: wp.array(dtype=wp.vec2i),
flex_edgeflap: wp.array(dtype=wp.vec2i), flex_edgeflap: wp.array(dtype=wp.vec2i),
flex_bending: wp.array(dtype=wp.mat44f), flex_bending: wp.array(dtype=float),
# Data in: # Data in:
flexvert_xpos_in: wp.array2d(dtype=wp.vec3), flexvert_xpos_in: wp.array2d(dtype=wp.vec3),
# Data out: # Data out:
@@ -481,7 +481,7 @@ def _flex_bending(
for i in range(nvert): for i in range(nvert):
for j in range(nvert): for j in range(nvert):
for x in range(3): for x in range(3):
force[i, x] -= flex_bending[edgeid][i, j] * flexvert_xpos_in[worldid, v[j]][x] force[i, x] -= flex_bending[17*edgeid + 4*i + j] * flexvert_xpos_in[worldid, v[j]][x]
for i in range(nvert): for i in range(nvert):
bodyid = flex_vertbodyid[flex_vertadr[f] + v[i]] bodyid = flex_vertbodyid[flex_vertadr[f] + v[i]]
mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py
Vendored
+1 -1
View File
@@ -1199,7 +1199,7 @@ class Model:
flex_elemedge: wp.array(dtype=int) flex_elemedge: wp.array(dtype=int)
flexedge_length0: wp.array(dtype=float) flexedge_length0: wp.array(dtype=float)
flex_stiffness: wp.array(dtype=float) flex_stiffness: wp.array(dtype=float)
flex_bending: wp.array(dtype=wp.mat44f) flex_bending: wp.array(dtype=float)
flex_damping: wp.array(dtype=float) flex_damping: wp.array(dtype=float)
mesh_vertadr: wp.array(dtype=int) mesh_vertadr: wp.array(dtype=int)
mesh_vertnum: wp.array(dtype=int) mesh_vertnum: wp.array(dtype=int)
mjx/mujoco/mjx/warp/forward.py
+2 -2
View File
@@ -124,7 +124,7 @@ def _forward_shim(
eq_solref: wp.array2d(dtype=wp.vec2), eq_solref: wp.array2d(dtype=wp.vec2),
eq_ten_adr: wp.array(dtype=int), eq_ten_adr: wp.array(dtype=int),
eq_wld_adr: wp.array(dtype=int), eq_wld_adr: wp.array(dtype=int),
flex_bending: wp.array(dtype=wp.mat44f), flex_bending: wp.array(dtype=float),
flex_damping: wp.array(dtype=float), flex_damping: wp.array(dtype=float),
flex_dim: wp.array(dtype=int), flex_dim: wp.array(dtype=int),
flex_edge: wp.array(dtype=wp.vec2i), flex_edge: wp.array(dtype=wp.vec2i),
@@ -2148,7 +2148,7 @@ def _step_shim(
eq_solref: wp.array2d(dtype=wp.vec2), eq_solref: wp.array2d(dtype=wp.vec2),
eq_ten_adr: wp.array(dtype=int), eq_ten_adr: wp.array(dtype=int),
eq_wld_adr: wp.array(dtype=int), eq_wld_adr: wp.array(dtype=int),
flex_bending: wp.array(dtype=wp.mat44f), flex_bending: wp.array(dtype=float),
flex_damping: wp.array(dtype=float), flex_damping: wp.array(dtype=float),
flex_dim: wp.array(dtype=int), flex_dim: wp.array(dtype=int),
flex_edge: wp.array(dtype=wp.vec2i), flex_edge: wp.array(dtype=wp.vec2i),
mjx/mujoco/mjx/warp/types.py
+1 -1
View File
@@ -737,7 +737,7 @@ _NDIM = {
'eq_type': 1, 'eq_type': 1,
'eq_wld_adr': 1, 'eq_wld_adr': 1,
'exclude_signature': 1, 'exclude_signature': 1,
'flex_bending': 3, 'flex_bending': 1,
'flex_damping': 1, 'flex_damping': 1,
'flex_dim': 1, 'flex_dim': 1,
'flex_edge': 2, 'flex_edge': 2,
model/flex/basket.xml
+48
View File
@@ -0,0 +1,48 @@
<!-- Copyright 2025 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="Basket">
<include file="scene.xml"/>
<compiler autolimits="true"/>
<option solver="CG" tolerance="1e-6"/>
<size memory="10M"/>
<visual>
<map stiffness="100"/>
</visual>
<worldbody>
<flexcomp type="box" count="8 8 8" spacing=".1 .1 .1" pos="0 0 1"
radius=".01" rgba=".68 .53 .38 1" name="box" dim="2" mass="0.25">
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
<edge equality="true" damping="10"/>
<elasticity young="6e6" poisson="0.2" thickness="8e-3" elastic2d="bend"/>
</flexcomp>
<replicate count="3" offset=".2 0 0">
<replicate count="3" offset="0 .2 0">
<replicate count="5" offset="0 0 .2">
<body name="ball" pos="-.2 -.2 1.8">
<freejoint/>
<geom type="sphere" size="0.1" rgba="1 0 .2 1" mass=".03"/>
</body>
</replicate>
</replicate>
</replicate>
</worldbody>
</mujoco>
python/mujoco/introspect/structs.py
+1 -1
View File
@@ -2811,7 +2811,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
inner_type=ValueType(name='mjtNum'), inner_type=ValueType(name='mjtNum'),
), ),
doc='bending stiffness', doc='bending stiffness',
array_extent=('nflexedge', 16), array_extent=('nflexedge', 17),
), ),
StructFieldDecl( StructFieldDecl(
name='flex_damping', name='flex_damping',
src/engine/engine_passive.c
+25 -5
View File
@@ -116,7 +116,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// flex elasticity // flex elasticity
for (int f=0; f < m->nflex; f++) { for (int f=0; f < m->nflex; f++) {
mjtNum* k = m->flex_stiffness + 21*m->flex_elemadr[f]; mjtNum* k = m->flex_stiffness + 21*m->flex_elemadr[f];
mjtNum* b = m->flex_bending + 16*m->flex_edgeadr[f]; mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f];
int dim = m->flex_dim[f]; int dim = m->flex_dim[f];
if (dim == 1 || m->flex_rigid[f]) { if (dim == 1 || m->flex_rigid[f]) {
@@ -136,12 +136,32 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// skip boundary edges // skip boundary edges
continue; continue;
} }
// flap edges
mjtNum ed[3][3];
mju_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]);
mju_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]);
mju_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]);
// forces at the vertices due to curved reference
mjtNum frc[4][3];
mju_cross(frc[1], ed[1], ed[2]);
mju_cross(frc[2], ed[2], ed[0]);
mju_cross(frc[3], ed[0], ed[1]);
frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]);
frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]);
frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]);
// force
mjtNum force[12] = {0}; mjtNum force[12] = {0};
for (int i = 0; i < 4; i++) { for (int x = 0; x < 3; x++) {
for (int j = 0; j < 4; j++) { for (int i = 0; i < 4; i++) {
for (int x = 0; x < 3; x++) { for (int j = 0; j < 4; j++) {
force[3*i+x] += b[16*e+4*i+j] * xpos[3*v[j]+x]; // thin plate bending force
force[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
} }
// curved reference contribution
force[3*i+x] += b[17*e+16] * frc[i][x];
} }
} }
src/user/user_flexcomp.cc
+124 -44
View File
@@ -161,7 +161,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
case mjFCOMPTYPE_BOX: case mjFCOMPTYPE_BOX:
case mjFCOMPTYPE_CYLINDER: case mjFCOMPTYPE_CYLINDER:
case mjFCOMPTYPE_ELLIPSOID: case mjFCOMPTYPE_ELLIPSOID:
res = MakeBox(error, error_sz); res = MakeBox(error, error_sz, dflex->dim);
break; break;
case mjFCOMPTYPE_SQUARE: case mjFCOMPTYPE_SQUARE:
@@ -853,18 +853,26 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) {
static int mat2lin(int ix, int iy, int iz, const int count[3]) {
return ix*count[1]*count[2] + iy*count[2] + iz;
}
// make 3d box, ellipsoid or cylinder // make 3d box, ellipsoid or cylinder
bool mjCFlexcomp::MakeBox(char* error, int error_sz) { bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) {
double pos[3]; double pos[3];
bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0]; bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0];
// set 3D // set dimension
def.spec.flex->dim = 3; def.spec.flex->dim = dim;
// add center point // add center point
point.push_back(0); if (dim == 3) {
point.push_back(0); point.push_back(0);
point.push_back(0); point.push_back(0);
point.push_back(0);
}
// add texture coordinates, if not specified explicitly // add texture coordinates, if not specified explicitly
if (needtex) { if (needtex) {
@@ -872,34 +880,30 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
texcoord.push_back(0); texcoord.push_back(0);
} }
// add points
int n = 0;
std::vector<int> idx(count[0]*count[1]*count[2]);
// iz=0/max // iz=0/max
for (int iz=0; iz < count[2]; iz+=count[2]-1) { for (int iz=0; iz < count[2]; iz+=count[2]-1) {
for (int ix=0; ix < count[0]; ix++) { for (int ix=0; ix < count[0]; ix++) {
for (int iy=0; iy < count[1]; iy++) { for (int iy=0; iy < count[1]; iy++) {
if (open && dim == 2 && iz != 0) {
continue;
}
// add point // add point
BoxProject(pos, ix, iy, iz); BoxProject(pos, ix, iy, iz);
point.push_back(pos[0]); point.push_back(pos[0]);
point.push_back(pos[1]); point.push_back(pos[1]);
point.push_back(pos[2]); point.push_back(pos[2]);
idx[mat2lin(ix, iy, iz, count)] = n++;
// add texture coordinates, if not specified explicitly // add texture coordinates, if not specified explicitly
if (needtex) { if (needtex) {
texcoord.push_back(ix/(float)std::max(count[0]-1, 1)); texcoord.push_back(ix/(float)std::max(count[0]-1, 1));
texcoord.push_back(iy/(float)std::max(count[1]-1, 1)); texcoord.push_back(iy/(float)std::max(count[1]-1, 1));
} }
// add elements
if (ix < count[0]-1 && iy < count[1]-1) {
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix+1, iy, iz));
element.push_back(BoxID(ix+1, iy+1, iz));
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix, iy+1, iz));
element.push_back(BoxID(ix+1, iy+1, iz));
}
} }
} }
} }
@@ -909,11 +913,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
for (int ix=0; ix < count[0]; ix++) { for (int ix=0; ix < count[0]; ix++) {
for (int iz=0; iz < count[2]; iz++) { for (int iz=0; iz < count[2]; iz++) {
// add point // add point
if (iz > 0 && iz < count[2]-1) { if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1))) {
BoxProject(pos, ix, iy, iz); BoxProject(pos, ix, iy, iz);
point.push_back(pos[0]); point.push_back(pos[0]);
point.push_back(pos[1]); point.push_back(pos[1]);
point.push_back(pos[2]); point.push_back(pos[2]);
idx[mat2lin(ix, iy, iz, count)] = n++;
// add texture coordinates // add texture coordinates
if (needtex) { if (needtex) {
@@ -921,19 +926,6 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
texcoord.push_back(iz/(float)std::max(count[2]-1, 1)); texcoord.push_back(iz/(float)std::max(count[2]-1, 1));
} }
} }
// add elements
if (ix < count[0]-1 && iz < count[2]-1) {
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix+1, iy, iz));
element.push_back(BoxID(ix+1, iy, iz+1));
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix, iy, iz+1));
element.push_back(BoxID(ix+1, iy, iz+1));
}
} }
} }
} }
@@ -943,11 +935,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
for (int iy=0; iy < count[1]; iy++) { for (int iy=0; iy < count[1]; iy++) {
for (int iz=0; iz < count[2]; iz++) { for (int iz=0; iz < count[2]; iz++) {
// add point // add point
if (iz > 0 && iz < count[2]-1 && iy > 0 && iy < count[1]-1) { if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1)) && iy > 0 && iy < count[1]-1) {
BoxProject(pos, ix, iy, iz); BoxProject(pos, ix, iy, iz);
point.push_back(pos[0]); point.push_back(pos[0]);
point.push_back(pos[1]); point.push_back(pos[1]);
point.push_back(pos[2]); point.push_back(pos[2]);
idx[mat2lin(ix, iy, iz, count)] = n++;
// add texture coordinates // add texture coordinates
if (needtex) { if (needtex) {
@@ -955,18 +948,105 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
texcoord.push_back(iz/(float)std::max(count[2]-1, 1)); texcoord.push_back(iz/(float)std::max(count[2]-1, 1));
} }
} }
}
}
}
// add elements // add elements
// iz=0/max
for (int iz=0; iz < count[2]; iz+=count[2]-1) {
for (int ix=0; ix < count[0]; ix++) {
for (int iy=0; iy < count[1]; iy++) {
if (open && dim == 2 && iz != 0) {
continue;
}
if (ix < count[0]-1 && iy < count[1]-1) {
if (dim==3) {
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix+1, iy, iz));
element.push_back(BoxID(ix+1, iy+1, iz));
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix, iy+1, iz));
element.push_back(BoxID(ix+1, iy+1, iz));
} else {
int step1 = iz == 0 ? 1 : 0;
int step2 = iz == 0 ? 0 : 1;
element.push_back(idx[mat2lin(ix, iy, iz, count)]);
element.push_back(idx[mat2lin(ix+1, iy+step1, iz, count)]);
element.push_back(idx[mat2lin(ix+1, iy+step2, iz, count)]);
element.push_back(idx[mat2lin(ix, iy, iz, count)]);
element.push_back(idx[mat2lin(ix+step2, iy+1, iz, count)]);
element.push_back(idx[mat2lin(ix+step1, iy+1, iz, count)]);
}
}
}
}
}
// iy=0/max
for (int iy=0; iy < count[1]; iy+=count[1]-1) {
for (int ix=0; ix < count[0]; ix++) {
for (int iz=0; iz < count[2]; iz++) {
if (ix < count[0]-1 && iz < count[2]-1) {
if (dim==3) {
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix+1, iy, iz));
element.push_back(BoxID(ix+1, iy, iz+1));
element.push_back(0);
element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix, iy, iz+1));
element.push_back(BoxID(ix+1, iy, iz+1));
} else {
int ix0 = iy == 0 ? ix : ix+1;
int dx = iy == 0 ? 1 : -1;
element.push_back(idx[mat2lin(ix0, iy, iz, count)]);
element.push_back(idx[mat2lin(ix0+dx, iy, iz, count)]);
element.push_back(idx[mat2lin(ix0+dx, iy, iz+1, count)]);
element.push_back(idx[mat2lin(ix0, iy, iz, count)]);
element.push_back(idx[mat2lin(ix0+dx, iy, iz+1, count)]);
element.push_back(idx[mat2lin(ix0, iy, iz+1, count)]);
}
}
}
}
}
// ix=0/max
for (int ix=0; ix < count[0]; ix+=count[0]-1) {
for (int iy=0; iy < count[1]; iy++) {
for (int iz=0; iz < count[2]; iz++) {
if (iy < count[1]-1 && iz < count[2]-1) { if (iy < count[1]-1 && iz < count[2]-1) {
element.push_back(0); if (dim==3) {
element.push_back(BoxID(ix, iy, iz)); element.push_back(0);
element.push_back(BoxID(ix, iy+1, iz)); element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix, iy+1, iz+1)); element.push_back(BoxID(ix, iy+1, iz));
element.push_back(BoxID(ix, iy+1, iz+1));
element.push_back(0); element.push_back(0);
element.push_back(BoxID(ix, iy, iz)); element.push_back(BoxID(ix, iy, iz));
element.push_back(BoxID(ix, iy, iz+1)); element.push_back(BoxID(ix, iy, iz+1));
element.push_back(BoxID(ix, iy+1, iz+1)); element.push_back(BoxID(ix, iy+1, iz+1));
} else {
int iy0 = ix != 0 ? iy : iy+1;
int dy = ix != 0 ? 1 : -1;
element.push_back(idx[mat2lin(ix, iy0, iz, count)]);
element.push_back(idx[mat2lin(ix, iy0+dy, iz, count)]);
element.push_back(idx[mat2lin(ix, iy0+dy, iz+1, count)]);
element.push_back(idx[mat2lin(ix, iy0, iz, count)]);
element.push_back(idx[mat2lin(ix, iy0+dy, iz+1, count)]);
element.push_back(idx[mat2lin(ix, iy0, iz+1, count)]);
}
} }
} }
} }
src/user/user_flexcomp.h
+1 -1
View File
@@ -55,7 +55,7 @@ class mjCFlexcomp {
bool Make(mjsBody* body, char* error, int error_sz); bool Make(mjsBody* body, char* error, int error_sz);
bool MakeGrid(char* error, int error_sz); bool MakeGrid(char* error, int error_sz);
bool MakeBox(char* error, int error_sz); bool MakeBox(char* error, int error_sz, int dim, bool open = true);
bool MakeSquare(char* error, int error_sz); bool MakeSquare(char* error, int error_sz);
bool MakeMesh(mjCModel* model, char* error, int error_sz); bool MakeMesh(mjCModel* model, char* error, int error_sz);
bool MakeGMSH(mjCModel* model, char* error, int error_sz); bool MakeGMSH(mjCModel* model, char* error, int error_sz);
src/user/user_mesh.cc
+30 -12
View File
@@ -3716,7 +3716,7 @@ static void CreateFlapStencil(std::vector<StencilFlap>& flaps,
} }
// cotangent between two edges // cotangent between two edges
double inline cot(double* x, int v0, int v1, int v2) { double inline cot(const double* x, int v0, int v1, int v2) {
double normal[3]; 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 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]}; 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]};
@@ -3749,20 +3749,38 @@ void inline ComputeBending(double* bending, double* pos, const int v[4], double
// cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature // cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
// Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf // Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
mjtNum a01 = cot(pos, v[0], v[1], v[2]); double a01 = cot(pos, v[0], v[1], v[2]);
mjtNum a02 = cot(pos, v[0], v[3], v[1]); double a02 = cot(pos, v[0], v[3], v[1]);
mjtNum a03 = cot(pos, v[1], v[2], v[0]); double a03 = cot(pos, v[1], v[2], v[0]);
mjtNum a04 = cot(pos, v[1], v[0], v[3]); double a04 = cot(pos, v[1], v[0], v[3]);
mjtNum c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)}; double c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
mjtNum volume = ComputeVolume(pos, v) + double volume = ComputeVolume(pos, v) + ComputeVolume(pos, vadj);
ComputeVolume(pos, vadj); double stiffness = 3 * mu * pow(thickness, 3) / (24 * volume);
// Garg et al., "Cubic Shells", https://cims.nyu.edu/gcl/papers/garg2007cs.pdf
const double* v0 = pos + 3*v[0];
const double* v1 = pos + 3*v[1];
const double* v2 = pos + 3*v[2];
const double* v3 = pos + 3*v[3];
double e0[3] = {v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]};
double e1[3] = {v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]};
double e2[3] = {v3[0] - v0[0], v3[1] - v0[1], v3[2] - v0[2]};
double e3[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]};
double e4[3] = {v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]};
double t0[3] = {-(a03*e1[0] + a01*e3[0]), -(a03*e1[1] + a01*e3[1]), -(a03*e1[2] + a01*e3[2])};
double t1[3] = {-(a04*e2[0] + a02*e4[0]), -(a04*e2[1] + a02*e4[1]), -(a04*e2[2] + a02*e4[2])};
double sqr = mjuu_dot3(e0, e0);
double cos_theta = -mjuu_dot3(t0, t1) / sqr;
for (int v1 = 0; v1 < T::kNumVerts; v1++) { for (int v1 = 0; v1 < T::kNumVerts; v1++) {
for (int v2 = 0; v2 < T::kNumVerts; v2++) { for (int v2 = 0; v2 < T::kNumVerts; v2++) {
bending[4 * v1 + v2] += bending[4 * v1 + v2] += c[v1] * c[v2] * cos_theta * stiffness;
1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12;
} }
} }
double n[3];
mjuu_crossvec(n, e0, e1);
bending[16] = mjuu_dot3(n, e2) * (a01 - a03) * (a04 - a02) * stiffness / (sqr * sqrt(sqr));
} }
//----------------------------- linear elasticity -------------------------------------------------- //----------------------------- linear elasticity --------------------------------------------------
@@ -4305,10 +4323,10 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (thickness < 0) { if (thickness < 0) {
throw mjCError(this, "thickness must be positive for bending stiffness"); throw mjCError(this, "thickness must be positive for bending stiffness");
} }
bending.assign(nedge*16, 0); bending.assign(nedge*17, 0);
for (unsigned int e = 0; e < nedge; e++) { for (unsigned int e = 0; e < nedge; e++) {
ComputeBending<StencilFlap>(bending.data() + 16 * e, vertxpos.data(), flaps[e].vertices, ComputeBending<StencilFlap>(bending.data() + 17 * e, vertxpos.data(), flaps[e].vertices,
young / (2 * (1 + poisson)), thickness); young / (2 * (1 + poisson)), thickness);
} }
} }
src/user/user_model.cc
+2 -2
View File
@@ -3267,9 +3267,9 @@ void mjCModel::CopyObjects(mjModel* m) {
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()) { if (!pfl->bending.empty()) {
mjuu_copyvec(m->flex_bending + 16 * edge_adr, pfl->bending.data(), pfl->bending.size()); mjuu_copyvec(m->flex_bending + 17 * edge_adr, pfl->bending.data(), pfl->bending.size());
} else { } else {
mjuu_zerovec(m->flex_bending + 16 * edge_adr, 16 * pfl->nedge); mjuu_zerovec(m->flex_bending + 17 * edge_adr, 17 * pfl->nedge);
} }
m->flex_damping[i] = (mjtNum)pfl->damping; m->flex_damping[i] = (mjtNum)pfl->damping;
test/engine/engine_passive_test.cc
+242
View File
@@ -14,6 +14,7 @@
// Tests for engine/engine_core_smooth.c. // Tests for engine/engine_core_smooth.c.
#include <limits>
#include <string> #include <string>
#include <gmock/gmock.h> #include <gmock/gmock.h>
@@ -166,5 +167,246 @@ TEST_F(TendonTest, SpringrangeDeadband) {
mj_deleteModel(model); mj_deleteModel(model);
} }
// -------------------------------- flex ------------------------------------
using ElasticityTest = MujocoTest;
TEST_F(ElasticityTest, FlexCompatibility) {
static constexpr char flex_xml[] = R"(
<mujoco>
<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"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- shell -----------------------------------
TEST_F(ElasticityTest, ElasticEnergyShell) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<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"/>
</flexcomp>
</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);
mj_kinematics(m, d);
mj_flex(m, d);
// check that a plane is in the kernel of the energy
for (mjtNum scale = 1; scale < 4; scale++) {
for (int e = 0; e < m->flex_edgenum[0]; e++) {
int* edge = m->flex_edge + 2*(m->flex_edgeadr[0] + e);
int* flap = m->flex_edgeflap + 2*(m->flex_edgeadr[0] + e);
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
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 * d->flexvert_xpos[3*v[i]+x];
mjtNum elongation2 = scale * d->flexvert_xpos[3*v[j]+x];
energy += m->flex_bending[17*e+4*i+j] * elongation1 * elongation2;
}
}
}
EXPECT_NEAR(
4*energy/volume, 0, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(ElasticityTest, CurvedShell) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<worldbody>
<body name="v0" pos="-0.5 -0.5 -0.5">
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
<joint axis="1 0 0" type="slide"/>
<joint axis="0 1 0" type="slide"/>
<joint axis="0 0 1" type="slide"/>
</body>
<body name="v1" pos="-0.5 0.5 -0.5">
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
<joint axis="1 0 0" type="slide"/>
<joint axis="0 1 0" type="slide"/>
<joint axis="0 0 1" type="slide"/>
</body>
<body name="v2" pos="0.5 -0.5 -0.5">
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
<joint axis="1 0 0" type="slide"/>
<joint axis="0 1 0" type="slide"/>
<joint axis="0 0 1" type="slide"/>
</body>
<body name="v3" pos="-0.5 -0.5 0.5">
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
<joint axis="1 0 0" type="slide"/>
<joint axis="0 1 0" type="slide"/>
<joint axis="0 0 1" type="slide"/>
</body>
</worldbody>
<deformable>
<flex name="test" radius="0.025" flatskin="true" body="v0 v1 v2 v3"
element="0 2 3 0 3 1">
<elasticity young="2" thickness="1" elastic2d="bend"/>
</flex>
</deformable>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_kinematics(m, d);
mj_flex(m, d);
mj_passive(m, d);
// v1 force component is in-plane along v1-v0 edge (y-axis)
EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6);
EXPECT_NEAR(d->qfrc_spring[5], 0, 1e-6);
// v2 force component is in-plane along v2-v0 edge (x-axis)
EXPECT_NEAR(d->qfrc_spring[7], 0, 1e-6);
EXPECT_NEAR(d->qfrc_spring[8], 0, 1e-6);
// v3 force component is in-plane along v3-v0 edge (z-axis)
EXPECT_NEAR(d->qfrc_spring[9], 0, 1e-6);
EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6);
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- membrane -----------------------------------
TEST_F(ElasticityTest, ElasticEnergyMembrane) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<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" elastic2d="stretch"/>
<edge equality="false"/>
</flexcomp>
</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);
mj_kinematics(m, d);
mj_flex(m, d);
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
// check that if the entire geometry is rescaled by a factor "scale", then
// trace(strain^2) = 2*scale^2
for (mjtNum scale = 1; scale < 4; scale++) {
for (int t = 0; t < m->flex_elemnum[0]; t++) {
mjtNum energy = 0;
mjtNum volume = 1./2.;
int idx = 0;
for (int e1 = 0; e1 < 3; e1++) {
for (int e2 = e1; e2 < 3; e2++) {
int idx1 = m->flex_elemedge[3*t+e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[3*t+e2 + m->flex_elemedgeadr[0]];
mjtNum elong1 =
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
EXPECT_NEAR(
4*energy/volume, 2*scale*scale, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- solid -----------------------------------
TEST_F(ElasticityTest, ElasticEnergySolid) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<worldbody>
<flexcomp type="grid" count="8 8 8" spacing="1 1 1"
radius=".025" name="test" dim="3">
<elasticity young="2" poisson="0"/>
<edge equality="false"/>
</flexcomp>
</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);
mj_kinematics(m, d);
mj_flex(m, d);
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[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 < m->flex_elemnum[0]; t++) {
mjtNum energy = 0;
mjtNum volume = 1./6.;
int idx = 0;
for (int e1 = 0; e1 < 6; e1++) {
for (int e2 = e1; e2 < 6; e2++) {
int idx1 = m->flex_elemedge[6*t+e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[6*t+e2 + m->flex_elemedgeadr[0]];
mjtNum elong1 =
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
EXPECT_NEAR(
energy/volume, 3*scale*scale, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco
test/plugin/elasticity/elasticity_test.cc
-175
View File
@@ -28,181 +28,6 @@ namespace {
using ElasticityTest = PluginTest; using ElasticityTest = PluginTest;
// -------------------------------- flex ------------------------------------
TEST_F(ElasticityTest, FlexCompatibility) {
static constexpr char flex_xml[] = R"(
<mujoco>
<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"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- shell -----------------------------------
TEST_F(ElasticityTest, ElasticEnergyShell) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<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"/>
</flexcomp>
</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);
mj_kinematics(m, d);
mj_flex(m, d);
// check that a plane is in the kernel of the energy
for (mjtNum scale = 1; scale < 4; scale++) {
for (int e = 0; e < m->flex_edgenum[0]; e++) {
int* edge = m->flex_edge + 2*(m->flex_edgeadr[0] + e);
int* flap = m->flex_edgeflap + 2*(m->flex_edgeadr[0] + e);
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
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 * d->flexvert_xpos[3*v[i]+x];
mjtNum elongation2 = scale * d->flexvert_xpos[3*v[j]+x];
energy += m->flex_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);
}
// -------------------------------- membrane -----------------------------------
TEST_F(PluginTest, ElasticEnergyMembrane) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<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" elastic2d="stretch"/>
<edge equality="false"/>
</flexcomp>
</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);
mj_kinematics(m, d);
mj_flex(m, d);
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
// check that if the entire geometry is rescaled by a factor "scale", then
// trace(strain^2) = 2*scale^2
for (mjtNum scale = 1; scale < 4; scale++) {
for (int t = 0; t < m->flex_elemnum[0]; t++) {
mjtNum energy = 0;
mjtNum volume = 1./2.;
int idx = 0;
for (int e1 = 0; e1 < 3; e1++) {
for (int e2 = e1; e2 < 3; e2++) {
int idx1 = m->flex_elemedge[3*t+e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[3*t+e2 + m->flex_elemedgeadr[0]];
mjtNum elong1 =
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
EXPECT_NEAR(
4*energy/volume, 2*scale*scale, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- solid -----------------------------------
TEST_F(ElasticityTest, ElasticEnergySolid) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<worldbody>
<flexcomp type="grid" count="8 8 8" spacing="1 1 1"
radius=".025" name="test" dim="3">
<elasticity young="2" poisson="0"/>
<edge equality="false"/>
</flexcomp>
</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);
mj_kinematics(m, d);
mj_flex(m, d);
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[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 < m->flex_elemnum[0]; t++) {
mjtNum energy = 0;
mjtNum volume = 1./6.;
int idx = 0;
for (int e1 = 0; e1 < 6; e1++) {
for (int e2 = e1; e2 < 6; e2++) {
int idx1 = m->flex_elemedge[6*t+e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[6*t+e2 + m->flex_elemedgeadr[0]];
mjtNum elong1 =
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
EXPECT_NEAR(
energy/volume, 3*scale*scale, std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- cable ----------------------------------- // -------------------------------- cable -----------------------------------
TEST_F(ElasticityTest, CantileverIntoCircle) { TEST_F(ElasticityTest, CantileverIntoCircle) {