Skip flex edge Jacobian if not needed.

When equality constraints and built-in passive forces are not present, edge Jacobians are unused. This gives a 3x speedup in `mj_kinematics` in `floppy_flex` (from 0.2ms/step to 0.06ms/step).

Changed the default options of `floppy_flex` to match those of `floppy` for a fair comparison.

PiperOrigin-RevId: 575850622
Change-Id: I4c618536fd17ef930153bbd1f1da7d9e0e44abbe
This commit is contained in:
Alessio Quaglino
2023-10-23 10:00:33 -07:00
committed by Copybara-Service
parent 2bb8652bf6
commit 643c1248b8
2 changed files with 12 additions and 2 deletions
+10
View File
@@ -541,6 +541,11 @@ void mj_flex(const mjModel* m, mjData* d) {
continue;
}
// skip Jacobian if no built-in passive force is needed
int skipjacobian = !m->flex_edgeequality[f] &&
!m->flex_edgedamping[f] &&
!m->flex_edgestiffness[f];
// process edges of this flex
int vbase = m->flex_vertadr[f];
int ebase = m->flex_edgeadr[f];
@@ -557,6 +562,11 @@ void mj_flex(const mjModel* m, mjData* d) {
mju_sub3(vec, pos2, pos1);
d->flexedge_length[ebase+e] = mju_normalize3(vec);
// skip Jacobian if not needed
if (skipjacobian) {
continue;
}
// sparse edge Jacobian
if (issparse) {
// set rowadr