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

Move flex damping to the engine and remove membrane and solid plugins.

Browse Source 
PiperOrigin-RevId: 676434954
Change-Id: I24e8dbaa90afcffd613a9cf106ef8b7262195328
This commit is contained in:
Alessio Quaglino
2024-09-19 09:00:13 -07:00
committed by Copybara-Service
parent 6c7e1095c5
commit 4998e7b392
26 changed files with 88 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/elasticity/elasticity.h
-101
View File
@@ -58,107 +58,6 @@ struct Stencil3D {
int edges[kNumEdges];
};
// gradients of edge lengths with respect to vertex positions
template <typename T>
void inline GradSquaredLengths(mjtNum gradient[T::kNumEdges][2][3],
const mjtNum* x,
const int v[T::kNumVerts]) {
for (int e = 0; e < T::kNumEdges; e++) {
for (int d = 0; d < 3; d++) {
gradient[e][0][d] = x[3*v[T::edge[e][0]]+d] - x[3*v[T::edge[e][1]]+d];
gradient[e][1][d] = x[3*v[T::edge[e][1]]+d] - x[3*v[T::edge[e][0]]+d];
}
}
}
template <typename T>
inline void ComputeForce(std::vector<mjtNum>& qfrc_passive,
const std::vector<mjtNum>& elongationglob,
const mjModel* m, int flex,
const mjtNum* xpos) {
mju_zero(qfrc_passive.data(), qfrc_passive.size());
mjtNum* k = m->flex_stiffness + 21 * m->flex_elemadr[flex];
int dim = m->flex_dim[flex];
const int* elem = m->flex_elem + m->flex_elemdataadr[flex];
const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[flex];
// compute force element-by-element
for (int t = 0; t < m->flex_elemnum[flex]; t++) {
const int* v = elem + (dim+1) * t;
// compute length gradient with respect to dofs
mjtNum gradient[T::kNumEdges][2][3];
GradSquaredLengths<T>(gradient, xpos, v);
// extract elongation of edges belonging to this element
mjtNum elongation[T::kNumEdges];
for (int e = 0; e < T::kNumEdges; e++) {
int idx = edgeelem[t * T::kNumEdges + e];
elongation[e] = elongationglob[idx];
}
// unpack triangular representation
mjtNum metric[T::kNumEdges*T::kNumEdges];
int id = 0;
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = ed1; ed2 < T::kNumEdges; ed2++) {
metric[T::kNumEdges*ed1 + ed2] = k[21*t + id];
metric[T::kNumEdges*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[T::kNumVerts*3] = {0};
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = 0; ed2 < T::kNumEdges; ed2++) {
for (int i = 0; i < 2; i++) {
for (int x = 0; x < 3; x++) {
force[3 * T::edge[ed2][i] + x] -=
elongation[ed1] * gradient[ed2][i][x] *
metric[T::kNumEdges * ed1 + ed2];
}
}
}
}
// insert into global force
for (int i = 0; i < T::kNumVerts; i++) {
for (int x = 0; x < 3; x++) {
qfrc_passive[3*v[i]+x] += force[3*i+x];
}
}
}
}
// add flex force to degrees of freedom
inline void AddFlexForce(mjtNum* qfrc,
const std::vector<mjtNum>& force,
const mjModel* m, mjData* d,
const mjtNum* xpos,
int f0) {
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f0];
for (int v = 0; v < m->flex_vertnum[f0]; v++) {
int bid = bodyid[v];
if (m->body_simple[bid] != 2) {
// this should only occur for pinned flex vertices
mj_applyFT(m, d, force.data() + 3*v, 0, xpos + 3*v, bid, qfrc);
} else {
int body_dofnum = m->body_dofnum[bid];
int body_dofadr = m->body_dofadr[bid];
for (int x = 0; x < body_dofnum; x++) {
qfrc[body_dofadr+x] += force[3*v+x];
}
}
}
}
// copied from mjXUtil
void String2Vector(const std::string& txt, std::vector<int>& vec);