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

Align flex_vert0 to the XY plane during model initialization if dim=2.

Browse Source 
PiperOrigin-RevId: 860435368
Change-Id: Ida616e1e69c224f7c0dbad2a10dd59e64cbadf49
This commit is contained in:
Alessio Quaglino
2026-01-24 00:48:56 -08:00
committed by Copybara-Service
parent 769109630f
commit e9c61694e1
2 changed files with 56 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+8 -24
View File
@@ -711,39 +711,23 @@ void mj_flex(const mjModel* m, mjData* d) {
// Chen, Kry, and Vouga, "Locking-free Simulation of Isometric Thin Plates", 2019.
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
int nvert = m->flex_vertnum[f];
mjtNum edge1[3], edge2[3], normal[3];
mjtNum quat[4], mat[9];
int t_adr, t0, t1, t2;
mj_markStack(d);
// compute normal from first element
t_adr = m->flex_elemdataadr[f];
t0 = m->flex_elem[t_adr];
t1 = m->flex_elem[t_adr+1];
t2 = m->flex_elem[t_adr+2];
mju_sub3(edge1, m->flex_vert0 + 3*(vbase+t1), m->flex_vert0 + 3*(vbase+t0));
mju_sub3(edge2, m->flex_vert0 + 3*(vbase+t2), m->flex_vert0 + 3*(vbase+t0));
mji_cross(normal, edge1, edge2);
mju_normalize3(normal);
// compute rotation to Z
mju_quatZ2Vec(quat, normal);
mju_quat2Mat(mat, quat);
// compute edge vectors
mjtNum* edge_dx = mjSTACKALLOC(d, 3*m->flex_edgenum[f], mjtNum);
mjtNum* edge_dy = mjSTACKALLOC(d, 3*m->flex_edgenum[f], mjtNum);
for (int e=0; e < m->flex_edgenum[f]; e++) {
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
mjtNum dx3[3];
mju_sub3(dx3, m->flex_vert0 + 3*(vbase+v2), m->flex_vert0 + 3*(vbase+v1));
dx3[0] *= 2*m->flex_size[3*f+0];
dx3[1] *= 2*m->flex_size[3*f+1];
dx3[2] *= 2*m->flex_size[3*f+2];
mji_mulMatTVec3(edge_dx+3*e, mat, dx3);
mju_sub3(edge_dx + 3 * e, m->flex_vert0 + 3 * (vbase + v2),
m->flex_vert0 + 3 * (vbase + v1));
// apply scaling since they are half sizes
(edge_dx + 3 * e)[0] *= 2 * m->flex_size[3 * f + 0];
(edge_dx + 3 * e)[1] *= 2 * m->flex_size[3 * f + 1];
(edge_dx + 3 * e)[2] *= 2 * m->flex_size[3 * f + 2];
if (mju_abs((edge_dx+3*e)[2]) > mjMINVAL) {
mjERROR("flex vertices are not in the same plane"); // SHOULD NOT OCCUR
}
src/engine/engine_setconst.c
+48
View File
@@ -438,9 +438,57 @@ static void makeFlexSparse(mjModel* m, mjData* d) {
mj_freeStack(d);
}
// align 2D flexes to the XY plane
static void mj_alignFlex(mjModel* m, mjData* d) {
for (int f = 0; f < m->nflex; f++) {
// only for 2D flexes with vertex equality constraints
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
// get element data
int t_adr = m->flex_elemdataadr[f];
int vbase = m->flex_vertadr[f];
int t0 = m->flex_elem[t_adr];
int t1 = m->flex_elem[t_adr + 1];
int t2 = m->flex_elem[t_adr + 2];
// compute normal from first element
mjtNum edge1[3], edge2[3], normal[3];
mju_sub3(edge1, m->flex_vert0 + 3 * (vbase + t1), m->flex_vert0 + 3 * (vbase + t0));
mju_sub3(edge2, m->flex_vert0 + 3 * (vbase + t2),
m->flex_vert0 + 3 * (vbase + t0));
mju_cross(normal, edge1, edge2);
mju_normalize3(normal);
// compute rotation to Z
mjtNum quat[4], mat[9];
mju_quatZ2Vec(quat, normal);
mju_quat2Mat(mat, quat);
// rotate all vertices of this flex
int nvert = m->flex_vertnum[f];
for (int v = 0; v < nvert; v++) {
mjtNum* vert = m->flex_vert0 + 3 * (vbase + v);
mjtNum res[3];
mju_mulMatTVec3(res, mat, vert);
mju_copy3(vert, res);
// check planarity (warning if not planar)
if (mju_abs(vert[2] - m->flex_vert0[3 * (vbase + t0) + 2]) > 100 * mjMINVAL) {
static int warned = 0;
if (!warned) {
warned = 1;
mju_warning("flex %d is not planar", f);
}
}
}
}
}
}
// set quantities that depend on qpos0
static void set0(mjModel* m, mjData* d) {
makeFlexSparse(m, d);
mj_alignFlex(m, d);
int nv = m->nv;
mjtNum A[36] = {0}, pos[3], quat[4];
mj_markStack(d);