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
src/engine/engine_passive.c
+25 -5
View File
@@ -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];
}
}
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_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<int> 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)]);
}
}
}
}
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 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);
src/user/user_mesh.cc
+30 -12
View File
@@ -3716,7 +3716,7 @@ static void CreateFlapStencil(std::vector<StencilFlap>& 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<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);
}
}
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);
}
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;