Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity
7,432 Commits 2 Branches 165 Tags
ccc9740d56ec79382930c945bcbb415943eacb85
Commit Graph

683 Commits

Author SHA1 Message Date
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
Joseph Eng
d98ad815b1 [wpimath] Adjust applyDeadband logic (#7816) 
Reduces nesting by returning when the value is within the deadband.

Adjusts the algorithm to handle large values of maxMagnitude naturally (instead of needing a separate check).

Reformats the math comments.
 2025-04-23 08:41:03 -06:00
Tyler Veness
c13b2f45c1 [wpimath] Update link to Ramsete controller derivation (#7887) 
The derivation was moved from the Controls Engineering in FRC book to a
separate paper.
 2025-03-30 16:25:08 -07:00
Jack Cammarata
1efaaefd78 [wpimath] Fix UnscentedKalmanFilter and improve math docs (#7850) 
Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular.

This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed.

To fix the bug, a couple things needed to be changed:
all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor,
In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular,
P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively.

Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
 2025-03-14 10:05:15 -07:00
Tyler Veness
1a835fec01 [wpimath] Fix singularities in Ellipse2d::Nearest() (#7851) 
The problem was ill-conditioned if either semiaxis had zero length.
 2025-03-04 18:52:17 -08:00
crueter
0ad595c33c [wpimath] Add Debouncer type and time setters (#7839) 
Signed-off-by: swurl <swurl@swurl.xyz>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
 2025-03-03 17:21:38 -07:00
Tyler Veness
822457d45b [wpimath] Fix feedforward returning NaN when kᵥ = 0 (#7790) 2025-02-25 19:07:51 -08:00
Adrien Bourdeaux
75321f1d84 [wpimath] Add Translation2d/Translation3d slew rate limiter (#7806) 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
 2025-02-25 19:06:00 -08:00
Tyler Veness
517344fe80 [wpimath] Fix another infinite loop in ArmFeedforward (#7823) 2025-02-25 19:04:31 -08:00
Tyler Veness
13626063dc [wpimath] Fix units typo (#7789) 2025-02-13 23:22:44 -08:00
Peter Johnson
7e6077c546 [wpimath] Fix up order and docs for Feedforward gain setters (#7788) 2025-02-13 23:20:08 -08:00
Jade
4d126b158c [wpimath] Add setters to Feedforward gains (#7784) 
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
 2025-02-13 21:09:28 -08:00
Joseph Eng
296986397b [wpimath] Document drift from desaturating discretized chassis speeds (NFC) (#7741) 2025-02-03 11:46:18 -07:00
Tyler Veness
b31fd17d05 [wpimath] Fix infinite loop in ArmFeedforward::Calculate(xₖ, vₖ, vₖ₊₁) (#7745) 
Small values of kₐ make the iterative solver ill-conditioned. This
change reverts to the constant-acceleration feedforward in that case. It
gives _very_ bad results (hence why we added the iterative solver in the
first place), but it's better than hanging.

```
TEST(ArmFeedforwardTest, CalculateIllConditioned) {
  constexpr auto Ks = 0.5_V;
  constexpr auto Kv = 20_V / 1_rad_per_s;
  constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
  constexpr auto Kg = 0_V;
  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};

  // Calculate(currentAngle, currentVelocity, nextAngle, dt)
  CalculateAndSimulate(armFF, 0_rad, 0_rad_per_s, 2_rad_per_s, 20_ms);
}
```
This produces 1 V and doesn't accelerate the system at all. Using
nextVelocity instead of currentVelocity in the feedforward outputs 41 V
and still only accelerates to 0.4 rad/s of the requested 2 rad/s.

I picked the kₐ cutoff by increasing kₐ until the iterative solver
started converging.

Fixes #7743.
 2025-01-30 13:33:39 -07:00
Matthew Wozniak
8fc3767b30 [wpimath] Fix macro name typo (#7707) 2025-01-17 22:19:21 -08:00
David Racovan
a14545102f [wpimath] DifferentialDriveWheelPositions: tag as Proto/StructSerializable (#7622) 2025-01-13 14:52:40 -07:00
Tyler Veness
25e6549398 [wpimath] Fix various constexpr support bugs (#7676) 2025-01-13 14:44:55 -07:00
ハイドラント
cd92b07321 [wpimath] Add Pose2d and Pose3d rotateAround() (#7659) 2025-01-13 12:55:26 -07:00
Tyler Veness
007526089e [wpimath] Fix LinearSystemId return type and docs (#7675) 
Fixes #7674.
 2025-01-13 12:22:53 -07:00
HarryXChen
e37c35746a [wpimath] Implement Translation3d.RotateAround (#7661) 2025-01-09 20:31:34 -08:00
Tyler Veness
11a0c36737 [wpimath] Make Rotation2d member initialization order match declaration order (#7632) 2025-01-03 22:44:17 -08:00
sciencewhiz
203487a6aa [wpimath] improve LTVUnicycleController docs (NFC) (#7599) 
Document the states and inputs so it isn't necessary to look at the code
Fix max velocity throws doc
 2024-12-28 16:03:47 -06:00
Tyler Veness
57344ef3b2 [wpimath] Use ct_matrix instead of Eigen/LU for determinant in headers (#7600) 
This caught a bug in ct_matrix's 3x3 determinant.
 2024-12-28 16:03:29 -06:00
Nicholas Armstrong
fe49cbe429 [wpimath] Remove Units class from Elevator and Arm Feedforwards (#7570) 2024-12-26 20:21:19 -06:00
Tyler Veness
02a0adc653 [wpimath] Add Rotation3d rotation vector getter (#7564) 
The code churn in Java is just making the function order consistent
between languages.
 2024-12-22 13:34:51 -08:00
Peter Johnson
4bca79b9af [wpimath] Revert "ChassisSpeeds fromRelative and discretize methods made instance methods (#7115)" (#7549) 
This reverts commit a3b12b3bd9.
 2024-12-14 10:42:49 -08:00
Joseph Eng
4225b732fd Remove unnecessary boxing (#7539) 
* Remove unnecessary boxing
Also remove unnecessary warning suppression

* Use more idiomatic functional interfaces in NumericalIntegration
 2024-12-12 19:18:40 -08:00
Wispy
cc41a0ed24 [wpilib] Replace MecanumDriveMotorVoltages with a functional interface (#6760) 2024-12-08 17:05:06 -08:00
Tyler Veness
d5edb4060d [upstream_utils] Upgrade Sleipnir (#7512) 
It now uses SQP for problems without inequality constraints, which is
faster.

main:
```
[ RUN      ] Ellipse2dTest.DistanceToPoint
0.203 ms
[       OK ] Ellipse2dTest.DistanceToPoint (0 ms)
[ RUN      ] Ellipse2dTest.FindNearestPoint
0.019 ms
[       OK ] Ellipse2dTest.FindNearestPoint (0 ms)
```

upgrade:
```
[ RUN      ] Ellipse2dTest.DistanceToPoint
0.197 ms
[       OK ] Ellipse2dTest.DistanceToPoint (0 ms)
[ RUN      ] Ellipse2dTest.FindNearestPoint
0.015 ms
[       OK ] Ellipse2dTest.FindNearestPoint (0 ms)
```
 2024-12-07 23:02:39 -08:00
Tyler Veness
e08fdeba21 [wpimath] Rename FindNearestPoint() to Nearest() (#7513) 
This is easier to type and follows the naming of Pose2d::Nearest().

Since Ellipse2d and Rectangle2d were added for the 2025 season, we don't
need to add deprecation notices.
 2024-12-07 23:01:18 -08:00
Tyler Veness
e222efaa01 [wpimath] Add affine transformation constructors and getters to geometry API (#7430) 
Fixes #7429.
 2024-12-07 15:49:17 -08:00
Tyler Veness
278efa6384 [upstream_utils] Remove Sleipnir patches no longer needed with GCC 11 (#7491) 2024-12-07 12:36:25 -08:00
Tyler Veness
882233bede [wpimath] Improve SimpleMotorFeedforward argument names and deprecation message (#7489) 
Also roll back SimpleMotorFeedforward unit API until 2027.
 2024-12-06 22:32:40 -08:00
sciencewhiz
892e062316 [hal,wpilib,wpimath] Add Usage Reporting for Choreo and PathWeaver (#7464) 2024-11-30 20:33:09 -08:00