Resolves#8587
This PR implements the requested anti-tipping utility and refactors the
math to correctly adhere to the NWU coordinate system.
**Key Changes:**
* Fixed the axis mapping: Positive pitch now correctly maps to a forward
tip (+X), and positive roll maps to a rightward tip (-Y).
* Inverted the proportional control logic: The correction vector now
applies a positive `kP` to drive *into* the direction of the fall to get
the wheels back under the center of gravity, rather than driving away
from it.
* Added a comprehensive JUnit test suite (`AntiTippingTest.java`) to
verify the calculated `ChassisSpeeds` correctly zero out the orthogonal
axis and provide the correct positive/negative velocity across all four
tipping directions.
Tested locally against `testDesktopJava` and passes all style/formatting
guidelines.
---------
Co-authored-by: Claude Opus 4.8 <noreply@anthropic.com>
The original implementation was `m_gyroOffset + (rotation -
m_pose.Rotation())`, which means the first `RotateBy` should be using
`-m_pose.Rotation()` (see `ResetPose()`). Java is already correct. This
also adds new tests to catch this particular error.
People generally have expressed a dislike for the Hungarian notation
used in member variables, especially in examples/templates, and our
styleguide shouldn't be forced on downstream consumers, so this removes
all Hungarian notation from the examples/templates.
There are _some_ benefits to Hungarian for private member variables
(like knowing what's a member vs. local in a PR review) so we'll keep
private member variables the same for now, but public variables should
no longer use Hungarian notation, since it looks much worse. A new PMD
XPath rule has been added to accomplish this goal. Some other
non-compliant variables were fixed for the new rule.
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>
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.
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.
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()
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.
libprotobuf is a very annoying dependency to deal with, and with the switch to nanopb for generated C++ code, libprotobuf is only used for dynamic decode in the GUI apps. libprotobuf has been swapped out with upb, a much smaller C-based library that supports reflection and can therefore do dynamic decode. This means we can remove the libprotobuf dependency and stop dealing with build issues because of it.
The UKF test was calling `.value()` on an implicit
`units::millisecond_t` type assuming it was `units::second_t`.
I normalized the rest of the dt declarations while I was at it.
Adds methods to compute the dot and cross products between Translation2ds and Translation3ds, as well as methods to compute the square of Distance and Norm, which allows avoiding some calls to sqrt in many cases.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Adds S3SigmaPoints based on MerweScaledSigmaPoints. In addition, restructures UnscentedKalmanFilter to support different sigma point generators and provides MerweUKF and S3UKF for convenience when working with either kind of filter.
S3UKFTest is copied from MerweUKFTest (which is a rename of UnscentedKalmanFilterTest). Curiously, however, in Java the original tolerance used in MerweUKFTest.testDriveConvergence() for the final rotation was too low for S3UKFTest, so the tolerance is increased from 0.000005 (5e-6) radians to 0.00015 (1.5e-4) radians. However, the C++ version still uses the original tolerance. (This difference is probably because Java uses a final rotation of 5.846 degrees while C++ uses a final rotation of 5.846 radians)
Closes#8072.
Breaking changes:
- (C++) UnscentedKalmanFilter has a new template parameter for the sigma point generator type.
- (Java) UnscentedKalmanFilter has an additional parameter to every constructor providing an instance of a sigma point generator.
- (C++) int MerweScaledSigmaPoints.NumSigmas() has been replaced with constexpr int MerweScaledSigmaPoints::NumSigmas.
- (C++) The second parameter of SquareRootUnscentedTransform has been changed from States to NumSigmas.
