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/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
new file mode 100644
index 0000000..ad07d05
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
@@ -0,0 +1,239 @@
+// 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.
+
+package edu.wpi.first.math;
+
+import org.ejml.simple.SimpleMatrix;
+
+public final class DARE {
+ private DARE() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * <p>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>R isn't symmetric positive definite
+ * <li>The (A, B) pair isn't stabilizable
+ * <li>The (A, C) pair where Q = CᵀC isn't detectable
+ * </ul>
+ *
+ * <p>Only use this function if you're sure the preconditions are met.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param A System matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @return Solution of DARE.
+ */
+ public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R) {
+ var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareDetailABQR(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
+ A.getNumCols(),
+ B.getNumCols(),
+ S.getStorage().getDDRM().getData());
+ return S;
+ }
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ * <p>This is equivalent to solving the original DARE:
+ *
+ * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+ *
+ * <p>where A₂ and Q₂ are a change of variables:
+ *
+ * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+ *
+ * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>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ₖ:
+ *
+ * <pre>
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>This can be refactored as:
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>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>R isn't symmetric positive definite
+ * <li>The (A₂, B) pair isn't stabilizable
+ * <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable
+ * </ul>
+ *
+ * <p>Only use this function if you're sure the preconditions are met.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param A System matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @param N State-input cross-term cost matrix.
+ * @return Solution of DARE.
+ */
+ public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R,
+ Matrix<States, Inputs> N) {
+ var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareDetailABQRN(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
+ N.getStorage().getDDRM().getData(),
+ A.getNumCols(),
+ B.getNumCols(),
+ S.getStorage().getDDRM().getData());
+ return S;
+ }
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param A System matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @return Solution of DARE.
+ * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+ * @throws IllegalArgumentException if R isn't symmetric positive definite.
+ * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+ * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+ */
+ public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R) {
+ var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareABQR(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
+ A.getNumCols(),
+ B.getNumCols(),
+ S.getStorage().getDDRM().getData());
+ return S;
+ }
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ * <p>This is equivalent to solving the original DARE:
+ *
+ * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+ *
+ * <p>where A₂ and Q₂ are a change of variables:
+ *
+ * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+ *
+ * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>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ₖ:
+ *
+ * <pre>
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>This can be refactored as:
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param A System matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @param N State-input cross-term cost matrix.
+ * @return Solution of DARE.
+ * @throws IllegalArgumentException if Q₂ isn't symmetric positive semidefinite.
+ * @throws IllegalArgumentException if R isn't symmetric positive definite.
+ * @throws IllegalArgumentException if the (A₂, B) pair isn't stabilizable.
+ * @throws IllegalArgumentException if the (A₂, C) pair where Q₂ = CᵀC isn't detectable.
+ */
+ public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R,
+ Matrix<States, Inputs> N) {
+ var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareABQRN(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
+ N.getStorage().getDDRM().getData(),
+ A.getNumCols(),
+ B.getNumCols(),
+ S.getStorage().getDDRM().getData());
+ return S;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
deleted file mode 100644
index 55bc5b2..0000000
--- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ /dev/null
@@ -1,116 +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.
-
-package edu.wpi.first.math;
-
-import org.ejml.simple.SimpleMatrix;
-
-public final class Drake {
- private Drake() {}
-
- /**
- * Solves the discrete algebraic Riccati equation.
- *
- * @param A System matrix.
- * @param B Input matrix.
- * @param Q State cost matrix.
- * @param R Input cost matrix.
- * @return Solution of DARE.
- */
- public static SimpleMatrix discreteAlgebraicRiccatiEquation(
- SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
- var S = new SimpleMatrix(A.numRows(), A.numCols());
- WPIMathJNI.discreteAlgebraicRiccatiEquation(
- A.getDDRM().getData(),
- B.getDDRM().getData(),
- Q.getDDRM().getData(),
- R.getDDRM().getData(),
- A.numCols(),
- B.numCols(),
- S.getDDRM().getData());
- return S;
- }
-
- /**
- * Solves the discrete algebraic Riccati equation.
- *
- * @param <States> Number of states.
- * @param <Inputs> Number of inputs.
- * @param A System matrix.
- * @param B Input matrix.
- * @param Q State cost matrix.
- * @param R Input cost matrix.
- * @return Solution of DARE.
- */
- public static <States extends Num, Inputs extends Num>
- Matrix<States, States> discreteAlgebraicRiccatiEquation(
- Matrix<States, States> A,
- Matrix<States, Inputs> B,
- Matrix<States, States> Q,
- Matrix<Inputs, Inputs> R) {
- return new Matrix<>(
- discreteAlgebraicRiccatiEquation(
- A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
- }
-
- /**
- * Solves the discrete algebraic Riccati equation.
- *
- * @param A System matrix.
- * @param B Input matrix.
- * @param Q State cost matrix.
- * @param R Input cost matrix.
- * @param N State-input cross-term cost matrix.
- * @return Solution of DARE.
- */
- public static SimpleMatrix discreteAlgebraicRiccatiEquation(
- SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
- // See
- // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
- // for the change of variables used here.
- var scrA = A.minus(B.mult(R.solve(N.transpose())));
- var scrQ = Q.minus(N.mult(R.solve(N.transpose())));
-
- var S = new SimpleMatrix(A.numRows(), A.numCols());
- WPIMathJNI.discreteAlgebraicRiccatiEquation(
- scrA.getDDRM().getData(),
- B.getDDRM().getData(),
- scrQ.getDDRM().getData(),
- R.getDDRM().getData(),
- A.numCols(),
- B.numCols(),
- S.getDDRM().getData());
- return S;
- }
-
- /**
- * Solves the discrete algebraic Riccati equation.
- *
- * @param <States> Number of states.
- * @param <Inputs> Number of inputs.
- * @param A System matrix.
- * @param B Input matrix.
- * @param Q State cost matrix.
- * @param R Input cost matrix.
- * @param N State-input cross-term cost matrix.
- * @return Solution of DARE.
- */
- public static <States extends Num, Inputs extends Num>
- Matrix<States, States> discreteAlgebraicRiccatiEquation(
- Matrix<States, States> A,
- Matrix<States, Inputs> B,
- Matrix<States, States> Q,
- Matrix<Inputs, Inputs> R,
- Matrix<States, Inputs> N) {
- // See
- // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
- // for the change of variables used here.
- var scrA = A.minus(B.times(R.solve(N.transpose())));
- var scrQ = Q.minus(N.times(R.solve(N.transpose())));
-
- return new Matrix<>(
- discreteAlgebraicRiccatiEquation(
- scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage()));
- }
-}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
index 483dad3..3c2b843 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
@@ -20,4 +20,11 @@
* @param count the usage count
*/
void reportUsage(MathUsageId id, int count);
+
+ /**
+ * Get the current time.
+ *
+ * @return Time in seconds
+ */
+ double getTimestamp();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
index 0dbc03d..d6e3e96 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
@@ -4,6 +4,8 @@
package edu.wpi.first.math;
+import edu.wpi.first.util.WPIUtilJNI;
+
public final class MathSharedStore {
private static MathShared mathShared;
@@ -23,6 +25,11 @@
@Override
public void reportUsage(MathUsageId id, int count) {}
+
+ @Override
+ public double getTimestamp() {
+ return WPIUtilJNI.now() * 1.0e-6;
+ }
};
}
return mathShared;
@@ -56,4 +63,13 @@
public static void reportUsage(MathUsageId id, int count) {
getMathShared().reportUsage(id, count);
}
+
+ /**
+ * Get the time.
+ *
+ * @return The time in seconds.
+ */
+ public static double getTimestamp() {
+ return getMathShared().getTimestamp();
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 95ed5bf..3785780 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -145,4 +145,67 @@
public static double interpolate(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
}
+
+ /**
+ * Return where within interpolation range [0, 1] q is between startValue and endValue.
+ *
+ * @param startValue Lower part of interpolation range.
+ * @param endValue Upper part of interpolation range.
+ * @param q Query.
+ * @return Interpolant in range [0, 1].
+ */
+ public static double inverseInterpolate(double startValue, double endValue, double q) {
+ double totalRange = endValue - startValue;
+ if (totalRange <= 0) {
+ return 0.0;
+ }
+ double queryToStart = q - startValue;
+ if (queryToStart <= 0) {
+ return 0.0;
+ }
+ return queryToStart / totalRange;
+ }
+
+ /**
+ * 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
+ */
+ public static boolean isNear(double expected, double actual, double tolerance) {
+ if (tolerance < 0) {
+ throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+ }
+ return 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.
+ *
+ * <p>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
+ */
+ public static boolean isNear(
+ double expected, double actual, double tolerance, double min, double max) {
+ if (tolerance < 0) {
+ throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+ }
+ // Max error is exactly halfway between the min and max
+ double errorBound = (max - min) / 2.0;
+ double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
+ return Math.abs(error) < tolerance;
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index b8e7c28..b7a86fa 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -42,6 +42,18 @@
* Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
* provided generic bounds match the shape of the provided {@link Matrix}.
*
+ * @param rows The number of rows of the matrix.
+ * @param columns The number of columns of the matrix.
+ * @param storage The double array to back this value.
+ */
+ public Matrix(Nat<R> rows, Nat<C> columns, double[] storage) {
+ this.m_storage = new SimpleMatrix(rows.getNum(), columns.getNum(), true, storage);
+ }
+
+ /**
+ * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
+ * provided generic bounds match the shape of the provided {@link Matrix}.
+ *
* <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
* absolutely necessary due to the desired function not being accessible through the {@link
* Matrix} wrapper.
@@ -80,7 +92,7 @@
* @return The number of columns, according to the internal storage.
*/
public final int getNumCols() {
- return this.m_storage.numCols();
+ return this.m_storage.getNumCols();
}
/**
@@ -89,7 +101,7 @@
* @return The number of rows, according to the internal storage.
*/
public final int getNumRows() {
- return this.m_storage.numRows();
+ return this.m_storage.getNumRows();
}
/**
@@ -222,7 +234,7 @@
*
* <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
*
- * @param other The other {@link Matrix} to preform element multiplication on.
+ * @param other The other {@link Matrix} to perform element multiplication on.
* @return The element by element multiplication of "this" and other.
*/
public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
@@ -582,7 +594,7 @@
SimpleMatrix temp = m_storage.copy();
CholeskyDecomposition_F64<DMatrixRMaj> chol =
- DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+ DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
if (!chol.decompose(temp.getMatrix())) {
// check that the input is not all zeros -- if they are, we special case and return all
// zeros.
@@ -592,7 +604,7 @@
isZeros &= Math.abs(matDatum) < 1e-6;
}
if (isZeros) {
- return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
+ return new Matrix<>(new SimpleMatrix(temp.getNumRows(), temp.getNumCols()));
}
throw new RuntimeException(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
index e1680eb..487faf4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
@@ -68,7 +68,7 @@
private static Pair<SimpleMatrix, SimpleMatrix> pade3(SimpleMatrix A) {
double[] b = new double[] {120, 60, 12, 1};
- SimpleMatrix ident = eye(A.numRows(), A.numCols());
+ SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
@@ -78,7 +78,7 @@
private static Pair<SimpleMatrix, SimpleMatrix> pade5(SimpleMatrix A) {
double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
- SimpleMatrix ident = eye(A.numRows(), A.numCols());
+ SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
@@ -90,7 +90,7 @@
private static Pair<SimpleMatrix, SimpleMatrix> pade7(SimpleMatrix A) {
double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
- SimpleMatrix ident = eye(A.numRows(), A.numCols());
+ SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
@@ -108,7 +108,7 @@
new double[] {
17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
};
- SimpleMatrix ident = eye(A.numRows(), A.numCols());
+ SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
@@ -149,7 +149,7 @@
182,
1
};
- SimpleMatrix ident = eye(A.numRows(), A.numCols());
+ SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
@@ -213,7 +213,7 @@
SimpleMatrix temp = src.copy();
CholeskyDecomposition_F64<DMatrixRMaj> chol =
- DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+ DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
if (!chol.decompose(temp.getMatrix())) {
// check that the input is not all zeros -- if they are, we special case and return all
// zeros.
@@ -223,7 +223,7 @@
isZeros &= Math.abs(matDatum) < 1e-6;
}
if (isZeros) {
- return new SimpleMatrix(temp.numRows(), temp.numCols());
+ return new SimpleMatrix(temp.getNumRows(), temp.getNumCols());
}
throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
@@ -239,8 +239,8 @@
* @return the exponential of A.
*/
public static SimpleMatrix exp(SimpleMatrix A) {
- SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
- WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
+ SimpleMatrix toReturn = new SimpleMatrix(A.getNumRows(), A.getNumRows());
+ WPIMathJNI.exp(A.getDDRM().getData(), A.getNumRows(), toReturn.getDDRM().getData());
return toReturn;
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index a041845..27ed9e0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -15,7 +15,7 @@
private static Random rand = new Random();
private StateSpaceUtil() {
- // Utility class
+ throw new UnsupportedOperationException("This is a utility class!");
}
/**
@@ -90,7 +90,7 @@
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(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) %3C n
+ * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n
* where n is the number of states.
*
* @param <States> Num representing the size of A.
@@ -108,7 +108,7 @@
* Returns true if (A, C) is a detectable pair.
*
* <p>(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) %3C n where n is
+ * values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) %3C n where n is
* the number of states.
*
* @param <States> Num representing the size of A.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
index 670611a..9ba91b0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
@@ -14,6 +14,8 @@
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.math.numbers.N8;
import edu.wpi.first.math.numbers.N9;
+import java.util.Objects;
+import org.ejml.simple.SimpleMatrix;
/**
* A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
@@ -26,7 +28,15 @@
}
private Vector<N> fillVec(double... data) {
- return new Vector<>(fill(data));
+ if (Objects.requireNonNull(data).length != this.m_rows.getNum()) {
+ throw new IllegalArgumentException(
+ "Invalid vector data provided. Wanted "
+ + this.m_rows.getNum()
+ + " vector, but got "
+ + data.length
+ + " elements");
+ }
+ return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data));
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 1aebdde..bc46741 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -5,6 +5,7 @@
package edu.wpi.first.math;
import edu.wpi.first.math.numbers.N1;
+import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
/**
@@ -63,6 +64,26 @@
}
/**
+ * Adds the given vector to this vector.
+ *
+ * @param value The vector to add.
+ * @return The resultant vector.
+ */
+ public final Vector<R> plus(Vector<R> value) {
+ return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+ }
+
+ /**
+ * Subtracts the given vector to this vector.
+ *
+ * @param value The vector to add.
+ * @return The resultant vector.
+ */
+ public final Vector<R> minus(Vector<R> value) {
+ return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+ }
+
+ /**
* Returns the dot product of this vector with another.
*
* @param other The other vector.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 40a5c63..ef47a3b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -43,8 +43,25 @@
libraryLoaded = true;
}
+ // DARE wrappers
+
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * <p>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>R isn't symmetric positive definite
+ * <li>The (A, B) pair isn't stabilizable
+ * <li>The (A, C) pair where Q = CᵀC isn't detectable
+ * </ul>
+ *
+ * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
+ * alegebraic Riccati equation.
*
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
@@ -54,10 +71,148 @@
* @param inputs Number of inputs in B matrix.
* @param S Array storage for DARE solution.
*/
- public static native void discreteAlgebraicRiccatiEquation(
+ public static native void dareDetailABQR(
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>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ₖ:
+ *
+ * <pre>
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>This can be refactored as:
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>This internal function skips expensive precondition checks for increased performance. The
+ * solver may hang if any of the following occur:
+ *
+ * <ul>
+ * <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+ * <li>R isn't symmetric positive definite
+ * <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
+ * <li>The (A, C) pair where Q = CᵀC isn't detectable
+ * </ul>
+ *
+ * <p>Only use this function if you're sure the preconditions are met.
+ *
+ * @param A Array containing elements of A in row-major order.
+ * @param B Array containing elements of B in row-major order.
+ * @param Q Array containing elements of Q in row-major order.
+ * @param R Array containing elements of R in row-major order.
+ * @param N Array containing elements of N in row-major order.
+ * @param states Number of states in A matrix.
+ * @param inputs Number of inputs in B matrix.
+ * @param S Array storage for DARE solution.
+ */
+ public static native void dareDetailABQRN(
+ double[] A,
+ double[] B,
+ double[] Q,
+ double[] R,
+ double[] N,
+ int states,
+ int inputs,
+ double[] S);
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @param A Array containing elements of A in row-major order.
+ * @param B Array containing elements of B in row-major order.
+ * @param Q Array containing elements of Q in row-major order.
+ * @param R Array containing elements of R in row-major order.
+ * @param states Number of states in A matrix.
+ * @param inputs Number of inputs in B matrix.
+ * @param S Array storage for DARE solution.
+ * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+ * @throws IllegalArgumentException if R isn't symmetric positive definite.
+ * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+ * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+ */
+ public static native void dareABQR(
+ double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>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ₖ:
+ *
+ * <pre>
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ * </pre>
+ *
+ * <p>This can be refactored as:
+ *
+ * <pre>
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ * </pre>
+ *
+ * @param A Array containing elements of A in row-major order.
+ * @param B Array containing elements of B in row-major order.
+ * @param Q Array containing elements of Q in row-major order.
+ * @param R Array containing elements of R in row-major order.
+ * @param N Array containing elements of N in row-major order.
+ * @param states Number of states in A matrix.
+ * @param inputs Number of inputs in B matrix.
+ * @param S Array storage for DARE solution.
+ * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
+ * @throws IllegalArgumentException if R isn't symmetric positive definite.
+ * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
+ * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+ */
+ public static native void dareABQRN(
+ double[] A,
+ double[] B,
+ double[] Q,
+ double[] R,
+ double[] N,
+ int states,
+ int inputs,
+ double[] S);
+
+ // Eigen wrappers
+
+ /**
* Computes the matrix exp.
*
* @param src Array of elements of the matrix to be exponentiated.
@@ -77,6 +232,109 @@
public static native void pow(double[] src, int rows, double exponent, double[] dst);
/**
+ * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
+ * matrix.
+ *
+ * @param mat Array of elements of the matrix to be updated.
+ * @param lowerTriangular Whether mat is lower triangular.
+ * @param rows How many rows there are.
+ * @param vec Vector to use for the rank update.
+ * @param sigma Sigma value to use for the rank update.
+ */
+ public static native void rankUpdate(
+ double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
+
+ /**
+ * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
+ *
+ * @param A Array of elements of the A matrix.
+ * @param Arows Number of rows of the A matrix.
+ * @param Acols Number of rows of the A matrix.
+ * @param B Array of elements of the B matrix.
+ * @param Brows Number of rows of the B matrix.
+ * @param Bcols Number of rows of the B matrix.
+ * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
+ */
+ public static native void solveFullPivHouseholderQr(
+ double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
+
+ // Pose3d wrappers
+
+ /**
+ * Obtain a Pose3d from a (constant curvature) velocity.
+ *
+ * <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
+ *
+ * @param poseX The pose's translational X component.
+ * @param poseY The pose's translational Y component.
+ * @param poseZ The pose's translational Z component.
+ * @param poseQw The pose quaternion's W component.
+ * @param poseQx The pose quaternion's X component.
+ * @param poseQy The pose quaternion's Y component.
+ * @param poseQz The pose quaternion's Z component.
+ * @param twistDx The twist's dx value.
+ * @param twistDy The twist's dy value.
+ * @param twistDz The twist's dz value.
+ * @param twistRx The twist's rx value.
+ * @param twistRy The twist's ry value.
+ * @param twistRz The twist's rz value.
+ * @return The new pose as a double array.
+ */
+ public static native double[] expPose3d(
+ double poseX,
+ double poseY,
+ double poseZ,
+ double poseQw,
+ double poseQx,
+ double poseQy,
+ double poseQz,
+ double twistDx,
+ double twistDy,
+ double twistDz,
+ double twistRx,
+ double twistRy,
+ double twistRz);
+
+ /**
+ * Returns a Twist3d that maps the starting pose to the end pose.
+ *
+ * <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
+ *
+ * @param startX The starting pose's translational X component.
+ * @param startY The starting pose's translational Y component.
+ * @param startZ The starting pose's translational Z component.
+ * @param startQw The starting pose quaternion's W component.
+ * @param startQx The starting pose quaternion's X component.
+ * @param startQy The starting pose quaternion's Y component.
+ * @param startQz The starting pose quaternion's Z component.
+ * @param endX The ending pose's translational X component.
+ * @param endY The ending pose's translational Y component.
+ * @param endZ The ending pose's translational Z component.
+ * @param endQw The ending pose quaternion's W component.
+ * @param endQx The ending pose quaternion's X component.
+ * @param endQy The ending pose quaternion's Y component.
+ * @param endQz The ending pose quaternion's Z component.
+ * @return The twist that maps start to end as a double array.
+ */
+ public static native double[] logPose3d(
+ double startX,
+ double startY,
+ double startZ,
+ double startQw,
+ double startQx,
+ double startQy,
+ double startQz,
+ double endX,
+ double endY,
+ double endZ,
+ double endQw,
+ double endQx,
+ double endQy,
+ double endQz);
+
+ // StateSpaceUtil wrappers
+
+ /**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
@@ -91,6 +349,8 @@
*/
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
+ // Trajectory wrappers
+
/**
* Loads a Pathweaver JSON.
*
@@ -125,19 +385,6 @@
*/
public static native String serializeTrajectory(double[] elements);
- /**
- * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
- * matrix.
- *
- * @param mat Array of elements of the matrix to be updated.
- * @param lowerTriangular Whether mat is lower triangular.
- * @param rows How many rows there are.
- * @param vec Vector to use for the rank update.
- * @param sigma Sigma value to use for the rank update.
- */
- public static native void rankUpdate(
- double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
-
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
@@ -149,18 +396,4 @@
extractOnStaticLoad.set(load);
}
}
-
- /**
- * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
- *
- * @param A Array of elements of the A matrix.
- * @param Arows Number of rows of the A matrix.
- * @param Acols Number of rows of the A matrix.
- * @param B Array of elements of the B matrix.
- * @param Brows Number of rows of the B matrix.
- * @param Bcols Number of rows of the B matrix.
- * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
- */
- public static native void solveFullPivHouseholderQr(
- double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
index 385c5c0..a2a5944 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
@@ -181,6 +181,9 @@
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
var rDot = (nextR.minus(r)).div(m_dt);
+ // ṙ = f(r) + Bu
+ // Bu = ṙ − f(r)
+ // u = B⁺(ṙ − f(r))
m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
m_r = nextR;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
index 9f4dd86..cfe7c2b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -4,6 +4,10 @@
package edu.wpi.first.math.controller;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
/**
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
* against the force of gravity).
@@ -54,6 +58,48 @@
}
/**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * <p>Note this method is inaccurate when the velocity crosses 0.
+ *
+ * @param currentVelocity The current velocity setpoint.
+ * @param nextVelocity The next velocity setpoint.
+ * @param dtSeconds Time between velocity setpoints in seconds.
+ * @return The computed feedforward.
+ */
+ public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+ // 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)
+ var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+ var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+ var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
+ var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+
+ return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+ }
+
+ /**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
index e43c6ff..87148a4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
@@ -139,4 +139,31 @@
public void setEnabled(boolean enabled) {
m_enabled = enabled;
}
+
+ /**
+ * Returns the heading controller.
+ *
+ * @return heading ProfiledPIDController
+ */
+ public ProfiledPIDController getThetaController() {
+ return m_thetaController;
+ }
+
+ /**
+ * Returns the x controller.
+ *
+ * @return X PIDController
+ */
+ public PIDController getXController() {
+ return m_xController;
+ }
+
+ /**
+ * Returns the y controller.
+ *
+ * @return Y PIDController
+ */
+ public PIDController getYController() {
+ return m_yController;
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
index 46254a3..2be2a47 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
@@ -4,6 +4,7 @@
package edu.wpi.first.math.controller;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
@@ -15,14 +16,22 @@
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.trajectory.Trajectory;
/**
* 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.
+ * model used to compute the controller gain is the nonlinear 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.
+ *
+ * <p>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.
*
* <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
* shown in theorem 8.7.4.
@@ -55,12 +64,18 @@
/**
* Constructs a linear time-varying differential drive controller.
*
+ * <p>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 in
* meters.
* @param qelems The maximum desired error tolerance for each state.
* @param relems The maximum desired control effort for each input.
* @param dt Discretization timestep in seconds.
+ * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0 m/s or >=
+ * 15 m/s.
*/
public LTVDifferentialDriveController(
LinearSystem<N2, N2, N2> plant,
@@ -127,15 +142,39 @@
.times(-1.0)
.get(0, 0);
+ if (maxV <= 0.0) {
+ throw new IllegalArgumentException(
+ "Max velocity of plant with 12 V input must be greater than 0 m/s.");
+ }
+ if (maxV >= 15.0) {
+ throw new IllegalArgumentException(
+ "Max velocity of plant with 12 V input must be less than 15 m/s.");
+ }
+
for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (Math.abs(velocity) < 1e-4) {
- m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
+ A.set(State.kY.value, State.kHeading.value, 1e-4);
} else {
A.set(State.kY.value, State.kHeading.value, velocity);
- m_table.put(velocity, new LinearQuadraticRegulator<N5, N2, N5>(A, B, Q, R, dt).getK());
}
+
+ var discABPair = Discretization.discretizeAB(A, B, dt);
+ var discA = discABPair.getFirst();
+ var discB = discABPair.getSecond();
+
+ var S = DARE.dareDetail(discA, discB, Q, R);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.put(
+ velocity,
+ discB
+ .transpose()
+ .times(S)
+ .times(discB)
+ .plus(R)
+ .solve(discB.transpose().times(S).times(discA)));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
index 701f21b..7e3391f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
@@ -4,6 +4,7 @@
package edu.wpi.first.math.controller;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
@@ -15,12 +16,16 @@
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.trajectory.Trajectory;
/**
* 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.
+ * compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's
+ * current state.
+ *
+ * <p>This controller is a roughly drop-in replacement for {@link RamseteController} with more
+ * optimal feedback gains in the "least-squares error" sense.
*
* <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
* shown in theorem 8.9.1.
@@ -66,6 +71,7 @@
* @param dt Discretization timestep in seconds.
* @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
* table. The default is 9 m/s.
+ * @throws IllegalArgumentException if maxVelocity <= 0.
*/
public LTVUnicycleController(double dt, double maxVelocity) {
this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
@@ -74,6 +80,10 @@
/**
* Constructs a linear time-varying unicycle controller.
*
+ * <p>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 in seconds.
@@ -85,14 +95,26 @@
/**
* Constructs a linear time-varying unicycle controller.
*
+ * <p>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 in seconds.
* @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
* table. The default is 9 m/s.
+ * @throws IllegalArgumentException if maxVelocity <= 0 m/s or >= 15 m/s.
*/
public LTVUnicycleController(
Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+ if (maxVelocity <= 0.0) {
+ throw new IllegalArgumentException("Max velocity must be greater than 0 m/s.");
+ }
+ if (maxVelocity >= 15.0) {
+ throw new IllegalArgumentException("Max velocity must be less than 15 m/s.");
+ }
+
// The change in global pose for a unicycle is defined by the following
// three equations.
//
@@ -135,11 +157,26 @@
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (Math.abs(velocity) < 1e-4) {
- m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
+ A.set(State.kY.value, State.kHeading.value, 1e-4);
} else {
A.set(State.kY.value, State.kHeading.value, velocity);
- m_table.put(velocity, new LinearQuadraticRegulator<N3, N2, N3>(A, B, Q, R, dt).getK());
}
+
+ var discABPair = Discretization.discretizeAB(A, B, dt);
+ var discA = discABPair.getFirst();
+ var discB = discABPair.getSecond();
+
+ var S = DARE.dareDetail(discA, discB, Q, R);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.put(
+ velocity,
+ discB
+ .transpose()
+ .times(S)
+ .times(discB)
+ .plus(R)
+ .solve(discB.transpose().times(S).times(discA)));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
index a762851..b0f23a8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
@@ -139,6 +139,9 @@
* @return The calculated feedforward.
*/
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+ // rₖ₊₁ = Arₖ + Buₖ
+ // Buₖ = rₖ₊₁ − Arₖ
+ // uₖ = B⁺(rₖ₊₁ − Arₖ)
m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
m_r = nextR;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
index dce1748..658be69 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -4,7 +4,7 @@
package edu.wpi.first.math.controller;
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
@@ -35,6 +35,10 @@
/**
* Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
*
+ * <p>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 qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
@@ -57,6 +61,10 @@
/**
* Constructs a controller with the given coefficients and plant.
*
+ * <p>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 qelms The maximum desired error tolerance for each state.
@@ -111,7 +119,7 @@
throw new IllegalArgumentException(msg);
}
- var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+ var S = DARE.dare(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K =
@@ -150,7 +158,7 @@
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
- var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+ var S = DARE.dare(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K =
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index 5e82909..1d98900 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -24,6 +24,9 @@
// Factor for "derivative" control
private double m_kd;
+ // The error range where "integral" control applies
+ private double m_iZone = Double.POSITIVE_INFINITY;
+
// The period (in seconds) of the loop that calls the controller
private final double m_period;
@@ -55,6 +58,9 @@
private double m_setpoint;
private double m_measurement;
+ private boolean m_haveMeasurement;
+ private boolean m_haveSetpoint;
+
/**
* Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
* 0.02 seconds.
@@ -139,6 +145,22 @@
}
/**
+ * 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 {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+ *
+ * @param iZone Maximum magnitude of error to allow integral control.
+ */
+ public void setIZone(double iZone) {
+ if (iZone < 0) {
+ throw new IllegalArgumentException("IZone must be a non-negative number!");
+ }
+ m_iZone = iZone;
+ }
+
+ /**
* Get the Proportional coefficient.
*
* @return proportional coefficient
@@ -166,6 +188,15 @@
}
/**
+ * Get the IZone range.
+ *
+ * @return Maximum magnitude of error to allow integral control.
+ */
+ public double getIZone() {
+ return m_iZone;
+ }
+
+ /**
* Returns the period of this controller.
*
* @return the period of the controller.
@@ -199,6 +230,7 @@
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
+ m_haveSetpoint = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -227,7 +259,9 @@
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint() {
- return Math.abs(m_positionError) < m_positionTolerance
+ return m_haveMeasurement
+ && m_haveSetpoint
+ && Math.abs(m_positionError) < m_positionTolerance
&& Math.abs(m_velocityError) < m_velocityTolerance;
}
@@ -321,6 +355,7 @@
*/
public double calculate(double measurement, double setpoint) {
m_setpoint = setpoint;
+ m_haveSetpoint = true;
return calculate(measurement);
}
@@ -333,6 +368,7 @@
public double calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
+ m_haveMeasurement = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -343,7 +379,10 @@
m_velocityError = (m_positionError - m_prevError) / m_period;
- if (m_ki != 0) {
+ // If the absolute value of the position error is greater than IZone, reset the total error
+ if (Math.abs(m_positionError) > m_iZone) {
+ m_totalError = 0;
+ } else if (m_ki != 0) {
m_totalError =
MathUtil.clamp(
m_totalError + m_positionError * m_period,
@@ -360,6 +399,7 @@
m_prevError = 0;
m_totalError = 0;
m_velocityError = 0;
+ m_haveMeasurement = false;
}
@Override
@@ -368,6 +408,7 @@
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
+ builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 02cc17d..80311f5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -22,9 +22,11 @@
private PIDController m_controller;
private double m_minimumInput;
private double m_maximumInput;
+
+ private TrapezoidProfile.Constraints m_constraints;
+ private TrapezoidProfile m_profile;
private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
- private TrapezoidProfile.Constraints m_constraints;
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
@@ -52,6 +54,7 @@
double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
m_controller = new PIDController(Kp, Ki, Kd, period);
m_constraints = constraints;
+ m_profile = new TrapezoidProfile(m_constraints);
instances++;
SendableRegistry.add(this, "ProfiledPIDController", instances);
@@ -99,6 +102,19 @@
}
/**
+ * 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 {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+ *
+ * @param iZone Maximum magnitude of error to allow integral control.
+ */
+ public void setIZone(double iZone) {
+ m_controller.setIZone(iZone);
+ }
+
+ /**
* Gets the proportional coefficient.
*
* @return proportional coefficient
@@ -126,6 +142,15 @@
}
/**
+ * Get the IZone range.
+ *
+ * @return Maximum magnitude of error to allow integral control.
+ */
+ public double getIZone() {
+ return m_controller.getIZone();
+ }
+
+ /**
* Gets the period of this controller.
*
* @return The period of the controller.
@@ -197,6 +222,16 @@
*/
public void setConstraints(TrapezoidProfile.Constraints constraints) {
m_constraints = constraints;
+ m_profile = new TrapezoidProfile(m_constraints);
+ }
+
+ /**
+ * Get the velocity and acceleration constraints for this controller.
+ *
+ * @return Velocity and acceleration constraints.
+ */
+ public TrapezoidProfile.Constraints getConstraints() {
+ return m_constraints;
}
/**
@@ -312,8 +347,7 @@
m_setpoint.position = setpointMinDistance + measurement;
}
- var profile = new TrapezoidProfile(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, m_setpoint.position);
}
@@ -391,6 +425,7 @@
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
+ builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index b5fe591..7918f9f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -4,22 +4,15 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Map;
-import java.util.Objects;
/**
* This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
@@ -34,15 +27,7 @@
* <p>{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
* you want; if you never call it then this class will behave exactly like regular encoder odometry.
*/
-public class DifferentialDrivePoseEstimator {
- private final DifferentialDriveKinematics m_kinematics;
- private final DifferentialDriveOdometry m_odometry;
- private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
- private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
- private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
- TimeInterpolatableBuffer.createBuffer(1.5);
-
+public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDriveWheelPositions> {
/**
* Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
* vision measurements.
@@ -96,44 +81,12 @@
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
- m_kinematics = kinematics;
- m_odometry =
+ super(
+ kinematics,
new DifferentialDriveOdometry(
- gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
-
- for (int i = 0; i < 3; ++i) {
- m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
- }
-
- // Initialize vision R
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- }
-
- /**
- * Sets the pose estimator's trust of global 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 measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
- */
- public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
- var r = new double[3];
- for (int i = 0; i < 3; ++i) {
- r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
- }
-
- // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
- // and C = I. See wpimath/algorithms.md.
- for (int row = 0; row < 3; ++row) {
- if (m_q.get(row, 0) == 0.0) {
- m_visionK.set(row, row, 0.0);
- } else {
- m_visionK.set(
- row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
- }
- }
+ gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
+ stateStdDevs,
+ visionMeasurementStdDevs);
}
/**
@@ -152,111 +105,10 @@
double leftPositionMeters,
double rightPositionMeters,
Pose2d poseMeters) {
- // Reset state estimate and error covariance
- m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
- m_poseBuffer.clear();
- }
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose in meters.
- */
- public Pose2d getEstimatedPosition() {
- return m_odometry.getPoseMeters();
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
- * while still accounting for measurement noise.
- *
- * <p>This method can be called as infrequently as you want, as long as you are calling {@link
- * DifferentialDrivePoseEstimator#update} every loop.
- *
- * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link
- * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you
- * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
- * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
- * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
- * or sync the epochs.
- */
- public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
- // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
- var sample = m_poseBuffer.getSample(timestampSeconds);
-
- if (sample.isEmpty()) {
- return;
- }
-
- // Step 2: Measure the twist between the odometry pose and the vision pose.
- var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
- // 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.
- var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
- // Step 4: Convert back to Twist2d.
- var scaledTwist =
- new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.resetPosition(
- sample.get().gyroAngle,
- sample.get().leftMeters,
- sample.get().rightMeters,
- sample.get().poseMeters.exp(scaledTwist));
-
- // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
- // pose buffer and correct odometry.
- for (Map.Entry<Double, InterpolationRecord> entry :
- m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
- updateWithTime(
- entry.getKey(),
- entry.getValue().gyroAngle,
- entry.getValue().leftMeters,
- entry.getValue().rightMeters);
- }
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
- * while still accounting for measurement noise.
- *
- * <p>This method can be called as infrequently as you want, as long as you are calling {@link
- * DifferentialDrivePoseEstimator#update} every loop.
- *
- * <p>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.
- *
- * <p>Note that the vision measurement standard deviations passed into this method will continue
- * to apply to future measurements until a subsequent call to {@link
- * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
- *
- * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link
- * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you
- * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
- * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that
- * you should use {@link edu.wpi.first.wpilibj.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.
- */
- public void addVisionMeasurement(
- Pose2d visionRobotPoseMeters,
- double timestampSeconds,
- Matrix<N3, N1> visionMeasurementStdDevs) {
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+ resetPosition(
+ gyroAngle,
+ new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
+ poseMeters);
}
/**
@@ -270,8 +122,8 @@
*/
public Pose2d update(
Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
- return updateWithTime(
- WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
+ return update(
+ gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}
/**
@@ -289,98 +141,9 @@
Rotation2d gyroAngle,
double distanceLeftMeters,
double distanceRightMeters) {
- m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
- m_poseBuffer.addSample(
+ return updateWithTime(
currentTimeSeconds,
- new InterpolationRecord(
- getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
-
- return getEstimatedPosition();
- }
-
- /**
- * Represents an odometry record. The record contains the inputs provided as well as the pose that
- * was observed based on these inputs, as well as the previous record and its inputs.
- */
- private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
- // The pose observed given the current sensor inputs and the previous pose.
- private final Pose2d poseMeters;
-
- // The current gyro angle.
- private final Rotation2d gyroAngle;
-
- // The distance traveled by the left encoder.
- private final double leftMeters;
-
- // The distance traveled by the right encoder.
- private final double rightMeters;
-
- /**
- * Constructs an Interpolation Record with the specified parameters.
- *
- * @param pose The pose observed given the current sensor inputs and the previous pose.
- * @param gyro The current gyro angle.
- * @param leftMeters The distance traveled by the left encoder.
- * @param rightMeters The distanced traveled by the right encoder.
- */
- private InterpolationRecord(
- Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) {
- this.poseMeters = poseMeters;
- this.gyroAngle = gyro;
- this.leftMeters = leftMeters;
- this.rightMeters = rightMeters;
- }
-
- /**
- * Return the interpolated record. This object is assumed to be the starting position, or lower
- * bound.
- *
- * @param endValue The upper bound, or end.
- * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
- * @return The interpolated value.
- */
- @Override
- public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
- if (t < 0) {
- return this;
- } else if (t >= 1) {
- return endValue;
- } else {
- // Find the new left distance.
- var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t);
-
- // Find the new right distance.
- var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t);
-
- // Find the new gyro angle.
- var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
- // Create a twist to represent this change based on the interpolated sensor inputs.
- Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters);
- twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
- return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp);
- }
- }
-
- @Override
- public boolean equals(Object obj) {
- if (this == obj) {
- return true;
- }
- if (!(obj instanceof InterpolationRecord)) {
- return false;
- }
- InterpolationRecord record = (InterpolationRecord) obj;
- return Objects.equals(gyroAngle, record.gyroAngle)
- && Double.compare(leftMeters, record.leftMeters) == 0
- && Double.compare(rightMeters, record.rightMeters) == 0
- && Objects.equals(poseMeters, record.poseMeters);
- }
-
- @Override
- public int hashCode() {
- return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters);
- }
+ gyroAngle,
+ new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
index 5f9e52e..59323b2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
@@ -4,7 +4,7 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -59,6 +59,10 @@
/**
* Constructs an extended Kalman filter.
*
+ * <p>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 states a Nat representing the number of states.
* @param inputs a Nat representing the number of inputs.
* @param outputs a Nat representing the number of outputs.
@@ -93,6 +97,10 @@
/**
* Constructs an extended Kalman filter.
*
+ * <p>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 states a Nat representing the number of states.
* @param inputs a Nat representing the number of inputs.
* @param outputs a Nat representing the number of outputs.
@@ -138,15 +146,14 @@
NumericalJacobian.numericalJacobianX(
outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
- final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+ final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
final var discR = Discretization.discretizeR(m_contR, dtSeconds);
if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
- m_initP =
- Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
+ m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = new Matrix<>(states, states);
}
@@ -260,7 +267,7 @@
final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
// Find discrete A and Q
- final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+ final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
@@ -286,24 +293,14 @@
/**
* Correct the state estimate x-hat using the measurements in y.
*
- * <p>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).
+ * <p>This is useful for when the measurement noise covariances vary.
*
- * @param <Rows> Number of rows in the result of f(x, u).
- * @param rows Number of rows in the result of f(x, u).
* @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 contR Continuous measurement noise covariance matrix.
+ * @param R Continuous measurement noise covariance matrix.
*/
- public <Rows extends Num> void correct(
- Nat<Rows> rows,
- Matrix<Inputs, N1> u,
- Matrix<Rows, N1> y,
- BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
- Matrix<Rows, Rows> contR) {
- correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus);
+ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+ correct(m_outputs, u, y, m_h, R, m_residualFuncY, m_addFuncX);
}
/**
@@ -318,7 +315,30 @@
* @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 contR Continuous measurement noise covariance matrix.
+ * @param R Continuous measurement noise covariance matrix.
+ */
+ public <Rows extends Num> void correct(
+ Nat<Rows> rows,
+ Matrix<Inputs, N1> u,
+ Matrix<Rows, N1> y,
+ BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+ Matrix<Rows, Rows> R) {
+ correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+ }
+
+ /**
+ * Correct the state estimate x-hat using the measurements in y.
+ *
+ * <p>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 <Rows> Number of rows in the result of f(x, u).
+ * @param rows Number of rows in the result of f(x, u).
+ * @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.
* @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.
@@ -328,11 +348,11 @@
Matrix<Inputs, N1> u,
Matrix<Rows, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
- Matrix<Rows, Rows> contR,
+ Matrix<Rows, Rows> R,
BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
- final var discR = Discretization.discretizeR(contR, m_dtSeconds);
+ final var discR = Discretization.discretizeR(R, m_dtSeconds);
final var S = C.times(m_P).times(C.transpose()).plus(discR);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
index 24d6d91..08e3270 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -4,7 +4,7 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -43,6 +43,10 @@
/**
* Constructs a state-space observer with the given plant.
*
+ * <p>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 states A Nat representing the states of the system.
* @param outputs A Nat representing the outputs of the system.
* @param plant The plant used for the prediction step.
@@ -65,7 +69,7 @@
var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
- var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
+ var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
var discA = pair.getFirst();
var discQ = pair.getSecond();
@@ -87,9 +91,7 @@
throw new IllegalArgumentException(msg);
}
- var P =
- new Matrix<>(
- Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR));
+ var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
// S = CPCᵀ + R
var S = C.times(P).times(C.transpose()).plus(discR);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
index 7a100f5..7b14839 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
@@ -8,7 +8,7 @@
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
-interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
Matrix<States, States> getP();
double getP(int i, int j);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index 448b8d3..59bbb32 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -4,23 +4,15 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Map;
-import java.util.Objects;
/**
* This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
@@ -33,15 +25,7 @@
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
*/
-public class MecanumDrivePoseEstimator {
- private final MecanumDriveKinematics m_kinematics;
- private final MecanumDriveOdometry m_odometry;
- private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
- private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
- private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
- TimeInterpolatableBuffer.createBuffer(1.5);
-
+public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPositions> {
/**
* Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
* vision measurements.
@@ -90,294 +74,10 @@
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
- m_kinematics = kinematics;
- m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
-
- for (int i = 0; i < 3; ++i) {
- m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
- }
-
- // Initialize vision R
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- }
-
- /**
- * Sets the pose estimator's trust of global 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 measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
- */
- public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
- var r = new double[3];
- for (int i = 0; i < 3; ++i) {
- r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
- }
-
- // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
- // and C = I. See wpimath/algorithms.md.
- for (int row = 0; row < 3; ++row) {
- if (m_q.get(row, 0) == 0.0) {
- m_visionK.set(row, row, 0.0);
- } else {
- m_visionK.set(
- row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
- }
- }
- }
-
- /**
- * Resets the robot's position on the field.
- *
- * <p>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 driven by each wheel.
- * @param poseMeters The position on the field that your robot is at.
- */
- public void resetPosition(
- Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
- // Reset state estimate and error covariance
- m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
- m_poseBuffer.clear();
- }
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose in meters.
- */
- public Pose2d getEstimatedPosition() {
- return m_odometry.getPoseMeters();
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
- * while still accounting for measurement noise.
- *
- * <p>This method can be called as infrequently as you want, as long as you are calling {@link
- * MecanumDrivePoseEstimator#update} every loop.
- *
- * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link
- * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}
- * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
- * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.)
- * This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
- * your time source or sync the epochs.
- */
- public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
- // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
- var sample = m_poseBuffer.getSample(timestampSeconds);
-
- if (sample.isEmpty()) {
- return;
- }
-
- // Step 2: Measure the twist between the odometry pose and the vision pose.
- var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
- // 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.
- var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
- // Step 4: Convert back to Twist2d.
- var scaledTwist =
- new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.resetPosition(
- sample.get().gyroAngle,
- sample.get().wheelPositions,
- sample.get().poseMeters.exp(scaledTwist));
-
- // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
- // pose buffer and correct odometry.
- for (Map.Entry<Double, InterpolationRecord> entry :
- m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
- updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
- }
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
- * while still accounting for measurement noise.
- *
- * <p>This method can be called as infrequently as you want, as long as you are calling {@link
- * MecanumDrivePoseEstimator#update} every loop.
- *
- * <p>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.
- *
- * <p>Note that the vision measurement standard deviations passed into this method will continue
- * to apply to future measurements until a subsequent call to {@link
- * MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
- *
- * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link
- * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)},
- * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
- * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
- * This means that you should use {@link edu.wpi.first.wpilibj.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.
- */
- public void addVisionMeasurement(
- Pose2d visionRobotPoseMeters,
- double timestampSeconds,
- Matrix<N3, N1> visionMeasurementStdDevs) {
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
- }
-
- /**
- * 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 driven by each wheel.
- * @return The estimated pose of the robot in meters.
- */
- public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
- }
-
- /**
- * Updates the pose estimator with wheel encoder and gyro information. This should be called every
- * loop.
- *
- * @param currentTimeSeconds Time at which this method was called, in seconds.
- * @param gyroAngle The current gyroscope angle.
- * @param wheelPositions The distances driven by each wheel.
- * @return The estimated pose of the robot in meters.
- */
- public Pose2d updateWithTime(
- double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
- m_odometry.update(gyroAngle, wheelPositions);
-
- m_poseBuffer.addSample(
- currentTimeSeconds,
- new InterpolationRecord(
- getEstimatedPosition(),
- gyroAngle,
- new MecanumDriveWheelPositions(
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters)));
-
- return getEstimatedPosition();
- }
-
- /**
- * Represents an odometry record. The record contains the inputs provided as well as the pose that
- * was observed based on these inputs, as well as the previous record and its inputs.
- */
- private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
- // The pose observed given the current sensor inputs and the previous pose.
- private final Pose2d poseMeters;
-
- // The current gyro angle.
- private final Rotation2d gyroAngle;
-
- // The distances traveled by each wheel encoder.
- private final MecanumDriveWheelPositions wheelPositions;
-
- /**
- * Constructs an Interpolation Record with the specified parameters.
- *
- * @param pose The pose observed given the current sensor inputs and the previous pose.
- * @param gyro The current gyro angle.
- * @param wheelPositions The distances traveled by each wheel encoder.
- */
- private InterpolationRecord(
- Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions wheelPositions) {
- this.poseMeters = poseMeters;
- this.gyroAngle = gyro;
- this.wheelPositions = wheelPositions;
- }
-
- /**
- * Return the interpolated record. This object is assumed to be the starting position, or lower
- * bound.
- *
- * @param endValue The upper bound, or end.
- * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
- * @return The interpolated value.
- */
- @Override
- public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
- if (t < 0) {
- return this;
- } else if (t >= 1) {
- return endValue;
- } else {
- // Find the new wheel distances.
- var wheels_lerp =
- new MecanumDriveWheelPositions(
- MathUtil.interpolate(
- this.wheelPositions.frontLeftMeters,
- endValue.wheelPositions.frontLeftMeters,
- t),
- MathUtil.interpolate(
- this.wheelPositions.frontRightMeters,
- endValue.wheelPositions.frontRightMeters,
- t),
- MathUtil.interpolate(
- this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t),
- MathUtil.interpolate(
- this.wheelPositions.rearRightMeters,
- endValue.wheelPositions.rearRightMeters,
- t));
-
- // Find the distance travelled between this measurement and the interpolated measurement.
- var wheels_delta =
- new MecanumDriveWheelPositions(
- wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters,
- wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters,
- wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters,
- wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters);
-
- // Find the new gyro angle.
- var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
- // Create a twist to represent this change based on the interpolated sensor inputs.
- Twist2d twist = m_kinematics.toTwist2d(wheels_delta);
- twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
- return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp);
- }
- }
-
- @Override
- public boolean equals(Object obj) {
- if (this == obj) {
- return true;
- }
- if (!(obj instanceof InterpolationRecord)) {
- return false;
- }
- InterpolationRecord record = (InterpolationRecord) obj;
- return Objects.equals(gyroAngle, record.gyroAngle)
- && Objects.equals(wheelPositions, record.wheelPositions)
- && Objects.equals(poseMeters, record.poseMeters);
- }
-
- @Override
- public int hashCode() {
- return Objects.hash(gyroAngle, wheelPositions, poseMeters);
- }
+ super(
+ kinematics,
+ new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
+ stateStdDevs,
+ visionMeasurementStdDevs);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
new file mode 100644
index 0000000..bb30f25
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -0,0 +1,333 @@
+// 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.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.Odometry;
+import edu.wpi.first.math.kinematics.WheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Map;
+import java.util.NoSuchElementException;
+import java.util.Objects;
+
+/**
+ * This class wraps {@link 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., {@link DifferentialDrivePoseEstimator}). It is intended to be a drop-in
+ * replacement for {@link Odometry}; in fact, if you never call {@link
+ * PoseEstimator#addVisionMeasurement} and only call {@link PoseEstimator#update} then this will
+ * behave exactly the same as Odometry.
+ *
+ * <p>{@link PoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link PoseEstimator#addVisionMeasurement} can be called as infrequently as you want; if you
+ * never call it then this class will behave exactly like regular encoder odometry.
+ */
+public class PoseEstimator<T extends WheelPositions<T>> {
+ private final Kinematics<?, T> m_kinematics;
+ private final Odometry<T> m_odometry;
+ private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+ private static final double kBufferDuration = 1.5;
+ private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(kBufferDuration);
+
+ /**
+ * 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.
+ */
+ public PoseEstimator(
+ Kinematics<?, T> kinematics,
+ Odometry<T> odometry,
+ Matrix<N3, N1> stateStdDevs,
+ Matrix<N3, N1> visionMeasurementStdDevs) {
+ m_kinematics = kinematics;
+ m_odometry = odometry;
+
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
+ setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+ }
+
+ /**
+ * Sets the pose estimator's trust of global 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 measurements. Increase these
+ * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+ * theta]ᵀ, with units in meters and radians.
+ */
+ public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * <p>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 encoder readings.
+ * @param poseMeters The position on the field that your robot is at.
+ */
+ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+ // Reset state estimate and error covariance
+ m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
+ m_poseBuffer.clear();
+ }
+
+ /**
+ * Gets the estimated robot pose.
+ *
+ * @return The estimated robot pose in meters.
+ */
+ public Pose2d getEstimatedPosition() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
+ *
+ * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+ * PoseEstimator#update} every loop.
+ *
+ * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+ * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+ * don't use your own time source by calling {@link
+ * PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} then you must use a
+ * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
+ * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you
+ * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or
+ * sync the epochs.
+ */
+ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+ // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
+ try {
+ if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+ return;
+ }
+ } catch (NoSuchElementException ex) {
+ return;
+ }
+
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+ var sample = m_poseBuffer.getSample(timestampSeconds);
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // 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.
+ var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+ // Step 4: Convert back to Twist2d.
+ var scaledTwist =
+ new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.resetPosition(
+ sample.get().gyroAngle,
+ sample.get().wheelPositions,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+ m_poseBuffer.addSample(
+ timestampSeconds,
+ new InterpolationRecord(
+ getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
+
+ // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
+ // pose buffer and correct odometry.
+ for (Map.Entry<Double, InterpolationRecord> entry :
+ m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
+ updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
+ }
+ }
+
+ /**
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
+ *
+ * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+ * PoseEstimator#update} every loop.
+ *
+ * <p>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.
+ *
+ * <p>Note that the vision measurement standard deviations passed into this method will continue
+ * to apply to future measurements until a subsequent call to {@link
+ * PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+ *
+ * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+ * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+ * don't use your own time source by calling {@link #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 {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you
+ * should use {@link edu.wpi.first.wpilibj.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.
+ */
+ public void addVisionMeasurement(
+ Pose2d visionRobotPoseMeters,
+ double timestampSeconds,
+ Matrix<N3, N1> visionMeasurementStdDevs) {
+ setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+ addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+ }
+
+ /**
+ * 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 current encoder readings.
+ * @return The estimated pose of the robot in meters.
+ */
+ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+ return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
+ }
+
+ /**
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+ * loop.
+ *
+ * @param currentTimeSeconds Time at which this method was called, in seconds.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The current encoder readings.
+ * @return The estimated pose of the robot in meters.
+ */
+ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) {
+ m_odometry.update(gyroAngle, wheelPositions);
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
+
+ return getEstimatedPosition();
+ }
+
+ /**
+ * Represents an odometry record. The record contains the inputs provided as well as the pose that
+ * was observed based on these inputs, as well as the previous record and its inputs.
+ */
+ private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
+ // The pose observed given the current sensor inputs and the previous pose.
+ private final Pose2d poseMeters;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The current encoder readings.
+ private final T wheelPositions;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param poseMeters The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param wheelPositions The current encoder readings.
+ */
+ private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.wheelPositions = wheelPositions;
+ }
+
+ /**
+ * Return the interpolated record. This object is assumed to be the starting position, or lower
+ * bound.
+ *
+ * @param endValue The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @Override
+ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
+ if (t < 0) {
+ return this;
+ } else if (t >= 1) {
+ return endValue;
+ } else {
+ // Find the new wheel distances.
+ var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
+
+ // Find the new gyro angle.
+ var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent the change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp);
+ twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof PoseEstimator.InterpolationRecord)) {
+ return false;
+ }
+ var record = (PoseEstimator<?>.InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Objects.equals(wheelPositions, record.wheelPositions)
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(gyroAngle, wheelPositions, poseMeters);
+ }
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index 57451a0..f778b62 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -4,24 +4,16 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Arrays;
-import java.util.Map;
-import java.util.Objects;
/**
* This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
@@ -33,15 +25,8 @@
* <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave as regular encoder odometry.
*/
-public class SwerveDrivePoseEstimator {
- private final SwerveDriveKinematics m_kinematics;
- private final SwerveDriveOdometry m_odometry;
- private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPositions> {
private final int m_numModules;
- private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
- private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
- TimeInterpolatableBuffer.createBuffer(1.5);
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
@@ -91,43 +76,13 @@
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
- m_kinematics = kinematics;
- m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
-
- for (int i = 0; i < 3; ++i) {
- m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
- }
+ super(
+ kinematics,
+ new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
+ stateStdDevs,
+ visionMeasurementStdDevs);
m_numModules = modulePositions.length;
-
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- }
-
- /**
- * Sets the pose estimator's trust of global 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 measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
- */
- public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
- var r = new double[3];
- for (int i = 0; i < 3; ++i) {
- r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
- }
-
- // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
- // and C = I. See wpimath/algorithms.md.
- for (int row = 0; row < 3; ++row) {
- if (m_q.get(row, 0) == 0.0) {
- m_visionK.set(row, row, 0.0);
- } else {
- m_visionK.set(
- row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
- }
- }
}
/**
@@ -142,106 +97,7 @@
*/
public void resetPosition(
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
- // Reset state estimate and error covariance
- m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
- m_poseBuffer.clear();
- }
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose in meters.
- */
- public Pose2d getEstimatedPosition() {
- return m_odometry.getPoseMeters();
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
- * while still accounting for measurement noise.
- *
- * <p>This method can be called as infrequently as you want, as long as you are calling {@link
- * SwerveDrivePoseEstimator#update} every loop.
- *
- * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link
- * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you
- * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
- * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
- * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
- * or sync the epochs.
- */
- public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
- // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
- var sample = m_poseBuffer.getSample(timestampSeconds);
-
- if (sample.isEmpty()) {
- return;
- }
-
- // Step 2: Measure the twist between the odometry pose and the vision pose.
- var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
- // 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.
- var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
- // Step 4: Convert back to Twist2d.
- var scaledTwist =
- new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.resetPosition(
- sample.get().gyroAngle,
- sample.get().modulePositions,
- sample.get().poseMeters.exp(scaledTwist));
-
- // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
- // pose buffer and correct odometry.
- for (Map.Entry<Double, InterpolationRecord> entry :
- m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
- updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().modulePositions);
- }
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
- * while still accounting for measurement noise.
- *
- * <p>This method can be called as infrequently as you want, as long as you are calling {@link
- * SwerveDrivePoseEstimator#update} every loop.
- *
- * <p>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.
- *
- * <p>Note that the vision measurement standard deviations passed into this method will continue
- * to apply to future measurements until a subsequent call to {@link
- * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
- *
- * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link
- * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then
- * you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
- * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
- * This means that you should use {@link edu.wpi.first.wpilibj.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.
- */
- public void addVisionMeasurement(
- Pose2d visionRobotPoseMeters,
- double timestampSeconds,
- Matrix<N3, N1> visionMeasurementStdDevs) {
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+ resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
}
/**
@@ -253,7 +109,7 @@
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, modulePositions);
+ return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
}
/**
@@ -267,118 +123,19 @@
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
- if (modulePositions.length != m_numModules) {
+ return updateWithTime(
+ currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+ }
+
+ @Override
+ public Pose2d updateWithTime(
+ double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
+ if (wheelPositions.positions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
- var internalModulePositions = new SwerveModulePosition[m_numModules];
-
- for (int i = 0; i < m_numModules; i++) {
- internalModulePositions[i] =
- new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
- }
-
- m_odometry.update(gyroAngle, internalModulePositions);
-
- m_poseBuffer.addSample(
- currentTimeSeconds,
- new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions));
-
- return getEstimatedPosition();
- }
-
- /**
- * Represents an odometry record. The record contains the inputs provided as well as the pose that
- * was observed based on these inputs, as well as the previous record and its inputs.
- */
- private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
- // The pose observed given the current sensor inputs and the previous pose.
- private final Pose2d poseMeters;
-
- // The current gyro angle.
- private final Rotation2d gyroAngle;
-
- // The distances and rotations measured at each module.
- private final SwerveModulePosition[] modulePositions;
-
- /**
- * Constructs an Interpolation Record with the specified parameters.
- *
- * @param pose The pose observed given the current sensor inputs and the previous pose.
- * @param gyro The current gyro angle.
- * @param wheelPositions The distances and rotations measured at each wheel.
- */
- private InterpolationRecord(
- Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) {
- this.poseMeters = poseMeters;
- this.gyroAngle = gyro;
- this.modulePositions = modulePositions;
- }
-
- /**
- * Return the interpolated record. This object is assumed to be the starting position, or lower
- * bound.
- *
- * @param endValue The upper bound, or end.
- * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
- * @return The interpolated value.
- */
- @Override
- public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
- if (t < 0) {
- return this;
- } else if (t >= 1) {
- return endValue;
- } else {
- // Find the new wheel distances.
- var modulePositions = new SwerveModulePosition[m_numModules];
-
- // Find the distance travelled between this measurement and the interpolated measurement.
- var moduleDeltas = new SwerveModulePosition[m_numModules];
-
- for (int i = 0; i < m_numModules; i++) {
- double ds =
- MathUtil.interpolate(
- this.modulePositions[i].distanceMeters,
- endValue.modulePositions[i].distanceMeters,
- t);
- Rotation2d theta =
- this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
- modulePositions[i] = new SwerveModulePosition(ds, theta);
- moduleDeltas[i] =
- new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
- }
-
- // Find the new gyro angle.
- var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
- // Create a twist to represent this change based on the interpolated sensor inputs.
- Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
- twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
- return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions);
- }
- }
-
- @Override
- public boolean equals(Object obj) {
- if (this == obj) {
- return true;
- }
- if (!(obj instanceof InterpolationRecord)) {
- return false;
- }
- InterpolationRecord record = (InterpolationRecord) obj;
- return Objects.equals(gyroAngle, record.gyroAngle)
- && Arrays.equals(modulePositions, record.modulePositions)
- && Objects.equals(poseMeters, record.poseMeters);
- }
-
- @Override
- public int hashCode() {
- return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
- }
+ return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
index d668564..a5de856 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -64,6 +64,10 @@
/**
* Constructs an Unscented Kalman Filter.
*
+ * <p>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 states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -100,6 +104,10 @@
* custom functions for arithmetic can be useful if you have angles in the state or measurements,
* because they allow you to correctly account for the modular nature of angle arithmetic.
*
+ * <p>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 states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -331,7 +339,7 @@
// Discretize Q before projecting mean and covariance forward
Matrix<States, States> contA =
NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
- var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+ var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
var squareRootDiscQ = discQ.lltDecompose(true);
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
@@ -373,6 +381,19 @@
/**
* Correct the state estimate x-hat using the measurements in y.
*
+ * <p>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.
+ */
+ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+ correct(m_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.
+ *
* <p>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).
@@ -382,7 +403,7 @@
* @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 Measurement noise covariance matrix (continuous-time).
+ * @param R Continuous measurement noise covariance matrix.
*/
public <R extends Num> void correct(
Nat<R> rows,
@@ -411,7 +432,7 @@
* @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 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 measurement vectors (i.e. it
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
index 8da45e9..3472198 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
@@ -4,7 +4,7 @@
package edu.wpi.first.math.filter;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.math.MathSharedStore;
/**
* A simple debounce filter for boolean streams. Requires that the boolean change value from
@@ -60,11 +60,11 @@
}
private void resetTimer() {
- m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6;
+ m_prevTimeSeconds = MathSharedStore.getTimestamp();
}
private boolean hasElapsed() {
- return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds;
+ return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds;
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
index 668b8b1..6c34f35 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
@@ -4,8 +4,8 @@
package edu.wpi.first.math.filter;
+import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.WPIUtilJNI;
/**
* A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -33,21 +33,7 @@
m_positiveRateLimit = positiveRateLimit;
m_negativeRateLimit = negativeRateLimit;
m_prevVal = initialValue;
- m_prevTime = WPIUtilJNI.now() * 1e-6;
- }
-
- /**
- * 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, in units per second.
- * @param initalValue The initial value of the input.
- * @deprecated Use SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double
- * initalValue) instead.
- */
- @Deprecated(since = "2023", forRemoval = true)
- public SlewRateLimiter(double rateLimit, double initalValue) {
- this(rateLimit, -rateLimit, initalValue);
+ m_prevTime = MathSharedStore.getTimestamp();
}
/**
@@ -67,7 +53,7 @@
* @return The filtered value, which will not change faster than the slew rate.
*/
public double calculate(double input) {
- double currentTime = WPIUtilJNI.now() * 1e-6;
+ double currentTime = MathSharedStore.getTimestamp();
double elapsedTime = currentTime - m_prevTime;
m_prevVal +=
MathUtil.clamp(
@@ -85,6 +71,6 @@
*/
public void reset(double value) {
m_prevVal = value;
- m_prevTime = WPIUtilJNI.now() * 1e-6;
+ m_prevTime = MathSharedStore.getTimestamp();
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
index 5733177..9e827c3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
@@ -124,7 +124,9 @@
*/
public static Transform3d convert(
Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
+ var coordRot = from.m_rotation.minus(to.m_rotation);
return new Transform3d(
- convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
+ convert(transform.getTranslation(), from, to),
+ coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index bce832e..bc7f536 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -9,7 +9,15 @@
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/** Represents a 2D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -136,6 +144,16 @@
}
/**
+ * Rotates the pose around the origin and returns the new pose.
+ *
+ * @param other The rotation to transform the pose by.
+ * @return The transformed pose.
+ */
+ public Pose2d rotateBy(Rotation2d other) {
+ return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+ }
+
+ /**
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
@@ -238,6 +256,23 @@
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
+ /**
+ * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same
+ * distance from this pose, return the one with the closest rotation component.
+ *
+ * @param poses The list of poses to find the nearest.
+ * @return The nearest Pose2d from the list.
+ */
+ public Pose2d nearest(List<Pose2d> poses) {
+ return Collections.min(
+ poses,
+ Comparator.comparing(
+ (Pose2d other) -> this.getTranslation().getDistance(other.getTranslation()))
+ .thenComparing(
+ (Pose2d other) ->
+ Math.abs(this.getRotation().minus(other.getRotation()).getRadians())));
+ }
+
@Override
public String toString() {
return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
@@ -275,4 +310,83 @@
return this.exp(scaledTwist);
}
}
+
+ public static final class AStruct implements Struct<Pose2d> {
+ @Override
+ public Class<Pose2d> getTypeClass() {
+ return Pose2d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Pose2d";
+ }
+
+ @Override
+ public int getSize() {
+ return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+ }
+
+ @Override
+ public String getSchema() {
+ return "Translation2d translation;Rotation2d rotation";
+ }
+
+ @Override
+ public Struct<?>[] getNested() {
+ return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+ }
+
+ @Override
+ public Pose2d unpack(ByteBuffer bb) {
+ Translation2d translation = Translation2d.struct.unpack(bb);
+ Rotation2d rotation = Rotation2d.struct.unpack(bb);
+ return new Pose2d(translation, rotation);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Pose2d value) {
+ Translation2d.struct.pack(bb, value.m_translation);
+ Rotation2d.struct.pack(bb, value.m_rotation);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Pose2d, ProtobufPose2d> {
+ @Override
+ public Class<Pose2d> getTypeClass() {
+ return Pose2d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufPose2d.getDescriptor();
+ }
+
+ @Override
+ public Protobuf<?, ?>[] getNested() {
+ return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+ }
+
+ @Override
+ public ProtobufPose2d createMessage() {
+ return ProtobufPose2d.newInstance();
+ }
+
+ @Override
+ public Pose2d unpack(ProtobufPose2d msg) {
+ return new Pose2d(
+ Translation2d.proto.unpack(msg.getTranslation()),
+ Rotation2d.proto.unpack(msg.getRotation()));
+ }
+
+ @Override
+ public void pack(ProtobufPose2d msg, Pose2d value) {
+ Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+ Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
index 8e3a3fe..2ea7094 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -8,14 +8,14 @@
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/** Represents a 3D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -68,7 +68,10 @@
}
/**
- * Transforms the pose by the given transformation and returns the new transformed pose.
+ * 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 {@link
+ * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+ * origin.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
@@ -156,8 +159,21 @@
}
/**
- * 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.
+ */
+ public Pose3d rotateBy(Rotation3d other) {
+ return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+ }
+
+ /**
+ * 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 {@link
+ * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+ * origin.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
@@ -200,36 +216,28 @@
* @return The new pose of the robot.
*/
public Pose3d exp(Twist3d twist) {
- final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
- final var OmegaSq = Omega.times(Omega);
-
- double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
-
- // Get left Jacobian of SO3. See first line in right column of
- // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
- Matrix<N3, N3> J;
- if (thetaSq < 1E-9 * 1E-9) {
- // J = I + 0.5ω
- J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
- } else {
- double theta = Math.sqrt(thetaSq);
- // J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
- J =
- Matrix.eye(Nat.N3())
- .plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
- .plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
- }
-
- // Get translation component
- final var translation =
- J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
-
- final var transform =
- new Transform3d(
- new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
- new Rotation3d(twist.rx, twist.ry, twist.rz));
-
- return this.plus(transform);
+ var quaternion = this.getRotation().getQuaternion();
+ double[] resultArray =
+ WPIMathJNI.expPose3d(
+ this.getX(),
+ this.getY(),
+ this.getZ(),
+ quaternion.getW(),
+ quaternion.getX(),
+ quaternion.getY(),
+ quaternion.getZ(),
+ twist.dx,
+ twist.dy,
+ twist.dz,
+ twist.rx,
+ twist.ry,
+ twist.rz);
+ return new Pose3d(
+ resultArray[0],
+ resultArray[1],
+ resultArray[2],
+ new Rotation3d(
+ new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
}
/**
@@ -240,50 +248,31 @@
* @return The twist that maps this to end.
*/
public Twist3d log(Pose3d end) {
- final var transform = end.relativeTo(this);
-
- final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
-
- final var Omega = rotationVectorToMatrix(rotVec);
- final var OmegaSq = Omega.times(Omega);
-
- double thetaSq =
- rotVec.get(0, 0) * rotVec.get(0, 0)
- + rotVec.get(1, 0) * rotVec.get(1, 0)
- + rotVec.get(2, 0) * rotVec.get(2, 0);
-
- // Get left Jacobian inverse of SO3. See fourth line in right column of
- // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
- Matrix<N3, N3> Jinv;
- if (thetaSq < 1E-9 * 1E-9) {
- // J⁻¹ = I − 0.5ω + 1/12 ω²
- Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
- } else {
- double theta = Math.sqrt(thetaSq);
- double halfTheta = 0.5 * theta;
-
- // J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
- Jinv =
- Matrix.eye(Nat.N3())
- .minus(Omega.times(0.5))
- .plus(
- OmegaSq.times(
- (1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
- }
-
- // Get dtranslation component
- final var dtranslation =
- Jinv.times(
- new MatBuilder<>(Nat.N3(), Nat.N1())
- .fill(transform.getX(), transform.getY(), transform.getZ()));
-
+ var thisQuaternion = this.getRotation().getQuaternion();
+ var endQuaternion = end.getRotation().getQuaternion();
+ double[] resultArray =
+ WPIMathJNI.logPose3d(
+ this.getX(),
+ this.getY(),
+ this.getZ(),
+ thisQuaternion.getW(),
+ thisQuaternion.getX(),
+ thisQuaternion.getY(),
+ thisQuaternion.getZ(),
+ end.getX(),
+ end.getY(),
+ end.getZ(),
+ endQuaternion.getW(),
+ endQuaternion.getX(),
+ endQuaternion.getY(),
+ endQuaternion.getZ());
return new Twist3d(
- dtranslation.get(0, 0),
- dtranslation.get(1, 0),
- dtranslation.get(2, 0),
- rotVec.get(0, 0),
- rotVec.get(1, 0),
- rotVec.get(2, 0));
+ resultArray[0],
+ resultArray[1],
+ resultArray[2],
+ resultArray[3],
+ resultArray[4],
+ resultArray[5]);
}
/**
@@ -335,30 +324,82 @@
}
}
- /**
- * Applies the hat operator to a rotation vector.
- *
- * <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
- * algebra element (a 3x3 rotation matrix).
- *
- * @param rotation The rotation vector.
- * @return The rotation vector as a 3x3 rotation matrix.
- */
- private Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
- // Given a rotation vector <a, b, c>,
- // [ 0 -c b]
- // Omega = [ c 0 -a]
- // [-b a 0]
- return new MatBuilder<>(Nat.N3(), Nat.N3())
- .fill(
- 0.0,
- -rotation.get(2, 0),
- rotation.get(1, 0),
- rotation.get(2, 0),
- 0.0,
- -rotation.get(0, 0),
- -rotation.get(1, 0),
- rotation.get(0, 0),
- 0.0);
+ public static final class AStruct implements Struct<Pose3d> {
+ @Override
+ public Class<Pose3d> getTypeClass() {
+ return Pose3d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Pose3d";
+ }
+
+ @Override
+ public int getSize() {
+ return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+ }
+
+ @Override
+ public String getSchema() {
+ return "Translation3d translation;Rotation3d rotation";
+ }
+
+ @Override
+ public Struct<?>[] getNested() {
+ return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+ }
+
+ @Override
+ public Pose3d unpack(ByteBuffer bb) {
+ Translation3d translation = Translation3d.struct.unpack(bb);
+ Rotation3d rotation = Rotation3d.struct.unpack(bb);
+ return new Pose3d(translation, rotation);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Pose3d value) {
+ Translation3d.struct.pack(bb, value.m_translation);
+ Rotation3d.struct.pack(bb, value.m_rotation);
+ }
}
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Pose3d, ProtobufPose3d> {
+ @Override
+ public Class<Pose3d> getTypeClass() {
+ return Pose3d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufPose3d.getDescriptor();
+ }
+
+ @Override
+ public Protobuf<?, ?>[] getNested() {
+ return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+ }
+
+ @Override
+ public ProtobufPose3d createMessage() {
+ return ProtobufPose3d.newInstance();
+ }
+
+ @Override
+ public Pose3d unpack(ProtobufPose3d msg) {
+ return new Pose3d(
+ Translation3d.proto.unpack(msg.getTranslation()),
+ Rotation3d.proto.unpack(msg.getRotation()));
+ }
+
+ @Override
+ public void pack(ProtobufPose3d msg, Pose3d value) {
+ Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+ Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
index cadfaa4..23fd26b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
@@ -11,18 +11,30 @@
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Quaternion {
- private final double m_r;
- private final Vector<N3> m_v;
+ // Scalar r in versor form
+ private final double m_w;
+
+ // Vector v in versor form
+ private final double m_x;
+ private final double m_y;
+ private final double m_z;
/** Constructs a quaternion with a default angle of 0 degrees. */
public Quaternion() {
- m_r = 1.0;
- m_v = VecBuilder.fill(0.0, 0.0, 0.0);
+ m_w = 1.0;
+ m_x = 0.0;
+ m_y = 0.0;
+ m_z = 0.0;
}
/**
@@ -39,8 +51,52 @@
@JsonProperty(required = true, value = "X") double x,
@JsonProperty(required = true, value = "Y") double y,
@JsonProperty(required = true, value = "Z") double z) {
- m_r = w;
- m_v = VecBuilder.fill(x, y, z);
+ m_w = w;
+ m_x = x;
+ m_y = y;
+ m_z = z;
+ }
+
+ /**
+ * Adds another quaternion to this quaternion entrywise.
+ *
+ * @param other The other quaternion.
+ * @return The quaternion sum.
+ */
+ public Quaternion plus(Quaternion other) {
+ return new Quaternion(
+ getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
+ }
+
+ /**
+ * Subtracts another quaternion from this quaternion entrywise.
+ *
+ * @param other The other quaternion.
+ * @return The quaternion difference.
+ */
+ public Quaternion minus(Quaternion other) {
+ return new Quaternion(
+ getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
+ }
+
+ /**
+ * Divides by a scalar.
+ *
+ * @param scalar The value to scale each component by.
+ * @return The scaled quaternion.
+ */
+ public Quaternion divide(double scalar) {
+ return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
+ }
+
+ /**
+ * Multiplies with a scalar.
+ *
+ * @param scalar The value to scale each component by.
+ * @return The scaled quaternion.
+ */
+ public Quaternion times(double scalar) {
+ return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
}
/**
@@ -51,28 +107,29 @@
*/
public Quaternion times(Quaternion other) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
- final var r1 = m_r;
- final var v1 = m_v;
- final var r2 = other.m_r;
- final var v2 = other.m_v;
+ final var r1 = m_w;
+ final var r2 = other.m_w;
+
+ // v₁ ⋅ v₂
+ double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
// v₁ x v₂
- var cross =
- VecBuilder.fill(
- v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
- v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
- v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
+ double cross_x = m_y * other.m_z - other.m_y * m_z;
+ double cross_y = other.m_x * m_z - m_x * other.m_z;
+ double cross_z = m_x * other.m_y - other.m_x * m_y;
- // v = r₁v₂ + r₂v₁ + v₁ x v₂
- final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
-
- return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+ return new Quaternion(
+ // r = r₁r₂ − v₁ ⋅ v₂
+ r1 * r2 - dot,
+ // v = r₁v₂ + r₂v₁ + v₁ x v₂
+ r1 * other.m_x + r2 * m_x + cross_x,
+ r1 * other.m_y + r2 * m_y + cross_y,
+ r1 * other.m_z + r2 * m_z + cross_z);
}
@Override
public String toString() {
- return String.format(
- "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
+ return String.format("Quaternion(%s, %s, %s, %s)", getW(), getX(), getY(), getZ());
}
/**
@@ -86,14 +143,37 @@
if (obj instanceof Quaternion) {
var other = (Quaternion) obj;
- return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+ return Math.abs(dot(other) - norm() * other.norm()) < 1e-9
+ && Math.abs(norm() - other.norm()) < 1e-9;
}
return false;
}
@Override
public int hashCode() {
- return Objects.hash(m_r, m_v);
+ return Objects.hash(m_w, m_x, m_y, m_z);
+ }
+
+ /**
+ * Returns the conjugate of the quaternion.
+ *
+ * @return The conjugate quaternion.
+ */
+ public Quaternion conjugate() {
+ return new Quaternion(getW(), -getX(), -getY(), -getZ());
+ }
+
+ /**
+ * Returns the elementwise product of two quaternions.
+ *
+ * @param other The other quaternion.
+ * @return The dot product of two quaternions.
+ */
+ public double dot(final Quaternion other) {
+ return getW() * other.getW()
+ + getX() * other.getX()
+ + getY() * other.getY()
+ + getZ() * other.getZ();
}
/**
@@ -102,7 +182,17 @@
* @return The inverse quaternion.
*/
public Quaternion inverse() {
- return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
+ var norm = norm();
+ return conjugate().divide(norm * norm);
+ }
+
+ /**
+ * Calculates the L2 norm of the quaternion.
+ *
+ * @return The L2 norm.
+ */
+ public double norm() {
+ return Math.sqrt(dot(this));
}
/**
@@ -111,7 +201,7 @@
* @return The normalized quaternion.
*/
public Quaternion normalize() {
- double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
+ double norm = norm();
if (norm == 0.0) {
return new Quaternion();
} else {
@@ -120,13 +210,111 @@
}
/**
+ * Rational power of a quaternion.
+ *
+ * @param t the power to raise this quaternion to.
+ * @return The quaternion power
+ */
+ public Quaternion pow(double t) {
+ // q^t = e^(ln(q^t)) = e^(t * ln(q))
+ return this.log().times(t).exp();
+ }
+
+ /**
+ * Matrix exponential of a quaternion.
+ *
+ * @param adjustment the "Twist" that will be applied to this quaternion.
+ * @return The quaternion product of exp(adjustment) * this
+ */
+ public Quaternion exp(Quaternion adjustment) {
+ return adjustment.exp().times(this);
+ }
+
+ /**
+ * Matrix exponential of a quaternion.
+ *
+ * <p>source: wpimath/algorithms.md
+ *
+ * <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
+ * fromRotationVector}
+ *
+ * @return The Matrix exponential of this quaternion.
+ */
+ public Quaternion exp() {
+ var scalar = Math.exp(getW());
+
+ var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+ var cosine = Math.cos(axial_magnitude);
+
+ double axial_scalar;
+
+ if (axial_magnitude < 1e-9) {
+ // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
+ var axial_magnitude_sq = axial_magnitude * axial_magnitude;
+ var 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 = Math.sin(axial_magnitude) / axial_magnitude;
+ }
+
+ return new Quaternion(
+ cosine * scalar,
+ getX() * axial_scalar * scalar,
+ getY() * axial_scalar * scalar,
+ getZ() * axial_scalar * scalar);
+ }
+
+ /**
+ * Log operator of a quaternion.
+ *
+ * @param end The quaternion to map this quaternion onto.
+ * @return The "Twist" that maps this quaternion to the argument.
+ */
+ public Quaternion log(Quaternion end) {
+ return end.times(this.inverse()).log();
+ }
+
+ /**
+ * The Log operator of a general quaternion.
+ *
+ * <p>source: wpimath/algorithms.md
+ *
+ * <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
+ * toRotationVector}
+ *
+ * @return The logarithm of this quaternion.
+ */
+ public Quaternion log() {
+ var scalar = Math.log(norm());
+
+ var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+
+ var s_norm = getW() / norm();
+
+ if (Math.abs(s_norm + 1) < 1e-9) {
+ return new Quaternion(scalar, -Math.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²/3*x³ + O(y⁴)
+ v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
+ } else {
+ v_scalar = Math.atan2(v_norm, getW()) / v_norm;
+ }
+
+ return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
+ }
+
+ /**
* Returns W component of the quaternion.
*
* @return W component of the quaternion.
*/
@JsonProperty(value = "W")
public double getW() {
- return m_r;
+ return m_w;
}
/**
@@ -136,7 +324,7 @@
*/
@JsonProperty(value = "X")
public double getX() {
- return m_v.get(0, 0);
+ return m_x;
}
/**
@@ -146,7 +334,7 @@
*/
@JsonProperty(value = "Y")
public double getY() {
- return m_v.get(1, 0);
+ return m_y;
}
/**
@@ -156,7 +344,38 @@
*/
@JsonProperty(value = "Z")
public double getZ() {
- return m_v.get(2, 0);
+ return m_z;
+ }
+
+ /**
+ * Returns the quaternion representation of this rotation vector.
+ *
+ * <p>This is also the exp operator of 𝖘𝖔(3).
+ *
+ * <p>source: wpimath/algorithms.md
+ *
+ * @param rvec The rotation vector.
+ * @return The quaternion representation of this rotation vector.
+ */
+ public static Quaternion fromRotationVector(Vector<N3> rvec) {
+ double theta = rvec.norm();
+
+ double cos = Math.cos(theta / 2);
+
+ double axial_scalar;
+
+ if (theta < 1e-9) {
+ // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
+ axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+ } else {
+ axial_scalar = Math.sin(theta / 2) / theta;
+ }
+
+ return new Quaternion(
+ cos,
+ axial_scalar * rvec.get(0, 0),
+ axial_scalar * rvec.get(1, 0),
+ axial_scalar * rvec.get(2, 0));
}
/**
@@ -171,16 +390,89 @@
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
- double norm = m_v.norm();
+ double norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+ double coeff;
if (norm < 1e-9) {
- return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
+ coeff = 2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW());
} else {
if (getW() < 0.0) {
- return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
+ coeff = 2.0 * Math.atan2(-norm, -getW()) / norm;
} else {
- return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
+ coeff = 2.0 * Math.atan2(norm, getW()) / norm;
}
}
+
+ return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
}
+
+ public static final class AStruct implements Struct<Quaternion> {
+ @Override
+ public Class<Quaternion> getTypeClass() {
+ return Quaternion.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Quaternion";
+ }
+
+ @Override
+ public int getSize() {
+ return kSizeDouble * 4;
+ }
+
+ @Override
+ public String getSchema() {
+ return "double w;double x;double y;double z";
+ }
+
+ @Override
+ public Quaternion unpack(ByteBuffer bb) {
+ double w = bb.getDouble();
+ double x = bb.getDouble();
+ double y = bb.getDouble();
+ double z = bb.getDouble();
+ return new Quaternion(w, x, y, z);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Quaternion value) {
+ bb.putDouble(value.getW());
+ bb.putDouble(value.getX());
+ bb.putDouble(value.getY());
+ bb.putDouble(value.getZ());
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Quaternion, ProtobufQuaternion> {
+ @Override
+ public Class<Quaternion> getTypeClass() {
+ return Quaternion.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufQuaternion.getDescriptor();
+ }
+
+ @Override
+ public ProtobufQuaternion createMessage() {
+ return ProtobufQuaternion.newInstance();
+ }
+
+ @Override
+ public Quaternion unpack(ProtobufQuaternion msg) {
+ return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
+ }
+
+ @Override
+ public void pack(ProtobufQuaternion msg, Quaternion value) {
+ msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ());
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 5be6156..0556669 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,8 +10,13 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/**
* A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
@@ -256,4 +261,67 @@
public Rotation2d interpolate(Rotation2d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
+
+ public static final class AStruct implements Struct<Rotation2d> {
+ @Override
+ public Class<Rotation2d> getTypeClass() {
+ return Rotation2d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Rotation2d";
+ }
+
+ @Override
+ public int getSize() {
+ return kSizeDouble;
+ }
+
+ @Override
+ public String getSchema() {
+ return "double value";
+ }
+
+ @Override
+ public Rotation2d unpack(ByteBuffer bb) {
+ return new Rotation2d(bb.getDouble());
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Rotation2d value) {
+ bb.putDouble(value.m_value);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
+ @Override
+ public Class<Rotation2d> getTypeClass() {
+ return Rotation2d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufRotation2d.getDescriptor();
+ }
+
+ @Override
+ public ProtobufRotation2d createMessage() {
+ return ProtobufRotation2d.newInstance();
+ }
+
+ @Override
+ public Rotation2d unpack(ProtobufRotation2d msg) {
+ return new Rotation2d(msg.getValue());
+ }
+
+ @Override
+ public void pack(ProtobufRotation2d msg, Rotation2d value) {
+ msg.setValue(value.m_value);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
index 3226e31..b434523 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -17,17 +17,24 @@
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/** A rotation in a 3D coordinate frame represented by a quaternion. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation3d implements Interpolatable<Rotation3d> {
- private Quaternion m_q = new Quaternion();
+ private final Quaternion m_q;
/** Constructs a Rotation3d with a default angle of 0 degrees. */
- public Rotation3d() {}
+ public Rotation3d() {
+ m_q = new Quaternion();
+ }
/**
* Constructs a Rotation3d from a quaternion.
@@ -73,6 +80,17 @@
}
/**
+ * 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.
+ */
+ public Rotation3d(Vector<N3> rvec) {
+ this(rvec, rvec.norm());
+ }
+
+ /**
* Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
* normalized.
*
@@ -82,6 +100,7 @@
public Rotation3d(Vector<N3> axis, double angleRadians) {
double norm = axis.norm();
if (norm == 0.0) {
+ m_q = new Quaternion();
return;
}
@@ -175,6 +194,7 @@
if (dotNorm > 1.0 - 1E-9) {
// If the dot product is 1, the two vectors point in the same direction so
// there's no rotation. The default initialization of m_q will work.
+ m_q = new Quaternion();
return;
} else if (dotNorm < -1.0 + 1E-9) {
// If the dot product is -1, the two vectors point in opposite directions
@@ -267,9 +287,14 @@
}
/**
- * 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, {@code new
+ * Rotation3d(Units.degreesToRadians(90), 0, 0).rotateBy(new Rotation3d(0,
+ * Units.degreesToRadians(45), 0))} rotates by 90 degrees around the +X axis and then by 45
+ * degrees around the global +Y axis. (This is equivalent to {@code new
+ * Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(45), 0)})
*
- * @param other The rotation to rotate by.
+ * @param other The extrinsic rotation to rotate by.
* @return The new rotated Rotation3d.
*/
public Rotation3d rotateBy(Rotation3d other) {
@@ -297,8 +322,15 @@
final var y = m_q.getY();
final var z = m_q.getZ();
- // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
- return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
+ // wpimath/algorithms.md
+ final var cxcy = 1.0 - 2.0 * (x * x + y * y);
+ final var sxcy = 2.0 * (w * x + y * z);
+ final var cy_sq = cxcy * cxcy + sxcy * sxcy;
+ if (cy_sq > 1e-20) {
+ return Math.atan2(sxcy, cxcy);
+ } else {
+ return 0.0;
+ }
}
/**
@@ -312,7 +344,7 @@
final var y = m_q.getY();
final var z = m_q.getZ();
- // 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 (Math.abs(ratio) >= 1.0) {
return Math.copySign(Math.PI / 2.0, ratio);
@@ -332,8 +364,15 @@
final var y = m_q.getY();
final var z = m_q.getZ();
- // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
- return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
+ // wpimath/algorithms.md
+ final var cycz = 1.0 - 2.0 * (y * y + z * z);
+ final var cysz = 2.0 * (w * z + x * y);
+ final var cy_sq = cycz * cycz + cysz * cysz;
+ if (cy_sq > 1e-20) {
+ return Math.atan2(cysz, cycz);
+ } else {
+ return Math.atan2(2.0 * w * z, w * w - z * z);
+ }
}
/**
@@ -386,7 +425,7 @@
public boolean equals(Object obj) {
if (obj instanceof Rotation3d) {
var other = (Rotation3d) obj;
- return m_q.equals(other.m_q);
+ return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9;
}
return false;
}
@@ -400,4 +439,77 @@
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
+
+ public static final class AStruct implements Struct<Rotation3d> {
+ @Override
+ public Class<Rotation3d> getTypeClass() {
+ return Rotation3d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Rotation3d";
+ }
+
+ @Override
+ public int getSize() {
+ return Quaternion.struct.getSize();
+ }
+
+ @Override
+ public String getSchema() {
+ return "Quaternion q";
+ }
+
+ @Override
+ public Struct<?>[] getNested() {
+ return new Struct<?>[] {Quaternion.struct};
+ }
+
+ @Override
+ public Rotation3d unpack(ByteBuffer bb) {
+ return new Rotation3d(Quaternion.struct.unpack(bb));
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Rotation3d value) {
+ Quaternion.struct.pack(bb, value.m_q);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Rotation3d, ProtobufRotation3d> {
+ @Override
+ public Class<Rotation3d> getTypeClass() {
+ return Rotation3d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufRotation3d.getDescriptor();
+ }
+
+ @Override
+ public Protobuf<?, ?>[] getNested() {
+ return new Protobuf<?, ?>[] {Quaternion.proto};
+ }
+
+ @Override
+ public ProtobufRotation3d createMessage() {
+ return ProtobufRotation3d.newInstance();
+ }
+
+ @Override
+ public Rotation3d unpack(ProtobufRotation3d msg) {
+ return new Rotation3d(Quaternion.proto.unpack(msg.getQ()));
+ }
+
+ @Override
+ public void pack(ProtobufRotation3d msg, Rotation3d value) {
+ Quaternion.proto.pack(msg.getMutableQ(), value.m_q);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
index c3c6b0c..c7959ba 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
@@ -4,9 +4,14 @@
package edu.wpi.first.math.geometry;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
-/** Represents a transformation for a Pose2d. */
+/** Represents a transformation for a Pose2d in the pose's frame. */
public class Transform2d {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
@@ -40,6 +45,18 @@
m_rotation = 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.
+ */
+ public Transform2d(double x, double y, Rotation2d rotation) {
+ m_translation = new Translation2d(x, y);
+ m_rotation = rotation;
+ }
+
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform2d() {
m_translation = new Translation2d();
@@ -67,7 +84,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.
@@ -150,4 +168,83 @@
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
+
+ public static final class AStruct implements Struct<Transform2d> {
+ @Override
+ public Class<Transform2d> getTypeClass() {
+ return Transform2d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Transform2d";
+ }
+
+ @Override
+ public int getSize() {
+ return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+ }
+
+ @Override
+ public String getSchema() {
+ return "Translation2d translation;Rotation2d rotation";
+ }
+
+ @Override
+ public Struct<?>[] getNested() {
+ return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+ }
+
+ @Override
+ public Transform2d unpack(ByteBuffer bb) {
+ Translation2d translation = Translation2d.struct.unpack(bb);
+ Rotation2d rotation = Rotation2d.struct.unpack(bb);
+ return new Transform2d(translation, rotation);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Transform2d value) {
+ Translation2d.struct.pack(bb, value.m_translation);
+ Rotation2d.struct.pack(bb, value.m_rotation);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Transform2d, ProtobufTransform2d> {
+ @Override
+ public Class<Transform2d> getTypeClass() {
+ return Transform2d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufTransform2d.getDescriptor();
+ }
+
+ @Override
+ public Protobuf<?, ?>[] getNested() {
+ return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+ }
+
+ @Override
+ public ProtobufTransform2d createMessage() {
+ return ProtobufTransform2d.newInstance();
+ }
+
+ @Override
+ public Transform2d unpack(ProtobufTransform2d msg) {
+ return new Transform2d(
+ Translation2d.proto.unpack(msg.getTranslation()),
+ Rotation2d.proto.unpack(msg.getRotation()));
+ }
+
+ @Override
+ public void pack(ProtobufTransform2d msg, Transform2d value) {
+ Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+ Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
index 4920ef6..223a14b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
@@ -4,9 +4,14 @@
package edu.wpi.first.math.geometry;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
-/** Represents a transformation for a Pose3d. */
+/** Represents a transformation for a Pose3d in the pose's frame. */
public class Transform3d {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
@@ -40,6 +45,19 @@
m_rotation = 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.
+ */
+ public Transform3d(double x, double y, double z, Rotation3d rotation) {
+ m_translation = new Translation3d(x, y, z);
+ m_rotation = rotation;
+ }
+
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform3d() {
m_translation = new Translation3d();
@@ -67,7 +85,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.
@@ -159,4 +178,83 @@
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
+
+ public static final class AStruct implements Struct<Transform3d> {
+ @Override
+ public Class<Transform3d> getTypeClass() {
+ return Transform3d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Transform3d";
+ }
+
+ @Override
+ public int getSize() {
+ return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+ }
+
+ @Override
+ public String getSchema() {
+ return "Translation3d translation;Rotation3d rotation";
+ }
+
+ @Override
+ public Struct<?>[] getNested() {
+ return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+ }
+
+ @Override
+ public Transform3d unpack(ByteBuffer bb) {
+ Translation3d translation = Translation3d.struct.unpack(bb);
+ Rotation3d rotation = Rotation3d.struct.unpack(bb);
+ return new Transform3d(translation, rotation);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Transform3d value) {
+ Translation3d.struct.pack(bb, value.m_translation);
+ Rotation3d.struct.pack(bb, value.m_rotation);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Transform3d, ProtobufTransform3d> {
+ @Override
+ public Class<Transform3d> getTypeClass() {
+ return Transform3d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufTransform3d.getDescriptor();
+ }
+
+ @Override
+ public Protobuf<?, ?>[] getNested() {
+ return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+ }
+
+ @Override
+ public ProtobufTransform3d createMessage() {
+ return ProtobufTransform3d.newInstance();
+ }
+
+ @Override
+ public Transform3d unpack(ProtobufTransform3d msg) {
+ return new Transform3d(
+ Translation3d.proto.unpack(msg.getTranslation()),
+ Rotation3d.proto.unpack(msg.getRotation()));
+ }
+
+ @Override
+ public void pack(ProtobufTransform3d msg, Transform3d value) {
+ Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+ Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 2d57edc..96e0001 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -10,7 +10,15 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/**
* Represents a translation in 2D space. This object can be used to represent a point or a vector.
@@ -185,6 +193,16 @@
return new Translation2d(m_x / scalar, m_y / scalar);
}
+ /**
+ * Returns the nearest Translation2d from a list of translations.
+ *
+ * @param translations The list of translations.
+ * @return The nearest Translation2d from the list.
+ */
+ public Translation2d nearest(List<Translation2d> translations) {
+ return Collections.min(translations, Comparator.comparing(this::getDistance));
+ }
+
@Override
public String toString() {
return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
@@ -216,4 +234,70 @@
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t));
}
+
+ public static final class AStruct implements Struct<Translation2d> {
+ @Override
+ public Class<Translation2d> getTypeClass() {
+ return Translation2d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Translation2d";
+ }
+
+ @Override
+ public int getSize() {
+ return kSizeDouble * 2;
+ }
+
+ @Override
+ public String getSchema() {
+ return "double x;double y";
+ }
+
+ @Override
+ public Translation2d unpack(ByteBuffer bb) {
+ double x = bb.getDouble();
+ double y = bb.getDouble();
+ return new Translation2d(x, y);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Translation2d value) {
+ bb.putDouble(value.m_x);
+ bb.putDouble(value.m_y);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Translation2d, ProtobufTranslation2d> {
+ @Override
+ public Class<Translation2d> getTypeClass() {
+ return Translation2d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufTranslation2d.getDescriptor();
+ }
+
+ @Override
+ public ProtobufTranslation2d createMessage() {
+ return ProtobufTranslation2d.newInstance();
+ }
+
+ @Override
+ public Translation2d unpack(ProtobufTranslation2d msg) {
+ return new Translation2d(msg.getX(), msg.getY());
+ }
+
+ @Override
+ public void pack(ProtobufTranslation2d msg, Translation2d value) {
+ msg.setX(value.m_x).setY(value.m_y);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 810f56c..bc55f65 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -10,7 +10,12 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/**
* Represents a translation in 3D space. This object can be used to represent a point or a vector.
@@ -231,4 +236,72 @@
MathUtil.interpolate(this.getY(), endValue.getY(), t),
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
+
+ public static final class AStruct implements Struct<Translation3d> {
+ @Override
+ public Class<Translation3d> getTypeClass() {
+ return Translation3d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Translation3d";
+ }
+
+ @Override
+ public int getSize() {
+ return kSizeDouble * 3;
+ }
+
+ @Override
+ public String getSchema() {
+ return "double x;double y;double z";
+ }
+
+ @Override
+ public Translation3d unpack(ByteBuffer bb) {
+ double x = bb.getDouble();
+ double y = bb.getDouble();
+ double z = bb.getDouble();
+ return new Translation3d(x, y, z);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Translation3d value) {
+ bb.putDouble(value.m_x);
+ bb.putDouble(value.m_y);
+ bb.putDouble(value.m_z);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Translation3d, ProtobufTranslation3d> {
+ @Override
+ public Class<Translation3d> getTypeClass() {
+ return Translation3d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufTranslation3d.getDescriptor();
+ }
+
+ @Override
+ public ProtobufTranslation3d createMessage() {
+ return ProtobufTranslation3d.newInstance();
+ }
+
+ @Override
+ public Translation3d unpack(ProtobufTranslation3d msg) {
+ return new Translation3d(msg.getX(), msg.getY(), msg.getZ());
+ }
+
+ @Override
+ public void pack(ProtobufTranslation3d msg, Translation3d value) {
+ msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
index be6831e..a4ae9f8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
@@ -4,11 +4,16 @@
package edu.wpi.first.math.geometry;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/**
* A change in distance along a 2D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose2d objects from a Twist2d and vise versa.
+ * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@@ -62,4 +67,72 @@
public int hashCode() {
return Objects.hash(dx, dy, dtheta);
}
+
+ public static final class AStruct implements Struct<Twist2d> {
+ @Override
+ public Class<Twist2d> getTypeClass() {
+ return Twist2d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Twist2d";
+ }
+
+ @Override
+ public int getSize() {
+ return kSizeDouble * 3;
+ }
+
+ @Override
+ public String getSchema() {
+ return "double dx;double dy;double dtheta";
+ }
+
+ @Override
+ public Twist2d unpack(ByteBuffer bb) {
+ double dx = bb.getDouble();
+ double dy = bb.getDouble();
+ double dtheta = bb.getDouble();
+ return new Twist2d(dx, dy, dtheta);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Twist2d value) {
+ bb.putDouble(value.dx);
+ bb.putDouble(value.dy);
+ bb.putDouble(value.dtheta);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Twist2d, ProtobufTwist2d> {
+ @Override
+ public Class<Twist2d> getTypeClass() {
+ return Twist2d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufTwist2d.getDescriptor();
+ }
+
+ @Override
+ public ProtobufTwist2d createMessage() {
+ return ProtobufTwist2d.newInstance();
+ }
+
+ @Override
+ public Twist2d unpack(ProtobufTwist2d msg) {
+ return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta());
+ }
+
+ @Override
+ public void pack(ProtobufTwist2d msg, Twist2d value) {
+ msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
index b78505e..d08d5cf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
@@ -4,11 +4,16 @@
package edu.wpi.first.math.geometry;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
/**
* A change in distance along a 3D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose3d objects from a Twist3d and vise versa.
+ * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@@ -82,4 +87,84 @@
public int hashCode() {
return Objects.hash(dx, dy, dz, rx, ry, rz);
}
+
+ public static final class AStruct implements Struct<Twist3d> {
+ @Override
+ public Class<Twist3d> getTypeClass() {
+ return Twist3d.class;
+ }
+
+ @Override
+ public String getTypeString() {
+ return "struct:Twist3d";
+ }
+
+ @Override
+ public int getSize() {
+ return kSizeDouble * 6;
+ }
+
+ @Override
+ public String getSchema() {
+ return "double dx;double dy;double dz;double rx;double ry;double rz";
+ }
+
+ @Override
+ public Twist3d unpack(ByteBuffer bb) {
+ double dx = bb.getDouble();
+ double dy = bb.getDouble();
+ double dz = bb.getDouble();
+ double rx = bb.getDouble();
+ double ry = bb.getDouble();
+ double rz = bb.getDouble();
+ return new Twist3d(dx, dy, dz, rx, ry, rz);
+ }
+
+ @Override
+ public void pack(ByteBuffer bb, Twist3d value) {
+ bb.putDouble(value.dx);
+ bb.putDouble(value.dy);
+ bb.putDouble(value.dz);
+ bb.putDouble(value.rx);
+ bb.putDouble(value.ry);
+ bb.putDouble(value.rz);
+ }
+ }
+
+ public static final AStruct struct = new AStruct();
+
+ public static final class AProto implements Protobuf<Twist3d, ProtobufTwist3d> {
+ @Override
+ public Class<Twist3d> getTypeClass() {
+ return Twist3d.class;
+ }
+
+ @Override
+ public Descriptor getDescriptor() {
+ return ProtobufTwist3d.getDescriptor();
+ }
+
+ @Override
+ public ProtobufTwist3d createMessage() {
+ return ProtobufTwist3d.newInstance();
+ }
+
+ @Override
+ public Twist3d unpack(ProtobufTwist3d msg) {
+ return new Twist3d(
+ msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz());
+ }
+
+ @Override
+ public void pack(ProtobufTwist3d msg, Twist3d value) {
+ msg.setDx(value.dx)
+ .setDy(value.dy)
+ .setDz(value.dz)
+ .setRx(value.rx)
+ .setRy(value.ry)
+ .setRz(value.rz);
+ }
+ }
+
+ public static final AProto proto = new AProto();
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
new file mode 100644
index 0000000..bcd57a3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
@@ -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.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+ public InterpolatingDoubleTreeMap() {
+ super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
new file mode 100644
index 0000000..22eb232
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
@@ -0,0 +1,104 @@
+// 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.
+
+package edu.wpi.first.math.interpolation;
+
+import java.util.Comparator;
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be
+ * provided.
+ *
+ * @param <K> The type of keys held in this map.
+ * @param <V> The type of values held in this map.
+ */
+public class InterpolatingTreeMap<K, V> {
+ private final TreeMap<K, V> m_map;
+
+ private final InverseInterpolator<K> m_inverseInterpolator;
+ private final Interpolator<V> m_interpolator;
+
+ /**
+ * Constructs an InterpolatingTreeMap.
+ *
+ * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+ * @param interpolator Function to use for interpolation of the values.
+ */
+ public InterpolatingTreeMap(
+ InverseInterpolator<K> inverseInterpolator, Interpolator<V> interpolator) {
+ m_map = new TreeMap<>();
+ m_inverseInterpolator = inverseInterpolator;
+ m_interpolator = interpolator;
+ }
+
+ /**
+ * Constructs an InterpolatingTreeMap using {@code comparator}.
+ *
+ * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+ * @param interpolator Function to use for interpolation of the values.
+ * @param comparator Comparator to use on keys.
+ */
+ public InterpolatingTreeMap(
+ InverseInterpolator<K> inverseInterpolator,
+ Interpolator<V> interpolator,
+ Comparator<K> comparator) {
+ m_inverseInterpolator = inverseInterpolator;
+ m_interpolator = interpolator;
+ m_map = new TreeMap<>(comparator);
+ }
+
+ /**
+ * Inserts a key-value pair.
+ *
+ * @param key The key.
+ * @param value The value.
+ */
+ public void put(K key, V value) {
+ m_map.put(key, value);
+ }
+
+ /**
+ * Returns the value associated with a given key.
+ *
+ * <p>If there's no matching key, the value returned will be an interpolation between the keys
+ * before and after the provided one, using the {@link Interpolator} and {@link
+ * InverseInterpolator} provided.
+ *
+ * @param key The key.
+ * @return The value associated with the given key.
+ */
+ public V get(K key) {
+ V val = m_map.get(key);
+ if (val == null) {
+ K ceilingKey = m_map.ceilingKey(key);
+ K floorKey = m_map.floorKey(key);
+
+ if (ceilingKey == null && floorKey == null) {
+ return null;
+ }
+ if (ceilingKey == null) {
+ return m_map.get(floorKey);
+ }
+ if (floorKey == null) {
+ return m_map.get(ceilingKey);
+ }
+ V floor = m_map.get(floorKey);
+ V ceiling = m_map.get(ceilingKey);
+
+ return m_interpolator.interpolate(
+ floor, ceiling, m_inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key));
+ } else {
+ return val;
+ }
+ }
+
+ /** Clears the contents. */
+ public void clear() {
+ m_map.clear();
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
new file mode 100644
index 0000000..be6d8a2
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
@@ -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.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An interpolation function that returns a value interpolated between an upper and lower bound.
+ * This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link Interpolator} will operate on.
+ */
+@FunctionalInterface
+public interface Interpolator<T> {
+ /**
+ * Perform interpolation between two values.
+ *
+ * @param startValue The value to start at.
+ * @param endValue The value to end at.
+ * @param t How far between the two values to interpolate. This should be bounded to [0, 1].
+ * @return The interpolated value.
+ */
+ T interpolate(T startValue, T endValue, double t);
+
+ static Interpolator<Double> forDouble() {
+ return MathUtil::interpolate;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
new file mode 100644
index 0000000..8278af3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
@@ -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.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An inverse interpolation function which determines where within an interpolation range an object
+ * lies. This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link InverseInterpolator} will operate on.
+ */
+@FunctionalInterface
+public interface InverseInterpolator<T> {
+ /**
+ * Return where within interpolation range [0, 1] q is between startValue and endValue.
+ *
+ * @param startValue Lower part of interpolation range.
+ * @param endValue Upper part of interpolation range.
+ * @param q Query.
+ * @return Interpolant in range [0, 1].
+ */
+ double inverseInterpolate(T startValue, T endValue, T q);
+
+ static InverseInterpolator<Double> forDouble() {
+ return MathUtil::inverseInterpolate;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
index 7e0712d..f9f20c3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
@@ -19,11 +19,10 @@
*/
public final class TimeInterpolatableBuffer<T> {
private final double m_historySize;
- private final InterpolateFunction<T> m_interpolatingFunc;
+ private final Interpolator<T> m_interpolatingFunc;
private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
- private TimeInterpolatableBuffer(
- InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+ private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySizeSeconds) {
this.m_historySize = historySizeSeconds;
this.m_interpolatingFunc = interpolateFunction;
}
@@ -37,7 +36,7 @@
* @return The new TimeInterpolatableBuffer.
*/
public static <T> TimeInterpolatableBuffer<T> createBuffer(
- InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+ Interpolator<T> interpolateFunction, double historySizeSeconds) {
return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
}
@@ -143,17 +142,4 @@
public NavigableMap<Double, T> getInternalBuffer() {
return m_pastSnapshots;
}
-
- public interface InterpolateFunction<T> {
- /**
- * Return the interpolated value. This object is assumed to be the starting position, or lower
- * bound.
- *
- * @param start The lower bound, or start.
- * @param end The upper bound, or end.
- * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
- * @return The interpolated value.
- */
- T interpolate(T start, T end, double t);
- }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index 6c98337..ffbce71 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -4,23 +4,24 @@
package edu.wpi.first.math.kinematics;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
/**
- * 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.
+ * Represents the speed of a robot chassis. Although this class 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, a ChassisSpeeds object represents a robot's velocity.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
* will often have all three components.
*/
public class ChassisSpeeds {
- /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
+ /** Velocity along the x-axis. (Fwd is +) */
public double vxMetersPerSecond;
- /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
+ /** Velocity along the y-axis. (Left is +) */
public double vyMetersPerSecond;
/** Represents the angular velocity of the robot frame. (CCW is +) */
@@ -44,6 +45,60 @@
}
/**
+ * Discretizes a continuous-time chassis speed.
+ *
+ * <p>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).
+ *
+ * <p>This is useful for compensating for translational skew when translating and rotating a
+ * swerve drivetrain.
+ *
+ * @param vxMetersPerSecond Forward velocity.
+ * @param vyMetersPerSecond Sideways velocity.
+ * @param omegaRadiansPerSecond Angular velocity.
+ * @param dtSeconds The duration of the timestep the speeds should be applied for.
+ * @return Discretized ChassisSpeeds.
+ */
+ public static ChassisSpeeds discretize(
+ double vxMetersPerSecond,
+ double vyMetersPerSecond,
+ double omegaRadiansPerSecond,
+ double dtSeconds) {
+ var desiredDeltaPose =
+ new Pose2d(
+ vxMetersPerSecond * dtSeconds,
+ vyMetersPerSecond * dtSeconds,
+ new Rotation2d(omegaRadiansPerSecond * dtSeconds));
+ var twist = new Pose2d().log(desiredDeltaPose);
+ return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
+ }
+
+ /**
+ * Discretizes a continuous-time chassis speed.
+ *
+ * <p>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).
+ *
+ * <p>This is useful for compensating for translational skew when translating and rotating a
+ * swerve drivetrain.
+ *
+ * @param continuousSpeeds The continuous speeds.
+ * @param dtSeconds The duration of the timestep the speeds should be applied for.
+ * @return Discretized ChassisSpeeds.
+ */
+ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
+ return discretize(
+ continuousSpeeds.vxMetersPerSecond,
+ continuousSpeeds.vyMetersPerSecond,
+ continuousSpeeds.omegaRadiansPerSecond,
+ dtSeconds);
+ }
+
+ /**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
@@ -62,10 +117,10 @@
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
- return new ChassisSpeeds(
- vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
- -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
- omegaRadiansPerSecond);
+ // CW rotation into chassis frame
+ var rotated =
+ new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
+ return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
}
/**
@@ -89,6 +144,119 @@
robotAngle);
}
+ /**
+ * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
+ * object.
+ *
+ * @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
+ * Positive x is towards the robot's front.
+ * @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
+ * Positive y is towards the robot's left.
+ * @param omegaRadiansPerSecond 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.
+ */
+ public static ChassisSpeeds fromRobotRelativeSpeeds(
+ double vxMetersPerSecond,
+ double vyMetersPerSecond,
+ double omegaRadiansPerSecond,
+ Rotation2d robotAngle) {
+ // CCW rotation out of chassis frame
+ var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
+ return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
+ }
+
+ /**
+ * 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 towards the 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.
+ */
+ public static ChassisSpeeds fromRobotRelativeSpeeds(
+ ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
+ return fromRobotRelativeSpeeds(
+ robotRelativeSpeeds.vxMetersPerSecond,
+ robotRelativeSpeeds.vyMetersPerSecond,
+ robotRelativeSpeeds.omegaRadiansPerSecond,
+ robotAngle);
+ }
+
+ /**
+ * Adds two ChassisSpeeds and returns the sum.
+ *
+ * <p>For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} =
+ * ChassisSpeeds{3.0, 2.0, 1.0}
+ *
+ * @param other The ChassisSpeeds to add.
+ * @return The sum of the ChassisSpeeds.
+ */
+ public ChassisSpeeds plus(ChassisSpeeds other) {
+ return new ChassisSpeeds(
+ vxMetersPerSecond + other.vxMetersPerSecond,
+ vyMetersPerSecond + other.vyMetersPerSecond,
+ omegaRadiansPerSecond + other.omegaRadiansPerSecond);
+ }
+
+ /**
+ * 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.
+ */
+ public ChassisSpeeds minus(ChassisSpeeds other) {
+ return new ChassisSpeeds(
+ vxMetersPerSecond - other.vxMetersPerSecond,
+ vyMetersPerSecond - other.vyMetersPerSecond,
+ omegaRadiansPerSecond - other.omegaRadiansPerSecond);
+ }
+
+ /**
+ * Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components
+ * of the ChassisSpeeds.
+ *
+ * @return The inverse of the current ChassisSpeeds.
+ */
+ public ChassisSpeeds unaryMinus() {
+ return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond);
+ }
+
+ /**
+ * 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.
+ */
+ public ChassisSpeeds times(double scalar) {
+ return new ChassisSpeeds(
+ vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar);
+ }
+
+ /**
+ * 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.
+ */
+ public ChassisSpeeds div(double scalar) {
+ return new ChassisSpeeds(
+ vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar);
+ }
+
@Override
public String toString() {
return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
index 0dfb016..a1fb2db 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -16,7 +16,8 @@
* whereas forward kinematics converts left and right component velocities into a linear and angular
* chassis speed.
*/
-public class DifferentialDriveKinematics {
+public class DifferentialDriveKinematics
+ implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
public final double trackWidthMeters;
/**
@@ -37,6 +38,7 @@
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
+ @Override
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
@@ -51,6 +53,7 @@
* chassis' speed.
* @return The left and right velocities.
*/
+ @Override
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vxMetersPerSecond
@@ -59,6 +62,12 @@
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
}
+ @Override
+ public Twist2d toTwist2d(
+ DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
+ return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
+ }
+
/**
* Performs forward kinematics to return the resulting Twist2d from the given left and right side
* distance deltas. This method is often used for odometry -- determining the robot's position on
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
index e8f97f5..db5e8a3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
@@ -8,7 +8,6 @@
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
/**
* Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -20,15 +19,7 @@
* <p>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.
*/
-public class DifferentialDriveOdometry {
- private Pose2d m_poseMeters;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private double m_prevLeftDistance;
- private double m_prevRightDistance;
-
+public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
/**
* Constructs a DifferentialDriveOdometry object.
*
@@ -42,13 +33,11 @@
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
- m_poseMeters = initialPoseMeters;
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
-
- m_prevLeftDistance = leftDistanceMeters;
- m_prevRightDistance = rightDistanceMeters;
-
+ super(
+ new DifferentialDriveKinematics(1),
+ gyroAngle,
+ new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+ initialPoseMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
@@ -80,21 +69,10 @@
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d poseMeters) {
- m_poseMeters = poseMeters;
- m_previousAngle = poseMeters.getRotation();
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-
- m_prevLeftDistance = leftDistanceMeters;
- m_prevRightDistance = rightDistanceMeters;
- }
-
- /**
- * Returns the position of the robot on the field.
- *
- * @return The pose of the robot (x and y are in meters).
- */
- public Pose2d getPoseMeters() {
- return m_poseMeters;
+ super.resetPosition(
+ gyroAngle,
+ new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+ poseMeters);
}
/**
@@ -109,22 +87,7 @@
*/
public Pose2d update(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
- double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
- double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
-
- m_prevLeftDistance = leftDistanceMeters;
- m_prevRightDistance = rightDistanceMeters;
-
- double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
- var angle = gyroAngle.plus(m_gyroOffset);
-
- var newPose =
- m_poseMeters.exp(
- new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
-
- m_previousAngle = angle;
-
- m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
- return m_poseMeters;
+ return super.update(
+ gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
new file mode 100644
index 0000000..18866af
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
@@ -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.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class DifferentialDriveWheelPositions
+ implements WheelPositions<DifferentialDriveWheelPositions> {
+ /** Distance measured by the left side. */
+ public double leftMeters;
+
+ /** Distance measured by the right side. */
+ public double rightMeters;
+
+ /**
+ * Constructs a DifferentialDriveWheelPositions.
+ *
+ * @param leftMeters Distance measured by the left side.
+ * @param rightMeters Distance measured by the right side.
+ */
+ public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
+ this.leftMeters = leftMeters;
+ this.rightMeters = rightMeters;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof DifferentialDriveWheelPositions) {
+ DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj;
+ return Math.abs(other.leftMeters - leftMeters) < 1E-9
+ && Math.abs(other.rightMeters - rightMeters) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(leftMeters, rightMeters);
+ }
+
+ @Override
+ public String toString() {
+ return String.format(
+ "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
+ }
+
+ @Override
+ public DifferentialDriveWheelPositions copy() {
+ return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
+ }
+
+ @Override
+ public DifferentialDriveWheelPositions interpolate(
+ DifferentialDriveWheelPositions endValue, double t) {
+ return new DifferentialDriveWheelPositions(
+ MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
+ MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
index d4b235e..ec874b6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
@@ -46,6 +46,77 @@
}
}
+ /**
+ * 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.
+ */
+ public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
+ return new DifferentialDriveWheelSpeeds(
+ leftMetersPerSecond + other.leftMetersPerSecond,
+ rightMetersPerSecond + other.rightMetersPerSecond);
+ }
+
+ /**
+ * 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.
+ */
+ public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
+ return new DifferentialDriveWheelSpeeds(
+ leftMetersPerSecond - other.leftMetersPerSecond,
+ rightMetersPerSecond - other.rightMetersPerSecond);
+ }
+
+ /**
+ * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
+ * all components of the DifferentialDriveWheelSpeeds.
+ *
+ * @return The inverse of the current DifferentialDriveWheelSpeeds.
+ */
+ public DifferentialDriveWheelSpeeds unaryMinus() {
+ return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond);
+ }
+
+ /**
+ * 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.
+ */
+ public DifferentialDriveWheelSpeeds times(double scalar) {
+ return new DifferentialDriveWheelSpeeds(
+ leftMetersPerSecond * scalar, rightMetersPerSecond * scalar);
+ }
+
+ /**
+ * 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.
+ */
+ public DifferentialDriveWheelSpeeds div(double scalar) {
+ return new DifferentialDriveWheelSpeeds(
+ leftMetersPerSecond / scalar, rightMetersPerSecond / scalar);
+ }
+
@Override
public String toString() {
return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java
new file mode 100644
index 0000000..35e2642
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java
@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
+ * code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
+ * {@link DifferentialDriveKinematics}).
+ *
+ * @param <S> The type of the wheel speeds.
+ * @param <P> The type of the wheel positions.
+ */
+public interface Kinematics<S, P> {
+ /**
+ * 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.
+ */
+ ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
+
+ /**
+ * 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.
+ */
+ S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
+
+ /**
+ * 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.
+ */
+ Twist2d toTwist2d(P start, P end);
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index 23204d4..76c857a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -30,7 +30,8 @@
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
-public class MecanumDriveKinematics {
+public class MecanumDriveKinematics
+ implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
@@ -125,6 +126,7 @@
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
*/
+ @Override
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toWheelSpeeds(chassisSpeeds, new Translation2d());
}
@@ -137,6 +139,7 @@
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @return The resulting chassis speed.
*/
+ @Override
public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
var wheelSpeedsVector = new SimpleMatrix(4, 1);
wheelSpeedsVector.setColumn(
@@ -154,6 +157,20 @@
chassisSpeedsVector.get(2, 0));
}
+ @Override
+ public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) {
+ var wheelDeltasVector = new SimpleMatrix(4, 1);
+ wheelDeltasVector.setColumn(
+ 0,
+ 0,
+ end.frontLeftMeters - start.frontLeftMeters,
+ end.frontRightMeters - start.frontRightMeters,
+ end.rearLeftMeters - start.rearLeftMeters,
+ end.rearRightMeters - start.rearRightMeters);
+ var twist = m_forwardKinematics.mult(wheelDeltasVector);
+ return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
+ }
+
/**
* Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
* method is often used for odometry -- determining the robot's position on the field using
@@ -172,7 +189,6 @@
wheelDeltas.rearLeftMeters,
wheelDeltas.rearRightMeters);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
-
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
index 40a77e2..32bc9bf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
@@ -16,14 +16,7 @@
* <p>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.
*/
-public class MecanumDriveOdometry {
- private final MecanumDriveKinematics m_kinematics;
- private Pose2d m_poseMeters;
- private MecanumDriveWheelPositions m_previousWheelPositions;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
+public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
/**
* Constructs a MecanumDriveOdometry object.
*
@@ -37,16 +30,7 @@
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters) {
- m_kinematics = kinematics;
- m_poseMeters = initialPoseMeters;
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
- m_previousWheelPositions =
- new MecanumDriveWheelPositions(
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters);
+ super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
@@ -63,72 +47,4 @@
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, new Pose2d());
}
-
- /**
- * Resets the robot's position on the field.
- *
- * <p>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 distances driven by each wheel.
- * @param poseMeters The position on the field that your robot is at.
- */
- public void resetPosition(
- Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
- m_poseMeters = poseMeters;
- m_previousAngle = poseMeters.getRotation();
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- m_previousWheelPositions =
- new MecanumDriveWheelPositions(
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters);
- }
-
- /**
- * Returns the position of the robot on the field.
- *
- * @return The pose of the robot (x and y are in meters).
- */
- public Pose2d getPoseMeters() {
- return m_poseMeters;
- }
-
- /**
- * 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 distances driven by each wheel.
- * @return The new pose of the robot.
- */
- public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
- var angle = gyroAngle.plus(m_gyroOffset);
-
- var wheelDeltas =
- new MecanumDriveWheelPositions(
- wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
-
- var twist = m_kinematics.toTwist2d(wheelDeltas);
- twist.dtheta = angle.minus(m_previousAngle).getRadians();
- var newPose = m_poseMeters.exp(twist);
-
- m_previousAngle = angle;
- m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
- m_previousWheelPositions =
- new MecanumDriveWheelPositions(
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters);
-
- return m_poseMeters;
- }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
index 9ff3341..2b284cb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
@@ -4,7 +4,10 @@
package edu.wpi.first.math.kinematics;
-public class MecanumDriveWheelPositions {
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
/** Distance measured by the front left wheel. */
public double frontLeftMeters;
@@ -40,10 +43,42 @@
}
@Override
+ public boolean equals(Object obj) {
+ if (obj instanceof MecanumDriveWheelPositions) {
+ MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
+ return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
+ && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
+ && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
+ && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+ }
+
+ @Override
public String toString() {
return String.format(
"MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
+ "Rear Left: %.2f m, Rear Right: %.2f m)",
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
+
+ @Override
+ public MecanumDriveWheelPositions copy() {
+ return new MecanumDriveWheelPositions(
+ frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+ }
+
+ @Override
+ public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
+ return new MecanumDriveWheelPositions(
+ MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
+ MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
+ MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
+ MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
index 1dcfc85..63cef18 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -73,6 +73,89 @@
}
}
+ /**
+ * 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.
+ */
+ public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) {
+ return new MecanumDriveWheelSpeeds(
+ frontLeftMetersPerSecond + other.frontLeftMetersPerSecond,
+ frontRightMetersPerSecond + other.frontRightMetersPerSecond,
+ rearLeftMetersPerSecond + other.rearLeftMetersPerSecond,
+ rearRightMetersPerSecond + other.rearRightMetersPerSecond);
+ }
+
+ /**
+ * 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.
+ */
+ public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) {
+ return new MecanumDriveWheelSpeeds(
+ frontLeftMetersPerSecond - other.frontLeftMetersPerSecond,
+ frontRightMetersPerSecond - other.frontRightMetersPerSecond,
+ rearLeftMetersPerSecond - other.rearLeftMetersPerSecond,
+ rearRightMetersPerSecond - other.rearRightMetersPerSecond);
+ }
+
+ /**
+ * Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all
+ * components of the MecanumDriveWheelSpeeds.
+ *
+ * @return The inverse of the current MecanumDriveWheelSpeeds.
+ */
+ public MecanumDriveWheelSpeeds unaryMinus() {
+ return new MecanumDriveWheelSpeeds(
+ -frontLeftMetersPerSecond,
+ -frontRightMetersPerSecond,
+ -rearLeftMetersPerSecond,
+ -rearRightMetersPerSecond);
+ }
+
+ /**
+ * 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.
+ */
+ public MecanumDriveWheelSpeeds times(double scalar) {
+ return new MecanumDriveWheelSpeeds(
+ frontLeftMetersPerSecond * scalar,
+ frontRightMetersPerSecond * scalar,
+ rearLeftMetersPerSecond * scalar,
+ rearRightMetersPerSecond * scalar);
+ }
+
+ /**
+ * 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.
+ */
+ public MecanumDriveWheelSpeeds div(double scalar) {
+ return new MecanumDriveWheelSpeeds(
+ frontLeftMetersPerSecond / scalar,
+ frontRightMetersPerSecond / scalar,
+ rearLeftMetersPerSecond / scalar,
+ rearRightMetersPerSecond / scalar);
+ }
+
@Override
public String toString() {
return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
new file mode 100644
index 0000000..b2e5054
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
@@ -0,0 +1,97 @@
+// 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.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the particular type for
+ * your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from encoders and a
+ * gyroscope.
+ *
+ * <p>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.
+ */
+public class Odometry<T extends WheelPositions<T>> {
+ private final Kinematics<?, T> m_kinematics;
+ private Pose2d m_poseMeters;
+
+ private Rotation2d m_gyroOffset;
+ private Rotation2d m_previousAngle;
+ private T m_previousWheelPositions;
+
+ /**
+ * Constructs an Odometry object.
+ *
+ * @param kinematics The kinematics of the drivebase.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The current encoder readings.
+ * @param initialPoseMeters The starting position of the robot on the field.
+ */
+ public Odometry(
+ Kinematics<?, T> kinematics,
+ Rotation2d gyroAngle,
+ T wheelPositions,
+ Pose2d initialPoseMeters) {
+ m_kinematics = kinematics;
+ m_poseMeters = initialPoseMeters;
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousAngle = m_poseMeters.getRotation();
+ m_previousWheelPositions = wheelPositions.copy();
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * <p>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 encoder readings.
+ * @param poseMeters The position on the field that your robot is at.
+ */
+ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+ m_poseMeters = poseMeters;
+ m_previousAngle = m_poseMeters.getRotation();
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousWheelPositions = wheelPositions.copy();
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ *
+ * @return The pose of the robot (x and y are in meters).
+ */
+ public Pose2d getPoseMeters() {
+ return m_poseMeters;
+ }
+
+ /**
+ * 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 encoder readings.
+ * @return The new pose of the robot.
+ */
+ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+ var angle = gyroAngle.plus(m_gyroOffset);
+
+ var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
+ twist.dtheta = angle.minus(m_previousAngle).getRadians();
+
+ var newPose = m_poseMeters.exp(twist);
+
+ m_previousWheelPositions = wheelPositions.copy();
+ m_previousAngle = angle;
+ m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+ return m_poseMeters;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index 98df547..aa5338e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -10,7 +10,6 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import java.util.Arrays;
-import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
/**
@@ -33,32 +32,50 @@
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
-public class SwerveDriveKinematics {
+public class SwerveDriveKinematics
+ implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
+ public static class SwerveDriveWheelStates {
+ public SwerveModuleState[] states;
+
+ /**
+ * Creates a new SwerveDriveWheelStates instance.
+ *
+ * @param states The swerve module states. This will be deeply copied.
+ */
+ public SwerveDriveWheelStates(SwerveModuleState[] states) {
+ this.states = new SwerveModuleState[states.length];
+ for (int i = 0; i < states.length; i++) {
+ this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle);
+ }
+ }
+ }
+
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
private final int m_numModules;
private final Translation2d[] m_modules;
- private final SwerveModuleState[] m_moduleStates;
+ private Rotation2d[] m_moduleHeadings;
private Translation2d m_prevCoR = new Translation2d();
/**
- * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations
- * as Translation2d objects. The order in which you pass in the wheel 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.
+ * Constructs a swerve drive kinematics object. This takes in a variable number of module
+ * locations as Translation2d objects. 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 wheelsMeters The locations of the wheels relative to the physical center of the robot.
+ * @param moduleTranslationsMeters The locations of the modules relative to the physical center of
+ * the robot.
*/
- public SwerveDriveKinematics(Translation2d... wheelsMeters) {
- if (wheelsMeters.length < 2) {
+ public SwerveDriveKinematics(Translation2d... moduleTranslationsMeters) {
+ if (moduleTranslationsMeters.length < 2) {
throw new IllegalArgumentException("A swerve drive requires at least two modules");
}
- m_numModules = wheelsMeters.length;
- m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
- m_moduleStates = new SwerveModuleState[m_numModules];
- Arrays.fill(m_moduleStates, new SwerveModuleState());
+ m_numModules = moduleTranslationsMeters.length;
+ m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules);
+ m_moduleHeadings = new Rotation2d[m_numModules];
+ Arrays.fill(m_moduleHeadings, new Rotation2d());
m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
for (int i = 0; i < m_numModules; i++) {
@@ -71,6 +88,21 @@
}
/**
+ * 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.
+ */
+ public void resetHeadings(Rotation2d... moduleHeadings) {
+ if (moduleHeadings.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of headings is not consistent with number of module locations provided in "
+ + "constructor");
+ }
+ m_moduleHeadings = Arrays.copyOf(moduleHeadings, m_numModules);
+ }
+
+ /**
* 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.
*
@@ -91,17 +123,18 @@
* attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
* DesaturateWheelSpeeds} function to rectify this issue.
*/
- @SuppressWarnings("PMD.MethodReturnsInternalArray")
public SwerveModuleState[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+ var moduleStates = new SwerveModuleState[m_numModules];
+
if (chassisSpeeds.vxMetersPerSecond == 0.0
&& chassisSpeeds.vyMetersPerSecond == 0.0
&& chassisSpeeds.omegaRadiansPerSecond == 0.0) {
for (int i = 0; i < m_numModules; i++) {
- m_moduleStates[i].speedMetersPerSecond = 0.0;
+ moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]);
}
- return m_moduleStates;
+ return moduleStates;
}
if (!centerOfRotationMeters.equals(m_prevCoR)) {
@@ -139,10 +172,11 @@
double speed = Math.hypot(x, y);
Rotation2d angle = new Rotation2d(x, y);
- m_moduleStates[i] = new SwerveModuleState(speed, angle);
+ moduleStates[i] = new SwerveModuleState(speed, angle);
+ m_moduleHeadings[i] = angle;
}
- return m_moduleStates;
+ return moduleStates;
}
/**
@@ -156,26 +190,31 @@
return toSwerveModuleStates(chassisSpeeds, new Translation2d());
}
+ @Override
+ public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+ return new SwerveDriveWheelStates(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) as measured from
+ * @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.
*/
- public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
- if (wheelStates.length != m_numModules) {
+ public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) {
+ if (moduleStates.length != m_numModules) {
throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
+ "Number of modules is not consistent with number of module locations provided in "
+ "constructor");
}
var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
for (int i = 0; i < m_numModules; i++) {
- var module = wheelStates[i];
+ var module = moduleStates[i];
moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
}
@@ -187,26 +226,31 @@
chassisSpeedsVector.get(2, 0));
}
+ @Override
+ public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) {
+ return toChassisSpeeds(wheelStates.states);
+ }
+
/**
* 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 wheelDeltas The latest change in position of the modules (as a SwerveModulePosition
+ * @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.
*/
- public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) {
- if (wheelDeltas.length != m_numModules) {
+ public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) {
+ if (moduleDeltas.length != m_numModules) {
throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
+ "Number of modules is not consistent with number of module locations provided in "
+ "constructor");
}
var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
for (int i = 0; i < m_numModules; i++) {
- var module = wheelDeltas[i];
+ var module = moduleDeltas[i];
moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
}
@@ -216,6 +260,22 @@
chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
}
+ @Override
+ public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) {
+ if (start.positions.length != end.positions.length) {
+ throw new IllegalArgumentException("Inconsistent number of modules!");
+ }
+ var newPositions = new SwerveModulePosition[start.positions.length];
+ for (int i = 0; i < start.positions.length; i++) {
+ var startModule = start.positions[i];
+ var endModule = end.positions[i];
+ newPositions[i] =
+ new SwerveModulePosition(
+ endModule.distanceMeters - startModule.distanceMeters, endModule.angle);
+ }
+ return toTwist2d(newPositions);
+ }
+
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*
@@ -230,7 +290,10 @@
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
- double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+ double realMaxSpeed = 0;
+ for (SwerveModuleState moduleState : moduleStates) {
+ realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
+ }
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond =
@@ -250,7 +313,7 @@
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
- * @param currentChassisSpeed The current speed of the robot
+ * @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
* @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
* can reach while translating
@@ -259,11 +322,14 @@
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates,
- ChassisSpeeds currentChassisSpeed,
+ ChassisSpeeds desiredChassisSpeed,
double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond) {
- double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+ double realMaxSpeed = 0;
+ for (SwerveModuleState moduleState : moduleStates) {
+ realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
+ }
if (attainableMaxTranslationalSpeedMetersPerSecond == 0
|| attainableMaxRotationalVelocityRadiansPerSecond == 0
@@ -271,10 +337,10 @@
return;
}
double translationalK =
- Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond)
+ Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond)
/ attainableMaxTranslationalSpeedMetersPerSecond;
double rotationalK =
- Math.abs(currentChassisSpeed.omegaRadiansPerSecond)
+ Math.abs(desiredChassisSpeed.omegaRadiansPerSecond)
/ attainableMaxRotationalVelocityRadiansPerSecond;
double k = Math.max(translationalK, rotationalK);
double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
index c2e188f..4f954e9 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
@@ -17,14 +17,8 @@
* <p>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.
*/
-public class SwerveDriveOdometry {
- private final SwerveDriveKinematics m_kinematics;
- private Pose2d m_poseMeters;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
+public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
private final int m_numModules;
- private SwerveModulePosition[] m_previousModulePositions;
/**
* Constructs a SwerveDriveOdometry object.
@@ -39,18 +33,9 @@
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPose) {
- m_kinematics = kinematics;
- m_poseMeters = initialPose;
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPose.getRotation();
- m_numModules = modulePositions.length;
+ super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose);
- m_previousModulePositions = new SwerveModulePosition[m_numModules];
- for (int index = 0; index < m_numModules; index++) {
- m_previousModulePositions[index] =
- new SwerveModulePosition(
- modulePositions[index].distanceMeters, modulePositions[index].angle);
- }
+ m_numModules = modulePositions.length;
MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
}
@@ -83,29 +68,18 @@
*/
public void resetPosition(
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
- if (modulePositions.length != m_numModules) {
+ resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
+ }
+
+ @Override
+ public void resetPosition(
+ Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
+ if (modulePositions.positions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
-
- m_poseMeters = pose;
- m_previousAngle = pose.getRotation();
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- for (int index = 0; index < m_numModules; index++) {
- m_previousModulePositions[index] =
- new SwerveModulePosition(
- modulePositions[index].distanceMeters, modulePositions[index].angle);
- }
- }
-
- /**
- * Returns the position of the robot on the field.
- *
- * @return The pose of the robot (x and y are in meters).
- */
- public Pose2d getPoseMeters() {
- return m_poseMeters;
+ super.resetPosition(gyroAngle, modulePositions, pose);
}
/**
@@ -121,32 +95,16 @@
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
- if (modulePositions.length != m_numModules) {
+ return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+ }
+
+ @Override
+ public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
+ if (modulePositions.positions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
-
- var moduleDeltas = new SwerveModulePosition[m_numModules];
- for (int index = 0; index < m_numModules; index++) {
- var current = modulePositions[index];
- var previous = m_previousModulePositions[index];
-
- moduleDeltas[index] =
- new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
- previous.distanceMeters = current.distanceMeters;
- }
-
- var angle = gyroAngle.plus(m_gyroOffset);
-
- var twist = m_kinematics.toTwist2d(moduleDeltas);
- twist.dtheta = angle.minus(m_previousAngle).getRadians();
-
- var newPose = m_poseMeters.exp(twist);
-
- m_previousAngle = angle;
- m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
- return m_poseMeters;
+ return super.update(gyroAngle, modulePositions);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
new file mode 100644
index 0000000..e88f044
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
@@ -0,0 +1,61 @@
+// 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.
+
+package edu.wpi.first.math.kinematics;
+
+import java.util.Arrays;
+import java.util.Objects;
+
+public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
+ public SwerveModulePosition[] positions;
+
+ /**
+ * Creates a new SwerveDriveWheelPositions instance.
+ *
+ * @param positions The swerve module positions. This will be deeply copied.
+ */
+ public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
+ this.positions = new SwerveModulePosition[positions.length];
+ for (int i = 0; i < positions.length; i++) {
+ this.positions[i] = positions[i].copy();
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof SwerveDriveWheelPositions) {
+ SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
+ return Arrays.equals(this.positions, other.positions);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ // Cast to interpret positions as single argument, not array of the arguments
+ return Objects.hash((Object) positions);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
+ }
+
+ @Override
+ public SwerveDriveWheelPositions copy() {
+ return new SwerveDriveWheelPositions(positions);
+ }
+
+ @Override
+ public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
+ if (endValue.positions.length != positions.length) {
+ throw new IllegalArgumentException("Inconsistent number of modules!");
+ }
+ var newPositions = new SwerveModulePosition[positions.length];
+ for (int i = 0; i < positions.length; i++) {
+ newPositions[i] = positions[i].interpolate(endValue.positions[i], t);
+ }
+ return new SwerveDriveWheelPositions(newPositions);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
index cdd7834..d058764 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
@@ -4,11 +4,14 @@
package edu.wpi.first.math.kinematics;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/** Represents the state of one swerve module. */
-public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+public class SwerveModulePosition
+ implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
/** Distance measured by the wheel of the module. */
public double distanceMeters;
@@ -32,14 +35,15 @@
@Override
public boolean equals(Object obj) {
if (obj instanceof SwerveModulePosition) {
- return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+ SwerveModulePosition other = (SwerveModulePosition) obj;
+ return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
}
return false;
}
@Override
public int hashCode() {
- return Objects.hash(distanceMeters);
+ return Objects.hash(distanceMeters, angle);
}
/**
@@ -59,4 +63,20 @@
return String.format(
"SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
}
+
+ /**
+ * Returns a copy of this swerve module position.
+ *
+ * @return A copy.
+ */
+ public SwerveModulePosition copy() {
+ return new SwerveModulePosition(distanceMeters, angle);
+ }
+
+ @Override
+ public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
+ return new SwerveModulePosition(
+ MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
+ this.angle.interpolate(endValue.angle, t));
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
index ec7fd9f..10dee4f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
@@ -32,15 +32,16 @@
@Override
public boolean equals(Object obj) {
if (obj instanceof SwerveModuleState) {
- return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
- == 0;
+ SwerveModuleState other = (SwerveModuleState) obj;
+ return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
+ && angle.equals(other.angle);
}
return false;
}
@Override
public int hashCode() {
- return Objects.hash(speedMetersPerSecond);
+ return Objects.hash(speedMetersPerSecond, angle);
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
new file mode 100644
index 0000000..029a0ac
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
@@ -0,0 +1,16 @@
+// 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.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.interpolation.Interpolatable;
+
+public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
+ /**
+ * Returns a copy of this instance.
+ *
+ * @return A copy.
+ */
+ T copy();
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
index c1a87f2..b2a34a5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
@@ -71,8 +71,8 @@
private SplineParameterizer() {}
/**
- * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
- * dy, and dtheta are within specific tolerances.
+ * 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.
* @return A list of poses and curvatures that represents various points on the spline.
@@ -84,8 +84,8 @@
}
/**
- * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
- * dy, and dtheta are within specific tolerances.
+ * 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.
* @param t0 Starting internal spline parameter. It is recommended to use 0.0.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
index 1871803..b2fe9c8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
@@ -109,93 +109,6 @@
}
/**
- * Discretizes the given continuous A and Q matrices.
- *
- * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (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>
- *
- * @param <States> Nat representing the number of states.
- * @param contA Continuous system matrix.
- * @param contQ Continuous process noise covariance matrix.
- * @param dtSeconds Discretization timestep.
- * @return a pair representing the discrete system matrix and process noise covariance matrix.
- */
- public static <States extends Num>
- Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
- Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
- // 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
- Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
-
- Matrix<States, States> lastTerm = Q.copy();
- double lastCoeff = dtSeconds;
-
- // Aᵀⁿ
- Matrix<States, States> ATn = contA.transpose();
-
- Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
-
- // i = 6 i.e. 5th order should be enough precision
- for (int i = 2; i < 6; ++i) {
- lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(ATn));
- lastCoeff *= dtSeconds / ((double) i);
-
- phi12 = phi12.plus(lastTerm.times(lastCoeff));
-
- ATn = ATn.times(contA.transpose());
- }
-
- var discA = discretizeA(contA, dtSeconds);
- Q = discA.times(phi12);
-
- // Make Q symmetric if it isn't already
- var discQ = Q.plus(Q.transpose()).div(2.0);
-
- return new Pair<>(discA, discQ);
- }
-
- /**
* Returns a discretized version of the provided continuous measurement noise covariance matrix.
* Note that dt=0.0 divides R by zero.
*
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
index 94be369..e9b46fc 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
@@ -238,7 +238,11 @@
.times(h))
.normF();
- h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+ if (truncationError == 0.0) {
+ h = dtSeconds - dtElapsed;
+ } else {
+ h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+ }
} while (truncationError > maxError);
dtElapsed += h;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
index cd431f7..0c6e334 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
@@ -21,7 +21,7 @@
* Constructs a DC motor.
*
* @param nominalVoltageVolts Voltage at which the motor constants were measured.
- * @param stallTorqueNewtonMeters Current draw when stalled.
+ * @param stallTorqueNewtonMeters Torque when stalled.
* @param stallCurrentAmps Current draw when stalled.
* @param freeCurrentAmps Current draw under no load.
* @param freeSpeedRadPerSec Angular velocity under no load.
@@ -47,10 +47,10 @@
}
/**
- * Estimate the current being drawn by this motor.
+ * Calculate current drawn by motor with given speed and input voltage.
*
- * @param speedRadiansPerSec The speed of the motor.
- * @param voltageInputVolts The input voltage.
+ * @param speedRadiansPerSec The current angular velocity of the motor.
+ * @param voltageInputVolts The voltage being applied to the motor.
* @return The estimated current.
*/
public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
@@ -58,20 +58,20 @@
}
/**
- * Calculate the torque produced by the motor for a given current.
+ * Calculate torque produced by the motor with a given current.
*
* @param currentAmpere The current drawn by the motor.
- * @return The torque produced.
+ * @return The torque output.
*/
public double getTorque(double currentAmpere) {
return currentAmpere * KtNMPerAmp;
}
/**
- * Calculate the voltage provided to the motor at a given torque and angular velocity.
+ * Calculate the voltage provided to the motor for a given torque and angular velocity.
*
* @param torqueNm The torque produced by the motor.
- * @param speedRadiansPerSec The speed of the motor.
+ * @param speedRadiansPerSec The current angular velocity of the motor.
* @return The voltage of the motor.
*/
public double getVoltage(double torqueNm, double speedRadiansPerSec) {
@@ -79,14 +79,15 @@
}
/**
- * Calculate the speed of the motor at a given torque and input voltage.
+ * Calculates the angular speed produced by the motor at a given torque and input voltage.
*
* @param torqueNm The torque produced by the motor.
* @param voltageInputVolts The voltage applied to the motor.
- * @return The speed of the motor.
+ * @return The angular speed of the motor.
*/
public double getSpeed(double torqueNm, double voltageInputVolts) {
- return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+ return voltageInputVolts * KvRadPerSecPerVolt
+ - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
}
/**
@@ -227,6 +228,18 @@
}
/**
+ * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control) enabled.
+ *
+ * @param numMotors Number of motors in the gearbox.
+ * @return A gearbox of Falcon 500 FOC enabled motors.
+ */
+ public static DCMotor getFalcon500Foc(int numMotors) {
+ // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+ return new DCMotor(
+ 12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors);
+ }
+
+ /**
* Return a gearbox of Romi/TI_RSLK MAX motors.
*
* @param numMotors Number of motors in the gearbox.
@@ -237,4 +250,40 @@
return new DCMotor(
4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
}
+
+ /**
+ * Return a gearbox of Kraken X60 brushless motors.
+ *
+ * @param numMotors Number of motors in the gearbox.
+ * @return a gearbox of Kraken X60 motors.
+ */
+ public static DCMotor getKrakenX60(int numMotors) {
+ // From https://store.ctr-electronics.com/announcing-kraken-x60/
+ return new DCMotor(
+ 12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(6000), numMotors);
+ }
+
+ /**
+ * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented Control) enabled.
+ *
+ * @param numMotors Number of motors in the gearbox.
+ * @return A gearbox of Kraken X60 FOC enabled motors.
+ */
+ public static DCMotor getKrakenX60Foc(int numMotors) {
+ // From https://store.ctr-electronics.com/announcing-kraken-x60/
+ return new DCMotor(
+ 12, 9.37, 483, 2, Units.rotationsPerMinuteToRadiansPerSecond(5800), numMotors);
+ }
+
+ /**
+ * Return a gearbox of Neo Vortex brushless motors.
+ *
+ * @param numMotors Number of motors in the gearbox.
+ * @return a gearbox of Neo Vortex motors.
+ */
+ public static DCMotor getNeoVortex(int numMotors) {
+ // From https://www.revrobotics.com/next-generation-spark-neo/
+ return new DCMotor(
+ 12, 3.60, 211, 3.6, Units.rotationsPerMinuteToRadiansPerSecond(6784), numMotors);
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
index 332e690..8377022 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
@@ -123,6 +123,39 @@
}
/**
+ * Create a state-space model of a DC motor system. The states of the system are [angular
+ * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
+ * velocity].
+ *
+ * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+ * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+ *
+ * <p>The parameters provided by the user are from this feedforward model:
+ *
+ * <p>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^2)
+ * @return A LinearSystem representing the given characterized constants.
+ * @throws IllegalArgumentException if kV <= 0 or kA <= 0.
+ * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+ */
+ public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
+ if (kV <= 0.0) {
+ throw new IllegalArgumentException("Kv must be greater than zero.");
+ }
+ if (kA <= 0.0) {
+ throw new IllegalArgumentException("Ka must be greater than zero.");
+ }
+
+ return new LinearSystem<>(
+ Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA),
+ VecBuilder.fill(0, 1 / kA),
+ Matrix.eye(Nat.N2()),
+ new Matrix<>(Nat.N2(), Nat.N1()));
+ }
+
+ /**
* Create a state-space model of a differential drive drivetrain. In this model, the states are
* [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
* [left velocity, right velocity]ᵀ.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
new file mode 100644
index 0000000..1580e85
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
@@ -0,0 +1,449 @@
+// 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.
+
+package edu.wpi.first.math.trajectory;
+
+import java.util.Objects;
+
+/**
+ * A exponential curve-shaped velocity profile.
+ *
+ * <p>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 state-space model dynamics. To compute the reference
+ * obeying this constraint, do the following.
+ *
+ * <p>Initialization:
+ *
+ * <pre><code>
+ * ExponentialProfile.Constraints constraints =
+ * ExponentialProfile.Constraints.fromCharacteristics(kMaxV, kV, kA);
+ * ExponentialProfile.State previousProfiledReference =
+ * new ExponentialProfile.State(initialReference, 0.0);
+ * ExponentialProfile profile = new ExponentialProfile(constraints);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ *
+ * <pre><code>
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
+ * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
+ * determine when the profile has completed via `isFinished()`.
+ */
+public class ExponentialProfile {
+ private final Constraints m_constraints;
+
+ public static class ProfileTiming {
+ public final double inflectionTime;
+ public final double totalTime;
+
+ protected ProfileTiming(double inflectionTime, double totalTime) {
+ this.inflectionTime = inflectionTime;
+ this.totalTime = totalTime;
+ }
+
+ /**
+ * Decides if the profile is finished by time t.
+ *
+ * @param t The time since the beginning of the profile.
+ * @return if the profile is finished at time t.
+ */
+ public boolean isFinished(double t) {
+ return t > inflectionTime;
+ }
+ }
+
+ public static class Constraints {
+ public final double maxInput;
+
+ public final double A;
+ public final double B;
+
+ /**
+ * Construct constraints for an ExponentialProfile.
+ *
+ * @param maxInput maximum unsigned input voltage
+ * @param A The State-Space 1x1 system matrix.
+ * @param B The State-Space 1x1 input matrix.
+ */
+ private Constraints(double maxInput, double A, double B) {
+ this.maxInput = maxInput;
+ this.A = A;
+ this.B = B;
+ }
+
+ /**
+ * Computes the max achievable velocity for an Exponential Profile.
+ *
+ * @return The seady-state velocity achieved by this profile.
+ */
+ public double maxVelocity() {
+ return -maxInput * B / A;
+ }
+
+ /**
+ * Construct constraints for an ExponentialProfile from characteristics.
+ *
+ * @param maxInput maximum unsigned input voltage
+ * @param kV The velocity gain.
+ * @param kA The acceleration gain.
+ * @return The Constraints object.
+ */
+ public static Constraints fromCharacteristics(double maxInput, double kV, double kA) {
+ return new Constraints(maxInput, -kV / kA, 1.0 / kA);
+ }
+
+ /**
+ * Construct constraints for an ExponentialProfile from State-Space parameters.
+ *
+ * @param maxInput maximum unsigned input voltage
+ * @param A The State-Space 1x1 system matrix.
+ * @param B The State-Space 1x1 input matrix.
+ * @return The Constraints object.
+ */
+ public static Constraints fromStateSpace(double maxInput, double A, double B) {
+ return new Constraints(maxInput, A, B);
+ }
+ }
+
+ public static class State {
+ public final double position;
+
+ public final double velocity;
+
+ /**
+ * Construct a state within an exponential profile.
+ *
+ * @param position The position at this state.
+ * @param velocity The velocity at this state.
+ */
+ public State(double position, double velocity) {
+ this.position = position;
+ this.velocity = velocity;
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (other instanceof State) {
+ State rhs = (State) other;
+ return this.position == rhs.position && this.velocity == rhs.velocity;
+ } else {
+ return false;
+ }
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(position, velocity);
+ }
+ }
+
+ /**
+ * Construct an ExponentialProfile.
+ *
+ * @param constraints The constraints on the profile, like maximum input.
+ */
+ public ExponentialProfile(Constraints constraints) {
+ m_constraints = constraints;
+ }
+
+ /**
+ * Calculate the correct position and velocity for the profile at a time t where the current state
+ * is at time t = 0.
+ *
+ * @param t The time since the beginning of the profile.
+ * @param current The current state.
+ * @param goal The desired state when the profile is complete.
+ * @return The position and velocity of the profile at time t.
+ */
+ public State calculate(double t, State current, State goal) {
+ var direction = shouldFlipInput(current, goal) ? -1 : 1;
+ var u = direction * m_constraints.maxInput;
+
+ var inflectionPoint = calculateInflectionPoint(current, goal, u);
+ var timing = calculateProfileTiming(current, inflectionPoint, goal, u);
+
+ if (t < 0) {
+ return current;
+ } else if (t < timing.inflectionTime) {
+ return new State(
+ computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current));
+ } else if (t < timing.totalTime) {
+ return new State(
+ computeDistanceFromTime(t - timing.totalTime, -u, goal),
+ computeVelocityFromTime(t - timing.totalTime, -u, goal));
+ } else {
+ return goal;
+ }
+ }
+
+ /**
+ * Calculate the point after which the fastest way to reach the goal state is to apply input in
+ * the opposite direction.
+ *
+ * @param current The current state.
+ * @param goal The desired state when the profile is complete.
+ * @return The position and velocity of the profile at the inflection point.
+ */
+ public State calculateInflectionPoint(State current, State goal) {
+ var direction = shouldFlipInput(current, goal) ? -1 : 1;
+ var u = direction * m_constraints.maxInput;
+
+ return calculateInflectionPoint(current, goal, u);
+ }
+
+ /**
+ * Calculate the point after which the fastest way to reach the goal state is to apply input in
+ * the opposite direction.
+ *
+ * @param current The current state.
+ * @param goal The desired state when the profile is complete.
+ * @param input The signed input applied to this profile from the current state.
+ * @return The position and velocity of the profile at the inflection point.
+ */
+ private State calculateInflectionPoint(State current, State goal, double input) {
+ var u = input;
+
+ if (current.equals(goal)) {
+ return current;
+ }
+
+ var inflectionVelocity = solveForInflectionVelocity(u, current, goal);
+ var inflectionPosition = computeDistanceFromVelocity(inflectionVelocity, -u, goal);
+
+ return new State(inflectionPosition, inflectionVelocity);
+ }
+
+ /**
+ * Calculate the time it will take for this profile to reach the goal state.
+ *
+ * @param current The current state.
+ * @param goal The desired state when the profile is complete.
+ * @return The total duration of this profile.
+ */
+ public double timeLeftUntil(State current, State goal) {
+ var timing = calculateProfileTiming(current, goal);
+
+ return timing.totalTime;
+ }
+
+ /**
+ * 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.
+ *
+ * @param current The current state.
+ * @param goal The desired state when the profile is complete.
+ * @return The timing information for this profile.
+ */
+ public ProfileTiming calculateProfileTiming(State current, State goal) {
+ var direction = shouldFlipInput(current, goal) ? -1 : 1;
+ var u = direction * m_constraints.maxInput;
+
+ var inflectionPoint = calculateInflectionPoint(current, goal, u);
+ return calculateProfileTiming(current, inflectionPoint, goal, u);
+ }
+
+ /**
+ * 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.
+ *
+ * @param current The current state.
+ * @param inflectionPoint The inflection point of this profile.
+ * @param goal The desired state when the profile is complete.
+ * @param input The signed input applied to this profile from the current state.
+ * @return The timing information for this profile.
+ */
+ private ProfileTiming calculateProfileTiming(
+ State current, State inflectionPoint, State goal, double input) {
+ var u = input;
+
+ double 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.
+ double epsilon = 1e-9;
+ if (Math.abs(Math.signum(input) * m_constraints.maxVelocity() - inflectionPoint.velocity)
+ < epsilon) {
+ double solvableV = inflectionPoint.velocity;
+ double t_to_solvable_v;
+ double x_at_solvable_v;
+ if (Math.abs(current.velocity - inflectionPoint.velocity) < epsilon) {
+ t_to_solvable_v = 0;
+ x_at_solvable_v = current.position;
+ } else {
+ if (Math.abs(current.velocity) > m_constraints.maxVelocity()) {
+ solvableV += Math.signum(u) * epsilon;
+ } else {
+ solvableV -= Math.signum(u) * epsilon;
+ }
+
+ t_to_solvable_v = computeTimeFromVelocity(solvableV, u, current.velocity);
+ x_at_solvable_v = computeDistanceFromVelocity(solvableV, u, current);
+ }
+
+ inflectionT_forward =
+ t_to_solvable_v
+ + Math.signum(input)
+ * (inflectionPoint.position - x_at_solvable_v)
+ / m_constraints.maxVelocity();
+ } else {
+ inflectionT_forward = computeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
+ }
+
+ var inflectionT_backward = computeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
+
+ return new ProfileTiming(inflectionT_forward, inflectionT_forward - inflectionT_backward);
+ }
+
+ /**
+ * Calculate the position reached after t seconds when applying an input from the initial state.
+ *
+ * @param t The time since the initial state.
+ * @param input The signed input applied to this profile from the initial state.
+ * @param initial The initial state.
+ * @return The distance travelled by this profile.
+ */
+ private double computeDistanceFromTime(double t, double input, State initial) {
+ var A = m_constraints.A;
+ var B = m_constraints.B;
+ var u = input;
+
+ return initial.position
+ + (-B * u * t + (initial.velocity + B * u / A) * (Math.exp(A * t) - 1)) / A;
+ }
+
+ /**
+ * Calculate the velocity reached after t seconds when applying an input from the initial state.
+ *
+ * @param t The time since the initial state.
+ * @param input The signed input applied to this profile from the initial state.
+ * @param initial The initial state.
+ * @return The distance travelled by this profile.
+ */
+ private double computeVelocityFromTime(double t, double input, State initial) {
+ var A = m_constraints.A;
+ var B = m_constraints.B;
+ var u = input;
+
+ return (initial.velocity + B * u / A) * Math.exp(A * t) - B * u / A;
+ }
+
+ /**
+ * Calculate the time required to reach a specified velocity given the initial velocity.
+ *
+ * @param velocity The goal velocity.
+ * @param input The signed input applied to this profile from the initial state.
+ * @param initial The initial velocity.
+ * @return The time required to reach the goal velocity.
+ */
+ private double computeTimeFromVelocity(double velocity, double input, double initial) {
+ var A = m_constraints.A;
+ var B = m_constraints.B;
+ var u = input;
+
+ return Math.log((A * velocity + B * u) / (A * initial + B * u)) / A;
+ }
+
+ /**
+ * Calculate the distance reached at the same time as the given velocity when applying the given
+ * input from the initial state.
+ *
+ * @param velocity The velocity reached by this profile
+ * @param input The signed input applied to this profile from the initial state.
+ * @param initial The initial state.
+ * @return The distance reached when the given velocity is reached.
+ */
+ private double computeDistanceFromVelocity(double velocity, double input, State initial) {
+ var A = m_constraints.A;
+ var B = m_constraints.B;
+ var u = input;
+
+ return initial.position
+ + (velocity - initial.velocity) / A
+ - B * u / (A * A) * Math.log((A * velocity + B * u) / (A * initial.velocity + B * u));
+ }
+
+ /**
+ * Calculate the velocity at which input should be reversed in order to reach the goal state from
+ * the current state.
+ *
+ * @param input The signed input applied to this profile from the current state.
+ * @param current The current state.
+ * @param goal The goal state.
+ * @return The inflection velocity.
+ */
+ private double solveForInflectionVelocity(double input, State current, State goal) {
+ var A = m_constraints.A;
+ var B = m_constraints.B;
+ var u = input;
+
+ var U_dir = Math.signum(u);
+
+ var position_delta = goal.position - current.position;
+ var velocity_delta = goal.velocity - current.velocity;
+
+ var scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
+ var power = -A / B / u * (A * position_delta - velocity_delta);
+
+ var a = -A * A;
+ var c = (B * B) * (u * u) + scalar * Math.exp(power);
+
+ if (-1e-9 < c && c < 0) {
+ // Numerical stability issue - the heuristic gets it right but c is around -1e-13
+ return 0;
+ }
+
+ return U_dir * Math.sqrt(-c / a);
+ }
+
+ /**
+ * 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.
+ *
+ * @param current The initial state (usually the current state).
+ * @param goal The desired state when the profile is complete.
+ */
+ @SuppressWarnings("UnnecessaryParentheses")
+ private boolean shouldFlipInput(State current, State goal) {
+ var u = m_constraints.maxInput;
+
+ var xf = goal.position;
+ var v0 = current.velocity;
+ var vf = goal.velocity;
+
+ var x_forward = computeDistanceFromVelocity(vf, u, current);
+ var x_reverse = computeDistanceFromVelocity(vf, -u, current);
+
+ if (v0 >= m_constraints.maxVelocity()) {
+ return xf < x_reverse;
+ }
+
+ if (v0 <= -m_constraints.maxVelocity()) {
+ return xf < x_forward;
+ }
+
+ var a = v0 >= 0;
+ var b = vf >= 0;
+ var c = xf >= x_forward;
+ var d = xf >= x_reverse;
+
+ return (a && !d) || (b && !c) || (!c && !d);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
index f716dc0..f7120be 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
@@ -30,9 +30,15 @@
* Constructs a trajectory from a vector of states.
*
* @param states A vector of states.
+ * @throws IllegalArgumentException if the vector of states is empty.
*/
public Trajectory(final List<State> states) {
m_states = states;
+
+ if (m_states.isEmpty()) {
+ throw new IllegalArgumentException("Trajectory manually created with no states.");
+ }
+
m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
}
@@ -92,8 +98,13 @@
*
* @param timeSeconds The point in time since the beginning of the trajectory to sample.
* @return The state at that point in time.
+ * @throws IllegalStateException if the trajectory has no states.
*/
public State sample(double timeSeconds) {
+ if (m_states.isEmpty()) {
+ throw new IllegalStateException("Trajectory cannot be sampled if it has no states.");
+ }
+
if (timeSeconds <= m_states.get(0).timeSeconds) {
return m_states.get(0);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
index cf9b6f8..a741aa0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
@@ -271,11 +271,7 @@
if (minMaxAccel.minAccelerationMetersPerSecondSq
> minMaxAccel.maxAccelerationMetersPerSecondSq) {
throw new TrajectoryGenerationException(
- "The constraint's min acceleration "
- + "was greater than its max acceleration.\n Offending Constraint: "
- + constraint.getClass().getName()
- + "\n If the offending constraint was packaged with WPILib, please file a bug"
- + " report.");
+ "Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n");
}
state.minAccelerationMetersPerSecondSq =
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index 3b6559e..fd7494c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -22,14 +22,14 @@
* new TrapezoidProfile.Constraints(kMaxV, kMaxA);
* TrapezoidProfile.State previousProfiledReference =
* new TrapezoidProfile.State(initialReference, 0.0);
+ * TrapezoidProfile profile = new TrapezoidProfile(constraints);
* </code></pre>
*
* <p>Run on update:
*
* <pre><code>
- * TrapezoidProfile profile =
- * new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
- * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
* </code></pre>
*
* <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -42,9 +42,10 @@
// The direction of the profile, either 1 for forwards or -1 for inverted
private int m_direction;
- private Constraints m_constraints;
- private State m_initial;
- private State m_goal;
+ private final Constraints m_constraints;
+ private State m_current;
+ private State m_goal; // TODO: Remove
+ private final boolean m_newAPI; // TODO: Remove
private double m_endAccel;
private double m_endFullSpeed;
@@ -100,23 +101,36 @@
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
+ */
+ public TrapezoidProfile(Constraints constraints) {
+ m_constraints = constraints;
+ m_newAPI = true;
+ }
+
+ /**
+ * 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
*/
+ @Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfile(Constraints constraints, State goal, State initial) {
m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
m_constraints = constraints;
- m_initial = direct(initial);
+ m_current = direct(initial);
m_goal = direct(goal);
-
- if (m_initial.velocity > m_constraints.maxVelocity) {
- m_initial.velocity = m_constraints.maxVelocity;
+ 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
- double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+ double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
@@ -126,7 +140,7 @@
// of a truncated one
double fullTrapezoidDist =
- cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+ cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist =
@@ -148,7 +162,10 @@
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
+ * @deprecated Pass the desired and current state into calculate instead of constructing a new
+ * TrapezoidProfile with the desired and current state
*/
+ @Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfile(Constraints constraints, State goal) {
this(constraints, goal, new State(0, 0));
}
@@ -159,17 +176,23 @@
*
* @param t The time since the beginning of the profile.
* @return The position and velocity of the profile at time t.
+ * @deprecated Pass the desired and current state into calculate instead of constructing a new
+ * TrapezoidProfile with the desired and current state
*/
+ @Deprecated(since = "2024", forRemoval = true)
public State calculate(double t) {
- State result = new State(m_initial.position, m_initial.velocity);
+ if (m_newAPI) {
+ throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
+ }
+ State result = new State(m_current.position, m_current.velocity);
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
- result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+ 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_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+ (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 = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
@@ -185,14 +208,83 @@
}
/**
+ * 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 current state.
+ * @return The position and velocity of the profile at time t.
+ */
+ public State calculate(double 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
+ double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
+ double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+ double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
+ double 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
+
+ double fullTrapezoidDist =
+ cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
+ double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+ double fullSpeedDist =
+ fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+ // Handle the case where the profile never reaches full speed
+ if (fullSpeedDist < 0) {
+ accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+ fullSpeedDist = 0;
+ }
+
+ m_endAccel = accelerationTime - cutoffBegin;
+ m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+ m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+ State result = new State(m_current.position, m_current.velocity);
+
+ 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;
+ double timeLeft = m_endDeccel - t;
+ result.position =
+ goal.position
+ - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
+ } else {
+ result = goal;
+ }
+
+ return direct(result);
+ }
+
+ /**
* Returns the time left until a target distance in the profile is reached.
*
* @param target The target distance.
* @return The time left until a target distance in the profile is reached.
*/
public double timeLeftUntil(double target) {
- double position = m_initial.position * m_direction;
- double velocity = m_initial.velocity * m_direction;
+ double position = m_current.position * m_direction;
+ double velocity = m_current.velocity * m_direction;
double endAccel = m_endAccel * m_direction;
double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
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.
##
diff --git a/wpimath/src/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto
new file mode 100644
index 0000000..0d0d3fb
--- /dev/null
+++ b/wpimath/src/main/proto/controller.proto
@@ -0,0 +1,37 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufArmFeedforward {
+ double ks = 1;
+ double kg = 2;
+ double kv = 3;
+ double ka = 4;
+}
+
+message ProtobufDifferentialDriveFeedforward {
+ double kv_linear = 1;
+ double ka_linear = 2;
+ double kv_angular = 3;
+ double ka_angular = 4;
+}
+
+message ProtobufElevatorFeedforward {
+ double ks = 1;
+ double kg = 2;
+ double kv = 3;
+ double ka = 4;
+}
+
+message ProtobufSimpleMotorFeedforward {
+ double ks = 1;
+ double kv = 2;
+ double ka = 3;
+}
+
+message ProtobufDifferentialDriveWheelVoltages {
+ double left = 1;
+ double right = 2;
+}
diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto
new file mode 100644
index 0000000..d52da45
--- /dev/null
+++ b/wpimath/src/main/proto/geometry2d.proto
@@ -0,0 +1,30 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTranslation2d {
+ double x = 1;
+ double y = 2;
+}
+
+message ProtobufRotation2d {
+ double value = 1;
+}
+
+message ProtobufPose2d {
+ ProtobufTranslation2d translation = 1;
+ ProtobufRotation2d rotation = 2;
+}
+
+message ProtobufTransform2d {
+ ProtobufTranslation2d translation = 1;
+ ProtobufRotation2d rotation = 2;
+}
+
+message ProtobufTwist2d {
+ double dx = 1;
+ double dy = 2;
+ double dtheta = 3;
+}
diff --git a/wpimath/src/main/proto/geometry3d.proto b/wpimath/src/main/proto/geometry3d.proto
new file mode 100644
index 0000000..23b4d64
--- /dev/null
+++ b/wpimath/src/main/proto/geometry3d.proto
@@ -0,0 +1,41 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTranslation3d {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+}
+
+message ProtobufQuaternion {
+ double w = 1;
+ double x = 2;
+ double y = 3;
+ double z = 4;
+}
+
+message ProtobufRotation3d {
+ ProtobufQuaternion q = 1;
+}
+
+message ProtobufPose3d {
+ ProtobufTranslation3d translation = 1;
+ ProtobufRotation3d rotation = 2;
+}
+
+message ProtobufTransform3d {
+ ProtobufTranslation3d translation = 1;
+ ProtobufRotation3d rotation = 2;
+}
+
+message ProtobufTwist3d {
+ double dx = 1;
+ double dy = 2;
+ double dz = 3;
+ double rx = 4;
+ double ry = 5;
+ double rz = 6;
+}
diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto
new file mode 100644
index 0000000..8fdbf2d
--- /dev/null
+++ b/wpimath/src/main/proto/kinematics.proto
@@ -0,0 +1,64 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "geometry2d.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufChassisSpeeds {
+ double vx = 1;
+ double vy = 2;
+ double omega = 3;
+}
+
+message ProtobufDifferentialDriveKinematics {
+ double track_width = 1;
+}
+
+message ProtobufDifferentialDriveWheelSpeeds {
+ double left = 1;
+ double right = 2;
+}
+
+message ProtobufMecanumDriveKinematics {
+ ProtobufTranslation2d front_left = 1;
+ ProtobufTranslation2d front_right = 2;
+ ProtobufTranslation2d rear_left = 3;
+ ProtobufTranslation2d rear_right = 4;
+}
+
+message ProtobufMecanumDriveMotorVoltages {
+ double front_left = 1;
+ double front_right = 2;
+ double rear_left = 3;
+ double rear_right = 4;
+}
+
+message ProtobufMecanumDriveWheelPositions {
+ double front_left = 1;
+ double front_right = 2;
+ double rear_left = 3;
+ double rear_right = 4;
+}
+
+message ProtobufMecanumDriveWheelSpeeds {
+ double front_left = 1;
+ double front_right = 2;
+ double rear_left = 3;
+ double rear_right = 4;
+}
+
+message ProtobufSwerveDriveKinematics {
+ repeated ProtobufTranslation2d modules = 1;
+}
+
+message ProtobufSwerveModulePosition {
+ double distance = 1;
+ ProtobufRotation2d angle = 2;
+}
+
+message ProtobufSwerveModuleState {
+ double speed = 1;
+ ProtobufRotation2d angle = 2;
+}
diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto
new file mode 100644
index 0000000..d0d9eab
--- /dev/null
+++ b/wpimath/src/main/proto/plant.proto
@@ -0,0 +1,16 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufDCMotor {
+ double nominal_voltage = 1;
+ double stall_torque = 2;
+ double stall_current = 3;
+ double free_current = 4;
+ double free_speed = 5;
+ double r = 6;
+ double kv = 7;
+ double kt = 8;
+}
diff --git a/wpimath/src/main/proto/spline.proto b/wpimath/src/main/proto/spline.proto
new file mode 100644
index 0000000..ef0a3c9
--- /dev/null
+++ b/wpimath/src/main/proto/spline.proto
@@ -0,0 +1,19 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufCubicHermiteSpline {
+ repeated double x_initial = 1;
+ repeated double x_final = 2;
+ repeated double y_initial = 3;
+ repeated double y_final = 4;
+}
+
+message ProtobufQuinticHermiteSpline {
+ repeated double x_initial = 1;
+ repeated double x_final = 2;
+ repeated double y_initial = 3;
+ repeated double y_final = 4;
+}
diff --git a/wpimath/src/main/proto/system.proto b/wpimath/src/main/proto/system.proto
new file mode 100644
index 0000000..4818c19
--- /dev/null
+++ b/wpimath/src/main/proto/system.proto
@@ -0,0 +1,17 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "wpimath.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufLinearSystem {
+ uint32 num_states = 1;
+ uint32 num_inputs = 2;
+ uint32 num_outputs = 3;
+ ProtobufMatrix a = 4;
+ ProtobufMatrix b = 5;
+ ProtobufMatrix c = 6;
+ ProtobufMatrix d = 7;
+}
diff --git a/wpimath/src/main/proto/trajectory.proto b/wpimath/src/main/proto/trajectory.proto
new file mode 100644
index 0000000..a37a501
--- /dev/null
+++ b/wpimath/src/main/proto/trajectory.proto
@@ -0,0 +1,20 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "geometry2d.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTrajectoryState {
+ double time = 1;
+ double velocity = 2;
+ double acceleration = 3;
+ ProtobufPose2d pose = 4;
+ double curvature = 5;
+}
+
+message ProtobufTrajectory {
+ double total_time = 1;
+ repeated ProtobufTrajectoryState states = 2;
+}
diff --git a/wpimath/src/main/proto/wpimath.proto b/wpimath/src/main/proto/wpimath.proto
new file mode 100644
index 0000000..06b993a
--- /dev/null
+++ b/wpimath/src/main/proto/wpimath.proto
@@ -0,0 +1,15 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufMatrix {
+ uint32 num_rows = 1;
+ uint32 num_cols = 2;
+ repeated double data = 3;
+}
+
+message ProtobufVector {
+ repeated double rows = 1;
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
index 2927847..3c281e5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
@@ -11,9 +11,14 @@
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.UtilityClassTest;
import org.junit.jupiter.api.Test;
-class ComputerVisionUtilTest {
+class ComputerVisionUtilTest extends UtilityClassTest<ComputerVisionUtil> {
+ ComputerVisionUtilTest() {
+ super(ComputerVisionUtil.class);
+ }
+
@Test
void testObjectToRobotPose() {
var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
new file mode 100644
index 0000000..0d697bd
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
@@ -0,0 +1,335 @@
+// 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.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
+class DARETest extends UtilityClassTest<DARE> {
+ DARETest() {
+ super(DARE.class);
+ }
+
+ public static <R extends Num, C extends Num> void assertMatrixEqual(
+ Matrix<R, C> A, Matrix<R, C> B) {
+ for (int i = 0; i < A.getNumRows(); i++) {
+ for (int j = 0; j < A.getNumCols(); j++) {
+ assertEquals(A.get(i, j), B.get(i, j), 1e-4);
+ }
+ }
+ }
+
+ <States extends Num, Inputs extends Num> void assertDARESolution(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R,
+ Matrix<States, States> X) {
+ // Check that X is the solution to the DARE
+ // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ var Y =
+ (A.transpose().times(X).times(A))
+ .minus(X)
+ .minus(
+ (A.transpose().times(X).times(B))
+ .times((B.transpose().times(X).times(B).plus(R)).inv())
+ .times(B.transpose().times(X).times(A)))
+ .plus(Q);
+ assertMatrixEqual(
+ new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+ }
+
+ <States extends Num, Inputs extends Num> void assertDARESolution(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R,
+ Matrix<States, Inputs> N,
+ Matrix<States, States> X) {
+ // Check that X is the solution to the DARE
+ // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ var Y =
+ (A.transpose().times(X).times(A))
+ .minus(X)
+ .minus(
+ (A.transpose().times(X).times(B).plus(N))
+ .times((B.transpose().times(X).times(B).plus(R)).inv())
+ .times(B.transpose().times(X).times(A).plus(N.transpose())))
+ .plus(Q);
+ assertMatrixEqual(
+ new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+ }
+
+ @Test
+ void testNonInvertibleA_ABQR() {
+ // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+ // Riccati Equation"
+
+ var A =
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+ var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
+ var Q =
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
+
+ var X = DARE.dare(A, B, Q, R);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, X);
+ }
+
+ @Test
+ void testNonInvertibleA_ABQRN() {
+ // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+ // Riccati Equation"
+
+ var A =
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+ var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
+ var Q =
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
+
+ var Aref =
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+ Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+ R = B.transpose().times(Q).times(B).plus(R);
+ var N = A.minus(Aref).transpose().times(Q).times(B);
+
+ var X = DARE.dare(A, B, Q, R, N);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, N, X);
+ }
+
+ @Test
+ void testInvertibleA_ABQR() {
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
+
+ var X = DARE.dare(A, B, Q, R);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, X);
+ }
+
+ @Test
+ void testInvertibleA_ABQRN() {
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
+
+ var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.5, 1, 0, 1});
+ Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+ R = B.transpose().times(Q).times(B).plus(R);
+ var N = A.minus(Aref).transpose().times(Q).times(B);
+
+ var X = DARE.dare(A, B, Q, R, N);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, N, X);
+ }
+
+ @Test
+ void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() {
+ // The first generalized eigenvalue of (S, T) is stable
+
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
+
+ var X = DARE.dare(A, B, Q, R);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, X);
+ }
+
+ @Test
+ void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() {
+ // The first generalized eigenvalue of (S, T) is stable
+
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
+
+ var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 0.5, 0, 0});
+ Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+ R = B.transpose().times(Q).times(B).plus(R);
+ var N = A.minus(Aref).transpose().times(Q).times(B);
+
+ var X = DARE.dare(A, B, Q, R, N);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, N, X);
+ }
+
+ @Test
+ void testIdentitySystem_ABQR() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+
+ var X = DARE.dare(A, B, Q, R);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, X);
+ }
+
+ @Test
+ void testIdentitySystem_ABQRN() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+ var N = Matrix.eye(Nat.N2());
+
+ var X = DARE.dare(A, B, Q, R, N);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, N, X);
+ }
+
+ @Test
+ void testMoreInputsThanStates_ABQR() {
+ var A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N3());
+
+ var X = DARE.dare(A, B, Q, R);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, X);
+ }
+
+ @Test
+ void testMoreInputsThanStates_ABQRN() {
+ var A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N3());
+ var N = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 1, 0});
+
+ var X = DARE.dare(A, B, Q, R, N);
+ assertMatrixEqual(X, X.transpose());
+ assertDARESolution(A, B, Q, R, N, X);
+ }
+
+ @Test
+ void testQNotSymmetricPositiveSemidefinite_ABQR() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+ var R = Matrix.eye(Nat.N2());
+
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+ }
+
+ @Test
+ void testQNotSymmetricPositiveSemidefinite_ABQRN() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+ var N = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {2.0, 0.0, 0.0, 2.0});
+
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+ }
+
+ @Test
+ void testRNotSymmetricPositiveDefinite_ABQR() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+
+ var R1 = new Matrix<>(Nat.N2(), Nat.N2());
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
+
+ var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2));
+ }
+
+ @Test
+ void testRNotSymmetricPositiveDefinite_ABQRN() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var N = Matrix.eye(Nat.N2());
+
+ var R1 = new Matrix<>(Nat.N2(), Nat.N2());
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
+
+ var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2, N));
+ }
+
+ @Test
+ void testABNotStabilizable_ABQR() {
+ var A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+ }
+
+ @Test
+ void testABNotStabilizable_ABQRN() {
+ var A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+ var N = Matrix.eye(Nat.N2());
+
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+ }
+
+ @Test
+ void testACNotDetectable_ABQR() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = new Matrix<>(Nat.N2(), Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+ }
+
+ @Test
+ void testACNotDetectable_ABQRN() {
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = new Matrix<>(Nat.N2(), Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+ var N = new Matrix<>(Nat.N2(), Nat.N2());
+
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+ }
+
+ @Test
+ void testQDecomposition() {
+ // Ensures the decomposition of Q into CᵀC is correct
+
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 0.0, 0.0, 0.0});
+ var B = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+
+ // (A, C₁) should be detectable pair
+ var C_1 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 0.0, 1.0, 0.0});
+ var Q_1 = C_1.transpose().times(C_1);
+ assertDoesNotThrow(() -> DARE.dare(A, B, Q_1, R));
+
+ // (A, C₂) shouldn't be detectable pair
+ var C_2 = C_1.transpose();
+ var Q_2 = C_2.transpose().times(C_2);
+ assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q_2, R));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
deleted file mode 100644
index 3050867..0000000
--- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ /dev/null
@@ -1,73 +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.
-
-package edu.wpi.first.math;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-
-class DrakeTest {
- public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
- for (int i = 0; i < A.numRows(); i++) {
- for (int j = 0; j < A.numCols(); j++) {
- assertEquals(A.get(i, j), B.get(i, j), 1e-4);
- }
- }
- }
-
- private boolean solveDAREandVerify(
- SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
- var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
-
- // expect that x is the same as it's transpose
- assertEquals(X.numRows(), X.numCols());
- assertMatrixEqual(X, X.transpose());
-
- // Verify that this is a solution to the DARE.
- SimpleMatrix Y =
- A.transpose()
- .mult(X)
- .mult(A)
- .minus(X)
- .minus(
- A.transpose()
- .mult(X)
- .mult(B)
- .mult(((B.transpose().mult(X).mult(B)).plus(R)).invert())
- .mult(B.transpose())
- .mult(X)
- .mult(A))
- .plus(Q);
- assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
-
- return true;
- }
-
- @Test
- void testDiscreteAlgebraicRicattiEquation() {
- int n1 = 4;
- int m1 = 1;
-
- // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
- SimpleMatrix A1 =
- new SimpleMatrix(
- n1, n1, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0})
- .transpose();
- SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[] {0, 0, 0, 1});
- SimpleMatrix Q1 =
- new SimpleMatrix(
- n1, n1, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
- SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[] {0.25});
- assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
-
- SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
- SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[] {0, 1});
- SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
- SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[] {0.3});
- assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
- }
-}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
index a3de9cc..44f7c50 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -5,10 +5,32 @@
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import edu.wpi.first.wpilibj.UtilityClassTest;
import org.junit.jupiter.api.Test;
-class MathUtilTest {
+class MathUtilTest extends UtilityClassTest<MathUtil> {
+ MathUtilTest() {
+ super(MathUtil.class);
+ }
+
+ @Test
+ void testClamp() {
+ // int
+ assertEquals(5, MathUtil.clamp(10, 1, 5));
+
+ // double
+ assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5));
+
+ // negative int
+ assertEquals(-5, MathUtil.clamp(-10, -5, -1));
+
+ // negative double
+ assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5));
+ }
+
@Test
void testApplyDeadbandUnityScale() {
// < 0
@@ -88,4 +110,61 @@
assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
}
+
+ @Test
+ void testInterpolate() {
+ assertEquals(50, MathUtil.interpolate(0, 100, 0.5));
+ assertEquals(-50, MathUtil.interpolate(0, -100, 0.5));
+ assertEquals(0, MathUtil.interpolate(-50, 50, 0.5));
+ assertEquals(-25, MathUtil.interpolate(-50, 50, 0.25));
+ assertEquals(25, MathUtil.interpolate(-50, 50, 0.75));
+
+ assertEquals(0, MathUtil.interpolate(0, -100, -0.5));
+ }
+
+ @Test
+ void testIsNear() {
+ // The answer is always 42
+ // Positive integer checks
+ assertTrue(MathUtil.isNear(42, 42, 1));
+ assertTrue(MathUtil.isNear(42, 41, 2));
+ assertTrue(MathUtil.isNear(42, 43, 2));
+ assertFalse(MathUtil.isNear(42, 44, 1));
+
+ // Negative integer checks
+ assertTrue(MathUtil.isNear(-42, -42, 1));
+ assertTrue(MathUtil.isNear(-42, -41, 2));
+ assertTrue(MathUtil.isNear(-42, -43, 2));
+ assertFalse(MathUtil.isNear(-42, -44, 1));
+
+ // Mixed sign integer checks
+ assertFalse(MathUtil.isNear(-42, 42, 1));
+ assertFalse(MathUtil.isNear(-42, 41, 2));
+ assertFalse(MathUtil.isNear(-42, 43, 2));
+ assertFalse(MathUtil.isNear(42, -42, 1));
+ assertFalse(MathUtil.isNear(42, -41, 2));
+ assertFalse(MathUtil.isNear(42, -43, 2));
+
+ // Floating point checks
+ assertTrue(MathUtil.isNear(42, 41.5, 1));
+ assertTrue(MathUtil.isNear(42, 42.5, 1));
+ assertTrue(MathUtil.isNear(42, 41.5, 0.75));
+ assertTrue(MathUtil.isNear(42, 42.5, 0.75));
+
+ // Wraparound checks
+ assertTrue(MathUtil.isNear(0, 356, 5, 0, 360));
+ assertTrue(MathUtil.isNear(0, -356, 5, 0, 360));
+ assertTrue(MathUtil.isNear(0, 4, 5, 0, 360));
+ assertTrue(MathUtil.isNear(0, -4, 5, 0, 360));
+ assertTrue(MathUtil.isNear(400, 41, 5, 0, 360));
+ assertTrue(MathUtil.isNear(400, -319, 5, 0, 360));
+ assertTrue(MathUtil.isNear(400, 401, 5, 0, 360));
+ assertFalse(MathUtil.isNear(0, 356, 2.5, 0, 360));
+ assertFalse(MathUtil.isNear(0, -356, 2.5, 0, 360));
+ assertFalse(MathUtil.isNear(0, 4, 2.5, 0, 360));
+ assertFalse(MathUtil.isNear(0, -4, 2.5, 0, 360));
+ assertFalse(MathUtil.isNear(400, 35, 5, 0, 360));
+ assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
+ assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index 054c29a..ee33f01 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -12,13 +12,18 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.wpilibj.UtilityClassTest;
import java.util.ArrayList;
import java.util.List;
import org.ejml.dense.row.MatrixFeatures_DDRM;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
-class StateSpaceUtilTest {
+class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
+ StateSpaceUtilTest() {
+ super(StateSpaceUtil.class);
+ }
+
@Test
void testCostArray() {
var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 5488b68..008953f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -10,6 +10,44 @@
class VectorTest {
@Test
+ void testVectorPlus() {
+ var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+ var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+ var result1 = vec1.plus(vec2);
+
+ assertEquals(5.0, result1.get(0, 0));
+ assertEquals(7.0, result1.get(1, 0));
+ assertEquals(9.0, result1.get(2, 0));
+
+ var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+ var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+ var result2 = vec3.plus(vec4);
+
+ assertEquals(3.0, result2.get(0, 0));
+ assertEquals(-3.0, result2.get(1, 0));
+ assertEquals(3.0, result2.get(2, 0));
+ }
+
+ @Test
+ void testVectorMinus() {
+ var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+ var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+ var result1 = vec1.minus(vec2);
+
+ assertEquals(-3.0, result1.get(0, 0));
+ assertEquals(-3.0, result1.get(1, 0));
+ assertEquals(-3.0, result1.get(2, 0));
+
+ var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+ var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+ var result2 = vec3.minus(vec4);
+
+ assertEquals(-5.0, result2.get(0, 0));
+ assertEquals(7.0, result2.get(1, 0));
+ assertEquals(-9.0, result2.get(2, 0));
+ }
+
+ @Test
void testVectorDot() {
var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
new file mode 100644
index 0000000..7b5cf77
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardTest {
+ private static final double ks = 0.5;
+ private static final double kg = 1;
+ private static final double kv = 1.5;
+ private static final double ka = 2;
+ private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+ @Test
+ void testCalculate() {
+ assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
+ assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
+ assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
+ assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
+ }
+
+ @Test
+ void testAcheviableVelocity() {
+ assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
+ assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
+ }
+
+ @Test
+ void testAcheviableAcceleration() {
+ assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+ assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+ assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+ assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
index b23ee1e..faf5563 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
@@ -231,7 +231,6 @@
.times(xAccelLimiter)
.plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
- System.out.println(a);
assertTrue(Math.abs(a) > maxA);
assertTrue(Math.abs(a) > -minA);
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
new file mode 100644
index 0000000..6c978ac
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
@@ -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.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardTest {
+ private static final double ks = 0.5;
+ private static final double kg = 1;
+ private static final double kv = 1.5;
+ private static final double ka = 2;
+
+ private final ElevatorFeedforward m_elevatorFF = new ElevatorFeedforward(ks, kg, kv, ka);
+
+ @Test
+ void testCalculate() {
+ assertEquals(1, m_elevatorFF.calculate(0), 0.002);
+ assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
+ assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
+ assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
+
+ var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
+ var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
+ final double dt = 0.02;
+ var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+
+ var r = VecBuilder.fill(2.0);
+ var nextR = VecBuilder.fill(3.0);
+ assertEquals(
+ plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
+ m_elevatorFF.calculate(2.0, 3.0, dt),
+ 0.002);
+ }
+
+ @Test
+ void testAcheviableVelocity() {
+ assertEquals(5, m_elevatorFF.maxAchievableVelocity(11, 1), 0.002);
+ assertEquals(-9, m_elevatorFF.minAchievableVelocity(11, 1), 0.002);
+ }
+
+ @Test
+ void testAcheviableAcceleration() {
+ assertEquals(3.75, m_elevatorFF.maxAchievableAcceleration(12, 2), 0.002);
+ assertEquals(7.25, m_elevatorFF.maxAchievableAcceleration(12, -2), 0.002);
+ assertEquals(-8.25, m_elevatorFF.minAchievableAcceleration(12, 2), 0.002);
+ assertEquals(-4.75, m_elevatorFF.minAchievableAcceleration(12, -2), 0.002);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
index 31213f6..634ed1c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -148,10 +148,10 @@
assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
// QRN overload
- var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0));
+ var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0));
var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
assertEquals(0.0, Kimf.get(0, 0), 1e-10);
- assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10);
+ assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);
}
@Test
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
index 7b8484d..7ea8caa 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
@@ -58,12 +58,12 @@
TrapezoidProfile profile;
TrapezoidProfile.State state;
for (int i = 0; i < 1000; i++) {
- profile =
- new TrapezoidProfile(
- constraints,
+ profile = new TrapezoidProfile(constraints);
+ state =
+ profile.calculate(
+ kDt,
new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
- state = profile.calculate(kDt);
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
index 1fe4cb1..b25240e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
@@ -55,4 +55,24 @@
assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
}
+
+ @Test
+ void iZoneNoOutputTest() {
+ m_controller.setI(1);
+ m_controller.setIZone(1);
+
+ double out = m_controller.calculate(2, 0);
+
+ assertEquals(0, out, 1e-5);
+ }
+
+ @Test
+ void iZoneOutputTest() {
+ m_controller.setI(1);
+ m_controller.setIZone(1);
+
+ double out = m_controller.calculate(1, 0);
+
+ assertEquals(-1 * m_controller.getPeriod(), out, 1e-5);
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
index b525f49..4fdb867 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
@@ -19,7 +19,7 @@
try (var controller = new PIDController(0.05, 0.0, 0.0)) {
controller.enableContinuousInput(-kRange / 2, kRange / 2);
- assertTrue(controller.atSetpoint());
+ assertFalse(controller.atSetpoint());
}
}
@@ -28,10 +28,7 @@
try (var controller = new PIDController(0.05, 0.0, 0.0)) {
controller.enableContinuousInput(-kRange / 2, kRange / 2);
- assertTrue(
- controller.atSetpoint(),
- "Error was not in tolerance when it should have been. Error was "
- + controller.getPositionError());
+ assertFalse(controller.atSetpoint());
controller.setTolerance(kTolerance);
controller.setSetpoint(kSetpoint);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index c3906d4..e0e202f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -143,8 +143,6 @@
double t = 0.0;
- System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
-
final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
@@ -191,16 +189,6 @@
leftDistanceMeters,
rightDistanceMeters);
- System.out.printf(
- "%f, %f, %f, %f, %f, %f, %f\n",
- t,
- xHat.getX(),
- xHat.getY(),
- xHat.getRotation().getRadians(),
- groundTruthState.poseMeters.getX(),
- groundTruthState.poseMeters.getY(),
- groundTruthState.poseMeters.getRotation().getRadians());
-
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
@@ -227,4 +215,92 @@
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}
+
+ @Test
+ void testSimultaneousVisionMeasurements() {
+ // This tests for multiple vision measurements appled at the same time. The expected behavior
+ // is that all measurements affect the estimated pose. The alternative result is that only one
+ // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+ // estimated pose would converge to that measurement.
+ var kinematics = new DifferentialDriveKinematics(1);
+
+ var estimator =
+ new DifferentialDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ 0,
+ 0,
+ new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+ VecBuilder.fill(0.02, 0.02, 0.01),
+ VecBuilder.fill(0.1, 0.1, 0.1));
+
+ estimator.updateWithTime(0, new Rotation2d(), 0, 0);
+
+ var visionMeasurements =
+ new Pose2d[] {
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+ new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+ };
+
+ for (int i = 0; i < 1000; i++) {
+ for (var measurement : visionMeasurements) {
+ estimator.addVisionMeasurement(measurement, 0);
+ }
+ }
+
+ for (var measurement : visionMeasurements) {
+ var errorLog =
+ "Estimator converged to one vision measurement: "
+ + estimator.getEstimatedPosition().toString()
+ + " -> "
+ + measurement.toString();
+
+ var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+ var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+ var dtheta =
+ Math.abs(
+ measurement.getRotation().getDegrees()
+ - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+ assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+ }
+ }
+
+ @Test
+ void testDiscardsStaleVisionMeasurements() {
+ var kinematics = new DifferentialDriveKinematics(1);
+ var estimator =
+ new DifferentialDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ 0,
+ 0,
+ new Pose2d(),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
+
+ double time = 0;
+
+ // Add enough measurements to fill up the buffer
+ for (; time < 4; time += 0.02) {
+ estimator.updateWithTime(time, new Rotation2d(), 0, 0);
+ }
+
+ var odometryPose = estimator.getEstimatedPosition();
+
+ // Apply a vision measurement made 3 seconds ago
+ // This test passes if this does not cause a ConcurrentModificationException.
+ estimator.addVisionMeasurement(
+ new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+ 1,
+ VecBuilder.fill(0.1, 0.1, 0.1));
+
+ assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+ assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+ assertEquals(
+ odometryPose.getRotation().getRadians(),
+ estimator.getEstimatedPosition().getRotation().getRadians(),
+ "Incorrect Final Theta");
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index 02d2d52..d844c5d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -151,8 +151,6 @@
double t = 0.0;
- System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
-
final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
@@ -200,16 +198,6 @@
.minus(trajectory.getInitialPose().getRotation()),
wheelPositions);
- System.out.printf(
- "%f, %f, %f, %f, %f, %f, %f\n",
- t,
- xHat.getX(),
- xHat.getY(),
- xHat.getRotation().getRadians(),
- groundTruthState.poseMeters.getX(),
- groundTruthState.poseMeters.getY(),
- groundTruthState.poseMeters.getRotation().getRadians());
-
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
@@ -236,4 +224,100 @@
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}
+
+ @Test
+ void testSimultaneousVisionMeasurements() {
+ // This tests for multiple vision measurements appled at the same time. The expected behavior
+ // is that all measurements affect the estimated pose. The alternative result is that only one
+ // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+ // estimated pose would converge to that measurement.
+ var kinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(1, 1), new Translation2d(1, -1),
+ new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+ var wheelPositions = new MecanumDriveWheelPositions();
+
+ var estimator =
+ new MecanumDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ wheelPositions,
+ new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.45, 0.45, 0.1));
+
+ estimator.updateWithTime(0, new Rotation2d(), wheelPositions);
+
+ var visionMeasurements =
+ new Pose2d[] {
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+ new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+ };
+
+ for (int i = 0; i < 1000; i++) {
+ for (var measurement : visionMeasurements) {
+ estimator.addVisionMeasurement(measurement, 0);
+ }
+ }
+
+ for (var measurement : visionMeasurements) {
+ var errorLog =
+ "Estimator converged to one vision measurement: "
+ + estimator.getEstimatedPosition().toString()
+ + " -> "
+ + measurement.toString();
+
+ var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+ var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+ var dtheta =
+ Math.abs(
+ measurement.getRotation().getDegrees()
+ - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+ assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+ }
+ }
+
+ @Test
+ void testDiscardsOldVisionMeasurements() {
+ var kinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(1, 1),
+ new Translation2d(-1, 1),
+ new Translation2d(1, -1),
+ new Translation2d(-1, -1));
+ var estimator =
+ new MecanumDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ new MecanumDriveWheelPositions(),
+ new Pose2d(),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
+
+ double time = 0;
+
+ // Add enough measurements to fill up the buffer
+ for (; time < 4; time += 0.02) {
+ estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions());
+ }
+
+ var odometryPose = estimator.getEstimatedPosition();
+
+ // Apply a vision measurement made 3 seconds ago
+ // This test passes if this does not cause a ConcurrentModificationException.
+ estimator.addVisionMeasurement(
+ new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+ 1,
+ VecBuilder.fill(0.1, 0.1, 0.1));
+
+ assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+ assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+ assertEquals(
+ odometryPose.getRotation().getRadians(),
+ estimator.getEstimatedPosition().getRotation().getRadians(),
+ "Incorrect Final Theta");
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index fde2c39..9fcd852 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -165,11 +165,6 @@
double t = 0.0;
- System.out.print(
- "time, est_x, est_y, est_theta, true_x, true_y, true_theta, "
- + "distance_1, distance_2, distance_3, distance_4, "
- + "angle_1, angle_2, angle_3, angle_4\n");
-
final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
@@ -219,24 +214,6 @@
.minus(trajectory.getInitialPose().getRotation()),
positions);
- System.out.printf(
- "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
- t,
- xHat.getX(),
- xHat.getY(),
- xHat.getRotation().getRadians(),
- groundTruthState.poseMeters.getX(),
- groundTruthState.poseMeters.getY(),
- groundTruthState.poseMeters.getRotation().getRadians(),
- positions[0].distanceMeters,
- positions[1].distanceMeters,
- positions[2].distanceMeters,
- positions[3].distanceMeters,
- positions[0].angle.getRadians(),
- positions[1].angle.getRadians(),
- positions[2].angle.getRadians(),
- positions[3].angle.getRadians());
-
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
@@ -263,4 +240,118 @@
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}
+
+ @Test
+ void testSimultaneousVisionMeasurements() {
+ // This tests for multiple vision measurements appled at the same time. The expected behavior
+ // is that all measurements affect the estimated pose. The alternative result is that only one
+ // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+ // estimated pose would converge to that measurement.
+ var kinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(1, 1),
+ new Translation2d(1, -1),
+ new Translation2d(-1, -1),
+ new Translation2d(-1, 1));
+
+ var fl = new SwerveModulePosition();
+ var fr = new SwerveModulePosition();
+ var bl = new SwerveModulePosition();
+ var br = new SwerveModulePosition();
+
+ var estimator =
+ new SwerveDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ new SwerveModulePosition[] {fl, fr, bl, br},
+ new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
+
+ estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br});
+
+ var visionMeasurements =
+ new Pose2d[] {
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+ new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+ };
+
+ for (int i = 0; i < 1000; i++) {
+ for (var measurement : visionMeasurements) {
+ estimator.addVisionMeasurement(measurement, 0);
+ }
+ }
+
+ for (var measurement : visionMeasurements) {
+ var errorLog =
+ "Estimator converged to one vision measurement: "
+ + estimator.getEstimatedPosition().toString()
+ + " -> "
+ + measurement.toString();
+
+ var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+ var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+ var dtheta =
+ Math.abs(
+ measurement.getRotation().getDegrees()
+ - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+ assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+ }
+ }
+
+ @Test
+ void testDiscardsOldVisionMeasurements() {
+ var kinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(1, 1),
+ new Translation2d(-1, 1),
+ new Translation2d(1, -1),
+ new Translation2d(-1, -1));
+ var estimator =
+ new SwerveDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ },
+ new Pose2d(),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
+
+ double time = 0;
+
+ // Add enough measurements to fill up the buffer
+ for (; time < 4; time += 0.02) {
+ estimator.updateWithTime(
+ time,
+ new Rotation2d(),
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ });
+ }
+
+ var odometryPose = estimator.getEstimatedPosition();
+
+ // Apply a vision measurement made 3 seconds ago
+ // This test passes if this does not cause a ConcurrentModificationException.
+ estimator.addVisionMeasurement(
+ new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+ 1,
+ VecBuilder.fill(0.1, 0.1, 0.1));
+
+ assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+ assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+ assertEquals(
+ odometryPose.getRotation().getRadians(),
+ estimator.getEstimatedPosition().getRotation().getRadians(),
+ "Incorrect Final Theta");
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
index 68babdd..4dc1826 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
@@ -38,18 +38,12 @@
assertEquals(
transformTo.getTranslation(),
CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
- assertEquals(
- transformTo.getRotation(),
- CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo));
assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
// "to" to "from"
assertEquals(
transformFrom.getTranslation(),
CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
- assertEquals(
- transformFrom.getRotation(),
- CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom));
assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
}
@@ -158,9 +152,7 @@
// No rotation from EDN to NWU
checkTransform3dConvert(
new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
- new Transform3d(
- new Translation3d(3.0, -1.0, -2.0),
- new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+ new Transform3d(new Translation3d(3.0, -1.0, -2.0), new Rotation3d()),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
@@ -171,7 +163,7 @@
new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
- new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+ new Rotation3d(0.0, Units.degreesToRadians(-45.0), 0.0)),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
@@ -182,7 +174,7 @@
new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
- new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+ new Rotation3d(0.0, 0.0, Units.degreesToRadians(-45.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
@@ -193,10 +185,7 @@
new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
- new Rotation3d(
- Units.degreesToRadians(-90.0),
- Units.degreesToRadians(45.0),
- Units.degreesToRadians(-90.0))),
+ new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
}
@@ -206,9 +195,7 @@
// No rotation from EDN to NED
checkTransform3dConvert(
new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
- new Transform3d(
- new Translation3d(3.0, 1.0, 2.0),
- new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+ new Transform3d(new Translation3d(3.0, 1.0, 2.0), new Rotation3d()),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
@@ -219,7 +206,7 @@
new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
- new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+ new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
@@ -230,7 +217,7 @@
new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
- new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+ new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
@@ -241,10 +228,7 @@
new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
- new Rotation3d(
- Units.degreesToRadians(90.0),
- Units.degreesToRadians(-45.0),
- Units.degreesToRadians(90.0))),
+ new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index 780c816..d57362d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -8,12 +8,35 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import java.util.List;
import org.junit.jupiter.api.Test;
class Pose2dTest {
private static final double kEpsilon = 1E-9;
@Test
+ void testRotateBy() {
+ final double x = 1.0;
+ final double y = 2.0;
+ var initial = new Pose2d(new Translation2d(x, y), Rotation2d.fromDegrees(45.0));
+
+ var rotation = Rotation2d.fromDegrees(5.0);
+ var rotated = initial.rotateBy(rotation);
+
+ // Translation is rotated by CCW rotation matrix
+ double c = rotation.getCos();
+ double s = rotation.getSin();
+ assertAll(
+ () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+ () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+ () ->
+ assertEquals(
+ initial.getRotation().getDegrees() + rotation.getDegrees(),
+ rotated.getRotation().getDegrees(),
+ kEpsilon));
+ }
+
+ @Test
void testTransformBy() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
@@ -65,4 +88,50 @@
() -> assertEquals(0.0, transform.getY(), kEpsilon),
() -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
}
+
+ @Test
+ void testNearest() {
+ var origin = new Pose2d();
+
+ // Distance sort
+ // each poseX is X units away from the origin at a random angle.
+ final var pose1 =
+ new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), new Rotation2d());
+ final var pose2 =
+ new Pose2d(new Translation2d(2, Rotation2d.fromDegrees(90)), new Rotation2d());
+ final var pose3 =
+ new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), new Rotation2d());
+ final var pose4 =
+ new Pose2d(new Translation2d(4, Rotation2d.fromDegrees(180)), new Rotation2d());
+ final var pose5 =
+ new Pose2d(new Translation2d(5, Rotation2d.fromDegrees(270)), new Rotation2d());
+
+ assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
+ assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
+ assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
+
+ // Rotation component sort (when distance is the same)
+ // Use the same translation because using different angles at the same distance can cause
+ // rounding error.
+ final var translation = new Translation2d(1, new Rotation2d());
+
+ final var poseA = new Pose2d(translation, Rotation2d.fromDegrees(0));
+ final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30));
+ final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120));
+ final var poseD = new Pose2d(translation, Rotation2d.fromDegrees(90));
+ final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180));
+
+ assertEquals(
+ poseA, new Pose2d(0, 0, Rotation2d.fromDegrees(360)).nearest(List.of(poseA, poseB, poseD)));
+ assertEquals(
+ poseB,
+ new Pose2d(0, 0, Rotation2d.fromDegrees(-335)).nearest(List.of(poseB, poseC, poseD)));
+ assertEquals(
+ poseC,
+ new Pose2d(0, 0, Rotation2d.fromDegrees(-120)).nearest(List.of(poseB, poseC, poseD)));
+ assertEquals(
+ poseD, new Pose2d(0, 0, Rotation2d.fromDegrees(85)).nearest(List.of(poseA, poseC, poseD)));
+ assertEquals(
+ poseE, new Pose2d(0, 0, Rotation2d.fromDegrees(170)).nearest(List.of(poseA, poseD, poseE)));
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
index f13819f..ee1d98c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
@@ -6,16 +6,50 @@
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units;
+import java.util.Arrays;
import org.junit.jupiter.api.Test;
class Pose3dTest {
private static final double kEpsilon = 1E-9;
@Test
+ void testRotateBy() {
+ final double x = 1.0;
+ final double y = 2.0;
+ var initial =
+ new Pose3d(
+ new Translation3d(x, y, 0.0),
+ new Rotation3d(
+ Units.degreesToRadians(0.0),
+ Units.degreesToRadians(0.0),
+ Units.degreesToRadians(45.0)));
+
+ double yaw = Units.degreesToRadians(5.0);
+ var rotation = new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), yaw);
+ var rotated = initial.rotateBy(rotation);
+
+ // Translation is rotated by CCW rotation matrix
+ double c = Math.cos(yaw);
+ double s = Math.sin(yaw);
+ assertAll(
+ () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+ () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+ () -> assertEquals(0.0, rotated.getZ(), kEpsilon),
+ () -> assertEquals(0.0, rotated.getRotation().getX(), kEpsilon),
+ () -> assertEquals(0.0, rotated.getRotation().getY(), kEpsilon),
+ () ->
+ assertEquals(
+ initial.getRotation().getZ() + rotation.getZ(),
+ rotated.getRotation().getZ(),
+ kEpsilon));
+ }
+
+ @Test
void testTransformByRotations() {
var initialPose =
new Pose3d(
@@ -49,10 +83,13 @@
Units.degreesToRadians(-45.0),
Units.degreesToRadians(0.0)));
- // This sequence of rotations should diverge from the origin and eventually return to it. When
- // each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and
+ // This sequence of rotations should diverge from the origin and eventually
+ // return to it. When
+ // each rotation occurs, it should be performed intrinsicly, i.e. 'from the
+ // viewpoint' of and
// with
- // the axes of the pose that is being transformed, just like how the translation is done 'from
+ // the axes of the pose that is being transformed, just like how the translation
+ // is done 'from
// the
// viewpoint' of the pose that is being transformed.
var finalPose =
@@ -153,4 +190,115 @@
assertEquals(expected, pose.toPose2d());
}
+
+ @Test
+ void testComplexTwists() {
+ var initial_poses =
+ Arrays.asList(
+ new Pose3d(
+ new Translation3d(0.698303, -0.959096, 0.271076),
+ new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
+ new Pose3d(
+ new Translation3d(0.634892, -0.765209, 0.117543),
+ new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))),
+ new Pose3d(
+ new Translation3d(0.584827, -0.590303, -0.02557),
+ new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))),
+ new Pose3d(
+ new Translation3d(0.505038, -0.451479, -0.112835),
+ new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))),
+ new Pose3d(
+ new Translation3d(0.428178, -0.329692, -0.189707),
+ new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
+
+ var final_poses =
+ Arrays.asList(
+ new Pose3d(
+ new Translation3d(-0.230448, -0.511957, 0.198406),
+ new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
+ new Pose3d(
+ new Translation3d(-0.088932, -0.343253, 0.095018),
+ new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))),
+ new Pose3d(
+ new Translation3d(-0.107908, -0.317552, 0.133946),
+ new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))),
+ new Pose3d(
+ new Translation3d(-0.123383, -0.156411, -0.047435),
+ new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))),
+ new Pose3d(
+ new Translation3d(-0.084654, -0.019305, -0.030022),
+ new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873))));
+
+ final var eps = 1E-5;
+ for (int i = 0; i < initial_poses.size(); i++) {
+ var start = initial_poses.get(i);
+ var end = final_poses.get(i);
+
+ var twist = start.log(end);
+ var start_exp = start.exp(twist);
+
+ assertAll(
+ () -> assertEquals(start_exp.getX(), end.getX(), eps),
+ () -> assertEquals(start_exp.getY(), end.getY(), eps),
+ () -> assertEquals(start_exp.getZ(), end.getZ(), eps),
+ () ->
+ assertEquals(
+ start_exp.getRotation().getQuaternion().getW(),
+ end.getRotation().getQuaternion().getW(),
+ eps),
+ () ->
+ assertEquals(
+ start_exp.getRotation().getQuaternion().getX(),
+ end.getRotation().getQuaternion().getX(),
+ eps),
+ () ->
+ assertEquals(
+ start_exp.getRotation().getQuaternion().getY(),
+ end.getRotation().getQuaternion().getY(),
+ eps),
+ () ->
+ assertEquals(
+ start_exp.getRotation().getQuaternion().getZ(),
+ end.getRotation().getQuaternion().getZ(),
+ eps));
+ }
+ }
+
+ @Test
+ void testTwistNaN() {
+ var initial_poses =
+ Arrays.asList(
+ new Pose3d(
+ new Translation3d(6.32, 4.12, 0.00),
+ new Rotation3d(
+ new Quaternion(-0.9999999999999999, 0.0, 0.0, 1.9208309264993548E-8))),
+ new Pose3d(
+ new Translation3d(3.75, 2.95, 0.00),
+ new Rotation3d(
+ new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
+ var final_poses =
+ Arrays.asList(
+ new Pose3d(
+ new Translation3d(6.33, 4.15, 0.00),
+ new Rotation3d(
+ new Quaternion(-0.9999999999999999, 0.0, 0.0, 2.416890209039172E-8))),
+ new Pose3d(
+ new Translation3d(3.66, 2.93, 0.00),
+ new Rotation3d(
+ new Quaternion(0.9999999999999782, 0.0, 0.0, 2.0859477994905617E-7))));
+
+ for (int i = 0; i < initial_poses.size(); i++) {
+ var start = initial_poses.get(i);
+ var end = final_poses.get(i);
+
+ var twist = start.log(end);
+ assertAll(
+ () -> assertFalse(((Double) twist.dx).isNaN()),
+ () -> assertFalse(((Double) twist.dy).isNaN()),
+ () -> assertFalse(((Double) twist.dz).isNaN()),
+ () -> assertFalse(((Double) twist.rx).isNaN()),
+ () -> assertFalse(((Double) twist.ry).isNaN()),
+ () -> assertFalse(((Double) twist.rz).isNaN()));
+ }
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
index 7c7e103..458b14d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
@@ -4,7 +4,9 @@
package edu.wpi.first.math.geometry;
+import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
@@ -14,37 +16,91 @@
void testInit() {
// Identity
var q1 = new Quaternion();
- assertEquals(1.0, q1.getW());
- assertEquals(0.0, q1.getX());
- assertEquals(0.0, q1.getY());
- assertEquals(0.0, q1.getZ());
+ assertAll(
+ () -> assertEquals(1.0, q1.getW()),
+ () -> assertEquals(0.0, q1.getX()),
+ () -> assertEquals(0.0, q1.getY()),
+ () -> assertEquals(0.0, q1.getZ()));
// Normalized
var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
- assertEquals(0.5, q2.getW());
- assertEquals(0.5, q2.getX());
- assertEquals(0.5, q2.getY());
- assertEquals(0.5, q2.getZ());
+ assertAll(
+ () -> assertEquals(0.5, q2.getW()),
+ () -> assertEquals(0.5, q2.getX()),
+ () -> assertEquals(0.5, q2.getY()),
+ () -> assertEquals(0.5, q2.getZ()));
// Unnormalized
var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
- assertEquals(0.75, q3.getW());
- assertEquals(0.3, q3.getX());
- assertEquals(0.4, q3.getY());
- assertEquals(0.5, q3.getZ());
+ assertAll(
+ () -> assertEquals(0.75, q3.getW()),
+ () -> assertEquals(0.3, q3.getX()),
+ () -> assertEquals(0.4, q3.getY()),
+ () -> assertEquals(0.5, q3.getZ()));
- q3 = q3.normalize();
+ var q3_norm = q3.normalize();
double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
- assertEquals(0.75 / norm, q3.getW());
- assertEquals(0.3 / norm, q3.getX());
- assertEquals(0.4 / norm, q3.getY());
- assertEquals(0.5 / norm, q3.getZ());
- assertEquals(
- 1.0,
- q3.getW() * q3.getW()
- + q3.getX() * q3.getX()
- + q3.getY() * q3.getY()
- + q3.getZ() * q3.getZ());
+ assertAll(
+ () -> assertEquals(0.75 / norm, q3_norm.getW()),
+ () -> assertEquals(0.3 / norm, q3_norm.getX()),
+ () -> assertEquals(0.4 / norm, q3_norm.getY()),
+ () -> assertEquals(0.5 / norm, q3_norm.getZ()),
+ () -> assertEquals(1.0, q3_norm.dot(q3_norm)));
+ }
+
+ @Test
+ void testAddition() {
+ var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+ var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+ var sum = q.plus(p);
+ assertAll(
+ () -> assertEquals(q.getW() + p.getW(), sum.getW()),
+ () -> assertEquals(q.getX() + p.getX(), sum.getX()),
+ () -> assertEquals(q.getY() + p.getY(), sum.getY()),
+ () -> assertEquals(q.getZ() + p.getZ(), sum.getZ()));
+ }
+
+ @Test
+ void testSubtraction() {
+ var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+ var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+ var difference = q.minus(p);
+
+ assertAll(
+ () -> assertEquals(q.getW() - p.getW(), difference.getW()),
+ () -> assertEquals(q.getX() - p.getX(), difference.getX()),
+ () -> assertEquals(q.getY() - p.getY(), difference.getY()),
+ () -> assertEquals(q.getZ() - p.getZ(), difference.getZ()));
+ }
+
+ @Test
+ void testScalarMultiplication() {
+ var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+ var scalar = 2;
+
+ var product = q.times(scalar);
+
+ assertAll(
+ () -> assertEquals(q.getW() * scalar, product.getW()),
+ () -> assertEquals(q.getX() * scalar, product.getX()),
+ () -> assertEquals(q.getY() * scalar, product.getY()),
+ () -> assertEquals(q.getZ() * scalar, product.getZ()));
+ }
+
+ @Test
+ void testScalarDivision() {
+ var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+ var scalar = 2;
+
+ var product = q.divide(scalar);
+
+ assertAll(
+ () -> assertEquals(q.getW() / scalar, product.getW()),
+ () -> assertEquals(q.getX() / scalar, product.getX()),
+ () -> assertEquals(q.getY() / scalar, product.getY()),
+ () -> assertEquals(q.getZ() / scalar, product.getZ()));
}
@Test
@@ -59,31 +115,131 @@
// 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
// produce a 90° CCW Y rotation
var expected = yRot;
- var actual = zRot.times(yRot).times(xRot);
- assertEquals(expected.getW(), actual.getW(), 1e-9);
- assertEquals(expected.getX(), actual.getX(), 1e-9);
- assertEquals(expected.getY(), actual.getY(), 1e-9);
- assertEquals(expected.getZ(), actual.getZ(), 1e-9);
+ final var actual = zRot.times(yRot).times(xRot);
+ assertAll(
+ () -> assertEquals(expected.getW(), actual.getW(), 1e-9),
+ () -> assertEquals(expected.getX(), actual.getX(), 1e-9),
+ () -> assertEquals(expected.getY(), actual.getY(), 1e-9),
+ () -> assertEquals(expected.getZ(), actual.getZ(), 1e-9));
// Identity
var q =
new Quaternion(
0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
- actual = q.times(q.inverse());
- assertEquals(1.0, actual.getW());
- assertEquals(0.0, actual.getX());
- assertEquals(0.0, actual.getY());
- assertEquals(0.0, actual.getZ());
+ final var actual2 = q.times(q.inverse());
+ assertAll(
+ () -> assertEquals(1.0, actual2.getW()),
+ () -> assertEquals(0.0, actual2.getX()),
+ () -> assertEquals(0.0, actual2.getY()),
+ () -> assertEquals(0.0, actual2.getZ()));
+ }
+
+ @Test
+ void testConjugate() {
+ var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+ var inv = q.conjugate();
+
+ assertAll(
+ () -> assertEquals(q.getW(), inv.getW()),
+ () -> assertEquals(-q.getX(), inv.getX()),
+ () -> assertEquals(-q.getY(), inv.getY()),
+ () -> assertEquals(-q.getZ(), inv.getZ()));
}
@Test
void testInverse() {
var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
var inv = q.inverse();
+ var norm = q.norm();
- assertEquals(q.getW(), inv.getW());
- assertEquals(-q.getX(), inv.getX());
- assertEquals(-q.getY(), inv.getY());
- assertEquals(-q.getZ(), inv.getZ());
+ assertAll(
+ () -> assertEquals(q.getW() / (norm * norm), inv.getW(), 1e-10),
+ () -> assertEquals(-q.getX() / (norm * norm), inv.getX(), 1e-10),
+ () -> assertEquals(-q.getY() / (norm * norm), inv.getY(), 1e-10),
+ () -> assertEquals(-q.getZ() / (norm * norm), inv.getZ(), 1e-10));
+ }
+
+ @Test
+ void testNorm() {
+ var q = new Quaternion(3, 4, 12, 84);
+
+ // pythagorean triples (3, 4, 5), (5, 12, 13), (13, 84, 85)
+ assertEquals(q.norm(), 85, 1e-10);
+ }
+
+ @Test
+ void testExponential() {
+ var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+ var q_exp =
+ new Quaternion(
+ 2.81211398529184, -0.392521193481878, -0.588781790222817, -0.785042386963756);
+
+ assertEquals(q_exp, q.exp());
+ }
+
+ @Test
+ void testLogarithm() {
+ var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+ var q_log =
+ new Quaternion(1.7959088706354, 0.515190292664085, 0.772785438996128, 1.03038058532817);
+
+ assertEquals(q_log, q.log());
+
+ var zero = new Quaternion(0, 0, 0, 0);
+ var one = new Quaternion();
+
+ assertEquals(zero, one.log());
+
+ var i = new Quaternion(0, 1, 0, 0);
+ assertEquals(i.times(Math.PI / 2), i.log());
+
+ var j = new Quaternion(0, 0, 1, 0);
+ assertEquals(j.times(Math.PI / 2), j.log());
+
+ var k = new Quaternion(0, 0, 0, 1);
+ assertEquals(k.times(Math.PI / 2), k.log());
+ assertEquals(i.times(-Math.PI), one.times(-1).log());
+
+ var ln_half = Math.log(0.5);
+ assertEquals(new Quaternion(ln_half, -Math.PI, 0, 0), one.times(-0.5).log());
+ }
+
+ @Test
+ void testLogarithmIsInverseOfExponential() {
+ var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+
+ // These operations are order-dependent: ln(exp(q)) is congruent
+ // but not necessarily equal to exp(ln(q)) due to the multi-valued nature of the complex
+ // logarithm.
+
+ var q_log_exp = q.log().exp();
+
+ assertEquals(q, q_log_exp);
+
+ var start = new Quaternion(1, 2, 3, 4);
+ var expect = new Quaternion(5, 6, 7, 8);
+
+ var twist = start.log(expect);
+ var actual = start.exp(twist);
+
+ assertEquals(expect, actual);
+ }
+
+ @Test
+ void testDotProduct() {
+ var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+ var p = new Quaternion(5.5, 6.6, 7.7, 8.8);
+
+ assertEquals(
+ q.getW() * p.getW() + q.getX() * p.getX() + q.getY() * p.getY() + q.getZ() * p.getZ(),
+ q.dot(p));
+ }
+
+ @Test
+ void testDotProductAsEquality() {
+ var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+ var q_conj = q.conjugate();
+
+ assertAll(() -> assertEquals(q, q), () -> assertNotEquals(q, q_conj));
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
index 072a2e6..d80344d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
@@ -20,21 +20,60 @@
private static final double kEpsilon = 1E-9;
@Test
+ void testGimbalLockAccuracy() {
+ var rot1 = new Rotation3d(0, 0, Math.PI / 2);
+ var rot2 = new Rotation3d(Math.PI, 0, 0);
+ var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+ final var result1 = rot1.plus(rot2).plus(rot3);
+ final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
+ assertAll(
+ () -> assertEquals(expected1, result1),
+ () -> assertEquals(Math.PI / 2, result1.getX() + result1.getZ(), kEpsilon),
+ () -> assertEquals(-Math.PI / 2, result1.getY(), kEpsilon));
+
+ rot1 = new Rotation3d(0, 0, Math.PI / 2);
+ rot2 = new Rotation3d(-Math.PI, 0, 0);
+ rot3 = new Rotation3d(Math.PI / 2, 0, 0);
+ final var result2 = rot1.plus(rot2).plus(rot3);
+ final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
+ assertAll(
+ () -> assertEquals(expected2, result2),
+ () -> assertEquals(Math.PI / 2, result2.getZ() - result2.getX(), kEpsilon),
+ () -> assertEquals(Math.PI / 2, result2.getY(), kEpsilon));
+
+ rot1 = new Rotation3d(0, 0, Math.PI / 2);
+ rot2 = new Rotation3d(0, Math.PI / 3, 0);
+ rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+ final var result3 = rot1.plus(rot2).plus(rot3);
+ final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
+ assertAll(
+ () -> assertEquals(expected3, result3),
+ () -> assertEquals(Math.PI / 6, result3.getZ() - result3.getX(), kEpsilon),
+ () -> assertEquals(Math.PI / 2, result3.getY(), kEpsilon));
+ }
+
+ @Test
void testInitAxisAngleAndRollPitchYaw() {
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
+ final var rvec1 = new Rotation3d(xAxis.times(Math.PI / 3));
assertEquals(rot1, rot2);
+ assertEquals(rot1, rvec1);
final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
+ final var rvec2 = new Rotation3d(yAxis.times(Math.PI / 3));
assertEquals(rot3, rot4);
+ assertEquals(rot3, rvec2);
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
+ final var rvec3 = new Rotation3d(zAxis.times(Math.PI / 3));
assertEquals(rot5, rot6);
+ assertEquals(rot5, rvec3);
}
@Test
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index c8f5690..6ed8a61 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
@@ -8,6 +8,7 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import java.util.List;
import org.junit.jupiter.api.Test;
class Translation2dTest {
@@ -114,4 +115,20 @@
() -> assertEquals(1.0, two.getX(), kEpsilon),
() -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
}
+
+ @Test
+ void testNearest() {
+ var origin = new Translation2d();
+
+ // each translationX is X units away from the origin at a random angle.
+ var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45));
+ var translation2 = new Translation2d(2, Rotation2d.fromDegrees(90));
+ var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135));
+ var translation4 = new Translation2d(4, Rotation2d.fromDegrees(180));
+ var translation5 = new Translation2d(5, Rotation2d.fromDegrees(270));
+
+ assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3);
+ assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
+ assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
new file mode 100644
index 0000000..fb74b92
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
@@ -0,0 +1,76 @@
+// 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.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingDoubleTreeMapTest {
+ @Test
+ void testInterpolationDouble() {
+ InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+ table.put(125.0, 450.0);
+ table.put(200.0, 510.0);
+ table.put(268.0, 525.0);
+ table.put(312.0, 550.0);
+ table.put(326.0, 650.0);
+
+ // Key below minimum gives the smallest value
+ assertEquals(450.0, table.get(100.0));
+
+ // Minimum key gives exact value
+ assertEquals(450.0, table.get(125.0));
+
+ // Key gives interpolated value
+ assertEquals(480.0, table.get(162.5));
+
+ // Key at right of interpolation range gives exact value
+ assertEquals(510.0, table.get(200.0));
+
+ // Maximum key gives exact value
+ assertEquals(650.0, table.get(326.0));
+
+ // Key above maximum gives largest value
+ assertEquals(650.0, table.get(400.0));
+ }
+
+ @Test
+ void testInterpolationClear() {
+ InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+ table.put(125.0, 450.0);
+ table.put(200.0, 510.0);
+ table.put(268.0, 525.0);
+ table.put(312.0, 550.0);
+ table.put(326.0, 650.0);
+
+ // Key below minimum gives the smallest value
+ assertEquals(450.0, table.get(100.0));
+
+ // Minimum key gives exact value
+ assertEquals(450.0, table.get(125.0));
+
+ // Key gives interpolated value
+ assertEquals(480.0, table.get(162.5));
+
+ // Key at right of interpolation range gives exact value
+ assertEquals(510.0, table.get(200.0));
+
+ // Maximum key gives exact value
+ assertEquals(650.0, table.get(326.0));
+
+ // Key above maximum gives largest value
+ assertEquals(650.0, table.get(400.0));
+
+ table.clear();
+
+ table.put(100.0, 250.0);
+ table.put(200.0, 500.0);
+
+ assertEquals(375.0, table.get(150.0));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
new file mode 100644
index 0000000..b04b40a
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingTreeMapTest {
+ @Test
+ void testInterpolation() {
+ InterpolatingTreeMap<Double, Double> table =
+ new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolator.forDouble());
+
+ table.put(125.0, 450.0);
+ table.put(200.0, 510.0);
+ table.put(268.0, 525.0);
+ table.put(312.0, 550.0);
+ table.put(326.0, 650.0);
+
+ // Key below minimum gives the smallest value
+ assertEquals(450.0, table.get(100.0));
+
+ // Minimum key gives exact value
+ assertEquals(450.0, table.get(125.0));
+
+ // Key gives interpolated value
+ assertEquals(480.0, table.get(162.5));
+
+ // Key at right of interpolation range gives exact value
+ assertEquals(510.0, table.get(200.0));
+
+ // Maximum key gives exact value
+ assertEquals(650.0, table.get(326.0));
+
+ // Key above maximum gives largest value
+ assertEquals(650.0, table.get(400.0));
+
+ table.clear();
+
+ table.put(100.0, 250.0);
+ table.put(200.0, 500.0);
+
+ assertEquals(375.0, table.get(150.0));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
index b9c3785..2401342 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
@@ -7,14 +7,45 @@
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
import org.junit.jupiter.api.Test;
class ChassisSpeedsTest {
private static final double kEpsilon = 1E-9;
@Test
- void testFieldRelativeConstruction() {
+ void testDiscretize() {
+ final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
+ final var duration = 1.0;
+ final var dt = 0.01;
+
+ final var speeds = ChassisSpeeds.discretize(target, duration);
+ final var twist =
+ new Twist2d(
+ speeds.vxMetersPerSecond * dt,
+ speeds.vyMetersPerSecond * dt,
+ speeds.omegaRadiansPerSecond * dt);
+
+ var pose = new Pose2d();
+ for (double time = 0; time < duration; time += dt) {
+ pose = pose.exp(twist);
+ }
+
+ final var result = pose; // For lambda capture
+ assertAll(
+ () -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon),
+ () -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon),
+ () ->
+ assertEquals(
+ target.omegaRadiansPerSecond * duration,
+ result.getRotation().getRadians(),
+ kEpsilon));
+ }
+
+ @Test
+ void testFromFieldRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
@@ -23,4 +54,71 @@
() -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
+
+ @Test
+ void testFromRobotRelativeSpeeds() {
+ final var chassisSpeeds =
+ ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
+
+ assertAll(
+ () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+ }
+
+ @Test
+ void testPlus() {
+ final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+ final var right = new ChassisSpeeds(2.0, 1.5, 0.25);
+
+ final var chassisSpeeds = left.plus(right);
+
+ assertAll(
+ () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond),
+ () -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond),
+ () -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond));
+ }
+
+ @Test
+ void testMinus() {
+ final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+ final var right = new ChassisSpeeds(2.0, 0.5, 0.25);
+
+ final var chassisSpeeds = left.minus(right);
+
+ assertAll(
+ () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+ () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond),
+ () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond));
+ }
+
+ @Test
+ void testUnaryMinus() {
+ final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
+
+ assertAll(
+ () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+ () -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond),
+ () -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond));
+ }
+
+ @Test
+ void testMultiplication() {
+ final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
+
+ assertAll(
+ () -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond),
+ () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond),
+ () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond));
+ }
+
+ @Test
+ void testDivision() {
+ final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
+
+ assertAll(
+ () -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond),
+ () -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond),
+ () -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond));
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java
new file mode 100644
index 0000000..7f26180
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java
@@ -0,0 +1,63 @@
+// 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.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsTest {
+ @Test
+ void testPlus() {
+ final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
+ final var right = new DifferentialDriveWheelSpeeds(2.0, 1.5);
+
+ final var wheelSpeeds = left.plus(right);
+
+ assertAll(
+ () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond),
+ () -> assertEquals(2.0, wheelSpeeds.rightMetersPerSecond));
+ }
+
+ @Test
+ void testMinus() {
+ final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
+ final var right = new DifferentialDriveWheelSpeeds(2.0, 0.5);
+
+ final var wheelSpeeds = left.minus(right);
+
+ assertAll(
+ () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
+ () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond));
+ }
+
+ @Test
+ void testUnaryMinus() {
+ final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).unaryMinus();
+
+ assertAll(
+ () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
+ () -> assertEquals(-0.5, wheelSpeeds.rightMetersPerSecond));
+ }
+
+ @Test
+ void testMultiplication() {
+ final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).times(2.0);
+
+ assertAll(
+ () -> assertEquals(2.0, wheelSpeeds.leftMetersPerSecond),
+ () -> assertEquals(1.0, wheelSpeeds.rightMetersPerSecond));
+ }
+
+ @Test
+ void testDivision() {
+ final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).div(2.0);
+
+ assertAll(
+ () -> assertEquals(0.5, wheelSpeeds.leftMetersPerSecond),
+ () -> assertEquals(0.25, wheelSpeeds.rightMetersPerSecond));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java
new file mode 100644
index 0000000..22efb44
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java
@@ -0,0 +1,73 @@
+// 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.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsTest {
+ @Test
+ void testPlus() {
+ final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
+ final var right = new MecanumDriveWheelSpeeds(2.0, 1.5, 0.5, 1.0);
+
+ final var wheelSpeeds = left.plus(right);
+
+ assertAll(
+ () -> assertEquals(3.0, wheelSpeeds.frontLeftMetersPerSecond),
+ () -> assertEquals(2.0, wheelSpeeds.frontRightMetersPerSecond),
+ () -> assertEquals(2.5, wheelSpeeds.rearLeftMetersPerSecond),
+ () -> assertEquals(2.5, wheelSpeeds.rearRightMetersPerSecond));
+ }
+
+ @Test
+ void testMinus() {
+ final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
+ final var right = new MecanumDriveWheelSpeeds(2.0, 0.5, 0.5, 1.0);
+
+ final var wheelSpeeds = left.minus(right);
+
+ assertAll(
+ () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
+ () -> assertEquals(0.0, wheelSpeeds.frontRightMetersPerSecond),
+ () -> assertEquals(1.5, wheelSpeeds.rearLeftMetersPerSecond),
+ () -> assertEquals(0.5, wheelSpeeds.rearRightMetersPerSecond));
+ }
+
+ @Test
+ void testUnaryMinus() {
+ final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).unaryMinus();
+
+ assertAll(
+ () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
+ () -> assertEquals(-0.5, wheelSpeeds.frontRightMetersPerSecond),
+ () -> assertEquals(-2.0, wheelSpeeds.rearLeftMetersPerSecond),
+ () -> assertEquals(-1.5, wheelSpeeds.rearRightMetersPerSecond));
+ }
+
+ @Test
+ void testMultiplication() {
+ final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).times(2.0);
+
+ assertAll(
+ () -> assertEquals(2.0, wheelSpeeds.frontLeftMetersPerSecond),
+ () -> assertEquals(1.0, wheelSpeeds.frontRightMetersPerSecond),
+ () -> assertEquals(4.0, wheelSpeeds.rearLeftMetersPerSecond),
+ () -> assertEquals(3.0, wheelSpeeds.rearRightMetersPerSecond));
+ }
+
+ @Test
+ void testDivision() {
+ final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).div(2.0);
+
+ assertAll(
+ () -> assertEquals(0.5, wheelSpeeds.frontLeftMetersPerSecond),
+ () -> assertEquals(0.25, wheelSpeeds.frontRightMetersPerSecond),
+ () -> assertEquals(1.0, wheelSpeeds.rearLeftMetersPerSecond),
+ () -> assertEquals(0.75, wheelSpeeds.rearRightMetersPerSecond));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
index 43dd02a..f429a68 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -120,6 +120,28 @@
}
@Test
+ void testResetWheelAngle() {
+ Rotation2d fl = new Rotation2d(0);
+ Rotation2d fr = new Rotation2d(Math.PI / 2);
+ Rotation2d bl = new Rotation2d(Math.PI);
+ Rotation2d br = new Rotation2d(3 * Math.PI / 2);
+ m_kinematics.resetHeadings(fl, fr, bl, br);
+ var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
+
+ // Robot is stationary, but module angles are preserved.
+
+ assertAll(
+ () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(270.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+ }
+
+ @Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
@@ -371,4 +393,21 @@
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
}
+
+ @Test
+ void testDesaturateNegativeSpeed() {
+ SwerveModuleState fl = new SwerveModuleState(1, new Rotation2d());
+ SwerveModuleState fr = new SwerveModuleState(1, new Rotation2d());
+ SwerveModuleState bl = new SwerveModuleState(-2, new Rotation2d());
+ SwerveModuleState br = new SwerveModuleState(-2, new Rotation2d());
+
+ SwerveModuleState[] arr = {fl, fr, bl, br};
+ SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
+
+ assertAll(
+ () -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon));
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
index 2182674..7f62933 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -122,98 +122,6 @@
+ discQIntegrated);
}
- // Test that the Taylor series discretization produces nearly identical results.
- @Test
- void testDiscretizeSlowModelAQTaylor() {
- final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
- final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
-
- final var dt = 1.0;
-
- // Continuous Q should be positive semidefinite
- final var esCont = contQ.getStorage().eig();
- for (int i = 0; i < contQ.getNumRows(); ++i) {
- assertTrue(esCont.getEigenvalue(i).real >= 0);
- }
-
- // T
- // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
- final var discQIntegrated =
- RungeKuttaTimeVarying.rungeKuttaTimeVarying(
- (Double t, Matrix<N2, N2> x) ->
- contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
- 0.0,
- new Matrix<>(Nat.N2(), Nat.N2()),
- dt);
-
- var discA = Discretization.discretizeA(contA, dt);
-
- var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
- var discATaylor = discAQPair.getFirst();
- var discQTaylor = discAQPair.getSecond();
-
- assertTrue(
- discQIntegrated.minus(discQTaylor).normF() < 1e-10,
- "Expected these to be nearly equal:\ndiscQTaylor:\n"
- + discQTaylor
- + "\ndiscQIntegrated:\n"
- + discQIntegrated);
- assertTrue(discA.minus(discATaylor).normF() < 1e-10);
-
- // Discrete Q should be positive semidefinite
- final var esDisc = discQTaylor.getStorage().eig();
- for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
- assertTrue(esDisc.getEigenvalue(i).real >= 0);
- }
- }
-
- // Test that the Taylor series discretization produces nearly identical results.
- @Test
- void testDiscretizeFastModelAQTaylor() {
- final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
- final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
-
- final var dt = 0.005;
-
- // Continuous Q should be positive semidefinite
- final var esCont = contQ.getStorage().eig();
- for (int i = 0; i < contQ.getNumRows(); ++i) {
- assertTrue(esCont.getEigenvalue(i).real >= 0);
- }
-
- // T
- // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
- final var discQIntegrated =
- RungeKuttaTimeVarying.rungeKuttaTimeVarying(
- (Double t, Matrix<N2, N2> x) ->
- contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
- 0.0,
- new Matrix<>(Nat.N2(), Nat.N2()),
- dt);
-
- var discA = Discretization.discretizeA(contA, dt);
-
- var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
- var discATaylor = discAQPair.getFirst();
- var discQTaylor = discAQPair.getSecond();
-
- assertTrue(
- discQIntegrated.minus(discQTaylor).normF() < 1e-3,
- "Expected these to be nearly equal:\ndiscQTaylor:\n"
- + discQTaylor
- + "\ndiscQIntegrated:\n"
- + discQIntegrated);
- assertTrue(discA.minus(discATaylor).normF() < 1e-10);
-
- // Discrete Q should be positive semidefinite
- final var esDisc = discQTaylor.getStorage().eig();
- for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
- assertTrue(esDisc.getEigenvalue(i).real >= 0);
- }
- }
-
// Test that DiscretizeR() works
@Test
void testDiscretizeR() {
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
index 72fb5ea..9b47148 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
@@ -31,6 +31,20 @@
}
@Test
+ void testZeroRKDP() {
+ var y1 =
+ NumericalIntegration.rkdp(
+ (x, u) -> {
+ return VecBuilder.fill(0);
+ },
+ VecBuilder.fill(0),
+ VecBuilder.fill(0),
+ 0.1);
+
+ assertEquals(0.0, y1.get(0, 0), 1e-3);
+ }
+
+ @Test
void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
new file mode 100644
index 0000000..e9c535e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
@@ -0,0 +1,364 @@
+// 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.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class ExponentialProfileTest {
+ private static final double kDt = 0.01;
+ private static final SimpleMotorFeedforward feedforward =
+ new SimpleMotorFeedforward(0, 2.5629, 0.43277);
+ private static final ExponentialProfile.Constraints constraints =
+ ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
+
+ /**
+ * Asserts "val1" is within "eps" of "val2".
+ *
+ * @param val1 First operand in comparison.
+ * @param val2 Second operand in comparison.
+ * @param eps Tolerance for whether values are near to each other.
+ */
+ private static void assertNear(double val1, double val2, double eps) {
+ assertTrue(
+ Math.abs(val1 - val2) <= eps,
+ "Difference between " + val1 + " and " + val2 + " is greater than " + eps);
+ }
+
+ private static void assertNear(
+ ExponentialProfile.State val1, ExponentialProfile.State val2, double eps) {
+ assertAll(
+ () -> assertNear(val1.position, val2.position, eps),
+ () -> assertNear(val1.position, val2.position, eps));
+ }
+
+ private static ExponentialProfile.State checkDynamics(
+ ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
+ var next = profile.calculate(kDt, current, goal);
+
+ var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
+
+ assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
+
+ return next;
+ }
+
+ @Test
+ void reachesGoal() {
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+
+ ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ for (int i = 0; i < 450; ++i) {
+ state = checkDynamics(profile, state, goal);
+ }
+ assertEquals(state, goal);
+ }
+
+ // Tests that decreasing the maximum velocity in the middle when it is already
+ // moving faster than the new max is handled correctly
+ @Test
+ void posContinuousUnderVelChange() {
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+
+ ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ for (int i = 0; i < 300; ++i) {
+ if (i == 150) {
+ profile =
+ new ExponentialProfile(
+ ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
+ }
+
+ state = checkDynamics(profile, state, goal);
+ }
+ assertEquals(state, goal);
+ }
+
+ // Tests that decreasing the maximum velocity in the middle when it is already
+ // moving faster than the new max is handled correctly
+ @Test
+ void posContinuousUnderVelChangeBackward() {
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+
+ ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ for (int i = 0; i < 300; ++i) {
+ if (i == 150) {
+ profile =
+ new ExponentialProfile(
+ ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
+ }
+
+ state = checkDynamics(profile, state, goal);
+ }
+ assertEquals(state, goal);
+ }
+
+ // There is some somewhat tricky code for dealing with going backwards
+ @Test
+ void backwards() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+
+ for (int i = 0; i < 400; ++i) {
+ state = checkDynamics(profile, state, goal);
+ }
+ assertEquals(state, goal);
+ }
+
+ @Test
+ void switchGoalInMiddle() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+ for (int i = 0; i < 50; ++i) {
+ state = checkDynamics(profile, state, goal);
+ }
+ assertNotEquals(state, goal);
+
+ goal = new ExponentialProfile.State(0.0, 0.0);
+ for (int i = 0; i < 100; ++i) {
+ state = checkDynamics(profile, state, goal);
+ }
+ assertEquals(state, goal);
+ }
+
+ // Checks to make sure that it hits top speed
+ @Test
+ void topSpeed() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+ double maxSpeed = 0;
+ for (int i = 0; i < 900; ++i) {
+ state = checkDynamics(profile, state, goal);
+ maxSpeed = Math.max(maxSpeed, state.velocity);
+ }
+
+ assertNear(constraints.maxVelocity(), maxSpeed, 10e-5);
+ assertEquals(state, goal);
+ }
+
+ @Test
+ void topSpeedBackward() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+ double maxSpeed = 0;
+ for (int i = 0; i < 900; ++i) {
+ state = checkDynamics(profile, state, goal);
+ maxSpeed = Math.min(maxSpeed, state.velocity);
+ }
+
+ assertNear(-constraints.maxVelocity(), maxSpeed, 10e-5);
+ assertEquals(state, goal);
+ }
+
+ @Test
+ void largeInitialVelocity() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 8);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+ for (int i = 0; i < 900; ++i) {
+ state = checkDynamics(profile, state, goal);
+ }
+
+ assertEquals(state, goal);
+ }
+
+ @Test
+ void largeNegativeInitialVelocity() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, -8);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+ for (int i = 0; i < 900; ++i) {
+ state = checkDynamics(profile, state, goal);
+ }
+
+ assertEquals(state, goal);
+ }
+
+ @SuppressWarnings("PMD.TestClassWithoutTestCases")
+ static class TestCase {
+ public final ExponentialProfile.State initial;
+ public final ExponentialProfile.State goal;
+ public final ExponentialProfile.State inflectionPoint;
+
+ TestCase(
+ ExponentialProfile.State initial,
+ ExponentialProfile.State goal,
+ ExponentialProfile.State inflectionPoint) {
+ this.initial = initial;
+ this.goal = goal;
+ this.inflectionPoint = inflectionPoint;
+ }
+ }
+
+ @Test
+ void testHeuristic() {
+ List<TestCase> testCases =
+ List.of(
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(0.75, -4),
+ new ExponentialProfile.State(1.3758, 4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(1.4103, 4),
+ new ExponentialProfile.State(1.3758, 4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(0.75, -4),
+ new ExponentialProfile.State(1.3758, 4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(1.4103, 4),
+ new ExponentialProfile.State(1.3758, 4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(0.5, -2),
+ new ExponentialProfile.State(0.4367, 3.7217)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(0.546, 2),
+ new ExponentialProfile.State(0.4367, 3.7217)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(0.5, -2),
+ new ExponentialProfile.State(0.5560, -2.9616)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(0.546, 2),
+ new ExponentialProfile.State(0.5560, -2.9616)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(-0.75, -4),
+ new ExponentialProfile.State(-0.7156, -4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(-0.0897, 4),
+ new ExponentialProfile.State(-0.7156, -4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(-0.75, -4),
+ new ExponentialProfile.State(-0.7156, -4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(-0.0897, 4),
+ new ExponentialProfile.State(-0.7156, -4.4304)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(-0.5, -4.5),
+ new ExponentialProfile.State(1.095, 4.314)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -4),
+ new ExponentialProfile.State(1.0795, 4.5),
+ new ExponentialProfile.State(-0.5122, -4.351)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(-0.5, -4.5),
+ new ExponentialProfile.State(1.095, 4.314)),
+ new TestCase(
+ new ExponentialProfile.State(0.6603, 4),
+ new ExponentialProfile.State(1.0795, 4.5),
+ new ExponentialProfile.State(-0.5122, -4.351)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -8),
+ new ExponentialProfile.State(0, 0),
+ new ExponentialProfile.State(-0.1384, 3.342)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, -8),
+ new ExponentialProfile.State(-1, 0),
+ new ExponentialProfile.State(-0.562, -6.792)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, 8),
+ new ExponentialProfile.State(1, 0),
+ new ExponentialProfile.State(0.562, 6.792)),
+ new TestCase(
+ new ExponentialProfile.State(0.0, 8),
+ new ExponentialProfile.State(-1, 0),
+ new ExponentialProfile.State(-0.785, -4.346)));
+
+ var profile = new ExponentialProfile(constraints);
+
+ for (var testCase : testCases) {
+ var state = profile.calculateInflectionPoint(testCase.initial, testCase.goal);
+ assertNear(testCase.inflectionPoint, state, 1e-3);
+ }
+ }
+
+ @Test
+ void timingToCurrent() {
+ ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+ for (int i = 0; i < 400; i++) {
+ state = checkDynamics(profile, state, goal);
+ assertNear(profile.timeLeftUntil(state, state), 0, 2e-2);
+ }
+ }
+
+ @Test
+ void timingToGoal() {
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+
+ ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ double predictedTimeLeft = profile.timeLeftUntil(state, goal);
+ boolean reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ state = checkDynamics(profile, state, goal);
+
+ if (!reachedGoal && state.equals(goal)) {
+ // Expected value using for loop index is just an approximation since
+ // the time left in the profile doesn't increase linearly at the
+ // endpoints
+ assertNear(predictedTimeLeft, i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+ }
+
+ @Test
+ void timingToNegativeGoal() {
+ ExponentialProfile profile = new ExponentialProfile(constraints);
+
+ ExponentialProfile.State goal = new ExponentialProfile.State(-2, 0);
+ ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+ double predictedTimeLeft = profile.timeLeftUntil(state, goal);
+ boolean reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ state = checkDynamics(profile, state, goal);
+
+ if (!reachedGoal && state.equals(goal)) {
+ // Expected value using for loop index is just an approximation since
+ // the time left in the profile doesn't increase linearly at the
+ // endpoints
+ assertNear(predictedTimeLeft, i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
index ee6cc8f..fb28b69 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -57,9 +57,9 @@
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 450; ++i) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -67,21 +67,21 @@
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
@Test
- void posContinousUnderVelChange() {
+ void posContinuousUnderVelChange() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
- TrapezoidProfile.State state = profile.calculate(kDt);
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
+ TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ profile = new TrapezoidProfile(constraints);
}
- profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
double estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
@@ -105,9 +105,9 @@
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 400; ++i) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -118,16 +118,16 @@
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
}
assertNotEquals(state, goal);
goal = new TrapezoidProfile.State(0.0, 0.0);
+ profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 550; ++i) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -139,15 +139,15 @@
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
}
assertNear(constraints.maxVelocity, state.velocity, 10e-5);
+ profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 2000; ++i) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -158,9 +158,9 @@
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 400; i++) {
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
}
}
@@ -170,14 +170,13 @@
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
- TrapezoidProfile.State state = profile.calculate(kDt);
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
+ TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
@@ -193,14 +192,13 @@
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
- TrapezoidProfile.State state = profile.calculate(kDt);
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
+ TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
@@ -213,14 +211,13 @@
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
- TrapezoidProfile.State state = profile.calculate(kDt);
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
+ TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
@@ -236,14 +233,13 @@
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
- TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
- TrapezoidProfile.State state = profile.calculate(kDt);
+ TrapezoidProfile profile = new TrapezoidProfile(constraints);
+ TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(-1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = new TrapezoidProfile(constraints, goal, state);
- state = profile.calculate(kDt);
+ state = profile.calculate(kDt, goal, state);
if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
index e397af1..bbada9e 100644
--- a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
@@ -2,8 +2,9 @@
// 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 <gtest/gtest.h>
+
#include "frc/ComputerVisionUtil.h"
-#include "gtest/gtest.h"
TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};
diff --git a/wpimath/src/test/native/cpp/DARETest.cpp b/wpimath/src/test/native/cpp/DARETest.cpp
new file mode 100644
index 0000000..e19fa3c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest.cpp
@@ -0,0 +1,299 @@
+// 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 <stdexcept>
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+#include "DARETestUtil.h"
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/fmt/Eigen.h"
+
+// 2x1
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+ const Eigen::Matrix<double, 2, 1>& N);
+
+// 4x1
+extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+ const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+ const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
+extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+ const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+ const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
+ const Eigen::Matrix<double, 4, 1>& N);
+
+// 2x2
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+ const Eigen::Matrix<double, 2, 2>& N);
+
+// 2x3
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
+ const Eigen::Matrix<double, 2, 3>& N);
+
+TEST(DARETest, NonInvertibleA_ABQR) {
+ // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+ // Riccati Equation"
+
+ frc::Matrixd<4, 4> A{
+ {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+ frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
+ frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+ frc::Matrixd<1, 1> R{0.25};
+
+ frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, NonInvertibleA_ABQRN) {
+ // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+ // Riccati Equation"
+
+ frc::Matrixd<4, 4> A{
+ {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+ frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
+ frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+ frc::Matrixd<1, 1> R{0.25};
+
+ frc::Matrixd<4, 4> Aref{
+ {0.25, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+ Q = (A - Aref).transpose() * Q * (A - Aref);
+ R = B.transpose() * Q * B + R;
+ frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
+
+ frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R, N);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, InvertibleA_ABQR) {
+ frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
+ frc::Matrixd<2, 1> B{{0}, {1}};
+ frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
+ frc::Matrixd<1, 1> R{{0.3}};
+
+ frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, InvertibleA_ABQRN) {
+ frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
+ frc::Matrixd<2, 1> B{{0}, {1}};
+ frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
+ frc::Matrixd<1, 1> R{0.3};
+
+ frc::Matrixd<2, 2> Aref{{0.5, 1}, {0, 1}};
+ Q = (A - Aref).transpose() * Q * (A - Aref);
+ R = B.transpose() * Q * B + R;
+ frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
+
+ frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
+ // The first generalized eigenvalue of (S, T) is stable
+
+ frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
+ frc::Matrixd<2, 1> B{{0}, {1}};
+ frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
+ frc::Matrixd<1, 1> R{1};
+
+ frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
+ // The first generalized eigenvalue of (S, T) is stable
+
+ frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
+ frc::Matrixd<2, 1> B{{0}, {1}};
+ frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
+ frc::Matrixd<1, 1> R{1};
+
+ frc::Matrixd<2, 2> Aref{{0, 0.5}, {0, 0}};
+ Q = (A - Aref).transpose() * Q * (A - Aref);
+ R = B.transpose() * Q * B + R;
+ frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
+
+ frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, IdentitySystem_ABQR) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+ Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, IdentitySystem_ABQRN) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+ Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R, N);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, MoreInputsThanStates_ABQR) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
+
+ Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, MoreInputsThanStates_ABQRN) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
+ const frc::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
+
+ Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R, N);
+ ExpectMatrixEqual(X, X.transpose(), 1e-10);
+ ExpectPositiveSemidefinite(X);
+ ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{-Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
+
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+
+ const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument);
+
+ const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument);
+}
+
+TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+ const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument);
+
+ const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument);
+}
+
+TEST(DARETest, ABNotStabilizable_ABQR) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, ABNotStabilizable_ABQRN) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, ACNotDetectable_ABQR) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, ACNotDetectable_ABQRN) {
+ const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d N{Eigen::Matrix2d::Zero()};
+
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, QDecomposition) {
+ // Ensures the decomposition of Q into CᵀC is correct
+
+ const Eigen::Matrix2d A{{1.0, 0.0}, {0.0, 0.0}};
+ const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+ const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+ // (A, C₁) should be detectable pair
+ const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
+ const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
+ EXPECT_NO_THROW((frc::DARE<2, 2>(A, B, Q_1, R)));
+
+ // (A, C₂) shouldn't be detectable pair
+ const Eigen::Matrix2d C_2 = C_1.transpose();
+ const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
+ EXPECT_THROW((frc::DARE<2, 2>(A, B, Q_2, R)), std::invalid_argument);
+}
diff --git a/wpimath/src/test/native/cpp/DARETestUtil.cpp b/wpimath/src/test/native/cpp/DARETestUtil.cpp
new file mode 100644
index 0000000..452e6ce
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETestUtil.cpp
@@ -0,0 +1,72 @@
+// 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 "DARETestUtil.h"
+
+#include <Eigen/Eigenvalues>
+#include <fmt/core.h>
+#include <gtest/gtest.h>
+
+#include "frc/fmt/Eigen.h"
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+ double tolerance) {
+ for (int row = 0; row < lhs.rows(); ++row) {
+ for (int col = 0; col < lhs.cols(); ++col) {
+ EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance)
+ << fmt::format("row = {}, col = {}", row, col);
+ }
+ }
+
+ if (::testing::Test::HasFailure()) {
+ fmt::print("lhs =\n{}\n", lhs);
+ fmt::print("rhs =\n{}\n", rhs);
+ fmt::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
+ }
+}
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
+ Eigen::EigenvaluesOnly};
+ for (int i = 0; i < X.rows(); ++i) {
+ EXPECT_GE(eigX.eigenvalues()[i], 0.0);
+ }
+}
+
+void ExpectDARESolution(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>& X) {
+ // Check that X is the solution to the DARE
+ // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ // clang-format off
+ Eigen::MatrixXd Y =
+ A.transpose() * X * A
+ - X
+ - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
+ * B.transpose() * X * A)
+ + Q;
+ // clang-format on
+ ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}
+
+void ExpectDARESolution(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,
+ const Eigen::Ref<const Eigen::MatrixXd>& X) {
+ // Check that X is the solution to the DARE
+ // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ // clang-format off
+ Eigen::MatrixXd Y =
+ A.transpose() * X * A
+ - X
+ - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
+ * (B.transpose() * X * A + N.transpose()))
+ + Q;
+ // clang-format on
+ ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}
diff --git a/wpimath/src/test/native/cpp/DARETestUtil.h b/wpimath/src/test/native/cpp/DARETestUtil.h
new file mode 100644
index 0000000..f0d2f66
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETestUtil.h
@@ -0,0 +1,25 @@
+// 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>
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+ double tolerance);
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X);
+
+void ExpectDARESolution(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>& X);
+
+void ExpectDARESolution(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,
+ const Eigen::Ref<const Eigen::MatrixXd>& X);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
new file mode 100644
index 0000000..3ba3d71
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
@@ -0,0 +1,13 @@
+// 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/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+ const Eigen::Matrix<double, 2, 1>& N);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
new file mode 100644
index 0000000..75b17b8
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
@@ -0,0 +1,13 @@
+// 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/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+ const Eigen::Matrix<double, 2, 2>& N);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
new file mode 100644
index 0000000..8d98553
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
@@ -0,0 +1,13 @@
+// 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/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
+ const Eigen::Matrix<double, 2, 3>& N);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
new file mode 100644
index 0000000..840285c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
@@ -0,0 +1,13 @@
+// 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/DARE.h"
+
+template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+ const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+ const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
+template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+ const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+ const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
+ const Eigen::Matrix<double, 4, 1>& N);
diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index 03be9b3..74c8577 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -2,9 +2,10 @@
// 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 "Eigen/LU"
+#include <Eigen/LU>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
-#include "gtest/gtest.h"
TEST(EigenTest, Multiplication) {
frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
@@ -29,17 +30,17 @@
}
TEST(EigenTest, Transpose) {
- frc::Vectord<3> vec{1, 2, 3};
+ Eigen::Vector3d vec{1, 2, 3};
const auto transpose = vec.transpose();
- Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
+ Eigen::RowVector3d expectedTranspose{1, 2, 3};
EXPECT_TRUE(expectedTranspose.isApprox(transpose));
}
TEST(EigenTest, Inverse) {
- frc::Matrixd<3, 3> mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
+ Eigen::Matrix3d mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
const auto inverse = mat.inverse();
const auto identity = Eigen::MatrixXd::Identity(3, 3);
diff --git a/wpimath/src/test/native/cpp/FormatterTest.cpp b/wpimath/src/test/native/cpp/FormatterTest.cpp
index 0e2f77c..0b6d590 100644
--- a/wpimath/src/test/native/cpp/FormatterTest.cpp
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -5,10 +5,9 @@
#include <vector>
#include <fmt/format.h>
+#include <gtest/gtest.h>
#include "frc/fmt/Eigen.h"
-#include "frc/fmt/Units.h"
-#include "gtest/gtest.h"
#include "units/velocity.h"
TEST(FormatterTest, Eigen) {
@@ -17,14 +16,14 @@
" 0.000000 1.000000\n"
" 2.000000 3.000000\n"
" 4.000000 5.000000",
- fmt::format("{}", A));
+ fmt::format("{:f}", A));
Eigen::MatrixXd B{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
EXPECT_EQ(
" 0.000000 1.000000\n"
" 2.000000 3.000000\n"
" 4.000000 5.000000",
- fmt::format("{}", B));
+ fmt::format("{:f}", B));
Eigen::SparseMatrix<double> C{3, 2};
std::vector<Eigen::Triplet<double>> triplets;
@@ -38,7 +37,7 @@
" 0.000000 1.000000\n"
" 2.000000 3.000000\n"
" 4.000000 5.000000",
- fmt::format("{}", C));
+ fmt::format("{:f}", C));
}
TEST(FormatterTest, Units) {
diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
index a836a77..cee9831 100644
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -4,8 +4,9 @@
#include <limits>
+#include <gtest/gtest.h>
+
#include "frc/MathUtil.h"
-#include "gtest/gtest.h"
#include "units/angle.h"
#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
@@ -117,3 +118,49 @@
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
units::radian_t{-std::numbers::pi / 2});
}
+
+TEST(MathUtilTest, IsNear) {
+ // The answer is always 42
+ // Positive integer checks
+ EXPECT_TRUE(frc::IsNear(42, 42, 1));
+ EXPECT_TRUE(frc::IsNear(42, 41, 2));
+ EXPECT_TRUE(frc::IsNear(42, 43, 2));
+ EXPECT_FALSE(frc::IsNear(42, 44, 1));
+
+ // Negative integer checks
+ EXPECT_TRUE(frc::IsNear(-42, -42, 1));
+ EXPECT_TRUE(frc::IsNear(-42, -41, 2));
+ EXPECT_TRUE(frc::IsNear(-42, -43, 2));
+ EXPECT_FALSE(frc::IsNear(-42, -44, 1));
+
+ // Mixed sign integer checks
+ EXPECT_FALSE(frc::IsNear(-42, 42, 1));
+ EXPECT_FALSE(frc::IsNear(-42, 41, 2));
+ EXPECT_FALSE(frc::IsNear(-42, 43, 2));
+ EXPECT_FALSE(frc::IsNear(42, -42, 1));
+ EXPECT_FALSE(frc::IsNear(42, -41, 2));
+ EXPECT_FALSE(frc::IsNear(42, -43, 2));
+
+ // Floating point checks
+ EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
+ EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
+ EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
+ EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
+
+ // Wraparound checks
+ EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
+ EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
+ EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
+ EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
+ EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
+ EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
+ EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
+ EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
+ EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
+ EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
+ EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
+ EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
+ EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
+ EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
+ EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
+}
diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index ee7842c..07907e2 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -2,11 +2,11 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
#include <random>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 105a434..758dc30 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
-
#include <array>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/NumericalIntegration.h"
diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index 1059260..1464dcd 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -7,10 +7,12 @@
#include <string>
#include <type_traits>
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
#include "units/acceleration.h"
#include "units/angle.h"
#include "units/angular_acceleration.h"
+#include "units/angular_jerk.h"
#include "units/angular_velocity.h"
#include "units/area.h"
#include "units/capacitance.h"
@@ -51,6 +53,7 @@
using namespace units::acceleration;
using namespace units::angle;
using namespace units::angular_acceleration;
+using namespace units::angular_jerk;
using namespace units::angular_velocity;
using namespace units::area;
using namespace units::capacitance;
@@ -1420,7 +1423,7 @@
}
#endif
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
TEST_F(UnitContainer, fmtlib) {
testing::internal::CaptureStdout();
fmt::print("{}", degree_t(349.87));
@@ -2002,6 +2005,23 @@
EXPECT_NEAR(1.537e-16, test, 5.0e-20);
}
+TEST_F(UnitConversion, angular_jerk) {
+ double test;
+ bool same;
+
+ same =
+ std::is_same_v<radians_per_second_cubed,
+ unit<std::ratio<1>, category::angular_jerk_unit>>;
+ EXPECT_TRUE(same);
+ same = traits::is_convertible_unit_v<deg_per_s_cu, radians_per_second_cubed>;
+ EXPECT_TRUE(same);
+
+ test = convert<degrees_per_second_cubed, radians_per_second_cubed>(1.0);
+ EXPECT_NEAR(0.0174533, test, 5.0e-8);
+ test = convert<turns_per_second_cubed, radians_per_second_cubed>(1.0);
+ EXPECT_NEAR(6.283185307, test, 5.0e-6);
+}
+
TEST_F(UnitConversion, acceleration) {
double test;
diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
new file mode 100644
index 0000000..b7853b1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
@@ -0,0 +1,76 @@
+// 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 <cmath>
+#include <numbers>
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/ArmFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+static constexpr auto Ks = 0.5_V;
+static constexpr auto Kv = 1.5_V * 1_s / 1_rad;
+static constexpr auto Ka = 2_V * 1_s * 1_s / 1_rad;
+static constexpr auto Kg = 1_V;
+
+TEST(ArmFeedforwardTest, Calculate) {
+ frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+ EXPECT_NEAR(
+ armFF.Calculate(std::numbers::pi * 1_rad / 3, 0_rad / 1_s).value(), 0.5,
+ 0.002);
+ EXPECT_NEAR(
+ armFF.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s).value(), 2.5,
+ 0.002);
+ EXPECT_NEAR(armFF
+ .Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s,
+ 2_rad / 1_s / 1_s)
+ .value(),
+ 6.5, 0.002);
+ EXPECT_NEAR(armFF
+ .Calculate(std::numbers::pi * 1_rad / 3, -1_rad / 1_s,
+ 2_rad / 1_s / 1_s)
+ .value(),
+ 2.5, 0.002);
+}
+
+TEST(ArmFeedforwardTest, AchievableVelocity) {
+ frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+ EXPECT_NEAR(armFF
+ .MaxAchievableVelocity(12_V, std::numbers::pi * 1_rad / 3,
+ 1_rad / 1_s / 1_s)
+ .value(),
+ 6, 0.002);
+ EXPECT_NEAR(armFF
+ .MinAchievableVelocity(11.5_V, std::numbers::pi * 1_rad / 3,
+ 1_rad / 1_s / 1_s)
+ .value(),
+ -9, 0.002);
+}
+
+TEST(ArmFeedforwardTest, AchievableAcceleration) {
+ frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+ EXPECT_NEAR(armFF
+ .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+ 1_rad / 1_s)
+ .value(),
+ 4.75, 0.002);
+ EXPECT_NEAR(armFF
+ .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+ -1_rad / 1_s)
+ .value(),
+ 6.75, 0.002);
+ EXPECT_NEAR(armFF
+ .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+ 1_rad / 1_s)
+ .value(),
+ -7.25, 0.002);
+ EXPECT_NEAR(armFF
+ .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+ -1_rad / 1_s)
+ .value(),
+ -5.25, 0.002);
+}
diff --git a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
index 4dfaa0e..f931590 100644
--- a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
@@ -2,8 +2,9 @@
// 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 <gtest/gtest.h>
+
#include "frc/controller/BangBangController.h"
-#include "gtest/gtest.h"
class BangBangInputOutputTest : public testing::Test {
protected:
diff --git a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
index e582720..3484e6e 100644
--- a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
@@ -2,8 +2,9 @@
// 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 <gtest/gtest.h>
+
#include "frc/controller/BangBangController.h"
-#include "gtest/gtest.h"
class BangBangToleranceTest : public testing::Test {
protected:
diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index b2bcee6..9db64da 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
#include "units/time.h"
diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
index 74d945c..5428d84 100644
--- a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
@@ -2,11 +2,11 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
-#include "frc/EigenCore.h"
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
#include "frc/controller/DifferentialDriveFeedforward.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
@@ -38,9 +38,9 @@
auto [left, right] = differentialDriveFeedforward.Calculate(
currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
nextRightVelocity, dt);
- frc::Matrixd<2, 1> nextX = plant.CalculateX(
- frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
- frc::Vectord<2>{left, right}, dt);
+ Eigen::Vector2d nextX = plant.CalculateX(
+ Eigen::Vector2d{currentLeftVelocity, currentRightVelocity},
+ Eigen::Vector2d{left, right}, dt);
EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
}
@@ -72,9 +72,9 @@
auto [left, right] = differentialDriveFeedforward.Calculate(
currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
nextRightVelocity, dt);
- frc::Matrixd<2, 1> nextX = plant.CalculateX(
- frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
- frc::Vectord<2>{left, right}, dt);
+ Eigen::Vector2d nextX = plant.CalculateX(
+ Eigen::Vector2d{currentLeftVelocity, currentRightVelocity},
+ Eigen::Vector2d{left, right}, dt);
EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
}
diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
new file mode 100644
index 0000000..a2db70d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
@@ -0,0 +1,60 @@
+// 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 <cmath>
+
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/ElevatorFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+static constexpr auto Ks = 0.5_V;
+static constexpr auto Kv = 1.5_V * 1_s / 1_m;
+static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m;
+static constexpr auto Kg = 1_V;
+
+TEST(ElevatorFeedforwardTest, Calculate) {
+ frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+
+ EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
+ EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
+ EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
+ 0.002);
+ EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
+ 0.002);
+
+ frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
+ frc::Matrixd<1, 1> B{1.0 / Ka.value()};
+ constexpr units::second_t dt = 20_ms;
+ frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+
+ frc::Vectord<1> r{2.0};
+ frc::Vectord<1> nextR{3.0};
+ EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
+ elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+}
+
+TEST(ElevatorFeedforwardTest, AchievableVelocity) {
+ frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+ EXPECT_NEAR(elevatorFF.MaxAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
+ 5, 0.002);
+ EXPECT_NEAR(elevatorFF.MinAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
+ -9, 0.002);
+}
+
+TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
+ frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+ EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, 2_m / 1_s).value(),
+ 3.75, 0.002);
+ EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, -2_m / 1_s).value(),
+ 7.25, 0.002);
+ EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, 2_m / 1_s).value(),
+ -8.25, 0.002);
+ EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, -2_m / 1_s).value(),
+ -4.75, 0.002);
+}
diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
index 134a4e0..4b4a883 100644
--- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -4,10 +4,11 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/MathUtil.h"
#include "frc/controller/HolonomicDriveController.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
#include "units/angular_acceleration.h"
#include "units/math.h"
#include "units/time.h"
@@ -21,7 +22,7 @@
TEST(HolonomicDriveControllerTest, ReachesReference) {
frc::HolonomicDriveController controller{
- frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+ frc::PIDController{1.0, 0.0, 0.0}, frc::PIDController{1.0, 0.0, 0.0},
frc::ProfiledPIDController<units::radian>{
1.0, 0.0, 0.0,
frc::TrapezoidProfile<units::radian>::Constraints{
@@ -53,7 +54,7 @@
TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
frc::HolonomicDriveController controller{
- frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+ frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
frc::ProfiledPIDController<units::radian>{
1, 0, 0,
frc::TrapezoidProfile<units::radian>::Constraints{
diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
index 753ea34..d96c67b 100644
--- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
@@ -4,12 +4,13 @@
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/MathUtil.h"
#include "frc/controller/LTVDifferentialDriveController.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
#include "units/math.h"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
index 56faf1d..ff2c7d3 100644
--- a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
@@ -2,10 +2,11 @@
// 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 <gtest/gtest.h>
+
#include "frc/MathUtil.h"
#include "frc/controller/LTVUnicycleController.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
#include "units/math.h"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index e7107d0..a92d4f0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "units/time.h"
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index 1127fc2..45f6473 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/system/LinearSystem.h"
@@ -158,10 +158,10 @@
EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
// QRN overload
- Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
+ Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 5.0)}};
Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
- EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
+ EXPECT_NEAR(-6.9190500116751458e-05, Kimf(0, 1), 1e-10);
}
TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index f1034f5..7992622 100644
--- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -2,14 +2,15 @@
// 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 <gtest/gtest.h>
+
#include "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
class PIDInputOutputTest : public testing::Test {
protected:
- frc2::PIDController* controller;
+ frc::PIDController* controller;
- void SetUp() override { controller = new frc2::PIDController{0, 0, 0}; }
+ void SetUp() override { controller = new frc::PIDController{0, 0, 0}; }
void TearDown() override { delete controller; }
};
@@ -49,3 +50,21 @@
EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
controller->Calculate(0.0025, 0));
}
+
+TEST_F(PIDInputOutputTest, IZoneNoOutput) {
+ controller->SetI(1);
+ controller->SetIZone(1);
+
+ double out = controller->Calculate(2, 0);
+
+ EXPECT_DOUBLE_EQ(0, out);
+}
+
+TEST_F(PIDInputOutputTest, IZoneOutput) {
+ controller->SetI(1);
+ controller->SetIZone(1);
+
+ double out = controller->Calculate(1, 0);
+
+ EXPECT_DOUBLE_EQ(-1 * controller->GetPeriod().value(), out);
+}
diff --git a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
index 0aec438..f37be89 100644
--- a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -2,27 +2,26 @@
// 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 <gtest/gtest.h>
+
#include "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
static constexpr double kSetpoint = 50.0;
static constexpr double kRange = 200;
static constexpr double kTolerance = 10.0;
TEST(PIDToleranceTest, InitialTolerance) {
- frc2::PIDController controller{0.5, 0.0, 0.0};
+ frc::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
- EXPECT_TRUE(controller.AtSetpoint());
+ EXPECT_FALSE(controller.AtSetpoint());
}
TEST(PIDToleranceTest, AbsoluteTolerance) {
- frc2::PIDController controller{0.5, 0.0, 0.0};
+ frc::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
- EXPECT_TRUE(controller.AtSetpoint())
- << "Error was not in tolerance when it should have been. Error was "
- << controller.GetPositionError();
+ EXPECT_FALSE(controller.AtSetpoint());
controller.SetTolerance(kTolerance);
controller.SetSetpoint(kSetpoint);
diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
index 44d1f41..5e4f01a 100644
--- a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -4,8 +4,9 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/controller/ProfiledPIDController.h"
-#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/angular_acceleration.h"
#include "units/angular_velocity.h"
diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
index 2fd26bb..d39c679 100644
--- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -2,10 +2,11 @@
// 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 <gtest/gtest.h>
+
#include "frc/MathUtil.h"
#include "frc/controller/RamseteController.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
#include "units/math.h"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
index 6d10e05..7878011 100644
--- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/SimpleMotorFeedforward.h"
diff --git a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
deleted file mode 100644
index 8631d6f..0000000
--- a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Eigenvalues>
-#include <gtest/gtest.h>
-
-#include "drake/common/test_utilities/eigen_matrix_compare.h"
-// #include "drake/math/autodiff.h"
-
-using Eigen::MatrixXd;
-
-namespace drake {
-namespace math {
-namespace {
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
- const Eigen::Ref<const MatrixXd>& B,
- const Eigen::Ref<const MatrixXd>& Q,
- const Eigen::Ref<const MatrixXd>& R) {
- MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
- // Check that X is positive semi-definite.
- EXPECT_TRUE(
- CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
- int n = X.rows();
- Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
- for (int i = 0; i < n; i++) {
- EXPECT_GE(es.eigenvalues()[i], 0);
- }
- // Check that X is the solution to the discrete time ARE.
- // clang-format off
- MatrixXd Y =
- A.transpose() * X * A
- - X
- - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
- * B.transpose() * X * A)
- + Q;
- // clang-format on
- EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
- MatrixCompareType::absolute));
-}
-
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
- const Eigen::Ref<const MatrixXd>& B,
- const Eigen::Ref<const MatrixXd>& Q,
- const Eigen::Ref<const MatrixXd>& R,
- const Eigen::Ref<const MatrixXd>& N) {
- MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N);
- // Check that X is positive semi-definite.
- EXPECT_TRUE(
- CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
- int n = X.rows();
- Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
- for (int i = 0; i < n; i++) {
- EXPECT_GE(es.eigenvalues()[i], 0);
- }
- // Check that X is the solution to the discrete time ARE.
- // clang-format off
- MatrixXd Y =
- A.transpose() * X * A
- - X
- - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
- * (B.transpose() * X * A + N.transpose()))
- + Q;
- // clang-format on
- EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
- MatrixCompareType::absolute));
-}
-
-GTEST_TEST(DARE, SolveDAREandVerify) {
- // Test 1: non-invertible A
- // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
- // Riccati Equation"
- int n1 = 4, m1 = 1;
- MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
- A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
- B1 << 0, 0, 0, 1;
- Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
- R1 << 0.25;
- SolveDAREandVerify(A1, B1, Q1, R1);
-
- MatrixXd Aref1(n1, n1);
- Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
- SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1),
- B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1);
-
- // Test 2: invertible A
- int n2 = 2, m2 = 1;
- MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
- A2 << 1, 1, 0, 1;
- B2 << 0, 1;
- Q2 << 1, 0, 0, 0;
- R2 << 0.3;
- SolveDAREandVerify(A2, B2, Q2, R2);
-
- MatrixXd Aref2(n2, n2);
- Aref2 << 0.5, 1, 0, 1;
- SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2),
- B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2);
-
- // Test 3: the first generalized eigenvalue of (S,T) is stable
- int n3 = 2, m3 = 1;
- MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
- A3 << 0, 1, 0, 0;
- B3 << 0, 1;
- Q3 << 1, 0, 0, 1;
- R3 << 1;
- SolveDAREandVerify(A3, B3, Q3, R3);
-
- MatrixXd Aref3(n3, n3);
- Aref3 << 0, 0.5, 0, 0;
- SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3),
- B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3);
-
- // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
- const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()};
- const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()};
- const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()};
- const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()};
- SolveDAREandVerify(A4, B4, Q4, R4);
-
- const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()};
- SolveDAREandVerify(A4, B4, Q4, R4, N4);
-}
-} // namespace
-} // namespace math
-} // namespace drake
diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index b2ee87d..d62decd 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
-
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/estimator/AngleStatistics.h"
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 1d623ca..3103068 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -7,13 +7,14 @@
#include <tuple>
#include <utility>
+#include <gtest/gtest.h>
+
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
@@ -138,6 +139,7 @@
0.15);
if (checkError) {
+ // NOLINTNEXTLINE(bugprone-integer-division)
EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
EXPECT_LT(maxError, 0.2);
}
@@ -207,3 +209,87 @@
}
}
}
+
+TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+ // This tests for multiple vision measurements appled at the same time.
+ // The expected behavior is that all measurements affect the estimated pose.
+ // The alternative result is that only one vision measurement affects the
+ // outcome. If that were the case, after 1000 measurements, the estimated
+ // pose would converge to that measurement.
+ frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+ frc::DifferentialDrivePoseEstimator estimator{
+ kinematics,
+ frc::Rotation2d{},
+ 0_m,
+ 0_m,
+ frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+ {0.02, 0.02, 0.01},
+ {0.1, 0.1, 0.1}};
+
+ estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
+
+ for (int i = 0; i < 1000; i++) {
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+ frc::DifferentialDriveKinematics kinematics{1_m};
+
+ frc::DifferentialDrivePoseEstimator estimator{
+ kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+ {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+ // Add enough measurements to fill up the buffer
+ for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+ estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
+ }
+
+ auto odometryPose = estimator.GetEstimatedPosition();
+
+ // Apply a vision measurement from 3 seconds ago
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+ 1_s, {0.1, 0.1, 0.1});
+
+ EXPECT_NEAR(odometryPose.X().value(),
+ estimator.GetEstimatedPosition().X().value(), 1e-6);
+ EXPECT_NEAR(odometryPose.Y().value(),
+ estimator.GetEstimatedPosition().Y().value(), 1e-6);
+ EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+ estimator.GetEstimatedPosition().Rotation().Radians().value(),
+ 1e-6);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 1924b73..2a5ce80 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -2,12 +2,12 @@
// 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 <gtest/gtest.h>
-
#include <array>
#include <cmath>
-#include "Eigen/QR"
+#include <Eigen/QR>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
index fc373ca..6697ec0 100644
--- a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
@@ -2,12 +2,12 @@
// 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 <gtest/gtest.h>
-
#include <array>
#include <cmath>
-#include "Eigen/Core"
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
#include "frc/estimator/KalmanFilter.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 13dc5aa..8b3a586 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -6,11 +6,12 @@
#include <random>
#include <tuple>
+#include <gtest/gtest.h>
+
#include "frc/estimator/MecanumDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
@@ -129,6 +130,7 @@
0.15);
if (checkError) {
+ // NOLINTNEXTLINE(bugprone-integer-division)
EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
EXPECT_LT(maxError, 0.2);
}
@@ -206,3 +208,90 @@
}
}
}
+
+TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+ // This tests for multiple vision measurements appled at the same time.
+ // The expected behavior is that all measurements affect the estimated pose.
+ // The alternative result is that only one vision measurement affects the
+ // outcome. If that were the case, after 1000 measurements, the estimated
+ // pose would converge to that measurement.
+ frc::MecanumDriveKinematics kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::MecanumDriveWheelPositions wheelPositions;
+
+ frc::MecanumDrivePoseEstimator estimator{
+ kinematics, frc::Rotation2d{},
+ wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+ {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+
+ estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions);
+
+ for (int i = 0; i < 1000; i++) {
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+}
+
+TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+ frc::MecanumDriveKinematics kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::MecanumDrivePoseEstimator estimator{
+ kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
+ frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+ // Add enough measurements to fill up the buffer
+ for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+ estimator.UpdateWithTime(time, frc::Rotation2d{},
+ frc::MecanumDriveWheelPositions{});
+ }
+
+ auto odometryPose = estimator.GetEstimatedPosition();
+
+ // Apply a vision measurement from 3 seconds ago
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+ 1_s, {0.1, 0.1, 0.1});
+
+ EXPECT_NEAR(odometryPose.X().value(),
+ estimator.GetEstimatedPosition().X().value(), 1e-6);
+ EXPECT_NEAR(odometryPose.Y().value(),
+ estimator.GetEstimatedPosition().Y().value(), 1e-6);
+ EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+ estimator.GetEstimatedPosition().Rotation().Radians().value(),
+ 1e-6);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index 19a734b..4471dc9 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -6,8 +6,6 @@
#include "frc/estimator/MerweScaledSigmaPoints.h"
-namespace drake::math {
-namespace {
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
@@ -30,5 +28,3 @@
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}
-} // namespace
-} // namespace drake::math
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index 554ca59..f1024ef 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -8,12 +8,13 @@
#include <tuple>
#include <fmt/format.h>
+#include <gtest/gtest.h>
+#include <wpi/timestamp.h>
#include "frc/estimator/SwerveDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
void testFollowTrajectory(
const frc::SwerveDriveKinematics<4>& kinematics,
@@ -132,6 +133,7 @@
0.15);
if (checkError) {
+ // NOLINTNEXTLINE(bugprone-integer-division)
EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
EXPECT_LT(maxError, 0.2);
}
@@ -215,3 +217,97 @@
}
}
}
+
+TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+ // This tests for multiple vision measurements appled at the same time.
+ // The expected behavior is that all measurements affect the estimated pose.
+ // The alternative result is that only one vision measurement affects the
+ // outcome. If that were the case, after 1000 measurements, the estimated
+ // pose would converge to that measurement.
+ frc::SwerveDriveKinematics<4> kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::SwerveModulePosition fl;
+ frc::SwerveModulePosition fr;
+ frc::SwerveModulePosition bl;
+ frc::SwerveModulePosition br;
+
+ frc::SwerveDrivePoseEstimator<4> estimator{
+ kinematics, frc::Rotation2d{},
+ {fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+ {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+ estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
+
+ for (int i = 0; i < 1000; i++) {
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+
+ {
+ auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+ auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+ auto dtheta = units::math::abs(
+ estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+ EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+ }
+}
+
+TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+ frc::SwerveDriveKinematics<4> kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::SwerveModulePosition fl;
+ frc::SwerveModulePosition fr;
+ frc::SwerveModulePosition bl;
+ frc::SwerveModulePosition br;
+
+ frc::SwerveDrivePoseEstimator<4> estimator{
+ kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
+ frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+ // Add enough measurements to fill up the buffer
+ for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+ estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
+ }
+
+ auto odometryPose = estimator.GetEstimatedPosition();
+
+ // Apply a vision measurement from 3 seconds ago
+ estimator.AddVisionMeasurement(
+ frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+ 1_s, {0.1, 0.1, 0.1});
+
+ EXPECT_NEAR(odometryPose.X().value(),
+ estimator.GetEstimatedPosition().X().value(), 1e-6);
+ EXPECT_NEAR(odometryPose.Y().value(),
+ estimator.GetEstimatedPosition().Y().value(), 1e-6);
+ EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+ estimator.GetEstimatedPosition().Rotation().Radians().value(),
+ 1e-6);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 68f9c40..0f97a88 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -2,12 +2,12 @@
// 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 <gtest/gtest.h>
-
#include <array>
#include <cmath>
-#include "Eigen/QR"
+#include <Eigen/QR>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
index 2f64908..d232739 100644
--- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
#include <wpi/timestamp.h>
#include "frc/filter/Debouncer.h"
-#include "gtest/gtest.h"
#include "units/time.h"
static units::second_t now = 0_s;
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
index 8299a71..e0dea2d 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
@@ -8,7 +8,8 @@
#include <numbers>
#include <random>
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
#include "units/time.h"
// Filter constants
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
index 152737d..9195525 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -10,9 +10,9 @@
#include <numbers>
#include <random>
+#include <gtest/gtest.h>
#include <wpi/array.h>
-#include "gtest/gtest.h"
#include "units/time.h"
// Filter constants
diff --git a/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
index 8151a45..f0f7d99 100644
--- a/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
@@ -2,8 +2,9 @@
// 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 <gtest/gtest.h>
+
#include "frc/filter/MedianFilter.h"
-#include "gtest/gtest.h"
TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
frc::MedianFilter<double> filter{10};
diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
index 5dbe8c8..9657543 100644
--- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -2,10 +2,10 @@
// 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 <gtest/gtest.h>
#include <wpi/timestamp.h>
#include "frc/filter/SlewRateLimiter.h"
-#include "gtest/gtest.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
index fc44fa5..198f90f 100644
--- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
@@ -2,10 +2,11 @@
// 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 <gtest/gtest.h>
+
#include "frc/geometry/CoordinateSystem.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Transform3d.h"
-#include "gtest/gtest.h"
using namespace frc;
@@ -37,9 +38,6 @@
EXPECT_EQ(transformTo.Translation(),
CoordinateSystem::Convert(transformFrom.Translation(), coordFrom,
coordTo));
- EXPECT_EQ(
- transformTo.Rotation(),
- CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo));
EXPECT_EQ(transformTo,
CoordinateSystem::Convert(transformFrom, coordFrom, coordTo));
@@ -47,9 +45,6 @@
EXPECT_EQ(
transformFrom.Translation(),
CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom));
- EXPECT_EQ(
- transformFrom.Rotation(),
- CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom));
EXPECT_EQ(transformFrom,
CoordinateSystem::Convert(transformTo, coordTo, coordFrom));
}
@@ -106,29 +101,28 @@
// No rotation from EDN to NWU
CheckTransform3dConvert(
Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
- Transform3d{Translation3d{3_m, -1_m, -2_m},
- Rotation3d{-90_deg, 0_deg, -90_deg}},
+ Transform3d{Translation3d{3_m, -1_m, -2_m}, Rotation3d{}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
// 45° roll from EDN to NWU
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{45_deg, 0_deg, 0_deg}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
- Rotation3d{-45_deg, 0_deg, -90_deg}},
+ Rotation3d{0_deg, -45_deg, 0_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
// 45° pitch from EDN to NWU
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 45_deg, 0_deg}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
- Rotation3d{-90_deg, 0_deg, -135_deg}},
+ Rotation3d{0_deg, 0_deg, -45_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
// 45° yaw from EDN to NWU
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 0_deg, 45_deg}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
- Rotation3d{-90_deg, 45_deg, -90_deg}},
+ Rotation3d{45_deg, 0_deg, 0_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
}
@@ -136,28 +130,27 @@
// No rotation from EDN to NED
CheckTransform3dConvert(
Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
- Transform3d{Translation3d{3_m, 1_m, 2_m},
- Rotation3d{90_deg, 0_deg, 90_deg}},
+ Transform3d{Translation3d{3_m, 1_m, 2_m}, Rotation3d{}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
// 45° roll from EDN to NED
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{45_deg, 0_deg, 0_deg}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
- Rotation3d{135_deg, 0_deg, 90_deg}},
+ Rotation3d{0_deg, 45_deg, 0_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
// 45° pitch from EDN to NED
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 45_deg, 0_deg}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
- Rotation3d{90_deg, 0_deg, 135_deg}},
+ Rotation3d{0_deg, 0_deg, 45_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
// 45° yaw from EDN to NED
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 0_deg, 45_deg}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
- Rotation3d{90_deg, -45_deg, 90_deg}},
+ Rotation3d{45_deg, 0_deg, 0_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
}
diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index 5ce6819..bd5a9aa 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -3,12 +3,32 @@
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
+#include <cstdlib>
+
+#include <gtest/gtest.h>
#include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
using namespace frc;
+TEST(Pose2dTest, RotateBy) {
+ constexpr auto x = 1_m;
+ constexpr auto y = 2_m;
+ const Pose2d initial{x, y, 45_deg};
+
+ const Rotation2d rotation{5_deg};
+ const auto rotated = initial.RotateBy(rotation);
+
+ // Translation is rotated by CCW rotation matrix
+ double c = rotation.Cos();
+ double s = rotation.Sin();
+ EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+ EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+ EXPECT_DOUBLE_EQ(
+ initial.Rotation().Degrees().value() + rotation.Degrees().value(),
+ rotated.Rotation().Degrees().value());
+}
+
TEST(Pose2dTest, TransformBy) {
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};
@@ -26,9 +46,9 @@
const auto finalRelativeToInitial = final.RelativeTo(initial);
- EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+ EXPECT_NEAR(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value(), 1e-9);
EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
- EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
+ EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Degrees().value(), 1e-9);
}
TEST(Pose2dTest, Equality) {
@@ -49,9 +69,76 @@
const auto transform = final - initial;
- EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+ EXPECT_NEAR(5.0 * std::sqrt(2.0), transform.X().value(), 1e-9);
EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
- EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
+ EXPECT_NEAR(0.0, transform.Rotation().Degrees().value(), 1e-9);
+}
+
+TEST(Pose2dTest, Nearest) {
+ const Pose2d origin{0_m, 0_m, 0_deg};
+
+ const Pose2d pose1{Translation2d{1_m, Rotation2d{45_deg}}, 0_deg};
+ const Pose2d pose2{Translation2d{2_m, Rotation2d{90_deg}}, 0_deg};
+ const Pose2d pose3{Translation2d{3_m, Rotation2d{135_deg}}, 0_deg};
+ const Pose2d pose4{Translation2d{4_m, Rotation2d{180_deg}}, 0_deg};
+ const Pose2d pose5{Translation2d{5_m, Rotation2d{270_deg}}, 0_deg};
+
+ EXPECT_DOUBLE_EQ(pose3.X().value(),
+ origin.Nearest({pose5, pose3, pose4}).X().value());
+ EXPECT_DOUBLE_EQ(pose3.Y().value(),
+ origin.Nearest({pose5, pose3, pose4}).Y().value());
+
+ EXPECT_DOUBLE_EQ(pose1.X().value(),
+ origin.Nearest({pose1, pose2, pose3}).X().value());
+ EXPECT_DOUBLE_EQ(pose1.Y().value(),
+ origin.Nearest({pose1, pose2, pose3}).Y().value());
+
+ EXPECT_DOUBLE_EQ(pose2.X().value(),
+ origin.Nearest({pose4, pose2, pose3}).X().value());
+ EXPECT_DOUBLE_EQ(pose2.Y().value(),
+ origin.Nearest({pose4, pose2, pose3}).Y().value());
+
+ // Rotation component sort (when distance is the same)
+ // Use the same translation because using different angles at the same
+ // distance can cause rounding error.
+ const Translation2d translation{1_m, Rotation2d{0_deg}};
+
+ const Pose2d poseA{translation, 0_deg};
+ const Pose2d poseB{translation, Rotation2d{30_deg}};
+ const Pose2d poseC{translation, Rotation2d{120_deg}};
+ const Pose2d poseD{translation, Rotation2d{90_deg}};
+ const Pose2d poseE{translation, Rotation2d{-180_deg}};
+
+ EXPECT_DOUBLE_EQ(poseA.Rotation().Degrees().value(),
+ Pose2d(0_m, 0_m, Rotation2d{360_deg})
+ .Nearest({poseA, poseB, poseD})
+ .Rotation()
+ .Degrees()
+ .value());
+ EXPECT_DOUBLE_EQ(poseB.Rotation().Degrees().value(),
+ Pose2d(0_m, 0_m, Rotation2d{-335_deg})
+ .Nearest({poseB, poseC, poseD})
+ .Rotation()
+ .Degrees()
+ .value());
+ EXPECT_DOUBLE_EQ(poseC.Rotation().Degrees().value(),
+ Pose2d(0_m, 0_m, Rotation2d{-120_deg})
+ .Nearest({poseB, poseC, poseD})
+ .Rotation()
+ .Degrees()
+ .value());
+ EXPECT_DOUBLE_EQ(poseD.Rotation().Degrees().value(),
+ Pose2d(0_m, 0_m, Rotation2d{85_deg})
+ .Nearest({poseA, poseC, poseD})
+ .Rotation()
+ .Degrees()
+ .value());
+ EXPECT_DOUBLE_EQ(poseE.Rotation().Degrees().value(),
+ Pose2d(0_m, 0_m, Rotation2d{170_deg})
+ .Nearest({poseA, poseD, poseE})
+ .Rotation()
+ .Degrees()
+ .value());
}
TEST(Pose2dTest, Constexpr) {
diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
index 8c4452c..6d689db 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
@@ -4,11 +4,34 @@
#include <cmath>
+#include <gtest/gtest.h>
+#include <wpi/array.h>
+
#include "frc/geometry/Pose3d.h"
-#include "gtest/gtest.h"
using namespace frc;
+TEST(Pose3dTest, RotateBy) {
+ constexpr auto x = 1_m;
+ constexpr auto y = 2_m;
+ const Pose3d initial{x, y, 0_m, Rotation3d{0_deg, 0_deg, 45_deg}};
+
+ constexpr units::radian_t yaw = 5_deg;
+ const Rotation3d rotation{0_deg, 0_deg, yaw};
+ const auto rotated = initial.RotateBy(rotation);
+
+ // Translation is rotated by CCW rotation matrix
+ double c = std::cos(yaw.value());
+ double s = std::sin(yaw.value());
+ EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+ EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+ EXPECT_DOUBLE_EQ(0.0, rotated.Z().value());
+ EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().X().value());
+ EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().Y().value());
+ EXPECT_DOUBLE_EQ(initial.Rotation().Z().value() + rotation.Z().value(),
+ rotated.Rotation().Z().value());
+}
+
TEST(Pose3dTest, TestTransformByRotations) {
const double kEpsilon = 1E-9;
@@ -95,3 +118,86 @@
EXPECT_EQ(expected, pose.ToPose2d());
}
+
+TEST(Pose3dTest, ComplexTwists) {
+ wpi::array<Pose3d, 5> initial_poses{
+ Pose3d{0.698303_m, -0.959096_m, 0.271076_m,
+ Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}},
+ Pose3d{0.634892_m, -0.765209_m, 0.117543_m,
+ Rotation3d{Quaternion{0.84987, -0.070829, 0.162097, 0.496415}}},
+ Pose3d{0.584827_m, -0.590303_m, -0.02557_m,
+ Rotation3d{Quaternion{0.832743, -0.041991, 0.202188, 0.513708}}},
+ Pose3d{0.505038_m, -0.451479_m, -0.112835_m,
+ Rotation3d{Quaternion{0.816515, -0.002673, 0.226182, 0.531166}}},
+ Pose3d{0.428178_m, -0.329692_m, -0.189707_m,
+ Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}},
+ };
+
+ wpi::array<Pose3d, 5> final_poses{
+ Pose3d{-0.230448_m, -0.511957_m, 0.198406_m,
+ Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}},
+ Pose3d{-0.088932_m, -0.343253_m, 0.095018_m,
+ Rotation3d{Quaternion{0.638738, 0.413016, 0.536281, 0.365833}}},
+ Pose3d{-0.107908_m, -0.317552_m, 0.133946_m,
+ Rotation3d{Quaternion{0.653444, 0.417069, 0.465505, 0.427046}}},
+ Pose3d{-0.123383_m, -0.156411_m, -0.047435_m,
+ Rotation3d{Quaternion{0.652983, 0.40644, 0.431566, 0.47135}}},
+ Pose3d{-0.084654_m, -0.019305_m, -0.030022_m,
+ Rotation3d{Quaternion{0.620243, 0.429104, 0.479384, 0.44873}}},
+ };
+
+ for (size_t i = 0; i < initial_poses.size(); i++) {
+ auto start = initial_poses[i];
+ auto end = final_poses[i];
+
+ auto twist = start.Log(end);
+ auto start_exp = start.Exp(twist);
+
+ auto eps = 1E-5;
+
+ EXPECT_NEAR(start_exp.X().value(), end.X().value(), eps);
+ EXPECT_NEAR(start_exp.Y().value(), end.Y().value(), eps);
+ EXPECT_NEAR(start_exp.Z().value(), end.Z().value(), eps);
+ EXPECT_NEAR(start_exp.Rotation().GetQuaternion().W(),
+ end.Rotation().GetQuaternion().W(), eps);
+ EXPECT_NEAR(start_exp.Rotation().GetQuaternion().X(),
+ end.Rotation().GetQuaternion().X(), eps);
+ EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Y(),
+ end.Rotation().GetQuaternion().Y(), eps);
+ EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Z(),
+ end.Rotation().GetQuaternion().Z(), eps);
+ }
+}
+
+TEST(Pose3dTest, TwistNaN) {
+ wpi::array<Pose3d, 2> initial_poses{
+ Pose3d{6.32_m, 4.12_m, 0.00_m,
+ Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
+ 1.9208309264993548E-8}}},
+ Pose3d{3.75_m, 2.95_m, 0.00_m,
+ Rotation3d{Quaternion{0.9999999999999793, 0.0, 0.0,
+ 2.0352360299846772E-7}}},
+ };
+
+ wpi::array<Pose3d, 2> final_poses{
+ Pose3d{6.33_m, 4.15_m, 0.00_m,
+ Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
+ 2.416890209039172E-8}}},
+ Pose3d{3.66_m, 2.93_m, 0.00_m,
+ Rotation3d{Quaternion{0.9999999999999782, 0.0, 0.0,
+ 2.0859477994905617E-7}}},
+ };
+
+ for (size_t i = 0; i < initial_poses.size(); i++) {
+ auto start = initial_poses[i];
+ auto end = final_poses[i];
+ auto twist = start.Log(end);
+
+ EXPECT_FALSE(std::isnan(twist.dx.value()));
+ EXPECT_FALSE(std::isnan(twist.dy.value()));
+ EXPECT_FALSE(std::isnan(twist.dz.value()));
+ EXPECT_FALSE(std::isnan(twist.rx.value()));
+ EXPECT_FALSE(std::isnan(twist.ry.value()));
+ EXPECT_FALSE(std::isnan(twist.rz.value()));
+ }
+}
diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
index 5b95abb..ef0e95e 100644
--- a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
@@ -4,8 +4,9 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Quaternion.h"
-#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/math.h"
@@ -43,6 +44,54 @@
q3.Z() * q3.Z());
}
+TEST(QuaternionTest, Addition) {
+ Quaternion q{0.1, 0.2, 0.3, 0.4};
+ Quaternion p{0.5, 0.6, 0.7, 0.8};
+
+ auto sum = q + p;
+
+ EXPECT_DOUBLE_EQ(q.W() + p.W(), sum.W());
+ EXPECT_DOUBLE_EQ(q.X() + p.X(), sum.X());
+ EXPECT_DOUBLE_EQ(q.Y() + p.Y(), sum.Y());
+ EXPECT_DOUBLE_EQ(q.Z() + p.Z(), sum.Z());
+}
+
+TEST(QuaternionTest, Subtraction) {
+ Quaternion q{0.1, 0.2, 0.3, 0.4};
+ Quaternion p{0.5, 0.6, 0.7, 0.8};
+
+ auto difference = q - p;
+
+ EXPECT_DOUBLE_EQ(q.W() - p.W(), difference.W());
+ EXPECT_DOUBLE_EQ(q.X() - p.X(), difference.X());
+ EXPECT_DOUBLE_EQ(q.Y() - p.Y(), difference.Y());
+ EXPECT_DOUBLE_EQ(q.Z() - p.Z(), difference.Z());
+}
+
+TEST(QuaternionTest, ScalarMultiplication) {
+ Quaternion q{0.1, 0.2, 0.3, 0.4};
+ auto scalar = 2;
+
+ auto product = q * scalar;
+
+ EXPECT_DOUBLE_EQ(q.W() * scalar, product.W());
+ EXPECT_DOUBLE_EQ(q.X() * scalar, product.X());
+ EXPECT_DOUBLE_EQ(q.Y() * scalar, product.Y());
+ EXPECT_DOUBLE_EQ(q.Z() * scalar, product.Z());
+}
+
+TEST(QuaternionTest, ScalarDivision) {
+ Quaternion q{0.1, 0.2, 0.3, 0.4};
+ auto scalar = 2;
+
+ auto product = q / scalar;
+
+ EXPECT_DOUBLE_EQ(q.W() / scalar, product.W());
+ EXPECT_DOUBLE_EQ(q.X() / scalar, product.X());
+ EXPECT_DOUBLE_EQ(q.Y() / scalar, product.Y());
+ EXPECT_DOUBLE_EQ(q.Z() / scalar, product.Z());
+}
+
TEST(QuaternionTest, Multiply) {
// 90° CCW rotations around each axis
double c = units::math::cos(90_deg / 2.0);
@@ -64,19 +113,110 @@
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
0.48507125007266594};
actual = q * q.Inverse();
- EXPECT_DOUBLE_EQ(1.0, actual.W());
- EXPECT_DOUBLE_EQ(0.0, actual.X());
- EXPECT_DOUBLE_EQ(0.0, actual.Y());
- EXPECT_DOUBLE_EQ(0.0, actual.Z());
+ EXPECT_NEAR(1.0, actual.W(), 1e-9);
+ EXPECT_NEAR(0.0, actual.X(), 1e-9);
+ EXPECT_NEAR(0.0, actual.Y(), 1e-9);
+ EXPECT_NEAR(0.0, actual.Z(), 1e-9);
+}
+
+TEST(QuaternionTest, Conjugate) {
+ Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
+ 0.48507125007266594};
+ auto conj = q.Conjugate();
+
+ EXPECT_DOUBLE_EQ(q.W(), conj.W());
+ EXPECT_DOUBLE_EQ(-q.X(), conj.X());
+ EXPECT_DOUBLE_EQ(-q.Y(), conj.Y());
+ EXPECT_DOUBLE_EQ(-q.Z(), conj.Z());
}
TEST(QuaternionTest, Inverse) {
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
0.48507125007266594};
+ auto norm = q.Norm();
+
auto inv = q.Inverse();
- EXPECT_DOUBLE_EQ(q.W(), inv.W());
- EXPECT_DOUBLE_EQ(-q.X(), inv.X());
- EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
- EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
+ EXPECT_DOUBLE_EQ(q.W() / (norm * norm), inv.W());
+ EXPECT_DOUBLE_EQ(-q.X() / (norm * norm), inv.X());
+ EXPECT_DOUBLE_EQ(-q.Y() / (norm * norm), inv.Y());
+ EXPECT_DOUBLE_EQ(-q.Z() / (norm * norm), inv.Z());
+}
+
+TEST(QuaternionTest, Norm) {
+ Quaternion q{3, 4, 12, 84};
+ auto norm = q.Norm();
+
+ EXPECT_NEAR(85, norm, 1e-9);
+}
+
+TEST(QuaternionTest, Exponential) {
+ Quaternion q{1.1, 2.2, 3.3, 4.4};
+ Quaternion expect{2.81211398529184, -0.392521193481878, -0.588781790222817,
+ -0.785042386963756};
+
+ auto q_exp = q.Exp();
+
+ EXPECT_EQ(expect, q_exp);
+}
+
+TEST(QuaternionTest, Logarithm) {
+ Quaternion q{1.1, 2.2, 3.3, 4.4};
+ Quaternion expect{1.7959088706354, 0.515190292664085, 0.772785438996128,
+ 1.03038058532817};
+
+ auto q_log = q.Log();
+
+ EXPECT_EQ(expect, q_log);
+
+ Quaternion zero{0, 0, 0, 0};
+ Quaternion one{1, 0, 0, 0};
+ Quaternion i{0, 1, 0, 0};
+ Quaternion j{0, 0, 1, 0};
+ Quaternion k{0, 0, 0, 1};
+ Quaternion ln_half{std::log(0.5), -std::numbers::pi, 0, 0};
+
+ EXPECT_EQ(zero, one.Log());
+ EXPECT_EQ(i * std::numbers::pi / 2, i.Log());
+ EXPECT_EQ(j * std::numbers::pi / 2, j.Log());
+ EXPECT_EQ(k * std::numbers::pi / 2, k.Log());
+
+ EXPECT_EQ(i * -std::numbers::pi, (one * -1).Log());
+ EXPECT_EQ(ln_half, (one * -0.5).Log());
+}
+
+TEST(QuaternionTest, LogarithmAndExponentialInverse) {
+ Quaternion q{1.1, 2.2, 3.3, 4.4};
+
+ // These operations are order-dependent: ln(exp(q)) is congruent but not
+ // necessarily equal to exp(ln(q)) due to the multi-valued nature of the
+ // complex logarithm.
+
+ auto q_log_exp = q.Log().Exp();
+
+ EXPECT_EQ(q, q_log_exp);
+
+ Quaternion start{1, 2, 3, 4};
+ Quaternion expect{5, 6, 7, 8};
+
+ auto twist = start.Log(expect);
+ auto actual = start.Exp(twist);
+
+ EXPECT_EQ(expect, actual);
+}
+
+TEST(QuaternionTest, DotProduct) {
+ Quaternion q{1.1, 2.2, 3.3, 4.4};
+ Quaternion p{5.5, 6.6, 7.7, 8.8};
+
+ EXPECT_NEAR(q.W() * p.W() + q.X() * p.X() + q.Y() * p.Y() + q.Z() * p.Z(),
+ q.Dot(p), 1e-9);
+}
+
+TEST(QuaternionTest, DotProductAsEquality) {
+ Quaternion q{1.1, 2.2, 3.3, 4.4};
+ auto q_conj = q.Conjugate();
+
+ EXPECT_NEAR(q.Dot(q), q.Norm() * q.Norm(), 1e-9);
+ EXPECT_GT(std::abs(q.Dot(q_conj) - q.Norm() * q_conj.Norm()), 1e-9);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index 4e2e683..de2fc17 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -5,8 +5,9 @@
#include <cmath>
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Rotation2d.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
index 4709ed0..01b37e5 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
@@ -5,52 +5,93 @@
#include <cmath>
#include <numbers>
+#include <Eigen/Core>
+#include <gtest/gtest.h>
#include <wpi/MathExtras.h>
-#include "frc/EigenCore.h"
#include "frc/geometry/Rotation3d.h"
-#include "gtest/gtest.h"
using namespace frc;
+TEST(Rotation3dTest, GimbalLockAccuracy) {
+ auto rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+ auto rot2 = Rotation3d{units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
+ auto rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+ const auto result1 = rot1 + rot2 + rot3;
+ const auto expected1 =
+ Rotation3d{0_rad, -units::radian_t{std::numbers::pi / 2},
+ units::radian_t{std::numbers::pi / 2}};
+ EXPECT_EQ(expected1, result1);
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result1.X() + result1.Z()).value());
+ EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, result1.Y().value());
+
+ rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+ rot2 = Rotation3d{units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
+ rot3 = Rotation3d{units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+ const auto result2 = rot1 + rot2 + rot3;
+ const auto expected2 =
+ Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
+ units::radian_t{std::numbers::pi / 2}};
+ EXPECT_EQ(expected2, result2);
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result2.Z() - result2.X()).value());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result2.Y().value());
+
+ rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+ rot2 = Rotation3d{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+ rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+ const auto result3 = rot1 + rot2 + rot3;
+ const auto expected3 =
+ Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
+ units::radian_t{std::numbers::pi / 6}};
+ EXPECT_EQ(expected3, result3);
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 6, (result3.Z() - result3.X()).value());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result3.Y().value());
+}
+
TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot2{units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
+ const Rotation3d rvec1{Eigen::Vector3d{xAxis * std::numbers::pi / 3}};
EXPECT_EQ(rot1, rot2);
+ EXPECT_EQ(rot1, rvec1);
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Rotation3d rot3{yAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot4{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+ const Rotation3d rvec2{Eigen::Vector3d{yAxis * std::numbers::pi / 3}};
EXPECT_EQ(rot3, rot4);
+ EXPECT_EQ(rot3, rvec2);
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d rot5{zAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot6{0_rad, 0_rad, units::radian_t{std::numbers::pi / 3}};
+ const Rotation3d rvec3{Eigen::Vector3d{zAxis * std::numbers::pi / 3}};
EXPECT_EQ(rot5, rot6);
+ EXPECT_EQ(rot5, rvec3);
}
TEST(Rotation3dTest, InitRotationMatrix) {
// No rotation
- const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity();
+ const Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity();
const Rotation3d rot1{R1};
EXPECT_EQ(Rotation3d{}, rot1);
// 90 degree CCW rotation around z-axis
- Matrixd<3, 3> R2;
- R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0};
- R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0};
- R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0};
+ Eigen::Matrix3d R2;
+ R2.block<3, 1>(0, 0) = Eigen::Vector3d{0.0, 1.0, 0.0};
+ R2.block<3, 1>(0, 1) = Eigen::Vector3d{-1.0, 0.0, 0.0};
+ R2.block<3, 1>(0, 2) = Eigen::Vector3d{0.0, 0.0, 1.0};
const Rotation3d rot2{R2};
const Rotation3d expected2{0_deg, 0_deg, 90_deg};
EXPECT_EQ(expected2, rot2);
// Matrix that isn't orthogonal
- const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
+ const Eigen::Matrix3d R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
EXPECT_THROW(Rotation3d{R3}, std::domain_error);
// Matrix that's orthogonal but not special orthogonal
- const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0;
+ const Eigen::Matrix3d R4 = Eigen::Matrix3d::Identity() * 2.0;
EXPECT_THROW(Rotation3d{R4}, std::domain_error);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index 1b0934d..49c9466 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -4,11 +4,12 @@
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
index 904ec9c..8a6bbc0 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
@@ -4,11 +4,12 @@
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Translation3d.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 5493c43..1c92c0a 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -4,8 +4,9 @@
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
using namespace frc;
@@ -94,6 +95,37 @@
EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
}
+TEST(Translation2dTest, Nearest) {
+ const Translation2d origin{0_m, 0_m};
+
+ const Translation2d translation1{1_m, Rotation2d{45_deg}};
+ const Translation2d translation2{2_m, Rotation2d{90_deg}};
+ const Translation2d translation3{3_m, Rotation2d{135_deg}};
+ const Translation2d translation4{4_m, Rotation2d{180_deg}};
+ const Translation2d translation5{5_m, Rotation2d{270_deg}};
+
+ EXPECT_DOUBLE_EQ(
+ origin.Nearest({translation5, translation3, translation4}).X().value(),
+ translation3.X().value());
+ EXPECT_DOUBLE_EQ(
+ origin.Nearest({translation5, translation3, translation4}).Y().value(),
+ translation3.Y().value());
+
+ EXPECT_DOUBLE_EQ(
+ origin.Nearest({translation1, translation2, translation3}).X().value(),
+ translation1.X().value());
+ EXPECT_DOUBLE_EQ(
+ origin.Nearest({translation1, translation2, translation3}).Y().value(),
+ translation1.Y().value());
+
+ EXPECT_DOUBLE_EQ(
+ origin.Nearest({translation4, translation2, translation3}).X().value(),
+ translation2.X().value());
+ EXPECT_DOUBLE_EQ(
+ origin.Nearest({translation4, translation2, translation3}).Y().value(),
+ translation2.Y().value());
+}
+
TEST(Translation2dTest, Constexpr) {
constexpr Translation2d defaultCtor;
constexpr Translation2d componentCtor{1_m, 2_m};
diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
index 58c611e..b7aa8ce 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
@@ -4,8 +4,9 @@
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Translation3d.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index 33970cd..7f121c0 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -5,8 +5,9 @@
#include <cmath>
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
index 0d8a8f4..245ec17 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
@@ -5,8 +5,9 @@
#include <cmath>
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose3d.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
index 8a920f6..e448db2 100644
--- a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
+++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
@@ -4,10 +4,11 @@
#include <cmath>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
-#include "gtest/gtest.h"
#include "units/time.h"
TEST(TimeInterpolatableBufferTest, Interpolation) {
diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 4e0b3e5..97de03b 100644
--- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -2,12 +2,34 @@
// 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 <cmath>
+
+#include <gtest/gtest.h>
+
#include "frc/kinematics/ChassisSpeeds.h"
-#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
-TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
+TEST(ChassisSpeedsTest, Discretize) {
+ constexpr frc::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
+ constexpr units::second_t duration = 1_s;
+ constexpr units::second_t dt = 10_ms;
+
+ const auto speeds = frc::ChassisSpeeds::Discretize(target, duration);
+ const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
+
+ frc::Pose2d pose;
+ for (units::second_t time = 0_s; time < duration; time += dt) {
+ pose = pose.Exp(twist);
+ }
+
+ EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
+ EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
+ EXPECT_NEAR((target.omega * duration).value(),
+ pose.Rotation().Radians().value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
@@ -15,3 +37,64 @@
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
+
+TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) {
+ const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds(
+ 1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg);
+
+ EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
+ EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
+ EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Plus) {
+ const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+ const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
+
+ const frc::ChassisSpeeds result = left + right;
+
+ EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
+ EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
+ EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Minus) {
+ const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+ const frc::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
+
+ const frc::ChassisSpeeds result = left - right;
+
+ EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
+ EXPECT_NEAR(0, result.vy.value(), kEpsilon);
+ EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, UnaryMinus) {
+ const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+ const frc::ChassisSpeeds result = -speeds;
+
+ EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
+ EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
+ EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Multiplication) {
+ const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+ const frc::ChassisSpeeds result = speeds * 2;
+
+ EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
+ EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
+ EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Division) {
+ const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+ const frc::ChassisSpeeds result = speeds / 2;
+
+ EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
+ EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
+ EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 4af5ac8..782fed0 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -4,9 +4,10 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "gtest/gtest.h"
#include "units/angular_velocity.h"
#include "units/length.h"
#include "units/velocity.h"
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index e480941..a228357 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -4,9 +4,10 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
new file mode 100644
index 0000000..92cc583
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
@@ -0,0 +1,54 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+TEST(DifferentialDriveWheelSpeedsTest, Plus) {
+ const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
+ const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
+
+ const frc::DifferentialDriveWheelSpeeds result = left + right;
+
+ EXPECT_EQ(3.0, result.left.value());
+ EXPECT_EQ(2.0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Minus) {
+ const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
+ const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
+
+ const frc::DifferentialDriveWheelSpeeds result = left - right;
+
+ EXPECT_EQ(-1.0, result.left.value());
+ EXPECT_EQ(0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, UnaryMinus) {
+ const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+ const frc::DifferentialDriveWheelSpeeds result = -speeds;
+
+ EXPECT_EQ(-1.0, result.left.value());
+ EXPECT_EQ(-0.5, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Multiplication) {
+ const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+ const frc::DifferentialDriveWheelSpeeds result = speeds * 2;
+
+ EXPECT_EQ(2.0, result.left.value());
+ EXPECT_EQ(1.0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Division) {
+ const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+ const frc::DifferentialDriveWheelSpeeds result = speeds / 2;
+
+ EXPECT_EQ(0.5, result.left.value());
+ EXPECT_EQ(0.25, result.right.value());
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 364163e..04a3f1f 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -4,9 +4,10 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "gtest/gtest.h"
#include "units/angular_velocity.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index bfaf91b..be20655 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -5,9 +5,10 @@
#include <limits>
#include <random>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/MecanumDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
new file mode 100644
index 0000000..c10cfb5
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
@@ -0,0 +1,64 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+TEST(MecanumDriveWheelSpeedsTest, Plus) {
+ const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+ const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+
+ const frc::MecanumDriveWheelSpeeds result = left + right;
+
+ EXPECT_EQ(3.0, result.frontLeft.value());
+ EXPECT_EQ(2.0, result.frontRight.value());
+ EXPECT_EQ(2.5, result.rearLeft.value());
+ EXPECT_EQ(2.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Minus) {
+ const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+ const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+
+ const frc::MecanumDriveWheelSpeeds result = left - right;
+
+ EXPECT_EQ(-1.0, result.frontLeft.value());
+ EXPECT_EQ(-1.0, result.frontRight.value());
+ EXPECT_EQ(1.5, result.rearLeft.value());
+ EXPECT_EQ(0.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
+ const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+ const frc::MecanumDriveWheelSpeeds result = -speeds;
+
+ EXPECT_EQ(-1.0, result.frontLeft.value());
+ EXPECT_EQ(-0.5, result.frontRight.value());
+ EXPECT_EQ(-2.0, result.rearLeft.value());
+ EXPECT_EQ(-1.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
+ const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+ const frc::MecanumDriveWheelSpeeds result = speeds * 2;
+
+ EXPECT_EQ(2.0, result.frontLeft.value());
+ EXPECT_EQ(1.0, result.frontRight.value());
+ EXPECT_EQ(4.0, result.rearLeft.value());
+ EXPECT_EQ(3.0, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Division) {
+ const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+ const frc::MecanumDriveWheelSpeeds result = speeds / 2;
+
+ EXPECT_EQ(0.5, result.frontLeft.value());
+ EXPECT_EQ(0.25, result.frontRight.value());
+ EXPECT_EQ(1.0, result.rearLeft.value());
+ EXPECT_EQ(0.75, result.rearRight.value());
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 8e0dc8f..e7726e6 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -4,9 +4,10 @@
#include <numbers>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "gtest/gtest.h"
#include "units/angular_velocity.h"
using namespace frc;
@@ -125,6 +126,25 @@
EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
+TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
+ Rotation2d fl = {0_deg};
+ Rotation2d fr = {90_deg};
+ Rotation2d bl = {180_deg};
+ Rotation2d br = {270_deg};
+ m_kinematics.ResetHeadings(fl, fr, bl, br);
+ auto [flMod, frMod, blMod, brMod] =
+ m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
+
+ EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon);
+
+ EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon);
+ EXPECT_NEAR(blMod.angle.Degrees().value(), 180.0, kEpsilon);
+ EXPECT_NEAR(brMod.angle.Degrees().value(), 270.0, kEpsilon);
+}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, 135_deg};
@@ -274,3 +294,18 @@
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}
+
+TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
+ SwerveModuleState state1{1.0_mps, 0_deg};
+ SwerveModuleState state2{1.0_mps, 0_deg};
+ SwerveModuleState state3{-2.0_mps, 0_deg};
+ SwerveModuleState state4{-2.0_mps, 0_deg};
+
+ wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+ SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
+
+ EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
+ EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
+ EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
+ EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 8c67ab5..265b0cc 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -5,12 +5,13 @@
#include <limits>
#include <random>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp
new file mode 100644
index 0000000..14155f1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModulePosition.h"
+
+TEST(SwerveModulePositionTest, Equality) {
+ frc::SwerveModulePosition position1{2_m, 90_deg};
+ frc::SwerveModulePosition position2{2_m, 90_deg};
+
+ EXPECT_EQ(position1, position2);
+}
+
+TEST(SwerveModulePositionTest, Inequality) {
+ frc::SwerveModulePosition position1{1_m, 90_deg};
+ frc::SwerveModulePosition position2{2_m, 90_deg};
+ frc::SwerveModulePosition position3{1_m, 89_deg};
+
+ EXPECT_NE(position1, position2);
+ EXPECT_NE(position1, position3);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
index 4880bef..efc3975 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -2,9 +2,10 @@
// 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 <gtest/gtest.h>
+
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/SwerveModuleState.h"
-#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
@@ -39,3 +40,19 @@
EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
}
+
+TEST(SwerveModuleStateTest, Equality) {
+ frc::SwerveModuleState state1{2_mps, 90_deg};
+ frc::SwerveModuleState state2{2_mps, 90_deg};
+
+ EXPECT_EQ(state1, state2);
+}
+
+TEST(SwerveModuleStateTest, Inequality) {
+ frc::SwerveModuleState state1{1_mps, 90_deg};
+ frc::SwerveModuleState state2{2_mps, 90_deg};
+ frc::SwerveModuleState state3{1_mps, 89_deg};
+
+ EXPECT_NE(state1, state2);
+ EXPECT_NE(state1, state3);
+}
diff --git a/wpimath/src/test/native/cpp/main.cpp b/wpimath/src/test/native/cpp/main.cpp
index 09072ee..e993c1f 100644
--- a/wpimath/src/test/native/cpp/main.cpp
+++ b/wpimath/src/test/native/cpp/main.cpp
@@ -2,7 +2,7 @@
// 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 "gtest/gtest.h"
+#include <gtest/gtest.h>
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 7422a7f..c98d190 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -5,12 +5,13 @@
#include <chrono>
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/spline/QuinticHermiteSpline.h"
#include "frc/spline/SplineHelper.h"
#include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
#include "units/length.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index e45df7b..a0df9bb 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -4,12 +4,13 @@
#include <chrono>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/spline/QuinticHermiteSpline.h"
#include "frc/spline/SplineHelper.h"
#include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/length.h"
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index d735338..c42fcf0 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -2,11 +2,11 @@
// 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 <gtest/gtest.h>
-
#include <functional>
-#include "Eigen/Eigenvalues"
+#include <Eigen/Eigenvalues>
+#include <gtest/gtest.h>
+
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
@@ -114,98 +114,6 @@
<< discQIntegrated;
}
-// Test that the Taylor series discretization produces nearly identical results.
-TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
- frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
- frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
-
- constexpr auto dt = 1_s;
-
- frc::Matrixd<2, 2> discQTaylor;
- frc::Matrixd<2, 2> discA;
- frc::Matrixd<2, 2> discATaylor;
-
- // Continuous Q should be positive semidefinite
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
- for (int i = 0; i < contQ.rows(); ++i) {
- EXPECT_GE(esCont.eigenvalues()[i], 0);
- }
-
- // T
- // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
- frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
- std::function<frc::Matrixd<2, 2>(units::second_t,
- const frc::Matrixd<2, 2>&)>,
- frc::Matrixd<2, 2>>(
- [&](units::second_t t, const frc::Matrixd<2, 2>&) {
- return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
- (contA.transpose() * t.value()).exp());
- },
- 0_s, frc::Matrixd<2, 2>::Zero(), dt);
-
- frc::DiscretizeA<2>(contA, dt, &discA);
- frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
-
- EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
- << "Expected these to be nearly equal:\ndiscQTaylor:\n"
- << discQTaylor << "\ndiscQIntegrated:\n"
- << discQIntegrated;
- EXPECT_LT((discA - discATaylor).norm(), 1e-10);
-
- // Discrete Q should be positive semidefinite
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
- for (int i = 0; i < discQTaylor.rows(); ++i) {
- EXPECT_GE(esDisc.eigenvalues()[i], 0);
- }
-}
-
-// Test that the Taylor series discretization produces nearly identical results.
-TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
- frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
- frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
-
- constexpr auto dt = 5_ms;
-
- frc::Matrixd<2, 2> discQTaylor;
- frc::Matrixd<2, 2> discA;
- frc::Matrixd<2, 2> discATaylor;
-
- // Continuous Q should be positive semidefinite
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
- for (int i = 0; i < contQ.rows(); ++i) {
- EXPECT_GE(esCont.eigenvalues()[i], 0);
- }
-
- // T
- // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
- frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
- std::function<frc::Matrixd<2, 2>(units::second_t,
- const frc::Matrixd<2, 2>&)>,
- frc::Matrixd<2, 2>>(
- [&](units::second_t t, const frc::Matrixd<2, 2>&) {
- return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
- (contA.transpose() * t.value()).exp());
- },
- 0_s, frc::Matrixd<2, 2>::Zero(), dt);
-
- frc::DiscretizeA<2>(contA, dt, &discA);
- frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
-
- EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
- << "Expected these to be nearly equal:\ndiscQTaylor:\n"
- << discQTaylor << "\ndiscQIntegrated:\n"
- << discQIntegrated;
- EXPECT_LT((discA - discATaylor).norm(), 1e-10);
-
- // Discrete Q should be positive semidefinite
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
- for (int i = 0; i < discQTaylor.rows(); ++i) {
- EXPECT_GE(esDisc.eigenvalues()[i], 0);
- }
-}
-
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index dbc4284..4dc6263 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -5,6 +5,7 @@
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
+
#include <gtest/gtest.h>
#include "frc/system/plant/LinearSystemId.h"
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index b1793ad..1c73195 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -2,10 +2,11 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
#include "frc/system/NumericalIntegration.h"
// Tests that integrating dx/dt = e^x works.
@@ -30,6 +31,16 @@
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
+// Tests that integrating dx/dt = 0 works with RKDP
+TEST(NumericalIntegrationTest, ZeroRKDP) {
+ frc::Vectord<1> y1 = frc::RKDP(
+ [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+ return frc::Vectord<1>::Zero();
+ },
+ frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s);
+ EXPECT_NEAR(y1(0), 0.0, 1e-3);
+}
+
// Tests that integrating dx/dt = e^x works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
frc::Vectord<1> y0{0.0};
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
index 70f1938..e502e95 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -2,10 +2,11 @@
// 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 <gtest/gtest.h>
-
#include <cmath>
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
diff --git a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
index e2f7112..fafcbd4 100644
--- a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -5,9 +5,10 @@
#include <memory>
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/angle.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
index e3723b5..09abc76 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -5,9 +5,10 @@
#include <memory>
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/time.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index e3d6b7f..5282638 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -5,11 +5,12 @@
#include <memory>
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/length.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 8d9e221..c39bb15 100644
--- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -4,10 +4,11 @@
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/angle.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
new file mode 100644
index 0000000..8df3aa1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
@@ -0,0 +1,338 @@
+// 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/trajectory/ExponentialProfile.h" // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/acceleration.h"
+#include "units/frequency.h"
+#include "units/length.h"
+#include "units/math.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+static constexpr auto kDt = 10_ms;
+static constexpr auto kV = 2.5629_V / 1_mps;
+static constexpr auto kA = 0.43277_V / 1_mps_sq;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+ if (val1 <= val2) { \
+ EXPECT_LE(val1, val2); \
+ } else { \
+ EXPECT_NEAR_UNITS(val1, val2, eps); \
+ }
+
+frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
+ frc::ExponentialProfile<units::meter, units::volts> profile,
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints
+ constraints,
+ frc::SimpleMotorFeedforward<units::meter> feedforward,
+ frc::ExponentialProfile<units::meter, units::volts>::State current,
+ frc::ExponentialProfile<units::meter, units::volts>::State goal) {
+ auto next = profile.Calculate(kDt, current, goal);
+ auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
+
+ EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
+
+ return next;
+}
+
+TEST(ExponentialProfileTest, ReachesGoal) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ for (int i = 0; i < 450; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ for (int i = 0; i < 300; ++i) {
+ if (i == 150) {
+ constraints.maxInput = 9_V;
+ profile =
+ frc::ExponentialProfile<units::meter, units::volts>{constraints};
+ }
+
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ for (int i = 0; i < 300; ++i) {
+ if (i == 150) {
+ constraints.maxInput = 9_V;
+ profile =
+ frc::ExponentialProfile<units::meter, units::volts>{constraints};
+ }
+
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(ExponentialProfileTest, Backwards) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+ for (int i = 0; i < 400; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ for (int i = 0; i < 50; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+ EXPECT_NE(state, goal);
+
+ goal = {0.0_m, 0.0_mps};
+ for (int i = 0; i < 100; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, TopSpeed) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+ units::meters_per_second_t maxSpeed = 0_mps;
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ maxSpeed = units::math::max(state.velocity, maxSpeed);
+ }
+
+ EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
+ EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, TopSpeedBackward) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+ units::meters_per_second_t maxSpeed = 0_mps;
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ maxSpeed = units::math::min(state.velocity, maxSpeed);
+ }
+
+ EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
+ EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, HighInitialSpeed) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+
+ EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ }
+
+ EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TestHeuristic) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ std::vector<std::tuple<
+ frc::ExponentialProfile<units::meter, units::volts>::State, // initial
+ frc::ExponentialProfile<units::meter, units::volts>::State, // goal
+ frc::ExponentialProfile<units::meter, units::volts>::State> // inflection
+ // point
+ >
+ testCases{
+ // red > green and purple => always positive => false
+ {{0_m, -4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
+ {{0_m, -4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
+ {{0.6603_m, 4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
+ {{0.6603_m, 4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
+
+ // purple > red > green => positive if v0 < 0 => c && !d && a
+ {{0_m, -4_mps}, {0.5_m, -2_mps}, {0.4367_m, 3.7217_mps}},
+ {{0_m, -4_mps}, {0.546_m, 2_mps}, {0.4367_m, 3.7217_mps}},
+ {{0.6603_m, 4_mps}, {0.5_m, -2_mps}, {0.5560_m, -2.9616_mps}},
+ {{0.6603_m, 4_mps}, {0.546_m, 2_mps}, {0.5560_m, -2.9616_mps}},
+
+ // red < green and purple => always negative => true => !c && !d
+ {{0_m, -4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
+ {{0_m, -4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
+ {{0.6603_m, 4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
+ {{0.6603_m, 4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
+
+ // green > red > purple => positive if vf < 0 => !c && d && b
+ {{0_m, -4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
+ {{0_m, -4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
+ {{0.6603_m, 4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
+ {{0.6603_m, 4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
+
+ // tests for initial velocity > V/kV
+ {{0_m, -8_mps}, {0_m, 0_mps}, {-0.1384_m, 3.342_mps}},
+ {{0_m, -8_mps}, {-1_m, 0_mps}, {-0.562_m, -6.792_mps}},
+ {{0_m, 8_mps}, {1_m, 0_mps}, {0.562_m, 6.792_mps}},
+ {{0_m, 8_mps}, {-1_m, 0_mps}, {-0.785_m, -4.346_mps}},
+ };
+
+ for (auto& testCase : testCases) {
+ auto state = profile.CalculateInflectionPoint(std::get<0>(testCase),
+ std::get<1>(testCase));
+ EXPECT_NEAR_UNITS(std::get<2>(testCase).position / 1_m,
+ state.position / 1_m, 1e-3);
+ EXPECT_NEAR_UNITS(std::get<2>(testCase).velocity / 1_mps,
+ state.velocity / 1_mps, 1e-3);
+ }
+}
+
+TEST(ExponentialProfileTest, TimingToCurrent) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state, state), 0_s, 2e-2_s);
+ }
+
+ EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TimingToGoal) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ auto prediction = profile.TimeLeftUntil(state, goal);
+ auto reachedGoal = false;
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ if (!reachedGoal && state == goal) {
+ EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
+ reachedGoal = true;
+ }
+ }
+
+ EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TimingToNegativeGoal) {
+ frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+ 12_V, -kV / kA, 1 / kA};
+ frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+ frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+ 0.43277_V / 1_mps_sq};
+ frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
+ frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+ auto prediction = profile.TimeLeftUntil(state, goal);
+ auto reachedGoal = false;
+
+ for (int i = 0; i < 900; ++i) {
+ state = CheckDynamics(profile, constraints, feedforward, state, goal);
+ if (!reachedGoal && state == goal) {
+ EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
+ reachedGoal = true;
+ }
+ }
+
+ EXPECT_EQ(state, goal);
+}
diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 0bf6b1a..8ec3a70 100644
--- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -4,10 +4,11 @@
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/length.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
index 2b733a8..2bfd541 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
@@ -2,9 +2,10 @@
// 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 <gtest/gtest.h>
+
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
TEST(TrajectoryConcatenateTest, States) {
auto t1 = frc::TrajectoryGenerator::GenerateTrajectory(
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 5892461..8b50160 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -4,11 +4,12 @@
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/math.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
index 90c6dc0..9411696 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -2,9 +2,10 @@
// 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 <gtest/gtest.h>
+
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryUtil.h"
-#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
using namespace frc;
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 5c77a5b..f088fa3 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -4,10 +4,11 @@
#include <vector>
+#include <gtest/gtest.h>
+
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
std::vector<frc::Trajectory::State> statesB) {
diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index 6a35261..b2e5b18 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -7,7 +7,8 @@
#include <chrono>
#include <cmath>
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
#include "units/acceleration.h"
#include "units/length.h"
#include "units/math.h"
@@ -31,9 +32,9 @@
frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
+ frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 450; ++i) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -45,17 +46,18 @@
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
- frc::TrapezoidProfile<units::meter> profile{constraints, goal};
- auto state = profile.Calculate(kDt);
+ frc::TrapezoidProfile<units::meter> profile{constraints};
+ auto state = profile.Calculate(kDt, goal,
+ frc::TrapezoidProfile<units::meter>::State{});
auto lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75_mps;
+ profile = frc::TrapezoidProfile<units::meter>{constraints};
}
- profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
auto estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
@@ -79,9 +81,9 @@
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
+ frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 400; ++i) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -92,16 +94,16 @@
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
+ frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
+ profile = frc::TrapezoidProfile<units::meter>{constraints};
for (int i = 0; i < 550; ++i) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -113,15 +115,15 @@
frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
+ frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
+ profile = frc::TrapezoidProfile<units::meter>{constraints};
for (int i = 0; i < 2000; ++i) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -132,9 +134,9 @@
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
+ frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 400; i++) {
- frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
}
}
@@ -146,14 +148,14 @@
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
- frc::TrapezoidProfile<units::meter> profile{constraints, goal};
- auto state = profile.Calculate(kDt);
+ frc::TrapezoidProfile<units::meter> profile{constraints};
+ auto state = profile.Calculate(kDt, goal,
+ frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
// time left in the profile doesn't increase linearly at the endpoints
@@ -170,14 +172,14 @@
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
- frc::TrapezoidProfile<units::meter> profile{constraints, goal};
- auto state = profile.Calculate(kDt);
+ frc::TrapezoidProfile<units::meter> profile{constraints};
+ auto state = profile.Calculate(kDt, goal,
+ frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -193,14 +195,14 @@
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
- frc::TrapezoidProfile<units::meter> profile{constraints, goal};
- auto state = profile.Calculate(kDt);
+ frc::TrapezoidProfile<units::meter> profile{constraints};
+ auto state = profile.Calculate(kDt, goal,
+ frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
// time left in the profile doesn't increase linearly at the endpoints
@@ -217,14 +219,14 @@
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
- frc::TrapezoidProfile<units::meter> profile{constraints, goal};
- auto state = profile.Calculate(kDt);
+ frc::TrapezoidProfile<units::meter> profile{constraints};
+ auto state = profile.Calculate(kDt, goal,
+ frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
- state = profile.Calculate(kDt);
+ state = profile.Calculate(kDt, goal, state);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
diff --git a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
deleted file mode 100644
index d6bcbb8..0000000
--- a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
+++ /dev/null
@@ -1,116 +0,0 @@
-#pragma once
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-
-#include <Eigen/Core>
-#include <gtest/gtest.h>
-
-// #include "drake/common/text_logging.h"
-
-namespace drake {
-
-enum class MatrixCompareType { absolute, relative };
-
-/**
- * Compares two matrices to determine whether they are equal to within a certain
- * threshold.
- *
- * @param m1 The first matrix to compare.
- * @param m2 The second matrix to compare.
- * @param tolerance The tolerance for determining equivalence.
- * @param compare_type Whether the tolereance is absolute or relative.
- * @param explanation A pointer to a string variable for saving an explanation
- * of why @p m1 and @p m2 are unequal. This parameter is optional and defaults
- * to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an
- * explanation is logged as an error message.
- * @return true if the two matrices are equal based on the specified tolerance.
- */
-template <typename DerivedA, typename DerivedB>
-[[nodiscard]] ::testing::AssertionResult CompareMatrices(
- const Eigen::MatrixBase<DerivedA>& m1,
- const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
- MatrixCompareType compare_type = MatrixCompareType::absolute) {
- if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
- return ::testing::AssertionFailure()
- << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
- << " vs. " << m2.rows() << " x " << m2.cols() << ")";
- }
-
- for (int ii = 0; ii < m1.rows(); ii++) {
- for (int jj = 0; jj < m1.cols(); jj++) {
- // First handle the corner cases of positive infinity, negative infinity,
- // and NaN
- const auto both_positive_infinity =
- m1(ii, jj) == std::numeric_limits<double>::infinity() &&
- m2(ii, jj) == std::numeric_limits<double>::infinity();
-
- const auto both_negative_infinity =
- m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
- m2(ii, jj) == -std::numeric_limits<double>::infinity();
-
- using std::isnan;
- const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
-
- if (both_positive_infinity || both_negative_infinity || both_nan)
- continue;
-
- // Check for case where one value is NaN and the other is not
- if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
- (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
- return ::testing::AssertionFailure() << "NaN mismatch at (" << ii
- << ", " << jj << "):\nm1 =\n"
- << m1 << "\nm2 =\n"
- << m2;
- }
-
- // Determine whether the difference between the two matrices is less than
- // the tolerance.
- using std::abs;
- const auto delta = abs(m1(ii, jj) - m2(ii, jj));
-
- if (compare_type == MatrixCompareType::absolute) {
- // Perform comparison using absolute tolerance.
-
- if (delta > tolerance) {
- return ::testing::AssertionFailure()
- << "Values at (" << ii << ", " << jj
- << ") exceed tolerance: " << m1(ii, jj) << " vs. "
- << m2(ii, jj) << ", diff = " << delta
- << ", tolerance = " << tolerance << "\nm1 =\n"
- << m1 << "\nm2 =\n"
- << m2 << "\ndelta=\n"
- << (m1 - m2);
- }
- } else {
- // Perform comparison using relative tolerance, see:
- // http://realtimecollisiondetection.net/blog/?p=89
- using std::max;
- const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
- const auto relative_tolerance =
- tolerance * max(decltype(max_value){1}, max_value);
-
- if (delta > relative_tolerance) {
- return ::testing::AssertionFailure()
- << "Values at (" << ii << ", " << jj
- << ") exceed tolerance: " << m1(ii, jj) << " vs. "
- << m2(ii, jj) << ", diff = " << delta
- << ", tolerance = " << tolerance
- << ", relative tolerance = " << relative_tolerance
- << "\nm1 =\n"
- << m1 << "\nm2 =\n"
- << m2 << "\ndelta=\n"
- << (m1 - m2);
- }
- }
- }
- }
-
- return ::testing::AssertionSuccess() << "m1 =\n"
- << m1
- << "\nis approximately equal to m2 =\n"
- << m2;
-}
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
index 5e7b165..6273532 100644
--- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
@@ -6,7 +6,6 @@
#include <array>
-#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {