diff --git a/doc/changelog.rst b/doc/changelog.rst index ab19efef..9afb1087 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,6 +5,11 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ +- Added support for shells with a curved reference configuration. See this `example + `__ + MJX ^^^ - Promote ``ten_length`` to the public MJX API. Add Warp support for ``mjx.tendon``. diff --git a/doc/includes/references.h b/doc/includes/references.h index 19ecfb67..b769c729 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1268,7 +1268,7 @@ struct mjModel_ { mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21) - mjtNum* flex_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_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 55c2f3c6..99c9347d 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -958,7 +958,7 @@ struct mjModel_ { mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21) - mjtNum* flex_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_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 9396a2a9..b5a1cdb8 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -372,7 +372,7 @@ X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ X ( mjtNum, flex_radius, nflex, 1 ) \ 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_edgestiffness, nflex, 1 ) \ X ( mjtNum, flex_edgedamping, nflex, 1 ) \ diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 9508e14f..4ec248bd 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -609,7 +609,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: flex_elemedge=wp.array(mjm.flex_elemedge, dtype=int), flexedge_length0=wp.array(mjm.flexedge_length0, 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), mesh_vertadr=wp.array(mjm.mesh_vertadr, dtype=int), mesh_vertnum=wp.array(mjm.mesh_vertnum, dtype=int), diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py index 72b00d90..da4b4623 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py @@ -454,7 +454,7 @@ def _flex_bending( flex_vertbodyid: wp.array(dtype=int), flex_edge: 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: flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # Data out: @@ -481,7 +481,7 @@ def _flex_bending( for i in range(nvert): for j in range(nvert): 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): bodyid = flex_vertbodyid[flex_vertadr[f] + v[i]] diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 2a1c1613..55f09ffe 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -1199,7 +1199,7 @@ class Model: flex_elemedge: wp.array(dtype=int) flexedge_length0: 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) mesh_vertadr: wp.array(dtype=int) mesh_vertnum: wp.array(dtype=int) diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index c1ec298e..3ba74834 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -124,7 +124,7 @@ def _forward_shim( eq_solref: wp.array2d(dtype=wp.vec2), eq_ten_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_dim: wp.array(dtype=int), flex_edge: wp.array(dtype=wp.vec2i), @@ -2148,7 +2148,7 @@ def _step_shim( eq_solref: wp.array2d(dtype=wp.vec2), eq_ten_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_dim: wp.array(dtype=int), flex_edge: wp.array(dtype=wp.vec2i), diff --git a/mjx/mujoco/mjx/warp/types.py b/mjx/mujoco/mjx/warp/types.py index 2d70fb60..9c3a188d 100644 --- a/mjx/mujoco/mjx/warp/types.py +++ b/mjx/mujoco/mjx/warp/types.py @@ -737,7 +737,7 @@ _NDIM = { 'eq_type': 1, 'eq_wld_adr': 1, 'exclude_signature': 1, - 'flex_bending': 3, + 'flex_bending': 1, 'flex_damping': 1, 'flex_dim': 1, 'flex_edge': 2, diff --git a/model/flex/basket.xml b/model/flex/basket.xml new file mode 100644 index 00000000..8dc15fe3 --- /dev/null +++ b/model/flex/basket.xml @@ -0,0 +1,48 @@ + + + + + + + + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index c2c0cff2..4407bd8d 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2811,7 +2811,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='bending stiffness', - array_extent=('nflexedge', 16), + array_extent=('nflexedge', 17), ), StructFieldDecl( name='flex_damping', diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 5e222340..ae5287a7 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -116,7 +116,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // flex elasticity for (int f=0; f < m->nflex; 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]; if (dim == 1 || m->flex_rigid[f]) { @@ -136,12 +136,32 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // skip boundary edges 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}; - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - for (int x = 0; x < 3; x++) { - force[3*i+x] += b[16*e+4*i+j] * xpos[3*v[j]+x]; + for (int x = 0; x < 3; x++) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + // 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]; } } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index b74a1c1f..8d378b8c 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -161,7 +161,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { case mjFCOMPTYPE_BOX: case mjFCOMPTYPE_CYLINDER: case mjFCOMPTYPE_ELLIPSOID: - res = MakeBox(error, error_sz); + res = MakeBox(error, error_sz, dflex->dim); break; 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 -bool mjCFlexcomp::MakeBox(char* error, int error_sz) { +bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { double pos[3]; bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0]; - // set 3D - def.spec.flex->dim = 3; + // set dimension + def.spec.flex->dim = dim; // add center point - point.push_back(0); - point.push_back(0); - point.push_back(0); + if (dim == 3) { + point.push_back(0); + point.push_back(0); + point.push_back(0); + } // add texture coordinates, if not specified explicitly if (needtex) { @@ -872,34 +880,30 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) { texcoord.push_back(0); } + // add points + int n = 0; + std::vector idx(count[0]*count[1]*count[2]); + // 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; + } + // add point BoxProject(pos, ix, iy, iz); point.push_back(pos[0]); point.push_back(pos[1]); point.push_back(pos[2]); + idx[mat2lin(ix, iy, iz, count)] = n++; // add texture coordinates, if not specified explicitly if (needtex) { texcoord.push_back(ix/(float)std::max(count[0]-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 iz=0; iz < count[2]; iz++) { // add point - if (iz > 0 && iz < count[2]-1) { + if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1))) { BoxProject(pos, ix, iy, iz); point.push_back(pos[0]); point.push_back(pos[1]); point.push_back(pos[2]); + idx[mat2lin(ix, iy, iz, count)] = n++; // add texture coordinates 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)); } } - - // 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 iz=0; iz < count[2]; iz++) { // 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); point.push_back(pos[0]); point.push_back(pos[1]); point.push_back(pos[2]); + idx[mat2lin(ix, iy, iz, count)] = n++; // add texture coordinates 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)); } } + } + } + } - // 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) { - element.push_back(0); - element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix, iy+1, iz)); - element.push_back(BoxID(ix, iy+1, iz+1)); + if (dim==3) { + element.push_back(0); + element.push_back(BoxID(ix, iy, iz)); + element.push_back(BoxID(ix, iy+1, iz)); + element.push_back(BoxID(ix, iy+1, 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, iy+1, 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, 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)]); + + } } } } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index ff3e3706..5e2108fd 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -55,7 +55,7 @@ class mjCFlexcomp { bool Make(mjsBody* body, 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 MakeMesh(mjCModel* model, char* error, int error_sz); bool MakeGMSH(mjCModel* model, char* error, int error_sz); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 46ba842a..a4f82f66 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3716,7 +3716,7 @@ static void CreateFlapStencil(std::vector& flaps, } // 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 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]}; @@ -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 // 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); + double a01 = cot(pos, v[0], v[1], v[2]); + double a02 = cot(pos, v[0], v[3], v[1]); + double a03 = cot(pos, v[1], v[2], v[0]); + double a04 = cot(pos, v[1], v[0], v[3]); + double c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)}; + double volume = ComputeVolume(pos, v) + 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 v2 = 0; v2 < T::kNumVerts; v2++) { - bending[4 * v1 + v2] += - 1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12; + bending[4 * v1 + v2] += c[v1] * c[v2] * cos_theta * stiffness; } } + + double n[3]; + mjuu_crossvec(n, e0, e1); + bending[16] = mjuu_dot3(n, e2) * (a01 - a03) * (a04 - a02) * stiffness / (sqr * sqrt(sqr)); } //----------------------------- linear elasticity -------------------------------------------------- @@ -4305,10 +4323,10 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (thickness < 0) { 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++) { - ComputeBending(bending.data() + 16 * e, vertxpos.data(), flaps[e].vertices, + ComputeBending(bending.data() + 17 * e, vertxpos.data(), flaps[e].vertices, young / (2 * (1 + poisson)), thickness); } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 0bb485d7..36ab30a6 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3267,9 +3267,9 @@ void mjCModel::CopyObjects(mjModel* m) { 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()); + mjuu_copyvec(m->flex_bending + 17 * edge_adr, pfl->bending.data(), pfl->bending.size()); } 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; diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 184771ac..9de5e314 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -14,6 +14,7 @@ // Tests for engine/engine_core_smooth.c. +#include #include #include @@ -166,5 +167,246 @@ TEST_F(TendonTest, SpringrangeDeadband) { mj_deleteModel(model); } +// -------------------------------- flex ------------------------------------ + +using ElasticityTest = MujocoTest; + +TEST_F(ElasticityTest, FlexCompatibility) { + static constexpr char flex_xml[] = R"( + + + + + + + + + + + )"; + + 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"( + + + + + + + + )"; + + 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::epsilon()); + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(ElasticityTest, CurvedShell) { + static constexpr char cantilever_xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + )"; + + 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::epsilon()); + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// -------------------------------- solid ----------------------------------- +TEST_F(ElasticityTest, ElasticEnergySolid) { + static constexpr char cantilever_xml[] = R"( + + + + + + + + + )"; + + 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::epsilon()); + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index ea4df2e3..c7a06439 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -28,181 +28,6 @@ namespace { using ElasticityTest = PluginTest; -// -------------------------------- flex ------------------------------------ -TEST_F(ElasticityTest, FlexCompatibility) { - static constexpr char flex_xml[] = R"( - - - - - - - - - - - )"; - - 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"( - - - - - - - - )"; - - 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::epsilon()); - } - } - - mj_deleteData(d); - mj_deleteModel(m); -} - -// -------------------------------- membrane ----------------------------------- -TEST_F(PluginTest, ElasticEnergyMembrane) { - static constexpr char cantilever_xml[] = R"( - - - - - - - - - )"; - - 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::epsilon()); - } - } - - mj_deleteData(d); - mj_deleteModel(m); -} - -// -------------------------------- solid ----------------------------------- -TEST_F(ElasticityTest, ElasticEnergySolid) { - static constexpr char cantilever_xml[] = R"( - - - - - - - - - )"; - - 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::epsilon()); - } - } - - mj_deleteData(d); - mj_deleteModel(m); -} // -------------------------------- cable ----------------------------------- TEST_F(ElasticityTest, CantileverIntoCircle) {