This is a more general API in preparation of a higher interpolation order
PiperOrigin-RevId: 820174741
Change-Id: I81bd52ab44e0485d72579e21d1c8a48a1b35ec3f
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
Notes:
- Currently always uses dense math, even for sparse models. This should be easy to change in the future.
- Does not support geom wrapping. This is possible but harder, requires derivatives of mju_wrap.
PiperOrigin-RevId: 740378741
Change-Id: Id39ef2c4bfbb7ee11ec33c97d7d83140441cdab2
These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh.
On an 8x8x8 cube, the performance using DOFs at all vertices is
```
Simulation time : 18.74 s
Steps per second : 533
Realtime factor : 0.53 x
Time per step : 1874.4 µs
Contacts per step : 114.88
Constraints per step : 3322.51
Degrees of freedom : 1536
```
With the new implementation, it is the following:
```
Simulation time : 1.82 s
Steps per second : 5507
Realtime factor : 5.51 x
Time per step : 181.6 µs
Contacts per step : 38.84
Constraints per step : 155.36
Degrees of freedom : 24
```
PiperOrigin-RevId: 721008829
Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22
- `qLDiagSqrtInv` is only required for the dual solvers. It is now computed as-needed rather than unconditionally.
- `mj_solveM2` now requires a new input array `sqrtInvD` which contains the square root of the inverse diagonal D (formerly saved in `qLDiagSqrtInv`).
PiperOrigin-RevId: 710805133
Change-Id: I0622d6a8da3882916824e9c10bad9223c122c321
This allows for better sanitizer coverage, errors will be caught upon read rather than upon use.
PiperOrigin-RevId: 698458735
Change-Id: I3003f3a122b83bbf42a6510260abbb8353c56a40
The introduction of site specification to connects and welds in 3.2.3 conditionally changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`. These changes were not properly propagated in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints.
PiperOrigin-RevId: 690670420
Change-Id: I55ee8a013cbee8457f8d6c7f33c2981aedafbab6
The associated row in `actuator_moment` was not cleared before being updated.
PiperOrigin-RevId: 688207085
Change-Id: I2f925e984c5c493e64ead66667aef57c4844acfe
Alessio Quaglino