Refactor flex passive forces into dedicated functions.
The code for computing passive forces for flex elements is moved into new static functions `mj_flexPassiveInterp`, `mj_flexPassiveBend`, and `mj_flexPassiveStretch`. This improves the structure of `mj_springdamper`. PiperOrigin-RevId: 907671516 Change-Id: I6d1e781bbd370331ae645a954b78092ecb1925b9
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
e552a5f80d
commit
e71bd3db8e
+322
-302
@@ -59,6 +59,320 @@ static void inline GradSquaredLengths(mjtNum gradient[6][2][3],
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// passive forces for interpolated flex (stretch + bending)
|
||||||
|
static void mj_flexPassiveInterp(const mjModel* m, mjData* d, int f,
|
||||||
|
int enbl_spring, int enbl_damper) {
|
||||||
|
mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f];
|
||||||
|
int nodenum = m->flex_nodenum[f];
|
||||||
|
|
||||||
|
int order = m->flex_interp[f];
|
||||||
|
int shell_mode = order < 0;
|
||||||
|
order = order < 0 ? -order : order;
|
||||||
|
int cx = m->flex_cellnum[3*f+0];
|
||||||
|
int cy = m->flex_cellnum[3*f+1];
|
||||||
|
int cz = m->flex_cellnum[3*f+2];
|
||||||
|
|
||||||
|
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
|
||||||
|
int npe; // nodes per element
|
||||||
|
int nelem_fe; // total finite elements
|
||||||
|
|
||||||
|
if (shell_mode) {
|
||||||
|
npe = (order+1)*(order+1);
|
||||||
|
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
|
||||||
|
} else {
|
||||||
|
npe = (order+1)*(order+1)*(order+1);
|
||||||
|
nelem_fe = cx * cy * cz;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check if we have any work to do
|
||||||
|
int has_stretch = k[0] != 0 && m->flex_edgeequality[f] != 3;
|
||||||
|
if (!has_stretch) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
mj_markStack(d);
|
||||||
|
|
||||||
|
// allocate global arrays
|
||||||
|
mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||||
|
mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||||
|
mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||||
|
mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
||||||
|
mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
|
||||||
|
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
|
||||||
|
|
||||||
|
// gather global node positions and velocities (unrotated)
|
||||||
|
mju_flexGatherState(m, d, f, xpos_g, vel_g);
|
||||||
|
|
||||||
|
// zero global force accumulators
|
||||||
|
mju_zero(frc_g, 3*nodenum);
|
||||||
|
mju_zero(dmp_g, 3*nodenum);
|
||||||
|
|
||||||
|
// per-element arrays (sized for npe)
|
||||||
|
mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||||
|
mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||||
|
mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||||
|
mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||||
|
mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||||
|
mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||||
|
int* gindices = mjSTACKALLOC(d, npe, int);
|
||||||
|
|
||||||
|
// -------------------- stretch forces --------------------
|
||||||
|
if (has_stretch) {
|
||||||
|
for (int fe = 0; fe < nelem_fe; fe++) {
|
||||||
|
// get element stiffness matrix
|
||||||
|
mjtNum* k_elem = k + fe * 3*npe * 3*npe;
|
||||||
|
|
||||||
|
// skip empty elements (zero stiffness)
|
||||||
|
if (k_elem[0] == 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// gather element-local node data and compute corotational rotation
|
||||||
|
mjtNum quat[4];
|
||||||
|
if (shell_mode) {
|
||||||
|
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0,
|
||||||
|
xpos_e, vel_e, xpos0_e, gindices, quat);
|
||||||
|
} else {
|
||||||
|
int ci = fe / (cy * cz);
|
||||||
|
int cj = (fe / cz) % cy;
|
||||||
|
int ck = fe % cz;
|
||||||
|
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g,
|
||||||
|
xpos0, xpos_e, vel_e, xpos0_e, gindices,
|
||||||
|
quat);
|
||||||
|
}
|
||||||
|
|
||||||
|
// rotate to corotational frame
|
||||||
|
for (int n = 0; n < npe; n++) {
|
||||||
|
mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat);
|
||||||
|
mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat);
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute displacement
|
||||||
|
for (int n = 0; n < npe; n++) {
|
||||||
|
mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute force in corotational frame
|
||||||
|
if (enbl_spring) {
|
||||||
|
mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe);
|
||||||
|
}
|
||||||
|
if (enbl_damper) {
|
||||||
|
mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe);
|
||||||
|
}
|
||||||
|
|
||||||
|
// rotate back to global frame and scatter using node indices
|
||||||
|
mju_negQuat(quat, quat);
|
||||||
|
for (int n = 0; n < npe; n++) {
|
||||||
|
mjtNum qfrc[3], qdmp[3];
|
||||||
|
mji_rotVecQuat(qfrc, frc_e+3*n, quat);
|
||||||
|
mji_rotVecQuat(qdmp, dmp_e+3*n, quat);
|
||||||
|
int gidx = gindices[n];
|
||||||
|
if (enbl_spring) {
|
||||||
|
mji_addTo3(frc_g + 3*gidx, qfrc);
|
||||||
|
}
|
||||||
|
if (enbl_damper) {
|
||||||
|
mji_addTo3(dmp_g + 3*gidx, qdmp);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// apply accumulated forces to bodies
|
||||||
|
for (int i = 0; i < nodenum; i++) {
|
||||||
|
mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]);
|
||||||
|
int bid = bodyid[i];
|
||||||
|
int nidx = i + m->flex_nodeadr[f];
|
||||||
|
|
||||||
|
// fast path: node at body origin (not pinned), direct DOF write
|
||||||
|
if (m->body_dofnum[bid] > 0 &&
|
||||||
|
(m->flex_centered[f] ||
|
||||||
|
(m->flex_node[3*nidx+0] == 0 &&
|
||||||
|
m->flex_node[3*nidx+1] == 0 &&
|
||||||
|
m->flex_node[3*nidx+2] == 0))) {
|
||||||
|
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
|
||||||
|
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
|
||||||
|
} else {
|
||||||
|
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
|
||||||
|
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
mj_freeStack(d);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// passive forces for flex bending
|
||||||
|
static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f,
|
||||||
|
int enbl_spring, int enbl_damper) {
|
||||||
|
if (m->flex_dim[f] != 2) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int edgenum = m->flex_edgenum[f];
|
||||||
|
mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
|
||||||
|
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
|
||||||
|
mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f];
|
||||||
|
|
||||||
|
for (int e = 0; e < edgenum; e++) {
|
||||||
|
const int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f]);
|
||||||
|
const int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f]);
|
||||||
|
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
|
||||||
|
if (v[3] == -1) {
|
||||||
|
// skip boundary edges
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// flap edges
|
||||||
|
mjtNum ed[3][3];
|
||||||
|
mji_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]);
|
||||||
|
mji_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]);
|
||||||
|
mji_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]);
|
||||||
|
|
||||||
|
// forces at the vertices due to curved reference
|
||||||
|
mjtNum frc[4][3];
|
||||||
|
mji_cross(frc[1], ed[1], ed[2]);
|
||||||
|
mji_cross(frc[2], ed[2], ed[0]);
|
||||||
|
mji_cross(frc[3], ed[0], ed[1]);
|
||||||
|
frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]);
|
||||||
|
frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]);
|
||||||
|
frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]);
|
||||||
|
|
||||||
|
// velocities
|
||||||
|
mjtNum* vel[4];
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
vel[i] = d->qvel + m->body_dofadr[bodyid[v[i]]];
|
||||||
|
}
|
||||||
|
|
||||||
|
// force
|
||||||
|
mjtNum spring[12] = {0};
|
||||||
|
mjtNum damper[12] = {0};
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
for (int x = 0; x < 3; x++) {
|
||||||
|
for (int j = 0; j < 4; j++) {
|
||||||
|
// thin plate bending force
|
||||||
|
if (enbl_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
|
||||||
|
|
||||||
|
// thin plate damping force
|
||||||
|
// TODO: do not assume DOFs are in the world frame
|
||||||
|
if (enbl_damper) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
|
||||||
|
}
|
||||||
|
|
||||||
|
// curved reference contribution
|
||||||
|
if (enbl_spring) spring[3*i+x] += b[17*e+16] * frc[i][x];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// insert into global force
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
int bid = bodyid[v[i]];
|
||||||
|
int body_dofnum = m->body_dofnum[bid];
|
||||||
|
int body_dofadr = m->body_dofadr[bid];
|
||||||
|
for (int x = 0; x < body_dofnum; x++) {
|
||||||
|
if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
|
||||||
|
if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// passive forces for flex stretch
|
||||||
|
static void mj_flexPassiveStretch(const mjModel* m, mjData* d, int f,
|
||||||
|
int enbl_spring, int enbl_damper) {
|
||||||
|
mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f];
|
||||||
|
if (k[0] == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int dim = m->flex_dim[f];
|
||||||
|
int nedge = (dim == 2) ? 3 : 6;
|
||||||
|
int nvert = (dim == 2) ? 3 : 4;
|
||||||
|
const int* elem = m->flex_elem + m->flex_elemdataadr[f];
|
||||||
|
const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[f];
|
||||||
|
mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
|
||||||
|
mjtNum* vel = d->flexedge_velocity + m->flex_edgeadr[f];
|
||||||
|
mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f];
|
||||||
|
mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f];
|
||||||
|
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
|
||||||
|
mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0;
|
||||||
|
|
||||||
|
mj_markStack(d);
|
||||||
|
mjtNum* qfrc = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum);
|
||||||
|
mju_zero(qfrc, 3*m->flex_vertnum[f]);
|
||||||
|
|
||||||
|
// compute force element-by-element
|
||||||
|
int elemnum = m->flex_elemnum[f];
|
||||||
|
for (int t = 0; t < elemnum; t++) {
|
||||||
|
const int* vert = elem + (dim+1) * t;
|
||||||
|
|
||||||
|
// compute length gradient with respect to dofs
|
||||||
|
mjtNum gradient[6][2][3];
|
||||||
|
GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge);
|
||||||
|
|
||||||
|
// we add generalized Rayleigh damping as described in Section 5.2 of
|
||||||
|
// Kharevych et al., "Geometric, Variational Integrators for Computer
|
||||||
|
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
|
||||||
|
|
||||||
|
// extract elongation of edges belonging to this element
|
||||||
|
mjtNum elongation[6];
|
||||||
|
for (int e = 0; e < nedge; e++) {
|
||||||
|
int idx = edgeelem[t * nedge + e];
|
||||||
|
mjtNum previous = deformed[idx] - vel[idx] * m->opt.timestep;
|
||||||
|
elongation[e] = deformed[idx]*deformed[idx] - reference[idx]*reference[idx] +
|
||||||
|
(deformed[idx]*deformed[idx] - previous*previous) * kD;
|
||||||
|
}
|
||||||
|
|
||||||
|
// unpack triangular representation
|
||||||
|
mjtNum metric[36];
|
||||||
|
int id = 0;
|
||||||
|
for (int ed1 = 0; ed1 < nedge; ed1++) {
|
||||||
|
for (int ed2 = ed1; ed2 < nedge; ed2++) {
|
||||||
|
metric[nedge*ed1 + ed2] = k[21*t + id];
|
||||||
|
metric[nedge*ed2 + ed1] = k[21*t + id++];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute local force
|
||||||
|
mjtNum force[12] = {0};
|
||||||
|
for (int ed1 = 0; ed1 < nedge; ed1++) {
|
||||||
|
for (int ed2 = 0; ed2 < nedge; ed2++) {
|
||||||
|
for (int i = 0; i < 2; i++) {
|
||||||
|
for (int x = 0; x < 3; x++) {
|
||||||
|
force[3 * edges[dim-2][ed2][i] + x] -=
|
||||||
|
elongation[ed1] * gradient[ed2][i][x] *
|
||||||
|
metric[nedge * ed1 + ed2];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// insert into global force
|
||||||
|
for (int i = 0; i < nvert; i++) {
|
||||||
|
for (int x = 0; x < 3; x++) {
|
||||||
|
qfrc[3*vert[i]+x] += force[3*i+x];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// insert force into qfrc_passive, straightforward for simple bodies,
|
||||||
|
// need to distribute the force in case of pinned vertices
|
||||||
|
for (int v = 0; v < m->flex_vertnum[f]; v++) {
|
||||||
|
int bid = bodyid[v];
|
||||||
|
if (m->body_simple[bid] != 2) {
|
||||||
|
// this should only occur for pinned flex vertices
|
||||||
|
mj_applyFT(m, d, qfrc + 3*v, 0, xpos + 3*v, bid, d->qfrc_spring);
|
||||||
|
} else {
|
||||||
|
int body_dofnum = m->body_dofnum[bid];
|
||||||
|
int body_dofadr = m->body_dofadr[bid];
|
||||||
|
for (int x = 0; x < body_dofnum; x++) {
|
||||||
|
d->qfrc_spring[body_dofadr+x] += qfrc[3*v+x];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
mj_freeStack(d);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// spring and damper forces
|
// spring and damper forces
|
||||||
static void mj_springdamper(const mjModel* m, mjData* d) {
|
static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||||
@@ -147,314 +461,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
// flex elasticity
|
// flex elasticity
|
||||||
for (int f=0; f < m->nflex; f++) {
|
for (int f=0; f < m->nflex; f++) {
|
||||||
mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f];
|
if (m->flex_dim[f] == 1 || m->flex_rigid[f]) {
|
||||||
mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f];
|
|
||||||
int dim = m->flex_dim[f];
|
|
||||||
int nodenum = m->flex_nodenum[f];
|
|
||||||
int edgenum = m->flex_edgenum[f];
|
|
||||||
int vertnum = m->flex_vertnum[f];
|
|
||||||
|
|
||||||
if (dim == 1 || m->flex_rigid[f]) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// add bending forces to qfrc_spring
|
|
||||||
if (dim == 2) {
|
|
||||||
mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
|
|
||||||
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
|
|
||||||
|
|
||||||
for (int e = 0; e < edgenum; e++) {
|
|
||||||
const int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f]);
|
|
||||||
const int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f]);
|
|
||||||
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
|
|
||||||
if (v[3] == -1) {
|
|
||||||
// skip boundary edges
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// flap edges
|
|
||||||
mjtNum ed[3][3];
|
|
||||||
mji_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]);
|
|
||||||
mji_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]);
|
|
||||||
mji_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]);
|
|
||||||
|
|
||||||
// forces at the vertices due to curved reference
|
|
||||||
mjtNum frc[4][3];
|
|
||||||
mji_cross(frc[1], ed[1], ed[2]);
|
|
||||||
mji_cross(frc[2], ed[2], ed[0]);
|
|
||||||
mji_cross(frc[3], ed[0], ed[1]);
|
|
||||||
frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]);
|
|
||||||
frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]);
|
|
||||||
frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]);
|
|
||||||
|
|
||||||
// velocities
|
|
||||||
mjtNum* vel[4];
|
|
||||||
for (int i = 0; i < 4; i++) {
|
|
||||||
vel[i] = d->qvel + m->body_dofadr[bodyid[v[i]]];
|
|
||||||
}
|
|
||||||
|
|
||||||
// force
|
|
||||||
mjtNum spring[12] = {0};
|
|
||||||
mjtNum damper[12] = {0};
|
|
||||||
for (int i = 0; i < 4; i++) {
|
|
||||||
for (int x = 0; x < 3; x++) {
|
|
||||||
for (int j = 0; j < 4; j++) {
|
|
||||||
// thin plate bending force
|
|
||||||
if (enbl_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
|
|
||||||
|
|
||||||
// thin plate damping force
|
|
||||||
// TODO: do not assume DOFs are in the world frame
|
|
||||||
if (enbl_damper) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
|
|
||||||
}
|
|
||||||
|
|
||||||
// curved reference contribution
|
|
||||||
if (enbl_spring) spring[3*i+x] += b[17*e+16] * frc[i][x];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// insert into global force
|
|
||||||
for (int i = 0; i < 4; i++) {
|
|
||||||
int bid = bodyid[v[i]];
|
|
||||||
int body_dofnum = m->body_dofnum[bid];
|
|
||||||
int body_dofadr = m->body_dofadr[bid];
|
|
||||||
for (int x = 0; x < body_dofnum; x++) {
|
|
||||||
if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
|
|
||||||
if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (k[0] == 0) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// skip interpolated flex with strain constraints (stiffness in constraint solver)
|
|
||||||
if (m->flex_edgeequality[f] == 3) {
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (m->flex_interp[f]) {
|
if (m->flex_interp[f]) {
|
||||||
int order = m->flex_interp[f];
|
// interpolated flex
|
||||||
int shell_mode = order < 0;
|
mj_flexPassiveInterp(m, d, f, enbl_spring, enbl_damper);
|
||||||
order = order < 0 ? -order : order;
|
} else {
|
||||||
int cx = m->flex_cellnum[3*f+0];
|
// add bending forces
|
||||||
int cy = m->flex_cellnum[3*f+1];
|
mj_flexPassiveBend(m, d, f, enbl_spring, enbl_damper);
|
||||||
int cz = m->flex_cellnum[3*f+2];
|
|
||||||
|
|
||||||
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
|
// stretch forces
|
||||||
int npe; // nodes per element
|
mj_flexPassiveStretch(m, d, f, enbl_spring, enbl_damper);
|
||||||
int nelem_fe; // total finite elements
|
|
||||||
|
|
||||||
if (shell_mode) {
|
|
||||||
npe = (order+1)*(order+1);
|
|
||||||
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
|
|
||||||
} else {
|
|
||||||
npe = (order+1)*(order+1)*(order+1);
|
|
||||||
nelem_fe = cx * cy * cz;
|
|
||||||
}
|
|
||||||
|
|
||||||
mj_markStack(d);
|
|
||||||
|
|
||||||
// allocate global arrays
|
|
||||||
mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
|
||||||
mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
|
||||||
mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
|
||||||
mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum);
|
|
||||||
mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
|
|
||||||
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
|
|
||||||
|
|
||||||
// gather global node positions and velocities (unrotated)
|
|
||||||
mju_flexGatherState(m, d, f, xpos_g, vel_g);
|
|
||||||
|
|
||||||
// zero global force accumulators
|
|
||||||
mju_zero(frc_g, 3*nodenum);
|
|
||||||
mju_zero(dmp_g, 3*nodenum);
|
|
||||||
|
|
||||||
// per-element arrays (sized for npe)
|
|
||||||
mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
|
||||||
mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
|
||||||
mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
|
||||||
mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
|
||||||
mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
|
||||||
mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
|
||||||
int* gindices = mjSTACKALLOC(d, npe, int);
|
|
||||||
|
|
||||||
// loop over finite elements
|
|
||||||
for (int fe = 0; fe < nelem_fe; fe++) {
|
|
||||||
// get element stiffness matrix
|
|
||||||
mjtNum* k_elem = k + fe * 3*npe * 3*npe;
|
|
||||||
|
|
||||||
// skip empty elements (zero stiffness)
|
|
||||||
if (k_elem[0] == 0) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// gather element-local node data and compute corotational rotation
|
|
||||||
mjtNum quat[4];
|
|
||||||
if (shell_mode) {
|
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0,
|
|
||||||
xpos_e, vel_e, xpos0_e, gindices, quat);
|
|
||||||
} else {
|
|
||||||
int ci = fe / (cy * cz);
|
|
||||||
int cj = (fe / cz) % cy;
|
|
||||||
int ck = fe % cz;
|
|
||||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g,
|
|
||||||
xpos0, xpos_e, vel_e, xpos0_e, gindices,
|
|
||||||
quat);
|
|
||||||
}
|
|
||||||
|
|
||||||
// rotate to corotational frame
|
|
||||||
for (int n = 0; n < npe; n++) {
|
|
||||||
mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat);
|
|
||||||
mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat);
|
|
||||||
}
|
|
||||||
|
|
||||||
// compute displacement
|
|
||||||
for (int n = 0; n < npe; n++) {
|
|
||||||
mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1);
|
|
||||||
}
|
|
||||||
|
|
||||||
// compute force in corotational frame
|
|
||||||
if (enbl_spring) {
|
|
||||||
mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe);
|
|
||||||
}
|
|
||||||
if (enbl_damper) {
|
|
||||||
mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe);
|
|
||||||
}
|
|
||||||
|
|
||||||
// rotate back to global frame and scatter using node indices
|
|
||||||
mju_negQuat(quat, quat);
|
|
||||||
for (int n = 0; n < npe; n++) {
|
|
||||||
mjtNum qfrc[3], qdmp[3];
|
|
||||||
mji_rotVecQuat(qfrc, frc_e+3*n, quat);
|
|
||||||
mji_rotVecQuat(qdmp, dmp_e+3*n, quat);
|
|
||||||
int gidx = gindices[n];
|
|
||||||
if (enbl_spring) {
|
|
||||||
mji_addTo3(frc_g + 3*gidx, qfrc);
|
|
||||||
}
|
|
||||||
if (enbl_damper) {
|
|
||||||
mji_addTo3(dmp_g + 3*gidx, qdmp);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// apply accumulated forces to bodies
|
|
||||||
for (int i = 0; i < nodenum; i++) {
|
|
||||||
mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]);
|
|
||||||
int bid = bodyid[i];
|
|
||||||
int nidx = i + m->flex_nodeadr[f];
|
|
||||||
|
|
||||||
// fast path: node at body origin (not pinned), direct DOF write
|
|
||||||
if (m->body_dofnum[bid] > 0 &&
|
|
||||||
(m->flex_centered[f] ||
|
|
||||||
(m->flex_node[3*nidx+0] == 0 &&
|
|
||||||
m->flex_node[3*nidx+1] == 0 &&
|
|
||||||
m->flex_node[3*nidx+2] == 0))) {
|
|
||||||
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
|
|
||||||
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
|
|
||||||
} else {
|
|
||||||
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
|
|
||||||
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
mj_freeStack(d);
|
|
||||||
|
|
||||||
// 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];
|
|
||||||
const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[f];
|
|
||||||
mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
|
|
||||||
mjtNum* vel = d->flexedge_velocity + m->flex_edgeadr[f];
|
|
||||||
mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f];
|
|
||||||
mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f];
|
|
||||||
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
|
|
||||||
mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0;
|
|
||||||
|
|
||||||
mj_markStack(d);
|
|
||||||
mjtNum* qfrc = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum);
|
|
||||||
mju_zero(qfrc, 3*m->flex_vertnum[f]);
|
|
||||||
|
|
||||||
// compute force element-by-element
|
|
||||||
int elemnum = m->flex_elemnum[f];
|
|
||||||
for (int t = 0; t < elemnum; t++) {
|
|
||||||
const int* vert = elem + (dim+1) * t;
|
|
||||||
|
|
||||||
// compute length gradient with respect to dofs
|
|
||||||
mjtNum gradient[6][2][3];
|
|
||||||
GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge);
|
|
||||||
|
|
||||||
// we add generalized Rayleigh damping as described in Section 5.2 of
|
|
||||||
// Kharevych et al., "Geometric, Variational Integrators for Computer
|
|
||||||
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
|
|
||||||
|
|
||||||
// extract elongation of edges belonging to this element
|
|
||||||
mjtNum elongation[6];
|
|
||||||
for (int e = 0; e < nedge; e++) {
|
|
||||||
int idx = edgeelem[t * nedge + e];
|
|
||||||
mjtNum previous = deformed[idx] - vel[idx] * m->opt.timestep;
|
|
||||||
elongation[e] = deformed[idx]*deformed[idx] - reference[idx]*reference[idx] +
|
|
||||||
(deformed[idx]*deformed[idx] - previous*previous) * kD;
|
|
||||||
}
|
|
||||||
|
|
||||||
// unpack triangular representation
|
|
||||||
mjtNum metric[36];
|
|
||||||
int id = 0;
|
|
||||||
for (int ed1 = 0; ed1 < nedge; ed1++) {
|
|
||||||
for (int ed2 = ed1; ed2 < nedge; ed2++) {
|
|
||||||
metric[nedge*ed1 + ed2] = k[21*t + id];
|
|
||||||
metric[nedge*ed2 + ed1] = k[21*t + id++];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// we now multiply the elongations by the precomputed metric tensor,
|
|
||||||
// notice that if metric=diag(1/reference) then this would yield a
|
|
||||||
// mass-spring model
|
|
||||||
|
|
||||||
// compute local force
|
|
||||||
mjtNum force[12] = {0};
|
|
||||||
for (int ed1 = 0; ed1 < nedge; ed1++) {
|
|
||||||
for (int ed2 = 0; ed2 < nedge; ed2++) {
|
|
||||||
for (int i = 0; i < 2; i++) {
|
|
||||||
for (int x = 0; x < 3; x++) {
|
|
||||||
force[3 * edges[dim-2][ed2][i] + x] -=
|
|
||||||
elongation[ed1] * gradient[ed2][i][x] *
|
|
||||||
metric[nedge * ed1 + ed2];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// insert into global force
|
|
||||||
for (int i = 0; i < nvert; i++) {
|
|
||||||
for (int x = 0; x < 3; x++) {
|
|
||||||
qfrc[3*vert[i]+x] += force[3*i+x];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// insert force into qfrc_passive, straightforward for simple bodies,
|
|
||||||
// need to distribute the force in case of pinned vertices
|
|
||||||
for (int v = 0; v < vertnum; v++) {
|
|
||||||
int bid = bodyid[v];
|
|
||||||
if (m->body_simple[bid] != 2) {
|
|
||||||
// this should only occur for pinned flex vertices
|
|
||||||
mj_applyFT(m, d, qfrc + 3*v, 0, xpos + 3*v, bid, d->qfrc_spring);
|
|
||||||
} else {
|
|
||||||
int body_dofnum = m->body_dofnum[bid];
|
|
||||||
int body_dofadr = m->body_dofadr[bid];
|
|
||||||
for (int x = 0; x < body_dofnum; x++) {
|
|
||||||
d->qfrc_spring[body_dofadr+x] += qfrc[3*v+x];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
mj_freeStack(d);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// flexedge-level spring-dampers
|
// flexedge-level spring-dampers
|
||||||
|
|||||||
Reference in New Issue
Block a user