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
}
}
}
}
}
}
src/user/user_flexcomp.cc
+65 -12
View File
@@ -86,6 +86,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false;
centered = false;
doftype = mjFCOMPDOF_FULL;
mjs_defaultPlugin(&plugin);
mjs_defaultOrientation(&alt);
@@ -102,10 +103,6 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
mjCModel* model = static_cast<mjCBody*>(body->element)->model;
mjsCompiler* compiler = static_cast<mjCBody*>(body->element)->compiler;
mjsFlex* dflex = def.spec.flex;
bool radial = (type == mjFCOMPTYPE_BOX ||
type == mjFCOMPTYPE_CYLINDER ||
type == mjFCOMPTYPE_ELLIPSOID);
bool direct = (type == mjFCOMPTYPE_DIRECT ||
type == mjFCOMPTYPE_MESH ||
type == mjFCOMPTYPE_GMSH);
@@ -122,7 +119,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
// check counts
for (int i=0; i < 3; i++) {
if (count[i] < 1 || ((radial && count[i] < 2) && dflex->dim == 3)) {
if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) {
return comperr(error, "Count too small", error_sz);
}
}
@@ -260,6 +257,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
point[3*i+2] = newp[2];
}
// compute bounding box of points
double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
for (int i=0; i < npnt; i++) {
for (int j=0; j < 3; j++) {
minmax[j+0] = std::min(minmax[j+0], point[3*i+j]);
minmax[j+3] = std::max(minmax[j+3], point[3*i+j]);
}
}
// construct pinned array
pinned = vector<bool>(npnt, rigid);
@@ -337,7 +343,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// center of radial body is always pinned
if (radial) {
if (doftype == mjFCOMPDOF_RADIAL) {
pinned[0] = true;
}
@@ -434,8 +440,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
continue;
}
// pinned: parent body
if (pinned[i]) {
// pinned or trilinear: parent body
if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR) {
mjs_appendString(pf->vertbody, mjs_getString(body->name));
// add plugin
@@ -448,7 +454,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
// not pinned: new body
// not pinned and not trilinear: new body
else {
// add new body at vertex coordinates
mjsBody* pb = mjs_addBody(body, 0);
@@ -465,7 +471,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
pb->explicitinertial = true;
// add radial slider
if (radial) {
if (doftype == mjFCOMPDOF_RADIAL) {
mjsJoint* jnt = mjs_addJoint(pb, 0);
// set properties
@@ -476,7 +482,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// add three orthogonal sliders
else {
else if (doftype == mjFCOMPDOF_FULL) {
for (int j=0; j < 3; j++) {
// add joint to body
mjsJoint* jnt = mjs_addJoint(pb, 0);
@@ -513,7 +519,54 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
if (!centered) {
// create nodal mesh for trilinear interpolation
if (doftype == mjFCOMPDOF_TRILINEAR) {
std::vector<double> node(24, 0);
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (pinned[i*4+j*2+k]) {
node[3*(i*4+j*2+k)+0] = i == 0 ? minmax[0] : minmax[3];
node[3*(i*4+j*2+k)+1] = j == 0 ? minmax[1] : minmax[4];
node[3*(i*4+j*2+k)+2] = k == 0 ? minmax[2] : minmax[5];
mjs_appendString(pf->nodebody, mjs_getString(body->name));
continue;
}
mjsBody* pb = mjs_addBody(body, 0);
pb->pos[0] = i == 0 ? minmax[0] : minmax[3];
pb->pos[1] = j == 0 ? minmax[1] : minmax[4];
pb->pos[2] = k == 0 ? minmax[2] : minmax[5];
mjuu_zerovec(pb->ipos, 3);
pb->mass = mass / 8;
pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->explicitinertial = true;
for (int d=0; d < 3; d++) {
mjsJoint* jnt = mjs_addJoint(pb, 0);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[d] = 1;
}
// construct node name, add to nodebody
char txt[100];
mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k);
mjs_setString(pb->name, txt);
mjs_appendString(pf->nodebody, mjs_getString(pb->name));
}
}
}
if (!centered) {
mjs_setDouble(pf->node, node.data(), node.size());
}
}
if (!centered || doftype == mjFCOMPDOF_TRILINEAR) {
mjs_setDouble(pf->vert, point.data(), point.size());
}
src/user/user_flexcomp.h
+10
View File
@@ -40,6 +40,15 @@ typedef enum _mjtFcompType {
} mjtFcompType;
typedef enum _mjtDof {
mjFCOMPDOF_FULL = 0,
mjFCOMPDOF_RADIAL,
mjFCOMPDOF_TRILINEAR,
mjNFCOMPDOFS
} mjtDof;
class mjCFlexcomp {
public:
mjCFlexcomp(void);
@@ -73,6 +82,7 @@ class mjCFlexcomp {
double inertiabox; // size of inertia box for each body
bool equality; // create edge equality constraint
std::string file; // mesh/gmsh file name
mjtDof doftype; // dof type, all vertices or trilinear interpolation
// pin specifications
std::vector<int> pinid; // ids of points to pin
src/user/user_mesh.cc
+237 -6
View File
@@ -13,6 +13,7 @@
// limitations under the License.
#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <csetjmp>
@@ -2402,7 +2403,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
//--------------------- elasticity implementation --------------------------------------------------
//-------------------------- nonlinear elasticity --------------------------------------------------
// hash function for std::pair
struct PairHash
@@ -2628,6 +2629,153 @@ void inline ComputeStiffness(std::vector<double>& stiffness,
MetricTensor<T>(stiffness.data(), t, mu, la, basis);
}
//----------------------------- linear elasticity --------------------------------------------------
// Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
void quadratureGaussLegendre(double* points, double* weights,
const int order, const double a, const double b) {
if (order > 2)
mju_error("Integration order > 2 not yet supported.");
// x is on [-1, 1], p on [a, b]
double p0 = (a+b)/2.;
double dpdx = (b-a)/2;
points[0] = -dpdx/sqrt(3) + p0;
points[1] = dpdx/sqrt(3) + p0;
weights[0] = dpdx;
weights[1] = dpdx;
}
// evaluate 1-dimensional basis function
double phi(const double s, const double component) {
if (component == 0) {
return 1-s;
} else {
return s;
}
}
// evaluate gradient fo 1-dimensional basis function
double dphi(const double s, const double component) {
if (component == 0) {
return -1;
} else {
return 1;
}
}
typedef std::array<std::array<double, 3>, 3> Matrix;
// symmetrize a tensor
Matrix inline sym(const Matrix& tensor) {
Matrix eps;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
eps[i][j] = (tensor[i][j] + tensor[j][i]) / 2;
}
}
return eps;
}
// compute tensor inner product
Matrix inline inner(const Matrix& tensor1, const Matrix& tensor2) {
Matrix inner;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
inner[i][j] = tensor1[i][0] * tensor2[0][j] +
tensor1[i][1] * tensor2[1][j] +
tensor1[i][2] * tensor2[2][j];
}
}
return inner;
}
// compute trace of a tensor
double inline trace(const Matrix& tensor) {
return tensor[0][0] + tensor[1][1] + tensor[2][2];
}
void inline ComputeLinearStiffness(std::vector<double>& K,
const double* pos,
double E, double nu) {
// only linear elements are supported for now
int order = 2;
int n = std::pow(order, 3);
int ndof = 3*n;
// compute quadrature points
std::vector<double> points(order); // quadrature points
std::vector<double> weight(order); // quadrature weights
quadratureGaussLegendre(points.data(), weight.data(), order, 0, 1);
// compute element transformation
double dx = (pos+12)[0] - pos[0];
double dy = (pos+ 6)[1] - pos[1];
double dz = (pos+ 3)[2] - pos[2];
double detJ = dx * dy * dz;
double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
// compute stiffness matrix
std::vector<std::array<double, 3>> F(n);
double la = E * nu / (1 + nu) / (1 - 2 * nu);
double mu = E / (2 * (1 + nu));
// loop over quadrature points
for (int ps=0; ps < order; ps++) {
for (int pt=0; pt < order; pt++) {
for (int pu=0; pu < order; pu++) {
double s = points[ps];
double t = points[pt];
double u = points[pu];
double dvol = weight[ps] * weight[pt] * weight[pu] * detJ;
int dof = 0;
// cartesian product of basis functions
for (int bx=0; bx < order; bx++) {
for (int by=0; by < order; by++) {
for (int bz=0; bz < order; bz++) {
std::array<double, 3> gradient;
gradient[0] = dphi(s, bx) * phi(t, by) * phi(u, bz);
gradient[1] = phi(s, bx) * dphi(t, by) * phi(u, bz);
gradient[2] = phi(s, bx) * phi(t, by) * dphi(u, bz);
F[dof++] = gradient;
}
}
}
if (dof != n) { // SHOULD NOT OCCUR
throw mjCError(NULL, "incorrect number of basis functions");
}
// tensor contraction of the gradients of elastic strains
// (d(F+F')/dx : d(F+F')/dx)
for (int i=0; i < n; i++) {
for (int j=0; j < n; j++) {
Matrix du;
Matrix dv;
du.fill({0, 0, 0});
dv.fill({0, 0, 0});
for (int k=0; k < 3; k++) {
for (int l=0; l < 3; l++) {
du[k][0] = invJ[0] * F[i][0];
du[k][1] = invJ[1] * F[i][1];
du[k][2] = invJ[2] * F[i][2];
dv[l][0] = invJ[0] * F[j][0];
dv[l][1] = invJ[1] * F[j][1];
dv[l][2] = invJ[2] * F[j][2];
K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol;
K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
mjuu_zerovec(du[k].data(), 3);
mjuu_zerovec(dv[l].data(), 3);
}
}
}
}
}
}
}
}
//------------------ class mjCFlex implementation --------------------------------------------------
// constructor
@@ -2641,6 +2789,7 @@ mjCFlex::mjCFlex(mjCModel* _model) {
// clear internal variables
nvert = 0;
nnode = 0;
nedge = 0;
nelem = 0;
matid = -1;
@@ -2673,13 +2822,17 @@ void mjCFlex::PointToLocal() {
spec.name = &name;
spec.material = &spec_material_;
spec.vertbody = &spec_vertbody_;
spec.nodebody = &spec_nodebody_;
spec.vert = &spec_vert_;
spec.node = &spec_node_;
spec.texcoord = &spec_texcoord_;
spec.elem = &spec_elem_;
spec.info = &info;
material = nullptr;
vertbody = nullptr;
nodebody = nullptr;
vert = nullptr;
node = nullptr;
texcoord = nullptr;
elem = nullptr;
}
@@ -2690,6 +2843,9 @@ void mjCFlex::NameSpace(const mjCModel* m) {
for (auto& name : spec_vertbody_) {
name = m->prefix + name + m->suffix;
}
for (auto& name : spec_nodebody_) {
name = m->prefix + name + m->suffix;
}
}
@@ -2699,7 +2855,9 @@ void mjCFlex::CopyFromSpec() {
spec.info = &info;
material_ = spec_material_;
vertbody_ = spec_vertbody_;
nodebody_ = spec_nodebody_;
vert_ = spec_vert_;
node_ = spec_node_;
texcoord_ = spec_texcoord_;
elem_ = spec_elem_;
@@ -2722,6 +2880,8 @@ void mjCFlex::DelTexcoord() {
void mjCFlex::ResolveReferences(const mjCModel* m) {
vertbodyid.clear();
nodebodyid.clear();
for (const auto& vertbody : vertbody_) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, vertbody);
if (pbody) {
@@ -2730,12 +2890,21 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
}
}
for (const auto& nodebody : nodebody_) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, nodebody);
if (pbody) {
nodebodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unknown body '%s' in flex", nodebody.c_str());
}
}
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
interpolated = !nodebody_.empty();
// set nelem; check sizes
if (dim<1 || dim>3) {
@@ -2747,8 +2916,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (elem_.size() % (dim+1)) {
throw mjCError(this, "elem size must be multiple of (dim+1)");
}
if (vertbody_.empty()) {
throw mjCError(this, "vertbody is empty");
if (vertbody_.empty() && !interpolated) {
throw mjCError(this, "vertbody and nodebody are both empty");
}
if (vert_.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
@@ -2756,6 +2925,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (edgestiffness>0 && dim>1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
}
if (interpolated && selfcollide != mjFLEXSELF_NONE) {
throw mjCError(this, "trilinear interpolation cannot do self-collision");
}
if (interpolated && internal) {
throw mjCError(this, "trilinear interpolation cannot do internal collisions");
}
nelem = (int)elem_.size()/(dim+1);
// set nvert, rigid, centered; check size
@@ -2773,6 +2948,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
throw mjCError(this, "not enough vertices");
}
// set nnode
nnode = (int)nodebody_.size();
if (nnode && nnode!=8) {
throw mjCError(this, "number of nodes must be 2^dim, it is %d", "", nnode);
}
// check elem vertex ids
for (const auto& elem : elem_) {
if (elem<0 || elem>=nvert) {
@@ -2812,7 +2993,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// determine rigid if not already set
if (!rigid) {
if (!rigid && !interpolated) {
rigid = true;
for (unsigned i=1; i < vertbodyid.size(); i++) {
if (vertbodyid[i]!=vertbodyid[0]) {
@@ -2823,7 +3004,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// determine centered if not already set
if (!centered) {
if (!centered && !interpolated) {
centered = true;
for (const auto& vert : vert_) {
if (vert!=0) {
@@ -2833,6 +3014,16 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
}
if (!centered && interpolated) {
centered = true;
for (const auto& node : node_) {
if (node!=0) {
centered = false;
break;
}
}
}
// compute global vertex positions
vertxpos = std::vector<double> (3*nvert);
for (int i=0; i < nvert; i++) {
@@ -2841,11 +3032,32 @@ void mjCFlex::Compile(const mjVFS* vfs) {
mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add vertex offset within body if not centered
if (!centered) {
if (!centered || interpolated) {
double offset[3];
mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(vertxpos.data()+3*i, offset, 3);
}
if (interpolated) {
// this should happen in ResolveReferences but we need a body id in this loop to compute
// the global vertex position, this is a hack since it is the id of the parent body
vertbodyid[i] = -1;
}
}
// compute global node positions
std::vector<double> nodexpos = std::vector<double> (3*nnode);
for (int i=0; i < nnode; i++) {
// get body id, set nodexpos = body.xpos0
int b = nodebodyid[i];
mjuu_copyvec(nodexpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add node offset within body if not centered
if (!centered) {
double offset[3];
mjuu_rotVecQuat(offset, node_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(nodexpos.data()+3*i, offset, 3);
}
}
// reorder tetrahedra so right-handed face orientation is outside
@@ -2909,7 +3121,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
}
stiffness.assign(21*nelem, 0);
if (interpolated) {
int min_size = ceil(nodexpos.size()*nodexpos.size() / 21);
if (min_size > nelem) {
throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size);
}
ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
}
for (unsigned int t = 0; t < nelem; t++) {
if (interpolated) {
continue;
}
if (dim==2) {
ComputeStiffness<Stencil2D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young,
@@ -2928,6 +3150,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
useredge = VectorToString(edgeidx_);
for (const auto& vbodyid : vertbodyid) {
if (vbodyid < 0) {
continue;
}
if (model->Bodies()[vbodyid]->plugin.element) {
mjCPlugin* plugin_instance =
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
@@ -2954,6 +3179,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / size + 0.5;
}
}
// store node cartesian positions
node0_.assign(3*nnode, 0);
for (int i=0; i < nnode; i++) {
mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3);
}
}
src/user/user_model.cc
+34 -4
View File
@@ -823,6 +823,7 @@ void mjCModel::Clear() {
nv = 0;
nu = 0;
na = 0;
nflexnode = 0;
nflexvert = 0;
nflexedge = 0;
nflexelem = 0;
@@ -1733,6 +1734,7 @@ void mjCModel::SetSizes() {
// flex counts
for (int i=0; i<nflex; i++) {
nflexnode += flexes_[i]->nnode;
nflexvert += flexes_[i]->nvert;
nflexedge += flexes_[i]->nedge;
nflexelem += flexes_[i]->nelem;
@@ -2746,7 +2748,7 @@ int mjCModel::CountNJmom(const mjModel* m) {
// copy objects outside kinematic tree
void mjCModel::CopyObjects(mjModel* m) {
int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr;
int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr;
int bonevert_adr, graph_adr, data_adr, bvh_adr;
@@ -2826,6 +2828,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// flexes
vert_adr = 0;
node_adr = 0;
edge_adr = 0;
elem_adr = 0;
elemdata_adr = 0;
@@ -2865,6 +2868,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_dim[i] = pfl->dim;
m->flex_vertadr[i] = vert_adr;
m->flex_vertnum[i] = pfl->nvert;
m->flex_nodeadr[i] = node_adr;
m->flex_nodenum[i] = pfl->nnode;
m->flex_edgeadr[i] = edge_adr;
m->flex_edgenum[i] = pfl->nedge;
m->flex_elemadr[i] = elem_adr;
@@ -2924,16 +2929,27 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// copy or set vert
if (pfl->centered) {
if (pfl->centered && !pfl->interpolated) {
mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
}
else {
mjuu_copyvec(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert);
}
// copy or set node
if (pfl->centered && pfl->interpolated) {
mjuu_zerovec(m->flex_node + 3*node_adr, 3*pfl->nnode);
}
else if (pfl->interpolated) {
mjuu_copyvec(m->flex_node + 3*node_adr, pfl->node_.data(), 3*pfl->nnode);
}
// copy vert0
mjuu_copyvec(m->flex_vert0 + 3*vert_adr, pfl->vert0_.data(), 3*pfl->nvert);
// copy node0
mjuu_copyvec(m->flex_node0 + 3*node_adr, pfl->node0_.data(), 3*pfl->nnode);
// copy or set vertbodyid
if (pfl->rigid) {
for (int k=0; k<pfl->nvert; k++) {
@@ -2944,6 +2960,18 @@ void mjCModel::CopyObjects(mjModel* m) {
memcpy(m->flex_vertbodyid + vert_adr, pfl->vertbodyid.data(), pfl->nvert*sizeof(int));
}
// copy or set nodebodyid
if (pfl->rigid) {
for (int k=0; k<pfl->nnode; k++) {
m->flex_nodebodyid[node_adr + k] = pfl->nodebodyid[0];
}
} else {
memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int));
}
// set interpolation type, only two types for now
m->flex_interp[i] = pfl->interpolated;
// convert edge pairs to int array, set edge rigid
for (int k=0; k<pfl->nedge; k++) {
m->flex_edge[2*(edge_adr+k)] = pfl->edge[k].first;
@@ -2951,8 +2979,9 @@ void mjCModel::CopyObjects(mjModel* m) {
if (pfl->rigid) {
m->flexedge_rigid[edge_adr+k] = 1;
} else {
} else if (!pfl->interpolated) {
// check if vertex body weldids are the same
// unsupported by trilinear interpolation
int b1 = pfl->vertbodyid[pfl->edge[k].first];
int b2 = pfl->vertbodyid[pfl->edge[k].second];
m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid);
@@ -2961,6 +2990,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// advance counters
vert_adr += pfl->nvert;
node_adr += pfl->nnode;
edge_adr += pfl->nedge;
elem_adr += pfl->nelem;
elemdata_adr += (pfl->dim+1) * pfl->nelem;
@@ -4317,7 +4347,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// create low-level model
mj_makeModel(&m,
nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
ncam, nlight, nflex, nflexvert, nflexedge, nflexelem,
ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
src/user/user_model.h
+1
View File
@@ -86,6 +86,7 @@ class mjCModel_ : public mjsElement {
int nbvh; // number of total boundary volume hierarchies
int nbvhstatic; // number of static boundary volume hierarchies
int nbvhdynamic; // number of dynamic boundary volume hierarchies
int nflexnode; // number of nodes in all flexes
int nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
int nflexelem; // number of elements in all flexes
src/user/user_objects.h
+9
View File
@@ -746,12 +746,15 @@ class mjCLight : public mjCLight_, private mjsLight {
class mjCFlex_ : public mjCBase {
protected:
int nvert; // number of verices
int nnode; // number of nodes
int nedge; // number of edges
int nelem; // number of elements
int matid; // material id
bool rigid; // all vertices attached to the same body
bool centered; // all vertices coordinates (0,0,0)
bool interpolated; // vertices are interpolated from nodes
std::vector<int> vertbodyid; // vertex body ids
std::vector<int> nodebodyid; // node body ids
std::vector<std::pair<int, int>> edge; // edge vertex ids
std::vector<int> shell; // shell fragment vertex ids (dim per fragment)
std::vector<int> elemlayer; // element layer (distance from border)
@@ -764,14 +767,18 @@ class mjCFlex_ : public mjCBase {
// variable-size data
std::vector<std::string> vertbody_; // vertex body names
std::vector<std::string> nodebody_; // node body names
std::vector<double> vert_; // vertex positions
std::vector<double> node_; // node positions
std::vector<int> elem_; // element vertex ids
std::vector<float> texcoord_; // vertex texture coordinates
std::string material_; // name of material used for rendering
std::string spec_material_;
std::vector<std::string> spec_vertbody_;
std::vector<std::string> spec_nodebody_;
std::vector<double> spec_vert_;
std::vector<double> spec_node_;
std::vector<int> spec_elem_;
std::vector<float> spec_texcoord_;
};
@@ -804,6 +811,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
const std::vector<double>& get_elemaabb() const { return elemaabb_; }
const std::vector<int>& get_elem() const { return elem_; }
const std::vector<float>& get_texcoord() const { return texcoord_; }
const std::vector<std::string>& get_nodebody() const { return nodebody_; }
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
@@ -816,6 +824,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
void CreateShellPair(void); // create shells and evpairs
std::vector<double> vert0_; // vertex positions in [0, 1]^d in the bounding box
std::vector<double> node0_; // node Cartesian positions
};
src/xml/xml_native_reader.cc
+20 -4
View File
@@ -311,7 +311,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"flexcomp", "*", "25", "name", "type", "group", "dim",
{"flexcomp", "*", "26", "name", "type", "group", "dim", "dof",
"count", "spacing", "radius", "rigid", "mass", "inertiabox",
"scale", "file", "point", "element", "texcoord", "material", "rgba",
"flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"},
@@ -331,8 +331,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"deformable", "*", "0"},
{"<"},
{"flex", "*", "11", "name", "group", "dim", "radius", "material",
"rgba", "flatskin", "body", "vertex", "element", "texcoord"},
{"flex", "*", "12", "name", "group", "dim", "radius", "material",
"rgba", "flatskin", "body", "vertex", "element", "texcoord", "node"},
{"<"},
{"contact", "?", "13", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap",
@@ -803,6 +803,14 @@ const mjMap fcomp_map[mjNFCOMPTYPES] = {
};
// flexcomp dof type
const mjMap fdof_map[mjNFCOMPDOFS] = {
{"full", mjFCOMPDOF_FULL},
{"radial", mjFCOMPDOF_RADIAL},
{"trilinear", mjFCOMPDOF_TRILINEAR}
};
// flex selfcollide type
const mjMap flexself_map[5] = {
{"none", mjFLEXSELF_NONE},
@@ -1324,7 +1332,7 @@ void mjXReader::Statistic(XMLElement* section) {
// flex element parser
void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
string text, name, material;
string text, name, material, nodebody;
int n;
// read attributes
@@ -1347,6 +1355,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
if (ReadAttrTxt(elem, "body", text, true)) {
mjs_setStringVec(flex->vertbody, text.c_str());
}
if (ReadAttrTxt(elem, "node", nodebody)) {
mjs_setStringVec(flex->nodebody, nodebody.c_str());
}
auto vert = ReadAttrVec<double>(elem, "vertex");
if (vert.has_value()) {
mjs_setDouble(flex->vert, vert->data(), vert->size());
@@ -2678,6 +2689,11 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
fcomp.texcoord = std::move(texcoord.value());
}
// dof type
if (MapValue(elem, "dof", &n, fdof_map, mjNFCOMPDOFS)) {
fcomp.doftype = (mjtDof)n;
}
// edge
XMLElement* edge = FirstChildElement(elem, "edge");
if (edge) {
src/xml/xml_native_writer.cc
+4
View File
@@ -158,6 +158,10 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
text = VectorToString(flex->get_texcoord());
WriteAttrTxt(elem, "texcoord", text);
}
if (!flex->get_nodebody().empty()) {
text = VectorToString(flex->get_nodebody());
WriteAttrTxt(elem, "node", text);
}
// contact subelement
XMLElement* cont = InsertEnd(elem, "contact");