diff --git a/doc/includes/references.h b/doc/includes/references.h index 155a5046..2ead7899 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index c0be229c..f19d5ea0 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index f0c3f64a..2252656d 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index ce548f11..67b9f2b5 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -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( diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index caf971fc..287b83ba 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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; kflex_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; iflex_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; jflexvert_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); diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index d95d97ec..b983e53f 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -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; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d2e8ab36..48bb77b1 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 18e54a4c..7a964c34 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -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)