Squashed 'third_party/allwpilib/' changes from 83f1860047..f1a82828fe
f1a82828fe [wpiutil] Add DataLog and DataLogManager Stop() (#5860)
2a04e12c6f [apriltag] AprilTagFieldLayout: Add accessors for origin and field dimensions (#5869)
33e0089afb Cleanup usages of std::function<void(void)> (#5864)
d06fa633d5 [build] Fix protobuf generation when building with make (#5867)
049732afb8 [cscore] Make camera connection logging clearer (#5866)
87f7c19f90 [wpimath] Make InterpolatingDoubleTreeMap constructor public (#5865)
6b53ef47cf [wpimath] Don't recreate TrapezoidProfile in ProfiledPIDController calculate() (#5863)
8a3a268ae6 [commands] Add finallyDo with zero-arg lambda (#5862)
1c35d42cd0 [wpilib] Pop diagnostic for deprecated function use (#5859)
ddc8db6c26 [wpimath] Add feedforward constant constructor to ElevatorSim (#5823)
c6aff2c431 [upstream_utils] Update to LLVM 17.0.4 (#5855)
a9c5b18a39 [build] Update OpenCV to 2024-4.8.0-2 (#5854)
9540b6922d [hal] Add CAN IDs for AndyMark and Vivid Hosting (#5852)
83a7d33c47 [glass] Improve display of protobuf/struct type strings (#5850)
a4a8ad9c75 [commands] Make Java SelectCommand generic (#5849)
9eecf2a456 [build] Add CMake option to build Java sources jars (#5768)
9536a311cb [wpilib] Add support for the PS5 DualSense controller (#5257)
8d5e6737fc [wpilibc] SolenoidSim: Add virtual destructor (#5848)
07e13d60a2 [ntcore] Fix write_impl (#5847)
1713386869 [wpiutil] ProtobufMessageDatabase: Fix out-of-order Add() rebuild (#5845)
35472f5fc9 [ntcore] Fix a use-after-free in client close (#5844)
ed168b522c [ntcore] Disable buf pool when asan is enabled (#5843)
3e7ba2cc6f [wpinet] WebSocket: Fix write behavior (#5841)
80c47da237 [sim] Disable the robot program when DS disconnects (#5818)
abe1cec90c [wpilib] Update Usage Reporting ResourceType from NI Libraries (#5842)
cdf981abba [glass] Fix position of data type in NT view (#5840)
04dcd80adb [build] Publish unit tests for examples (#5838)
49920234ac [build] Fix checkstyle rules to allow Windows paths (#5839)
366b715942 [wpilib] Fix SendableChooser test (#5835)
3ba501f947 [commands] Java: Fix CommandXboxController.leftTrigger() parameter order (#5831)
ec569a58ef [wpimath] Make KalmanTypeFilter interface public (#5830)
b91317fd36 [wpiutil] DataLog.addSchema(): Don't add into a set view (#5829)
2ab4fcbc24 [wpiutil] ProtobufMessageDatabase: Clear messages first (#5827)
98c14f1692 [wpimath] Add EKF/UKF u-y-R correct overload (#5832)
60bcdeded9 [ci] Disable java in sanitizer builds (#5833)
c87f8fd538 [commands] Add DeferredCommand (#5566)
ad80eb3a0b [ci] Update actions for comment-command (#5824)
c7d6ad5a0b [ntcore] WebSocketConnection: Use weak capture (#5822)
8a8e220792 [simgui] Add 'Invalid' option for AllianceStation (#5820)
cfc6a47f76 [sim] DS plugin: Fix off-by-one error when setting alliance station (#5819)
8efa586ace [ntcore] Don't check type string on publishing an entry (#5816)
23ea188e60 [glass] Add protobuf decode error log message (#5812)
928e87b4f4 [build] Add combined test meta-task (#5813)
63ef585d4b [wpiutil] Fix compilation of MathExtras.h on Windows with /sdl (#5809)
b03a7668f9 [build] Windows CMake/vcpkg fixes (#5807)
3f08bcde54 [hal] Fix HAL AllianceStation on rio (#5811)
196d963dc4 [ntcore] Fix off-by-one error in stream write (#5810)
f4cbcbc984 Fix typos (NFC) (#5804)
ec0f7fefb0 [myrobot] Update the myRobot JRE (#5805)
3d618bdbfd [wpiutil] Fix Java struct array unpacking (#5801)
1fa7445667 [ntcore] Check for valid client in incoming text and binary (#5799)
269b9647da [ci] Update JDK for combine step (#5794)
bee32f080e [docs] Add wpiunits to JavaDocs (#5793)
25dad5a531 [wpinet] TCPConnector_parallel: Don't use thread_local (#5791)
4a93581f1a [build] cmake: use default library type for libglassnt, libglass, wpigui, and imgui (#5797)
abb2857e03 [wpilib] Counter: Fix default distance per pulse, add distance and rate to C++ (#5796)
b14a61e1c0 [readme] Add link to QuickBuffers release page (#5795)
cf54d9ccb7 [wpiutil, ntcore] Add structured data support (#5391)
ecb7cfa9ef [wpimath] Add Exponential motion profile (#5720)
7c6fe56cf2 [ntcore] Fix crash on disconnect (#5788)
85147bf69e [wpinet] WebSocketSerializer: Fix UB (#5787)
244163acad [wpinet] uv::Stream::TryWrite(): Return 0 on EAGAIN (#5784)
820728503d [hal] Remove extra semicolon in RoboRioData (#5786)
45f307d87e [upstream_utils] Upgrade to LLVM 17.0.3 (#5785)
4ce4d63efc [wpilibj] Fix RobotBase.isSimulation() (#5783)
579007ceb3 [commands] Add requirements parameter to Commands.idle() (#5774)
3f3a169149 [wpilib] Make physics sim setState() functions public (#5779)
7501e4ac88 [wpilib] Close sim device in ADIS IMUs (#5776)
99630d2e78 [wpimath] Upgrade to EJML 0.43.1 (#5778)
02cbbc997d [wpimath] Make Vector-Vector binary operators return Vector (#5772)
ed93889e17 [examples] Fix typo in TimesliceRobot example name (#5773)
da70e4c262 [docs] Add jinja2 to CMake prerequisites (#5771)
e814595ea7 [wpimath] Add ChassisSpeeds.fromRobotRelativeSpeeds() (#5744)
f98c943445 [wpimath] LinearSystemId: Add DCMotorSystem overload (#5770)
b3eb64b0f7 [wpiutil] ct_string: Use inline namespace for literals (#5767)
7d9ba256c2 Revert "[build] Add CMake option to build Java source jars (#5756)" (#5766)
1f6492e3d8 [sysid] Update JSON library usage (#5765)
638f04f626 [wpiutil] Add protobuf to thirdparty sources (#5746)
210255bfff [wpiutil] Update json to 3.11.2 (#5680)
896772c750 [wpimath] Add DCMotor functions for Kraken X60 and Neo Vortex (#5759)
fd427f6c82 [wpimath] Fix hardcoded module count in SwerveDriveKinematics.resetHeading() (#5762)
c0b4c6cce6 [wpimath] Add overloads for Transform2d and Transform3d (#5757)
9a0aafd8ab [examples] Make swerve examples multiply desired module speeds by cosine of heading error (#5758)
1c724884ca [build] Add CMake option to build Java source jars (#5756)
5b0db6b93e [ci] Forward CI as well (#5755)
f8cbbbac12 [ci] Take 2 on passing GITHUB_REF (#5754)
b9944be09c [ci] Pass GITHUB_REF to docker container (#5753)
de5e4eda6c [build] Update apriltag, libssh, googletest for 2024 (#5752)
227e660e20 [upstream_utils] Upgrade to LLVM 17.0.2 (#5750)
36f94c9f21 [commands,romi,xrp] Add frcYear to vendordep (#5747)
741d166457 [glass] NT view: enhance array support (#5732)
1d23513945 [ntcore] Fix string array value comparison (#5745)
ff1849052e [commands] Make command scheduling order consistent (#5470)
58e8474368 [build] Disable armsimulation unit test (#5739)
fb07b0da49 [examples] Add XRP C++ Examples and Templates (#5743)
81893ad73d Run wpiformat with clang-format 17 (#5740)
faa1e665ba [wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715)
a789632052 [build] Update to native utils 2024.3.1 (#5738)
8f60ab5182 [build] Update OpenCV to 2024-4.8.0-1 (#5737)
33243f982b [wpimath] Expand Quaternion class with additional operators (#5600)
420f2f7c80 [ntcore] Add RTT-only subprotocol (#5731)
2b63e35ded [ntcore] Fix moving outgoing queue to new period (#5735)
be939cb636 [ntcore] Fix notification of SetDefaultEntryValue (#5733)
69a54de202 [build] Update enterprise plugin (#5730)
fef03a3ff5 [commands] Clean up C++ includes after Requirements was added (#5719)
8b7c6852cf [ntcore] Networking improvements (#5659)
1d19e09ca9 [wpiutil] Set WPI_{UN}IGNORE_DEPRECATED to empty when all else fails (#5728)
58141d6eb5 [wpilib] Make BooleanEvent more consistent (#5436)
6576d9b474 [wpilib] SendableChooser: implement Sendable instead of NTSendable (#5718)
a4030c670f [build] Update to gradle 8.4, enable win arm builds (#5727)
0960f11eba [wpinet] Revert removal of uv_clock_gettime() (#5723)
cb1bd0a3be [wpiutil] Get more precise system time on Windows (#5722)
4831277ffe [wpigui] Fix loading a maximized window on second monitor (#5721)
3eb372c25a [wpiutil] SendableBuilder: Add PublishConst methods (#5158)
1fec8596a4 [ci] Fix -dirty version (#5716)
f7e47d03f3 [build] Remove unnecessary CMake config installs (#5714)
a331ed2374 [sysid] Add SysId (#5672)
8d2cbfce16 [wpiutil] DataLog: Stop logging if insufficient free space (#5699)
48facb9cef [ntcoreffi] Add DataLogManager (#5702)
aecbcb08fc [ntcore] Correctly start DataLog for existing publishers (#5703)
5e295dfbda [wpiutil] DataLog: Limit total buffer allocation (#5700)
c7c7e05d9d [ci] Unbreak combiner (#5698)
c92bad52cb [wpilib] DataLogManager: Use system time valid function (#5697)
d404af5f24 [wpilib] RobotController: Add isSystemTimeValid() (#5696)
e56f1a3632 [ci] Run combine but skip all steps (#5695)
8f5bcad244 [ci] Use sccache for cmake builds (#5692)
703dedc4a6 [ci] Upgrade get-cmake action to fix node12 deprecation warning (#5694)
c69a0d7504 [ci] Don't run example unit test that segfaults (#5693)
66358d103e Add menu items for online docs to GUI tools (#5689)
4be8384a76 [ci] Disable combine on PR builds (#5691)
90288f06a6 [ci] Fix Gradle disk space issues (#5688)
9e9583412e [wpigui] Make wpi::gui::OpenURL() fork the process first (#5687)
d4fcd80b7b [ci] Gradle: Use container only for build step (#5684)
7b70e66772 [outlineviewer] Fix thirdparty library include sorting (#5683)
5f651df5d5 [build] Clean up Gradle configs (#5685)
65b26738d5 Add CMakeSettings.json to gitignore (#5682)
d0305951ad Fix GitHub inline warnings (#5681)
e8d4a20331 [build][cmake] Fix windows tests and re-enable CI tests (#5674)
2b58bbde0b [xrp] Add Reflectance sensor and rangefinder classes (#5673)
dd5612fbee [json] Add forward definition header (#5676)
eab44534c3 [wpimath] Remove unused SmallString include (#5677)
5ab54ff760 Replace wpi::raw_istream with wpi::MemoryBuffer (#5675)
1b6ec5a95d [wpiutil] Upgrade to LLVM 17.0.1 (#5482)
07a0d22fe6 [build] Build examples in CMake CI (#5667)
97021f074a [build] Upgrade imgui and implot (#5668)
87ce1e3761 [build] Fix wpilibNewCommands CMake install (#5671)
6ef94de9b5 [wpimath] Add tests for ArmFeedforward and ElevatorFeedforward (#5663)
c395b29fb4 [wpinet] Add WebSocket::TrySendFrames() (#5607)
c4643ba047 [romi/xrp] Fix version typo in vendordep json (#5664)
51dcb8b55a [examples] Make Romi/XRP Examples use appropriate vendordeps (#5665)
daf7702007 [build] Test each example in a new environment (#5662)
e67df8c180 [wpilib] Const-qualify EncoderSim getters (#5660)
7be290147c [wpiutil] Refactor SpanMatcher and TestPrinters from ntcore (#5658)
9fe258427a [commands] Add proxy factory to Commands (#5603)
633c5a8a22 [commands] Add C++ Requirements struct (#5504)
b265a68eea [commands] Add interruptor parameter to onCommandInterrupt callbacks (#5461)
e93c233d60 [ntcore] Compute Value memory size when creating value (#5657)
5383589f99 [wpinet] uv::Request: Return shared_ptr from Release() (#5656)
40b552be4a [wpinet] uv::Stream: Return error from TryWrite() (#5655)
202a75fe08 [wpinet] RequestImpl: Avoid infinite loop in shared_from_this() (#5654)
8896515eb7 [wpinet] uv::Buffer: Add bytes() accessor (#5653)
ae59a2fba2 [wpinet] uv::Error: Change default error to 0 (#5652)
3b51ecc35b [wpiutil] SpanExtras: Add take_back and take_front (#5651)
17f1062885 Replace std::snprintf() with wpi::format_to_n_c_str() (#5645)
bb39900353 [romi/xrp] Add Romi and XRP Vendordeps (#5644)
cb99517838 [build] cmake: Use default install location on windows for dlls (#5580)
25b0622d4c [build] Add Windows CMake CI (#5516)
34e7849605 Add warning to development builds instructions (NFC) (#5646)
e9e611c9d8 [cameraserver] Remove CameraServer.SetSize() (#5650)
94f58cc536 [wpilib] Remove Compressor.Enabled() (#5649)
4da5aee88a [wpimath] Remove SlewRateLimiter 2 argument constructor (#5648)
2e3ddf5502 Update versions in development builds instructions to 2024 (#5647)
19a8850fb1 [examples] Add TimesliceRobot templates (#3683)
9047682202 [sim] Add XRP-specific plugin (#5631)
575348b81c [wpilib] Use IsSimulation() consistently (#3534)
12e2043b77 [wpilib] Clean up Notifier (#5630)
4bac4dd0f4 [wpimath] Move PIDController from frc2 to frc namespace (#5640)
494cfd78c1 [wpiutil] Fix deprecation warning in LLVM for C++23 (#5642)
43a727e868 [apriltag] Make loadAprilTagFieldLayout throw an unchecked exception instead (#5629)
ad4b017321 [ci] Use Ninja for faster builds (#5626)
4f2114d6f5 Fix warnings from GCC 13 release build (#5637)
e7e927fe26 [build] Also compress debug info for CMake RelWithDebInfo build type (#5638)
205a40c895 [build] Specify zlib for debug info compression (#5636)
707444f000 [apriltag] Suppress -Wtype-limits warning in asserts from GCC 13 (#5635)
3b79cb6ed3 [commands] Revert SubsystemBase deprecation/removal (#5634)
bc7f23a632 [build] Compress Linux debug info (#5633)
57b2d6f254 [build] Update to image 2024 v1.0 (#5625)
339ef1ea39 [wpilib] DataLogManager: Warn user if logging to RoboRIO 1 internal storage (#5617)
7a9a901a73 [build] Fix cmake config files (#5624)
298f8a6e33 [wpilib] Add Mechanism2d tests and make Java impl match C++ (#5527)
d7ef817bae [apriltag] Update apriltag library (#5619)
c3fb31fd0e [docs] Switch to Java 17 api docs (#5613)
bd64f81cf9 [build] Run Google tests in release mode in CI (#5615)
66e6bd81ea [wpimath] Cleanup wpimath/algorithms.md (NFC) (#5621)
4fa56fd884 [build] Add missing find_dependency call (#5623)
f63d958995 [build] Update to native utils 2024.2.0 (#5601)
a9ab08f48b [wpimath] Rename ChassisSpeeds.fromDiscreteSpeeds() to discretize() (#5616)
8e05983a4a [wpimath] Add math docs to plant inversion feedforward internals (NFC) (#5618)
3a33ce918b [ntcore] Add missing StringMap include (#5620)
a6157f184d [wpiutil] timestamp: Add ShutdownNowRio (#5610)
e9f612f581 [build] Guard policy setting for CMake versions below 3.24 (#5612)
1a6df6fec6 [wpimath] Fix DARE Q decomposition (#5611)
9b3f7fb548 [build] Exclude IntelliJ folders from spotless XML (#5602)
814f18c7f5 [wpimath] Fix computation of C for DARE (A, C) detectability check (#5609)
ac23f92451 [hal] Add GetTeamNumber (#5596)
a750bee54d [wpimath] Use std::norm() in IsStabilizable() (#5599)
8e2465f8a0 [wpimath] Add arithmetic functions to wheel speeds classes (#5465)
10d4f5b5df [wpimath] Clean up notation in DARE precondition docs (#5595)
b2dd59450b [hal] Fix unfinished/incorrect GetCPUTemp functions (#5598)
99f66b1e24 [wpimath] Replace frc/EigenCore.h typedefs with Eigen's where possible (#5597)
383289bc4b [build] Make custom CMake macros use lowercase (#5594)
45e7720ec1 [build] Add error message when downloading files in CMake (#5593)
4e0d785356 [wpimath] ChassisSpeeds: document that values aren't relative to the robot (NFC) (#5551)
3c04580a57 [commands] ProxyCommand: Use inner command name in unique_ptr constructor (#5570)
cf19102c4a [commands] SelectCommand: Fix leakage and multiple composition bug (#5571)
171375f440 [ntcoreffi] Link to NI libraries (#5589)
89add5d05b Disable flaky tests (#5591)
a8d4b162ab [ntcore] Remove RPC manual tests (#5590)
39a73b5b58 [commands] C++: Add CommandPtr supplier constructor to ProxyCommand (#5572)
36d514eae7 [commands] Refactor C++ ScheduleCommand to use SmallSet (#5568)
52297ffe29 [commands] Add idle command (#5555)
67043a8eeb [wpimath] Add angular jerk unit (#5582)
51b0fb1492 [wpimath] Fix incorrect header inclusion in angular_acceleration.h (#5587)
b7657a8e28 [wpimath] Split WPIMathJNI into logical chunks (#5552)
ea17f90f87 [build] Fix tool builds with multiple arm platforms installed (#5586)
f1d7b05723 [wpimath] Clean up unit formatter (#5584)
d7264ff597 Replace wpi::errs() usage with fmtlib (#5560)
ab3bf39e0e [wpiutil] Upgrade to fmt 10.1.1 (#5585)
165ebe4c79 Upgrade to fmt 10.1.0 (#5326)
8e2a7fd306 Include thirdparty libraries with angle brackets (#5578)
e322ab8e46 [wpimath] Fix docs for DARE ABQRN stabilizability check (NFC) (#5579)
360fb835f4 [upstream_utils] Handle edge case in filename matches (#5576)
9d86624c00 [build] Fix CMake configure warnings (#5577)
969979d6c7 [wpiutil] Update to foonathan memory 0.7-3 (#5573)
0d2d989e84 [wpimath] Update to gcem 1.17.0 (#5575)
cf86af7166 [wpiutil] Update to mpack 1.1.1 (#5574)
a0c029a35b [commands] Fix dangling SelectCommand documentation (NFC) (#5567)
349141b91b [upstream_utils] Document adding a patch (NFC) (#5432)
7889b35b67 [wpimath] Add RamseteController comparison to LTV controller docs (NFC) (#5559)
b3ef536677 [build] Ignore nt/sim json files in spotless (#5565)
ed895815b5 [build] Compile Java with UTF-8 encoding (#5564)
2e4ad35e36 [wpiutil] jni_util: Add JSpan and CriticalJSpan (#5554)
8f3d6a1d4b [wpimath] Remove discretizeAQTaylor() (#5562)
7c20fa1b18 [wpimath] Refactor DARE tests to reduce RAM usage at compile time (#5557)
89e738262c [ntcore] Limit buffer pool size to 64KB per connection (#5485)
96f7fa662e Upgrade Maven dependencies (#5553)
7a2d336d52 [wpinet] Leak multicast handles during windows shutdown (#5550)
f9e2757d8f [wpimath] Use JDoubleArrayRef in all JNI functions (#5546)
0cf6e37dc1 [wpimath] Make LTV controller constructors use faster DARE solver (#5543)
6953a303b3 [build] Fix the windows build with fmt (#5544)
7a37e3a496 [wpimath] Correct Rotation3d::RotateBy doc comment (NFC) (#5541)
186b409e16 [wpimath] Remove internal Eigen header include (#5539)
03764dfe93 [wpimath] Add static matrix support to DARE solver (#5536)
394cfeadbd [wpimath] Use SDA algorithm instead of SSCA for DARE solver (#5526)
a4b7fde767 [wpilib] Add mechanism specific SetState overloads to physics sims (#5534)
8121566258 [wpimath] Fix CoordinateSystem.convert() Transform3d overload (#5532)
b542e01a0b [glass] Fix array crash when clearing existing workspace (#5535)
e2e1b763b2 [wpigui] Fix PFD file dialogs not closing after window closing (#5530)
86d7bbc4e4 [examples] Add Java Examples and Templates for the XRP (#5529)
e8b5d44752 [wpimath] Make Java Quaternion use doubles instead of Vector (#5525)
38c198fa64 [myRobot] Add apriltags to myRobot build (#5528)
00450c3548 [wpimath] Upgrade to EJML 0.42 (#5531)
faf3cecd83 [wpimath] Don't copy Matrix and underlying storage in VecBuilder (#5524)
6b896a38dc [build] Don't enforce WITH_FLAT_INSTALL with MSVC (part 2) (#5517)
c01814b80e [wpiutil] Add C API for DataLog (#5509)
b5bd0771eb [wpimath] Document extrinsic vs intrinsic rotations (NFC) (#5508)
84ed8aec05 [build] Don't enforce WITH_FLAT_INSTALL with MSVC (#5515)
999f677d8c [ntcoreffi] Add WPI_Impl_SetupNowRio to exported symbols (#5510)
338f37d302 Fix header sorting of libssh (#5507)
75cbd9d6d0 [glass] Add background color selector to glass plots (#5506)
e2c190487b [examples] Add flywheel bang-bang controller example (#4071)
c52dad609e [wpinet] WebSocket: Send pong in response to ping (#5498)
e2d17a24a6 [hal] Expose power rail disable and cpu temp functionality (#5477)
3ad5d2e42d [hal,wpiutil] Use HMB for FPGA Timestamps (#5499)
b46a872494 [ntcore] Remove pImpl from implementation (#5480)
d8c59ccc71 [wpimath] Add tests for MathUtil clamp() and interpolate() (#5501)
0552c8621d [glass,ov] Improve Glass and OutlineViewer title bar message (#5502)
90e37a129f [wpiutil,wpimath] Add generic InterpolatingTreeMap (#5372)
d83a6edc20 [wpilib] Update GetMatchTime docs and units (#5232)
6db2c42966 [wpimath] Trajectory: Throw on empty lists of States (#5497)
21439b606c [wpimath] Disallow LTV controller max velocities above 15 m/s (#5495)
7496e0d208 [ntcore] Value: More efficiently store arrays (#5484)
0c93aded8a [wpimath] Change kinematics.ToTwist2d(end - start) to kinematics.ToTwist2d(start, end) (#5493)
815a8403e5 [wpimath] Give infeasible trajectory constraints a better exception message (#5492)
35a8b129d9 [wpimath] Add RotateBy() function to pose classes (#5491)
26d6e68c8f [upstream_utils] Add GCEM to CI (#5483)
6aa469ae45 [wpilib] Document how to create LinearSystem object for physics sim classes (NFC) (#5488)
a01b6467d3 [wpimath] Link to docs on LQR and KF tolerances (#5486)
d814f1d123 [wpimath] Fix copy-paste error from Pose2d docs (NFC) (#5490)
98f074b072 [wpimath] Add folder prefix to geometry includes (#5489)
e9858c10e9 [glass] Add tooltips for NT settings (#5476)
12dda24f06 [examples] Fix C robot template not correctly looping (#5474)
fc75d31755 [apriltag] Update apriltaglib (#5475)
a95994fff6 [wpiutil] timestamp: Call FPGA functions directly (#5235)
2ba8fbb6f4 [wpimath] Improve documentation for SwerveModulePosition::operator- (#5468)
b8cdf97621 [build] Prepare for Windows arm64 builds (#5390)
552f4b76b5 [wpimath] Add FOC-enabled Falcon constants to the DCMotor class (#5469)
1938251436 [examples] Add Feedforward to ElevatorProfiledPid (#5300)
873c2a6c10 [examples] Update ElevatorTrapezoidProfile example (#5466)
99b88be4f3 [wpilib] Reduce usage of NTSendable (#5434)
d125711023 [hal] Fix Java REVPH faults bitfield (take 2) (#5464)
c3fab7f1f2 [ntcore] Don't update timestamp when value is unchanged (#5356)
5ec7f18bdc [wpilib] EventLoop docs: Remove BooleanEvent references (NFC) (#5463)
c065ae1fcf [wpiunits] Add subproject for a Java typesafe unit system (#5371)
44acca7c00 [wpiutil] Add ClassPreloader (#5365)
88b11832ec [hal] Fix Java REVPH faults bitfield (#5148)
fb57d82e52 [ntcore] Enhance Java raw value support
3a6e40a44b [wpiutil] Enhance DataLog Java raw value support
8dae5af271 [wpiutil] Add compile-time string utilities (ct_string) (#5462)
fc56f8049a [wpilib] DriverStation: Change alliance station to use optional (#5229)
ef155438bd [build] Consume libuv via cmake config instead of via pkg-config (#5438)
86e91e6724 [wpimath] Refactor TrapezoidProfile API (#5457)
72a4543493 [wpilib] DutyCycleEncoderSim: Expand API (#5443)
657338715d [wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425)
1af224c21b Add missing <functional> includes (#5459)
0b91ca6d5a [wpilib] SendableChooser: Add onChange listener (#5458)
6f7cdd460e [wpimath] Pose3d: Switch to JNI for exp and log (#5444)
c69e34c80c [wpimath] ChassisSpeeds: Add arithmetic functions (#5293)
335e7dd89d [wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288)
14f30752ab [wpilib] Deprecate Accelerometer and Gyro interfaces (#5445)
70b60e3a74 [commands] Trigger: Fix method names in requireNonNullParam (#5454)
593767c8c7 [wpimath] Improve Euler angle calculations in gimbal lock (#5437)
daf022d3da [build] Make devImplementation inherit from implementation (#5450)
9b8d90b852 [examples] Convert the unitless joystick inputs to actual physical units (#5451)
1f6428ab63 [ntcore] Fix undefined comparison behavior when array is empty (#5448)
17eb9161cd Update code owners for removal of old commands (#5447)
3c4b58ae1e [wpinet] Upgrade to libuv 1.46.0 (#5446)
aaea85ff16 [commands] Merge CommandBase into Command and SubsystemBase into Subsystem (#5392)
7ac932996a [ci] Use PAT for workflow dispatch (#5442)
efe1987e8b [ci] Trigger pages repo workflow (#5441)
828bc5276f [wpiutil] Upgrade to LLVM 16.0.6 (#5435)
701df9eb87 [ci] Change documentation publish to single-commit (#5440)
e5452e3f69 [wpiutil] Add WPICleaner and an example how to use it (#4850)
7a099cb02a [commands] Remove deprecated classes and functions (#5409)
b250a03944 [wpilib] Add function to wait for DS Connection (#5230)
a6463ed761 [wpiutil] Fix unused variable warning in release build (#5430)
f031513470 [ntcore] NetworkTable::GetSubTables(): Remove duplicates (#5076)
f8e74e2f7c [hal] Unify PWM simulation Speed, Position, and Raw (#5277)
fd5699b240 Remove references to Drake (#5427)
e2d385d80a [build] cmake: Respect USE_SYSTEM_FMTLIB (#5429)
d37f990ce3 [hal] Fix HAL Relay/Main doc module (NFC) (#5422)
a7a8b874ac [docs] Expand HAL_ENUM in doxygen docs (#5421)
3a61deedde [wpimath] Rotation2d: Only use gcem::hypot when constexpr evaluated (#5419)
96145de7db [examples] Fix formatting (NFC) (#5420)
fffe6a7b9a [examples] Improve Pneumatics example coverage in Solenoid and RapidReactCmdBot examples (#4998)
6b5817836d [wpimath] Add tolerance for some tests (#5416)
3233883f3e [cscore] Fix warnings on macos arm (#5415)
c4fc21838f [commands] Add ConditionalCommand getInterruptionBehavior (#5161)
89fc51f0d4 Add tests for SendableChooser and Command Sendable functionality (#5179)
663bf25aaf [docs] Generate docs for symbols in __cplusplus (#5412)
fe32127ea8 [command] Clean up Command doc comments (NFC) (#5321)
c1a01569b4 [wpilib][hal] PWM Raw using microseconds (#5283)
1fca519fb4 [wpiutil] Remove remnants of ghc fs and tcb_span libraries (#5411)
90602cc135 [github] Update issue template to collect more project info (#5090)
34412ac57e [build] Exclude files in bin from Spotless (#5410)
61aa60f0e3 [wpilib] Add robot callback that is called when the DS is initially connected (#5231)
ebae341a91 [commands] Add test for subsystem registration and periodic (#5408)
5d3a133f9f Remove spaces in NOLINT comments (#5407)
3a0e484691 [wpimath] Fix clang-tidy warnings (#5403)
eb3810c765 [wpiutil] Fix clang-tidy warnings (#5406)
c4dc697192 [hal] WS Simulation: Add message filtering capability (#5395)
0eccc3f247 [ntcore] Fix clang-tidy warnings (#5405)
f4dda4bac0 [hal] Add javadocs for JNI (NFC) (#5298)
1c20c69793 [cscore] Fix clang-tidy warnings (#5404)
1501607e48 [commands] Fix clang-tidy warnings (#5402)
991f4b0f62 [wpimath] PIDController: Add IZone (#5315)
f5b0d1484b [wpimath] Add isNear method to MathUtil (#5353)
2ce248f66c [hal] Fix clang-tidy warnings (#5401)
5fc4aee2d2 [wpimath] SwerveDriveKinematics: Rename currentChassisSpeed to desiredChassisSpeed (#5393)
50b90ceb54 [wpimath] SwerveDriveKinematics: Add reset method (#5398)
316cd2a453 [commands] Notify DriverStationSim in CommandTestBaseWithParam (#5400)
d4ea5fa902 [cscore] VideoMode: Add equals override (Java) (#5397)
d6bd72d738 [wpimath] ProfiledPIDController: Add getConstraints (#5399)
25ad5017a9 [wpimath] Refactor kinematics, odometry, and pose estimator (#5355)
5c2addda0f [doc] Add missing pneumatics docs (NFC) (#5389)
c3e04a6ea2 Fix loading tests on macos 12 (#5388)
d5ed9fb859 [wpimath] Create separate archive with just units headers (#5383)
901ab693d4 [wpimath] Use UtilityClassTest for more utility classes (#5384)
9d53231b01 [wpilib] DataLogManager: Add warning for low storage space (#5364)
d466933963 [wpiutil] Group doxygen into MPack module (#5380)
652d1c44e3 [wpiutil] Upgrade to macOS 12 to remove concept shims (#5379)
6414be0e5d [wpimath] Group units doxygen modules (#5382)
7ab5800487 [wpiutil] Fix docs typo in SmallVector (#5381)
59905ea721 Replace WPI_DEPRECATED() macro with [[deprecated]] attribute (#5373)
753cb49a5e [ntcore] Fix doxygen module in generated C types (NFC) (#5374)
1c00a52b67 [hal] Expose CAN timestamp base clock (#5357)
91cbcea841 Replace SFINAE with concepts (#5361)
d57d1a4598 [wpimath] Remove unnecessary template argument from unit formatter (#5367)
5acc5e22aa [wpimath] Only compute eigenvalues with EigenSolvers (#5369)
d3c9316a97 extend shuffleboard test timeout (#5377)
1ea868081a [ci] Fix /format command (#5376)
5fac18ff4a Update formatting to clang-format 16 (#5370)
a94a998002 [wpimath] Generalize Eigen formatter (#5360)
125f6ea101 [wpimath] Make SwerveDriveKinematics::ToChassisSpeeds() take const-ref argument (#5363)
51066a5a8a [wpimath] Move unit formatters into units library (#5358)
282c032b60 [wpilibc] Add unit-aware Joystick.GetDirection() (#5319)
073d19cb69 [build] Fix CMake warning (#5359)
01490fc77b [wpiutil] DataLog: Add documentation for append methods (NFC) (#5348)
c9b612c986 [wpilibcExamples] Make C++ state-space elevator KF and LQR match Java (#5346)
eed1e6e3cb [wpimath] Replace DiscretizeAQTaylor() with DiscretizeAQ() (#5344)
c976f40364 [readme] Document how to run examples in simulation (#5340)
4d28bdc19e [ci] Update Github Pages deploy action parameters (#5343)
e0f851871f [ci] Fix github pages deploy version (#5342)
063c8cbedc Run wpiformat (NFC) (#5341)
96e41c0447 [ci] Update deploy and sshagent actions (#5338)
fd294bdd71 [build] Fix compilation with GCC 13 (#5322)
d223e4040b [dlt] Add delete without download functionality (#5329)
abc19bcb43 [upstream_utils] Zero out commit hashes and show 40 digits in index hashes (#5336)
e909f2e687 [build] Update gradle cache repo name (#5334)
52bd5b972d [wpimath] Rewrite DARE solver (#5328)
3876a2523a [wpimath] Remove unused MatrixImpl() function (#5330)
c82fcb1975 [wpiutil] Add reflection based cleanup helper (#4919)
15ba95df7e [wpiutil] Use std::filesystem (#4941)
77c2124fc5 [wpimath] Remove Eigen's custom STL types (#4945)
27fb47ab10 [glass] Field2D: Embed standard field images (#5159)
102e4f2566 [wpilib] Remove deprecated and broken SPI methods (#5249)
463a90f1df [wpilib, hal] Add function to read the RSL state (#5312)
7a90475eec [wpilib] Update RobotBase documentation (NFC) (#5320)
218cfea16b [wpilib] DutyCycleEncoder: Fix reset behavior (#5287)
91392823ff [build] Update to gradle 8.1 (#5303)
258b7cc48b [wpilibj] Filesystem.getDeployDirectory(): Strip JNI path from user.dir (#5317)
26cc43bee1 [wpilib] Add documentation to SPI mode enum (NFC) (#5324)
ac4da9b1cb [hal] Add HAL docs for Addressable LED (NFC) (#5304)
21d4244cf7 [wpimath] Fix DCMotor docs (NFC) (#5309)
1dff81bea7 [hal] Miscellaneous HAL doc fixes (NFC) (#5306)
7ce75574bf [wpimath] Upgrade to Drake v1.15.0 (#5310)
576bd646ae [hal] Add CANManufacturer for Redux Robotics (#5305)
ee3b4621e5 [commands] Add onlyWhile and onlyIf (#5291)
40ca094686 [commands] Fix RepeatCommand calling end() twice (#5261)
9cbeb841f5 [rtns] Match imaging tool capitalization (#5265)
a63d06ff77 [examples] Add constants to java gearsbot example (#5248)
b6c43322a3 [wpilibc] XboxController: Add return tag to docs (NFC) (#5246)
5162d0001c [hal] Fix and document addressable LED timings (#5272)
90fabe9651 [wpilibj] Use method references in drive class initSendable() (#5251)
24828afd11 [wpimath] Fix desaturateWheelSpeeds to account for negative speeds (#5269)
e099948a77 [wpimath] Clean up rank notation in docs (NFC) (#5274)
fd2d8cb9c1 [hal] Use std::log2() for base-2 logarithm (#5278)
ba8c64bcff [wpimath] Fix misspelled Javadoc parameters in pose estimators (NFC) (#5292)
f53c6813d5 [wpimath] Patch Eigen warnings (#5290)
663703d370 [gitattributes] Mark json files as lf text files (#5256)
aa34aacf6e [wpilib] Shuffleboard: Keep duplicates on SelectTab() (#5198)
63512bbbb8 [wpimath] Fix potential divide-by-zero in RKDP (#5242)
9227b2166e [wpilibj] DriverStation: Fix joystick data logs (#5240)
fbf92e9190 [wpinet] ParallelTcpConnector: don't connect to duplicate addresses (#5169)
2108a61362 [ntcore] NT4 client: close timed-out connections (#5175)
0a66479693 [ntcore] Optimize scan of outgoing messages (#5227)
b510c17ef6 [hal] Fix RobotController.getComments() mishandling quotes inside the comments string (#5197)
e7a7eb2e93 [commands] WaitCommand: Remove subclass doc note (NFC) (#5200)
a465f2d8f0 [examples] Shuffleboard: Correct parameter order (#5204)
a3364422fa LICENSE.md: Bump year to 2023 (#5195)
df3242a40a [wpimath] Fix NaN in C++ MakeCostMatrix() that takes an array (#5194)
00abb8c1e0 [commands] RamseteCommand: default-initialize m_prevSpeeds (#5188)
c886273fd7 [wpilibj] DutyCycleEncoder.setDistancePerRotation(): fix simulation (#5147)
53b5fd2ace [ntcore] Use int64 for datalog type string (#5186)
56b758320f [wpilib] DataLogManager: increase time for datetime to be valid (#5185)
08f298e4cd [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-8 and 1E-7 (#5168)
6d0c5b19db [commands] CommandScheduler.isComposed: Remove incorrect throws clause (NFC) (#5183)
0d22cf5ff7 [wpilib] Fix enableLiveWindowInTest crashing in disabled (#5173)
32ec5b3f75 [wpilib] Add isTestEnabled and minor docs cleanup (#5172)
e5c4c6b1a7 [wpimath] Fix invalid iterator access in TimeInterpolatableBuffer (#5138)
099d048d9e [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-9 and 1E-8 (#5143)
4af84a1c12 Fix Typos (NFC) (#5137)
ce3686b80d [wpimath] Check LTV controller max velocity precondition (#5142)
4b0eecaee0 [commands] Subsystem: Add default command removal method (#5064)
edf4ded412 [wpilib] PH: Revert to 5V rail being fixed 5V (#5122)
4c46b6aff9 [wpilibc] Fix DataLogManager crash on exit in sim (#5125)
490ca4a68a [wpilibc] Fix XboxController::GetBackButton doc (NFC) (#5131)
cbb5b0b802 [hal] Simulation: Fix REV PH solenoids 8+ (#5132)
bb7053d9ee [hal] Fix HAL_GetRuntimeType being slow on the roboRIO (#5130)
9efed9a533 Update .clang-format to c++20 (#5121)
dbbfe1aed2 [wpilib] Use PH voltage to calc Analog pressure switch threshold (#5115)
de65a135c3 [wpilib] DutyCycleEncoderSim: Add channel number constructor (#5118)
3e9788cdff [docs] Strip path from generated NT docs (#5119)
ecb072724d [ntcore] Client::Disconnect(): actually close connection (#5113)
0d462a4561 [glass] NT view: Change string/string array to quoted (#5111)
ba37986561 [ntcore] NetworkClient::Disconnect: Add null check (#5112)
25ab9cda92 [glass,ov] Provide menu item to create topic from root (#5110)
2f6251d4a6 [glass] Set default value when publishing new topic (#5109)
e9a7bed988 [wpimath] Add timestamp getter to MathShared (#5091)
9cc14bbb43 [ntcore] Add stress test to dev executable (#5107)
8068369542 [wpinet] uv: Stop creating handles when closing loop (#5102)
805c837a42 [ntcore] Fix use-after-free in server (#5101)
fd18577ba0 [commands] Improve documentation of addRequirements (NFC) (#5103)
74dea9f05e [wpimath] Fix exception for empty pose buffer in pose estimators (#5106)
9eef79d638 [wpilib] PneumaticHub: Document range of enableCompressorAnalog (NFC) (#5099)
843574a810 [ntcore] Use wpi::Now instead of loop time for transmit time
226ef35212 [wpinet] WebSocket: Reduce server send frame overhead
b30664d630 [ntcore] Reduce initial connection overhead
804e5ce236 [examples] MecanumDrive: Fix axis comment in C++ example (NFC) (#5096)
49af88f2bb [examples] ArmSimulation: Fix flaky test (#5093)
d56314f866 [wpiutil] Disable mock time on the Rio (#5092)
43975ac7cc [examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052)
5483464158 [examples, templates] Improve descriptions (NFC) (#5051)
785e7dd85c [wpilibc] SendableChooser: static_assert copy- and default-constructibility (#5078)
e57ded8c39 [ntcore] Improve disconnect error reporting (#5085)
01f0394419 [wpinet] Revert WebSocket: When Close() is called, call closed immediately (#5084)
59be120982 [wpimath] Fix Pose3d exp()/log() and add rotation vector constructor to Rotation3d (#5072)
37f065032f [wpilib] Refactor TimedRobot tests (#5068)
22a170bee7 [wpilib] Add Notifier test (#5070)
2f310a748c [wpimath] Fix DCMotor.getSpeed() (#5061)
b43ec87f57 [wpilib] ElevatorSim: Fix WouldHitLimit methods (#5057)
19267bef0c [ntcore] Output warning on property set on unpublished topic (#5059)
84cbd48d84 [ntcore] Handle excludeSelf on SetDefault (#5058)
1f35750865 [cameraserver] Add GetInstance() to all functions (#5054)
8230fc631d [wpilib] Revert throw on nonexistent SimDevice name in SimDeviceSim (#5053)
b879a6f8c6 [wpinet] WebSocket: When Close() is called, call closed immediately (#5047)
49459d3e45 [ntcore] Change wire timeout to fixed 1 second (#5048)
4079eabe9b [wpimath] Discard stale pose estimates (#5045)
fe5d226a19 [glass] Fix option for debug-level NT logging (#5049)
b7535252c2 [ntcore] Don't leak buffers in rare WS shutdown case (#5046)
b61ac6db33 [ntcore] Add client disconnect function (#5022)
7b828ce84f [wpimath] Add nearest to Pose2d and Translation2d (#4882)
08a536291b [examples] Improvements to Elevator Simulation Example (#4937)
193a10d020 [wpigui] Limit frame rate to 120 fps by default (#5030)
7867bbde0e [wpilib] Clarify DS functions provided by FMS (NFC) (#5043)
fa7c01b598 [glass] Add option for debug-level NT logging (#5007)
2b81610248 [wpiutil] Add msgpack to datalog Python example (#5032)
a4a369b8da CONTRIBUTING.md: Add unicodeit CLI to math docs guidelines (#5031)
d991f6e435 [wpilib] Throw on nonexistent SimDevice name in SimDeviceSim constructor (#5041)
a27a047ae8 [hal] Check for null in getSimDeviceName JNI (#5038)
2f96cae31a [examples] Hatchbots: Add telemetry (#5011)
83ef8f9658 [simulation] GUI: Fix buffer overflow in joystick axes copy (#5036)
4054893669 [commands] Fix C++ Select() factory (#5024)
f75acd11ce [commands] Use Timer.restart() (#5023)
8bf67b1b33 [wpimath] PIDController::Calculate(double, double): update setpoint flag (#5021)
49bb1358d8 [wpiutil] MemoryBuffer: Fix GetMemoryBufferForStream (#5017)
9c4c07c0f9 [wpiutil] Remove NDEBUG check for debug-level logging (#5018)
1a47cc2e86 [ntcore] Use full handle when subscribing (#5013)
7cd30cffbc Ignore networktables.json (#5006)
92aecab2ef [commands] Command controllers are not subclasses (NFC) (#5000)
8785bba080 [ntcore] Special-case default timestamps (#5003)
9e5b7b8040 [ntcore] Handle topicsonly followed by value subscribe (#4991)
917906530a [wpilib] Add Timer::Restart() (#4963)
00aa66e4fd [wpimath] Remove extraneous assignments from DiscretizeAB() (#4967)
893320544a [examples] C++ RamseteCommand: Fix units (#4954)
b95d0e060d [wpilib] XboxController: Fix docs discrepancy (NFC) (#4993)
008232b43c [ntcore] Write empty persistent file if none found (#4996)
522be348f4 [examples] Rewrite tags (NFC) (#4961)
d48a83dee2 [wpimath] Update Wikipedia links for quaternion to Euler angle conversion (NFC) (#4995)
504fa22143 [wpimath] Workaround intellisense Eigen issue (#4992)
b2b25bf09f [commands] Fix docs inconsistency for toggleOnFalse(Command) (NFC) (#4978)
ce3dc4eb3b [hal] Properly use control word that is in sync with DS data (#4989)
1ea48caa7d [wpilib] Fix C++ ADXRS450 and Java SPI gyro defs (#4988)
fb101925a7 [build] Include wpimathjni in commands binaries (#4981)
657951f6dd [starter] Add a process starter for use by the installer for launching tools (#4931)
a60ca9d71c [examples] Update AprilTag field load API usage (#4975)
f8a45f1558 [wpimath] Remove print statements from tests (#4977)
ecba8b99a8 [examples] Fix swapped arguments in MecanumControllerCommand example (#4976)
e95e88fdf9 [examples] Add comment to drivedistanceoffboard example (#4877)
371d15dec3 [examples] Add Computer Vision Pose Estimation and Latency Compensation Example (#4901)
cb9b8938af [sim] Enable docking in the GUI (#4960)
3b084ecbe0 [apriltag] AprilTagFieldLayout: Improve API shape for loading builtin JSONs (#4949)
27ba096ea1 [wpilib] Fix MOI calculation error in SingleJointedArmSim (#4968)
42c997a3c4 [wpimath] Fix Pose3d exponential and clean up Pose3d logarithm (#4970)
5f1a025f27 [wpilibj] Fix typo in MecanumDrive docs (NFC) (#4969)
0ebf79b54c [wpimath] Fix typo in Pose3d::Exp() docs (NFC) (#4966)
a8c465f3fb [wpimath] HolonomicDriveController: Add getters for the controllers (#4948)
a7b1ab683d [wpilibc] Add unit test for fast deconstruction of GenericHID (#4953)
bd6479dc29 [build] Add Spotless for JSON (#4956)
5cb0340a8c [hal, wpilib] Load joystick values upon code initialization (#4950)
ab0e8c37a7 [readme] Update build requirements (NFC) (#4947)
b74ac1c645 [build] Add apriltag to C++ cmake example builds (#4944)
cf1a411acf [examples] Add example programs for AprilTags detection (#4932)
1e05b21ab5 [wpimath] Fix PID atSetpoint to not return true prematurely (#4906)
e5a6197633 [wpimath] Fix SwerveDriveKinematics not initializing a new array each time (#4942)
039edcc23f [ntcore] Queue current value on subscriber creation (#4938)
f7f19207e0 [wpimath] Allow multiple vision measurements from same timestamp (#4917)
befd12911c [commands] Delete UB-causing rvalue variants of CommandPtr methods (#4923)
34519de60a [commands] Fix spacing in command composition exception (#4924)
dc4355c031 [hal] Add handle constructor and name getters for sim devices (#4925)
53d8d33bca [hal, wpilibj] Add missing distance per pulse functions to EncoderSim (#4928)
530ae40614 [apriltag] Explain what April tag poses represent (NFC) (#4930)
79f565191e [examples] DigitalCommunication, I2CCommunication: Add tests (#4865)
2cd9be413f [wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893)
babb0c1fcf [apriltag] Add 2023 field layout JSON (#4912)
330ba45f9c [wpimath] Fix swerve kinematics util classes equals function (#4907)
51272ef6b3 [fieldImages] Add 2023 field (#4915)
0d105ab771 [commands] Deduplicate command test utils (#4897)
cf4235ea36 [wpiutil] Guard MSVC pragma in SymbolExports.h (#4911)
2d4b7b9147 [build] Update opencv version in opencv.gradle (#4909)
aec6f3d506 [ntcore] Fix client flush behavior (#4903)
bfe346c76a [build] Fix cmake java resources (#4898)
Change-Id: Ia1dd90fe42c6cd5df281b8a5b710e136f54355f4
git-subtree-dir: third_party/allwpilib
git-subtree-split: f1a82828fed8950f9a3f1586c44327027627a0c8
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/wpimath/src/main/native/cpp/MathShared.cpp b/wpimath/src/main/native/cpp/MathShared.cpp
index 5252e87..b53d080 100644
--- a/wpimath/src/main/native/cpp/MathShared.cpp
+++ b/wpimath/src/main/native/cpp/MathShared.cpp
@@ -5,6 +5,9 @@
#include "wpimath/MathShared.h"
#include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
using namespace wpi::math;
@@ -15,6 +18,9 @@
void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
}
void ReportUsage(MathUsageId id, int count) override {}
+ units::second_t GetTimestamp() override {
+ return units::second_t{wpi::Now() * 1.0e-6};
+ }
};
} // namespace
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index fc532db..758d2e7 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -6,16 +6,16 @@
namespace frc {
-Vectord<3> PoseTo3dVector(const Pose2d& pose) {
- return Vectord<3>{pose.Translation().X().value(),
- pose.Translation().Y().value(),
- pose.Rotation().Radians().value()};
+Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
+ return Eigen::Vector3d{pose.Translation().X().value(),
+ pose.Translation().Y().value(),
+ pose.Rotation().Radians().value()};
}
-Vectord<4> PoseTo4dVector(const Pose2d& pose) {
- return Vectord<4>{pose.Translation().X().value(),
- pose.Translation().Y().value(), pose.Rotation().Cos(),
- pose.Rotation().Sin()};
+Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
+ return Eigen::Vector4d{pose.Translation().X().value(),
+ pose.Translation().Y().value(), pose.Rotation().Cos(),
+ pose.Rotation().Sin()};
}
template <>
@@ -28,9 +28,15 @@
return detail::IsStabilizableImpl<2, 1>(A, B);
}
-Vectord<3> PoseToVector(const Pose2d& pose) {
- return Vectord<3>{pose.X().value(), pose.Y().value(),
- pose.Rotation().Radians().value()};
+template <>
+bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+ const Eigen::MatrixXd& B) {
+ return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+}
+
+Eigen::Vector3d PoseToVector(const Pose2d& pose) {
+ return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
+ pose.Rotation().Radians().value()};
}
} // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
index a4fda0a..4a093d6 100644
--- a/wpimath/src/main/native/cpp/controller/BangBangController.cpp
+++ b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
@@ -11,10 +11,7 @@
using namespace frc;
BangBangController::BangBangController(double tolerance)
- : m_tolerance(tolerance) {
- static int instances = 0;
- instances++;
-}
+ : m_tolerance(tolerance) {}
void BangBangController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
index 9788023..05be645 100644
--- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
@@ -6,7 +6,7 @@
#include <utility>
-#include "Eigen/QR"
+#include <Eigen/QR>
using namespace frc;
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
index e1b2732..257d7e1 100644
--- a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
@@ -4,7 +4,8 @@
#include "frc/controller/DifferentialDriveFeedforward.h"
-#include "frc/EigenCore.h"
+#include <Eigen/Core>
+
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
@@ -30,8 +31,8 @@
units::meters_per_second_t nextRightVelocity, units::second_t dt) {
frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
- frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
- frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
+ Eigen::Vector2d r{currentLeftVelocity, currentRightVelocity};
+ Eigen::Vector2d nextR{nextLeftVelocity, nextRightVelocity};
auto u = feedforward.Calculate(r, nextR);
return {units::volt_t{u(0)}, units::volt_t{u(1)}};
}
diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
index def7234..64210a7 100644
--- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -11,7 +11,7 @@
using namespace frc;
HolonomicDriveController::HolonomicDriveController(
- frc2::PIDController xController, frc2::PIDController yController,
+ PIDController xController, PIDController yController,
ProfiledPIDController<units::radian> thetaController)
: m_xController(std::move(xController)),
m_yController(std::move(yController)),
@@ -79,3 +79,16 @@
void HolonomicDriveController::SetEnabled(bool enabled) {
m_enabled = enabled;
}
+
+ProfiledPIDController<units::radian>&
+HolonomicDriveController::getThetaController() {
+ return m_thetaController;
+}
+
+PIDController& HolonomicDriveController::getXController() {
+ return m_xController;
+}
+
+PIDController& HolonomicDriveController::getYController() {
+ return m_yController;
+}
diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
index 17a0c56..173f2ea 100644
--- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
@@ -5,10 +5,14 @@
#include "frc/controller/LTVDifferentialDriveController.h"
#include <cmath>
+#include <stdexcept>
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
#include "frc/MathUtil.h"
#include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
using namespace frc;
@@ -63,18 +67,39 @@
// Ax = -Bu
// x = -A⁻¹Bu
units::meters_per_second_t maxV{
+ // NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
-plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
+ if (maxV <= 0_mps) {
+ throw std::domain_error(
+ "Max velocity of plant with 12 V input must be greater than 0 m/s.");
+ }
+ if (maxV >= 15_mps) {
+ throw std::domain_error(
+ "Max velocity of plant with 12 V input must be less than 15 m/s.");
+ }
+
+ auto R_llt = R.llt();
+
for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (units::math::abs(velocity) < 1e-4_mps) {
- m_table.insert(velocity, Matrixd<2, 5>::Zero());
+ A(State::kY, State::kHeading) = 1e-4;
} else {
A(State::kY, State::kHeading) = velocity.value();
- m_table.insert(velocity,
- frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
}
+
+ Matrixd<5, 5> discA;
+ Matrixd<5, 2> discB;
+ DiscretizeAB(A, B, dt, &discA, &discB);
+
+ Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.insert(velocity, (discB.transpose() * S * discB + R)
+ .llt()
+ .solve(discB.transpose() * S * discA));
}
}
diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
index 256b757..aca317a 100644
--- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
@@ -4,8 +4,13 @@
#include "frc/controller/LTVUnicycleController.h"
+#include <stdexcept>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
#include "units/math.h"
using namespace frc;
@@ -37,6 +42,13 @@
LTVUnicycleController::LTVUnicycleController(
const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
units::second_t dt, units::meters_per_second_t maxVelocity) {
+ if (maxVelocity <= 0_mps) {
+ throw std::domain_error("Max velocity must be greater than 0 m/s.");
+ }
+ if (maxVelocity >= 15_mps) {
+ throw std::domain_error("Max velocity must be less than 15 m/s.");
+ }
+
// The change in global pose for a unicycle is defined by the following three
// equations.
//
@@ -74,17 +86,28 @@
Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
+ auto R_llt = R.llt();
+
for (auto velocity = -maxVelocity; velocity < maxVelocity;
velocity += 0.01_mps) {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (units::math::abs(velocity) < 1e-4_mps) {
- m_table.insert(velocity, Matrixd<2, 3>::Zero());
+ A(State::kY, State::kHeading) = 1e-4;
} else {
A(State::kY, State::kHeading) = velocity.value();
- m_table.insert(velocity,
- frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
}
+
+ Matrixd<3, 3> discA;
+ Matrixd<3, 2> discB;
+ DiscretizeAB(A, B, dt, &discA, &discB);
+
+ Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.insert(velocity, (discB.transpose() * S * discB + R)
+ .llt()
+ .solve(discB.transpose() * S * discA));
}
}
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 6c89d25..2638f9e 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -13,7 +13,7 @@
#include "frc/MathUtil.h"
#include "wpimath/MathShared.h"
-using namespace frc2;
+using namespace frc;
PIDController::PIDController(double Kp, double Ki, double Kd,
units::second_t period)
@@ -52,6 +52,14 @@
m_Kd = Kd;
}
+void PIDController::SetIZone(double iZone) {
+ if (iZone < 0) {
+ wpi::math::MathSharedStore::ReportError(
+ "IZone must be a non-negative number, got {}!", iZone);
+ }
+ m_iZone = iZone;
+}
+
double PIDController::GetP() const {
return m_Kp;
}
@@ -64,6 +72,10 @@
return m_Kd;
}
+double PIDController::GetIZone() const {
+ return m_iZone;
+}
+
units::second_t PIDController::GetPeriod() const {
return m_period;
}
@@ -78,11 +90,12 @@
void PIDController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
+ m_haveSetpoint = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError =
- frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+ InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - m_measurement;
}
@@ -95,7 +108,8 @@
}
bool PIDController::AtSetpoint() const {
- return std::abs(m_positionError) < m_positionTolerance &&
+ return m_haveMeasurement && m_haveSetpoint &&
+ std::abs(m_positionError) < m_positionTolerance &&
std::abs(m_velocityError) < m_velocityTolerance;
}
@@ -137,18 +151,23 @@
double PIDController::Calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
+ m_haveMeasurement = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError =
- frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+ InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - m_measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period.value();
- if (m_Ki != 0) {
+ // If the absolute value of the position error is outside of IZone, reset the
+ // total error
+ if (std::abs(m_positionError) > m_iZone) {
+ m_totalError = 0;
+ } else if (m_Ki != 0) {
m_totalError =
std::clamp(m_totalError + m_positionError * m_period.value(),
m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
@@ -159,6 +178,7 @@
double PIDController::Calculate(double measurement, double setpoint) {
m_setpoint = setpoint;
+ m_haveSetpoint = true;
return Calculate(measurement);
}
@@ -167,6 +187,7 @@
m_prevError = 0;
m_totalError = 0;
m_velocityError = 0;
+ m_haveMeasurement = false;
}
void PIDController::InitSendable(wpi::SendableBuilder& builder) {
@@ -178,6 +199,9 @@
builder.AddDoubleProperty(
"d", [this] { return GetD(); }, [this](double value) { SetD(value); });
builder.AddDoubleProperty(
+ "izone", [this] { return GetIZone(); },
+ [this](double value) { SetIZone(value); });
+ builder.AddDoubleProperty(
"setpoint", [this] { return GetSetpoint(); },
[this](double value) { SetSetpoint(value); });
}
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 39e0d8c..213ba99 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -4,40 +4,8 @@
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
-#include <wpi/timestamp.h>
-
-#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/AngleStatistics.h"
-
using namespace frc;
-DifferentialDrivePoseEstimator::InterpolationRecord
-DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
- DifferentialDriveKinematics& kinematics, InterpolationRecord endValue,
- double i) const {
- if (i < 0) {
- return *this;
- } else if (i > 1) {
- return endValue;
- } else {
- // Find the interpolated left distance.
- auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i);
- // Find the interpolated right distance.
- auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i);
-
- // Find the new gyro angle.
- auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
- // Create a twist to represent this changed based on the interpolated
- // sensor inputs.
- auto twist =
- kinematics.ToTwist2d(left - leftDistance, right - rightDistance);
- twist.dtheta = (gyro - gyroAngle).Radians();
-
- return {pose.Exp(twist), gyro, left, right};
- }
-}
-
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
@@ -51,114 +19,7 @@
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
- : m_kinematics{kinematics},
- m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} {
- for (size_t i = 0; i < 3; ++i) {
- m_q[i] = stateStdDevs[i] * stateStdDevs[i];
- }
-
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-}
-
-void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
- const wpi::array<double, 3>& visionMeasurementStdDevs) {
- wpi::array<double, 3> r{wpi::empty_array};
- for (size_t i = 0; i < 3; ++i) {
- r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
- }
-
- // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
- // and C = I. See wpimath/algorithms.md.
- for (size_t row = 0; row < 3; ++row) {
- if (m_q[row] == 0.0) {
- m_visionK(row, row) = 0.0;
- } else {
- m_visionK(row, row) =
- m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
- }
- }
-}
-
-void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
- units::meter_t leftDistance,
- units::meter_t rightDistance,
- const Pose2d& pose) {
- // Reset state estimate and error covariance
- m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
- m_poseBuffer.Clear();
-}
-
-Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
- return m_odometry.GetPose();
-}
-
-void DifferentialDrivePoseEstimator::AddVisionMeasurement(
- const Pose2d& visionRobotPose, units::second_t timestamp) {
- // Step 1: Get the estimated pose from when the vision measurement was made.
- auto sample = m_poseBuffer.Sample(timestamp);
-
- if (!sample.has_value()) {
- return;
- }
-
- // Step 2: Measure the twist between the odometry pose and the vision pose.
- auto twist = sample.value().pose.Log(visionRobotPose);
-
- // Step 3: We should not trust the twist entirely, so instead we scale this
- // twist by a Kalman gain matrix representing how much we trust vision
- // measurements compared to our current pose.
- frc::Vectord<3> k_times_twist =
- m_visionK *
- frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
-
- // Step 4: Convert back to Twist2d.
- Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
- units::meter_t{k_times_twist(1)},
- units::radian_t{k_times_twist(2)}};
-
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.ResetPosition(
- sample.value().gyroAngle, sample.value().leftDistance,
- sample.value().rightDistance, sample.value().pose.Exp(scaledTwist));
-
- // Step 6: Replay odometry inputs between sample time and latest recorded
- // sample to update the pose buffer and correct odometry.
- auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
- auto first_newer_record =
- std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
- [](const auto& pair, auto t) { return t > pair.first; });
-
- for (auto entry = first_newer_record + 1; entry != internal_buf.end();
- entry++) {
- UpdateWithTime(entry->first, entry->second.gyroAngle,
- entry->second.leftDistance, entry->second.rightDistance);
- }
-}
-
-Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
- units::meter_t leftDistance,
- units::meter_t rightDistance) {
- return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- leftDistance, rightDistance);
-}
-
-Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& gyroAngle,
- units::meter_t leftDistance, units::meter_t rightDistance) {
- m_odometry.Update(gyroAngle, leftDistance, rightDistance);
-
- // fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
- // gyroAngle.Radians(),
- // leftDistance,
- // rightDistance,
- // GetEstimatedPosition().X(),
- // GetEstimatedPosition().Y(),
- // GetEstimatedPosition().Rotation().Radians()
- // );
-
- m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
- leftDistance, rightDistance});
-
- return GetEstimatedPosition();
-}
+ : PoseEstimator<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions>(
+ kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+ m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 9642e08..7c75e71 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -8,49 +8,10 @@
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
using namespace frc;
-frc::MecanumDrivePoseEstimator::InterpolationRecord
-frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
- MecanumDriveKinematics& kinematics, InterpolationRecord endValue,
- double i) const {
- if (i < 0) {
- return *this;
- } else if (i > 1) {
- return endValue;
- } else {
- // Find the new wheel distance measurements.
- MecanumDriveWheelPositions wheels_lerp{
- wpi::Lerp(this->wheelPositions.frontLeft,
- endValue.wheelPositions.frontLeft, i),
- wpi::Lerp(this->wheelPositions.frontRight,
- endValue.wheelPositions.frontRight, i),
- wpi::Lerp(this->wheelPositions.rearLeft,
- endValue.wheelPositions.rearLeft, i),
- wpi::Lerp(this->wheelPositions.rearRight,
- endValue.wheelPositions.rearRight, i)};
-
- // Find the distance between this measurement and the
- // interpolated measurement.
- MecanumDriveWheelPositions wheels_delta{
- wheels_lerp.frontLeft - this->wheelPositions.frontLeft,
- wheels_lerp.frontRight - this->wheelPositions.frontRight,
- wheels_lerp.rearLeft - this->wheelPositions.rearLeft,
- wheels_lerp.rearRight - this->wheelPositions.rearRight};
-
- // Find the new gyro angle.
- auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
- // Create a twist to represent this changed based on the interpolated
- // sensor inputs.
- auto twist = kinematics.ToTwist2d(wheels_delta);
- twist.dtheta = (gyro - gyroAngle).Radians();
-
- return {pose.Exp(twist), gyro, wheels_lerp};
- }
-}
-
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
@@ -63,107 +24,6 @@
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
- : m_kinematics{kinematics},
- m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} {
- for (size_t i = 0; i < 3; ++i) {
- m_q[i] = stateStdDevs[i] * stateStdDevs[i];
- }
-
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-}
-
-void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
- const wpi::array<double, 3>& visionMeasurementStdDevs) {
- wpi::array<double, 3> r{wpi::empty_array};
- for (size_t i = 0; i < 3; ++i) {
- r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
- }
-
- // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
- // and C = I. See wpimath/algorithms.md.
- for (size_t row = 0; row < 3; ++row) {
- if (m_q[row] == 0.0) {
- m_visionK(row, row) = 0.0;
- } else {
- m_visionK(row, row) =
- m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
- }
- }
-}
-
-void frc::MecanumDrivePoseEstimator::ResetPosition(
- const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
- // Reset state estimate and error covariance
- m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
- m_poseBuffer.Clear();
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
- return m_odometry.GetPose();
-}
-
-void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
- const Pose2d& visionRobotPose, units::second_t timestamp) {
- // Step 1: Get the estimated pose from when the vision measurement was made.
- auto sample = m_poseBuffer.Sample(timestamp);
-
- if (!sample.has_value()) {
- return;
- }
-
- // Step 2: Measure the twist between the odometry pose and the vision pose
- auto twist = sample.value().pose.Log(visionRobotPose);
-
- // Step 3: We should not trust the twist entirely, so instead we scale this
- // twist by a Kalman gain matrix representing how much we trust vision
- // measurements compared to our current pose.
- frc::Vectord<3> k_times_twist =
- m_visionK *
- frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
-
- // Step 4: Convert back to Twist2d
- Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
- units::meter_t{k_times_twist(1)},
- units::radian_t{k_times_twist(2)}};
-
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.ResetPosition(sample.value().gyroAngle,
- sample.value().wheelPositions,
- sample.value().pose.Exp(scaledTwist));
-
- // Step 6: Replay odometry inputs between sample time and latest recorded
- // sample to update the pose buffer and correct odometry.
- auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
- auto upper_bound =
- std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
- [](const auto& pair, auto t) { return t > pair.first; });
-
- for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
- UpdateWithTime(entry->first, entry->second.gyroAngle,
- entry->second.wheelPositions);
- }
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::Update(
- const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions) {
- return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- wheelPositions);
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions) {
- m_odometry.Update(gyroAngle, wheelPositions);
-
- MecanumDriveWheelPositions internalWheelPositions{
- wheelPositions.frontLeft, wheelPositions.frontRight,
- wheelPositions.rearLeft, wheelPositions.rearRight};
-
- m_poseBuffer.AddSample(
- currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
-
- return GetEstimatedPosition();
-}
+ : PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
+ kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+ m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
diff --git a/wpimath/src/main/native/cpp/filter/Debouncer.cpp b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
index 4e909a2..841768e 100644
--- a/wpimath/src/main/native/cpp/filter/Debouncer.cpp
+++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
@@ -4,6 +4,8 @@
#include "frc/filter/Debouncer.h"
+#include "wpimath/MathShared.h"
+
using namespace frc;
Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
@@ -21,11 +23,12 @@
}
void Debouncer::ResetTimer() {
- m_prevTime = units::microsecond_t(wpi::Now());
+ m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
}
bool Debouncer::HasElapsed() const {
- return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime;
+ return wpi::math::MathSharedStore::GetTimestamp() - m_prevTime >=
+ m_debounceTime;
}
bool Debouncer::Calculate(bool input) {
diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
index d1ee93d..2a8c9db 100644
--- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
@@ -8,7 +8,8 @@
#include <stdexcept>
#include <utility>
-#include "Eigen/QR"
+#include <Eigen/Core>
+#include <Eigen/QR>
using namespace frc;
@@ -18,7 +19,7 @@
// Construct a change of basis matrix from the source coordinate system to the
// NWU coordinate system. Each column vector in the change of basis matrix is
// one of the old basis vectors mapped to its representation in the new basis.
- Matrixd<3, 3> R;
+ Eigen::Matrix3d R;
R.block<3, 1>(0, 0) = positiveX.m_axis;
R.block<3, 1>(0, 1) = positiveY.m_axis;
R.block<3, 1>(0, 2) = positiveZ.m_axis;
@@ -68,6 +69,8 @@
Transform3d CoordinateSystem::Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to) {
- return Transform3d{Convert(transform.Translation(), from, to),
- Convert(transform.Rotation(), from, to)};
+ const auto coordRot = from.m_rotation - to.m_rotation;
+ return Transform3d{
+ Convert(transform.Translation(), from, to),
+ (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 2648a90..2e15216 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -8,6 +8,9 @@
#include <wpi/json.h>
+#include "frc/MathUtil.h"
+#include "geometry2d.pb.h"
+
using namespace frc;
Transform2d Pose2d::operator-(const Pose2d& other) const {
@@ -67,6 +70,36 @@
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
}
+Pose2d Pose2d::Nearest(std::span<const Pose2d> poses) const {
+ return *std::min_element(
+ poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+ auto aDistance = this->Translation().Distance(a.Translation());
+ auto bDistance = this->Translation().Distance(b.Translation());
+
+ // If the distances are equal sort by difference in rotation
+ if (aDistance == bDistance) {
+ return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
+ std::abs((this->Rotation() - b.Rotation()).Radians().value());
+ }
+ return aDistance < bDistance;
+ });
+}
+
+Pose2d Pose2d::Nearest(std::initializer_list<Pose2d> poses) const {
+ return *std::min_element(
+ poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+ auto aDistance = this->Translation().Distance(a.Translation());
+ auto bDistance = this->Translation().Distance(b.Translation());
+
+ // If the distances are equal sort by difference in rotation
+ if (aDistance == bDistance) {
+ return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
+ std::abs((this->Rotation() - b.Rotation()).Radians().value());
+ }
+ return aDistance < bDistance;
+ });
+}
+
void frc::to_json(wpi::json& json, const Pose2d& pose) {
json = wpi::json{{"translation", pose.Translation()},
{"rotation", pose.Rotation()}};
@@ -76,3 +109,23 @@
pose = Pose2d{json.at("translation").get<Translation2d>(),
json.at("rotation").get<Rotation2d>()};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+ arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+ return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+ const frc::Pose2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
index 4732c4d..ffbaecb 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -6,8 +6,11 @@
#include <cmath>
+#include <Eigen/Core>
#include <wpi/json.h>
+#include "geometry3d.pb.h"
+
using namespace frc;
namespace {
@@ -21,14 +24,14 @@
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
-Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
- return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
- {rotation(2), 0.0, -rotation(0)},
- {-rotation(1), rotation(0), 0.0}};
+ return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
+ {rotation(2), 0.0, -rotation(0)},
+ {-rotation(1), rotation(0), 0.0}};
}
} // namespace
@@ -60,6 +63,10 @@
return *this * (1.0 / scalar);
}
+Pose3d Pose3d::RotateBy(const Rotation3d& other) const {
+ return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
Pose3d Pose3d::TransformBy(const Transform3d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
@@ -71,73 +78,102 @@
}
Pose3d Pose3d::Exp(const Twist3d& twist) const {
- Matrixd<3, 3> Omega = RotationVectorToMatrix(
- Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
- Matrixd<3, 3> OmegaSq = Omega * Omega;
+ // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
+ Eigen::Vector3d u{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+ Eigen::Vector3d rvec{twist.rx.value(), twist.ry.value(), twist.rz.value()};
+ Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+ Eigen::Matrix3d omegaSq = omega * omega;
+ double theta = rvec.norm();
+ double thetaSq = theta * theta;
- double thetaSq =
- (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
-
- // Get left Jacobian of SO3. See first line in right column of
- // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
- Matrixd<3, 3> J;
- if (thetaSq < 1E-9 * 1E-9) {
- // V = I + 0.5ω
- J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+ double A;
+ double B;
+ double C;
+ if (std::abs(theta) < 1E-7) {
+ // Taylor Expansions around θ = 0
+ // A = 1/1! - θ²/3! + θ⁴/5!
+ // B = 1/2! - θ²/4! + θ⁴/6!
+ // C = 1/3! - θ²/5! + θ⁴/7!
+ // sources:
+ // A:
+ // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+ // B:
+ // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+ // C:
+ // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+ A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
+ B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
+ C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
} else {
- double theta = std::sqrt(thetaSq);
- // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
- J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
- (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+ // A = std::sin(θ)/θ
+ // B = (1 - std::cos(θ)) / θ²
+ // C = (1 - A) / θ²
+ A = std::sin(theta) / theta;
+ B = (1 - std::cos(theta)) / thetaSq;
+ C = (1 - A) / thetaSq;
}
- // Get translation component
- Vectord<3> translation =
- J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+ Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + A * omega + B * omegaSq;
+ Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + B * omega + C * omegaSq;
- const Transform3d transform{Translation3d{units::meter_t{translation(0)},
- units::meter_t{translation(1)},
- units::meter_t{translation(2)}},
- Rotation3d{twist.rx, twist.ry, twist.rz}};
+ auto translation_component = V * u;
+ const Transform3d transform{
+ Translation3d{units::meter_t{translation_component(0)},
+ units::meter_t{translation_component(1)},
+ units::meter_t{translation_component(2)}},
+ Rotation3d{R}};
return *this + transform;
}
Twist3d Pose3d::Log(const Pose3d& end) const {
+ // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
const auto transform = end.RelativeTo(*this);
- Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+ Eigen::Vector3d u{transform.X().value(), transform.Y().value(),
+ transform.Z().value()};
+ Eigen::Vector3d rvec =
+ transform.Rotation().GetQuaternion().ToRotationVector();
- Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
- Matrixd<3, 3> OmegaSq = Omega * Omega;
+ Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+ Eigen::Matrix3d omegaSq = omega * omega;
+ double theta = rvec.norm();
+ double thetaSq = theta * theta;
- double thetaSq = rotVec.squaredNorm();
-
- // Get left Jacobian inverse of SO3. See fourth line in right column of
- // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
- Matrixd<3, 3> Jinv;
- if (thetaSq < 1E-9 * 1E-9) {
- // J⁻¹ = I − 0.5ω + 1/12 ω²
- Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+ double C;
+ if (std::abs(theta) < 1E-7) {
+ // Taylor Expansions around θ = 0
+ // A = 1/1! - θ²/3! + θ⁴/5!
+ // B = 1/2! - θ²/4! + θ⁴/6!
+ // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
+ // sources:
+ // A:
+ // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+ // B:
+ // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+ // C:
+ // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+ C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
} else {
- double theta = std::sqrt(thetaSq);
- double halfTheta = 0.5 * theta;
-
- // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
- Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
- (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
- thetaSq * OmegaSq;
+ // A = std::sin(θ)/θ
+ // B = (1 - std::cos(θ)) / θ²
+ // C = (1 - A/(2*B)) / θ²
+ double A = std::sin(theta) / theta;
+ double B = (1 - std::cos(theta)) / thetaSq;
+ C = (1 - A / (2 * B)) / thetaSq;
}
- // Get dtranslation component
- Vectord<3> dtranslation =
- Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
- transform.Z().value()};
+ Eigen::Matrix3d V_inv =
+ Eigen::Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
- return Twist3d{
- units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
- units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
- units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}};
+ Eigen::Vector3d translation_component = V_inv * u;
+
+ return Twist3d{units::meter_t{translation_component(0)},
+ units::meter_t{translation_component(1)},
+ units::meter_t{translation_component(2)},
+ units::radian_t{rvec(0)},
+ units::radian_t{rvec(1)},
+ units::radian_t{rvec(2)}};
}
Pose2d Pose3d::ToPose2d() const {
@@ -153,3 +189,23 @@
pose = Pose3d{json.at("translation").get<Translation3d>(),
json.at("rotation").get<Rotation3d>()};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+ arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+ return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+ const frc::Pose3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
index 9c2ceda..37afbb8 100644
--- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
@@ -4,13 +4,53 @@
#include "frc/geometry/Quaternion.h"
+#include <numbers>
+
#include <wpi/json.h>
+#include "geometry3d.pb.h"
+
using namespace frc;
Quaternion::Quaternion(double w, double x, double y, double z)
: m_r{w}, m_v{x, y, z} {}
+Quaternion Quaternion::operator+(const Quaternion& other) const {
+ return Quaternion{
+ m_r + other.m_r,
+ m_v(0) + other.m_v(0),
+ m_v(1) + other.m_v(1),
+ m_v(2) + other.m_v(2),
+ };
+}
+
+Quaternion Quaternion::operator-(const Quaternion& other) const {
+ return Quaternion{
+ m_r - other.m_r,
+ m_v(0) - other.m_v(0),
+ m_v(1) - other.m_v(1),
+ m_v(2) - other.m_v(2),
+ };
+}
+
+Quaternion Quaternion::operator*(const double other) const {
+ return Quaternion{
+ m_r * other,
+ m_v(0) * other,
+ m_v(1) * other,
+ m_v(2) * other,
+ };
+}
+
+Quaternion Quaternion::operator/(const double other) const {
+ return Quaternion{
+ m_r / other,
+ m_v(0) / other,
+ m_v(1) / other,
+ m_v(2) / other,
+ };
+}
+
Quaternion Quaternion::operator*(const Quaternion& other) const {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
const auto& r1 = m_r;
@@ -26,26 +66,102 @@
// v = r₁v₂ + r₂v₁ + v₁ x v₂
Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
- return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+ return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
+ r1 * r2 - v1.dot(v2),
+ // v = r₁v₂ + r₂v₁ + v₁ x v₂
+ v(0), v(1), v(2)};
}
bool Quaternion::operator==(const Quaternion& other) const {
- return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+ return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
+ std::abs(Norm() - other.Norm()) < 1e-9;
+}
+
+Quaternion Quaternion::Conjugate() const {
+ return Quaternion{W(), -X(), -Y(), -Z()};
+}
+
+double Quaternion::Dot(const Quaternion& other) const {
+ return W() * other.W() + m_v.dot(other.m_v);
}
Quaternion Quaternion::Inverse() const {
- return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+ double norm = Norm();
+ return Conjugate() / (norm * norm);
+}
+
+double Quaternion::Norm() const {
+ return std::sqrt(Dot(*this));
}
Quaternion Quaternion::Normalize() const {
- double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+ double norm = Norm();
if (norm == 0.0) {
return Quaternion{};
} else {
- return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+ return Quaternion{W(), X(), Y(), Z()} / norm;
}
}
+Quaternion Quaternion::Pow(const double other) const {
+ return (Log() * other).Exp();
+}
+
+Quaternion Quaternion::Exp(const Quaternion& other) const {
+ return other.Exp() * *this;
+}
+
+Quaternion Quaternion::Exp() const {
+ double scalar = std::exp(m_r);
+
+ double axial_magnitude = m_v.norm();
+ double cosine = std::cos(axial_magnitude);
+
+ double axial_scalar;
+
+ if (axial_magnitude < 1e-9) {
+ // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
+ double axial_magnitude_sq = axial_magnitude * axial_magnitude;
+ double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+ axial_scalar =
+ 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+ } else {
+ axial_scalar = std::sin(axial_magnitude) / axial_magnitude;
+ }
+
+ return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
+ Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
+}
+
+Quaternion Quaternion::Log(const Quaternion& other) const {
+ return (other * Inverse()).Log();
+}
+
+Quaternion Quaternion::Log() const {
+ double scalar = std::log(Norm());
+
+ double v_norm = m_v.norm();
+
+ double s_norm = W() / Norm();
+
+ if (std::abs(s_norm + 1) < 1e-9) {
+ return Quaternion{scalar, -std::numbers::pi, 0, 0};
+ }
+
+ double v_scalar;
+
+ if (v_norm < 1e-9) {
+ // Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x -
+ // y^2/3*x^3 + O(y^4)
+ v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
+ } else {
+ v_scalar = std::atan2(v_norm, W()) / v_norm;
+ }
+
+ return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1),
+ v_scalar * m_v(2)};
+}
+
double Quaternion::W() const {
return m_r;
}
@@ -80,6 +196,30 @@
}
}
+Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) {
+ // 𝑣⃗ = θ * v̂
+ // v̂ = 𝑣⃗ / θ
+
+ // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
+ // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
+
+ double theta = rvec.norm();
+ double cos = std::cos(theta / 2);
+
+ double axial_scalar;
+
+ if (theta < 1e-9) {
+ // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
+ // O(θ⁴)
+ axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+ } else {
+ axial_scalar = std::sin(theta / 2) / theta;
+ }
+
+ return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
+ axial_scalar * rvec(2)};
+}
+
void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
json = wpi::json{{"W", quaternion.W()},
{"X", quaternion.X()},
@@ -92,3 +232,24 @@
Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
json.at("Y").get<double>(), json.at("Z").get<double>()};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+ arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+ return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+ const frc::Quaternion& value) {
+ auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+ m->set_w(value.W());
+ m->set_x(value.X());
+ m->set_y(value.Y());
+ m->set_z(value.Z());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 921e1f8..05a644e 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -8,6 +8,7 @@
#include <wpi/json.h>
+#include "geometry2d.pb.h"
#include "units/math.h"
using namespace frc;
@@ -19,3 +20,21 @@
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+ arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+ return frc::Rotation2d{units::radian_t{m->value()}};
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+ const frc::Rotation2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+ m->set_value(value.Radians().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
index 298f0e6..3a68870 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
@@ -7,12 +7,13 @@
#include <cmath>
#include <numbers>
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/QR>
#include <wpi/json.h>
-#include "Eigen/Core"
-#include "Eigen/LU"
-#include "Eigen/QR"
#include "frc/fmt/Eigen.h"
+#include "geometry3d.pb.h"
#include "units/math.h"
#include "wpimath/MathShared.h"
@@ -38,23 +39,26 @@
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
}
-Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
+ : Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
+
+Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
double norm = axis.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
- Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+ Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0);
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
-Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
+Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
const auto& R = rotationMatrix;
// Require that the rotation matrix is special orthogonal. This is true if the
// matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
- if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
+ if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) {
std::string msg =
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
@@ -109,7 +113,8 @@
m_q = Quaternion{w, x, y, z};
}
-Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
+Rotation3d::Rotation3d(const Eigen::Vector3d& initial,
+ const Eigen::Vector3d& final) {
double dot = initial.dot(final);
double normProduct = initial.norm() * final.norm();
double dotNorm = dot / normProduct;
@@ -170,6 +175,11 @@
return *this * (1.0 / scalar);
}
+bool Rotation3d::operator==(const Rotation3d& other) const {
+ return std::abs(std::abs(m_q.Dot(other.m_q)) -
+ m_q.Norm() * other.m_q.Norm()) < 1e-9;
+}
+
Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
return Rotation3d{other.m_q * m_q};
}
@@ -184,9 +194,15 @@
double y = m_q.Y();
double z = m_q.Z();
- // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
- return units::radian_t{
- std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+ // wpimath/algorithms.md
+ double cxcy = 1.0 - 2.0 * (x * x + y * y);
+ double sxcy = 2.0 * (w * x + y * z);
+ double cy_sq = cxcy * cxcy + sxcy * sxcy;
+ if (cy_sq > 1e-20) {
+ return units::radian_t{std::atan2(sxcy, cxcy)};
+ } else {
+ return 0_rad;
+ }
}
units::radian_t Rotation3d::Y() const {
@@ -195,7 +211,7 @@
double y = m_q.Y();
double z = m_q.Z();
- // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
double ratio = 2.0 * (w * y - z * x);
if (std::abs(ratio) >= 1.0) {
return units::radian_t{std::copysign(std::numbers::pi / 2.0, ratio)};
@@ -210,12 +226,18 @@
double y = m_q.Y();
double z = m_q.Z();
- // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
- return units::radian_t{
- std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+ // wpimath/algorithms.md
+ double cycz = 1.0 - 2.0 * (y * y + z * z);
+ double cysz = 2.0 * (w * z + x * y);
+ double cy_sq = cycz * cycz + cysz * cysz;
+ if (cy_sq > 1e-20) {
+ return units::radian_t{std::atan2(cysz, cycz)};
+ } else {
+ return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)};
+ }
}
-Vectord<3> Rotation3d::Axis() const {
+Eigen::Vector3d Rotation3d::Axis() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
if (norm == 0.0) {
return {0.0, 0.0, 0.0};
@@ -240,3 +262,21 @@
void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+ arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+ return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+ const frc::Rotation3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+ wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 25b0590..77f3cee 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -5,6 +5,7 @@
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
using namespace frc;
@@ -21,3 +22,23 @@
Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+ return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+ const frc::Transform2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
index 1cfabaa..de6c253 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
@@ -5,6 +5,7 @@
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
using namespace frc;
@@ -21,6 +22,10 @@
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+ Rotation3d rotation)
+ : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
Transform3d Transform3d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
@@ -31,3 +36,23 @@
Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+ return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+ const frc::Transform3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index d463696..6d5f315 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -6,6 +6,7 @@
#include <wpi/json.h>
+#include "geometry2d.pb.h"
#include "units/math.h"
using namespace frc;
@@ -23,6 +24,22 @@
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
+Translation2d Translation2d::Nearest(
+ std::span<const Translation2d> translations) const {
+ return *std::min_element(translations.begin(), translations.end(),
+ [this](Translation2d a, Translation2d b) {
+ return this->Distance(a) < this->Distance(b);
+ });
+}
+
+Translation2d Translation2d::Nearest(
+ std::initializer_list<Translation2d> translations) const {
+ return *std::min_element(translations.begin(), translations.end(),
+ [this](Translation2d a, Translation2d b) {
+ return this->Distance(a) < this->Distance(b);
+ });
+}
+
void frc::to_json(wpi::json& json, const Translation2d& translation) {
json =
wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
@@ -32,3 +49,22 @@
translation = Translation2d{units::meter_t{json.at("x").get<double>()},
units::meter_t{json.at("y").get<double>()}};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+ return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+ const frc::Translation2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+ m->set_x(value.X().value());
+ m->set_y(value.Y().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index 2c53791..90e94ae 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -6,6 +6,7 @@
#include <wpi/json.h>
+#include "geometry3d.pb.h"
#include "units/length.h"
#include "units/math.h"
@@ -52,3 +53,24 @@
units::meter_t{json.at("y").get<double>()},
units::meter_t{json.at("z").get<double>()}};
}
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+ return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
+ units::meter_t{m->z()}};
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+ const frc::Translation3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+ m->set_x(value.X().value());
+ m->set_y(value.Y().value());
+ m->set_z(value.Z().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
new file mode 100644
index 0000000..6c106eb
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Twist2d.h"
+
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
+ arena);
+}
+
+frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
+ return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
+ units::radian_t{m->dtheta()}};
+}
+
+void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
+ const frc::Twist2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTwist2d*>(msg);
+ m->set_dx(value.dx.value());
+ m->set_dy(value.dy.value());
+ m->set_dtheta(value.dtheta.value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
new file mode 100644
index 0000000..4f4ce86
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Twist3d.h"
+
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
+ arena);
+}
+
+frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
+ return frc::Twist3d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
+ units::meter_t{m->dz()}, units::radian_t{m->rx()},
+ units::radian_t{m->ry()}, units::radian_t{m->rz()}};
+}
+
+void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
+ const frc::Twist3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTwist3d*>(msg);
+ m->set_dx(value.dx.value());
+ m->set_dy(value.dy.value());
+ m->set_dz(value.dz.value());
+ m->set_rx(value.rx.value());
+ m->set_ry(value.ry.value());
+ m->set_rz(value.rz.value());
+}
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
deleted file mode 100644
index fd8a223..0000000
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ /dev/null
@@ -1,367 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include <jni.h>
-
-#include <exception>
-
-#include <wpi/jni_util.h>
-
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Eigenvalues"
-#include "Eigen/QR"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "edu_wpi_first_math_WPIMathJNI.h"
-#include "frc/trajectory/TrajectoryUtil.h"
-#include "unsupported/Eigen/MatrixFunctions"
-
-using namespace wpi::java;
-
-/**
- * Returns true if (A, B) is a stabilizable pair.
- *
- * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
- * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(λI - A, B) < n where n is the number of states.
- *
- * @param A System matrix.
- * @param B Input matrix.
- */
-bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& B) {
- int states = B.rows();
- int inputs = B.cols();
- Eigen::EigenSolver<Eigen::MatrixXd> es{A};
- for (int i = 0; i < states; ++i) {
- if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
- es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
- 1) {
- continue;
- }
-
- Eigen::MatrixXcd E{states, states + inputs};
- E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
- B;
- Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
- if (qr.rank() < states) {
- return false;
- }
- }
-
- return true;
-}
-
-std::vector<double> GetElementsFromTrajectory(
- const frc::Trajectory& trajectory) {
- std::vector<double> elements;
- elements.reserve(trajectory.States().size() * 7);
-
- for (auto&& state : trajectory.States()) {
- elements.push_back(state.t.value());
- elements.push_back(state.velocity.value());
- elements.push_back(state.acceleration.value());
- elements.push_back(state.pose.X().value());
- elements.push_back(state.pose.Y().value());
- elements.push_back(state.pose.Rotation().Radians().value());
- elements.push_back(state.curvature.value());
- }
-
- return elements;
-}
-
-frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
- // Make sure that the elements have the correct length.
- assert(elements.size() % 7 == 0);
-
- // Create a vector of states from the elements.
- std::vector<frc::Trajectory::State> states;
- states.reserve(elements.size() / 7);
-
- for (size_t i = 0; i < elements.size(); i += 7) {
- states.emplace_back(frc::Trajectory::State{
- units::second_t{elements[i]},
- units::meters_per_second_t{elements[i + 1]},
- units::meters_per_second_squared_t{elements[i + 2]},
- frc::Pose2d{units::meter_t{elements[i + 3]},
- units::meter_t{elements[i + 4]},
- units::radian_t{elements[i + 5]}},
- units::curvature_t{elements[i + 6]}});
- }
-
- return frc::Trajectory(states);
-}
-
-extern "C" {
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: discreteAlgebraicRiccatiEquation
- * Signature: ([D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
- (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
- jdoubleArray R, jint states, jint inputs, jdoubleArray S)
-{
- jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
- jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
- jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
- jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Amat{nativeA, states, states};
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Bmat{nativeB, states, inputs};
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Qmat{nativeQ, states, states};
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Rmat{nativeR, inputs, inputs};
-
- try {
- Eigen::MatrixXd result =
- drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
-
- env->ReleaseDoubleArrayElements(A, nativeA, 0);
- env->ReleaseDoubleArrayElements(B, nativeB, 0);
- env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
- env->ReleaseDoubleArrayElements(R, nativeR, 0);
-
- env->SetDoubleArrayRegion(S, 0, states * states, result.data());
- } catch (const std::runtime_error& e) {
- jclass cls = env->FindClass("java/lang/RuntimeException");
- if (cls) {
- env->ThrowNew(cls, e.what());
- }
- }
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: exp
- * Signature: ([DI[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_exp
- (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
-{
- jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Amat{arrayBody, rows, rows};
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
- Amat.exp();
-
- env->ReleaseDoubleArrayElements(src, arrayBody, 0);
- env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: pow
- * Signature: ([DID[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_pow
- (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
- jdoubleArray dst)
-{
- jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Amat{arrayBody, rows, rows}; // NOLINT
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
- Amat.pow(exponent);
-
- env->ReleaseDoubleArrayElements(src, arrayBody, 0);
- env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: isStabilizable
- * Signature: (II[D[D)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
- (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
- jdoubleArray bSrc)
-{
- jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr);
- jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr);
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- A{nativeA, states, states};
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- B{nativeB, states, inputs};
-
- bool isStabilizable = check_stabilizable(A, B);
-
- env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
- env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
-
- return isStabilizable;
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: fromPathweaverJson
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
- (JNIEnv* env, jclass, jstring path)
-{
- try {
- auto trajectory =
- frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
- std::vector<double> elements = GetElementsFromTrajectory(trajectory);
- return MakeJDoubleArray(env, elements);
- } catch (std::exception& e) {
- jclass cls = env->FindClass("java/io/IOException");
- if (cls) {
- env->ThrowNew(cls, e.what());
- }
- return nullptr;
- }
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: toPathweaverJson
- * Signature: ([DLjava/lang/String;)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
- (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
-{
- try {
- auto trajectory =
- CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
- frc::TrajectoryUtil::ToPathweaverJson(trajectory,
- JStringRef{env, path}.c_str());
- } catch (std::exception& e) {
- jclass cls = env->FindClass("java/io/IOException");
- if (cls) {
- env->ThrowNew(cls, e.what());
- }
- }
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: deserializeTrajectory
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
- (JNIEnv* env, jclass, jstring json)
-{
- try {
- auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
- JStringRef{env, json}.c_str());
- std::vector<double> elements = GetElementsFromTrajectory(trajectory);
- return MakeJDoubleArray(env, elements);
- } catch (std::exception& e) {
- jclass cls = env->FindClass(
- "edu/wpi/first/math/trajectory/TrajectoryUtil$"
- "TrajectorySerializationException");
- if (cls) {
- env->ThrowNew(cls, e.what());
- }
- return nullptr;
- }
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: serializeTrajectory
- * Signature: ([D)Ljava/lang/String;
- */
-JNIEXPORT jstring JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
- (JNIEnv* env, jclass, jdoubleArray elements)
-{
- try {
- auto trajectory =
- CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
- return MakeJString(env,
- frc::TrajectoryUtil::SerializeTrajectory(trajectory));
- } catch (std::exception& e) {
- jclass cls = env->FindClass(
- "edu/wpi/first/math/trajectory/TrajectoryUtil$"
- "TrajectorySerializationException");
- if (cls) {
- env->ThrowNew(cls, e.what());
- }
- return nullptr;
- }
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: rankUpdate
- * Signature: ([DI[DDZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
- (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
- jdouble sigma, jboolean lowerTriangular)
-{
- jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
- jdouble* vecBody = env->GetDoubleArrayElements(vec, nullptr);
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- L{matBody, rows, rows};
- Eigen::Map<Eigen::Vector<double, Eigen::Dynamic>> v{vecBody, rows};
-
- if (lowerTriangular == JNI_TRUE) {
- Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
- } else {
- Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
- }
-
- env->ReleaseDoubleArrayElements(mat, matBody, 0);
- env->ReleaseDoubleArrayElements(vec, vecBody, 0);
-}
-
-/*
- * Class: edu_wpi_first_math_WPIMathJNI
- * Method: solveFullPivHouseholderQr
- * Signature: ([DII[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
- (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
- jint Brows, jint Bcols, jdoubleArray dst)
-{
- jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
- jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Amat{nativeA, Arows, Acols};
- Eigen::Map<
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
- Bmat{nativeB, Brows, Bcols};
-
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
- Amat.fullPivHouseholderQr().solve(Bmat);
-
- env->ReleaseDoubleArrayElements(A, nativeA, 0);
- env->ReleaseDoubleArrayElements(B, nativeB, 0);
- env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
-}
-
-} // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
new file mode 100644
index 0000000..e5c3ac5
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <stdexcept>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/DARE.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: dareDetailABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+ (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+ jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+ JSpan<const jdouble> nativeA{env, A};
+ JSpan<const jdouble> nativeB{env, B};
+ JSpan<const jdouble> nativeQ{env, Q};
+ JSpan<const jdouble> nativeR{env, R};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{nativeA.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Bmat{nativeB.data(), states, inputs};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Qmat{nativeQ.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Rmat{nativeR.data(), inputs, inputs};
+
+ Eigen::MatrixXd RmatCopy{Rmat};
+ auto R_llt = RmatCopy.llt();
+
+ Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+ Amat, Bmat, Qmat, R_llt);
+
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: dareDetailABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+ (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+ jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+ JSpan<const jdouble> nativeA{env, A};
+ JSpan<const jdouble> nativeB{env, B};
+ JSpan<const jdouble> nativeQ{env, Q};
+ JSpan<const jdouble> nativeR{env, R};
+ JSpan<const jdouble> nativeN{env, N};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{nativeA.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Bmat{nativeB.data(), states, inputs};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Qmat{nativeQ.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Rmat{nativeR.data(), inputs, inputs};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Nmat{nativeN.data(), states, inputs};
+
+ Eigen::MatrixXd Rcopy{Rmat};
+ auto R_llt = Rcopy.llt();
+
+ Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+ Amat, Bmat, Qmat, R_llt, Nmat);
+
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: dareABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQR
+ (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+ jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+ JSpan<const jdouble> nativeA{env, A};
+ JSpan<const jdouble> nativeB{env, B};
+ JSpan<const jdouble> nativeQ{env, Q};
+ JSpan<const jdouble> nativeR{env, R};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{nativeA.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Bmat{nativeB.data(), states, inputs};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Qmat{nativeQ.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Rmat{nativeR.data(), inputs, inputs};
+
+ try {
+ Eigen::MatrixXd result =
+ frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
+
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+ } catch (const std::invalid_argument& e) {
+ illegalArgEx.Throw(env, e.what());
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: dareABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
+ (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+ jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+ JSpan<const jdouble> nativeA{env, A};
+ JSpan<const jdouble> nativeB{env, B};
+ JSpan<const jdouble> nativeQ{env, Q};
+ JSpan<const jdouble> nativeR{env, R};
+ JSpan<const jdouble> nativeN{env, N};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{nativeA.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Bmat{nativeB.data(), states, inputs};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Qmat{nativeQ.data(), states, states};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Rmat{nativeR.data(), inputs, inputs};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Nmat{nativeN.data(), states, inputs};
+
+ try {
+ Eigen::MatrixXd result =
+ frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
+
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+ } catch (const std::invalid_argument& e) {
+ illegalArgEx.Throw(env, e.what());
+ }
+}
+
+} // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
new file mode 100644
index 0000000..642a2ba
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: exp
+ * Signature: ([DI[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_exp
+ (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
+{
+ JSpan<const jdouble> arrayBody{env, src};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{arrayBody.data(), rows, rows};
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
+ Amat.exp();
+
+ env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+ (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+ jdoubleArray dst)
+{
+ JSpan<const jdouble> arrayBody{env, src};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{arrayBody.data(), rows, rows};
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+ Amat.pow(exponent);
+
+ env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: rankUpdate
+ * Signature: ([DI[DDZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+ (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
+ jdouble sigma, jboolean lowerTriangular)
+{
+ JSpan<jdouble> matBody{env, mat};
+ JSpan<const jdouble> vecBody{env, vec};
+
+ Eigen::Map<
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+ L{matBody.data(), rows, rows};
+ Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{vecBody.data(),
+ rows};
+
+ if (lowerTriangular == JNI_TRUE) {
+ Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
+ } else {
+ Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: solveFullPivHouseholderQr
+ * Signature: ([DII[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+ (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
+ jint Brows, jint Bcols, jdoubleArray dst)
+{
+ JSpan<const jdouble> nativeA{env, A};
+ JSpan<const jdouble> nativeB{env, B};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Amat{nativeA.data(), Arows, Acols};
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ Bmat{nativeB.data(), Brows, Bcols};
+
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
+ Amat.fullPivHouseholderQr().solve(Bmat);
+
+ env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
+}
+
+} // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
new file mode 100644
index 0000000..7f5a03f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WPIMathJNI_Exceptions.h"
+
+#include <jni.h>
+
+using namespace wpi::java;
+
+//
+// Globals and load/unload
+//
+
+JException illegalArgEx;
+JException ioEx;
+JException trajectorySerializationEx;
+
+static const JExceptionInit exceptions[] = {
+ {"java/lang/IllegalArgumentException", &illegalArgEx},
+ {"java/io/IOException", &ioEx},
+ {"edu/wpi/first/math/trajectory/"
+ "TrajectoryUtil$TrajectorySerializationException",
+ &trajectorySerializationEx}};
+
+extern "C" {
+
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+ JNIEnv* env;
+ if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+ return JNI_ERR;
+ }
+
+ // Cache references to exceptions
+ for (auto& c : exceptions) {
+ *c.cls = JException(env, c.name);
+ if (!*c.cls) {
+ return JNI_ERR;
+ }
+ }
+
+ return JNI_VERSION_1_6;
+}
+
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+ JNIEnv* env;
+ if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+ return;
+ }
+ // Delete global references
+ for (auto& c : exceptions) {
+ c.cls->free(env);
+ }
+}
+
+} // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
new file mode 100644
index 0000000..cf0e149
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/jni_util.h>
+
+extern wpi::java::JException illegalArgEx;
+extern wpi::java::JException ioEx;
+extern wpi::java::JException trajectorySerializationEx;
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
new file mode 100644
index 0000000..f985231
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/geometry/Pose3d.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: expPose3d
+ * Signature: (DDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_expPose3d
+ (JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ,
+ jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz,
+ jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
+ jdouble twistRy, jdouble twistRz)
+{
+ frc::Pose3d pose{
+ units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ},
+ frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}};
+ frc::Twist3d twist{units::meter_t{twistDx}, units::meter_t{twistDy},
+ units::meter_t{twistDz}, units::radian_t{twistRx},
+ units::radian_t{twistRy}, units::radian_t{twistRz}};
+
+ frc::Pose3d result = pose.Exp(twist);
+
+ const auto& resultQuaternion = result.Rotation().GetQuaternion();
+ return MakeJDoubleArray(
+ env, {{result.X().value(), result.Y().value(), result.Z().value(),
+ resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
+ resultQuaternion.Z()}});
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: logPose3d
+ * Signature: (DDDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_logPose3d
+ (JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ,
+ jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz,
+ jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,
+ jdouble endQy, jdouble endQz)
+{
+ frc::Pose3d startPose{
+ units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ},
+ frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}};
+ frc::Pose3d endPose{
+ units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
+ frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
+
+ frc::Twist3d result = startPose.Log(endPose);
+
+ return MakeJDoubleArray(
+ env, {{result.dx.value(), result.dy.value(), result.dz.value(),
+ result.rx.value(), result.ry.value(), result.rz.value()}});
+}
+
+} // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
new file mode 100644
index 0000000..dc3dfdd
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <Eigen/Core>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/StateSpaceUtil.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: isStabilizable
+ * Signature: (II[D[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
+ (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
+ jdoubleArray bSrc)
+{
+ JSpan<const jdouble> nativeA{env, aSrc};
+ JSpan<const jdouble> nativeB{env, bSrc};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ A{nativeA.data(), states, states};
+
+ Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>>
+ B{nativeB.data(), states, inputs};
+
+ bool isStabilizable =
+ frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+
+ return isStabilizable;
+}
+
+} // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
new file mode 100644
index 0000000..3359da9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
@@ -0,0 +1,138 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <exception>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+
+using namespace wpi::java;
+
+std::vector<double> GetElementsFromTrajectory(
+ const frc::Trajectory& trajectory) {
+ std::vector<double> elements;
+ elements.reserve(trajectory.States().size() * 7);
+
+ for (auto&& state : trajectory.States()) {
+ elements.push_back(state.t.value());
+ elements.push_back(state.velocity.value());
+ elements.push_back(state.acceleration.value());
+ elements.push_back(state.pose.X().value());
+ elements.push_back(state.pose.Y().value());
+ elements.push_back(state.pose.Rotation().Radians().value());
+ elements.push_back(state.curvature.value());
+ }
+
+ return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
+ // Make sure that the elements have the correct length.
+ assert(elements.size() % 7 == 0);
+
+ // Create a vector of states from the elements.
+ std::vector<frc::Trajectory::State> states;
+ states.reserve(elements.size() / 7);
+
+ for (size_t i = 0; i < elements.size(); i += 7) {
+ states.emplace_back(frc::Trajectory::State{
+ units::second_t{elements[i]},
+ units::meters_per_second_t{elements[i + 1]},
+ units::meters_per_second_squared_t{elements[i + 2]},
+ frc::Pose2d{units::meter_t{elements[i + 3]},
+ units::meter_t{elements[i + 4]},
+ units::radian_t{elements[i + 5]}},
+ units::curvature_t{elements[i + 6]}});
+ }
+
+ return frc::Trajectory(states);
+}
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+ (JNIEnv* env, jclass, jstring path)
+{
+ try {
+ auto trajectory =
+ frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+ std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+ return MakeJDoubleArray(env, elements);
+ } catch (std::exception& e) {
+ ioEx.Throw(env, e.what());
+ return nullptr;
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+ (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+ try {
+ auto trajectory =
+ CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+ frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+ JStringRef{env, path}.c_str());
+ } catch (std::exception& e) {
+ ioEx.Throw(env, e.what());
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+ (JNIEnv* env, jclass, jstring json)
+{
+ try {
+ auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+ JStringRef{env, json}.c_str());
+ std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+ return MakeJDoubleArray(env, elements);
+ } catch (std::exception& e) {
+ trajectorySerializationEx.Throw(env, e.what());
+ return nullptr;
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+ (JNIEnv* env, jclass, jdoubleArray elements)
+{
+ try {
+ auto trajectory =
+ CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+ return MakeJString(env,
+ frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+ } catch (std::exception& e) {
+ trajectorySerializationEx.Throw(env, e.what());
+ return nullptr;
+ }
+}
+
+} // extern "C"
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 1ff7a8a..346a232 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -11,32 +11,9 @@
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose)
- : m_pose(initialPose),
- m_prevLeftDistance(leftDistance),
- m_prevRightDistance(rightDistance) {
- m_previousAngle = m_pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ : Odometry<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>(
+ m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
+ initialPose) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}
-
-const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
- units::meter_t leftDistance,
- units::meter_t rightDistance) {
- auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
- auto deltaRightDistance = rightDistance - m_prevRightDistance;
-
- m_prevLeftDistance = leftDistance;
- m_prevRightDistance = rightDistance;
-
- auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
- auto angle = gyroAngle + m_gyroOffset;
-
- auto newPose = m_pose.Exp(
- {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
-
- m_previousAngle = angle;
- m_pose = {newPose.Translation(), angle};
-
- return m_pose;
-}
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index 298dd7f..c21a7b2 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -25,7 +25,7 @@
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
- Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
+ Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -37,7 +37,7 @@
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
- Vectord<4> wheelSpeedsVector{
+ Eigen::Vector4d wheelSpeedsVector{
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
@@ -50,15 +50,30 @@
}
Twist2d MecanumDriveKinematics::ToTwist2d(
+ const MecanumDriveWheelPositions& start,
+ const MecanumDriveWheelPositions& end) const {
+ Eigen::Vector4d wheelDeltasVector{
+ end.frontLeft.value() - start.frontLeft.value(),
+ end.frontRight.value() - start.frontRight.value(),
+ end.rearLeft.value() - start.rearLeft.value(),
+ end.rearRight.value() - start.rearRight.value()};
+
+ Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
+
+ return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+ units::radian_t{twistVector(2)}};
+}
+
+Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const {
- Vectord<4> wheelDeltasVector{
+ Eigen::Vector4d wheelDeltasVector{
wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
- return {units::meter_t{twistVector(0)}, // NOLINT
- units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
+ return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+ units::radian_t{twistVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 394adaf..55055be 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -11,35 +11,9 @@
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
- : m_kinematics(kinematics),
- m_pose(initialPose),
- m_previousWheelPositions(wheelPositions) {
- m_previousAngle = m_pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ : Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
+ m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
+ m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}
-
-const Pose2d& MecanumDriveOdometry::Update(
- const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions) {
- auto angle = gyroAngle + m_gyroOffset;
-
- MecanumDriveWheelPositions wheelDeltas{
- wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
- wheelPositions.frontRight - m_previousWheelPositions.frontRight,
- wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
- wheelPositions.rearRight - m_previousWheelPositions.rearRight,
- };
-
- auto twist = m_kinematics.ToTwist2d(wheelDeltas);
- twist.dtheta = (angle - m_previousAngle).Radians();
-
- auto newPose = m_pose.Exp(twist);
-
- m_previousAngle = angle;
- m_previousWheelPositions = wheelPositions;
- m_pose = {newPose.Translation(), angle};
-
- return m_pose;
-}
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp
new file mode 100644
index 0000000..5343de9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+#include "frc/kinematics/SwerveModuleState.h"
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const {
+ return units::math::abs(distance - other.distance) < 1E-9_m &&
+ angle == other.angle;
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp
new file mode 100644
index 0000000..071d53a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/SwerveModuleState.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModuleState::operator==(const SwerveModuleState& other) const {
+ return units::math::abs(speed - other.speed) < 1E-9_mps &&
+ angle == other.angle;
+}
+
+SwerveModuleState SwerveModuleState::Optimize(
+ const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
+ auto delta = desiredState.angle - currentAngle;
+ if (units::math::abs(delta.Degrees()) > 90_deg) {
+ return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+ } else {
+ return {desiredState.speed, desiredState.angle};
+ }
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index de47547..5393f83 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -5,6 +5,7 @@
#include "frc/trajectory/Trajectory.h"
#include <algorithm>
+#include <stdexcept>
#include <wpi/MathExtras.h>
#include <wpi/json.h>
@@ -54,10 +55,20 @@
}
Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+ if (m_states.empty()) {
+ throw std::invalid_argument(
+ "Trajectory manually initialized with no states.");
+ }
+
m_totalTime = states.back().t;
}
Trajectory::State Trajectory::Sample(units::second_t t) const {
+ if (m_states.empty()) {
+ throw std::runtime_error(
+ "Trajectory cannot be sampled if it has no states.");
+ }
+
if (t <= m_states.front().t) {
return m_states.front();
}
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index 92d52ed..4c0a55e 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -223,10 +223,9 @@
if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
throw std::runtime_error(
- "The constraint's min acceleration was greater than its max "
- "acceleration. To debug this, remove all constraints from the config "
- "and add each one individually. If the offending constraint was "
- "packaged with WPILib, please file a bug report.");
+ "There was an infeasible trajectory constraint. To determine which "
+ "one, remove all constraints from the TrajectoryConfig and add them "
+ "back one-by-one.");
}
state->minAcceleration = units::math::max(
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index 169b642..da9c955 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -7,9 +7,8 @@
#include <system_error>
#include <fmt/format.h>
-#include <wpi/SmallString.h>
+#include <wpi/MemoryBuffer.h>
#include <wpi/json.h>
-#include <wpi/raw_istream.h>
#include <wpi/raw_ostream.h>
using namespace frc;
@@ -29,15 +28,14 @@
}
Trajectory TrajectoryUtil::FromPathweaverJson(std::string_view path) {
- std::error_code error_code;
-
- wpi::raw_fd_istream input{path, error_code};
- if (error_code) {
+ std::error_code ec;
+ std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+ wpi::MemoryBuffer::GetFile(path, ec);
+ if (fileBuffer == nullptr || ec) {
throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
- wpi::json json;
- input >> json;
+ wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
return Trajectory{json.get<std::vector<Trajectory::State>>()};
}
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index 2a308db..460ff8b 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -7,9 +7,8 @@
using namespace frc;
DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
- const DifferentialDriveKinematics& kinematics,
- units::meters_per_second_t maxSpeed)
- : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+ DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+ : m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index d60b4b6..46c306e 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -15,9 +15,9 @@
DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
const SimpleMotorFeedforward<units::meter>& feedforward,
- const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage)
+ DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
: m_feedforward(feedforward),
- m_kinematics(kinematics),
+ m_kinematics(std::move(kinematics)),
m_maxVoltage(maxVoltage) {}
units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(