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

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
+6 -1
View File
@@ -106,9 +106,14 @@ void mj_passive(const mjModel* m, mjData* d) {
continue;
}
// process edges of this flex (global edge index)
// process non-rigid edges of this flex (global edge index)
int edgeend = m->flex_edgeadr[f] + m->flex_edgenum[f];
for (int e=m->flex_edgeadr[f]; e < edgeend; e++) {
// skip rigid
if (m->flexedge_rigid[e]) {
continue;
}
// compute spring-damper force along edge
frc = stiffness * (m->flexedge_length0[e] - d->flexedge_length[e])
- damping * d->flexedge_velocity[e];