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

Move flex vertex-edge adjacency to mjModel.

Browse Source 
PiperOrigin-RevId: 861717037
Change-Id: I2ad8090b0318cb04b43d13966cb42260c82ef930
This commit is contained in:
Alessio Quaglino
2026-01-27 07:41:01 -08:00
committed by Copybara-Service
parent 669e9b7484
commit ba32568138
8 changed files with 239 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+4
View File
@@ -1318,6 +1318,9 @@ struct mjModel_ {
int* flex_texcoordadr; // address in flex_texcoord; -1: none (nflex x 1)
int* flex_nodebodyid; // node body ids (nflexnode x 1)
int* flex_vertbodyid; // vertex body ids (nflexvert x 1)
int* flex_vertedgeadr; // first edge address (nflexvert x 1)
int* flex_vertedgenum; // number of edges (nflexvert x 1)
int* flex_vertedge; // edge indices (nflexedge x 2)
int* flex_edge; // edge vertex ids (2 per edge) (nflexedge x 2)
int* flex_edgeflap; // adjacent vertex ids (dim=2 only) (nflexedge x 2)
int* flex_elem; // element vertex ids (dim+1 per elem) (nflexelemdata x 1)
@@ -1328,6 +1331,7 @@ struct mjModel_ {
int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2)
mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3)
mjtNum* flex_vert0; // vertex positions in qpos0 on [0, 1]^d (nflexvert x 3)
mjtNum* flex_vertmetric; // inverse of reference shape matrix (nflexvert x 4)
mjtNum* flex_node; // node positions in local body frames (nflexnode x 3)
mjtNum* flex_node0; // Cartesian node positions in qpos0 (nflexnode x 3)
mjtNum* flexedge_length0; // edge lengths in qpos0 (nflexedge x 1)
include/mujoco/mjmodel.h
+4
View File
@@ -984,6 +984,9 @@ struct mjModel_ {
int* flex_texcoordadr; // address in flex_texcoord; -1: none (nflex x 1)
int* flex_nodebodyid; // node body ids (nflexnode x 1)
int* flex_vertbodyid; // vertex body ids (nflexvert x 1)
int* flex_vertedgeadr; // first edge address (nflexvert x 1)
int* flex_vertedgenum; // number of edges (nflexvert x 1)
int* flex_vertedge; // edge indices (nflexedge x 2)
int* flex_edge; // edge vertex ids (2 per edge) (nflexedge x 2)
int* flex_edgeflap; // adjacent vertex ids (dim=2 only) (nflexedge x 2)
int* flex_elem; // element vertex ids (dim+1 per elem) (nflexelemdata x 1)
@@ -994,6 +997,7 @@ struct mjModel_ {
int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2)
mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3)
mjtNum* flex_vert0; // vertex positions in qpos0 on [0, 1]^d (nflexvert x 3)
mjtNum* flex_vertmetric; // inverse of reference shape matrix (nflexvert x 4)
mjtNum* flex_node; // node positions in local body frames (nflexnode x 3)
mjtNum* flex_node0; // Cartesian node positions in qpos0 (nflexnode x 3)
mjtNum* flexedge_length0; // edge lengths in qpos0 (nflexedge x 1)
include/mujoco/mjxmacro.h
+4
View File
@@ -372,6 +372,9 @@
X ( int, flex_texcoordadr, nflex, 1 ) \
X ( int, flex_nodebodyid, nflexnode, 1 ) \
X ( int, flex_vertbodyid, nflexvert, 1 ) \
X ( int, flex_vertedgeadr, nflexvert, 1 ) \
X ( int, flex_vertedgenum, nflexvert, 1 ) \
X ( int, flex_vertedge, nflexedge, 2 ) \
X ( int, flex_edge, nflexedge, 2 ) \
X ( int, flex_edgeflap, nflexedge, 2 ) \
X ( int, flex_elem, nflexelemdata, 1 ) \
@@ -382,6 +385,7 @@
X ( int, flex_evpair, nflexevpair, 2 ) \
X ( mjtNum, flex_vert, nflexvert, 3 ) \
X ( mjtNum, flex_vert0, nflexvert, 3 ) \
X ( mjtNum, flex_vertmetric, nflexvert, 4 ) \
X ( mjtNum, flex_node, nflexnode, 3 ) \
X ( mjtNum, flex_node0, nflexnode, 3 ) \
X ( mjtNum, flexedge_length0, nflexedge, 1 ) \
python/mujoco/introspect/structs.py
+32
View File
@@ -2765,6 +2765,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='vertex body ids',
array_extent=('nflexvert',),
),
StructFieldDecl(
name='flex_vertedgeadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='first edge address',
array_extent=('nflexvert',),
),
StructFieldDecl(
name='flex_vertedgenum',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='number of edges',
array_extent=('nflexvert',),
),
StructFieldDecl(
name='flex_vertedge',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='edge indices',
array_extent=('nflexedge', 2),
),
StructFieldDecl(
name='flex_edge',
type=PointerType(
@@ -2845,6 +2869,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='vertex positions in qpos0 on [0, 1]^d',
array_extent=('nflexvert', 3),
),
StructFieldDecl(
name='flex_vertmetric',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='inverse of reference shape matrix',
array_extent=('nflexvert', 4),
),
StructFieldDecl(
name='flex_node',
type=PointerType(
src/engine/engine_core_smooth.c
+104 -169
View File
@@ -725,159 +725,45 @@ void mj_flex(const mjModel* m, mjData* d) {
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
int nvert = m->flex_vertnum[f];
// use global vertex adjacency list
int* v_edge_cnt = m->flex_vertedgenum + vbase;
int* v_edge_adr = m->flex_vertedgeadr + vbase;
int* adj_edges = m->flex_vertedge;
mj_markStack(d);
// compute edge vectors
mjtNum* edge_dx = mjSTACKALLOC(d, 3*edgenum, mjtNum);
mjtNum* edge_dy = mjSTACKALLOC(d, 3*edgenum, mjtNum);
for (int e=0; e < edgenum; e++) {
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
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
}
mju_sub3(edge_dy+3*e, d->flexvert_xpos+3*(vbase+v2), d->flexvert_xpos+3*(vbase+v1));
}
// build vertex adjacency list
int* v_edge_cnt = mjSTACKALLOC(d, nvert, int);
int* v_edge_adr = mjSTACKALLOC(d, nvert, int);
int* adj_edges = mjSTACKALLOC(d, 2*m->flex_edgenum[f], int);
mju_zeroInt(v_edge_cnt, nvert);
for (int e = 0; e < edgenum; ++e) {
v_edge_cnt[m->flex_edge[2*(ebase+e)+0]]++;
v_edge_cnt[m->flex_edge[2*(ebase+e)+1]]++;
}
int total_adj_edges = 0;
for (int v = 0; v < nvert; ++v) {
v_edge_adr[v] = total_adj_edges;
total_adj_edges += v_edge_cnt[v];
}
int* v_edge_fill = mjSTACKALLOC(d, nvert, int);
mju_zeroInt(v_edge_fill, nvert);
for (int e = 0; e < edgenum; ++e) {
int v1 = m->flex_edge[2*(ebase+e)+0];
int v2 = m->flex_edge[2*(ebase+e)+1];
adj_edges[v_edge_adr[v1] + v_edge_fill[v1]] = e;
v_edge_fill[v1]++;
adj_edges[v_edge_adr[v2] + v_edge_fill[v2]] = e;
v_edge_fill[v2]++;
}
mjtNum* F_vert = mjSTACKALLOC(d, 6*nvert, mjtNum);
mjtNum* Binv_vert = mjSTACKALLOC(d, 4*nvert, mjtNum);
// compute averaged Cauchy strain tensors for each vertex
for (int v=0; v < nvert; v++) {
mjtNum A[6] = {0}, B[4] = {0};
int k, edge_idx;
for (k=0; k<v_edge_cnt[v]; k++) {
edge_idx = adj_edges[v_edge_adr[v]+k];
mjtNum* dx = edge_dx+3*edge_idx;
mjtNum* dy = edge_dy+3*edge_idx;
// get mass of neighbor vertex
mjtNum weight = 1.0;
int v1 = m->flex_edge[2 * (ebase + edge_idx)];
int v2 = m->flex_edge[2 * (ebase + edge_idx) + 1];
int neighbor_v = (v == v1) ? v2 : v1;
int b_neighbor = m->flex_vertbodyid[vbase + neighbor_v];
if (b_neighbor >= 0) {
weight = m->body_mass[b_neighbor];
if (weight < mjMINVAL) weight = mjMINVAL;
}
// accumulate A += w * dy * dx', B += w * dx * dx'
A[0] += weight * dy[0] * dx[0];
A[1] += weight * dy[0] * dx[1];
A[2] += weight * dy[1] * dx[0];
A[3] += weight * dy[1] * dx[1];
A[4] += weight * dy[2] * dx[0];
A[5] += weight * dy[2] * dx[1];
B[0] += weight * dx[0] * dx[0];
B[1] += weight * dx[0] * dx[1];
B[2] += weight * dx[1] * dx[0];
B[3] += weight * dx[1] * dx[1];
}
int vadr = vbase+v;
mjtNum* F = F_vert + 6*v;
mjtNum* Binv = Binv_vert + 4*v;
mjtNum cauchy[2][2];
// compute Binv = B^-1
mjtNum det = B[0]*B[3] - B[1]*B[2];
if (mju_abs(det) < mjMINVAL) {
mju_zero(Binv, 4);
} else {
mjtNum invdet = 1/det;
Binv[0] = B[3]*invdet;
Binv[1] = -B[1]*invdet;
Binv[2] = -B[2]*invdet;
Binv[3] = B[0]*invdet;
}
// compute deformation gradient F = A * Binv
mju_mulMatMat322(F, A, Binv);
// compute Cauchy strain tensor F^T F
cauchy[0][0] = F[0]*F[0] + F[2]*F[2] + F[4]*F[4];
cauchy[0][1] = F[0]*F[1] + F[2]*F[3] + F[4]*F[5];
cauchy[1][0] = F[1]*F[0] + F[3]*F[2] + F[5]*F[4];
cauchy[1][1] = F[1]*F[1] + F[3]*F[3] + F[5]*F[5];
// compute tensor invariants
d->flexvert_length[2*vadr+0] = cauchy[0][0] + cauchy[1][1] - 2;
d->flexvert_length[2*vadr+1] = cauchy[0][0] * cauchy[1][1] -
cauchy[0][1] * cauchy[1][0] - 1;
}
// clear Jacobian and assemble vertex by vertex
int* chain1 = mjSTACKALLOC(d, nv, int);
int* chain2 = mjSTACKALLOC(d, nv, int);
mjtNum* J0_dense = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* J1_dense = mjSTACKALLOC(d, nv, mjtNum);
mjtNum dI1dy1[3], dI1dy2[3], FB[6];
mjtNum dI2dy1[3], dI2dy2[3];
mjtNum cauchy[4], adj[4], Fadj[6], FadjBinv[6], dI2dy[3];
mju_zero(J0_dense, nv);
mju_zero(J1_dense, nv);
// temporary buffer for Jacobian accumulation
mjtNum* J_local = mjSTACKALLOC(d, nv, mjtNum);
for (int v = 0; v < nvert; v++) {
mjtNum* F = F_vert + 6*v;
mjtNum* Binv = Binv_vert + 4*v;
mjtNum A[6] = {0};
int vadr = vbase + v;
mjtNum* metric = m->flex_vertmetric + 4 * vadr;
// precompute for I1
mju_mulMatMat322(FB, F, Binv);
for (int k = 0; k < v_edge_cnt[v]; k++) {
int e = adj_edges[v_edge_adr[v] + k];
// precompute for I2
cauchy[0] = F[0]*F[0] + F[2]*F[2] + F[4]*F[4]; // c00
cauchy[1] = F[0]*F[1] + F[2]*F[3] + F[4]*F[5]; // c01
cauchy[3] = F[1]*F[1] + F[3]*F[3] + F[5]*F[5]; // c11
adj[0] = cauchy[3];
adj[1] = -cauchy[1];
adj[2] = -cauchy[1];
adj[3] = cauchy[0];
mju_mulMatMat322(Fadj, F, adj);
mju_mulMatMat322(FadjBinv, Fadj, Binv);
// compute rest configuration edge vector
mjtNum dx[3];
int v1 = m->flex_edge[2 * (ebase + e)];
int v2 = m->flex_edge[2 * (ebase + e) + 1];
mju_sub3(dx, m->flex_vert0 + 3 * (vbase + v2), m->flex_vert0 + 3 * (vbase + v1));
for (int i=0; i<v_edge_cnt[v]; ++i) {
int e = adj_edges[v_edge_adr[v]+i];
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
// apply scaling since they are half sizes
dx[0] *= 2 * m->flex_size[3 * f + 0];
dx[1] *= 2 * m->flex_size[3 * f + 1];
dx[2] *= 2 * m->flex_size[3 * f + 2];
// reuse precomputed edge vector
mjtNum* dx = edge_dx + 3 * e;
mjtNum dy[3];
mju_sub3(dy, d->flexvert_xpos + 3 * (vbase + v2), d->flexvert_xpos + 3 * (vbase + v1));
// get mass of neighbor vertex
mjtNum weight = 1.0;
@@ -888,15 +774,80 @@ void mj_flex(const mjModel* m, mjData* d) {
if (weight < mjMINVAL) weight = mjMINVAL;
}
// accumulate A += w * dy * dx'
A[0] += weight * dy[0] * dx[0];
A[1] += weight * dy[0] * dx[1];
A[2] += weight * dy[1] * dx[0];
A[3] += weight * dy[1] * dx[1];
A[4] += weight * dy[2] * dx[0];
A[5] += weight * dy[2] * dx[1];
}
mjtNum F[6];
mju_mulMatMat322(F, A, metric);
// compute Cauchy strain tensor F^T F
mjtNum cauchy[4];
cauchy[0] = F[0] * F[0] + F[2] * F[2] + F[4] * F[4]; // c00
cauchy[1] = F[0] * F[1] + F[2] * F[3] + F[4] * F[5]; // c01
cauchy[2] = F[1] * F[0] + F[3] * F[2] + F[5] * F[4]; // c10
cauchy[3] = F[1] * F[1] + F[3] * F[3] + F[5] * F[5]; // c11
// mass scaling: scale constraint by sqrt(mass) to improve condition number
// note: departure from original algorithm in Chen, Kry, and Vouga 2019
mjtNum scale = 1.0;
int b = m->flex_vertbodyid[vadr];
if (b >= 0) {
mjtNum mass = m->body_mass[b];
if (mass > mjMINVAL) {
scale = mju_sqrt(mass);
}
}
// compute tensor invariants
d->flexvert_length[2 * vadr + 0] = (cauchy[0] + cauchy[3] - 2) * scale;
d->flexvert_length[2 * vadr + 1] =
(cauchy[0] * cauchy[3] - cauchy[1] * cauchy[2] - 1) * scale;
// Jacobian computation
mjtNum FB[6], adj[4], Fadj[6], FadjBinv[6];
mju_mulMatMat322(FB, F, metric);
adj[0] = cauchy[3];
adj[1] = -cauchy[1];
adj[2] = -cauchy[2];
adj[3] = cauchy[0];
mju_mulMatMat322(Fadj, F, adj);
mju_mulMatMat322(FadjBinv, Fadj, metric);
for (int k = 0; k < v_edge_cnt[v]; ++k) {
int e = adj_edges[v_edge_adr[v] + k];
mjtNum dx[3]; // rest edge vector
int v1 = m->flex_edge[2 * (ebase + e)];
int v2 = m->flex_edge[2 * (ebase + e) + 1];
mju_sub3(dx, m->flex_vert0 + 3 * (vbase + v2), m->flex_vert0 + 3 * (vbase + v1));
dx[0] *= 2 * m->flex_size[3 * f + 0];
dx[1] *= 2 * m->flex_size[3 * f + 1];
dx[2] *= 2 * m->flex_size[3 * f + 2];
mjtNum weight = 1.0;
int neighbor_v = (v == v1) ? v2 : v1;
int b_neighbor = m->flex_vertbodyid[vbase + neighbor_v];
if (b_neighbor >= 0) {
weight = m->body_mass[b_neighbor];
if (weight < mjMINVAL) weight = mjMINVAL;
}
mjtNum dI1dy1[3], dI1dy2[3], dI2dy[3], dI2dy1[3], dI2dy2[3];
// dI1/dy1, dI1/dy2 (scaled by weight)
mju_mulMatVec(dI1dy1, FB, dx, 3, 2);
mju_scl3(dI1dy1, dI1dy1, -2 * weight);
mju_scl3(dI1dy2, dI1dy1, -1); // dI1dy2 = -dI1dy1
mju_scl3(dI1dy2, dI1dy1, -1);
// dI2/dy1, dI2/dy2 (scaled by weight)
mju_mulMatVec(dI2dy, FadjBinv, dx, 3, 2);
mju_scl3(dI2dy1, dI2dy, -2 * weight);
mju_scl3(dI2dy2, dI2dy1, -1); // dI2dy2 = -dI2dy1
mju_scl3(dI2dy2, dI2dy1, -1);
// get endpoint Jacobians
int b1 = m->flex_vertbodyid[vbase+v1];
@@ -907,56 +858,40 @@ void mj_flex(const mjModel* m, mjData* d) {
mj_jacSparse(m, d, jac2, NULL, d->flexvert_xpos + 3*(vbase+v2), b2, NV2, chain2);
// accumulate dense Jacobians for vertex v
mju_mulMatTVec(J_local, jac1, dI1dy1, 3, NV1);
for (int j=0; j<NV1; j++) {
J0_dense[chain1[j]] += dI1dy1[0]*jac1[j] + dI1dy1[1]*jac1[j+NV1] + dI1dy1[2]*jac1[j+2*NV1];
J0_dense[chain1[j]] += J_local[j];
}
mju_mulMatTVec(J_local, jac2, dI1dy2, 3, NV2);
for (int j=0; j<NV2; j++) {
J0_dense[chain2[j]] += dI1dy2[0]*jac2[j] + dI1dy2[1]*jac2[j+NV2] + dI1dy2[2]*jac2[j+2*NV2];
J0_dense[chain2[j]] += J_local[j];
}
mju_mulMatTVec(J_local, jac1, dI2dy1, 3, NV1);
for (int j=0; j<NV1; j++) {
J1_dense[chain1[j]] += dI2dy1[0]*jac1[j] + dI2dy1[1]*jac1[j+NV1] + dI2dy1[2]*jac1[j+2*NV1];
J1_dense[chain1[j]] += J_local[j];
}
mju_mulMatTVec(J_local, jac2, dI2dy2, 3, NV2);
for (int j=0; j<NV2; j++) {
J1_dense[chain2[j]] += dI2dy2[0]*jac2[j] + dI2dy2[1]*jac2[j+NV2] + dI2dy2[2]*jac2[j+2*NV2];
J1_dense[chain2[j]] += J_local[j];
}
}
// copy to sparse flexvert_J
int row0 = 2*(vbase+v);
int row0 = 2 * vadr;
int nnz0 = vrownnz[row0];
for (int j = 0; j < nnz0; j++) {
int col = m->flexvert_J_colind[vrowadr[row0] + j];
d->flexvert_J[vrowadr[row0] + j] += J0_dense[col];
d->flexvert_J[vrowadr[row0] + j] += J0_dense[col] * scale;
J0_dense[col] = 0;
}
int row1 = 2*(vbase+v)+1;
int row1 = 2 * vadr + 1;
int nnz1 = vrownnz[row1];
for (int j = 0; j < nnz1; j++) {
int col = m->flexvert_J_colind[vrowadr[row1] + j];
d->flexvert_J[vrowadr[row1] + j] += J1_dense[col];
d->flexvert_J[vrowadr[row1] + j] += J1_dense[col] * scale;
J1_dense[col] = 0;
}
// mass scaling: scale constraint by sqrt(mass) to improve condition
// number
int b = m->flex_vertbodyid[vbase + v];
if (b >= 0) {
mjtNum mass = m->body_mass[b];
if (mass > mjMINVAL) {
mjtNum scale = mju_sqrt(mass);
d->flexvert_length[2 * (vbase + v) + 0] *= scale;
d->flexvert_length[2 * (vbase + v) + 1] *= scale;
nnz0 = vrownnz[row0];
for (int j = 0; j < nnz0; j++) {
d->flexvert_J[vrowadr[row0] + j] *= scale;
}
nnz1 = vrownnz[row1];
for (int j = 0; j < nnz1; j++) {
d->flexvert_J[vrowadr[row1] + j] *= scale;
}
}
}
}
mj_freeStack(d);
src/engine/engine_setconst.c
+71 -6
View File
@@ -307,7 +307,14 @@ static void makeFlexSparse(mjModel* m, mjData* d) {
mju_zeroInt(rowadr, m->nflexedge);
mju_zeroInt(rownnz, m->nflexedge);
mju_zeroInt(vrowadr, 2 * m->nflexvert);
mju_zeroInt(vrowadr, 2 * m->nflexvert);
mju_zeroInt(vrownnz, 2 * m->nflexvert);
mju_zeroInt(m->flex_vertedgeadr, m->nflexvert);
mju_zeroInt(m->flex_vertedgenum, m->nflexvert);
mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge);
mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge);
mju_zero(m->flex_vertmetric, 4 * m->nflexvert);
int current_adj_offset = 0;
// compute lengths and Jacobians of edges
for (int f = 0; f < m->nflex; f++) {
@@ -351,18 +358,18 @@ static void makeFlexSparse(mjModel* m, mjData* d) {
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
int nvert = m->flex_vertnum[f];
// build vertex adjacency list local to this function
int* v_edge_cnt = mjSTACKALLOC(d, nvert, int);
int* v_edge_adr = mjSTACKALLOC(d, nvert, int);
int* adj_edges = mjSTACKALLOC(d, 2 * m->flex_edgenum[f], int);
mju_zeroInt(v_edge_cnt, nvert);
// populate global vertex adjacency list
int* v_edge_cnt = m->flex_vertedgenum + vbase;
int* v_edge_adr = m->flex_vertedgeadr + vbase;
int* adj_edges = m->flex_vertedge; // global array
for (int e = 0; e < m->flex_edgenum[f]; ++e) {
v_edge_cnt[m->flex_edge[2 * (ebase + e) + 0]]++;
v_edge_cnt[m->flex_edge[2 * (ebase + e) + 1]]++;
}
int total_adj_edges = 0;
for (int v = 0; v < nvert; ++v) {
v_edge_adr[v] = total_adj_edges;
v_edge_adr[v] = current_adj_offset + total_adj_edges;
total_adj_edges += v_edge_cnt[v];
}
int* v_edge_fill = mjSTACKALLOC(d, nvert, int);
@@ -376,6 +383,64 @@ static void makeFlexSparse(mjModel* m, mjData* d) {
v_edge_fill[v2]++;
}
// precompute metric (Binv)
for (int v = 0; v < nvert; ++v) {
mjtNum B[4] = {0};
int v_global = vbase + v;
for (int k = 0; k < v_edge_cnt[v]; ++k) {
int e = adj_edges[v_edge_adr[v] + k];
// compute rest edge vector
mjtNum dx[3];
int v1 = m->flex_edge[2 * (ebase + e)];
int v2 = m->flex_edge[2 * (ebase + e) + 1];
mju_sub3(dx, m->flex_vert0 + 3 * (vbase + v2),
m->flex_vert0 + 3 * (vbase + v1));
// apply scaling since they are half sizes
dx[0] *= 2 * m->flex_size[3 * f + 0];
dx[1] *= 2 * m->flex_size[3 * f + 1];
dx[2] *= 2 * m->flex_size[3 * f + 2];
if (mju_abs(dx[2]) > mjMINVAL) {
mjERROR("flex vertices are not in the same plane");
}
// get mass of neighbor vertex
mjtNum weight = 1.0;
int neighbor_v = (v == v1) ? v2 : v1;
int b_neighbor = m->flex_vertbodyid[vbase + neighbor_v];
if (b_neighbor >= 0) {
weight = m->body_mass[b_neighbor];
if (weight < mjMINVAL) weight = mjMINVAL;
}
// accumulate B += w * dx * dx'
for (int row = 0; row < 2; row++) {
for (int col = 0; col < 2; col++) {
B[2 * row + col] += weight * dx[row] * dx[col];
}
}
}
mjtNum* metric = m->flex_vertmetric + 4 * v_global;
mjtNum det = B[0] * B[3] - B[1] * B[2];
if (mju_abs(det) < mjMINVAL) {
mju_zero(metric, 4);
} else {
mjtNum invdet = 1.0 / det;
metric[0] = B[3] * invdet;
metric[1] = -B[1] * invdet;
metric[2] = -B[2] * invdet;
metric[3] = B[0] * invdet;
}
}
// advance global offset
current_adj_offset += total_adj_edges;
// determine start address for this flex
int v0_base = 2 * vbase;
int current_adr = 0;
unity/Runtime/Bindings/MjBindings.cs
+4
View File
@@ -5561,6 +5561,9 @@ public unsafe struct mjModel_ {
public int* flex_texcoordadr;
public int* flex_nodebodyid;
public int* flex_vertbodyid;
public int* flex_vertedgeadr;
public int* flex_vertedgenum;
public int* flex_vertedge;
public int* flex_edge;
public int* flex_edgeflap;
public int* flex_elem;
@@ -5571,6 +5574,7 @@ public unsafe struct mjModel_ {
public int* flex_evpair;
public double* flex_vert;
public double* flex_vert0;
public double* flex_vertmetric;
public double* flex_node;
public double* flex_node0;
public double* flexedge_length0;
wasm/codegen/generated/bindings.cc
+16
View File
@@ -4486,6 +4486,15 @@ struct MjModel {
emscripten::val flex_vertbodyid() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexvert, ptr_->flex_vertbodyid));
}
emscripten::val flex_vertedgeadr() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexvert, ptr_->flex_vertedgeadr));
}
emscripten::val flex_vertedgenum() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexvert, ptr_->flex_vertedgenum));
}
emscripten::val flex_vertedge() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexedge * 2, ptr_->flex_vertedge));
}
emscripten::val flex_edge() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexedge * 2, ptr_->flex_edge));
}
@@ -4516,6 +4525,9 @@ struct MjModel {
emscripten::val flex_vert0() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexvert * 3, ptr_->flex_vert0));
}
emscripten::val flex_vertmetric() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexvert * 4, ptr_->flex_vertmetric));
}
emscripten::val flex_node() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexnode * 3, ptr_->flex_node));
}
@@ -11389,6 +11401,10 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("flex_vert0", &MjModel::flex_vert0)
.property("flex_vertadr", &MjModel::flex_vertadr)
.property("flex_vertbodyid", &MjModel::flex_vertbodyid)
.property("flex_vertedge", &MjModel::flex_vertedge)
.property("flex_vertedgeadr", &MjModel::flex_vertedgeadr)
.property("flex_vertedgenum", &MjModel::flex_vertedgenum)
.property("flex_vertmetric", &MjModel::flex_vertmetric)
.property("flex_vertnum", &MjModel::flex_vertnum)
.property("flexedge_J_colind", &MjModel::flexedge_J_colind)
.property("flexedge_J_rowadr", &MjModel::flexedge_J_rowadr)