Commit Graph

768 Commits

Author SHA1 Message Date
Tyler Veness
2c5529d714 [wpimath] Speed up pose estimator correction computation (#8574)
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.
2026-02-06 21:47:49 -08:00
Michael Lesirge
21b5389bbe [wpimath,cmd] Add multi tap boolean stream filter and multi tap trigger modifier (double tap detector) (#8307)
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.
2026-01-14 20:22:07 -08:00
Joseph Eng
9e1258440b [wpimath] Fix Rotation3d interpolation and document extrinsic vs intrinsic (#8544)
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.
2026-01-14 20:16:24 -08:00
Bryce Roethel
ab3af00d07 [wpimath] TrapezoidProfile.State implement StructSerializable (#8499)
Seems like this was missed in #8163
2025-12-21 10:08:32 -06:00
Benjamin Hall
3dc334c1ee [wpimath] Fix ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport (#8285)
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).
2025-12-13 15:53:53 -08:00
Peter Johnson
baa6379267 [wpimath] Add usage reporting for state-space classes (#8453)
- LinearQuadraticRegulator
- Kalman filters
- Pose estimators
- LinearSystemLoop

Fixes #2925.
2025-12-06 09:17:02 -08:00
Joseph Eng
b8d6bc2eb1 [wpimath] Scale transforms instead of twists in PoseEstimator (#8333)
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.
2025-11-07 18:07:43 -08:00
Dalton Smith
fea6883d98 [wpimath] DCMotor: Add X44 and Minion (#8319) 2025-11-01 09:19:36 -07:00
Tyler Veness
4aef52a117 [ci] Upgrade to wpiformat 2025.36 (#8308)
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.
2025-10-28 20:17:04 -07:00
Michael Lesirge
9e85f3cf55 [wpimath] Rename 1D copySignPow to match 2D copyDirectionPow (#8286) 2025-10-11 09:24:10 -07:00
Michael Lesirge
2b43541b94 [wpimath] MathUtil: Add 2D variants of applyDeadband and copySignPow (#8057) 2025-10-10 13:44:12 -07:00
Tyler Veness
f5990e8b40 [upstream_utils] Fix Eigen tag (#8283)
Upstream no longer seems to have the commit we were pointing to. We'll
just use the tag since that hasn't changed since the official release
announcement.
2025-10-09 21:50:55 -07:00
Joseph Eng
871769c815 [wpimath] Fix units overload resolution (#8267) 2025-10-02 17:36:30 -07:00
Gold856
f9899eb73f [wpimath] Fix odd header path in BangBangController (#8261) 2025-09-27 22:48:00 -07:00
Tyler Veness
5003939b64 [upstream_utils] Recopy Eigen source (#8251)
Upstream slid the tag (again).  Change to the SHA commit ID until things stabilize.
2025-09-24 09:03:16 -06:00
Tyler Veness
8fb5a1985a [upstream_utils] Recopy Eigen source (#8249)
Upstream slid the tag.
2025-09-21 21:58:43 -07:00
Tyler Veness
0a4e44ea06 [wpimath] Synchronize C++ and Java RK4 docs (NFC) (#8238) 2025-09-20 15:49:41 -07:00
Tyler Veness
a7e7f6912a [upstream_utils] Upgrade to Eigen 5.0.0 (#8240) 2025-09-20 15:49:23 -07:00
Tyler Veness
3dbdfa1839 [upstream_utils] Upgrade Sleipnir (#8235) 2025-09-20 11:21:06 -07:00
Tyler Veness
ee3d55e848 [upstream_utils] Upgrade Eigen to latest (#8228) 2025-09-19 17:52:48 -06:00
Tyler Veness
dbffe6e8ac [wpimath] Add readme (#8209) 2025-09-08 17:26:22 -07:00
Tyler Veness
632749e6f3 [build] Upgrade Maven dependencies (#8173) 2025-09-01 08:13:46 -07:00
Jade
b0829356fa [wpimath] Fix sysid links (NFC) (#8204)
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
2025-09-01 08:11:39 -07:00
Tyler Veness
0d9e850e22 [wpimath] Fix dt type in C++ tests (#8179)
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.
2025-08-16 22:51:13 -07:00
Daniel Chen
f209ecb0cb [wpimath] Add structs for TrapezoidProfile.State and ExponentialProfile.State (#8163) 2025-08-10 11:45:36 -07:00
Tyler Veness
9ac7e286f5 [build] Upgrade Gradle plugins (#8166)
I upgraded all plugins I could see except org.ysb33r.doxygen. 2.0 made
breaking changes, and I couldn't figure out how to migrate.

Most of the changes are for suppressing new linter purification rites.
2025-08-08 23:04:02 -07:00
Gold856
2d11946d98 [wpical] Use updated thirdparty-ceres and move resource files (#8151) 2025-08-03 11:41:25 -07:00
Rain Heuer
b3aeee18c8 [wpimath] Add vector product and squared length operations to Translation2d/3d (#8133)
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>
2025-07-31 21:05:39 -07:00
Tyler Veness
feee88f40d [wpimath] Remove redundant transposes on symmetric matrices (#8131)
This likely won't have a performance impact since it only affects matrix traversal order, but it does simplify the code.
2025-07-31 21:04:55 -07:00
Tyler Veness
e678a338b4 [ci] Upgrade wpiformat (#8124)
See https://github.com/wpilibsuite/styleguide/pull/312
2025-07-30 11:10:12 -06:00
Michael Lesirge
f308e9873a [wpimath] Add miles per hour conversation methods (#8026) 2025-07-17 21:19:32 -07:00
Austin Schuh
4b39853483 [build] Check results of subprocess.run in wpimath/generate_quickbuf.py (#8069)
The generator was calling a binary of the wrong architecture and failing
internally, but passing the build (incorrectly).  Explode right away.

Signed-off-by: Austin Schuh <austin.linux@gmail.com>
2025-07-06 07:24:09 -05:00
sciencewhiz
f2dd81c839 [build] Add checkstyle check for deprecation (#7738)
Ensures java deprecated notation is paired with javadoc and vice versa.
Adds javadoc deprecation for MecanumControllerCommand, ArmFeedForward,
ElevatorFeedforward, and MecanumDriveMotorVoltages
Fixes #7736
Supersedes #7737

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
2025-06-30 00:09:02 -07:00
ThePixelatedCat
ffe296892c [wpimath] Fix TrapezoidProfile limiting velocity incorrectly (#8030) 2025-06-24 22:36:53 -07:00
Michael Lesirge
fb399eef3d [wpimath] Add copySignPow to MathUtil for joystick input shaping (#8013) 2025-06-15 14:08:41 -07:00
Michael Lesirge
e2517b7a21 [wpimath] Update parameter type from list to general collection (#8020) 2025-06-15 14:08:06 -07:00
Michael Lesirge
05c080328b [wpimath] Pass Translation2d by const reference instead of by value (#8021) 2025-06-13 18:50:05 -07:00
Michael Lesirge
c01e318370 [wpimath] Add Translation3d.nearest() (#8015) 2025-06-12 22:14:00 -07:00
Michael Lesirge
075cc4a20f [wpimath] Add nearest() method to Pose3d (mirroring Pose2d) (#8010) 2025-06-05 22:11:49 -07:00
Gold856
ca05ffa1b9 [upstream_utils] Use pathlib instead of os.path (#7983)
A noteworthy change is the replacement of the `dp.startswith(os.path.join(".", "subdir"))` pattern. pathlib doesn't offer something with similar semantics besides `match` and `full_match`, so there's now a helper function that replicates the behavior.

Other notable changes include the addition of type annotations to ensure code correctness, using == to check file names instead of `endswith` for clarity (`endswith` is still used to check extensions), manual walking and copying being refactored in googletest, json, memory, nanopb, protobuf, and sleipnir to use `walk_cwd_and_copy_if`, and matching functions being shortened to the point where they can just be inlined into the lambda.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: David Vo <auscompgeek@users.noreply.github.com>
2025-05-29 16:05:22 -06:00
Tyler Veness
de718f7ae5 [upstream_utils] Upgrade Sleipnir (#7973) 2025-05-27 08:24:15 -06:00
Tyler Veness
b4823569a4 [upstream_utils] Upgrade Eigen to latest (#7982) 2025-05-19 14:26:53 -06:00
Lucien Morey
d32e60233f [wpimath] Add dynamic size support for angle statistics (#7964) 2025-05-15 18:43:46 -07:00
Lucien Morey
e1da917270 [wpimath] Add dynamic size support for numerical jacobian computation (#7959) 2025-05-05 21:55:51 -07:00
Tyler Veness
ba37e7eb3c [wpimath] Fix warning false positive from GCC 15 (#7948)
```
In file included from /usr/include/c++/15.1.1/functional:61,
                 from /home/tav/frc/wpilib/allwpilib/thirdparty/googletest/include/gtest/gtest-matchers.h:43,
                 from /home/tav/frc/wpilib/allwpilib/thirdparty/googletest/include/gtest/internal/gtest-death-test-internal.h:47,
                 from /home/tav/frc/wpilib/allwpilib/thirdparty/googletest/include/gtest/gtest-death-test.h:43,
                 from /home/tav/frc/wpilib/allwpilib/thirdparty/googletest/include/gtest/gtest.h:65,
                 from /home/tav/frc/wpilib/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp:7:
In copy constructor ‘std::function<_Res(_ArgTypes ...)>::function(const std::function<_Res(_ArgTypes ...)>&) [with _Res = Eigen::Matrix<double, 2, 1>; _ArgTypes = {const Eigen::Matrix<double, 2, 1, 0, 2, 1>&, const Eigen::Matrix<double, 1, 1, 0, 1, 1>&}]’,
    inlined from ‘frc::ControlAffinePlantInversionFeedforward<States, Inputs>::ControlAffinePlantInversionFeedforward(std::function<Eigen::Matrix<double, Size, 1, (Eigen::AutoAlign | (((Size == 1) && (1 != 1)) ? Eigen::RowMajor : (((1 == 1) && (Size != 1)) ? Eigen::ColMajor :  Eigen::ColMajor))), Size, 1>(const Eigen::Matrix<double, Size, 1, (Eigen::AutoAlign | (((Size == 1) && (1 != 1)) ? Eigen::RowMajor : (((1 == 1) && (Size != 1)) ? Eigen::ColMajor :  Eigen::ColMajor))), Size, 1>&, const Eigen::Matrix<double, Cols, 1, (Eigen::AutoAlign | (((Cols == 1) && (1 != 1)) ? Eigen::RowMajor : (((1 == 1) && (Cols != 1)) ? Eigen::ColMajor :  Eigen::ColMajor))), Cols, 1>&)>, units::time::second_t) [with int States = 2; int Inputs = 1]’ at /home/tav/frc/wpilib/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h:59:19,
    inlined from ‘virtual void frc::ControlAffinePlantInversionFeedforwardTest_Calculate_Test::TestBody()’ at /home/tav/frc/wpilib/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp:29:70:
/usr/include/c++/15.1.1/bits/std_function.h:393:17: error: ‘<anonymous>’ may be used uninitialized [-Werror=maybe-uninitialized]
  393 |             __x._M_manager(_M_functor, __x._M_functor, __clone_functor);
      |             ~~~~^~~~~~~~~~
/usr/include/c++/15.1.1/bits/std_function.h: In member function ‘virtual void frc::ControlAffinePlantInversionFeedforwardTest_Calculate_Test::TestBody()’:
/usr/include/c++/15.1.1/bits/std_function.h:269:7: note: by argument 2 of type ‘const std::_Any_data&’ to ‘static bool std::_Function_handler<_Res(_ArgTypes ...), _Functor>::_M_manager(std::_Any_data&, const std::_Any_data&, std::_Manager_operation) [with _Res = Eigen::Matrix<double, 2, 1>; _Functor = frc::ControlAffinePlantInversionFeedforwardTest_Calculate_Test::TestBody()::<lambda(auto:45&, auto:46&)>; _ArgTypes = {const Eigen::Matrix<double, 2, 1, 0, 2, 1>&, const Eigen::Matrix<double, 1, 1, 0, 1, 1>&}]’ declared here
  269 |       _M_manager(_Any_data& __dest, const _Any_data& __source,
      |       ^~~~~~~~~~
/home/tav/frc/wpilib/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp:29:70: note: ‘<anonymous>’ declared here
   29 |                                                                 20_ms};
      |                                                                      ^
In copy constructor ‘std::function<_Res(_ArgTypes ...)>::function(const std::function<_Res(_ArgTypes ...)>&) [with _Res = Eigen::Matrix<double, 2, 1>; _ArgTypes = {const Eigen::Matrix<double, 2, 1, 0, 2, 1>&}]’,
    inlined from ‘frc::ControlAffinePlantInversionFeedforward<States, Inputs>::ControlAffinePlantInversionFeedforward(std::function<Eigen::Matrix<double, Size, 1, (Eigen::AutoAlign | (((Size == 1) && (1 != 1)) ? Eigen::RowMajor : (((1 == 1) && (Size != 1)) ? Eigen::ColMajor :  Eigen::ColMajor))), Size, 1>(const Eigen::Matrix<double, Size, 1, (Eigen::AutoAlign | (((Size == 1) && (1 != 1)) ? Eigen::RowMajor : (((1 == 1) && (Size != 1)) ? Eigen::ColMajor :  Eigen::ColMajor))), Size, 1>&)>, frc::Matrixd<Rows, Cols>&, units::time::second_t) [with int States = 2; int Inputs = 1]’ at /home/tav/frc/wpilib/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h:79:11,
    inlined from ‘virtual void frc::ControlAffinePlantInversionFeedforwardTest_CalculateState_Test::TestBody()’ at /home/tav/frc/wpilib/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp:45:73:
/usr/include/c++/15.1.1/bits/std_function.h:393:17: error: ‘<anonymous>’ may be used uninitialized [-Werror=maybe-uninitialized]
  393 |             __x._M_manager(_M_functor, __x._M_functor, __clone_functor);
      |             ~~~~^~~~~~~~~~
/usr/include/c++/15.1.1/bits/std_function.h: In member function ‘virtual void frc::ControlAffinePlantInversionFeedforwardTest_CalculateState_Test::TestBody()’:
/usr/include/c++/15.1.1/bits/std_function.h:269:7: note: by argument 2 of type ‘const std::_Any_data&’ to ‘static bool std::_Function_handler<_Res(_ArgTypes ...), _Functor>::_M_manager(std::_Any_data&, const std::_Any_data&, std::_Manager_operation) [with _Res = Eigen::Matrix<double, 2, 1>; _Functor = frc::ControlAffinePlantInversionFeedforwardTest_CalculateState_Test::TestBody()::<lambda(auto:47&)>; _ArgTypes = {const Eigen::Matrix<double, 2, 1, 0, 2, 1>&}]’ declared here
  269 |       _M_manager(_Any_data& __dest, const _Any_data& __source,
      |       ^~~~~~~~~~
/home/tav/frc/wpilib/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp:45:73: note: ‘<anonymous>’ declared here
   45 |                                                                 B, 20_ms};
      |                                                                         ^
```
2025-05-01 11:40:13 -06:00
Cory
7036bd509e [wpimath] Correct mil unit (#7950) 2025-05-01 11:28:02 -06:00
Lucien Morey
f14af97dc7 [wpimath] Support dynamic matrix sizes in StateSpaceUtil (#7942) 2025-04-29 11:58:28 -06:00
Nolan Barker
40ce42712f [wpimath] Fix coordinate frame docs in HolonomicDriveController (#7938) 2025-04-28 16:27:38 -07:00
crueter
0695a4db89 [wpimath] Fix Debouncer type-changing behavior (#7870)
Closes #7867

Properly resets the baseline upon switching the debounce type, and adds
a test for such.

Signed-off-by: swurl <swurl@swurl.xyz>
2025-04-25 22:05:31 -07:00
me-it-is
3960045663 [wpimath] TrapezoidProfile: Fix null pointer when calling timeLeftUntil (#7894) 2025-04-25 21:58:05 -07:00