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
+10 -2
View File
@@ -1857,8 +1857,16 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEX:
// increment edgenum rows
i += m->flex_edgenum[m->eq_obj1id[id]];
// increment with number of non-rigid edges
k = m->eq_obj1id[id];
int flex_edgeadr = m->flex_edgeadr[k];
int flex_edgenum = m->flex_edgenum[k];
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
if (!m->flexedge_rigid[e]) {
i++;
}
}
break;
default: