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(
diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h
new file mode 100644
index 0000000..6a3104e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/DARE.h
@@ -0,0 +1,406 @@
+// 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 <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+
+// Works cited:
+//
+// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
+// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
+// International Journal of Control, 77:8, 767-788, 2004.
+// DOI: 10.1080/00207170410001714988
+
+namespace frc {
+
+namespace detail {
+
+/**
+ * Checks the preconditions of A, B, and Q for the DARE solver.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
+ * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
+ * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
+ * detectable.
+ */
+template <int States, int Inputs>
+void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
+ const Eigen::Matrix<double, States, Inputs>& B,
+ const Eigen::Matrix<double, States, States>& Q) {
+ // Require Q be symmetric
+ if ((Q - Q.transpose()).norm() > 1e-10) {
+ std::string msg = fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Q);
+ throw std::invalid_argument(msg);
+ }
+
+ // Require Q be positive semidefinite
+ //
+ // If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
+ // positive, negative, and zero diagonal entries in D equals the number of
+ // positive, negative, and zero eigenvalues respectively in Q (see
+ // https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
+ //
+ // Therefore, D having no negative diagonal entries is sufficient to prove Q
+ // is positive semidefinite.
+ auto Q_ldlt = Q.ldlt();
+ if (Q_ldlt.info() != Eigen::Success ||
+ (Q_ldlt.vectorD().array() < 0.0).any()) {
+ std::string msg =
+ fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", Q);
+ throw std::invalid_argument(msg);
+ }
+
+ // Require (A, B) pair be stabilizable
+ if (!IsStabilizable<States, Inputs>(A, B)) {
+ std::string msg = fmt::format(
+ "The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n", A, B);
+ throw std::invalid_argument(msg);
+ }
+
+ // Require (A, C) pair be detectable where Q = CᵀC
+ //
+ // Q = CᵀC = PᵀLDLᵀP
+ // Cᵀ = PᵀL√(D)
+ // C = (PᵀL√(D))ᵀ
+ {
+ Eigen::Matrix<double, States, States> C =
+ (Q_ldlt.transpositionsP().transpose() *
+ Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
+ Q_ldlt.vectorD().cwiseSqrt().asDiagonal())
+ .transpose();
+
+ if (!IsDetectable<States, States>(A, C)) {
+ std::string msg = fmt::format(
+ "The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
+ "=\n{}\n",
+ A, Q);
+ throw std::invalid_argument(msg);
+ }
+ }
+}
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * This internal function skips expensive precondition checks for increased
+ * performance. The solver may hang if any of the following occur:
+ * <ul>
+ * <li>Q isn't symmetric positive semidefinite</li>
+ * <li>R isn't symmetric positive definite</li>
+ * <li>The (A, B) pair isn't stabilizable</li>
+ * <li>The (A, C) pair where Q = CᵀC isn't detectable</li>
+ * </ul>
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R_llt The LLT decomposition of the input cost matrix.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+ const Eigen::Matrix<double, States, States>& A,
+ const Eigen::Matrix<double, States, Inputs>& B,
+ const Eigen::Matrix<double, States, States>& Q,
+ const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
+ using StateMatrix = Eigen::Matrix<double, States, States>;
+
+ // Implements the SDA algorithm on page 5 of [1].
+
+ // A₀ = A
+ StateMatrix A_k = A;
+
+ // G₀ = BR⁻¹Bᵀ
+ //
+ // See equation (4) of [1].
+ StateMatrix G_k = B * R_llt.solve(B.transpose());
+
+ // H₀ = Q
+ //
+ // See equation (4) of [1].
+ StateMatrix H_k;
+ StateMatrix H_k1 = Q;
+
+ do {
+ H_k = H_k1;
+
+ // W = I + GₖHₖ
+ StateMatrix W = StateMatrix::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
+
+ auto W_solver = W.lu();
+
+ // Solve WV₁ = Aₖ for V₁
+ StateMatrix V_1 = W_solver.solve(A_k);
+
+ // Solve V₂Wᵀ = Gₖ for V₂
+ //
+ // We want to put V₂Wᵀ = Gₖ into Ax = b form so we can solve it more
+ // efficiently.
+ //
+ // V₂Wᵀ = Gₖ
+ // (V₂Wᵀ)ᵀ = Gₖᵀ
+ // WV₂ᵀ = Gₖᵀ
+ //
+ // The solution of Ax = b can be found via x = A.solve(b).
+ //
+ // V₂ᵀ = W.solve(Gₖᵀ)
+ // V₂ = W.solve(Gₖᵀ)ᵀ
+ StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
+
+ // Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
+ G_k += A_k * V_2 * A_k.transpose();
+
+ // Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
+ H_k1 = H_k + V_1.transpose() * H_k * A_k;
+
+ // Aₖ₊₁ = AₖV₁
+ A_k *= V_1;
+
+ // while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
+ } while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
+
+ return H_k1;
+}
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+ AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+ A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+ A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+ ∞ [xₖ]ᵀ[Q N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+ ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+ ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+ k=0
+@endverbatim
+
+This internal function skips expensive precondition checks for increased
+performance. The solver may hang if any of the following occur:
+<ul>
+ <li>Q₂ isn't symmetric positive semidefinite</li>
+ <li>R isn't symmetric positive definite</li>
+ <li>The (A₂, B) pair isn't stabilizable</li>
+ <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable</li>
+</ul>
+Only use this function if you're sure the preconditions are met.
+
+@tparam States Number of states.
+@tparam Inputs Number of inputs.
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R_llt The LLT decomposition of the input cost matrix.
+@param N The state-input cross cost matrix.
+*/
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+ const Eigen::Matrix<double, States, States>& A,
+ const Eigen::Matrix<double, States, Inputs>& B,
+ const Eigen::Matrix<double, States, States>& Q,
+ const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt,
+ const Eigen::Matrix<double, Inputs, Inputs>& N) {
+ // This is a change of variables to make the DARE that includes Q, R, and N
+ // cost matrices fit the form of the DARE that includes only Q and R cost
+ // matrices.
+ //
+ // This is equivalent to solving the original DARE:
+ //
+ // A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+ //
+ // where A₂ and Q₂ are a change of variables:
+ //
+ // A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+ return detail::DARE<States, Inputs>(A - B * R_llt.solve(N.transpose()), B,
+ Q - N * R_llt.solve(N.transpose()),
+ R_llt);
+}
+
+} // namespace detail
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R The input cost matrix.
+ * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
+ * @throws std::invalid_argument if R isn't symmetric positive definite.
+ * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
+ * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
+ * detectable.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+ const Eigen::Matrix<double, States, States>& A,
+ const Eigen::Matrix<double, States, Inputs>& B,
+ const Eigen::Matrix<double, States, States>& Q,
+ const Eigen::Matrix<double, Inputs, Inputs>& R) {
+ // Require R be symmetric
+ if ((R - R.transpose()).norm() > 1e-10) {
+ std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
+ throw std::invalid_argument(msg);
+ }
+
+ // Require R be positive definite
+ auto R_llt = R.llt();
+ if (R_llt.info() != Eigen::Success) {
+ std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
+ throw std::invalid_argument(msg);
+ }
+
+ detail::CheckDARE_ABQ<States, Inputs>(A, B, Q);
+
+ return detail::DARE<States, Inputs>(A, B, Q, R_llt);
+}
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+ AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+ A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+ A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+ ∞ [xₖ]ᵀ[Q N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+ ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+ ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+ k=0
+@endverbatim
+
+@tparam States Number of states.
+@tparam Inputs Number of inputs.
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R The input cost matrix.
+@param N The state-input cross cost matrix.
+@throws std::invalid_argument if Q₂ isn't symmetric positive semidefinite.
+@throws std::invalid_argument if R isn't symmetric positive definite.
+@throws std::invalid_argument if the (A₂, B) pair isn't stabilizable.
+@throws std::invalid_argument if the (A₂, C) pair where Q₂ = CᵀC isn't
+ detectable.
+*/
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+ const Eigen::Matrix<double, States, States>& A,
+ const Eigen::Matrix<double, States, Inputs>& B,
+ const Eigen::Matrix<double, States, States>& Q,
+ const Eigen::Matrix<double, Inputs, Inputs>& R,
+ const Eigen::Matrix<double, States, Inputs>& N) {
+ // Require R be symmetric
+ if ((R - R.transpose()).norm() > 1e-10) {
+ std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
+ throw std::invalid_argument(msg);
+ }
+
+ // Require R be positive definite
+ auto R_llt = R.llt();
+ if (R_llt.info() != Eigen::Success) {
+ std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
+ throw std::invalid_argument(msg);
+ }
+
+ // This is a change of variables to make the DARE that includes Q, R, and N
+ // cost matrices fit the form of the DARE that includes only Q and R cost
+ // matrices.
+ //
+ // This is equivalent to solving the original DARE:
+ //
+ // A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+ //
+ // where A₂ and Q₂ are a change of variables:
+ //
+ // A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+ Eigen::Matrix<double, States, States> A_2 =
+ A - B * R_llt.solve(N.transpose());
+ Eigen::Matrix<double, States, States> Q_2 =
+ Q - N * R_llt.solve(N.transpose());
+
+ detail::CheckDARE_ABQ<States, Inputs>(A_2, B, Q_2);
+
+ return detail::DARE<States, Inputs>(A_2, B, Q_2, R_llt);
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/EigenCore.h b/wpimath/src/main/native/include/frc/EigenCore.h
index b33e9e2..1604a0d 100644
--- a/wpimath/src/main/native/include/frc/EigenCore.h
+++ b/wpimath/src/main/native/include/frc/EigenCore.h
@@ -4,7 +4,7 @@
#pragma once
-#include "Eigen/Core"
+#include <Eigen/Core>
namespace frc {
diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
index 24bf857..26f106e 100644
--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -5,6 +5,7 @@
#pragma once
#include <numbers>
+#include <type_traits>
#include <wpi/SymbolExports.h>
@@ -25,12 +26,11 @@
* be infinite.
* @return The value after the deadband is applied.
*/
-template <typename T,
- typename = std::enable_if_t<std::disjunction_v<
- std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+template <typename T>
+ requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
T magnitude;
- if constexpr (std::is_floating_point_v<T>) {
+ if constexpr (std::is_arithmetic_v<T>) {
magnitude = std::abs(value);
} else {
magnitude = units::math::abs(value);
@@ -106,6 +106,58 @@
}
/**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+ requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance) {
+ if constexpr (std::is_arithmetic_v<T>) {
+ return std::abs(expected - actual) < tolerance;
+ } else {
+ return units::math::abs(expected - actual) < tolerance;
+ }
+}
+
+/**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance. Supports continuous input for cases like absolute encoders.
+ *
+ * Continuous input means that the min and max value are considered to be the
+ * same point, and tolerances can be checked across them. A common example
+ * would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns
+ * true because 359 is 1 away from 360 (which is treated as the same as 0) and
+ * 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+ * given tolerance of 5.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @param min Smallest value before wrapping around to the largest value
+ * @param max Largest value before wrapping around to the smallest value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+ requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
+ T errorBound = (max - min) / 2.0;
+ T error = frc::InputModulus<T>(expected - actual, -errorBound, errorBound);
+
+ if constexpr (std::is_arithmetic_v<T>) {
+ return std::abs(error) < tolerance;
+ } else {
+ return units::math::abs(error) < tolerance;
+ }
+}
+
+/**
* Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
*
* @param angle Angle to wrap.
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 0345b46..3aa2e75 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -6,31 +6,21 @@
#include <array>
#include <cmath>
+#include <concepts>
#include <limits>
#include <random>
-#include <type_traits>
+#include <Eigen/Eigenvalues>
+#include <Eigen/QR>
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
-#include "Eigen/Eigenvalues"
-#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
namespace frc {
namespace detail {
-template <int Rows, int Cols, typename Matrix, typename T, typename... Ts>
-void MatrixImpl(Matrix& result, T elem, Ts... elems) {
- constexpr int count = Rows * Cols - (sizeof...(Ts) + 1);
-
- result(count / Cols, count % Cols) = elem;
- if constexpr (sizeof...(Ts) > 0) {
- MatrixImpl<Rows, Cols>(result, elems...);
- }
-}
-
template <typename Matrix, typename T, typename... Ts>
void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
if (elem == std::numeric_limits<double>::infinity()) {
@@ -66,26 +56,37 @@
template <int States, int Inputs>
bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
- Eigen::EigenSolver<Matrixd<States, States>> es{A};
+ Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
- 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) {
+ for (int i = 0; i < A.rows(); ++i) {
+ if (std::norm(es.eigenvalues()[i]) < 1) {
continue;
}
- Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
- E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
- States>::Identity() -
- A,
- B;
+ if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
+ Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
+ E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
+ States>::Identity() -
+ A,
+ B;
- Eigen::ColPivHouseholderQR<
- Eigen::Matrix<std::complex<double>, States, States + Inputs>>
- qr{E};
- if (qr.rank() < States) {
- return false;
+ Eigen::ColPivHouseholderQR<
+ Eigen::Matrix<std::complex<double>, States, States + Inputs>>
+ qr{E};
+ if (qr.rank() < States) {
+ return false;
+ }
+ } else {
+ Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
+ E << es.eigenvalues()[i] *
+ Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
+ A,
+ B;
+
+ Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+ if (qr.rank() < A.rows()) {
+ return false;
+ }
}
}
return true;
@@ -106,8 +107,7 @@
* of the control inputs from no actuation.
* @return State excursion or control effort cost matrix.
*/
-template <typename... Ts, typename = std::enable_if_t<
- std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CostMatrixImpl(result.diagonal(), tolerances...);
@@ -126,8 +126,7 @@
* output measurement.
* @return Process noise or measurement noise covariance matrix.
*/
-template <typename... Ts, typename = std::enable_if_t<
- std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CovMatrixImpl(result.diagonal(), stdDevs...);
@@ -138,7 +137,8 @@
* Creates a cost matrix from the given vector for use with LQR.
*
* The cost matrix is constructed using Bryson's rule. The inverse square of
- * each element in the input is taken and placed on the cost matrix diagonal.
+ * each element in the input is placed on the cost matrix diagonal. If a
+ * tolerance is infinity, its cost matrix entry is set to zero.
*
* @param costs An array. For a Q matrix, its elements are the maximum allowed
* excursions of the states from the reference. For an R matrix,
@@ -151,7 +151,11 @@
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
- diag(i) = 1.0 / std::pow(costs[i], 2);
+ if (costs[i] == std::numeric_limits<double>::infinity()) {
+ diag(i) = 0.0;
+ } else {
+ diag(i) = 1.0 / std::pow(costs[i], 2);
+ }
}
return result;
}
@@ -178,8 +182,7 @@
return result;
}
-template <typename... Ts, typename = std::enable_if_t<
- std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Matrixd<sizeof...(Ts), 1> result;
detail::WhiteNoiseVectorImpl(result, stdDevs...);
@@ -221,7 +224,7 @@
* @return The vector.
*/
WPILIB_DLLEXPORT
-Vectord<3> PoseTo3dVector(const Pose2d& pose);
+Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -231,14 +234,14 @@
* @return The vector.
*/
WPILIB_DLLEXPORT
-Vectord<4> PoseTo4dVector(const Pose2d& pose);
+Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
/**
* 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.
+ * uncontrollable if rank([λI - A, B]) < n where n is the number of states.
*
* @tparam States The number of states.
* @tparam Inputs The number of inputs.
@@ -256,7 +259,7 @@
*
* (A, C) is detectable if and only if the unobservable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is unobservable
- * if rank(λI - A; C) < n where n is the number of states.
+ * if rank([λI - A; C]) < n where n is the number of states.
*
* @tparam States The number of states.
* @tparam Outputs The number of outputs.
@@ -282,6 +285,12 @@
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B);
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
+ const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
+
/**
* Converts a Pose2d into a vector of [x, y, theta].
*
@@ -290,7 +299,7 @@
* @return The vector.
*/
WPILIB_DLLEXPORT
-Vectord<3> PoseToVector(const Pose2d& pose);
+Eigen::Vector3d PoseToVector(const Pose2d& pose);
/**
* Clamps input vector between system's minimum and maximum allowable input.
diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 21aad25..6306457 100644
--- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -7,7 +7,8 @@
#include <array>
#include <functional>
-#include "Eigen/QR"
+#include <Eigen/QR>
+
#include "frc/EigenCore.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
@@ -165,6 +166,9 @@
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
StateVector rDot = (nextR - r) / m_dt.value();
+ // ṙ = f(r) + Bu
+ // Bu = ṙ − f(r)
+ // u = B⁺(ṙ − f(r))
m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
m_r = nextR;
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
index 3a5148a..d84e3ce 100644
--- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
@@ -4,9 +4,9 @@
#pragma once
+#include <Eigen/Core>
#include <wpi/SymbolExports.h>
-#include "Eigen/Core"
#include "frc/controller/DifferentialDriveWheelVoltages.h"
#include "frc/system/LinearSystem.h"
#include "units/acceleration.h"
diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index bbe7720..62a7bad 100644
--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -6,6 +6,9 @@
#include <wpi/MathExtras.h>
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
#include "units/length.h"
#include "units/time.h"
#include "units/voltage.h"
@@ -54,6 +57,50 @@
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
}
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param currentVelocity The current velocity setpoint, in distance per
+ * second.
+ * @param nextVelocity The next velocity setpoint, in distance per second.
+ * @param dt Time between velocity setpoints in seconds.
+ * @return The computed feedforward, in volts.
+ */
+ units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+ units::unit_t<Velocity> nextVelocity,
+ units::second_t dt) const {
+ // Discretize the affine model.
+ //
+ // dx/dt = Ax + Bu + c
+ // dx/dt = Ax + B(u + B⁺c)
+ // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+ // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+ // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+ //
+ // Solve for uₖ.
+ //
+ // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+ // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+ // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+ //
+ // For an elevator with the model
+ // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+ // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+ // assuming sgn(x) is a constant for the duration of the step.
+ //
+ // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+ // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+ // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+ auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+ LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+ Vectord<1> r{currentVelocity.value()};
+ Vectord<1> nextR{nextVelocity.value()};
+
+ return kG + kS * wpi::sgn(currentVelocity.value()) +
+ units::volt_t{feedforward.Calculate(r, nextR)(0)};
+ }
+
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
index 9a749c6..6f9568e 100644
--- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -42,7 +42,7 @@
* angle.
*/
HolonomicDriveController(
- frc2::PIDController xController, frc2::PIDController yController,
+ PIDController xController, PIDController yController,
ProfiledPIDController<units::radian> thetaController);
HolonomicDriveController(const HolonomicDriveController&) = default;
@@ -103,14 +103,29 @@
*/
void SetEnabled(bool enabled);
+ /**
+ * Returns the rotation ProfiledPIDController
+ */
+ ProfiledPIDController<units::radian>& getThetaController();
+
+ /**
+ * Returns the X PIDController
+ */
+ PIDController& getXController();
+
+ /**
+ * Returns the Y PIDController
+ */
+ PIDController& getYController();
+
private:
Pose2d m_poseError;
Rotation2d m_rotationError;
Pose2d m_poseTolerance;
bool m_enabled = true;
- frc2::PIDController m_xController;
- frc2::PIDController m_yController;
+ PIDController m_xController;
+ PIDController m_yController;
ProfiledPIDController<units::radian> m_thetaController;
bool m_firstRun = true;
diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
index eef1fc0..3a1230d 100644
--- a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
+++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
@@ -6,7 +6,8 @@
#include <frc/system/LinearSystem.h>
-#include "Eigen/QR"
+#include <Eigen/QR>
+
#include "frc/EigenCore.h"
#include "units/time.h"
diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
index 0a336aa..94f3fa3 100644
--- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
@@ -23,9 +23,17 @@
/**
* The linear time-varying differential drive controller has a similar form to
* the LQR, but the model used to compute the controller gain is the nonlinear
- * model linearized around the drivetrain's current state. We precomputed gains
- * for important places in our state-space, then interpolated between them with
- * a LUT to save computational resources.
+ * differential drive model linearized around the drivetrain's current state. We
+ * precompute gains for important places in our state-space, then interpolate
+ * between them with a lookup table to save computational resources.
+ *
+ * This controller has a flat hierarchy with pose and wheel velocity references
+ * and voltage outputs. This is different from a Ramsete controller's nested
+ * hierarchy where the top-level controller has a pose reference and chassis
+ * velocity command outputs, and the low-level controller has wheel velocity
+ * references and voltage outputs. Flat hierarchies are easier to tune in one
+ * shot. Furthermore, this controller is more optimal in the "least-squares
+ * error" sense than a controller based on Ramsete.
*
* See section 8.7 in Controls Engineering in FRC for a derivation of the
* control law we used shown in theorem 8.7.4.
@@ -35,12 +43,18 @@
/**
* Constructs a linear time-varying differential drive controller.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+ * for how to select the tolerances.
+ *
* @param plant The differential drive velocity plant.
* @param trackwidth The distance between the differential drive's left and
* right wheels.
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
+ * @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s
+ * or >= 15 m/s.
*/
LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
units::meter_t trackwidth,
diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
index 83cfe4b..38c4287 100644
--- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
+++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
@@ -20,8 +20,11 @@
/**
* The linear time-varying unicycle controller has a similar form to the LQR,
- * but the model used to compute the controller gain is the nonlinear model
- * linearized around the drivetrain's current state.
+ * but the model used to compute the controller gain is the nonlinear unicycle
+ * model linearized around the drivetrain's current state.
+ *
+ * This controller is a roughly drop-in replacement for RamseteController with
+ * more optimal feedback gains in the "least-squares error" sense.
*
* See section 8.9 in Controls Engineering in FRC for a derivation of the
* control law we used shown in theorem 8.9.1.
@@ -36,6 +39,7 @@
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
+ * @throws std::domain_error if maxVelocity <= 0.
*/
explicit LTVUnicycleController(
units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
@@ -43,11 +47,16 @@
/**
* Constructs a linear time-varying unicycle controller.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+ * for how to select the tolerances.
+ *
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
+ * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s.
*/
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
const wpi::array<double, 2>& Relems, units::second_t dt,
diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index d4cc3c4..1d905e2 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -7,7 +7,8 @@
#include <array>
#include <functional>
-#include "Eigen/QR"
+#include <Eigen/QR>
+
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
@@ -137,6 +138,9 @@
* @return The calculated feedforward.
*/
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+ // rₖ₊₁ = Arₖ + Buₖ
+ // Buₖ = rₖ₊₁ − Arₖ
+ // uₖ = B⁺(rₖ₊₁ − Arₖ)
m_uff = m_B.householderQr().solve(nextR - (m_A * r));
m_r = nextR;
return m_uff;
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 50d6566..979e98a 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -36,6 +36,10 @@
/**
* Constructs a controller with the given coefficients and plant.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+ * for how to select the tolerances.
+ *
* @param plant The plant being controlled.
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
@@ -50,6 +54,10 @@
/**
* Constructs a controller with the given coefficients and plant.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+ * for how to select the tolerances.
+ *
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Qelems The maximum desired error tolerance for each state.
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
index 87d37ec..1871244 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
@@ -7,14 +7,14 @@
#include <stdexcept>
#include <string>
-#include "Eigen/Cholesky"
-#include "Eigen/Eigenvalues"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Cholesky>
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
-#include "unsupported/Eigen/MatrixFunctions"
#include "wpimath/MathShared.h"
namespace frc {
@@ -52,8 +52,7 @@
throw std::invalid_argument(msg);
}
- Matrixd<States, States> S =
- drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+ Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
@@ -72,8 +71,7 @@
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
- Matrixd<States, States> S =
- drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+ Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S * discB + R)
diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
index d6a41d1..0d5b0a3 100644
--- a/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -13,7 +13,7 @@
#include "units/time.h"
-namespace frc2 {
+namespace frc {
/**
* Implements a PID control loop.
@@ -74,6 +74,18 @@
void SetD(double Kd);
/**
+ * Sets the IZone range. When the absolute value of the position error is
+ * greater than IZone, the total accumulated error will reset to zero,
+ * disabling integral gain until the absolute value of the position error is
+ * less than IZone. This is used to prevent integral windup. Must be
+ * non-negative. Passing a value of zero will effectively disable integral
+ * gain. Passing a value of infinity disables IZone functionality.
+ *
+ * @param iZone Maximum magnitude of error to allow integral control.
+ */
+ void SetIZone(double iZone);
+
+ /**
* Gets the proportional coefficient.
*
* @return proportional coefficient
@@ -95,6 +107,13 @@
double GetD() const;
/**
+ * Get the IZone range.
+ *
+ * @return Maximum magnitude of error to allow integral control.
+ */
+ double GetIZone() const;
+
+ /**
* Gets the period of this controller.
*
* @return The period of the controller.
@@ -221,6 +240,9 @@
// Factor for "derivative" control
double m_Kd;
+ // The error range where "integral" control applies
+ double m_iZone = std::numeric_limits<double>::infinity();
+
// The period (in seconds) of the control loop running this controller
units::second_t m_period;
@@ -252,12 +274,9 @@
double m_setpoint = 0;
double m_measurement = 0;
+
+ bool m_haveSetpoint = false;
+ bool m_haveMeasurement = false;
};
-} // namespace frc2
-
-namespace frc {
-
-using frc2::PIDController;
-
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 8491118..8f211c6 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -58,7 +58,9 @@
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
Constraints constraints, units::second_t period = 20_ms)
- : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+ : m_controller{Kp, Ki, Kd, period},
+ m_constraints{constraints},
+ m_profile{m_constraints} {
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
@@ -107,6 +109,18 @@
void SetD(double Kd) { m_controller.SetD(Kd); }
/**
+ * Sets the IZone range. When the absolute value of the position error is
+ * greater than IZone, the total accumulated error will reset to zero,
+ * disabling integral gain until the absolute value of the position error is
+ * less than IZone. This is used to prevent integral windup. Must be
+ * non-negative. Passing a value of zero will effectively disable integral
+ * gain. Passing a value of infinity disables IZone functionality.
+ *
+ * @param iZone Maximum magnitude of error to allow integral control.
+ */
+ void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
+
+ /**
* Gets the proportional coefficient.
*
* @return proportional coefficient
@@ -128,6 +142,13 @@
double GetD() const { return m_controller.GetD(); }
/**
+ * Get the IZone range.
+ *
+ * @return Maximum magnitude of error to allow integral control.
+ */
+ double GetIZone() const { return m_controller.GetIZone(); }
+
+ /**
* Gets the period of this controller.
*
* @return The period of the controller.
@@ -183,7 +204,16 @@
*
* @param constraints Velocity and acceleration constraints for goal.
*/
- void SetConstraints(Constraints constraints) { m_constraints = constraints; }
+ void SetConstraints(Constraints constraints) {
+ m_constraints = constraints;
+ m_profile = TrapezoidProfile<Distance>{m_constraints};
+ }
+
+ /**
+ * Get the velocity and acceleration constraints for this controller.
+ * @return Velocity and acceleration constraints.
+ */
+ Constraints GetConstraints() { return m_constraints; }
/**
* Returns the current setpoint of the ProfiledPIDController.
@@ -292,8 +322,7 @@
m_setpoint.position = setpointMinDistance + measurement;
}
- frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
- m_setpoint = profile.Calculate(GetPeriod());
+ m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
return m_controller.Calculate(measurement.value(),
m_setpoint.position.value());
}
@@ -372,17 +401,22 @@
builder.AddDoubleProperty(
"d", [this] { return GetD(); }, [this](double value) { SetD(value); });
builder.AddDoubleProperty(
+ "izone", [this] { return GetIZone(); },
+ [this](double value) { SetIZone(value); });
+ builder.AddDoubleProperty(
"goal", [this] { return GetGoal().position.value(); },
[this](double value) { SetGoal(Distance_t{value}); });
}
private:
- frc2::PIDController m_controller;
+ PIDController m_controller;
Distance_t m_minimumInput{0};
Distance_t m_maximumInput{0};
+
+ typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
+ TrapezoidProfile<Distance> m_profile;
typename frc::TrapezoidProfile<Distance>::State m_goal;
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
- typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 0512c68c..026cc67 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -4,6 +4,7 @@
#pragma once
+#include <functional>
#include <numbers>
#include "frc/EigenCore.h"
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 339ccc9..89dcd35 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -7,13 +7,11 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
-#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
@@ -32,7 +30,9 @@
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
*/
-class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
+class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
+ : public PoseEstimator<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions> {
public:
/**
* Constructs a DifferentialDrivePoseEstimator with default standard
@@ -80,19 +80,6 @@
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
- * Sets the pose estimator's trust in vision measurements. This might be used
- * to change trust in vision measurements after the autonomous period, or to
- * change trust as distance to a vision target increases.
- *
- * @param visionMeasurementStdDevs Standard deviations of the vision pose
- * measurement (x position in meters, y position in meters, and heading in
- * radians). Increase these numbers to trust the vision pose measurement
- * less.
- */
- void SetVisionMeasurementStdDevs(
- const wpi::array<double, 3>& visionMeasurementStdDevs);
-
- /**
* Resets the robot's position on the field.
*
* @param gyroAngle The current gyro angle.
@@ -101,71 +88,10 @@
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
- units::meter_t rightDistance, const Pose2d& pose);
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose.
- */
- Pose2d GetEstimatedPosition() const;
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are
- * calling Update() every loop.
- *
- * To promote stability of the pose estimate and make it robust to bad vision
- * data, we recommend only adding vision measurements that are already within
- * one meter or so of the current pose estimate.
- *
- * @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds. Note
- * that if you don't use your own time source by calling UpdateWithTime(),
- * then you must use a timestamp with an epoch since FPGA startup (i.e.,
- * the epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp(). This means that you should use
- * frc::Timer::GetFPGATimestamp() as your time source in this case.
- */
- void AddVisionMeasurement(const Pose2d& visionRobotPose,
- units::second_t timestamp);
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are
- * calling Update() every loop.
- *
- * To promote stability of the pose estimate and make it robust to bad vision
- * data, we recommend only adding vision measurements that are already within
- * one meter or so of the current pose estimate.
- *
- * Note that the vision measurement standard deviations passed into this
- * method will continue to apply to future measurements until a subsequent
- * call to SetVisionMeasurementStdDevs() or this method.
- *
- * @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds. Note
- * that if you don't use your own time source by calling UpdateWithTime(),
- * then you must use a timestamp with an epoch since FPGA startup (i.e.,
- * the epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp(). This means that you should use
- * frc::Timer::GetFPGATimestamp() as your time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision pose
- * measurement (x position in meters, y position in meters, and heading in
- * radians). Increase these numbers to trust the vision pose measurement
- * less.
- */
- void AddVisionMeasurement(
- const Pose2d& visionRobotPose, units::second_t timestamp,
- const wpi::array<double, 3>& visionMeasurementStdDevs) {
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
- AddVisionMeasurement(visionRobotPose, timestamp);
+ units::meter_t rightDistance, const Pose2d& pose) {
+ PoseEstimator<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions>::
+ ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
/**
@@ -179,7 +105,12 @@
* @return The estimated pose of the robot.
*/
Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
- units::meter_t rightDistance);
+ units::meter_t rightDistance) {
+ return PoseEstimator<
+ DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions>::Update(gyroAngle,
+ {leftDistance, rightDistance});
+ }
/**
* Updates the pose estimator with wheel encoder and gyro information. This
@@ -195,61 +126,16 @@
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
units::meter_t leftDistance,
- units::meter_t rightDistance);
+ units::meter_t rightDistance) {
+ return PoseEstimator<
+ DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle,
+ {leftDistance,
+ rightDistance});
+ }
private:
- struct InterpolationRecord {
- // The pose observed given the current sensor inputs and the previous pose.
- Pose2d pose;
-
- // The current gyro angle.
- Rotation2d gyroAngle;
-
- // The distance traveled by the left encoder.
- units::meter_t leftDistance;
-
- // The distance traveled by the right encoder.
- units::meter_t rightDistance;
-
- /**
- * Checks equality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
- */
- bool operator==(const InterpolationRecord& other) const = default;
-
- /**
- * Checks inequality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const InterpolationRecord& other) const = default;
-
- /**
- * Interpolates between two InterpolationRecords.
- *
- * @param endValue The end value for the interpolation.
- * @param i The interpolant (fraction).
- *
- * @return The interpolated state.
- */
- InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics,
- InterpolationRecord endValue,
- double i) const;
- };
-
- DifferentialDriveKinematics& m_kinematics;
- DifferentialDriveOdometry m_odometry;
- wpi::array<double, 3> m_q{wpi::empty_array};
- Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
- TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
- 1.5_s, [this](const InterpolationRecord& start,
- const InterpolationRecord& end, double t) {
- return start.Interpolate(this->m_kinematics, end, t);
- }};
+ DifferentialDriveOdometry m_odometryImpl;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index b09e8d9..32ed558 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -52,6 +52,10 @@
/**
* Constructs an extended Kalman filter.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+ * for how to select the standard deviations.
+ *
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
@@ -69,6 +73,10 @@
/**
* Constructs an extended Kalman filter.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+ * for how to select the standard deviations.
+ *
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
@@ -163,6 +171,33 @@
Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
}
+ /**
+ * Correct the state estimate x-hat using the measurements in y.
+ *
+ * This is useful for when the measurement noise covariances vary.
+ *
+ * @param u Same control input used in the predict step.
+ * @param y Measurement vector.
+ * @param R Continuous measurement noise covariance matrix.
+ */
+ void Correct(const InputVector& u, const OutputVector& y,
+ const Matrixd<Outputs, Outputs>& R) {
+ Correct<Outputs>(u, y, m_h, R, m_residualFuncY, m_addFuncX);
+ }
+
+ /**
+ * Correct the state estimate x-hat using the measurements in y.
+ *
+ * This is useful for when the measurements available during a timestep's
+ * Correct() call vary. The h(x, u) passed to the constructor is used if one
+ * is not provided (the two-argument version of this function).
+ *
+ * @param u Same control input used in the predict step.
+ * @param y Measurement vector.
+ * @param h A vector-valued function of x and u that returns the measurement
+ * vector.
+ * @param R Continuous measurement noise covariance matrix.
+ */
template <int Rows>
void Correct(
const InputVector& u, const Vectord<Rows>& y,
@@ -180,7 +215,7 @@
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns
* the measurement vector.
- * @param R Discrete measurement noise covariance matrix.
+ * @param R Continuous measurement noise covariance matrix.
* @param residualFuncY A function that computes the residual of two
* measurement vectors (i.e. it subtracts them.)
* @param addFuncX A function that adds two state vectors.
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
index a56b8b5..0b0e9de 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
@@ -4,8 +4,11 @@
#pragma once
-#include "Eigen/Cholesky"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <functional>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/Discretization.h"
@@ -36,13 +39,13 @@
StateMatrix discA;
StateMatrix discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+ DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
- m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
- discA.transpose(), C.transpose(), discQ, discR);
+ m_initP =
+ DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
@@ -72,13 +75,13 @@
StateMatrix discA;
StateMatrix discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+ DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
- m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
- discA.transpose(), C.transpose(), discQ, discR);
+ m_initP =
+ DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
@@ -95,7 +98,7 @@
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+ DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index 2121284..f143493 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -46,6 +46,10 @@
/**
* Constructs a state-space observer with the given plant.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+ * for how to select the standard deviations.
+ *
* @param plant The plant used for the prediction step.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
index ca4f37c..7506c0d 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
@@ -10,8 +10,9 @@
#include <stdexcept>
#include <string>
-#include "Eigen/Cholesky"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/KalmanFilter.h"
#include "frc/system/Discretization.h"
@@ -31,7 +32,7 @@
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
- DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
+ DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
@@ -47,8 +48,8 @@
throw std::invalid_argument(msg);
}
- Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
- discA.transpose(), C.transpose(), discQ, discR);
+ Matrixd<States, States> P =
+ DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index d1967e9..d748164 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -9,7 +9,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,7 +32,9 @@
* never call it, then this class will behave mostly like regular encoder
* odometry.
*/
-class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
+class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
+ : public PoseEstimator<MecanumDriveWheelSpeeds,
+ MecanumDriveWheelPositions> {
public:
/**
* Constructs a MecanumDrivePoseEstimator with default standard deviations
@@ -76,172 +78,8 @@
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
- /**
- * Sets the pose estimator's trust in vision measurements. This might be used
- * to change trust in vision measurements after the autonomous period, or to
- * change trust as distance to a vision target increases.
- *
- * @param visionMeasurementStdDevs Standard deviations of the vision pose
- * measurement (x position in meters, y position in meters, and heading in
- * radians). Increase these numbers to trust the vision pose measurement
- * less.
- */
- void SetVisionMeasurementStdDevs(
- const wpi::array<double, 3>& visionMeasurementStdDevs);
-
- /**
- * Resets the robot's position on the field.
- *
- * The gyroscope angle does not need to be reset in the user's robot code.
- * The library automatically takes care of offsetting the gyro angle.
- *
- * @param gyroAngle The angle reported by the gyroscope.
- * @param wheelPositions The distances measured at each wheel.
- * @param pose The position on the field that your robot is at.
- */
- void ResetPosition(const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions,
- const Pose2d& pose);
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose in meters.
- */
- Pose2d GetEstimatedPosition() const;
-
- /**
- * Add a vision measurement to the Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are
- * calling Update() every loop.
- *
- * To promote stability of the pose estimate and make it robust to bad vision
- * data, we recommend only adding vision measurements that are already within
- * one meter or so of the current pose estimate.
- *
- * @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds. Note
- * that if you don't use your own time source by calling UpdateWithTime()
- * then you must use a timestamp with an epoch since FPGA startup (i.e.,
- * the epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp().) This means that you should use
- * frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
- */
- void AddVisionMeasurement(const Pose2d& visionRobotPose,
- units::second_t timestamp);
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are
- * calling Update() every loop.
- *
- * To promote stability of the pose estimate and make it robust to bad vision
- * data, we recommend only adding vision measurements that are already within
- * one meter or so of the current pose estimate.
- *
- * Note that the vision measurement standard deviations passed into this
- * method will continue to apply to future measurements until a subsequent
- * call to SetVisionMeasurementStdDevs() or this method.
- *
- * @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds. Note
- * that if you don't use your own time source by calling UpdateWithTime(),
- * then you must use a timestamp with an epoch since FPGA startup (i.e.,
- * the epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp(). This means that you should use
- * frc::Timer::GetFPGATimestamp() as your time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision pose
- * measurement (x position in meters, y position in meters, and heading in
- * radians). Increase these numbers to trust the vision pose measurement
- * less.
- */
- void AddVisionMeasurement(
- const Pose2d& visionRobotPose, units::second_t timestamp,
- const wpi::array<double, 3>& visionMeasurementStdDevs) {
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
- AddVisionMeasurement(visionRobotPose, timestamp);
- }
-
- /**
- * Updates the pose estimator with wheel encoder and gyro information. This
- * should be called every loop.
- *
- * @param gyroAngle The current gyro angle.
- * @param wheelPositions The distances measured at each wheel.
- * @return The estimated pose of the robot in meters.
- */
- Pose2d Update(const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions);
-
- /**
- * Updates the pose estimator with wheel encoder and gyro information. This
- * should be called every loop.
- *
- * @param currentTime Time at which this method was called, in seconds.
- * @param gyroAngle The current gyroscope angle.
- * @param wheelPositions The distances measured at each wheel.
- * @return The estimated pose of the robot in meters.
- */
- Pose2d UpdateWithTime(units::second_t currentTime,
- const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions);
-
private:
- struct InterpolationRecord {
- // The pose observed given the current sensor inputs and the previous pose.
- Pose2d pose;
-
- // The current gyroscope angle.
- Rotation2d gyroAngle;
-
- // The distances measured at each wheel.
- MecanumDriveWheelPositions wheelPositions;
-
- /**
- * Checks equality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
- */
- bool operator==(const InterpolationRecord& other) const = default;
-
- /**
- * Checks inequality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const InterpolationRecord& other) const = default;
-
- /**
- * Interpolates between two InterpolationRecords.
- *
- * @param endValue The end value for the interpolation.
- * @param i The interpolant (fraction).
- *
- * @return The interpolated state.
- */
- InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics,
- InterpolationRecord endValue,
- double i) const;
- };
-
- MecanumDriveKinematics& m_kinematics;
- MecanumDriveOdometry m_odometry;
- wpi::array<double, 3> m_q{wpi::empty_array};
- Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
- TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
- 1.5_s, [this](const InterpolationRecord& start,
- const InterpolationRecord& end, double t) {
- return start.Interpolate(this->m_kinematics, end, t);
- }};
+ MecanumDriveOdometry m_odometryImpl;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
new file mode 100644
index 0000000..eb437a4
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -0,0 +1,228 @@
+// 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 <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/Odometry.h"
+#include "frc/kinematics/WheelPositions.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * This class wraps odometry to fuse latency-compensated
+ * vision measurements with encoder measurements. Robot code should not use this
+ * directly- Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDrivePoseEstimator). It will correct for noisy vision
+ * measurements and encoder drift. It is intended to be an easy drop-in for
+ * Odometry.
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT PoseEstimator {
+ public:
+ /**
+ * Constructs a PoseEstimator.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param odometry A correctly-configured odometry object for your drivetrain.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
+ */
+ PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+ Odometry<WheelSpeeds, WheelPositions>& odometry,
+ const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+ /**
+ * Sets the pose estimator's trust in vision measurements. This might be used
+ * to change trust in vision measurements after the autonomous period, or to
+ * change trust as distance to a vision target increases.
+ *
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
+ */
+ void SetVisionMeasurementStdDevs(
+ const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * The gyroscope angle does not need to be reset in the user's robot code.
+ * The library automatically takes care of offsetting the gyro angle.
+ *
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distances traveled by the encoders.
+ * @param pose The estimated pose of the robot on the field.
+ */
+ void ResetPosition(const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions, const Pose2d& pose);
+
+ /**
+ * Gets the estimated robot pose.
+ *
+ * @return The estimated robot pose in meters.
+ */
+ Pose2d GetEstimatedPosition() const;
+
+ /**
+ * Adds a vision measurement to the Kalman Filter. This will correct
+ * the odometry pose estimate while still accounting for measurement noise.
+ *
+ * This method can be called as infrequently as you want, as long as you are
+ * calling Update() every loop.
+ *
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ */
+ void AddVisionMeasurement(const Pose2d& visionRobotPose,
+ units::second_t timestamp);
+
+ /**
+ * Adds a vision measurement to the Kalman Filter. This will correct
+ * the odometry pose estimate while still accounting for measurement noise.
+ *
+ * This method can be called as infrequently as you want, as long as you are
+ * calling Update() every loop.
+ *
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
+ * Note that the vision measurement standard deviations passed into this
+ * method will continue to apply to future measurements until a subsequent
+ * call to SetVisionMeasurementStdDevs() or this method.
+ *
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
+ */
+ void AddVisionMeasurement(
+ const Pose2d& visionRobotPose, units::second_t timestamp,
+ const wpi::array<double, 3>& visionMeasurementStdDevs) {
+ SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+ AddVisionMeasurement(visionRobotPose, timestamp);
+ }
+
+ /**
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
+ *
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distances traveled by the encoders.
+ *
+ * @return The estimated pose of the robot in meters.
+ */
+ Pose2d Update(const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions);
+
+ /**
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
+ *
+ * @param currentTime The time at which this method was called.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distances traveled by the encoders.
+ *
+ * @return The estimated pose of the robot in meters.
+ */
+ Pose2d UpdateWithTime(units::second_t currentTime,
+ const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions);
+
+ private:
+ struct InterpolationRecord {
+ // The pose observed given the current sensor inputs and the previous pose.
+ Pose2d pose;
+
+ // The current gyroscope angle.
+ Rotation2d gyroAngle;
+
+ // The distances traveled by the wheels.
+ WheelPositions wheelPositions;
+
+ /**
+ * Checks equality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const InterpolationRecord& other) const = default;
+
+ /**
+ * Checks inequality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const InterpolationRecord& other) const = default;
+
+ /**
+ * Interpolates between two InterpolationRecords.
+ *
+ * @param endValue The end value for the interpolation.
+ * @param i The interpolant (fraction).
+ *
+ * @return The interpolated state.
+ */
+ InterpolationRecord Interpolate(
+ Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+ InterpolationRecord endValue, double i) const;
+ };
+
+ static constexpr units::second_t kBufferDuration = 1.5_s;
+
+ Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+ Odometry<WheelSpeeds, WheelPositions>& m_odometry;
+ wpi::array<double, 3> m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+ TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
+ kBufferDuration, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
+};
+} // namespace frc
+
+#include "frc/estimator/PoseEstimator.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
new file mode 100644
index 0000000..79d71bc
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
@@ -0,0 +1,165 @@
+// 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 "frc/estimator/PoseEstimator.h"
+
+namespace frc {
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
+ Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+ Odometry<WheelSpeeds, WheelPositions>& odometry,
+ const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs)
+ : m_kinematics(kinematics), m_odometry(odometry) {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+ }
+
+ SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::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]));
+ }
+ }
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
+ const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+ const Pose2d& pose) {
+ // Reset state estimate and error covariance
+ m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+ m_poseBuffer.Clear();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
+ const {
+ return m_odometry.GetPose();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
+ const Pose2d& visionRobotPose, units::second_t timestamp) {
+ // Step 0: If this measurement is old enough to be outside the pose buffer's
+ // timespan, skip.
+ if (!m_poseBuffer.GetInternalBuffer().empty() &&
+ m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+ timestamp) {
+ return;
+ }
+
+ // 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.
+ Eigen::Vector3d k_times_twist =
+ m_visionK *
+ Eigen::Vector3d{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: Record the current pose to allow multiple measurements from the
+ // same timestamp
+ m_poseBuffer.AddSample(timestamp,
+ {GetEstimatedPosition(), sample.value().gyroAngle,
+ sample.value().wheelPositions});
+
+ // Step 7: 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);
+ }
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
+ const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
+ return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
+ wheelPositions);
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions) {
+ m_odometry.Update(gyroAngle, wheelPositions);
+
+ WheelPositions internalWheelPositions = wheelPositions;
+
+ m_poseBuffer.AddSample(
+ currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
+
+ return GetEstimatedPosition();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
+PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
+ Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+ InterpolationRecord endValue, double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the new wheel distance measurements.
+ WheelPositions wheels_lerp =
+ this->wheelPositions.Interpolate(endValue.wheelPositions, i);
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent the change based on the interpolated
+ // sensor inputs.
+ auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, wheels_lerp};
+ }
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index d07bafe..43831d7 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -6,17 +6,15 @@
#include <cmath>
-#include <fmt/format.h>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include <wpi/timestamp.h>
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
-#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/kinematics/SwerveDriveWheelPositions.h"
#include "units/time.h"
namespace frc {
@@ -33,7 +31,9 @@
* odometry.
*/
template <size_t NumModules>
-class SwerveDrivePoseEstimator {
+class SwerveDrivePoseEstimator
+ : public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -83,14 +83,10 @@
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
- : m_kinematics{kinematics},
- m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
- for (size_t i = 0; i < 3; ++i) {
- m_q[i] = stateStdDevs[i] * stateStdDevs[i];
- }
-
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
- }
+ : PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>(
+ kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+ m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
/**
* Resets the robot's position on the field.
@@ -107,143 +103,11 @@
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
- // Reset state estimate and error covariance
- m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
- m_poseBuffer.Clear();
- }
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose in meters.
- */
- Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
-
- /**
- * Sets the pose estimator's trust in vision measurements. This might be used
- * to change trust in vision measurements after the autonomous period, or to
- * change trust as distance to a vision target increases.
- *
- * @param visionMeasurementStdDevs Standard deviations of the vision pose
- * measurement (x position in meters, y position in meters, and heading in
- * radians). Increase these numbers to trust the vision pose measurement
- * less.
- */
- void 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]));
- }
- }
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the
- * odometry pose estimate while still accounting for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are
- * calling Update() every loop.
- *
- * To promote stability of the pose estimate and make it robust to bad vision
- * data, we recommend only adding vision measurements that are already within
- * one meter or so of the current pose estimate.
- *
- * @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds. Note
- * that if you don't use your own time source by calling UpdateWithTime()
- * then you must use a timestamp with an epoch since FPGA startup (i.e.,
- * the epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp().) This means that you should use
- * frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
- */
- void 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().modulePostions,
- 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.modulePostions);
- }
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the
- * odometry pose estimate while still accounting for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are
- * calling Update() every loop.
- *
- * To promote stability of the pose estimate and make it robust to bad vision
- * data, we recommend only adding vision measurements that are already within
- * one meter or so of the current pose estimate.
- *
- * Note that the vision measurement standard deviations passed into this
- * method will continue to apply to future measurements until a subsequent
- * call to SetVisionMeasurementStdDevs() or this method.
- *
- * @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds. Note
- * that if you don't use your own time source by calling UpdateWithTime(),
- * then you must use a timestamp with an epoch since FPGA startup (i.e.,
- * the epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp(). This means that you should use
- * frc::Timer::GetFPGATimestamp() as your time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision pose
- * measurement (x position in meters, y position in meters, and heading in
- * radians). Increase these numbers to trust the vision pose measurement
- * less.
- */
- void AddVisionMeasurement(
- const Pose2d& visionRobotPose, units::second_t timestamp,
- const wpi::array<double, 3>& visionMeasurementStdDevs) {
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
- AddVisionMeasurement(visionRobotPose, timestamp);
+ PoseEstimator<
+ SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+ {modulePositions},
+ pose);
}
/**
@@ -258,8 +122,10 @@
Pose2d Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
- return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- modulePositions);
+ return PoseEstimator<
+ SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+ {modulePositions});
}
/**
@@ -275,109 +141,13 @@
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
- m_odometry.Update(gyroAngle, modulePositions);
-
- wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
- wpi::empty_array};
-
- for (size_t i = 0; i < NumModules; i++) {
- internalModulePositions[i].distance = modulePositions[i].distance;
- internalModulePositions[i].angle = modulePositions[i].angle;
- }
-
- m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
- internalModulePositions});
-
- return GetEstimatedPosition();
+ return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>::
+ UpdateWithTime(currentTime, gyroAngle, {modulePositions});
}
private:
- struct InterpolationRecord {
- // The pose observed given the current sensor inputs and the previous pose.
- Pose2d pose;
-
- // The current gyroscope angle.
- Rotation2d gyroAngle;
-
- // The distances traveled and rotations meaured at each module.
- wpi::array<SwerveModulePosition, NumModules> modulePostions;
-
- /**
- * Checks equality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
- */
- bool operator==(const InterpolationRecord& other) const = default;
-
- /**
- * Checks inequality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const InterpolationRecord& other) const = default;
-
- /**
- * Interpolates between two InterpolationRecords.
- *
- * @param endValue The end value for the interpolation.
- * @param i The interpolant (fraction).
- *
- * @return The interpolated state.
- */
- InterpolationRecord Interpolate(
- SwerveDriveKinematics<NumModules>& kinematics,
- InterpolationRecord endValue, double i) const {
- if (i < 0) {
- return *this;
- } else if (i > 1) {
- return endValue;
- } else {
- // Find the new module distances.
- wpi::array<SwerveModulePosition, NumModules> modulePositions{
- wpi::empty_array};
- // Find the distance between this measurement and the
- // interpolated measurement.
- wpi::array<SwerveModulePosition, NumModules> modulesDelta{
- wpi::empty_array};
-
- for (size_t i = 0; i < NumModules; i++) {
- modulePositions[i].distance =
- wpi::Lerp(this->modulePostions[i].distance,
- endValue.modulePostions[i].distance, i);
- modulePositions[i].angle =
- wpi::Lerp(this->modulePostions[i].angle,
- endValue.modulePostions[i].angle, i);
-
- modulesDelta[i].distance =
- modulePositions[i].distance - this->modulePostions[i].distance;
- modulesDelta[i].angle = modulePositions[i].angle;
- }
-
- // 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(modulesDelta);
- twist.dtheta = (gyro - gyroAngle).Radians();
-
- return {pose.Exp(twist), gyro, modulePositions};
- }
- }
- };
-
- SwerveDriveKinematics<NumModules>& m_kinematics;
- SwerveDriveOdometry<NumModules> m_odometry;
- wpi::array<double, 3> m_q{wpi::empty_array};
- Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
- TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
- 1.5_s, [this](const InterpolationRecord& start,
- const InterpolationRecord& end, double t) {
- return start.Interpolate(this->m_kinematics, end, t);
- }};
+ SwerveDriveOdometry<NumModules> m_odometryImpl;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 39ce615..9526f0c 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -58,6 +58,10 @@
/**
* Constructs an unscented Kalman filter.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+ * for how to select the standard deviations.
+ *
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
@@ -78,6 +82,10 @@
* you have angles in the state or measurements, because they allow you to
* correctly account for the modular nature of angle arithmetic.
*
+ * See
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+ * for how to select the standard deviations.
+ *
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
@@ -205,6 +213,21 @@
/**
* Correct the state estimate x-hat using the measurements in y.
*
+ * This is useful for when the measurement noise covariances vary.
+ *
+ * @param u Same control input used in the predict step.
+ * @param y Measurement vector.
+ * @param R Continuous measurement noise covariance matrix.
+ */
+ void Correct(const InputVector& u, const OutputVector& y,
+ const Matrixd<Outputs, Outputs>& R) {
+ Correct<Outputs>(u, y, m_h, R, m_meanFuncY, m_residualFuncY,
+ m_residualFuncX, m_addFuncX);
+ }
+
+ /**
+ * Correct the state estimate x-hat using the measurements in y.
+ *
* This is useful for when the measurements available during a timestep's
* Correct() call vary. The h(x, u) passed to the constructor is used if one
* is not provided (the two-argument version of this function).
@@ -213,7 +236,7 @@
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement
* vector.
- * @param R Measurement noise covariance matrix (continuous-time).
+ * @param R Continuous measurement noise covariance matrix.
*/
template <int Rows>
void Correct(
@@ -232,7 +255,7 @@
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the
* measurement vector.
- * @param R Measurement noise covariance matrix (continuous-time).
+ * @param R Continuous measurement noise covariance matrix.
* @param meanFuncY A function that computes the mean of 2 * States + 1
* measurement vectors using a given set of weights.
* @param residualFuncY A function that computes the residual of two
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
index a6744bf..c693197 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
@@ -4,7 +4,10 @@
#pragma once
-#include "Eigen/Cholesky"
+#include <functional>
+
+#include <Eigen/Cholesky>
+
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/estimator/UnscentedTransform.h"
@@ -76,7 +79,7 @@
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
+ DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas =
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index e28f094..bec3bc8 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -4,9 +4,11 @@
#pragma once
+#include <functional>
#include <tuple>
-#include "Eigen/QR"
+#include <Eigen/QR>
+
#include "frc/EigenCore.h"
namespace frc {
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 0fb4f48..fc558df 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -11,10 +11,10 @@
#include <stdexcept>
#include <vector>
+#include <Eigen/QR>
#include <wpi/array.h>
#include <wpi/circular_buffer.h>
-#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -95,7 +95,7 @@
static int instances = 0;
instances++;
wpi::math::MathSharedStore::ReportUsage(
- wpi::math::MathUsageId::kFilter_Linear, 1);
+ wpi::math::MathUsageId::kFilter_Linear, instances);
}
/**
diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index 542cd94..17d0d21 100644
--- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -10,6 +10,7 @@
#include <wpi/timestamp.h>
#include "units/time.h"
+#include "wpimath/MathShared.h"
namespace frc {
/**
@@ -45,7 +46,8 @@
: m_positiveRateLimit{positiveRateLimit},
m_negativeRateLimit{negativeRateLimit},
m_prevVal{initialValue},
- m_prevTime{units::microsecond_t(wpi::Now())} {}
+ m_prevTime{
+ units::microsecond_t(wpi::math::MathSharedStore::GetTimestamp())} {}
/**
* Creates a new SlewRateLimiter with the given positive rate limit and
@@ -57,19 +59,6 @@
: SlewRateLimiter(rateLimit, -rateLimit) {}
/**
- * Creates a new SlewRateLimiter with the given positive rate limit and
- * negative rate limit of -rateLimit and initial value.
- *
- * @param rateLimit The rate-of-change limit.
- * @param initialValue The initial value of the input.
- */
- WPI_DEPRECATED(
- "Use SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit, "
- "Unit_t initalValue) instead")
- SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue)
- : SlewRateLimiter(rateLimit, -rateLimit, initialValue) {}
-
- /**
* Filters the input to limit its slew rate.
*
* @param input The input value whose slew rate is to be limited.
@@ -77,7 +66,7 @@
* rate.
*/
Unit_t Calculate(Unit_t input) {
- units::second_t currentTime = units::microsecond_t(wpi::Now());
+ units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
units::second_t elapsedTime = currentTime - m_prevTime;
m_prevVal +=
std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
@@ -94,7 +83,7 @@
*/
void Reset(Unit_t value) {
m_prevVal = value;
- m_prevTime = units::microsecond_t(wpi::Now());
+ m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
}
private:
diff --git a/wpimath/src/main/native/include/frc/fmt/Eigen.h b/wpimath/src/main/native/include/frc/fmt/Eigen.h
index c6b2ee6..2438b9f 100644
--- a/wpimath/src/main/native/include/frc/fmt/Eigen.h
+++ b/wpimath/src/main/native/include/frc/fmt/Eigen.h
@@ -4,120 +4,41 @@
#pragma once
+#include <concepts>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
#include <fmt/format.h>
-#include "Eigen/Core"
-#include "Eigen/SparseCore"
-
/**
- * Formatter for Eigen::Matrix<double, Rows, Cols>.
- *
- * @tparam Rows Number of rows.
- * @tparam Cols Number of columns.
- * @tparam Args Defaulted template arguments to Eigen::Matrix<>.
+ * Formatter for classes derived from Eigen::MatrixBase<Derived> or
+ * Eigen::SparseCompressedBase<Derived>.
*/
-template <int Rows, int Cols, int... Args>
-struct fmt::formatter<Eigen::Matrix<double, Rows, Cols, Args...>> {
- /**
- * Storage for format specifier.
- */
- char presentation = 'f';
-
- /**
- * Format string parser.
- *
- * @param ctx Format string context.
- */
+template <typename Derived, typename CharT>
+ requires std::derived_from<Derived, Eigen::MatrixBase<Derived>> ||
+ std::derived_from<Derived, Eigen::SparseCompressedBase<Derived>>
+struct fmt::formatter<Derived, CharT> {
constexpr auto parse(fmt::format_parse_context& ctx) {
- auto it = ctx.begin(), end = ctx.end();
- if (it != end && (*it == 'f' || *it == 'e')) {
- presentation = *it++;
- }
-
- if (it != end && *it != '}') {
- throw fmt::format_error("invalid format");
- }
-
- return it;
+ return m_underlying.parse(ctx);
}
- /**
- * Writes out a formatted matrix.
- *
- * @tparam FormatContext Format string context type.
- * @param mat Matrix to format.
- * @param ctx Format string context.
- */
- template <typename FormatContext>
- auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
- FormatContext& ctx) {
+ auto format(const Derived& mat, fmt::format_context& ctx) const {
auto out = ctx.out();
- for (int i = 0; i < mat.rows(); ++i) {
- for (int j = 0; j < mat.cols(); ++j) {
- out = fmt::format_to(out, " {:f}", mat(i, j));
+
+ for (int row = 0; row < mat.rows(); ++row) {
+ for (int col = 0; col < mat.cols(); ++col) {
+ out = fmt::format_to(out, " ");
+ out = m_underlying.format(mat.coeff(row, col), ctx);
}
- if (i < mat.rows() - 1) {
+ if (row < mat.rows() - 1) {
out = fmt::format_to(out, "\n");
}
}
return out;
}
-};
-/**
- * Formatter for Eigen::SparseMatrix<double>.
- *
- * @tparam Options Union of bit flags controlling the storage scheme.
- * @tparam StorageIndex The type of the indices.
- */
-template <int Options, typename StorageIndex>
-struct fmt::formatter<Eigen::SparseMatrix<double, Options, StorageIndex>> {
- /**
- * Storage for format specifier.
- */
- char presentation = 'f';
-
- /**
- * Format string parser.
- *
- * @param ctx Format string context.
- */
- constexpr auto parse(fmt::format_parse_context& ctx) {
- auto it = ctx.begin(), end = ctx.end();
- if (it != end && (*it == 'f' || *it == 'e')) {
- presentation = *it++;
- }
-
- if (it != end && *it != '}') {
- throw fmt::format_error("invalid format");
- }
-
- return it;
- }
-
- /**
- * Writes out a formatted matrix.
- *
- * @tparam FormatContext Format string context type.
- * @param mat Matrix to format.
- * @param ctx Format string context.
- */
- template <typename FormatContext>
- auto format(const Eigen::SparseMatrix<double, Options, StorageIndex>& mat,
- FormatContext& ctx) {
- auto out = ctx.out();
- for (int i = 0; i < mat.rows(); ++i) {
- for (int j = 0; j < mat.cols(); ++j) {
- out = fmt::format_to(out, " {:f}", mat.coeff(i, j));
- }
-
- if (i < mat.rows() - 1) {
- out = fmt::format_to(out, "\n");
- }
- }
-
- return out;
- }
+ private:
+ fmt::formatter<typename Derived::Scalar, CharT> m_underlying;
};
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
index 58b966a..2b471b7 100644
--- a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
@@ -4,9 +4,9 @@
#pragma once
+#include <Eigen/Core>
#include <wpi/SymbolExports.h>
-#include "frc/EigenCore.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
@@ -67,7 +67,7 @@
private:
friend class CoordinateSystem;
- Vectord<3> m_axis;
+ Eigen::Vector3d m_axis;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
index 232455f..f5e71f2 100644
--- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
@@ -6,7 +6,6 @@
#include <wpi/SymbolExports.h>
-#include "frc/EigenCore.h"
#include "frc/geometry/CoordinateAxis.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index d096e8c..970f792 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -4,15 +4,18 @@
#pragma once
+#include <initializer_list>
+#include <span>
+
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Transform2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
-
-namespace wpi {
-class json;
-} // namespace wpi
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
namespace frc {
@@ -120,6 +123,15 @@
constexpr Pose2d operator/(double scalar) const;
/**
+ * Rotates the pose around the origin and returns the new pose.
+ *
+ * @param other The rotation to transform the pose by.
+ *
+ * @return The rotated pose.
+ */
+ constexpr Pose2d RotateBy(const Rotation2d& other) const;
+
+ /**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
@@ -176,6 +188,20 @@
*/
Twist2d Log(const Pose2d& end) const;
+ /**
+ * Returns the nearest Pose2d from a collection of poses
+ * @param poses The collection of poses.
+ * @return The nearest Pose2d from the collection.
+ */
+ Pose2d Nearest(std::span<const Pose2d> poses) const;
+
+ /**
+ * Returns the nearest Pose2d from a collection of poses
+ * @param poses The collection of poses.
+ * @return The nearest Pose2d from the collection.
+ */
+ Pose2d Nearest(std::initializer_list<Pose2d> poses) const;
+
private:
Translation2d m_translation;
Rotation2d m_rotation;
@@ -189,4 +215,38 @@
} // namespace frc
-#include "Pose2d.inc"
+template <>
+struct wpi::Struct<frc::Pose2d> {
+ static constexpr std::string_view kTypeString = "struct:Pose2d";
+ static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+ wpi::Struct<frc::Rotation2d>::kSize;
+ static constexpr std::string_view kSchema =
+ "Translation2d translation;Rotation2d rotation";
+ static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data) {
+ return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+ wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+ }
+ static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value) {
+ wpi::PackStruct<0>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+ }
+ static void ForEachNested(
+ std::invocable<std::string_view, std::string_view> auto fn) {
+ wpi::ForEachStructSchema<frc::Translation2d>(fn);
+ wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+ }
+
+ private:
+ static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Pose2d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value);
+};
+
+#include "frc/geometry/Pose2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
index c549f26..2764d16 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
@@ -31,6 +31,10 @@
return *this * (1.0 / scalar);
}
+constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
+ return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
index 8c7ace3..b5a0e03 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
@@ -5,15 +5,15 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Pose2d.h"
-#include "Transform3d.h"
-#include "Translation3d.h"
-#include "Twist3d.h"
-
-namespace wpi {
-class json;
-} // namespace wpi
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Transform3d.h"
+#include "frc/geometry/Translation3d.h"
+#include "frc/geometry/Twist3d.h"
namespace frc {
@@ -56,7 +56,9 @@
/**
* Transforms the pose by the given transformation and returns the new
- * transformed pose.
+ * transformed pose. The transform is applied relative to the pose's frame.
+ * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+ * applied relative to the global frame and around the origin.
*
* @param other The transform to transform the pose by.
*
@@ -131,8 +133,20 @@
Pose3d operator/(double scalar) const;
/**
- * Transforms the pose by the given transformation and returns the new pose.
- * See + operator for the matrix multiplication performed.
+ * Rotates the pose around the origin and returns the new pose.
+ *
+ * @param other The rotation to transform the pose by, which is applied
+ * extrinsically (from the global frame).
+ *
+ * @return The rotated pose.
+ */
+ Pose3d RotateBy(const Rotation3d& other) const;
+
+ /**
+ * Transforms the pose by the given transformation and returns the new
+ * transformed pose. The transform is applied relative to the pose's frame.
+ * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+ * applied relative to the global frame and around the origin.
*
* @param other The transform to transform the pose by.
*
@@ -202,3 +216,37 @@
void from_json(const wpi::json& json, Pose3d& pose);
} // namespace frc
+
+template <>
+struct wpi::Struct<frc::Pose3d> {
+ static constexpr std::string_view kTypeString = "struct:Pose3d";
+ static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+ wpi::Struct<frc::Rotation3d>::kSize;
+ static constexpr std::string_view kSchema =
+ "Translation3d translation;Rotation3d rotation";
+ static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data) {
+ return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+ wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+ }
+ static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value) {
+ wpi::PackStruct<0>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+ }
+ static void ForEachNested(
+ std::invocable<std::string_view, std::string_view> auto fn) {
+ wpi::ForEachStructSchema<frc::Translation3d>(fn);
+ wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+ }
+
+ private:
+ static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Pose3d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
index b5a318b..63a3af2 100644
--- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h
+++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
@@ -4,13 +4,11 @@
#pragma once
+#include <Eigen/Core>
#include <wpi/SymbolExports.h>
-
-#include "frc/EigenCore.h"
-
-namespace wpi {
-class json;
-} // namespace wpi
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
namespace frc {
@@ -32,6 +30,34 @@
Quaternion(double w, double x, double y, double z);
/**
+ * Adds with another quaternion.
+ *
+ * @param other the other quaternion
+ */
+ Quaternion operator+(const Quaternion& other) const;
+
+ /**
+ * Subtracts another quaternion.
+ *
+ * @param other the other quaternion
+ */
+ Quaternion operator-(const Quaternion& other) const;
+
+ /**
+ * Multiples with a scalar value.
+ *
+ * @param other the scalar value
+ */
+ Quaternion operator*(const double other) const;
+
+ /**
+ * Divides by a scalar value.
+ *
+ * @param other the scalar value
+ */
+ Quaternion operator/(const double other) const;
+
+ /**
* Multiply with another quaternion.
*
* @param other The other quaternion.
@@ -47,6 +73,16 @@
bool operator==(const Quaternion& other) const;
/**
+ * Returns the elementwise product of two quaternions.
+ */
+ double Dot(const Quaternion& other) const;
+
+ /**
+ * Returns the conjugate of the quaternion.
+ */
+ Quaternion Conjugate() const;
+
+ /**
* Returns the inverse of the quaternion.
*/
Quaternion Inverse() const;
@@ -57,6 +93,52 @@
Quaternion Normalize() const;
/**
+ * Calculates the L2 norm of the quaternion.
+ */
+ double Norm() const;
+
+ /**
+ * Calculates this quaternion raised to a power.
+ *
+ * @param t the power to raise this quaternion to.
+ */
+ Quaternion Pow(const double t) const;
+
+ /**
+ * Matrix exponential of a quaternion.
+ *
+ * @param other the "Twist" that will be applied to this quaternion.
+ */
+ Quaternion Exp(const Quaternion& other) const;
+
+ /**
+ * Matrix exponential of a quaternion.
+ *
+ * source: wpimath/algorithms.md
+ *
+ * If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
+ * SO(3), use FromRotationVector
+ */
+ Quaternion Exp() const;
+
+ /**
+ * Log operator of a quaternion.
+ *
+ * @param other The quaternion to map this quaternion onto
+ */
+ Quaternion Log(const Quaternion& other) const;
+
+ /**
+ * Log operator of a quaternion.
+ *
+ * source: wpimath/algorithms.md
+ *
+ * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
+ * use ToRotationVector
+ */
+ Quaternion Log() const;
+
+ /**
* Returns W component of the quaternion.
*/
double W() const;
@@ -83,8 +165,20 @@
*/
Eigen::Vector3d ToRotationVector() const;
+ /**
+ * Returns the quaternion representation of this rotation vector.
+ *
+ * This is also the exp operator of 𝖘𝖔(3).
+ *
+ * source: wpimath/algorithms.md
+ */
+ static Quaternion FromRotationVector(const Eigen::Vector3d& rvec);
+
private:
+ // Scalar r in versor form
double m_r = 1.0;
+
+ // Vector v in versor form
Eigen::Vector3d m_v{0.0, 0.0, 0.0};
};
@@ -95,3 +189,32 @@
void from_json(const wpi::json& json, Quaternion& quaternion);
} // namespace frc
+
+template <>
+struct wpi::Struct<frc::Quaternion> {
+ static constexpr std::string_view kTypeString = "struct:Quaternion";
+ static constexpr size_t kSize = 32;
+ static constexpr std::string_view kSchema =
+ "double w;double x;double y;double z";
+ static frc::Quaternion Unpack(std::span<const uint8_t, 32> data) {
+ return {wpi::UnpackStruct<double, 0>(data),
+ wpi::UnpackStruct<double, 8>(data),
+ wpi::UnpackStruct<double, 16>(data),
+ wpi::UnpackStruct<double, 24>(data)};
+ }
+ static void Pack(std::span<uint8_t, 32> data, const frc::Quaternion& value) {
+ wpi::PackStruct<0>(data, value.W());
+ wpi::PackStruct<8>(data, value.X());
+ wpi::PackStruct<16>(data, value.Y());
+ wpi::PackStruct<24>(data, value.Z());
+ }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
+ static constexpr std::string_view kTypeString = "proto:Quaternion";
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Quaternion Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Quaternion& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 406ef3c..96041a4 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -5,13 +5,12 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
#include "units/angle.h"
-namespace wpi {
-class json;
-} // namespace wpi
-
namespace frc {
/**
@@ -179,4 +178,25 @@
} // namespace frc
-#include "Rotation2d.inc"
+template <>
+struct wpi::Struct<frc::Rotation2d> {
+ static constexpr std::string_view kTypeString = "struct:Rotation2d";
+ static constexpr size_t kSize = 8;
+ static constexpr std::string_view kSchema = "double value";
+ static frc::Rotation2d Unpack(std::span<const uint8_t, 8> data) {
+ return units::radian_t{wpi::UnpackStruct<double>(data)};
+ }
+ static void Pack(std::span<uint8_t, 8> data, const frc::Rotation2d& value) {
+ wpi::PackStruct(data, value.Radians().value());
+ }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Rotation2d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Rotation2d& value);
+};
+
+#include "frc/geometry/Rotation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
index eb31ebd..dbe9e35 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
@@ -23,7 +23,8 @@
: Rotation2d(units::radian_t{value}) {}
constexpr Rotation2d::Rotation2d(double x, double y) {
- const auto magnitude = gcem::hypot(x, y);
+ double magnitude =
+ std::is_constant_evaluated() ? gcem::hypot(x, y) : std::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
index 7c1a60d..6d04715 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
@@ -4,17 +4,16 @@
#pragma once
+#include <Eigen/Core>
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Quaternion.h"
-#include "Rotation2d.h"
-#include "frc/EigenCore.h"
+#include "frc/geometry/Quaternion.h"
+#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
-namespace wpi {
-class json;
-} // namespace wpi
-
namespace frc {
/**
@@ -57,7 +56,16 @@
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
- Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+ Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle);
+
+ /**
+ * Constructs a Rotation3d with the given rotation vector representation. This
+ * representation is equivalent to axis-angle, where the normalized axis is
+ * multiplied by the rotation around the axis in radians.
+ *
+ * @param rvec The rotation vector.
+ */
+ explicit Rotation3d(const Eigen::Vector3d& rvec);
/**
* Constructs a Rotation3d from a rotation matrix.
@@ -65,7 +73,7 @@
* @param rotationMatrix The rotation matrix.
* @throws std::domain_error if the rotation matrix isn't special orthogonal.
*/
- explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+ explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix);
/**
* Constructs a Rotation3d that rotates the initial vector onto the final
@@ -77,7 +85,7 @@
* @param initial The initial vector.
* @param final The final vector.
*/
- Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
+ Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final);
/**
* Adds two rotations together.
@@ -126,12 +134,16 @@
/**
* Checks equality between this Rotation3d and another object.
*/
- bool operator==(const Rotation3d&) const = default;
+ bool operator==(const Rotation3d&) const;
/**
- * Adds the new rotation to the current rotation.
+ * Adds the new rotation to the current rotation. The other rotation is
+ * applied extrinsically, which means that it rotates around the global axes.
+ * For example, Rotation3d{90_deg, 0, 0}.RotateBy(Rotation3d{0, 45_deg, 0})
+ * rotates by 90 degrees around the +X axis and then by 45 degrees around the
+ * global +Y axis. (This is equivalent to Rotation3d{90_deg, 45_deg, 0})
*
- * @param other The rotation to rotate by.
+ * @param other The extrinsic rotation to rotate by.
*
* @return The new rotated Rotation3d.
*/
@@ -160,7 +172,7 @@
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
- Vectord<3> Axis() const;
+ Eigen::Vector3d Axis() const;
/**
* Returns the angle in the axis-angle representation of this rotation.
@@ -184,3 +196,31 @@
void from_json(const wpi::json& json, Rotation3d& rotation);
} // namespace frc
+
+template <>
+struct wpi::Struct<frc::Rotation3d> {
+ static constexpr std::string_view kTypeString = "struct:Rotation3d";
+ static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
+ static constexpr std::string_view kSchema = "Quaternion q";
+ static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data) {
+ return frc::Rotation3d{wpi::UnpackStruct<frc::Quaternion, 0>(data)};
+ }
+ static void Pack(std::span<uint8_t, kSize> data,
+ const frc::Rotation3d& value) {
+ wpi::PackStruct<0>(data, value.GetQuaternion());
+ }
+ static void ForEachNested(
+ std::invocable<std::string_view, std::string_view> auto fn) {
+ wpi::ForEachStructSchema<frc::Quaternion>(fn);
+ }
+};
+
+static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Rotation3d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Rotation3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 3c21ec1..593dce0 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -5,15 +5,17 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Translation2d.h"
+#include "frc/geometry/Translation2d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose2d;
/**
- * Represents a transformation for a Pose2d.
+ * Represents a transformation for a Pose2d in the pose's frame.
*/
class WPILIB_DLLEXPORT Transform2d {
public:
@@ -34,6 +36,17 @@
constexpr Transform2d(Translation2d translation, Rotation2d rotation);
/**
+ * Constructs a transform with x and y translations instead of a separate
+ * Translation2d.
+ *
+ * @param x The x component of the translational component of the transform.
+ * @param y The y component of the translational component of the transform.
+ * @param rotation The rotational component of the transform.
+ */
+ constexpr Transform2d(units::meter_t x, units::meter_t y,
+ Rotation2d rotation);
+
+ /**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform2d() = default;
@@ -94,7 +107,8 @@
}
/**
- * Composes two transformations.
+ * Composes two transformations. The second transform is applied relative to
+ * the orientation of the first.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
@@ -112,4 +126,40 @@
};
} // namespace frc
-#include "Transform2d.inc"
+template <>
+struct wpi::Struct<frc::Transform2d> {
+ static constexpr std::string_view kTypeString = "struct:Transform2d";
+ static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+ wpi::Struct<frc::Rotation2d>::kSize;
+ static constexpr std::string_view kSchema =
+ "Translation2d translation;Rotation2d rotation";
+ static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data) {
+ return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+ wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+ }
+ static void Pack(std::span<uint8_t, kSize> data,
+ const frc::Transform2d& value) {
+ wpi::PackStruct<0>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+ }
+ static void ForEachNested(
+ std::invocable<std::string_view, std::string_view> auto fn) {
+ wpi::ForEachStructSchema<frc::Translation2d>(fn);
+ wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+ }
+
+ private:
+ static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Transform2d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Transform2d& value);
+};
+
+#include "frc/geometry/Transform2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
index f851a05..862b8d5 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
@@ -16,6 +16,10 @@
Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
+ Rotation2d rotation)
+ : m_translation(x, y), m_rotation(std::move(rotation)) {}
+
constexpr Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
index 5f50ec2..d5af97d 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
@@ -5,15 +5,17 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Translation3d.h"
+#include "frc/geometry/Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
/**
- * Represents a transformation for a Pose3d.
+ * Represents a transformation for a Pose3d in the pose's frame.
*/
class WPILIB_DLLEXPORT Transform3d {
public:
@@ -34,6 +36,18 @@
Transform3d(Translation3d translation, Rotation3d rotation);
/**
+ * Constructs a transform with x, y, and z translations instead of a separate
+ * Translation3d.
+ *
+ * @param x The x component of the translational component of the transform.
+ * @param y The y component of the translational component of the transform.
+ * @param z The z component of the translational component of the transform.
+ * @param rotation The rotational component of the transform.
+ */
+ Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+ Rotation3d rotation);
+
+ /**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform3d() = default;
@@ -99,7 +113,8 @@
Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
/**
- * Composes two transformations.
+ * Composes two transformations. The second transform is applied relative to
+ * the orientation of the first.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
@@ -116,3 +131,39 @@
Rotation3d m_rotation;
};
} // namespace frc
+
+template <>
+struct wpi::Struct<frc::Transform3d> {
+ static constexpr std::string_view kTypeString = "struct:Transform3d";
+ static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+ wpi::Struct<frc::Rotation3d>::kSize;
+ static constexpr std::string_view kSchema =
+ "Translation3d translation;Rotation3d rotation";
+ static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data) {
+ return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+ wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+ }
+ static void Pack(std::span<uint8_t, kSize> data,
+ const frc::Transform3d& value) {
+ wpi::PackStruct<0>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+ }
+ static void ForEachNested(
+ std::invocable<std::string_view, std::string_view> auto fn) {
+ wpi::ForEachStructSchema<frc::Translation3d>(fn);
+ wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+ }
+
+ private:
+ static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Transform3d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Transform3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index e168510..1568586 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -4,15 +4,17 @@
#pragma once
+#include <initializer_list>
+#include <span>
+
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Rotation2d.h"
+#include "frc/geometry/Rotation2d.h"
#include "units/length.h"
-namespace wpi {
-class json;
-} // namespace wpi
-
namespace frc {
/**
@@ -170,6 +172,21 @@
*/
bool operator==(const Translation2d& other) const;
+ /**
+ * Returns the nearest Translation2d from a collection of translations
+ * @param translations The collection of translations.
+ * @return The nearest Translation2d from the collection.
+ */
+ Translation2d Nearest(std::span<const Translation2d> translations) const;
+
+ /**
+ * Returns the nearest Translation2d from a collection of translations
+ * @param translations The collection of translations.
+ * @return The nearest Translation2d from the collection.
+ */
+ Translation2d Nearest(
+ std::initializer_list<Translation2d> translations) const;
+
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
@@ -183,4 +200,28 @@
} // namespace frc
-#include "Translation2d.inc"
+template <>
+struct wpi::Struct<frc::Translation2d> {
+ static constexpr std::string_view kTypeString = "struct:Translation2d";
+ static constexpr size_t kSize = 16;
+ static constexpr std::string_view kSchema = "double x;double y";
+ static frc::Translation2d Unpack(std::span<const uint8_t, 16> data) {
+ return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+ units::meter_t{wpi::UnpackStruct<double, 8>(data)}};
+ }
+ static void Pack(std::span<uint8_t, 16> data,
+ const frc::Translation2d& value) {
+ wpi::PackStruct<0>(data, value.X().value());
+ wpi::PackStruct<8>(data, value.Y().value());
+ }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Translation2d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Translation2d& value);
+};
+
+#include "frc/geometry/Translation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index ab641fa..47ba1f6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -5,15 +5,14 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
-#include "Rotation3d.h"
-#include "Translation2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation2d.h"
#include "units/length.h"
-namespace wpi {
-class json;
-} // namespace wpi
-
namespace frc {
/**
@@ -186,4 +185,30 @@
} // namespace frc
-#include "Translation3d.inc"
+template <>
+struct wpi::Struct<frc::Translation3d> {
+ static constexpr std::string_view kTypeString = "struct:Translation3d";
+ static constexpr size_t kSize = 24;
+ static constexpr std::string_view kSchema = "double x;double y;double z";
+ static frc::Translation3d Unpack(std::span<const uint8_t, 24> data) {
+ return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+ units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+ units::meter_t{wpi::UnpackStruct<double, 16>(data)}};
+ }
+ static void Pack(std::span<uint8_t, 24> data,
+ const frc::Translation3d& value) {
+ wpi::PackStruct<0>(data, value.X().value());
+ wpi::PackStruct<8>(data, value.Y().value());
+ wpi::PackStruct<16>(data, value.Z().value());
+ }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Translation3d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg,
+ const frc::Translation3d& value);
+};
+
+#include "frc/geometry/Translation3d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 620b688..257f1b6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -5,6 +5,8 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
#include "units/angle.h"
#include "units/length.h"
@@ -14,7 +16,7 @@
/**
* A change in distance along a 2D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose2ds from a Twist2d and
- * vise versa.
+ * vice versa.
*
* A Twist can be used to represent a difference between two poses.
*/
@@ -57,3 +59,28 @@
}
};
} // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist2d> {
+ static constexpr std::string_view kTypeString = "struct:Twist2d";
+ static constexpr size_t kSize = 24;
+ static constexpr std::string_view kSchema =
+ "double dx;double dy;double dtheta";
+ static frc::Twist2d Unpack(std::span<const uint8_t, 24> data) {
+ return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+ units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+ units::radian_t{wpi::UnpackStruct<double, 16>(data)}};
+ }
+ static void Pack(std::span<uint8_t, 24> data, const frc::Twist2d& value) {
+ wpi::PackStruct<0>(data, value.dx.value());
+ wpi::PackStruct<8>(data, value.dy.value());
+ wpi::PackStruct<16>(data, value.dtheta.value());
+ }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Twist2d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
index 3040ab3..4d902df 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
@@ -5,6 +5,8 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
#include "frc/geometry/Rotation3d.h"
#include "units/angle.h"
@@ -15,7 +17,7 @@
/**
* A change in distance along a 3D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose3ds from a Twist3d and
- * vise versa.
+ * vice versa.
*
* A Twist can be used to represent a difference between two poses.
*/
@@ -77,3 +79,35 @@
}
};
} // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist3d> {
+ static constexpr std::string_view kTypeString = "struct:Twist3d";
+ static constexpr size_t kSize = 48;
+ static constexpr std::string_view kSchema =
+ "double dx;double dy;double dz;double rx;double ry;double rz";
+ static frc::Twist3d Unpack(std::span<const uint8_t, 48> data) {
+ return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+ units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+ units::meter_t{wpi::UnpackStruct<double, 16>(data)},
+ units::radian_t{wpi::UnpackStruct<double, 24>(data)},
+ units::radian_t{wpi::UnpackStruct<double, 32>(data)},
+ units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
+ }
+ static void Pack(std::span<uint8_t, 48> data, const frc::Twist3d& value) {
+ wpi::PackStruct<0>(data, value.dx.value());
+ wpi::PackStruct<8>(data, value.dy.value());
+ wpi::PackStruct<16>(data, value.dz.value());
+ wpi::PackStruct<24>(data, value.rx.value());
+ wpi::PackStruct<32>(data, value.ry.value());
+ wpi::PackStruct<40>(data, value.rz.value());
+ }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
+ static constexpr std::string_view kTypeString = "proto:Twist3d";
+ static google::protobuf::Message* New(google::protobuf::Arena* arena);
+ static frc::Twist3d Unpack(const google::protobuf::Message& msg);
+ static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index 771fe84..9dd8e62 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -68,11 +68,26 @@
if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
m_pastSnapshots.emplace_back(time, sample);
} else {
- m_pastSnapshots.insert(
- std::upper_bound(
- m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
- [](auto t, const auto& pair) { return t < pair.first; }),
- std::pair(time, sample));
+ auto first_after = std::upper_bound(
+ m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+ [](auto t, const auto& pair) { return t < pair.first; });
+
+ // Don't access this before ensuring first_after isn't first.
+ auto last_not_greater_than = first_after - 1;
+
+ if (first_after == m_pastSnapshots.begin() ||
+ last_not_greater_than == m_pastSnapshots.begin() ||
+ last_not_greater_than->first < time) {
+ // Two cases handled together:
+ // 1. All entries come after the sample
+ // 2. Some entries come before the sample, but none are recorded with
+ // the same time
+ m_pastSnapshots.insert(first_after, std::pair(time, sample));
+ } else {
+ // Final case:
+ // 3. An entry exists with the same recorded time.
+ last_not_greater_than->second = sample;
+ }
}
while (time - m_pastSnapshots[0].first > m_historySize) {
m_pastSnapshots.erase(m_pastSnapshots.begin());
@@ -112,6 +127,10 @@
m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
[](const auto& pair, auto t) { return t > pair.first; });
+ if (upper_bound == m_pastSnapshots.begin()) {
+ return upper_bound->second;
+ }
+
auto lower_bound = upper_bound - 1;
double t = ((time - lower_bound->first) /
diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 37fe768..93c9044 100644
--- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
+#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
@@ -15,8 +16,7 @@
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
- * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
- * frame of reference.
+ * reference, a ChassisSpeeds struct represents a robot's velocity.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
@@ -24,12 +24,12 @@
*/
struct WPILIB_DLLEXPORT ChassisSpeeds {
/**
- * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+ * Velocity along the x-axis. (Fwd is +)
*/
units::meters_per_second_t vx = 0_mps;
/**
- * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
+ * Velocity along the y-axis. (Left is +)
*/
units::meters_per_second_t vy = 0_mps;
@@ -39,6 +39,57 @@
units::radians_per_second_t omega = 0_rad_per_s;
/**
+ * Disretizes a continuous-time chassis speed.
+ *
+ * This function converts a continuous-time chassis speed into a discrete-time
+ * one such that when the discrete-time chassis speed is applied for one
+ * timestep, the robot moves as if the velocity components are independent
+ * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+ * y-axis, and omega * dt around the z-axis).
+ *
+ * This is useful for compensating for translational skew when translating and
+ * rotating a swerve drivetrain.
+ *
+ * @param vx Forward velocity.
+ * @param vy Sideways velocity.
+ * @param omega Angular velocity.
+ * @param dt The duration of the timestep the speeds should be applied for.
+ *
+ * @return Discretized ChassisSpeeds.
+ */
+ static ChassisSpeeds Discretize(units::meters_per_second_t vx,
+ units::meters_per_second_t vy,
+ units::radians_per_second_t omega,
+ units::second_t dt) {
+ Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
+ auto twist = Pose2d{}.Log(desiredDeltaPose);
+ return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
+ }
+
+ /**
+ * Disretizes a continuous-time chassis speed.
+ *
+ * This function converts a continuous-time chassis speed into a discrete-time
+ * one such that when the discrete-time chassis speed is applied for one
+ * timestep, the robot moves as if the velocity components are independent
+ * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+ * y-axis, and omega * dt around the z-axis).
+ *
+ * This is useful for compensating for translational skew when translating and
+ * rotating a swerve drivetrain.
+ *
+ * @param continuousSpeeds The continuous speeds.
+ * @param dt The duration of the timestep the speeds should be applied for.
+ *
+ * @return Discretized ChassisSpeeds.
+ */
+ static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds,
+ units::second_t dt) {
+ return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
+ continuousSpeeds.omega, dt);
+ }
+
+ /**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
@@ -57,8 +108,12 @@
static ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
- return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
- -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
+ // CW rotation into chassis frame
+ auto rotated =
+ Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+ .RotateBy(-robotAngle);
+ return {units::meters_per_second_t{rotated.X().value()},
+ units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
@@ -81,5 +136,118 @@
fieldRelativeSpeeds.vy,
fieldRelativeSpeeds.omega, robotAngle);
}
+
+ /**
+ * Converts a user provided robot-relative set of speeds into a field-relative
+ * ChassisSpeeds object.
+ *
+ * @param vx The component of speed in the x direction relative to the robot.
+ * Positive x is towards the robot's front.
+ * @param vy The component of speed in the y direction relative to the robot.
+ * Positive y is towards the robot's left.
+ * @param omega The angular rate of the robot.
+ * @param robotAngle The angle of the robot as measured by a gyroscope. The
+ * robot's angle is considered to be zero when it is facing directly away from
+ * your alliance station wall. Remember that this should be CCW positive.
+ *
+ * @return ChassisSpeeds object representing the speeds in the field's frame
+ * of reference.
+ */
+ static ChassisSpeeds FromRobotRelativeSpeeds(
+ units::meters_per_second_t vx, units::meters_per_second_t vy,
+ units::radians_per_second_t omega, const Rotation2d& robotAngle) {
+ // CCW rotation out of chassis frame
+ auto rotated =
+ Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+ .RotateBy(robotAngle);
+ return {units::meters_per_second_t{rotated.X().value()},
+ units::meters_per_second_t{rotated.Y().value()}, omega};
+ }
+
+ /**
+ * Converts a user provided robot-relative ChassisSpeeds object into a
+ * field-relative ChassisSpeeds object.
+ *
+ * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
+ * in the robot frame of reference. Positive x is the towards robot's
+ * front. Positive y is towards the robot's left.
+ * @param robotAngle The angle of the robot as measured by a gyroscope. The
+ * robot's angle is considered to be zero when it is facing directly away
+ * from your alliance station wall. Remember that this should be CCW
+ * positive.
+ * @return ChassisSpeeds object representing the speeds in the field's frame
+ * of reference.
+ */
+ static ChassisSpeeds FromRobotRelativeSpeeds(
+ const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
+ return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
+ robotRelativeSpeeds.vy,
+ robotRelativeSpeeds.omega, robotAngle);
+ }
+
+ /**
+ * Adds two ChassisSpeeds and returns the sum.
+ *
+ * <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
+ * = ChassisSpeeds{3.0, 2.0, 2.0}
+ *
+ * @param other The ChassisSpeeds to add.
+ *
+ * @return The sum of the ChassisSpeeds.
+ */
+ constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
+ return {vx + other.vx, vy + other.vy, omega + other.omega};
+ }
+
+ /**
+ * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
+ * returns the difference.
+ *
+ * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
+ * = ChassisSpeeds{4.0, 2.0, 1.0}
+ *
+ * @param other The ChassisSpeeds to subtract.
+ *
+ * @return The difference between the two ChassisSpeeds.
+ */
+ constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
+ return *this + -other;
+ }
+
+ /**
+ * Returns the inverse of the current ChassisSpeeds.
+ * This is equivalent to negating all components of the ChassisSpeeds.
+ *
+ * @return The inverse of the current ChassisSpeeds.
+ */
+ constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
+
+ /**
+ * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+ *
+ * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
+ * = ChassisSpeeds{4.0, 5.0, 1.0}
+ *
+ * @param scalar The scalar to multiply by.
+ *
+ * @return The scaled ChassisSpeeds.
+ */
+ constexpr ChassisSpeeds operator*(double scalar) const {
+ return {scalar * vx, scalar * vy, scalar * omega};
+ }
+
+ /**
+ * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+ *
+ * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
+ * = ChassisSpeeds{1.0, 1.25, 0.5}
+ *
+ * @param scalar The scalar to divide by.
+ *
+ * @return The scaled ChassisSpeeds.
+ */
+ constexpr ChassisSpeeds operator/(double scalar) const {
+ return operator*(1.0 / scalar);
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 930c7a6..95f53fa 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -8,7 +8,9 @@
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
#include "units/angle.h"
#include "units/length.h"
#include "wpimath/MathShared.h"
@@ -22,7 +24,9 @@
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
*/
-class WPILIB_DLLEXPORT DifferentialDriveKinematics {
+class WPILIB_DLLEXPORT DifferentialDriveKinematics
+ : public Kinematics<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions> {
public:
/**
* Constructs a differential drive kinematics object.
@@ -46,7 +50,7 @@
* @return The chassis speed.
*/
constexpr ChassisSpeeds ToChassisSpeeds(
- const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
}
@@ -60,7 +64,7 @@
* @return The left and right velocities.
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
- const ChassisSpeeds& chassisSpeeds) const {
+ const ChassisSpeeds& chassisSpeeds) const override {
return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
@@ -79,6 +83,11 @@
(rightDistance - leftDistance) / trackWidth * 1_rad};
}
+ Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start,
+ const DifferentialDriveWheelPositions& end) const override {
+ return ToTwist2d(end.left - start.left, end.right - start.right);
+ }
+
units::meter_t trackWidth;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index cc198ac..279c1dc 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -7,6 +7,10 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
#include "units/length.h"
namespace frc {
@@ -22,7 +26,9 @@
* It is important that you reset your encoders to zero before using this class.
* Any subsequent pose resets also require the encoders to be reset to zero.
*/
-class WPILIB_DLLEXPORT DifferentialDriveOdometry {
+class WPILIB_DLLEXPORT DifferentialDriveOdometry
+ : public Odometry<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions> {
public:
/**
* Constructs a DifferentialDriveOdometry object.
@@ -56,21 +62,14 @@
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose) {
- m_pose = pose;
- m_previousAngle = pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
- m_prevLeftDistance = leftDistance;
- m_prevRightDistance = rightDistance;
+ Odometry<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
+ {leftDistance,
+ rightDistance},
+ pose);
}
/**
- * Returns the position of the robot on the field.
- * @return The pose of the robot.
- */
- const Pose2d& GetPose() const { return m_pose; }
-
- /**
* Updates the robot position on the field using distance measurements from
* encoders. This method is more numerically accurate than using velocities to
* integrate the pose and is also advantageous for teams that are using lower
@@ -82,15 +81,14 @@
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
- units::meter_t rightDistance);
+ units::meter_t rightDistance) {
+ return Odometry<DifferentialDriveWheelSpeeds,
+ DifferentialDriveWheelPositions>::Update(gyroAngle,
+ {leftDistance,
+ rightDistance});
+ }
private:
- Pose2d m_pose;
-
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
-
- units::meter_t m_prevLeftDistance = 0_m;
- units::meter_t m_prevRightDistance = 0_m;
+ DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
new file mode 100644
index 0000000..4dddbea
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
@@ -0,0 +1,51 @@
+// 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/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
+ /**
+ * Distance driven by the left side.
+ */
+ units::meter_t left = 0_m;
+
+ /**
+ * Distance driven by the right side.
+ */
+ units::meter_t right = 0_m;
+
+ /**
+ * Checks equality between this DifferentialDriveWheelPositions and another
+ * object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const DifferentialDriveWheelPositions& other) const = default;
+
+ /**
+ * Checks inequality between this DifferentialDriveWheelPositions and another
+ * object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
+
+ DifferentialDriveWheelPositions Interpolate(
+ const DifferentialDriveWheelPositions& endValue, double t) const {
+ return {wpi::Lerp(left, endValue.left, t),
+ wpi::Lerp(right, endValue.right, t)};
+ }
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
index fce2b96..9d9e705 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -36,5 +36,79 @@
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+ /**
+ * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+ *
+ * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} +
+ * DifferentialDriveWheelSpeeds{2.0, 1.5} =
+ * DifferentialDriveWheelSpeeds{3.0, 2.0}
+ *
+ * @param other The DifferentialDriveWheelSpeeds to add.
+ *
+ * @return The sum of the DifferentialDriveWheelSpeeds.
+ */
+ constexpr DifferentialDriveWheelSpeeds operator+(
+ const DifferentialDriveWheelSpeeds& other) const {
+ return {left + other.left, right + other.right};
+ }
+
+ /**
+ * Subtracts the other DifferentialDriveWheelSpeeds from the current
+ * DifferentialDriveWheelSpeeds and returns the difference.
+ *
+ * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} -
+ * DifferentialDriveWheelSpeeds{1.0, 2.0} =
+ * DifferentialDriveWheelSpeeds{4.0, 2.0}
+ *
+ * @param other The DifferentialDriveWheelSpeeds to subtract.
+ *
+ * @return The difference between the two DifferentialDriveWheelSpeeds.
+ */
+ constexpr DifferentialDriveWheelSpeeds operator-(
+ const DifferentialDriveWheelSpeeds& other) const {
+ return *this + -other;
+ }
+
+ /**
+ * Returns the inverse of the current DifferentialDriveWheelSpeeds.
+ * This is equivalent to negating all components of the
+ * DifferentialDriveWheelSpeeds.
+ *
+ * @return The inverse of the current DifferentialDriveWheelSpeeds.
+ */
+ constexpr DifferentialDriveWheelSpeeds operator-() const {
+ return {-left, -right};
+ }
+
+ /**
+ * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+ * DifferentialDriveWheelSpeeds.
+ *
+ * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2
+ * = DifferentialDriveWheelSpeeds{4.0, 5.0}
+ *
+ * @param scalar The scalar to multiply by.
+ *
+ * @return The scaled DifferentialDriveWheelSpeeds.
+ */
+ constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
+ return {scalar * left, scalar * right};
+ }
+
+ /**
+ * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+ * DifferentialDriveWheelSpeeds.
+ *
+ * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2
+ * = DifferentialDriveWheelSpeeds{1.0, 1.25}
+ *
+ * @param scalar The scalar to divide by.
+ *
+ * @return The scaled DifferentialDriveWheelSpeeds.
+ */
+ constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
+ return operator*(1.0 / scalar);
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h
new file mode 100644
index 0000000..e27cfc6
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h
@@ -0,0 +1,62 @@
+// 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/SymbolExports.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds. Robot code should not use this directly-
+ * Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDriveKinematics).
+ *
+ * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
+ * forward kinematics converts wheel speeds into chassis speed.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT Kinematics {
+ public:
+ /**
+ * Performs forward kinematics to return the resulting chassis speed from the
+ * wheel speeds. This method is often used for odometry -- determining the
+ * robot's position on the field using data from the real-world speed of each
+ * wheel on the robot.
+ *
+ * @param wheelSpeeds The speeds of the wheels.
+ * @return The chassis speed.
+ */
+ virtual ChassisSpeeds ToChassisSpeeds(
+ const WheelSpeeds& wheelSpeeds) const = 0;
+
+ /**
+ * Performs inverse kinematics to return the wheel speeds from a desired
+ * chassis velocity. This method is often used to convert joystick values into
+ * wheel speeds.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @return The wheel speeds.
+ */
+ virtual WheelSpeeds ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const = 0;
+
+ /**
+ * Performs forward kinematics to return the resulting Twist2d from the given
+ * change in wheel positions. This method is often used for odometry --
+ * determining the robot's position on the field using changes in the distance
+ * driven by each wheel on the robot.
+ *
+ * @param start The starting distances driven by the wheels.
+ * @param end The ending distances driven by the wheels.
+ *
+ * @return The resulting Twist2d in the robot's movement.
+ */
+ virtual Twist2d ToTwist2d(const WheelPositions& start,
+ const WheelPositions& end) const = 0;
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 2880cef..fe6eb51 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -4,13 +4,14 @@
#pragma once
+#include <Eigen/QR>
#include <wpi/SymbolExports.h>
-#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "wpimath/MathShared.h"
@@ -39,7 +40,8 @@
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
-class WPILIB_DLLEXPORT MecanumDriveKinematics {
+class WPILIB_DLLEXPORT MecanumDriveKinematics
+ : public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
public:
/**
* Constructs a mecanum drive kinematics object.
@@ -101,7 +103,12 @@
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
- const Translation2d& centerOfRotation = Translation2d{}) const;
+ const Translation2d& centerOfRotation) const;
+
+ MecanumDriveWheelSpeeds ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const override {
+ return ToWheelSpeeds(chassisSpeeds, {});
+ }
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -114,7 +121,10 @@
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
- const MecanumDriveWheelSpeeds& wheelSpeeds) const;
+ const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
+
+ Twist2d ToTwist2d(const MecanumDriveWheelPositions& start,
+ const MecanumDriveWheelPositions& end) const override;
/**
* Performs forward kinematics to return the resulting Twist2d from the
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index 5e949ca..0a6f537 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -9,7 +9,9 @@
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
#include "units/time.h"
namespace frc {
@@ -23,7 +25,8 @@
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
-class WPILIB_DLLEXPORT MecanumDriveOdometry {
+class WPILIB_DLLEXPORT MecanumDriveOdometry
+ : public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
public:
/**
* Constructs a MecanumDriveOdometry object.
@@ -38,52 +41,8 @@
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose = Pose2d{});
- /**
- * Resets the robot's position on the field.
- *
- * The gyroscope angle does not need to be reset here on the user's robot
- * code. The library automatically takes care of offsetting the gyro angle.
- *
- * @param gyroAngle The angle reported by the gyroscope.
- * @param wheelPositions The current distances measured by each wheel.
- * @param pose The position on the field that your robot is at.
- */
- void ResetPosition(const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions,
- const Pose2d& pose) {
- m_pose = pose;
- m_previousAngle = pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
- m_previousWheelPositions = wheelPositions;
- }
-
- /**
- * Returns the position of the robot on the field.
- * @return The pose of the robot.
- */
- const Pose2d& GetPose() const { return m_pose; }
-
- /**
- * Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method takes in an angle parameter
- * which is used instead of the angular rate that is calculated from forward
- * kinematics, in addition to the current distance measurement at each wheel.
- *
- * @param gyroAngle The angle reported by the gyroscope.
- * @param wheelPositions The current distances measured by each wheel.
- *
- * @return The new pose of the robot.
- */
- const Pose2d& Update(const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions);
-
private:
- MecanumDriveKinematics m_kinematics;
- Pose2d m_pose;
-
- MecanumDriveWheelPositions m_previousWheelPositions;
- Rotation2d m_previousAngle;
- Rotation2d m_gyroOffset;
+ MecanumDriveKinematics m_kinematicsImpl;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
index b69aceb..76c8b9d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
@@ -4,13 +4,14 @@
#pragma once
+#include <wpi/MathExtras.h>
#include <wpi/SymbolExports.h>
#include "units/length.h"
namespace frc {
/**
- * Represents the wheel speeds for a mecanum drive drivetrain.
+ * Represents the wheel positions for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
/**
@@ -49,5 +50,13 @@
* @return Whether the two objects are not equal.
*/
bool operator!=(const MecanumDriveWheelPositions& other) const = default;
+
+ MecanumDriveWheelPositions Interpolate(
+ const MecanumDriveWheelPositions& endValue, double t) const {
+ return {wpi::Lerp(frontLeft, endValue.frontLeft, t),
+ wpi::Lerp(frontRight, endValue.frontRight, t),
+ wpi::Lerp(rearLeft, endValue.rearLeft, t),
+ wpi::Lerp(rearRight, endValue.rearRight, t)};
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
index e698c5f..80e8460 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -46,5 +46,77 @@
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+ /**
+ * Adds two MecanumDriveWheelSpeeds and returns the sum.
+ *
+ * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} +
+ * MecanumDriveWheelSpeeds{2.0, 1.5, 0.5, 1.0} =
+ * MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
+ *
+ * @param other The MecanumDriveWheelSpeeds to add.
+ * @return The sum of the MecanumDriveWheelSpeeds.
+ */
+ constexpr MecanumDriveWheelSpeeds operator+(
+ const MecanumDriveWheelSpeeds& other) const {
+ return {frontLeft + other.frontLeft, frontRight + other.frontRight,
+ rearLeft + other.rearLeft, rearRight + other.rearRight};
+ }
+
+ /**
+ * Subtracts the other MecanumDriveWheelSpeeds from the current
+ * MecanumDriveWheelSpeeds and returns the difference.
+ *
+ * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} -
+ * MecanumDriveWheelSpeeds{1.0, 2.0, 3.0, 0.5} =
+ * MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
+ *
+ * @param other The MecanumDriveWheelSpeeds to subtract.
+ * @return The difference between the two MecanumDriveWheelSpeeds.
+ */
+ constexpr MecanumDriveWheelSpeeds operator-(
+ const MecanumDriveWheelSpeeds& other) const {
+ return *this + -other;
+ }
+
+ /**
+ * Returns the inverse of the current MecanumDriveWheelSpeeds.
+ * This is equivalent to negating all components of the
+ * MecanumDriveWheelSpeeds.
+ *
+ * @return The inverse of the current MecanumDriveWheelSpeeds.
+ */
+ constexpr MecanumDriveWheelSpeeds operator-() const {
+ return {-frontLeft, -frontRight, -rearLeft, -rearRight};
+ }
+
+ /**
+ * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new
+ * MecanumDriveWheelSpeeds.
+ *
+ * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 =
+ * MecanumDriveWheelSpeeds{4.0, 5.0, 6.0, 7.0}
+ *
+ * @param scalar The scalar to multiply by.
+ * @return The scaled MecanumDriveWheelSpeeds.
+ */
+ constexpr MecanumDriveWheelSpeeds operator*(double scalar) const {
+ return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
+ scalar * rearRight};
+ }
+
+ /**
+ * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new
+ * MecanumDriveWheelSpeeds.
+ *
+ * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 =
+ * MecanumDriveWheelSpeeds{1.0, 1.25, 0.75, 0.5}
+ *
+ * @param scalar The scalar to divide by.
+ * @return The scaled MecanumDriveWheelSpeeds.
+ */
+ constexpr MecanumDriveWheelSpeeds operator/(double scalar) const {
+ return operator*(1.0 / scalar);
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h
new file mode 100644
index 0000000..55c074c
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h
@@ -0,0 +1,88 @@
+// 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/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/WheelPositions.h"
+
+namespace frc {
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the
+ * particular type for your drivetrain (e.g., DifferentialDriveOdometry).
+ * Odometry allows you to track the robot's position on the field over a course
+ * of a match using readings from your wheel encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT Odometry {
+ public:
+ /**
+ * Constructs an Odometry object.
+ *
+ * @param kinematics The kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The current distances measured by each wheel.
+ * @param initialPose The starting position of the robot on the field.
+ */
+ explicit Odometry(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+ const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions,
+ const Pose2d& initialPose = Pose2d{});
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * The gyroscope angle does not need to be reset here on the user's robot
+ * code. The library automatically takes care of offsetting the gyro angle.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The current distances measured by each wheel.
+ * @param pose The position on the field that your robot is at.
+ */
+ void ResetPosition(const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions, const Pose2d& pose) {
+ m_pose = pose;
+ m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ m_previousWheelPositions = wheelPositions;
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ * @return The pose of the robot.
+ */
+ const Pose2d& GetPose() const { return m_pose; }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method takes in an angle parameter
+ * which is used instead of the angular rate that is calculated from forward
+ * kinematics, in addition to the current distance measurement at each wheel.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The current distances measured by each wheel.
+ *
+ * @return The new pose of the robot.
+ */
+ const Pose2d& Update(const Rotation2d& gyroAngle,
+ const WheelPositions& wheelPositions);
+
+ private:
+ const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+ Pose2d m_pose;
+
+ WheelPositions m_previousWheelPositions;
+ Rotation2d m_previousAngle;
+ Rotation2d m_gyroOffset;
+};
+} // namespace frc
+
+#include "Odometry.inc"
diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.inc b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc
new file mode 100644
index 0000000..688c1bd
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc
@@ -0,0 +1,38 @@
+// 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 "frc/kinematics/Odometry.h"
+
+namespace frc {
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Odometry<WheelSpeeds, WheelPositions>::Odometry(
+ const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+ const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+ const Pose2d& initialPose)
+ : m_kinematics(kinematics),
+ m_pose(initialPose),
+ m_previousWheelPositions(wheelPositions) {
+ m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
+ const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
+ auto angle = gyroAngle + m_gyroOffset;
+
+ auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
+ 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;
+}
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 97ee233..f6d4e5d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -4,23 +4,30 @@
#pragma once
+#include <concepts>
#include <cstddef>
+#include <Eigen/QR>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/SwerveDriveWheelPositions.h"
#include "frc/kinematics/SwerveModulePosition.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "units/velocity.h"
#include "wpimath/MathShared.h"
namespace frc {
+
+template <size_t NumModules>
+using SwerveDriveWheelSpeeds = wpi::array<SwerveModuleState, NumModules>;
+
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
@@ -44,27 +51,25 @@
* the robot on the field using encoders and a gyro.
*/
template <size_t NumModules>
-class SwerveDriveKinematics {
+class SwerveDriveKinematics
+ : public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a swerve drive kinematics object. This takes in a variable
- * number of wheel locations as Translation2ds. The order in which you pass in
- * the wheel locations is the same order that you will receive the module
+ * number of module locations as Translation2ds. The order in which you pass
+ * in the module locations is the same order that you will receive the module
* states when performing inverse kinematics. It is also expected that you
* pass in the module states in the same order when calling the forward
* kinematics methods.
*
- * @param wheel The location of the first wheel relative to the physical
- * center of the robot.
- * @param wheels The locations of the other wheels relative to the physical
- * center of the robot.
+ * @param moduleTranslations The locations of the modules relative to the
+ * physical center of the robot.
*/
- template <typename... Wheels>
- explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
- : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
- static_assert(sizeof...(wheels) >= 1,
- "A swerve drive requires at least two modules");
-
+ template <std::convertible_to<Translation2d>... ModuleTranslations>
+ requires(sizeof...(ModuleTranslations) == NumModules)
+ explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
+ : m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -80,8 +85,8 @@
}
explicit SwerveDriveKinematics(
- const wpi::array<Translation2d, NumModules>& wheels)
- : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
+ const wpi::array<Translation2d, NumModules>& modules)
+ : m_modules{modules}, m_moduleHeadings(wpi::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -99,6 +104,25 @@
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
/**
+ * Reset the internal swerve module headings.
+ * @param moduleHeadings The swerve module headings. The order of the module
+ * headings should be same as passed into the constructor of this class.
+ */
+ template <std::convertible_to<Rotation2d>... ModuleHeadings>
+ requires(sizeof...(ModuleHeadings) == NumModules)
+ void ResetHeadings(ModuleHeadings&&... moduleHeadings) {
+ return this->ResetHeadings(
+ wpi::array<Rotation2d, NumModules>{moduleHeadings...});
+ }
+
+ /**
+ * Reset the internal swerve module headings.
+ * @param moduleHeadings The swerve module headings. The order of the module
+ * headings should be same as passed into the constructor of this class.
+ */
+ void ResetHeadings(wpi::array<Rotation2d, NumModules> moduleHeadings);
+
+ /**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
@@ -133,20 +157,29 @@
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d{}) const;
+ SwerveDriveWheelSpeeds<NumModules> ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const override {
+ return ToSwerveModuleStates(chassisSpeeds);
+ }
+
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
*
- * @param wheelStates The state of the modules (as a SwerveModuleState type)
+ * @param moduleStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
*
* @return The resulting chassis speed.
*/
- template <typename... ModuleStates>
- ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
+ template <std::convertible_to<SwerveModuleState>... ModuleStates>
+ requires(sizeof...(ModuleStates) == NumModules)
+ ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
+ return this->ToChassisSpeeds(
+ wpi::array<SwerveModuleState, NumModules>{moduleStates...});
+ }
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -161,8 +194,8 @@
*
* @return The resulting chassis speed.
*/
- ChassisSpeeds ToChassisSpeeds(
- wpi::array<SwerveModuleState, NumModules> moduleStates) const;
+ ChassisSpeeds ToChassisSpeeds(const wpi::array<SwerveModuleState, NumModules>&
+ moduleStates) const override;
/**
* Performs forward kinematics to return the resulting Twist2d from the
@@ -170,15 +203,19 @@
* determining the robot's position on the field using data from the
* real-world position delta and angle of each module on the robot.
*
- * @param wheelDeltas The latest change in position of the modules (as a
+ * @param moduleDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* @return The resulting Twist2d.
*/
- template <typename... ModuleDeltas>
- Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
+ template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
+ requires(sizeof...(ModuleDeltas) == NumModules)
+ Twist2d ToTwist2d(ModuleDeltas&&... moduleDeltas) const {
+ return this->ToTwist2d(
+ wpi::array<SwerveModulePosition, NumModules>{moduleDeltas...});
+ }
/**
* Performs forward kinematics to return the resulting Twist2d from the
@@ -186,7 +223,7 @@
* determining the robot's position on the field using data from the
* real-world position delta and angle of each module on the robot.
*
- * @param wheelDeltas The latest change in position of the modules (as a
+ * @param moduleDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
@@ -194,7 +231,20 @@
* @return The resulting Twist2d.
*/
Twist2d ToTwist2d(
- wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+ wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
+
+ Twist2d ToTwist2d(
+ const SwerveDriveWheelPositions<NumModules>& start,
+ const SwerveDriveWheelPositions<NumModules>& end) const override {
+ auto result =
+ wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+ for (size_t i = 0; i < NumModules; i++) {
+ auto startModule = start.positions[i];
+ auto endModule = end.positions[i];
+ result[i] = {endModule.distance - startModule.distance, endModule.angle};
+ }
+ return ToTwist2d(result);
+ }
/**
* Renormalizes the wheel speeds if any individual speed is above the
@@ -229,7 +279,7 @@
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
- * @param currentChassisSpeed Current speed of the robot
+ * @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeed The absolute max speed a module can reach
* @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
* can reach while translating
@@ -238,7 +288,7 @@
*/
static void DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
- ChassisSpeeds currentChassisSpeed,
+ ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed,
units::radians_per_second_t attainableMaxRobotRotationSpeed);
@@ -247,7 +297,7 @@
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
- mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
+ mutable wpi::array<Rotation2d, NumModules> m_moduleHeadings;
mutable Translation2d m_previousCoR;
};
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index c7f26e0..db85d99 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -13,22 +13,32 @@
namespace frc {
-template <class... Wheels>
-SwerveDriveKinematics(Translation2d, Wheels...)
- -> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
+template <typename ModuleTranslation, typename... ModuleTranslations>
+SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
+ -> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
+
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::ResetHeadings(
+ wpi::array<Rotation2d, NumModules> moduleHeadings) {
+ for (size_t i = 0; i < NumModules; i++) {
+ m_moduleHeadings[i] = moduleHeadings[i];
+ }
+}
template <size_t NumModules>
wpi::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
+ wpi::array<SwerveModuleState, NumModules> moduleStates(wpi::empty_array);
+
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
for (size_t i = 0; i < NumModules; i++) {
- m_moduleStates[i].speed = 0_mps;
+ moduleStates[i] = {0_mps, m_moduleHeadings[i]};
}
- return m_moduleStates;
+ return moduleStates;
}
// We have a new center of rotation. We need to compute the matrix again.
@@ -58,28 +68,16 @@
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
- m_moduleStates[i] = {speed, rotation};
+ moduleStates[i] = {speed, rotation};
+ m_moduleHeadings[i] = rotation;
}
- return m_moduleStates;
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
- ModuleStates&&... wheelStates) const {
- static_assert(sizeof...(wheelStates) == NumModules,
- "Number of modules is not consistent with number of wheel "
- "locations provided in constructor.");
-
- wpi::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
-
- return this->ToChassisSpeeds(moduleStates);
+ return moduleStates;
}
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
- wpi::array<SwerveModuleState, NumModules> moduleStates) const {
+ const wpi::array<SwerveModuleState, NumModules>& moduleStates) const {
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
@@ -97,19 +95,6 @@
}
template <size_t NumModules>
-template <typename... ModuleDeltas>
-Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
- ModuleDeltas&&... wheelDeltas) const {
- static_assert(sizeof...(wheelDeltas) == NumModules,
- "Number of modules is not consistent with number of wheel "
- "locations provided in constructor.");
-
- wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
-
- return this->ToTwist2d(moduleDeltas);
-}
-
-template <size_t NumModules>
Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
@@ -134,12 +119,13 @@
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
- auto realMaxSpeed = std::max_element(states.begin(), states.end(),
- [](const auto& a, const auto& b) {
- return units::math::abs(a.speed) <
- units::math::abs(b.speed);
- })
- ->speed;
+ auto realMaxSpeed =
+ units::math::abs(std::max_element(states.begin(), states.end(),
+ [](const auto& a, const auto& b) {
+ return units::math::abs(a.speed) <
+ units::math::abs(b.speed);
+ })
+ ->speed);
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
@@ -151,18 +137,19 @@
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
- ChassisSpeeds currentChassisSpeed,
+ ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed,
units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
- auto realMaxSpeed = std::max_element(states.begin(), states.end(),
- [](const auto& a, const auto& b) {
- return units::math::abs(a.speed) <
- units::math::abs(b.speed);
- })
- ->speed;
+ auto realMaxSpeed =
+ units::math::abs(std::max_element(states.begin(), states.end(),
+ [](const auto& a, const auto& b) {
+ return units::math::abs(a.speed) <
+ units::math::abs(b.speed);
+ })
+ ->speed);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {
@@ -170,10 +157,10 @@
}
auto translationalK =
- units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) /
+ units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
- auto rotationalK = units::math::abs(currentChassisSpeed.omega) /
+ auto rotationalK = units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed;
auto k = units::math::max(translationalK, rotationalK);
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 015c2c0..2e0e553 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -11,8 +11,11 @@
#include <wpi/SymbolExports.h>
#include <wpi/timestamp.h>
+#include "Odometry.h"
#include "SwerveDriveKinematics.h"
+#include "SwerveDriveWheelPositions.h"
#include "SwerveModulePosition.h"
+#include "SwerveModuleState.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
@@ -28,7 +31,9 @@
* when using computer-vision systems.
*/
template <size_t NumModules>
-class SwerveDriveOdometry {
+class SwerveDriveOdometry
+ : public Odometry<SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a SwerveDriveOdometry object.
@@ -56,13 +61,13 @@
void ResetPosition(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
- const Pose2d& pose);
-
- /**
- * Returns the position of the robot on the field.
- * @return The pose of the robot.
- */
- const Pose2d& GetPose() const { return m_pose; }
+ const Pose2d& pose) {
+ Odometry<
+ SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+ {modulePositions},
+ pose);
+ }
/**
* Updates the robot's position on the field using forward kinematics and
@@ -79,17 +84,15 @@
*/
const Pose2d& Update(
const Rotation2d& gyroAngle,
- const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+ return Odometry<
+ SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+ {modulePositions});
+ }
private:
- SwerveDriveKinematics<NumModules> m_kinematics;
- Pose2d m_pose;
-
- Rotation2d m_previousAngle;
- Rotation2d m_gyroOffset;
-
- wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
- wpi::empty_array};
+ SwerveDriveKinematics<NumModules> m_kinematicsImpl;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 64b46c1..48ddfec 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -13,58 +13,11 @@
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
- : m_kinematics(kinematics), m_pose(initialPose) {
- m_previousAngle = m_pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
- for (size_t i = 0; i < NumModules; i++) {
- m_previousModulePositions[i] = {modulePositions[i].distance,
- modulePositions[i].angle};
- }
-
+ : Odometry<SwerveDriveWheelSpeeds<NumModules>,
+ SwerveDriveWheelPositions<NumModules>>(
+ m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose),
+ m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
-
-template <size_t NumModules>
-void SwerveDriveOdometry<NumModules>::ResetPosition(
- const Rotation2d& gyroAngle,
- const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
- const Pose2d& pose) {
- m_pose = pose;
- m_previousAngle = pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
- for (size_t i = 0; i < NumModules; i++) {
- m_previousModulePositions[i].distance = modulePositions[i].distance;
- }
-}
-
-template <size_t NumModules>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
- const Rotation2d& gyroAngle,
- const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
- auto moduleDeltas =
- wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
- for (size_t index = 0; index < NumModules; index++) {
- auto lastPosition = m_previousModulePositions[index];
- auto currentPosition = modulePositions[index];
- moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
- currentPosition.angle};
-
- m_previousModulePositions[index].distance = modulePositions[index].distance;
- }
-
- auto angle = gyroAngle + m_gyroOffset;
-
- auto twist = m_kinematics.ToTwist2d(moduleDeltas);
- twist.dtheta = (angle - m_previousAngle).Radians();
-
- auto newPose = m_pose.Exp(twist);
-
- m_previousAngle = angle;
- m_pose = {newPose.Translation(), angle};
-
- return m_pose;
-}
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h
new file mode 100644
index 0000000..c1686d2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h
@@ -0,0 +1,51 @@
+// 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/MathExtras.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a swerve drive drivetrain.
+ */
+template <size_t NumModules>
+struct WPILIB_DLLEXPORT SwerveDriveWheelPositions {
+ /**
+ * The distances driven by the wheels.
+ */
+ wpi::array<SwerveModulePosition, NumModules> positions;
+
+ /**
+ * Checks equality between this SwerveDriveWheelPositions and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const SwerveDriveWheelPositions& other) const = default;
+
+ /**
+ * Checks inequality between this SwerveDriveWheelPositions and another
+ * object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const SwerveDriveWheelPositions& other) const = default;
+
+ SwerveDriveWheelPositions<NumModules> Interpolate(
+ const SwerveDriveWheelPositions<NumModules>& endValue, double t) const {
+ auto result =
+ wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+ for (size_t i = 0; i < NumModules; i++) {
+ result[i] = positions[i].Interpolate(endValue.positions[i], t);
+ }
+ return {result};
+ }
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
index 18ed464..93f7465 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
@@ -4,6 +4,7 @@
#pragma once
+#include <wpi/MathExtras.h>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
@@ -25,5 +26,19 @@
* Angle of the module.
*/
Rotation2d angle;
+
+ /**
+ * Checks equality between this SwerveModulePosition and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const SwerveModulePosition& other) const;
+
+ SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
+ double t) const {
+ return {wpi::Lerp(distance, endValue.distance, t),
+ wpi::Lerp(angle, endValue.angle, t)};
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
index cae2d53..2f95d9b 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -27,6 +27,14 @@
Rotation2d angle;
/**
+ * Checks equality between this SwerveModuleState and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const SwerveModuleState& other) const;
+
+ /**
* Minimize the change in heading the desired swerve module state would
* require by potentially reversing the direction the wheel spins. If this is
* used with the PIDController class's continuous input functionality, the
@@ -36,13 +44,6 @@
* @param currentAngle The current module angle.
*/
static 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};
- }
- }
+ const Rotation2d& currentAngle);
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h
new file mode 100644
index 0000000..8867f66
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h
@@ -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.
+
+#pragma once
+
+#include <concepts>
+
+namespace frc {
+template <typename T>
+concept WheelPositions =
+ std::copy_constructible<T> && requires(T a, T b, double t) {
+ { a.Interpolate(b, t) } -> std::convertible_to<T>;
+ };
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index 0636707..1d6aaeb 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -49,7 +49,7 @@
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
- static Matrixd<4, 4> MakeHermiteBasis() {
+ static Eigen::Matrix4d MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
//
@@ -71,10 +71,10 @@
// [a₁] = [ 0 1 0 0][P(i+1) ]
// [a₀] = [ 1 0 0 0][P'(i+1)]
- static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
- {-3.0, -2.0, +3.0, -1.0},
- {+0.0, +1.0, +0.0, +0.0},
- {+1.0, +0.0, +0.0, +0.0}};
+ static const Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0},
+ {-3.0, -2.0, +3.0, -1.0},
+ {+0.0, +1.0, +0.0, +0.0},
+ {+1.0, +0.0, +0.0, +0.0}};
return basis;
}
diff --git a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
index 0720cd1..e1d2d52 100644
--- a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
+++ b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -56,7 +56,7 @@
};
/**
- * Parameterizes the spline. This method breaks up the spline into various
+ * Parametrizes the spline. This method breaks up the spline into various
* arcs until their dx, dy, and dtheta are within specific tolerances.
*
* @param spline The spline to parameterize.
diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h
index 957b875..72a3d43 100644
--- a/wpimath/src/main/native/include/frc/system/Discretization.h
+++ b/wpimath/src/main/native/include/frc/system/Discretization.h
@@ -4,9 +4,10 @@
#pragma once
+#include <unsupported/Eigen/MatrixFunctions>
+
#include "frc/EigenCore.h"
#include "units/time.h"
-#include "unsupported/Eigen/MatrixFunctions"
namespace frc {
@@ -44,9 +45,9 @@
// M = [A B]
// [0 0]
Matrixd<States + Inputs, States + Inputs> M;
- M.setZero();
M.template block<States, States>(0, 0) = contA;
M.template block<States, Inputs>(0, States) = contB;
+ M.template block<Inputs, States + Inputs>(States, 0).setZero();
// ϕ = eᴹᵀ = [A_d B_d]
// [ 0 I ]
@@ -101,95 +102,6 @@
}
/**
- * Discretizes the given continuous A and Q matrices.
- *
- * Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ()
- * (which is expensive), we take advantage of the structure of the block matrix
- * of A and Q.
- *
- * <ul>
- * <li>eᴬᵀ, which is only N x N, is relatively cheap.
- * <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate
- * using a taylor series to several terms and still be substantially
- * cheaper than taking the big exponential.
- * </ul>
- *
- * @tparam States Number of states.
- * @param contA Continuous system matrix.
- * @param contQ Continuous process noise covariance matrix.
- * @param dt Discretization timestep.
- * @param discA Storage for discrete system matrix.
- * @param discQ Storage for discrete process noise covariance matrix.
- */
-template <int States>
-void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
- const Matrixd<States, States>& contQ,
- units::second_t dt, Matrixd<States, States>* discA,
- Matrixd<States, States>* discQ) {
- // T
- // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
- //
- // M = [−A Q ]
- // [ 0 Aᵀ]
- // ϕ = eᴹᵀ
- // ϕ₁₂ = A_d⁻¹Q_d
- //
- // Taylor series of ϕ:
- //
- // ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
- // ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
- //
- // Taylor series of ϕ expanded for ϕ₁₂:
- //
- // ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
- //
- // ```
- // lastTerm = Q
- // lastCoeff = T
- // ATn = Aᵀ
- // ϕ₁₂ = lastTerm lastCoeff = QT
- //
- // for i in range(2, 6):
- // // i = 2
- // lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
- // lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
- // ATn *= Aᵀ = Aᵀ²
- //
- // // i = 3
- // lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
- // …
- // ```
-
- // Make continuous Q symmetric if it isn't already
- Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
-
- Matrixd<States, States> lastTerm = Q;
- double lastCoeff = dt.value();
-
- // Aᵀⁿ
- Matrixd<States, States> ATn = contA.transpose();
-
- Matrixd<States, States> phi12 = lastTerm * lastCoeff;
-
- // i = 6 i.e. 5th order should be enough precision
- for (int i = 2; i < 6; ++i) {
- lastTerm = -contA * lastTerm + Q * ATn;
- lastCoeff *= dt.value() / static_cast<double>(i);
-
- phi12 += lastTerm * lastCoeff;
-
- ATn *= contA.transpose();
- }
-
- DiscretizeA<States>(contA, dt, discA);
- Q = *discA * phi12;
-
- // Make discrete Q symmetric if it isn't already
- *discQ = (Q + Q.transpose()) / 2.0;
-}
-
-/**
* Returns a discretized version of the provided continuous measurement noise
* covariance matrix.
*
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index 1300a82..c61531a 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -4,6 +4,8 @@
#pragma once
+#include <functional>
+
#include <wpi/SymbolExports.h>
#include "frc/EigenCore.h"
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index bb856ec..98b6cc3 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -4,12 +4,10 @@
#pragma once
-#include <frc/StateSpaceUtil.h>
-
#include <algorithm>
#include <array>
+#include <cmath>
-#include "Eigen/Core"
#include "units/time.h"
namespace frc {
@@ -122,7 +120,11 @@
(b1[6] - b2[6]) * k7))
.norm();
- h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+ if (truncationError == 0.0) {
+ h = dt.value() - dtElapsed;
+ } else {
+ h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+ }
} while (truncationError > maxError);
dtElapsed += h;
diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
index 831f532..ad711ee 100644
--- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -97,7 +97,7 @@
}
/**
- * Returns the speed produced by the motor at a given torque and input
+ * Returns the angular speed produced by the motor at a given torque and input
* voltage.
*
* @param torque The torque produced by the motor.
@@ -119,89 +119,123 @@
}
/**
- * Returns instance of CIM.
+ * Returns a gearbox of CIM motors.
*/
static constexpr DCMotor CIM(int numMotors = 1) {
return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
}
/**
- * Returns instance of MiniCIM.
+ * Returns a gearbox of MiniCIM motors.
*/
static constexpr DCMotor MiniCIM(int numMotors = 1) {
return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
}
/**
- * Returns instance of Bag motor.
+ * Returns a gearbox of Bag motor motors.
*/
static constexpr DCMotor Bag(int numMotors = 1) {
return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
}
/**
- * Returns instance of Vex 775 Pro.
+ * Returns a gearbox of Vex 775 Pro motors.
*/
static constexpr DCMotor Vex775Pro(int numMotors = 1) {
return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
}
/**
- * Returns instance of Andymark RS 775-125.
+ * Returns a gearbox of Andymark RS 775-125 motors.
*/
static constexpr DCMotor RS775_125(int numMotors = 1) {
return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
}
/**
- * Returns instance of Banebots RS 775.
+ * Returns a gearbox of Banebots RS 775 motors.
*/
static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
}
/**
- * Returns instance of Andymark 9015.
+ * Returns a gearbox of Andymark 9015 motors.
*/
static constexpr DCMotor Andymark9015(int numMotors = 1) {
return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
}
/**
- * Returns instance of Banebots RS 550.
+ * Returns a gearbox of Banebots RS 550 motors.
*/
static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
}
/**
- * Returns instance of NEO brushless motor.
+ * Returns a gearbox of NEO brushless motors.
*/
static constexpr DCMotor NEO(int numMotors = 1) {
return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
}
/**
- * Returns instance of NEO 550 brushless motor.
+ * Returns a gearbox of NEO 550 brushless motors.
*/
static constexpr DCMotor NEO550(int numMotors = 1) {
return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
}
/**
- * Returns instance of Falcon 500 brushless motor.
+ * Returns a gearbox of Falcon 500 brushless motors.
*/
static constexpr DCMotor Falcon500(int numMotors = 1) {
return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
}
/**
+ * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control)
+ * enabled.
+ */
+ static constexpr DCMotor Falcon500FOC(int numMotors = 1) {
+ // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+ return DCMotor(12_V, 5.84_Nm, 304_A, 1.5_A, 6080_rpm, numMotors);
+ }
+
+ /**
* Return a gearbox of Romi/TI_RSLK MAX motors.
*/
static constexpr DCMotor RomiBuiltIn(int numMotors = 1) {
// From https://www.pololu.com/product/1520/specs
return DCMotor(4.5_V, 0.1765_Nm, 1.25_A, 0.13_A, 150_rpm, numMotors);
}
+
+ /**
+ * Return a gearbox of Kraken X60 brushless motors.
+ */
+ static constexpr DCMotor KrakenX60(int numMotors = 1) {
+ // From https://store.ctr-electronics.com/announcing-kraken-x60/
+ return DCMotor(12_V, 7.09_Nm, 366_A, 2_A, 6000_rpm, numMotors);
+ }
+
+ /**
+ * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented
+ * Control) enabled.
+ */
+ static constexpr DCMotor KrakenX60FOC(int numMotors = 1) {
+ // From https://store.ctr-electronics.com/announcing-kraken-x60/
+ return DCMotor(12_V, 9.37_Nm, 483_A, 2_A, 5800_rpm, numMotors);
+ }
+
+ /**
+ * Return a gearbox of Neo Vortex brushless motors.
+ */
+ static constexpr DCMotor NeoVortex(int numMotors = 1) {
+ // From https://www.revrobotics.com/next-generation-spark-neo/
+ return DCMotor(12_V, 3.60_Nm, 211_A, 3.615_A, 6784_rpm, numMotors);
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index 3e69545..64f3496 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -4,6 +4,7 @@
#pragma once
+#include <concepts>
#include <stdexcept>
#include <wpi/SymbolExports.h>
@@ -76,9 +77,9 @@
* @param kA The acceleration gain, in volts/(unit/sec²).
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
- template <typename Distance, typename = std::enable_if_t<
- std::is_same_v<units::meter, Distance> ||
- std::is_same_v<units::radian, Distance>>>
+ template <typename Distance>
+ requires std::same_as<units::meter, Distance> ||
+ std::same_as<units::radian, Distance>
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -117,9 +118,9 @@
*
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
- template <typename Distance, typename = std::enable_if_t<
- std::is_same_v<units::meter, Distance> ||
- std::is_same_v<units::radian, Distance>>>
+ template <typename Distance>
+ requires std::same_as<units::meter, Distance> ||
+ std::same_as<units::radian, Distance>
static LinearSystem<2, 1, 1> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -217,6 +218,48 @@
static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double G);
+
+ /**
+ * Create a state-space model of a DC motor system from its kV
+ * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
+ * found using SysId. the states of the system are [position, velocity],
+ * inputs are [voltage], and outputs are [position].
+ *
+ * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+ * argument. You may still use non-SI units (such as feet or inches) for the
+ * actual method arguments; they will automatically be converted to SI
+ * internally.
+ *
+ * The parameters provided by the user are from this feedforward model:
+ *
+ * u = K_v v + K_a a
+ *
+ * @param kV The velocity gain, in volts/(unit/sec).
+ * @param kA The acceleration gain, in volts/(unit/sec²).
+ *
+ * @throws std::domain_error if kV <= 0 or kA <= 0.
+ */
+ template <typename Distance>
+ requires std::same_as<units::meter, Distance> ||
+ std::same_as<units::radian, Distance>
+ static LinearSystem<2, 1, 2> DCMotorSystem(
+ decltype(1_V / Velocity_t<Distance>(1)) kV,
+ decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+ if (kV <= decltype(kV){0}) {
+ throw std::domain_error("Kv must be greater than zero.");
+ }
+ if (kA <= decltype(kA){0}) {
+ throw std::domain_error("Ka must be greater than zero.");
+ }
+
+ Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
+ Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
+ Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+ Matrixd<2, 1> D{{0.0}, {0.0}};
+
+ return LinearSystem<2, 1, 2>(A, B, C, D);
+ }
+
/**
* Create a state-space model of differential drive drivetrain. In this model,
* the states are [left velocity, right velocity], the inputs are [left
diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
new file mode 100644
index 0000000..f45987b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
@@ -0,0 +1,194 @@
+// 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 "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A Exponential-shaped velocity profile.
+ *
+ * While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on
+ * ExponentialProfile velocity constraints. To compute the reference obeying
+ * this constraint, do the following.
+ *
+ * Initialization:
+ * @code{.cpp}
+ * ExponentialProfile::Constraints constraints{kMaxV, kV, kA};
+ * State previousProfiledReference = {initialReference, 0_mps};
+ * @endcode
+ *
+ * Run on update:
+ * @code{.cpp}
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
+ * previousProfiledReference, unprofiledReference);
+ * @endcode
+ *
+ * where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `Calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * Otherwise, a timer can be started to provide monotonic values for
+ * `Calculate()` and to determine when the profile has completed via
+ * `IsFinished()`.
+ */
+template <class Distance, class Input>
+class ExponentialProfile {
+ public:
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using Input_t = units::unit_t<Input>;
+ using A_t = units::unit_t<units::inverse<units::seconds>>;
+ using B_t =
+ units::unit_t<units::compound_unit<Acceleration, units::inverse<Input>>>;
+ using KV = units::compound_unit<Input, units::inverse<Velocity>>;
+ using kV_t = units::unit_t<KV>;
+ using KA = units::compound_unit<Input, units::inverse<Acceleration>>;
+ using kA_t = units::unit_t<KA>;
+
+ class Constraints {
+ public:
+ Constraints(Input_t maxInput, A_t A, B_t B)
+ : maxInput{maxInput}, A{A}, B{B} {}
+ Constraints(Input_t maxInput, kV_t kV, kA_t kA)
+ : maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {}
+ Velocity_t MaxVelocity() const { return -maxInput * B / A; }
+
+ Input_t maxInput{0};
+ A_t A{0};
+ B_t B{0};
+ };
+
+ class State {
+ public:
+ Distance_t position{0};
+ Velocity_t velocity{0};
+ bool operator==(const State&) const = default;
+ };
+
+ class ProfileTiming {
+ public:
+ units::second_t inflectionTime;
+ units::second_t totalTime;
+
+ bool IsFinished(const units::second_t& time) const {
+ return time > totalTime;
+ }
+ };
+
+ /**
+ * Construct a ExponentialProfile.
+ *
+ * @param constraints The constraints on the profile, like maximum input.
+ */
+ explicit ExponentialProfile(Constraints constraints);
+
+ ExponentialProfile(const ExponentialProfile&) = default;
+ ExponentialProfile& operator=(const ExponentialProfile&) = default;
+ ExponentialProfile(ExponentialProfile&&) = default;
+ ExponentialProfile& operator=(ExponentialProfile&&) = default;
+
+ /**
+ * Calculate the correct position and velocity for the profile at a time t
+ * where the current state is at time t = 0.
+ */
+ State Calculate(const units::second_t& t, const State& current,
+ const State& goal) const;
+
+ /**
+ * Calculate the point after which the fastest way to reach the goal state is
+ * to apply input in the opposite direction.
+ */
+ State CalculateInflectionPoint(const State& current, const State& goal) const;
+
+ /**
+ * Calculate the time it will take for this profile to reach the goal state.
+ */
+ units::second_t TimeLeftUntil(const State& current, const State& goal) const;
+
+ /**
+ * Calculate the time it will take for this profile to reach the inflection
+ * point, and the time it will take for this profile to reach the goal state.
+ */
+ ProfileTiming CalculateProfileTiming(const State& current,
+ const State& goal) const;
+
+ private:
+ /**
+ * Calculate the point after which the fastest way to reach the goal state is
+ * to apply input in the opposite direction.
+ */
+ State CalculateInflectionPoint(const State& current, const State& goal,
+ const Input_t& input) const;
+
+ /**
+ * Calculate the time it will take for this profile to reach the inflection
+ * point, and the time it will take for this profile to reach the goal state.
+ */
+ ProfileTiming CalculateProfileTiming(const State& current,
+ const State& inflectionPoint,
+ const State& goal,
+ const Input_t& input) const;
+
+ /**
+ * Calculate the velocity reached after t seconds when applying an input from
+ * the initial state.
+ */
+ Velocity_t ComputeVelocityFromTime(const units::second_t& time,
+ const Input_t& input,
+ const State& initial) const;
+
+ /**
+ * Calculate the position reached after t seconds when applying an input from
+ * the initial state.
+ */
+ Distance_t ComputeDistanceFromTime(const units::second_t& time,
+ const Input_t& input,
+ const State& initial) const;
+
+ /**
+ * Calculate the distance reached at the same time as the given velocity when
+ * applying the given input from the initial state.
+ */
+ Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity,
+ const Input_t& input,
+ const State& initial) const;
+
+ /**
+ * Calculate the time required to reach a specified velocity given the initial
+ * velocity.
+ */
+ units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity,
+ const Input_t& input,
+ const Velocity_t& initial) const;
+
+ /**
+ * Calculate the velocity at which input should be reversed in order to reach
+ * the goal state from the current state.
+ */
+ Velocity_t SolveForInflectionVelocity(const Input_t& input,
+ const State& current,
+ const State& goal) const;
+
+ /**
+ * Returns true if the profile should be inverted.
+ *
+ * <p>The profile is inverted if we should first apply negative input in order
+ * to reach the goal state.
+ */
+ bool ShouldFlipInput(const State& current, const State& goal) const;
+
+ Constraints m_constraints;
+};
+} // namespace frc
+
+#include "ExponentialProfile.inc"
diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc
new file mode 100644
index 0000000..ded5245
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc
@@ -0,0 +1,253 @@
+// 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 <algorithm>
+
+#include <fmt/core.h>
+
+#include "frc/trajectory/ExponentialProfile.h"
+#include "units/math.h"
+
+namespace frc {
+template <class Distance, class Input>
+ExponentialProfile<Distance, Input>::ExponentialProfile(Constraints constraints)
+ : m_constraints(constraints) {}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::Calculate(const units::second_t& t,
+ const State& current,
+ const State& goal) const {
+ auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+ auto u = direction * m_constraints.maxInput;
+
+ auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
+ auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u);
+
+ if (t < 0_s) {
+ return current;
+ } else if (t < timing.inflectionTime) {
+ return {ComputeDistanceFromTime(t, u, current),
+ ComputeVelocityFromTime(t, u, current)};
+ } else if (t < timing.totalTime) {
+ return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal),
+ ComputeVelocityFromTime(t - timing.totalTime, -u, goal)};
+ } else {
+ return goal;
+ }
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
+ const State& current, const State& goal) const {
+ auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+ auto u = direction * m_constraints.maxInput;
+
+ return CalculateInflectionPoint(current, goal, u);
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
+ const State& current, const State& goal, const Input_t& input) const {
+ auto u = input;
+
+ if (current == goal) {
+ return current;
+ }
+
+ auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal);
+ auto inflectionPosition =
+ ComputeDistanceFromVelocity(inflectionVelocity, -u, goal);
+
+ return {inflectionPosition, inflectionVelocity};
+}
+
+template <class Distance, class Input>
+units::second_t ExponentialProfile<Distance, Input>::TimeLeftUntil(
+ const State& current, const State& goal) const {
+ auto timing = CalculateProfileTiming(current, goal);
+
+ return timing.totalTime;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::ProfileTiming
+ExponentialProfile<Distance, Input>::CalculateProfileTiming(
+ const State& current, const State& goal) const {
+ auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+ auto u = direction * m_constraints.maxInput;
+
+ auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
+ return CalculateProfileTiming(current, inflectionPoint, goal, u);
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::ProfileTiming
+ExponentialProfile<Distance, Input>::CalculateProfileTiming(
+ const State& current, const State& inflectionPoint, const State& goal,
+ const Input_t& input) const {
+ auto u = input;
+ auto u_dir = units::math::abs(u) / u;
+
+ units::second_t inflectionT_forward;
+
+ // We need to handle 5 cases here:
+ //
+ // - Approaching -maxVelocity from below
+ // - Approaching -maxVelocity from above
+ // - Approaching maxVelocity from below
+ // - Approaching maxVelocity from above
+ // - At +-maxVelocity
+ //
+ // For cases 1 and 3, we want to subtract epsilon from the inflection point
+ // velocity For cases 2 and 4, we want to add epsilon to the inflection point
+ // velocity. For case 5, we have reached inflection point velocity.
+ auto epsilon = Velocity_t(1e-9);
+ if (units::math::abs(u_dir * m_constraints.MaxVelocity() -
+ inflectionPoint.velocity) < epsilon) {
+ auto solvableV = inflectionPoint.velocity;
+ units::second_t t_to_solvable_v;
+ Distance_t x_at_solvable_v;
+ if (units::math::abs(current.velocity - inflectionPoint.velocity) <
+ epsilon) {
+ t_to_solvable_v = 0_s;
+ x_at_solvable_v = current.position;
+ } else {
+ if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
+ solvableV += u_dir * epsilon;
+ } else {
+ solvableV -= u_dir * epsilon;
+ }
+
+ t_to_solvable_v = ComputeTimeFromVelocity(solvableV, u, current.velocity);
+ x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current);
+ }
+
+ inflectionT_forward =
+ t_to_solvable_v + u_dir * (inflectionPoint.position - x_at_solvable_v) /
+ m_constraints.MaxVelocity();
+ } else {
+ inflectionT_forward =
+ ComputeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
+ }
+
+ auto inflectionT_backward =
+ ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
+
+ return {inflectionT_forward, inflectionT_forward - inflectionT_backward};
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Distance_t
+ExponentialProfile<Distance, Input>::ComputeDistanceFromTime(
+ const units::second_t& time, const Input_t& input,
+ const State& initial) const {
+ auto A = m_constraints.A;
+ auto B = m_constraints.B;
+ auto u = input;
+
+ return initial.position +
+ (-B * u * time +
+ (initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) /
+ A;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Velocity_t
+ExponentialProfile<Distance, Input>::ComputeVelocityFromTime(
+ const units::second_t& time, const Input_t& input,
+ const State& initial) const {
+ auto A = m_constraints.A;
+ auto B = m_constraints.B;
+ auto u = input;
+
+ return (initial.velocity + B * u / A) * units::math::exp(A * time) -
+ B * u / A;
+}
+
+template <class Distance, class Input>
+units::second_t ExponentialProfile<Distance, Input>::ComputeTimeFromVelocity(
+ const Velocity_t& velocity, const Input_t& input,
+ const Velocity_t& initial) const {
+ auto A = m_constraints.A;
+ auto B = m_constraints.B;
+ auto u = input;
+
+ return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Distance_t
+ExponentialProfile<Distance, Input>::ComputeDistanceFromVelocity(
+ const Velocity_t& velocity, const Input_t& input,
+ const State& initial) const {
+ auto A = m_constraints.A;
+ auto B = m_constraints.B;
+ auto u = input;
+
+ return initial.position + (velocity - initial.velocity) / A -
+ B * u / (A * A) *
+ units::math::log((A * velocity + B * u) /
+ (A * initial.velocity + B * u));
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Velocity_t
+ExponentialProfile<Distance, Input>::SolveForInflectionVelocity(
+ const Input_t& input, const State& current, const State& goal) const {
+ auto A = m_constraints.A;
+ auto B = m_constraints.B;
+ auto u = input;
+
+ auto u_dir = u / units::math::abs(u);
+
+ auto position_delta = goal.position - current.position;
+ auto velocity_delta = goal.velocity - current.velocity;
+
+ auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
+ auto power = -A / B / u * (A * position_delta - velocity_delta);
+
+ auto a = -A * A;
+ auto c = B * B * u * u + scalar * units::math::exp(power);
+
+ if (-1e-9 < c.value() && c.value() < 0) {
+ // numeric instability - the heuristic gets it right but c is around -1e-13
+ return Velocity_t(0);
+ }
+
+ return u_dir * units::math::sqrt(-c / a);
+}
+
+template <class Distance, class Input>
+bool ExponentialProfile<Distance, Input>::ShouldFlipInput(
+ const State& current, const State& goal) const {
+ auto u = m_constraints.maxInput;
+
+ auto v0 = current.velocity;
+ auto xf = goal.position;
+ auto vf = goal.velocity;
+
+ auto x_forward = ComputeDistanceFromVelocity(vf, u, current);
+ auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current);
+
+ if (v0 >= m_constraints.MaxVelocity()) {
+ return xf < x_reverse;
+ }
+
+ if (v0 <= -m_constraints.MaxVelocity()) {
+ return xf < x_forward;
+ }
+
+ auto a = v0 >= Velocity_t(0);
+ auto b = vf >= Velocity_t(0);
+ auto c = xf >= x_forward;
+ auto d = xf >= x_reverse;
+
+ return (a && !d) || (b && !c) || (!c && !d);
+}
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index f5ee79b..ca97593 100644
--- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -7,6 +7,7 @@
#include <vector>
#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Transform2d.h"
@@ -15,10 +16,6 @@
#include "units/time.h"
#include "units/velocity.h"
-namespace wpi {
-class json;
-} // namespace wpi
-
namespace frc {
/**
* Represents a time-parameterized trajectory. The trajectory contains of
@@ -66,6 +63,8 @@
/**
* Constructs a trajectory from a vector of states.
+ *
+ * @throws std::invalid_argument if the vector of states is empty.
*/
explicit Trajectory(const std::vector<State>& states);
@@ -77,6 +76,7 @@
/**
* Return the states of the trajectory.
+ *
* @return The states of the trajectory.
*/
const std::vector<State>& States() const { return m_states; }
@@ -86,6 +86,7 @@
*
* @param t The point in time since the beginning of the trajectory to sample.
* @return The state at that point in time.
+ * @throws std::runtime_error if the trajectory has no states.
*/
State Sample(units::second_t t) const;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index b1a0b52..4e6c1e6 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -4,6 +4,7 @@
#pragma once
+#include <concepts>
#include <memory>
#include <utility>
#include <vector>
@@ -74,8 +75,7 @@
* Adds a user-defined constraint to the trajectory.
* @param constraint The user-defined constraint.
*/
- template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
- TrajectoryConstraint, Constraint>>>
+ template <std::derived_from<TrajectoryConstraint> Constraint>
void AddConstraint(Constraint constraint) {
m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
}
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 24a8253..73aab38 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -4,6 +4,8 @@
#pragma once
+#include <wpi/deprecated.h>
+
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -21,13 +23,14 @@
* @code{.cpp}
* TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
* double previousProfiledReference = initialReference;
+ * TrapezoidProfile profile{constraints};
* @endcode
*
* Run on update:
* @code{.cpp}
- * TrapezoidProfile profile{constraints, unprofiledReference,
- * previousProfiledReference};
- * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
+ * unprofiledReference,
+ * previousProfiledReference);
* @endcode
*
* where `unprofiledReference` is free to change between calls. Note that when
@@ -75,9 +78,22 @@
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
+ */
+ TrapezoidProfile(Constraints constraints); // NOLINT
+
+ /**
+ * Construct a TrapezoidProfile.
+ *
+ * @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
+ * @deprecated Pass the desired and current state into calculate instead of
+ * constructing a new TrapezoidProfile with the desired and current state
*/
+ WPI_DEPRECATED(
+ "Pass the desired and current state into calculate instead of "
+ "constructing a new TrapezoidProfile with the desired and current "
+ "state")
TrapezoidProfile(Constraints constraints, State goal,
State initial = State{Distance_t{0}, Velocity_t{0}});
@@ -91,10 +107,26 @@
* where the beginning of the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
+ * @deprecated Pass the desired and current state into calculate instead of
+ * constructing a new TrapezoidProfile with the desired and current state
*/
+ [[deprecated(
+ "Pass the desired and current state into calculate instead of "
+ "constructing a new TrapezoidProfile with the desired and current "
+ "state")]]
State Calculate(units::second_t t) const;
/**
+ * Calculate the correct position and velocity for the profile at a time t
+ * where the beginning of the profile was at time t = 0.
+ *
+ * @param t The time since the beginning of the profile.
+ * @param goal The desired state when the profile is complete.
+ * @param current The initial state (usually the current state).
+ */
+ State Calculate(units::second_t t, State goal, State current);
+
+ /**
* Returns the time left until a target distance in the profile is reached.
*
* @param target The target distance.
@@ -141,8 +173,9 @@
int m_direction;
Constraints m_constraints;
- State m_initial;
- State m_goal;
+ State m_current;
+ State m_goal; // TODO: remove
+ bool m_newAPI; // TODO: remove
units::second_t m_endAccel;
units::second_t m_endFullSpeed;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 19eb1f3..24e0a46 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -11,21 +11,26 @@
namespace frc {
template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints)
+ : m_constraints(constraints), m_newAPI(true) {}
+
+template <class Distance>
TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
State goal, State initial)
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
m_constraints(constraints),
- m_initial(Direct(initial)),
- m_goal(Direct(goal)) {
- if (m_initial.velocity > m_constraints.maxVelocity) {
- m_initial.velocity = m_constraints.maxVelocity;
+ m_current(Direct(initial)),
+ m_goal(Direct(goal)),
+ m_newAPI(false) {
+ if (m_current.velocity > m_constraints.maxVelocity) {
+ m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
units::second_t cutoffBegin =
- m_initial.velocity / m_constraints.maxAcceleration;
+ m_current.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
@@ -37,7 +42,7 @@
// of a truncated one
Distance_t fullTrapezoidDist =
- cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+ cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
@@ -60,15 +65,19 @@
template <class Distance>
typename TrapezoidProfile<Distance>::State
TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
- State result = m_initial;
+ if (m_newAPI) {
+ throw std::runtime_error(
+ "Cannot use new constructor with deprecated Calculate()");
+ }
+ State result = m_current;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
- (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+ (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
- result.position += (m_initial.velocity +
+ result.position += (m_current.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
m_endAccel +
m_constraints.maxVelocity * (t - m_endAccel);
@@ -86,12 +95,83 @@
return Direct(result);
}
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
+ State current) {
+ m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
+ m_current = Direct(current);
+ goal = Direct(goal);
+ if (m_current.velocity > m_constraints.maxVelocity) {
+ m_current.velocity = m_constraints.maxVelocity;
+ }
+
+ // Deal with a possibly truncated motion profile (with nonzero initial or
+ // final velocity) by calculating the parameters as if the profile began and
+ // ended at zero velocity
+ units::second_t cutoffBegin =
+ m_current.velocity / m_constraints.maxAcceleration;
+ Distance_t cutoffDistBegin =
+ cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+ units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
+ Distance_t cutoffDistEnd =
+ cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+ // Now we can calculate the parameters as if it was a full trapezoid instead
+ // of a truncated one
+
+ Distance_t fullTrapezoidDist =
+ cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
+ units::second_t accelerationTime =
+ m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+ Distance_t fullSpeedDist =
+ fullTrapezoidDist -
+ accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+ // Handle the case where the profile never reaches full speed
+ if (fullSpeedDist < Distance_t{0}) {
+ accelerationTime =
+ units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+ fullSpeedDist = Distance_t{0};
+ }
+
+ m_endAccel = accelerationTime - cutoffBegin;
+ m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+ m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+ State result = m_current;
+
+ if (t < m_endAccel) {
+ result.velocity += t * m_constraints.maxAcceleration;
+ result.position +=
+ (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+ } else if (t < m_endFullSpeed) {
+ result.velocity = m_constraints.maxVelocity;
+ result.position += (m_current.velocity +
+ m_endAccel * m_constraints.maxAcceleration / 2.0) *
+ m_endAccel +
+ m_constraints.maxVelocity * (t - m_endAccel);
+ } else if (t <= m_endDeccel) {
+ result.velocity =
+ goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+ units::second_t timeLeft = m_endDeccel - t;
+ result.position =
+ goal.position -
+ (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+ timeLeft;
+ } else {
+ result = goal;
+ }
+
+ return Direct(result);
+}
template <class Distance>
units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
Distance_t target) const {
- Distance_t position = m_initial.position * m_direction;
- Velocity_t velocity = m_initial.velocity * m_direction;
+ Distance_t position = m_current.position * m_direction;
+ Velocity_t velocity = m_current.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
index 0edd8cc..0d96893 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -20,9 +20,8 @@
class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
: public TrajectoryConstraint {
public:
- DifferentialDriveKinematicsConstraint(
- const DifferentialDriveKinematics& kinematics,
- units::meters_per_second_t maxSpeed);
+ DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
+ units::meters_per_second_t maxSpeed);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
index 40a0d8e..8f7ba2c 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -34,7 +34,7 @@
*/
DifferentialDriveVoltageConstraint(
const SimpleMotorFeedforward<units::meter>& feedforward,
- const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage);
+ DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
index f9f0d2e..74f3c55 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -4,6 +4,7 @@
#pragma once
+#include <concepts>
#include <limits>
#include "frc/geometry/Rotation2d.h"
@@ -15,8 +16,7 @@
/**
* Enforces a particular constraint only within an elliptical region.
*/
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
- TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
class EllipticalRegionConstraint : public TrajectoryConstraint {
public:
/**
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
index 18522fe..f3b364b 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -4,6 +4,7 @@
#pragma once
+#include <concepts>
#include <limits>
#include "frc/geometry/Rotation2d.h"
@@ -14,8 +15,7 @@
/**
* Enforces a particular constraint only within a rectangular region.
*/
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
- TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
class RectangularRegionConstraint : public TrajectoryConstraint {
public:
/**
diff --git a/wpimath/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h
index 632982b..8174dac 100644
--- a/wpimath/src/main/native/include/units/angular_acceleration.h
+++ b/wpimath/src/main/native/include/units/angular_acceleration.h
@@ -4,7 +4,7 @@
#pragma once
-#include "units/angular_velocity.h"
+#include "units/angle.h"
#include "units/base.h"
#include "units/time.h"
diff --git a/wpimath/src/main/native/include/units/angular_jerk.h b/wpimath/src/main/native/include/units/angular_jerk.h
new file mode 100644
index 0000000..b58bc16
--- /dev/null
+++ b/wpimath/src/main/native/include/units/angular_jerk.h
@@ -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.
+
+#pragma once
+
+#include "units/angle.h"
+#include "units/base.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::angular_jerk
+ * @brief namespace for unit types and containers representing angular
+ * jerk values
+ * @details The SI unit for angular jerk is
+ * `radians_per_second_cubed`, and the corresponding `base_unit`
+ * category is`angular_jerk_unit`.
+ * @anchor angularJerkContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+UNIT_ADD(angular_jerk, radians_per_second_cubed, radians_per_second_cubed,
+ rad_per_s_cu, unit<std::ratio<1>, units::category::angular_jerk_unit>)
+UNIT_ADD(angular_jerk, degrees_per_second_cubed, degrees_per_second_cubed,
+ deg_per_s_cu,
+ compound_unit<angle::degrees, inverse<cubed<time::seconds>>>)
+UNIT_ADD(angular_jerk, turns_per_second_cubed, turns_per_second_cubed,
+ tr_per_s_cu,
+ compound_unit<angle::turns, inverse<cubed<time::seconds>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(angular_jerk)
+
+using namespace angular_jerk;
+} // namespace units
diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
index 9d021b1..34cd59e 100644
--- a/wpimath/src/main/native/include/units/base.h
+++ b/wpimath/src/main/native/include/units/base.h
@@ -76,7 +76,7 @@
#include <locale>
#include <string>
#endif
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
#include <locale>
#include <string>
#include <fmt/format.h>
@@ -176,7 +176,7 @@
* @param abbrev - abbreviated unit name, e.g. 'm'
* @note When UNIT_LIB_ENABLE_IOSTREAM isn't defined, the macro does not generate any code
*/
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
}\
template <>\
@@ -463,13 +463,19 @@
//----------------------------------
/**
+ * @defgroup Units Unit API
+ */
+
+ /**
* @defgroup UnitContainers Unit Containers
+ * @ingroup Units
* @brief Defines a series of classes which contain dimensioned values. Unit containers
* store a value, and support various arithmetic operations.
*/
/**
* @defgroup UnitTypes Unit Types
+ * @ingroup Units
* @brief Defines a series of classes which represent units. These types are tags used by
* the conversion function, to create compound units, or to create `unit_t` types.
* By themselves, they are not containers and have no stored value.
@@ -477,6 +483,7 @@
/**
* @defgroup UnitManipulators Unit Manipulators
+ * @ingroup Units
* @brief Defines a series of classes used to manipulate unit types, such as `inverse<>`, `squared<>`, and metric prefixes.
* Unit manipulators can be chained together, e.g. `inverse<squared<pico<time::seconds>>>` to
* represent picoseconds^-2.
@@ -484,6 +491,7 @@
/**
* @defgroup CompileTimeUnitManipulators Compile-time Unit Manipulators
+ * @ingroup Units
* @brief Defines a series of classes used to manipulate `unit_value_t` types at compile-time, such as `unit_value_add<>`, `unit_value_sqrt<>`, etc.
* Compile-time manipulators can be chained together, e.g. `unit_value_sqrt<unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>` to
* represent `c = sqrt(a^2 + b^2).
@@ -491,17 +499,20 @@
/**
* @defgroup UnitMath Unit Math
+ * @ingroup Units
* @brief Defines a collection of unit-enabled, strongly-typed versions of `<cmath>` functions.
* @details Includes most c++11 extensions.
*/
/**
* @defgroup Conversion Explicit Conversion
+ * @ingroup Units
* @brief Functions used to convert values of one logical type to another.
*/
/**
* @defgroup TypeTraits Type Traits
+ * @ingroup Units
* @brief Defines a series of classes to obtain unit type information at compile-time.
*/
@@ -810,6 +821,7 @@
typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-1>, std::ratio<1>> angular_velocity_unit; ///< Represents an SI derived unit of angular velocity
typedef base_unit<detail::meter_ratio<1>, std::ratio<0>, std::ratio<-2>> acceleration_unit; ///< Represents an SI derived unit of acceleration
typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-2>, std::ratio<1>> angular_acceleration_unit; ///< Represents an SI derived unit of angular acceleration
+ typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-3>, std::ratio<1>> angular_jerk_unit; ///< Represents an SI derived unit of angular jerk
typedef base_unit<detail::meter_ratio<1>, std::ratio<1>, std::ratio<-2>> force_unit; ///< Represents an SI derived unit of force
typedef base_unit<detail::meter_ratio<-1>, std::ratio<1>, std::ratio<-2>> pressure_unit; ///< Represents an SI derived unit of pressure
typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>, std::ratio<1>> charge_unit; ///< Represents an SI derived unit of charge
@@ -1394,7 +1406,7 @@
* error. This value should be chosen to be as high as possible before
* integer overflow errors occur in the compiler.
* @note USE WITH CAUTION. The is an approximate value. In general, squared<sqrt<meter>> != meter,
- * i.e. the operation is not reversible, and it will result in propogated approximations.
+ * i.e. the operation is not reversible, and it will result in propagated approximations.
* Use only when absolutely necessary.
*/
template<class U, std::intmax_t Eps = 10000000000>
@@ -1758,7 +1770,7 @@
#ifdef FOR_DOXYGEN_PURPOSOES_ONLY
/**
* @ingroup TypeTraits
- * @brief Trait for accessing the publically defined types of `units::unit_t`
+ * @brief Trait for accessing the publicly defined types of `units::unit_t`
* @details The units library determines certain properties of the unit_t types passed to them
* and what they represent by using the members of the corresponding unit_t_traits instantiation.
*/
@@ -1788,7 +1800,7 @@
/**
* @ingroup TypeTraits
- * @brief Trait for accessing the publically defined types of `units::unit_t`
+ * @brief Trait for accessing the publicly defined types of `units::unit_t`
* @details
*/
template<typename T>
@@ -2875,7 +2887,7 @@
}
#endif
}
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
template <>
struct fmt::formatter<units::dimensionless::dB_t> : fmt::formatter<double>
{
@@ -2972,7 +2984,7 @@
#ifdef FOR_DOXYGEN_PURPOSES_ONLY
/**
* @ingroup TypeTraits
- * @brief Trait for accessing the publically defined types of `units::unit_value_t_traits`
+ * @brief Trait for accessing the publicly defined types of `units::unit_value_t_traits`
* @details The units library determines certain properties of the `unit_value_t` types passed to
* them and what they represent by using the members of the corresponding `unit_value_t_traits`
* instantiation.
@@ -2999,7 +3011,7 @@
/**
* @ingroup TypeTraits
- * @brief Trait for accessing the publically defined types of `units::unit_value_t_traits`
+ * @brief Trait for accessing the publicly defined types of `units::unit_value_t_traits`
* @details
*/
template<typename T>
@@ -3440,6 +3452,6 @@
using namespace units::literals;
#endif // UNIT_HAS_LITERAL_SUPPORT
-#if !defined(UNIT_LIB_DISABLE_FMT)
-#include "frc/fmt/Units.h"
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
+#include "units/formatter.h"
#endif
diff --git a/wpimath/src/main/native/include/frc/fmt/Units.h b/wpimath/src/main/native/include/units/formatter.h
similarity index 88%
rename from wpimath/src/main/native/include/frc/fmt/Units.h
rename to wpimath/src/main/native/include/units/formatter.h
index 1ec61ca..1c17b0a 100644
--- a/wpimath/src/main/native/include/frc/fmt/Units.h
+++ b/wpimath/src/main/native/include/units/formatter.h
@@ -4,43 +4,41 @@
#pragma once
+#include <type_traits>
+
#include <fmt/format.h>
#include "units/base.h"
+// FIXME: Replace enable_if with requires clause and remove <type_traits>
+// include once using GCC >= 12. GCC 11 incorrectly emits a struct redefinition
+// error because it doesn't use the requires clause to disambiguate.
+
/**
* Formatter for unit types.
- *
- * @tparam Units Unit tag for which type of units the `unit_t` represents (e.g.
- * meters).
- * @tparam T Underlying type of the storage. Defaults to double.
- * @tparam NonLinearScale Optional scale class for the units. Defaults to linear
- * (i.e. does not scale the unit value). Examples of
- * non-linear scales could be logarithmic, decibel, or
- * richter scales. Non-linear scales must adhere to the
- * non-linear-scale concept.
*/
-template <class Units, typename T, template <typename> class NonLinearScale>
-struct fmt::formatter<units::unit_t<Units, T, NonLinearScale>>
- : fmt::formatter<double> {
+template <typename Unit, typename CharT>
+struct fmt::formatter<Unit, CharT,
+ std::enable_if_t<units::traits::is_unit_t_v<Unit>>> {
+ constexpr auto parse(fmt::format_parse_context& ctx) {
+ return m_underlying.parse(ctx);
+ }
+
/**
* Writes out a formatted unit.
*
- * @tparam FormatContext Format string context type.
* @param obj Unit instance.
* @param ctx Format string context.
*/
- template <typename FormatContext>
- auto format(const units::unit_t<Units, T, NonLinearScale>& obj,
- FormatContext& ctx) {
+ auto format(const Unit& obj, fmt::format_context& ctx) const {
+ using Units = typename Unit::unit_type;
using BaseUnits =
units::unit<std::ratio<1>,
typename units::traits::unit_traits<Units>::base_unit_type>;
auto out = ctx.out();
- out = fmt::formatter<double>::format(
- units::convert<Units, BaseUnits>(obj()), ctx);
+ out = m_underlying.format(units::convert<Units, BaseUnits>(obj()), ctx);
if constexpr (units::traits::unit_traits<
Units>::base_unit_type::meter_ratio::num != 0) {
@@ -215,4 +213,7 @@
return out;
}
+
+ private:
+ fmt::formatter<typename Unit::underlying_type, CharT> m_underlying;
};
diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
index 3b9b3cd..8e0698d 100644
--- a/wpimath/src/main/native/include/wpimath/MathShared.h
+++ b/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -9,6 +9,8 @@
#include <fmt/format.h>
#include <wpi/SymbolExports.h>
+#include "units/time.h"
+
namespace wpi::math {
enum class MathUsageId {
@@ -31,6 +33,7 @@
virtual void ReportWarningV(fmt::string_view format,
fmt::format_args args) = 0;
virtual void ReportUsage(MathUsageId id, int count) = 0;
+ virtual units::second_t GetTimestamp() = 0;
template <typename S, typename... Args>
inline void ReportError(const S& format, Args&&... args) {
@@ -70,6 +73,10 @@
static void ReportUsage(MathUsageId id, int count) {
GetMathShared().ReportUsage(id, count);
}
+
+ static units::second_t GetTimestamp() {
+ return GetMathShared().GetTimestamp();
+ }
};
} // namespace wpi::math
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
deleted file mode 100644
index 47097ed..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
+++ /dev/null
@@ -1,162 +0,0 @@
-#pragma once
-
-#include <type_traits>
-
-/// @file
-/// Provides Drake's assertion implementation. This is intended to be used
-/// both within Drake and by other software. Drake's asserts can be armed
-/// and disarmed independently from the system-wide asserts.
-
-#ifdef DRAKE_DOXYGEN_CXX
-/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
-/// from the C++ system header @p <cassert>. Unless Drake's assertions are
-/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
-/// will evaluate @p condition and iff the value is false will trigger an
-/// assertion failure with a message showing at least the condition text,
-/// function name, file, and line.
-///
-/// By default, assertion failures will :abort() the program. However, when
-/// using the pydrake python bindings, assertion failures will instead throw a
-/// C++ exception that causes a python SystemExit exception.
-///
-/// Assertions are enabled or disabled using the following pre-processor macros:
-///
-/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
-/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
-/// - If both macros are defined, then it is a compile-time error.
-/// - If neither are defined, then NDEBUG governs assertions as usual.
-///
-/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
-/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
-///
-/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed`
-/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals.
-///
-/// One difference versus the standard @p assert(condition) is that the
-/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
-/// Drake's assertions are disarmed.
-///
-/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
-/// in block scope, and must always be followed by a semicolon.
-#define DRAKE_ASSERT(condition)
-/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this
-/// allows for guarding expensive assertion-checking subroutines using the same
-/// macros as stand-alone assertions.
-#define DRAKE_ASSERT_VOID(expression)
-/// Evaluates @p condition and iff the value is false will trigger an assertion
-/// failure with a message showing at least the condition text, function name,
-/// file, and line.
-#define DRAKE_DEMAND(condition)
-/// Silences a "no return value" compiler warning by calling a function that
-/// always raises an exception or aborts (i.e., a function marked noreturn).
-/// Only use this macro at a point where (1) a point in the code is truly
-/// unreachable, (2) the fact that it's unreachable is knowable from only
-/// reading the function itself (and not, e.g., some larger design invariant),
-/// and (3) there is a compiler warning if this macro were removed. The most
-/// common valid use is with a switch-case-return block where all cases are
-/// accounted for but the enclosing function is supposed to return a value. Do
-/// *not* use this macro as a "logic error" assertion; it should *only* be used
-/// to silence false positive warnings. When in doubt, throw an exception
-/// manually instead of using this macro.
-#define DRAKE_UNREACHABLE()
-#else // DRAKE_DOXYGEN_CXX
-
-// Users should NOT set these; only this header should set them.
-#ifdef DRAKE_ASSERT_IS_ARMED
-# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
-#endif
-#ifdef DRAKE_ASSERT_IS_DISARMED
-# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
-#endif
-
-// Decide whether Drake assertions are enabled.
-#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
-# error Conflicting assertion toggles.
-#elif defined(DRAKE_ENABLE_ASSERTS)
-# define DRAKE_ASSERT_IS_ARMED
-#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
-# define DRAKE_ASSERT_IS_DISARMED
-#else
-# define DRAKE_ASSERT_IS_ARMED
-#endif
-
-namespace drake {
-namespace internal {
-// Abort the program with an error message.
-[[noreturn]] void Abort(const char* condition, const char* func,
- const char* file, int line);
-// Report an assertion failure; will either Abort(...) or throw.
-[[noreturn]] void AssertionFailed(const char* condition, const char* func,
- const char* file, int line);
-} // namespace internal
-namespace assert {
-// Allows for specialization of how to bool-convert Conditions used in
-// assertions, in case they are not intrinsically convertible. See
-// common/symbolic/expression/formula.h for an example use. This is a public
-// interface to extend; it is intended to be specialized by unusual Scalar
-// types that require special handling.
-template <typename Condition>
-struct ConditionTraits {
- static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
- static bool Evaluate(const Condition& value) {
- return value;
- }
-};
-} // namespace assert
-} // namespace drake
-
-#define DRAKE_UNREACHABLE() \
- ::drake::internal::Abort( \
- "Unreachable code was reached?!", __func__, __FILE__, __LINE__)
-
-#define DRAKE_DEMAND(condition) \
- do { \
- typedef ::drake::assert::ConditionTraits< \
- typename std::remove_cv_t<decltype(condition)>> Trait; \
- static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
- static_assert( \
- !std::is_pointer_v<decltype(condition)>, \
- "When using DRAKE_DEMAND on a raw pointer, always write out " \
- "DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) " \
- "and rely on implicit pointer-to-bool conversion."); \
- if (!Trait::Evaluate(condition)) { \
- ::drake::internal::AssertionFailed( \
- #condition, __func__, __FILE__, __LINE__); \
- } \
- } while (0)
-
-#ifdef DRAKE_ASSERT_IS_ARMED
-// Assertions are enabled.
-namespace drake {
-constexpr bool kDrakeAssertIsArmed = true;
-constexpr bool kDrakeAssertIsDisarmed = false;
-} // namespace drake
-# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
-# define DRAKE_ASSERT_VOID(expression) do { \
- static_assert( \
- std::is_convertible_v<decltype(expression), void>, \
- "Expression should be void."); \
- expression; \
- } while (0)
-#else
-// Assertions are disabled, so just typecheck the expression.
-namespace drake {
-constexpr bool kDrakeAssertIsArmed = false;
-constexpr bool kDrakeAssertIsDisarmed = true;
-} // namespace drake
-# define DRAKE_ASSERT(condition) do { \
- typedef ::drake::assert::ConditionTraits< \
- typename std::remove_cv_t<decltype(condition)>> Trait; \
- static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
- static_assert( \
- !std::is_pointer_v<decltype(condition)>, \
- "When using DRAKE_ASSERT on a raw pointer, always write out " \
- "DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) " \
- "and rely on implicit pointer-to-bool conversion."); \
- } while (0)
-# define DRAKE_ASSERT_VOID(expression) static_assert( \
- std::is_convertible_v<decltype(expression), void>, \
- "Expression should be void.")
-#endif
-
-#endif // DRAKE_DOXYGEN_CXX
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
deleted file mode 100644
index b428474..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <stdexcept>
-#include <string>
-
-namespace drake {
-namespace internal {
-
-// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
-// configured to throw.
-class assertion_error : public std::runtime_error {
- public:
- explicit assertion_error(const std::string& what_arg)
- : std::runtime_error(what_arg) {}
-};
-
-} // namespace internal
-} // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
deleted file mode 100644
index a96a6fb..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#pragma once
-
-// ============================================================================
-// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
-// file must be kept in sync!
-// ============================================================================
-
-/** @file
-Provides careful macros to selectively enable or disable the special member
-functions for copy-construction, copy-assignment, move-construction, and
-move-assignment.
-
-http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
-
-When enabled via these macros, the `= default` implementation is provided.
-Code that needs custom copy or move functions should not use these macros.
-*/
-
-/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
-copy-construction, copy-assignment, move-construction, and move-assignment.
-Drake's Doxygen is customized to render the deletions in detail, with
-appropriate comments. Invoke this macro in the public section of the class
-declaration, e.g.:
-<pre>
-class Foo {
- public:
- DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
-
- // ...
-};
-</pre>
-*/
-#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname) \
- Classname(const Classname&) = delete; \
- void operator=(const Classname&) = delete; \
- Classname(Classname&&) = delete; \
- void operator=(Classname&&) = delete;
-
-/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
-functions for copy-construction, copy-assignment, move-construction, and
-move-assignment. This macro should be used only when copy-construction and
-copy-assignment defaults are well-formed. Note that the defaulted move
-functions could conceivably still be ill-formed, in which case they will
-effectively not be declared or used -- but because the copy constructor exists
-the type will still be MoveConstructible. Drake's Doxygen is customized to
-render the functions in detail, with appropriate comments. Typically, you
-should invoke this macro in the public section of the class declaration, e.g.:
-<pre>
-class Foo {
- public:
- DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
-
- // ...
-};
-</pre>
-
-However, if Foo has a virtual destructor (i.e., is subclassable), then
-typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the
-public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the
-protected section, to prevent
-<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
-
-The macro contains a built-in self-check that copy-assignment is well-formed.
-This self-check proves that the Classname is CopyConstructible, CopyAssignable,
-MoveConstructible, and MoveAssignable (in all but the most arcane cases where a
-member of the Classname is somehow CopyAssignable but not CopyConstructible).
-Therefore, classes that use this macro typically will not need to have any unit
-tests that check for the presence nor correctness of these functions.
-
-However, the self-check does not provide any checks of the runtime efficiency
-of the functions. If it is important for performance that the move functions
-actually move (instead of making a copy), then you should consider capturing
-that in a unit test.
-*/
-#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname) \
- Classname(const Classname&) = default; \
- Classname& operator=(const Classname&) = default; \
- Classname(Classname&&) = default; \
- Classname& operator=(Classname&&) = default; \
- /* Fails at compile-time if copy-assign doesn't compile. */ \
- /* Note that we do not test the copy-ctor here, because */ \
- /* it will not exist when Classname is abstract. */ \
- static void DrakeDefaultCopyAndMoveAndAssign_DoAssign( \
- Classname* a, const Classname& b) { *a = b; } \
- static_assert( \
- &DrakeDefaultCopyAndMoveAndAssign_DoAssign == \
- &DrakeDefaultCopyAndMoveAndAssign_DoAssign, \
- "This assertion is never false; its only purpose is to " \
- "generate 'use of deleted function: operator=' errors " \
- "when Classname is a template.");
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
deleted file mode 100644
index fdd07a2..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#pragma once
-
-#include <type_traits>
-
-#include "drake/common/drake_assert.h"
-
-/// @file
-/// Provides a convenient wrapper to throw an exception when a condition is
-/// unmet. This is similar to an assertion, but uses exceptions instead of
-/// ::abort(), and cannot be disabled.
-
-namespace drake {
-namespace internal {
-// Throw an error message.
-[[noreturn]] void Throw(const char* condition, const char* func,
- const char* file, int line);
-} // namespace internal
-} // namespace drake
-
-/// Evaluates @p condition and iff the value is false will throw an exception
-/// with a message showing at least the condition text, function name, file,
-/// and line.
-///
-/// The condition must not be a pointer, where we'd implicitly rely on its
-/// nullness. Instead, always write out "!= nullptr" to be precise.
-///
-/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);`
-/// Incorrect: `DRAKE_THROW_UNLESS(foo);`
-///
-/// Because this macro is intended to provide a useful exception message to
-/// users, we should err on the side of extra detail about the failure. The
-/// meaning of "foo" isolated within error message text does not make it
-/// clear that a null pointer is the proximate cause of the problem.
-#define DRAKE_THROW_UNLESS(condition) \
- do { \
- typedef ::drake::assert::ConditionTraits< \
- typename std::remove_cv_t<decltype(condition)>> Trait; \
- static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
- static_assert( \
- !std::is_pointer_v<decltype(condition)>, \
- "When using DRAKE_THROW_UNLESS on a raw pointer, always write out " \
- "DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \
- "(foo) and rely on implicit pointer-to-bool conversion."); \
- if (!Trait::Evaluate(condition)) { \
- ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \
- } \
- } while (0)
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
deleted file mode 100644
index b3f369c..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
+++ /dev/null
@@ -1,62 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include <Eigen/Core>
-
-namespace drake {
-
-/// Returns true if and only if the two matrices are equal to within a certain
-/// absolute elementwise @p tolerance. Special values (infinities, NaN, etc.)
-/// do not compare as equal elements.
-template <typename DerivedA, typename DerivedB>
-bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
- const Eigen::MatrixBase<DerivedB>& m2,
- double tolerance) {
- return (
- (m1.rows() == m2.rows()) &&
- (m1.cols() == m2.cols()) &&
- ((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
-}
-
-/// Returns true if and only if a simple greedy search reveals a permutation
-/// of the columns of m2 to make the matrix equal to m1 to within a certain
-/// absolute elementwise @p tolerance. E.g., there exists a P such that
-/// <pre>
-/// forall i,j, |m1 - m2*P|_{i,j} <= tolerance
-/// where P is a permutation matrix:
-/// P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
-/// </pre>
-/// Note: Returns false for matrices of different sizes.
-/// Note: The current implementation is O(n^2) in the number of columns.
-/// Note: In marginal cases (with similar but not identical columns) this
-/// algorithm can fail to find a permutation P even if it exists because it
-/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
-/// pool. It is possible that other columns of m2 would also match m1(i) but
-/// that m2(j) is the only match possible for a later column of m1.
-template <typename DerivedA, typename DerivedB>
-bool IsApproxEqualAbsTolWithPermutedColumns(
- const Eigen::MatrixBase<DerivedA>& m1,
- const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
- if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
-
- std::vector<bool> available(m2.cols());
- for (int i = 0; i < m2.cols(); i++) available[i] = true;
-
- for (int i = 0; i < m1.cols(); i++) {
- bool found_match = false;
- for (int j = 0; j < m2.cols(); j++) {
- if (available[j] &&
- is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
- found_match = true;
- available[j] = false;
- break;
- }
- }
- if (!found_match) return false;
- }
- return true;
-}
-
-
-} // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
deleted file mode 100644
index 024b355..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
+++ /dev/null
@@ -1,103 +0,0 @@
-#pragma once
-
-#include <new>
-#include <type_traits>
-#include <utility>
-
-#include "drake/common/drake_copyable.h"
-
-namespace drake {
-
-/// Wraps an underlying type T such that its storage is a direct member field
-/// of this object (i.e., without any indirection into the heap), but *unlike*
-/// most member fields T's destructor is never invoked.
-///
-/// This is especially useful for function-local static variables that are not
-/// trivially destructable. We shouldn't call their destructor at program exit
-/// because of the "indeterminate order of ... destruction" as mentioned in
-/// cppguide's
-/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
-/// and Global Variables</a> section, but other solutions to this problem place
-/// the objects on the heap through an indirection.
-///
-/// Compared with other approaches, this mechanism more clearly describes the
-/// intent to readers, avoids "possible leak" warnings from memory-checking
-/// tools, and is probably slightly faster.
-///
-/// Example uses:
-///
-/// The singleton pattern:
-/// @code
-/// class Singleton {
-/// public:
-/// DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton)
-/// static Singleton& getInstance() {
-/// static never_destroyed<Singleton> instance;
-/// return instance.access();
-/// }
-/// private:
-/// friend never_destroyed<Singleton>;
-/// Singleton() = default;
-/// };
-/// @endcode
-///
-/// A lookup table, created on demand the first time its needed, and then
-/// reused thereafter:
-/// @code
-/// enum class Foo { kBar, kBaz };
-/// Foo ParseFoo(const std::string& foo_string) {
-/// using Dict = std::unordered_map<std::string, Foo>;
-/// static const drake::never_destroyed<Dict> string_to_enum{
-/// std::initializer_list<Dict::value_type>{
-/// {"bar", Foo::kBar},
-/// {"baz", Foo::kBaz},
-/// }
-/// };
-/// return string_to_enum.access().at(foo_string);
-/// }
-/// @endcode
-///
-/// In cases where computing the static data is more complicated than an
-/// initializer_list, you can use a temporary lambda to populate the value:
-/// @code
-/// const std::vector<double>& GetConstantMagicNumbers() {
-/// static const drake::never_destroyed<std::vector<double>> result{[]() {
-/// std::vector<double> prototype;
-/// std::mt19937 random_generator;
-/// for (int i = 0; i < 10; ++i) {
-/// double new_value = random_generator();
-/// prototype.push_back(new_value);
-/// }
-/// return prototype;
-/// }()};
-/// return result.access();
-/// }
-/// @endcode
-///
-/// Note in particular the `()` after the lambda. That causes it to be invoked.
-//
-// The above examples are repeated in the unit test; keep them in sync.
-template <typename T>
-class never_destroyed {
- public:
- DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
-
- /// Passes the constructor arguments along to T using perfect forwarding.
- template <typename... Args>
- explicit never_destroyed(Args&&... args) {
- // Uses "placement new" to construct a `T` in `storage_`.
- new (&storage_) T(std::forward<Args>(args)...);
- }
-
- /// Does nothing. Guaranteed!
- ~never_destroyed() = default;
-
- /// Returns the underlying T reference.
- T& access() { return *reinterpret_cast<T*>(&storage_); }
- const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
-
- private:
- typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
-};
-
-} // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
deleted file mode 100644
index 55b8442..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#pragma once
-
-#include <cmath>
-#include <cstdlib>
-
-#include <Eigen/Core>
-#include <wpi/SymbolExports.h>
-
-namespace drake {
-namespace math {
-
-/**
-Computes the unique stabilizing solution X to the discrete-time algebraic
-Riccati equation:
-
-AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
-
-@throws std::exception if Q is not positive semi-definite.
-@throws std::exception if R is not positive definite.
-
-Based on the Schur Vector approach outlined in this paper:
-"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
-by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
-*/
-WPILIB_DLLEXPORT
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
- const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& B,
- const Eigen::Ref<const Eigen::MatrixXd>& Q,
- const Eigen::Ref<const Eigen::MatrixXd>& R);
-
-/**
-Computes the unique stabilizing solution X to the discrete-time algebraic
-Riccati equation:
-
-AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
-
-This is equivalent to solving the original DARE:
-
-A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
-
-where A₂ and Q₂ are a change of variables:
-
-A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
-
-This overload of the DARE is useful for finding the control law uₖ that
-minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
-
-@verbatim
- ∞ [xₖ]ᵀ[Q N][xₖ]
-J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
- k=0
-@endverbatim
-
-This is a more general form of the following. The linear-quadratic regulator
-is the feedback control law uₖ that minimizes the following cost function
-subject to xₖ₊₁ = Axₖ + Buₖ:
-
-@verbatim
- ∞
-J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
- k=0
-@endverbatim
-
-This can be refactored as:
-
-@verbatim
- ∞ [xₖ]ᵀ[Q 0][xₖ]
-J = Σ [uₖ] [0 R][uₖ] ΔT
- k=0
-@endverbatim
-
-@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
-@throws std::runtime_error if R is not positive definite.
-*/
-WPILIB_DLLEXPORT
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
- const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& B,
- const Eigen::Ref<const Eigen::MatrixXd>& Q,
- const Eigen::Ref<const Eigen::MatrixXd>& R,
- const Eigen::Ref<const Eigen::MatrixXd>& N);
-} // namespace math
-} // namespace drake
-
diff --git a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
deleted file mode 100644
index 88e7e66..0000000
--- a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-// This file contains the implementation of both drake_assert and drake_throw.
-/* clang-format off to disable clang-format-includes */
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-/* clang-format on */
-
-#include <atomic>
-#include <cstdlib>
-#include <iostream>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-
-#include "drake/common/drake_assertion_error.h"
-#include "drake/common/never_destroyed.h"
-
-namespace drake {
-namespace internal {
-namespace {
-
-// Singleton to manage assertion configuration.
-struct AssertionConfig {
- static AssertionConfig& singleton() {
- static never_destroyed<AssertionConfig> global;
- return global.access();
- }
-
- std::atomic<bool> assertion_failures_are_exceptions;
-};
-
-// Stream into @p out the given failure details; only @p condition may be null.
-void PrintFailureDetailTo(std::ostream& out, const char* condition,
- const char* func, const char* file, int line) {
- out << "Failure at " << file << ":" << line << " in " << func << "()";
- if (condition) {
- out << ": condition '" << condition << "' failed.";
- } else {
- out << ".";
- }
-}
-} // namespace
-
-// Declared in drake_assert.h.
-void Abort(const char* condition, const char* func, const char* file,
- int line) {
- std::cerr << "abort: ";
- PrintFailureDetailTo(std::cerr, condition, func, file, line);
- std::cerr << std::endl;
- std::abort();
-}
-
-// Declared in drake_throw.h.
-void Throw(const char* condition, const char* func, const char* file,
- int line) {
- std::ostringstream what;
- PrintFailureDetailTo(what, condition, func, file, line);
- throw assertion_error(what.str().c_str());
-}
-
-// Declared in drake_assert.h.
-void AssertionFailed(const char* condition, const char* func, const char* file,
- int line) {
- if (AssertionConfig::singleton().assertion_failures_are_exceptions) {
- Throw(condition, func, file, line);
- } else {
- Abort(condition, func, file, line);
- }
-}
-
-} // namespace internal
-} // namespace drake
-
-// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling
-// behavior.
-//
-// By default, assertion failures will result in an ::abort(). If this method
-// has ever been called, failures will result in a thrown exception instead.
-//
-// Assertion configuration has process-wide scope. Changes here will affect
-// all assertions within the current process.
-//
-// This method is intended ONLY for use by pydrake bindings, and thus is not
-// declared in any header file, to discourage anyone from using it.
-extern "C" void drake_set_assertion_failure_to_throw_exception() {
- drake::internal::AssertionConfig::singleton().
- assertion_failures_are_exceptions = true;
-}
diff --git a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
deleted file mode 100644
index 20ea2b7..0000000
--- a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
+++ /dev/null
@@ -1,475 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Eigenvalues>
-#include <Eigen/QR>
-
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-#include "drake/common/is_approx_equal_abstol.h"
-
-namespace drake {
-namespace math {
-namespace {
-/* helper functions */
-template <typename T>
-int sgn(T val) {
- return (T(0) < val) - (val < T(0));
-}
-void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& B) {
- // This function checks 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[lambda * I - A, B] < n.
- int n = B.rows(), m = B.cols();
- Eigen::EigenSolver<Eigen::MatrixXd> es(A);
- for (int i = 0; i < n; i++) {
- if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
- es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
- 1)
- continue;
- Eigen::MatrixXcd E(n, n + m);
- E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
- Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
- DRAKE_THROW_UNLESS(qr.rank() == n);
- }
-}
-void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& Q) {
- // This function check if (A,C) is a detectable pair, where Q = C' * C.
- // (A,C) is detectable if and only if the unobservable eigenvalues of A,
- // if any, have absolute values less than one, where an eigenvalue is
- // unobservable if Rank[lambda * I - A; C] < n.
- // Also, (A,C) is detectable if and only if (A',C') is stabilizable.
- int n = A.rows();
- Eigen::LDLT<Eigen::MatrixXd> ldlt(Q);
- Eigen::MatrixXd L = ldlt.matrixL();
- Eigen::MatrixXd D = ldlt.vectorD();
- Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n);
- for (int i = 0; i < n; i++) {
- D_sqrt(i, i) = sqrt(D(i));
- }
- Eigen::MatrixXd C = L * D_sqrt;
- check_stabilizable(A.transpose(), C.transpose());
-}
-
-// "Givens rotation" computes an orthogonal 2x2 matrix R such that
-// it eliminates the 2nd coordinate of the vector [a,b]', i.e.,
-// R * [ a ] = [ a_hat ]
-// [ b ] [ 0 ]
-// The implementation is based on
-// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation
-void Givens_rotation(double a, double b, Eigen::Ref<Eigen::Matrix2d> R,
- double eps = 1e-10) {
- double c, s;
- if (fabs(b) < eps) {
- c = (a < -eps ? -1 : 1);
- s = 0;
- } else if (fabs(a) < eps) {
- c = 0;
- s = -sgn(b);
- } else if (fabs(a) > fabs(b)) {
- double t = b / a;
- double u = sgn(a) * fabs(sqrt(1 + t * t));
- c = 1 / u;
- s = -c * t;
- } else {
- double t = a / b;
- double u = sgn(b) * fabs(sqrt(1 + t * t));
- s = -1 / u;
- c = -s * t;
- }
- R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_11(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
- Eigen::Ref<Eigen::MatrixXd> Z, int p) {
- // Dooren, Case I, p124-125.
- int n2 = S.rows();
- Eigen::Matrix2d A = S.block<2, 2>(p, p);
- Eigen::Matrix2d B = T.block<2, 2>(p, p);
- Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A;
- Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p));
- S = (S * Z1).eval();
- T = (T * Z1).eval();
- Z = (Z * Z1).eval();
- Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2);
- Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p));
- S = (Q * S).eval();
- T = (Q * T).eval();
- S(p + 1, p) = 0;
- T(p + 1, p) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_21(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
- Eigen::Ref<Eigen::MatrixXd> Z, int p) {
- // Dooren, Case II, p126-127.
- int n2 = S.rows();
- Eigen::Matrix3d A = S.block<3, 3>(p, p);
- Eigen::Matrix3d B = T.block<3, 3>(p, p);
- // Compute H and eliminate H(1,0) by row operation.
- Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A;
- Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
- Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0));
- H = (R * H).eval();
- Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
- // Compute Z1, Z2, Q1, Q2.
- Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1));
- H = (H * Z1.block<3, 3>(p, p)).eval();
- Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p));
- S = (S * Z1).eval();
- T = (T * Z1).eval();
- Z = (Z * Z1 * Z2).eval();
- Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1),
- Q1.block<2, 2>(p + 1, p + 1));
- S = (Q1 * S * Z2).eval();
- T = (Q1 * T * Z2).eval();
- Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p));
- S = (Q2 * S).eval();
- T = (Q2 * T).eval();
- S(p + 1, p) = 0;
- S(p + 2, p) = 0;
- T(p + 1, p) = 0;
- T(p + 2, p) = 0;
- T(p + 2, p + 1) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_12(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
- Eigen::Ref<Eigen::MatrixXd> Z, int p) {
- int n2 = S.rows();
- // Swap the role of S and T.
- Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
- Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1),
- Q0.block<2, 2>(p + 1, p + 1));
- S = (Q0 * S).eval();
- T = (Q0 * T).eval();
- Eigen::Matrix3d A = S.block<3, 3>(p, p);
- Eigen::Matrix3d B = T.block<3, 3>(p, p);
- // Compute H and eliminate H(2,1) by column operation.
- Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B;
- Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
- Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1));
- H = (H * R).eval();
- // Compute Q1, Q2, Z1, Z2.
- Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p));
- H = (Q1.block<3, 3>(p, p) * H).eval();
- Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1));
- S = (Q1 * S).eval();
- T = (Q1 * T).eval();
- Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p));
- S = (Q2 * S * Z1).eval();
- T = (Q2 * T * Z1).eval();
- Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1),
- Z2.block<2, 2>(p + 1, p + 1));
- S = (S * Z2).eval();
- T = (T * Z2).eval();
- Z = (Z * Z1 * Z2).eval();
- // Swap back the role of S and T.
- Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
- S = (Q3 * S).eval();
- T = (Q3 * T).eval();
- S(p + 2, p) = 0;
- S(p + 2, p + 1) = 0;
- T(p + 1, p) = 0;
- T(p + 2, p) = 0;
- T(p + 2, p + 1) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_22(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
- Eigen::Ref<Eigen::MatrixXd> Z, int p) {
- // Direct Swapping Algorithm based on
- // "Numerical Methods for General and Structured Eigenvalue Problems" by
- // Daniel Kressner, p108-111.
- // ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ).
- // Also relevant but not applicable here:
- // "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James
- // W. Demmelt;
- int n2 = S.rows();
- Eigen::MatrixXd A = S.block<4, 4>(p, p);
- Eigen::MatrixXd B = T.block<4, 4>(p, p);
- // Solve
- // A11 * X - Y A22 = A12
- // B11 * X - Y B22 = B12
- // Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4].
- Eigen::Matrix<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
- Eigen::Matrix<double, 8, 1> D;
- // clang-format off
- C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0,
- 0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0,
- A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2),
- 0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3),
- B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0,
- 0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0,
- B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2),
- 0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3);
- // clang-format on
- D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3);
- Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D);
- // Q * [ -Y ] = [ R_Y ] , Z' * [ -X ] = [ R_X ] .
- // [ I ] [ 0 ] [ I ] = [ 0 ]
- Eigen::Matrix<double, 4, 2> X, Y;
- X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity();
- Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity();
- Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
- Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr1(X);
- Z1.block<4, 4>(p, p) = qr1.householderQ();
- Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr2(Y);
- Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint();
- // Apply transform Q1 * (S,T) * Z1.
- S = (Q1 * S * Z1).eval();
- T = (Q1 * T * Z1).eval();
- Z = (Z * Z1).eval();
- // Eliminate the T(p+3,p+2) entry.
- Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
- Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2),
- Q2.block<2, 2>(p + 2, p + 2));
- S = (Q2 * S).eval();
- T = (Q2 * T).eval();
- // Eliminate the T(p+1,p) entry.
- Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
- Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
- S = (Q3 * S).eval();
- T = (Q3 * T).eval();
- S(p + 2, p) = 0;
- S(p + 2, p + 1) = 0;
- S(p + 3, p) = 0;
- S(p + 3, p + 1) = 0;
- T(p + 1, p) = 0;
- T(p + 2, p) = 0;
- T(p + 2, p + 1) = 0;
- T(p + 3, p) = 0;
- T(p + 3, p + 1) = 0;
- T(p + 3, p + 2) = 0;
-}
-
-// Functionality of "swap_block" function:
-// swap the 1x1 or 2x2 blocks pointed by p and q.
-// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1
-// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices.
-// Algorithms are described in the papers
-// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
-// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and
-// "Numerical Methods for General and Structured Eigenvalue Problems" by
-// Daniel Kressner, 2005.
-void swap_block(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
- Eigen::Ref<Eigen::MatrixXd> Z, int p, int q, int q_block_size,
- double eps = 1e-10) {
- int p_tmp = q, p_block_size;
- while (p_tmp-- > p) {
- p_block_size = 1;
- if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) {
- p_block_size = 2;
- p_tmp--;
- }
- switch (p_block_size * 10 + q_block_size) {
- case 11:
- swap_block_11(S, T, Z, p_tmp);
- break;
- case 21:
- swap_block_21(S, T, Z, p_tmp);
- break;
- case 12:
- swap_block_12(S, T, Z, p_tmp);
- break;
- case 22:
- swap_block_22(S, T, Z, p_tmp);
- break;
- }
- }
-}
-
-// Functionality of "reorder_eigen" function:
-// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has
-// stable eigenvalues by multiplying Q's and Z's on the left and the right,
-// respectively.
-// Stable eigenvalues are inside the unit disk.
-//
-// Algorithm:
-// Go along the diagonals of (S,T) from the top left to the bottom right.
-// Once find a stable eigenvalue, push it to top left.
-// In implementation, use two pointers, p and q.
-// p points to the current block (1x1 or 2x2) and q points to the block with the
-// stable eigenvalue(s).
-// Push the block pointed by q to the position pointed by p.
-// Finish when n stable eigenvalues are placed at the top-left n by n matrix.
-// The algorithm for swapping blocks is described in the papers
-// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
-// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue
-// Problems" by Daniel Kressner, 2005.
-void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
- Eigen::Ref<Eigen::MatrixXd> Z, double eps = 1e-10) {
- // abs(a) < eps => a = 0
- int n2 = S.rows();
- int n = n2 / 2, p = 0, q = 0;
-
- // Find the first unstable p block.
- while (p < n) {
- if (fabs(S(p + 1, p)) < eps) { // p block size = 1
- if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) { // stable
- p++;
- continue;
- }
- } else { // p block size = 2
- const double det_T =
- T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1);
- if (fabs(det_T) > eps) {
- const double det_S =
- S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1);
- if (fabs(det_S) <= fabs(det_T)) { // stable
- p += 2;
- continue;
- }
- }
- }
- break;
- }
- q = p;
-
- // Make the first n generalized eigenvalues stable.
- while (p < n && q < n2) {
- // Update q.
- int q_block_size = 0;
- while (q < n2) {
- if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) { // block size = 1
- if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) {
- q_block_size = 1;
- break;
- }
- q++;
- } else { // block size = 2
- const double det_T =
- T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1);
- if (fabs(det_T) > eps) {
- const double det_S =
- S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1);
- if (fabs(det_S) <= fabs(det_T)) {
- q_block_size = 2;
- break;
- }
- }
- q += 2;
- }
- }
- if (q >= n2)
- throw std::runtime_error("fail to find enough stable eigenvalues");
- // Swap blocks pointed by p and q.
- if (p != q) {
- swap_block(S, T, Z, p, q, q_block_size);
- p += q_block_size;
- q += q_block_size;
- }
- }
- if (p < n && q >= n2)
- throw std::runtime_error("fail to find enough stable eigenvalues");
-}
-} // namespace
-
-/**
- * DiscreteAlgebraicRiccatiEquation function
- * computes the unique stabilizing solution X to the discrete-time algebraic
- * Riccati equation:
- *
- * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
- *
- * @throws std::exception if Q is not positive semi-definite.
- * @throws std::exception if R is not positive definite.
- *
- * Based on the Schur Vector approach outlined in this paper:
- * "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
- * by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980,
- * http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434
- *
- * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
- * R_half are sampled from standard normal distributions, where
- * Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution
- * is 10⁻⁶, while the absolute error of the solution computed by Matlab is
- * 10⁻⁸.
- *
- * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
- * accuracy, together with more thorough tests.
- */
-
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
- const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& B,
- const Eigen::Ref<const Eigen::MatrixXd>& Q,
- const Eigen::Ref<const Eigen::MatrixXd>& R) {
- int n = B.rows(), m = B.cols();
-
- DRAKE_DEMAND(m <= n);
- DRAKE_DEMAND(A.rows() == n && A.cols() == n);
- DRAKE_DEMAND(Q.rows() == n && Q.cols() == n);
- DRAKE_DEMAND(R.rows() == m && R.cols() == m);
- DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10));
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Q);
- for (int i = 0; i < n; i++) {
- DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0);
- }
- DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10));
- Eigen::LLT<Eigen::MatrixXd> R_cholesky(R);
- DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success);
- check_stabilizable(A, B);
- check_detectable(A, Q);
-
- Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n);
- M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n);
- L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(),
- Eigen::MatrixXd::Zero(n, n), A.transpose();
-
- // QZ decomposition of M and L
- // QMZ = S, QLZ = T
- // where Q and Z are real orthogonal matrixes
- // T is upper-triangular matrix, and S is upper quasi-triangular matrix
- Eigen::RealQZ<Eigen::MatrixXd> qz(2 * n);
- qz.compute(M, L); // M = Q S Z, L = Q T Z (Q and Z computed by Eigen package
- // are adjoints of Q and Z above)
- Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(),
- Z = qz.matrixZ().adjoint();
-
- // Reorder the generalized eigenvalues of (S,T).
- Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n);
- reorder_eigen(S, T, Z2);
- Z = (Z * Z2).eval();
-
- // The first n columns of Z is ( U1 ) .
- // ( U2 )
- // -1
- // X = U2 * U1 is a solution of the discrete time Riccati equation.
- Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n);
- Eigen::MatrixXd X = U2 * U1.inverse();
- X = (X + X.adjoint().eval()) / 2.0;
- return X;
-}
-
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
- const Eigen::Ref<const Eigen::MatrixXd>& A,
- const Eigen::Ref<const Eigen::MatrixXd>& B,
- const Eigen::Ref<const Eigen::MatrixXd>& Q,
- const Eigen::Ref<const Eigen::MatrixXd>& R,
- const Eigen::Ref<const Eigen::MatrixXd>& N) {
- DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
-
- // This is a change of variables to make the DARE that includes Q, R, and N
- // cost matrices fit the form of the DARE that includes only Q and R cost
- // matrices.
- Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
- Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
- return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
-}
-
-} // namespace math
-} // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
deleted file mode 100644
index bc68397..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
+++ /dev/null
@@ -1,27 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDDEQUE_MODULE_H
-#define EIGEN_STDDEQUE_MODULE_H
-
-#include "Core"
-#include <deque>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdDeque.h"
-
-#endif
-
-#endif // EIGEN_STDDEQUE_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
deleted file mode 100644
index 4c6262c..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
+++ /dev/null
@@ -1,26 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDLIST_MODULE_H
-#define EIGEN_STDLIST_MODULE_H
-
-#include "Core"
-#include <list>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdList.h"
-
-#endif
-
-#endif // EIGEN_STDLIST_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
deleted file mode 100644
index 0c4697a..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
+++ /dev/null
@@ -1,27 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDVECTOR_MODULE_H
-#define EIGEN_STDVECTOR_MODULE_H
-
-#include "Core"
-#include <vector>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdVector.h"
-
-#endif
-
-#endif // EIGEN_STDVECTOR_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
index d973255..9a630e4 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -78,7 +78,7 @@
// This warning is a false positive
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
- #if __GNUC__==12
+ #if __GNUC__>=12
// This warning is a false positive
#pragma GCC diagnostic ignored "-Warray-bounds"
#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
index 986c3d4..81986b9 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
@@ -58,6 +58,16 @@
// Compiler identification, EIGEN_COMP_*
//------------------------------------------------------------------------------------------
+/// \internal Disable NEON features in Intellisense
+#if __INTELLISENSE__
+#ifdef __ARM_NEON
+#undef __ARM_NEON
+#endif
+#ifdef __ARM_NEON__
+#undef __ARM_NEON__
+#endif
+#endif
+
/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
#ifdef __GNUC__
#define EIGEN_COMP_GNUC (__GNUC__*10+__GNUC_MINOR__)
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
index f9c56ba..7cb2c26 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
@@ -270,11 +270,11 @@
}
- Index count = 0;
+// Index count = 0;
// FIXME compute a reference value to filter zeros
for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
{
- ++ count;
+// ++ count;
// std::cerr << "fill " << it.index() << ", " << col << "\n";
// std::cout << it.value() << " ";
// FIXME use insertBack
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
index 6f75d50..7aecbca 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -75,8 +75,6 @@
// Identify the relaxed supernodes by postorder traversal of the etree
Index snode_start; // beginning of a snode
StorageIndex k;
- Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree
- Index nsuper_et = 0; // Number of relaxed snodes in the original etree
StorageIndex l;
for (j = 0; j < n; )
{
@@ -88,7 +86,6 @@
parent = et(j);
}
// Found a supernode in postordered etree, j is the last column
- ++nsuper_et_post;
k = StorageIndex(n);
for (Index i = snode_start; i <= j; ++i)
k = (std::min)(k, inv_post(i));
@@ -97,7 +94,6 @@
{
// This is also a supernode in the original etree
relax_end(k) = l; // Record last column
- ++nsuper_et;
}
else
{
@@ -107,7 +103,6 @@
if (descendants(i) == 0)
{
relax_end(l) = l;
- ++nsuper_et;
}
}
}
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
deleted file mode 100644
index 6d47e75..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
+++ /dev/null
@@ -1,116 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDDEQUE_H
-#define EIGEN_STDDEQUE_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::deque such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \
-namespace std \
-{ \
- template<> \
- class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
- : public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
- { \
- typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \
- public: \
- typedef __VA_ARGS__ value_type; \
- typedef deque_base::allocator_type allocator_type; \
- typedef deque_base::size_type size_type; \
- typedef deque_base::iterator iterator; \
- explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {} \
- template<typename InputIterator> \
- deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \
- deque(const deque& c) : deque_base(c) {} \
- explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
- deque(iterator start_, iterator end_) : deque_base(start_, end_) {} \
- deque& operator=(const deque& x) { \
- deque_base::operator=(x); \
- return *this; \
- } \
- }; \
-}
-
-// check whether we really need the std::deque specialization
-#if !EIGEN_HAS_CXX11_CONTAINERS && !(defined(_GLIBCXX_DEQUE) && (!EIGEN_GNUC_AT_LEAST(4,1))) /* Note that before gcc-4.1 we already have: std::deque::resize(size_type,const T&). */
-
-namespace std {
-
-#define EIGEN_STD_DEQUE_SPECIALIZATION_BODY \
- public: \
- typedef T value_type; \
- typedef typename deque_base::allocator_type allocator_type; \
- typedef typename deque_base::size_type size_type; \
- typedef typename deque_base::iterator iterator; \
- typedef typename deque_base::const_iterator const_iterator; \
- explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {} \
- template<typename InputIterator> \
- deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
- : deque_base(first, last, a) {} \
- deque(const deque& c) : deque_base(c) {} \
- explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
- deque(iterator start_, iterator end_) : deque_base(start_, end_) {} \
- deque& operator=(const deque& x) { \
- deque_base::operator=(x); \
- return *this; \
- }
-
- template<typename T>
- class deque<T,EIGEN_ALIGNED_ALLOCATOR<T> >
- : public deque<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
- Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-{
- typedef deque<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
- Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > deque_base;
- EIGEN_STD_DEQUE_SPECIALIZATION_BODY
-
- void resize(size_type new_size)
- { resize(new_size, T()); }
-
-#if defined(_DEQUE_)
- // workaround MSVC std::deque implementation
- void resize(size_type new_size, const value_type& x)
- {
- if (deque_base::size() < new_size)
- deque_base::_Insert_n(deque_base::end(), new_size - deque_base::size(), x);
- else if (new_size < deque_base::size())
- deque_base::erase(deque_base::begin() + new_size, deque_base::end());
- }
- void push_back(const value_type& x)
- { deque_base::push_back(x); }
- void push_front(const value_type& x)
- { deque_base::push_front(x); }
- using deque_base::insert;
- iterator insert(const_iterator position, const value_type& x)
- { return deque_base::insert(position,x); }
- void insert(const_iterator position, size_type new_size, const value_type& x)
- { deque_base::insert(position, new_size, x); }
-#else
- // default implementation which should always work.
- void resize(size_type new_size, const value_type& x)
- {
- if (new_size < deque_base::size())
- deque_base::erase(deque_base::begin() + new_size, deque_base::end());
- else if (new_size > deque_base::size())
- deque_base::insert(deque_base::end(), new_size - deque_base::size(), x);
- }
-#endif
- };
-}
-
-#endif // check whether specialization is actually required
-
-#endif // EIGEN_STDDEQUE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
deleted file mode 100644
index 8ba3fad..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
+++ /dev/null
@@ -1,106 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDLIST_H
-#define EIGEN_STDLIST_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::list such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \
-namespace std \
-{ \
- template<> \
- class list<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
- : public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
- { \
- typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \
- public: \
- typedef __VA_ARGS__ value_type; \
- typedef list_base::allocator_type allocator_type; \
- typedef list_base::size_type size_type; \
- typedef list_base::iterator iterator; \
- explicit list(const allocator_type& a = allocator_type()) : list_base(a) {} \
- template<typename InputIterator> \
- list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \
- list(const list& c) : list_base(c) {} \
- explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
- list(iterator start_, iterator end_) : list_base(start_, end_) {} \
- list& operator=(const list& x) { \
- list_base::operator=(x); \
- return *this; \
- } \
- }; \
-}
-
-// check whether we really need the std::list specialization
-#if !EIGEN_HAS_CXX11_CONTAINERS && !(defined(_GLIBCXX_LIST) && (!EIGEN_GNUC_AT_LEAST(4,1))) /* Note that before gcc-4.1 we already have: std::list::resize(size_type,const T&). */
-
-namespace std
-{
-
-#define EIGEN_STD_LIST_SPECIALIZATION_BODY \
- public: \
- typedef T value_type; \
- typedef typename list_base::allocator_type allocator_type; \
- typedef typename list_base::size_type size_type; \
- typedef typename list_base::iterator iterator; \
- typedef typename list_base::const_iterator const_iterator; \
- explicit list(const allocator_type& a = allocator_type()) : list_base(a) {} \
- template<typename InputIterator> \
- list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
- : list_base(first, last, a) {} \
- list(const list& c) : list_base(c) {} \
- explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
- list(iterator start_, iterator end_) : list_base(start_, end_) {} \
- list& operator=(const list& x) { \
- list_base::operator=(x); \
- return *this; \
- }
-
- template<typename T>
- class list<T,EIGEN_ALIGNED_ALLOCATOR<T> >
- : public list<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
- Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
- {
- typedef list<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
- Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > list_base;
- EIGEN_STD_LIST_SPECIALIZATION_BODY
-
- void resize(size_type new_size)
- { resize(new_size, T()); }
-
- void resize(size_type new_size, const value_type& x)
- {
- if (list_base::size() < new_size)
- list_base::insert(list_base::end(), new_size - list_base::size(), x);
- else
- while (new_size < list_base::size()) list_base::pop_back();
- }
-
-#if defined(_LIST_)
- // workaround MSVC std::list implementation
- void push_back(const value_type& x)
- { list_base::push_back(x); }
- using list_base::insert;
- iterator insert(const_iterator position, const value_type& x)
- { return list_base::insert(position,x); }
- void insert(const_iterator position, size_type new_size, const value_type& x)
- { list_base::insert(position, new_size, x); }
-#endif
- };
-}
-
-#endif // check whether specialization is actually required
-
-#endif // EIGEN_STDLIST_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
deleted file mode 100644
index 9fcf19b..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
+++ /dev/null
@@ -1,131 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDVECTOR_H
-#define EIGEN_STDVECTOR_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::vector such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) \
-namespace std \
-{ \
- template<> \
- class vector<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
- : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
- { \
- typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > vector_base; \
- public: \
- typedef __VA_ARGS__ value_type; \
- typedef vector_base::allocator_type allocator_type; \
- typedef vector_base::size_type size_type; \
- typedef vector_base::iterator iterator; \
- explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {} \
- template<typename InputIterator> \
- vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : vector_base(first, last, a) {} \
- vector(const vector& c) : vector_base(c) {} \
- explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
- vector(iterator start_, iterator end_) : vector_base(start_, end_) {} \
- vector& operator=(const vector& x) { \
- vector_base::operator=(x); \
- return *this; \
- } \
- }; \
-}
-
-// Don't specialize if containers are implemented according to C++11
-#if !EIGEN_HAS_CXX11_CONTAINERS
-
-namespace std {
-
-#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
- public: \
- typedef T value_type; \
- typedef typename vector_base::allocator_type allocator_type; \
- typedef typename vector_base::size_type size_type; \
- typedef typename vector_base::iterator iterator; \
- typedef typename vector_base::const_iterator const_iterator; \
- explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {} \
- template<typename InputIterator> \
- vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
- : vector_base(first, last, a) {} \
- vector(const vector& c) : vector_base(c) {} \
- explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
- vector(iterator start_, iterator end_) : vector_base(start_, end_) {} \
- vector& operator=(const vector& x) { \
- vector_base::operator=(x); \
- return *this; \
- }
-
- template<typename T>
- class vector<T,EIGEN_ALIGNED_ALLOCATOR<T> >
- : public vector<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
- Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-{
- typedef vector<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
- Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > vector_base;
- EIGEN_STD_VECTOR_SPECIALIZATION_BODY
-
- void resize(size_type new_size)
- { resize(new_size, T()); }
-
-#if defined(_VECTOR_)
- // workaround MSVC std::vector implementation
- void resize(size_type new_size, const value_type& x)
- {
- if (vector_base::size() < new_size)
- vector_base::_Insert_n(vector_base::end(), new_size - vector_base::size(), x);
- else if (new_size < vector_base::size())
- vector_base::erase(vector_base::begin() + new_size, vector_base::end());
- }
- void push_back(const value_type& x)
- { vector_base::push_back(x); }
- using vector_base::insert;
- iterator insert(const_iterator position, const value_type& x)
- { return vector_base::insert(position,x); }
- void insert(const_iterator position, size_type new_size, const value_type& x)
- { vector_base::insert(position, new_size, x); }
-#elif defined(_GLIBCXX_VECTOR) && (!(EIGEN_GNUC_AT_LEAST(4,1)))
- /* Note that before gcc-4.1 we already have: std::vector::resize(size_type,const T&).
- * However, this specialization is still needed to make the above EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION trick to work. */
- void resize(size_type new_size, const value_type& x)
- {
- vector_base::resize(new_size,x);
- }
-#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
- // workaround GCC std::vector implementation
- void resize(size_type new_size, const value_type& x)
- {
- if (new_size < vector_base::size())
- vector_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
- else
- vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
- }
-#else
- // either GCC 4.1 or non-GCC
- // default implementation which should always work.
- void resize(size_type new_size, const value_type& x)
- {
- if (new_size < vector_base::size())
- vector_base::erase(vector_base::begin() + new_size, vector_base::end());
- else if (new_size > vector_base::size())
- vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
- }
-#endif
- };
-}
-#endif // !EIGEN_HAS_CXX11_CONTAINERS
-
-
-#endif // EIGEN_STDVECTOR_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
deleted file mode 100644
index 2cfd13e..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
+++ /dev/null
@@ -1,84 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STL_DETAILS_H
-#define EIGEN_STL_DETAILS_H
-
-#ifndef EIGEN_ALIGNED_ALLOCATOR
- #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator
-#endif
-
-namespace Eigen {
-
- // This one is needed to prevent reimplementing the whole std::vector.
- template <class T>
- class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR<T>
- {
- public:
- typedef std::size_t size_type;
- typedef std::ptrdiff_t difference_type;
- typedef T* pointer;
- typedef const T* const_pointer;
- typedef T& reference;
- typedef const T& const_reference;
- typedef T value_type;
-
- template<class U>
- struct rebind
- {
- typedef aligned_allocator_indirection<U> other;
- };
-
- aligned_allocator_indirection() {}
- aligned_allocator_indirection(const aligned_allocator_indirection& ) : EIGEN_ALIGNED_ALLOCATOR<T>() {}
- aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<T>& ) {}
- template<class U>
- aligned_allocator_indirection(const aligned_allocator_indirection<U>& ) {}
- template<class U>
- aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<U>& ) {}
- ~aligned_allocator_indirection() {}
- };
-
-#if EIGEN_COMP_MSVC
-
- // sometimes, MSVC detects, at compile time, that the argument x
- // in std::vector::resize(size_t s,T x) won't be aligned and generate an error
- // even if this function is never called. Whence this little wrapper.
-#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \
- typename Eigen::internal::conditional< \
- Eigen::internal::is_arithmetic<T>::value, \
- T, \
- Eigen::internal::workaround_msvc_stl_support<T> \
- >::type
-
- namespace internal {
- template<typename T> struct workaround_msvc_stl_support : public T
- {
- inline workaround_msvc_stl_support() : T() {}
- inline workaround_msvc_stl_support(const T& other) : T(other) {}
- inline operator T& () { return *static_cast<T*>(this); }
- inline operator const T& () const { return *static_cast<const T*>(this); }
- template<typename OtherT>
- inline T& operator=(const OtherT& other)
- { T::operator=(other); return *this; }
- inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other)
- { T::operator=(other); return *this; }
- };
- }
-
-#else
-
-#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T
-
-#endif
-
-}
-
-#endif // EIGEN_STL_DETAILS_H
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
index cb28ff0..650d05d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
index 0a3ada6..6d7b66d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
index 9d3bc07..a47003d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
index 79579d7..8767200 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
index 210d9fc..6a79e87 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
index dfad57e..a5f3ff6 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
index 9aea85b..3f46974 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
index 97c8d6a..5ca55b7 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
index 2e1cb76..dfb4dc3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
index e888e47..e43d4fc 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
index fb050a2..0fc17f3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
index e8570ab..ff1097b 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
index a3bab74..c1741f7 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
index 0e98012..82f4c60 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
index b3cebb8..fc89c0d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
index afec09e..d0bc83a 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
index a93f8db..412d686 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
index 0c66829..595ffc2 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
@@ -28,6 +28,30 @@
namespace internal
{
+// see https://en.wikipedia.org/wiki/Euler%27s_continued_fraction_formula
+
+#if __cplusplus >= 201402L // C++14 version
+
+template<typename T>
+constexpr
+T
+exp_cf_recur(const T x, const int depth_end)
+noexcept
+{
+ int depth = GCEM_EXP_MAX_ITER_SMALL - 1;
+ T res = T(1);
+
+ while (depth > depth_end - 1) {
+ res = T(1) + x/T(depth - 1) - x/depth/res;
+
+ --depth;
+ }
+
+ return res;
+}
+
+#else // C++11 version
+
template<typename T>
constexpr
T
@@ -36,20 +60,20 @@
{
return( depth < GCEM_EXP_MAX_ITER_SMALL ? \
// if
- depth == 1 ? \
- T(1) - x/exp_cf_recur(x,depth+1) :
- T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) :
+ T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) :
// else
T(1) );
}
+#endif
+
template<typename T>
constexpr
T
exp_cf(const T x)
noexcept
{
- return( T(1)/exp_cf_recur(x,1) );
+ return( T(1) / (T(1) - x / exp_cf_recur(x,2)) );
}
template<typename T>
@@ -72,7 +96,7 @@
//
is_neginf(x) ? \
T(0) :
- //
+ // indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(1) :
//
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
index 11b2eb9..70c9ecf 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
index 539c3f3..ffb9c82 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
index 710adce..200e4e9 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
@@ -34,10 +34,20 @@
find_exponent(const T x, const llint_t exponent)
noexcept
{
- return( x < T(1) ? \
- find_exponent(x*T(10),exponent - llint_t(1)) :
+ return( // < 1
+ x < T(1e-03) ? \
+ find_exponent(x * T(1e+04), exponent - llint_t(4)) :
+ x < T(1e-01) ? \
+ find_exponent(x * T(1e+02), exponent - llint_t(2)) :
+ x < T(1) ? \
+ find_exponent(x * T(10), exponent - llint_t(1)) :
+ // > 10
x > T(10) ? \
- find_exponent(x/T(10),exponent + llint_t(1)) :
+ find_exponent(x / T(10), exponent + llint_t(1)) :
+ x > T(1e+02) ? \
+ find_exponent(x / T(1e+02), exponent + llint_t(2)) :
+ x > T(1e+04) ? \
+ find_exponent(x / T(1e+04), exponent + llint_t(4)) :
// else
exponent );
}
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
index d9769e6..5ed3d26 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
index bd5e0b9..d193632 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
index c60ff6a..8b260ff 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
index c804ce6..02459ef 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
index 4a10bbe..1e277fb 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
index cd2747c..4113738 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
@@ -53,7 +53,7 @@
#endif
#ifndef GCEM_VERSION_MINOR
- #define GCEM_VERSION_MINOR 16
+ #define GCEM_VERSION_MINOR 17
#endif
#ifndef GCEM_VERSION_PATCH
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
index 5a805ed..01ad4e9 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
index 5645dbe..dbb9f60 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
index f7fdfa0..9f575a3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
index 38734a5..9ee4146 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
index 1e57fc1..e5976d0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
index 0200f11..d0e33fb 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
index fa925bb..de0641d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
index 25f2e3c..b632fa3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
index 627c509..568614f 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
index a7a1af3..a3fcbc6 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
index e6da720..a74a8d3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
index 2213849..60c87b4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
index b0d8fb4..a7ca776 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
index 5d78eb3..507c6d4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
index 76bf833..58915dc 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
index 0d83e97..c2e24b0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
@@ -31,6 +31,28 @@
// continued fraction seems to be a better approximation for small x
// see http://functions.wolfram.com/ElementaryFunctions/Log/10/0005/
+#if __cplusplus >= 201402L // C++14 version
+
+template<typename T>
+constexpr
+T
+log_cf_main(const T xx, const int depth_end)
+noexcept
+{
+ int depth = GCEM_LOG_MAX_ITER_SMALL - 1;
+ T res = T(2*(depth+1) - 1);
+
+ while (depth > depth_end - 1) {
+ res = T(2*depth - 1) - T(depth*depth) * xx / res;
+
+ --depth;
+ }
+
+ return res;
+}
+
+#else // C++11 version
+
template<typename T>
constexpr
T
@@ -39,11 +61,13 @@
{
return( depth < GCEM_LOG_MAX_ITER_SMALL ? \
// if
- T(2*depth - 1) - T(depth*depth)*xx/log_cf_main(xx,depth+1) :
+ T(2*depth - 1) - T(depth*depth) * xx / log_cf_main(xx,depth+1) :
// else
T(2*depth - 1) );
}
+#endif
+
template<typename T>
constexpr
T
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
index 4a3c37d..cda8894 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
index 3883b22..ccd08b8 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
index 56b7f8e..a97fed4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
index 7aa9a2b..2bcaadd 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
index df6152b..af23ea2 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
@@ -35,9 +35,9 @@
noexcept
{
return( x < T(1) ? \
- mantissa(x*T(10)) :
+ mantissa(x * T(10)) :
x > T(10) ? \
- mantissa(x/T(10)) :
+ mantissa(x / T(10)) :
// else
x );
}
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
index 4aed84f..ddc3e4e 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
index d593dbc..5ce70b3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
index db33f87..79d24a4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
index 166a8c1..3891ede 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
index 3a902ca..4e67155 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
index e609b89..295f43d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
index 44281f9..d4e448c 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
index 43d7a5e..9ac4a09 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
index 605a35a..e2eec9e 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
index e207a5a..282e244 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
index 128cd32..56c8dca 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
index 5355301..fe3ecdd 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
index 0fd559d..1b2753c 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
@@ -37,16 +37,37 @@
return( abs(xn - x/xn) / (T(1) + xn) < GCLIM<T>::min() ? \
// if
xn :
- count < GCEM_SQRT_MAX_ITER ? \
// else
- sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
- xn );
+ count < GCEM_SQRT_MAX_ITER ? \
+ // if
+ sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
+ // else
+ xn );
}
template<typename T>
constexpr
T
-sqrt_check(const T x, const T m_val)
+sqrt_simplify(const T x, const T m_val)
+noexcept
+{
+ return( x > T(1e+08) ? \
+ sqrt_simplify(x / T(1e+08), T(1e+04) * m_val) :
+ x > T(1e+06) ? \
+ sqrt_simplify(x / T(1e+06), T(1e+03) * m_val) :
+ x > T(1e+04) ? \
+ sqrt_simplify(x / T(1e+04), T(1e+02) * m_val) :
+ x > T(100) ? \
+ sqrt_simplify(x / T(100), T(10) * m_val) :
+ x > T(4) ? \
+ sqrt_simplify(x / T(4), T(2) * m_val) :
+ m_val * sqrt_recur(x, x / T(2), 0) );
+}
+
+template<typename T>
+constexpr
+T
+sqrt_check(const T x)
noexcept
{
return( is_nan(x) ? \
@@ -63,9 +84,7 @@
GCLIM<T>::min() > abs(T(1) - x) ? \
x :
// else
- x > T(4) ? \
- sqrt_check(x/T(4), T(2)*m_val) :
- m_val * sqrt_recur(x, x/T(2), 0) );
+ sqrt_simplify(x, T(1)) );
}
}
@@ -84,7 +103,7 @@
sqrt(const T x)
noexcept
{
- return internal::sqrt_check( static_cast<return_t<T>>(x), return_t<T>(1) );
+ return internal::sqrt_check( static_cast<return_t<T>>(x) );
}
#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
index e53f5c8..386cce0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
index 109d751..30b4318 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
index 5a9ae97..deffd3a 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
index 4e19ef9..af3f448 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
@@ -1,6 +1,6 @@
/*################################################################################
##
- ## Copyright (C) 2016-2022 Keith O'Hara
+ ## Copyright (C) 2016-2023 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##