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

Introduce trilinear flex parametrization.

Browse Source 
These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh.

On an 8x8x8 cube, the performance using DOFs at all vertices is

```
 Simulation time      : 18.74 s
 Steps per second     : 533
 Realtime factor      : 0.53 x
 Time per step        : 1874.4 µs

 Contacts per step    : 114.88
 Constraints per step : 3322.51
 Degrees of freedom   : 1536
```

With the new implementation, it is the following:

```
 Simulation time      : 1.82 s
 Steps per second     : 5507
 Realtime factor      : 5.51 x
 Time per step        : 181.6 µs

 Contacts per step    : 38.84
 Constraints per step : 155.36
 Degrees of freedom   : 24
```

PiperOrigin-RevId: 721008829
Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22
This commit is contained in:
Alessio Quaglino
2025-01-29 09:37:37 -08:00
committed by Copybara-Service
parent 1a4b821b6b
commit 7cdf180641
47 changed files with 8967 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+2 -2
View File
@@ -1859,7 +1859,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f];
for (int i=0; i <= dim; i++) {
if (b == bdata[edata[i]]) {
if (b >= 0 && b == bdata[edata[i]]) {
return;
}
}
@@ -1983,7 +1983,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
for (int i1=0; i1 <= dim1; i1++) {
int b1 = bdata1[edata1[i1]];
for (int i2=0; i2 <= dim2; i2++) {
if (b1 == bdata2[edata2[i2]]) {
if (b1 >= 0 && b1 == bdata2[edata2[i2]]) {
return;
}
}
src/engine/engine_core_constraint.c
+86 -22
View File
@@ -181,7 +181,7 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
for (int i=0; i <= dim; i++) {
mjtNum dist = mju_dist3(point, vert+3*edata[i]);
weight[i] = 1.0/(mju_max(mjMINVAL, dist));
body[i] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[i]];
body[i] = m->flex_vertadr[f] + edata[i];
// check if element vertex matches v
if (edata[i] == v) {
@@ -206,6 +206,30 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
// compute body weights for a given contact vertex, return #bodies
static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v,
const mjtNum point[3], int* body, mjtNum* weight, mjtNum bw) {
mjtNum* coord = m->flex_vert0 + 3*v;
int nstart = m->flex_nodeadr[f];
int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
int nb = 0;
for (int i = nstart; i < nend; i++) {
mjtNum w = ((i-nstart)&1 ? coord[2] : 1-coord[2]) *
((i-nstart)&2 ? coord[1] : 1-coord[1]) *
((i-nstart)&4 ? coord[0] : 1-coord[0]);
if (w < 1e-5) {
continue;
}
if (weight) weight[nb] = w * bw;
body[nb++] = m->flex_nodebodyid[i];
}
return nb;
}
// add contact to d->contact list; return 0 if success; 1 if buffer full
int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
// if nconmax is specified and ncon >= nconmax, warn and return error
@@ -971,8 +995,8 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
con->efc_address = d->nefc;
// special case: single body on each side
if ((con->geom[0] >= 0 || con->vert[0] >= 0) &&
(con->geom[1] >= 0 || con->vert[1] >= 0)) {
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
@@ -995,9 +1019,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
else {
// get bodies and weights
int nb = 0;
int bid[8];
mjtNum bweight[8];
int bid[64];
mjtNum bweight[64];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
mjtNum bw[4];
// geom
if (con->geom[side] >= 0) {
bid[nb] = m->geom_bodyid[con->geom[side]];
@@ -1007,22 +1035,32 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// flex vert
else if (con->vert[side] >= 0) {
bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[nb] = side ? +1 : -1;
nb++;
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = side ? +1 : -1;
nw = 1;
}
// flex elem
else {
int nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid+nb, bweight+nb);
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw);
// negative sign for first side of contact
if (side == 0) {
mju_scl(bweight+nb, bweight+nb, -1, nw);
mju_scl(bw, bw, -1, nw);
}
}
nb += nw;
// get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) {
bid[nb] = m->flex_vertbodyid[vid[k]];
bweight[nb] = bw[k];
nb++;
} else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid+nb, bweight+nb, bw[k]);
}
}
}
@@ -1201,8 +1239,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
tran = rot = 0;
for (int side=0; side < 2; side++) {
// get bodies and weights
int nb, bid[4];
mjtNum bweight[4];
int nb = 0, bid[32], vid[4], nw = 0;
mjtNum bweight[32], bw[4];
// geom
if (con->geom[side] >= 0) {
@@ -1213,15 +1251,27 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// flex vert
else if (con->vert[side] >= 0) {
bid[0] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[0] = 1;
nb = 1;
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = 1;
nw = 1;
}
// flex elem
else {
nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid, bweight);
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw);
}
// get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) {
bid[k] = m->flex_vertbodyid[vid[k]];
bweight[k] = bw[k];
nb++;
} else {
nb = mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid, bweight, bw[k]);
}
}
// add weighted average over bodies
@@ -1881,8 +1931,11 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int NV = 0;
if (nnz) {
// get bodies
int nb = 0, bid[8];
int nb = 0, bid[64];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
// geom
if (con->geom[side] >= 0) {
bid[nb++] = m->geom_bodyid[con->geom[side]];
@@ -1890,7 +1943,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
// flex vert
else if (con->vert[side] >= 0) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side];
}
// flex elem
@@ -1899,7 +1952,18 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int fdim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
for (int k=0; k <= fdim; k++) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[k]];
vid[nw++] = m->flex_vertadr[f] + edata[k];
}
}
// get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) {
bid[nb] = m->flex_vertbodyid[vid[k]];
nb++;
} else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid+nb, NULL, 0);
}
}
}
src/engine/engine_core_smooth.c
+40 -8
View File
@@ -453,19 +453,51 @@ void mj_flex(const mjModel* m, mjData* d) {
for (int f=0; f < m->nflex; f++) {
int vstart = m->flex_vertadr[f];
int vend = m->flex_vertadr[f] + m->flex_vertnum[f];
int nstart = m->flex_nodeadr[f];
int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i < vend; i++) {
mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
// 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs
if (m->flex_interp[f] == 0) {
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i < vend; i++) {
mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
// non-centered: map from local to global
else {
for (int i=vstart; i < vend; i++) {
mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
}
// non-centered: map from local to global
// trilinear interpolation
else {
mjtNum nodexpos[mjMAXFLEXNODES];
if (m->flex_centered[f]) {
for (int i=nstart; i < nend; i++) {
mju_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]);
}
} else {
for (int i=nstart; i < nend; i++) {
int j = i - nstart;
mju_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i);
mju_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
}
}
for (int i=vstart; i < vend; i++) {
mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
mju_zero3(d->flexvert_xpos+3*i);
mjtNum* coord = m->flex_vert0 + 3*i;
for (int j=0; j < nend-nstart; j++) {
mjtNum coef = (j&1 ? coord[2] : 1-coord[2]) *
(j&2 ? coord[1] : 1-coord[1]) *
(j&4 ? coord[0] : 1-coord[0]);
mju_addToScl3(d->flexvert_xpos+3*i, nodexpos+3*j, coef);
}
}
}
}
@@ -542,7 +574,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// compute lengths and Jacobians of edges
for (int f=0; f < m->nflex; f++) {
// skip if edges cannot generate forces
if (m->flex_rigid[f]) {
if (m->flex_rigid[f] || m->flex_interp[f]) {
continue;
}
src/engine/engine_io.c
+4 -3
View File
@@ -460,7 +460,7 @@ static void freeModelBuffers(mjModel* m) {
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh,
int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
int nlight, int nflex, int nflexvert, int nflexedge, int nflexelem,
int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem,
int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
@@ -503,6 +503,7 @@ void mj_makeModel(mjModel** dest,
m->ncam = ncam;
m->nlight = nlight;
m->nflex = nflex;
m->nflexnode = nflexnode;
m->nflexvert = nflexvert;
m->nflexedge = nflexedge;
m->nflexelem = nflexelem;
@@ -634,7 +635,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
mj_makeModel(&dest,
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
@@ -828,7 +829,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63], ints[64]);
ints[63], ints[64], ints[65]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_warning("Corrupted model, wrong size parameters");
mj_deleteModel(m);
src/engine/engine_io.h
+1 -1
View File
@@ -53,7 +53,7 @@ void mj_defaultStatistic(mjStatistic* stat);
// allocate mjModel
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert,
int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata,
int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal,
int nmeshtexcoord, int nmeshface, int nmeshgraph, int nskin, int nskinvert, int nskintexvert,
src/engine/engine_passive.c
+82
View File
@@ -122,6 +122,88 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
continue;
}
if (m->flex_interp[f]) {
mjtNum xpos[mjMAXFLEXNODES], displ[mjMAXFLEXNODES], vel[mjMAXFLEXNODES];
mjtNum frc[mjMAXFLEXNODES], dmp[mjMAXFLEXNODES];
mjtNum com[3] = {0};
mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int nstart = m->flex_nodeadr[f];
// compute positions
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
}
} else {
mjtNum screw[6];
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0);
mju_copy3(vel + 3*i, screw + 3);
}
}
// compute center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addToScl3(com, xpos+3*i, 1.0/m->flex_nodenum[f]);
}
// re-center positions using center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addToScl3(xpos+3*i, com, -1);
}
// compute the Jacobian at the center of mass
mjtNum mat[9] = {0};
mjtNum p[3] = {.5, .5, .5};
mju_defGradient(mat, p, xpos, 1);
// find rotation
mjtNum quat[4] = {1, 0, 0, 0};
mju_mat2Rot(quat, mat);
mju_negQuat(quat, quat);
// rotate vertices to quat and add reference center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_rotVecQuat(xpos+3*i, xpos+3*i, quat);
mju_addTo3(xpos+3*i, p);
mju_rotVecQuat(vel+3*i, vel+3*i, quat);
}
// compute displacement
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1);
}
// compute force in the stretch frame
mju_mulMatVec(frc, k, displ, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
// compute damping force in stretch frame
mju_mulMatVec(dmp, k, vel, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
// rotate forces to global frame and add to qfrc
mju_negQuat(quat, quat);
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mjtNum qfrc[3], qdmp[3];
mju_rotVecQuat(qfrc, frc+3*i, quat);
mju_rotVecQuat(qdmp, dmp+3*i, quat);
mju_scl3(qdmp, qdmp, m->flex_damping[f]);
if (m->flex_centered[f]) {
mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
mju_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
} else {
mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
}
}
// do not continue with the rest of the flex passive forces
continue;
}
int nedge = (dim == 2) ? 3 : 6;
int nvert = (dim == 2) ? 3 : 4;
const int* elem = m->flex_elem + m->flex_elemdataadr[f];
src/engine/engine_setconst.c
+13
View File
@@ -213,6 +213,10 @@ static void set0(mjModel* m, mjData* d) {
if (nv) {
// compute flexedge_invweight0
for (int f=0; f < m->nflex; f++) {
if (m->flex_interp[f]) {
continue;
}
for (int i=m->flex_edgeadr[f]; i < m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
// bodies connected by edge
int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
@@ -498,13 +502,22 @@ static void setStat(mjModel* m, mjData* d) {
// adjust body size for flex edges involving body
for (int f=0; f < m->nflex; f++) {
mjtNum meanedge = 0;
for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
meanedge += m->flexedge_length0[e] / m->flex_edgenum[f];
if (m->flex_interp[f]) {
continue;
}
int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]];
body[b1] = mju_max(body[b1], m->flexedge_length0[e]);
body[b2] = mju_max(body[b2], m->flexedge_length0[e]);
}
for (int v=m->flex_nodeadr[f]; v < m->flex_nodeadr[f]+m->flex_node[f]; v++) {
body[m->flex_nodebodyid[v]] = mju_max(body[m->flex_nodebodyid[v]], meanedge);
}
}
// compute meansize, make sure all sizes are above min
src/engine/engine_util_misc.c
+45
View File
@@ -452,6 +452,51 @@ void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) {
// ----------------------------- Flex interpolation ------------------------------------------------
mjtNum static inline phi(mjtNum s, int i) {
if (i == 0) {
return 1-s;
} else {
return s;
}
}
mjtNum static inline dphi(mjtNum s, int i) {
if (i == 0) {
return -1;
} else {
return 1;
}
}
// evaluate the deformation gradient at p using the nodal dof values
void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order) {
mjtNum gradient[3];
mju_zero(res, 9);
for (int i = 0; i <= order; i++) {
for (int j = 0; j <= order; j++) {
for (int k = 0; k <= order; k++) {
int idx = 4*i + 2*j + k;
gradient[0] = dphi(p[0], i) * phi(p[1], j) * phi(p[2], k);
gradient[1] = phi(p[0], i) * dphi(p[1], j) * phi(p[2], k);
gradient[2] = phi(p[0], i) * phi(p[1], j) * dphi(p[2], k);
res[0] += dof[3*idx+0] * gradient[0];
res[1] += dof[3*idx+0] * gradient[1];
res[2] += dof[3*idx+0] * gradient[2];
res[3] += dof[3*idx+1] * gradient[0];
res[4] += dof[3*idx+1] * gradient[1];
res[5] += dof[3*idx+1] * gradient[2];
res[6] += dof[3*idx+2] * gradient[0];
res[7] += dof[3*idx+2] * gradient[1];
res[8] += dof[3*idx+2] * gradient[2];
}
}
}
}
//------------------------------ actuator models ---------------------------------------------------
// normalized muscle length-gain curve
src/engine/engine_util_misc.h
+5
View File
@@ -53,6 +53,11 @@ MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
// all 3 semi-axes of a geom
MJAPI void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]);
// ----------------------------- Flex interpolation ------------------------------------------------
// evaluate the deformation gradient at p using the nodal dof values
MJAPI void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order);
// ----------------------------- Base64 -----------------------------------------------------------
// encode data as Base64 into buf (including padding and null char)
src/engine/engine_vis_interact.c
+17 -2
View File
@@ -828,9 +828,24 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// update if closer intersection found
if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {
flexdist = newdist;
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
if (m->flex_interp[i]) {
mjtNum* vert0 = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid);
int l = vert0[0] > 0.5 ? 1 : 0;
int j = vert0[1] > 0.5 ? 1 : 0;
int k = vert0[2] > 0.5 ? 1 : 0;
int nodeid = 4*l+2*j+k;
flexbodyid = m->flex_nodebodyid[m->flex_nodeadr[i] + nodeid];
if (m->flex_centered[i]) {
mju_copy3(flexpnt, d->xpos + 3*flexbodyid);
} else {
mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*nodeid);
mju_addTo3(flexpnt, d->xpos + 3*flexbodyid);
}
} else {
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
}
*flexid = i;
mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
}
}
}
src/engine/engine_vis_visualize.c
+41
View File
@@ -713,6 +713,47 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
FINISH
}
}
if (!m->flex_interp[f]) {
continue;
}
// control points box
mjtNum xpos[mjMAXFLEXNODES];
int nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
} else {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
}
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (scn->ngeom >= scn->maxgeom) break;
if (i == 0) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
FINISH
}
if (j == 0) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
FINISH
}
if (k == 0) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
FINISH
}
}
}
}
}
}