I timed the DARE unit tests, and the new solver is 0 to 100% faster in
all cases (that is, it's at least as fast as Drake's and up to 2x faster
in some cases).
The new solver is also much simpler, takes less time to compile, and
drops the libwpimath.so size from 325 MB to 301 MB.
I think most of the compilation time is coming from the eigenvalue
decompositions used to enforce argument preconditions.
Now, implicit narrowing conversions are only used with wpi::Now(). This
also fixes clang-tidy warnings about C-style casts. For example:
```
== clang-tidy /__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc ==
/__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc:95:18: warning: C-style casts are discouraged; use static_cast/const_cast/reinterpret_cast [google-readability-casting]
auto curTime = units::second_t(m_timer.Get());
^
```
In that case at least, the cast was removed entirely since Get() already
returns a units::second_t.
This adds a unicycle controller that's a drop-in replacement for Ramsete
and a differential drive controller that controls the full pose and
outputs voltages. The main benefit is LQR-like tuning knobs using a
system model.
When trying to set the tolerance of a ProfiledPID, it fails if you don't give it a velocity value. It was missing a conversion from double to the appropiate unit.
It was using the continuous B matrix to compute the feedback gain
instead of the discrete B matrix.
Tests were added for the matrix constructor overloads.
The template argument order for UnscentedTransform was reversed to match
all the other UKF classes. Since UnscentedTransform is intended as a
class for internal use only, this shouldn't cause much breakage.
* Replace Matrix<> with Vector<> where vectors are explicitly intended.
I found these via `rg "Eigen::Matrix<double, \w+, 1>"`.
* Pass all Eigen matrices by const reference. I found these via `rg
"\(Eigen"` on main (the initializer list constructors make more false
positives).
* Replace MakeMatrix() and operator<< usage with initializer list
constructors. I found these via `rg MakeMatrix` and `rg "<<"`
respectively.
* Deprecate MakeMatrix()
Internal headers are no longer allowed as of
https://gitlab.com/libeigen/eigen/-/merge_requests/631. Based on
benchmarking I conducted in that thread, there doesn't seem to be a
performance penalty for including the full headers anymore.
The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.
This is a breaking change but is worthwhile to avoid hard-to-debug errors.
