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

Fix flex constraint Jacobian for edges belonging to a single body.

Browse Source 
PiperOrigin-RevId: 575161094
Change-Id: If4153bcb751cdd6dbdddeb08672f7107c7c5bbc2
This commit is contained in:
Alessio Quaglino
2023-10-20 02:48:46 -07:00
committed by Copybara-Service
parent e4a9f535a4
commit 01a932e052
12 changed files with 97 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_sensor.c
+8 -4
View File
@@ -956,10 +956,14 @@ void mj_energyPos(const mjModel* m, mjData* d) {
continue;
}
// process edges of this flex
for (int e=m->flex_edgeadr[i]; e < m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
// process non-rigid edges of this flex
int flex_edgeadr = m->flex_edgeadr[i];
int flex_edgenum = m->flex_edgenum[i];
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
if (!m->flexedge_rigid[e]) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
};
}
}
}