Refactor and speed up mj_crb and add a benchmark.

This is a no-op "prefactor" of `mj_crb` to reduce the number of lines changed in the upcoming sleeping CL. The main change here is that `mj_crb` now avoids accessing the model and data pointer repeatedly, but instead has the input and output pointers explicitly declared as local variables. A benchmark test found a healthy **14% perf bump** due to two changes:
- Adding `restrict` to `mju_mulInertVec`
- The function-local pointers.

Adding `restrict` to the local pointers had no effect. Note that `mj_crb` is not a particularly expensive function so these speed bumps are not significant per se, but rather indicative of possible future gains with these techniques.

```
Benchmark                Time(ns)        CPU(ns)     Iterations
---------------------------------------------------------------

ORGINAL BASELINE
BM_CRB_BASELINE_mean         4325           4348         993200  230.052k items/s

BASELINE + RESTRICT
BM_CRB_BASELINE_mean         4057           4090        1186150  244.573k items/s

LOCAL POINTERS + RESTRICT
BM_CRB_mean                  3772           3800        1001750  263.209k items/s
```

PiperOrigin-RevId: 815798225
Change-Id: Iffdf57b544e8e10562807c617f73ca4ccd1c614f
This commit is contained in:
Yuval Tassa
2025-10-06 10:53:09 -07:00
committed by Copybara-Service
parent b75af81c75
commit ab9575d8a6
5 changed files with 162 additions and 17 deletions
+1 -1
View File
@@ -102,7 +102,7 @@ void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9],
void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]);
// multiply 6D vector (rotation, translation) by 6D inertia matrix
void mju_mulInertVec(mjtNum res[6], const mjtNum inert[10], const mjtNum vec[6]);
MJAPI void mju_mulInertVec(mjtNum res[6], const mjtNum inert[10], const mjtNum vec[6]);
// multiply dof matrix by vector
void mju_mulDofVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n);