Originally started with just swerve, but expanded to diff and mecanum
(docs only) for parity across the drivetrains. Return value checks are
applied when possible to make migration easier and to error loudly if
people forget.
---------
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
There's changes to the diagnostic output and a performance improvement
for autodiff setup. I also updated Java's Options docs to more closely
match upstream.
The wrapper includes reverse mode autodiff, the Problem DSL, and the
optimal control problem API. I wrote it by directly translating the
upstream
[API](https://github.com/SleipnirGroup/Sleipnir/tree/main/include/sleipnir)
and [tests](https://github.com/SleipnirGroup/Sleipnir/tree/main/test) to
Java (i.e., copy-paste-modify).
I replaced the ArmFeedforward and Ellipse2d JNIs with implementations
using the Sleipnir Java bindings. Switching dev binary JNIs to release
by default sped up wpimath test runs from several minutes to 7 seconds.
- Remove status return from HAL level (clock getting should never fail)
- Remove 32-bit timestamp expand function
- Make monotonic_clock.hpp (formerly fpga_clock.hpp) header-only and
move to root hal include directory
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).
This PR also replaces the speed verbiage in MotorController with duty
cycle.
Fixes#8423.
In C++, we use a diagonal matrix to avoid an expensive matrix
multiplication. EJML doesn't have a diagonal matrix type, so in Java, we
use a double array and implement the multiplication manually.
Add a simple tap counting filter for boolean streams.
The filter activates when the input has risen (transitioned from false
to true, like when a button is tapped) the required number of times
within the time window after the first rising edge. Once activated, the
output remains true as long as the input is true. The tap count resets
when the time window expires or when the input goes false after
activation.
Example usage:
```java
xbox.a()
.multiPress(2, 0.2) // Detect a double tap within 0.2 seconds
.onTrue(Commands.print("Double tapped A button"));
xbox.y()
.multiPress(2, 0.5) // Detect a double tap within 0.5 seconds
.whileTrue(Commands.print("Y held after tap").repeatedly());
```
This is not a noise reduction and/or input smoothing filter, but it is
similar in usage to debounce, so I believe it could be considered a
filter, but am open to a better location.
I believe this would be a useful addition, as double/triple tapping a
button is a common control option in games, yet is not often utilized by
newer FRC teams. I believe adding it to WPILib in a standard way will
allow more teams to make the most out of their controls.
Documents the extrinsic vs intrinsic semantics of `plus()` and
`minus()`. (`rotateBy()` was documented in [a previous
PR](https://github.com/wpilibsuite/allwpilib/pull/5508))
Fixes usage of `plus()` and `minus()` in `Rotation3d.interpolate()`.
(Fixes#8523)
Fixes incorrect usages of `plus()`, `minus()`, and `rotateBy()`
throughout `Odometry3d`.
Adds explanatory comments for some `plus()`, `minus()`, and `rotateBy()`
operations.
Fixes `TimeInterpolatableBuffer` not using twists for `Pose3d` (this was
just because I happened to notice it, it isn't really related to the PR)
To check all of our usages of `plus()`, `minus()`, and `rotateBy()`, I
marked them as deprecated, checked compile errors from `./gradlew
compileJava`, and then undeprecated them. You can see all of the spots
that showed up (at least on the Java side) by viewing the diff for
241109c.
I wanted to present this alternative to #8526 because the change has its
own quirks, there's little time before kickoff, and there would be no
code-side warning to teams (and mentors) already used to the current
behavior.
Fixes https://github.com/wpilibsuite/allwpilib/issues/8284.
If we have vision updates at the time of the `Reset*` call, we can
correct the translation/rotation of the new odometry pose by adding a
new vision update where:
- `ResetTranslation`: the translation is hard-coded to the new
translation, and the rotation components are set to those of the latest
vision update (prior to clearing the map).
- `ResetRotation`: the rotation is hard-coded to the new rotation, and
the translation components are set to those of the latest vision update
(prior to clearing the map).
ChassisAccelerations and the drivetrain acceleration types are added in
both Java and C++. `ChassisAccelerations` is basically just
`ChassisSpeeds` but for accelerations!
`DifferentialDriveWheelAccelerations`, `MecanumDriveWheelAccelerations`,
and `SwerveModuleAccelerations` are the acceleration equivalent of the
drivetrain speeds types.
In Java, the `Kinematics` interface now has an additional generic
parameter `A` which represents the accelerations, and
`toChassisAccelerations` and `toWheelAccelerations` methods, which are
implemented the same way as `toChassisSpeeds` and `toWheelSpeeds`.
Protobuf and struct classes were also added for all four classes in Java
and C++.
---------
Signed-off-by: Zach Harel <zach@zharel.me>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
* Moved makeWhiteNoiseVector() to random.Normal.normal()
* Moved isControllable() and isDetectable() to system.LinearSystemUtil
* Renamed makeCostMatrix() to costMatrix() (Java)
* Renamed makeCovarianceMatrix() to covarianceMatrix() (Java)
* Renamed MakeCostMatrix() to CostMatrix() (C++)
* Renamed MakeCovMatrix() to CovarianceMatrix() (C++)
* Removed deprecated poseTo3dVector(), poseTo4dVector(), poseToVector()
* Removed clampInputMaxMagnitude()
* We don't use it, and Eigen has this functionality built in via `u =
u.array().min(u_max.array()).max(u_min.array());`
* Simplified implementation of desaturateInputVector()
The spiraling issue occurs when the vision rotation standard deviation is very high relative to the odometry rotation standard deviation and the vision measurements have a large rotation error. (Scaling the rotation component of a twist without scaling the translation component causes the direction of overall translation to change, leading to spiraling around (either towards or away) the vision measurement instead of moving towards it.) Using a transform instead of a twist avoids this issue.
In general, scaling twist components is more mathematically correct than scaling transform components. However, although twists are correct for modeling uncertainty in an odometry-only pose estimate, they are not correct for the difference between the odometry-only pose estimate and a vision measurement. Since neither twists nor transforms are completely correct (and the pose estimator as a whole is not mathematically correct), but using transforms can guarantee that the pose estimate approaches the vision measurement (instead of potentially spiraling away), they are the least bad option.
clang-format 21 made some formatting changes. Since wpiformat's stdlib
task was removed, I removed NOLINT comments for it and removed some
std:: prefixes it added to comments.
