Squashed 'third_party/allwpilib/' changes from 66b57f032..e473a00f9
e473a00f9 [wpiutil] Base64: Add unsigned span/vector variants (#3702)
52f2d580e [wpiutil] raw_uv_ostream: Add reset() (#3701)
d7b1e3576 [wpiutil] WebSocket: move std::function (#3700)
93799fbe9 [examples] Fix description of TrapezoidProfileSubsystem (#3699)
b84644740 [wpimath] Document pose estimator states, inputs, and outputs (#3698)
2dc35c139 [wpimath] Fix classpaths for JNI class loads (#3697)
2cb171f6f [docs] Set Doxygen extract_all to true and fix Doxygen failures (#3695)
a939cd9c8 [wpimath] Print uncontrollable/unobservable models in LQR and KF (#3694)
d5270d113 [wpimath] Clean up C++ StateSpaceUtil tests (#3692)
b20903960 [wpimath] Remove redundant discretization tests from StateSpaceUtilTest (#3689)
c0cb545b4 [wpilibc] Add deprecated Doxygen attribute to SpeedController (#3691)
35c9f66a7 [wpilib] Rename PneumaticsHub to PneumaticHub (#3686)
796d03d10 [wpiutil] Remove unused LLVM header (#3688)
8723caf78 [wpilibj] Make Java TrapezoidProfile.Constraints an immutable class (#3687)
187f50a34 [wpimath] Catch incorrect parameters to state-space models earlier (#3680)
8d04606c4 Replace instances of frc-characterization with SysId (NFC) (#3681)
b82d4f6e5 [hal, cscore, ntcore] Use WPI common handle type base
87e34967e [wpiutil] Add synchronization primitives
e32499c54 [wpiutil] Add ParallelTcpConnector (#3655)
aa0b49228 [wpilib] Remove redundant "quick turn" docs for curvature drive (NFC) (#3674)
57301a7f9 [hal] REVPH: Start closed-loop compressor control on init (#3673)
d1842ea8f [wpilib] Improve interrupt docs (NFC) (#3679)
558151061 [wpiutil] Add DsClient (#3654)
181723e57 Replace `.to<double>()` and `.template to<double>()` with `.value()` (#3667)
6bc1db44b [commands] Add pointer overload of AddRequirements (artf6003) (#3669)
737b57ed5 [wpimath] Update to drake v0.35.0 (#3665)
4d287d1ae [build] Upgrade WPIJREArtifact to JRE 2022-11.0.12u5 (#3666)
f26eb5ada [hal] Fix another typo (get -> gets) (NFC) (#3663)
94ed275ba [hal] Fix misspelling (numer -> number) (NFC) (#3662)
ac2f44da3 [wpiutil] uv: use move for std::function (#3653)
75fa1fbfb [wpiutil] json::serializer: Optimize construction (#3647)
5e689faea [wpiutil] Import MessagePack implementation (mpack) (#3650)
649a50b40 [wpiutil] Add LEB128 byte-by-byte reader (#3652)
e94397a97 [wpiutil] Move json_serializer.h to public headers (#3646)
4ec58724d [wpiutil] uv::Tcp: Clarify SetNoDelay documentation (#3649)
8cb294aa4 [wpiutil] WebSocket: Make Shutdown() public (#3651)
2b3a9a52b [wpiutil] json: Fix map iterator key() for std::string_view (#3645)
138cbb94b [wpiutil] uv::Async: Add direct call for no-parameter specialization (#3648)
e56d6dea8 [ci] Update testbench pool image to ubuntu-latest (#3643)
43f30e44e [build] Enable comments in doxygen source files (#3644)
9e6db17ef [build] Enable doxygen preprocessor expansion of WPI_DEPRECATED (#3642)
0e631ad2f Add WPILib version to issue template (#3641)
6229d8d2f [build] Docs: set case_sense_names to false (#3392)
4647d09b5 [docs] Fix Doxygen warnings, add CI docs lint job (#3639)
4ad3a5402 [hal] Fix PWM allocation channel (#3637)
05e5feac4 [docs] Fix brownout docs (NFC) (#3638)
67df469c5 [examples] Remove old command-based templates and examples (#3263)
689e9ccfb [hal, wpilib] Add brownout voltage configuration (#3632)
9cd4bc407 [docs] Add useLocal to avoid using installer artifacts (#3634)
61996c2bb [cscore] Fix Java direct callback notifications (#3631)
6d3dd99eb [build] Update to newest native-utils (#3633)
f0b484892 [wpiutil] Fix StringMap iterator equality check (#3629)
8352cbb7a Update development build instructions for 2022 (#3616)
6da08b71d [examples] Fix Intermediate Vision Java Example description (#3628)
5d99059bf [wpiutil] Remove optional.h (#3627)
fa41b106a [glass, wpiutil] Add missing format args (#3626)
4e3fd7d42 [build] Enable Zc:__cplusplus for Windows (#3625)
791d8354d [build] Suppress deprecation/removal warnings for old commands (#3618)
10f19e6fc [hal, wpilib] Add REV PneumaticsHub (#3600)
4c61a1305 [ntcore] Revert to per-element copy for toNative() (#3621)
7b3f62244 [wpiutil] SendableRegistry: Print exception stacktrace (#3620)
d347928e4 [hal] Use better error for when console out is enabled while attempting to use onboard serial port (#3622)
cc31079a1 [hal] Use setcap instead of setuid for setting thread priorities (#3613)
4676648b7 [wpimath] Upgrade to Drake v0.34.0 (#3607)
c7594c911 [build] Allow building wpilibc in cmake without cscore and opencv (#3605)
173cb7359 [wpilib] Add TimesliceRobot (#3502)
af295879f [hal] Set error status for I2C port out of range (#3603)
95dd20a15 [build] Enable spotbugs (#3601)
b65fce86b [wpilib] Remove Timer lock in wpilibj and update docs (#3602)
3b8d3bbcb Remove unused and add missing deprecated.h includes (#3599)
f9e976467 [examples] Rename DriveTrain classes to Drivetrain (#3594)
118a27be2 [wpilib] Add Timer tests (#3595)
59c89428e [wpilib] Deprecate Timer::HasPeriodPassed() (#3598)
202ca5e78 Force C++17 in .clang-format (#3597)
d6f185d8e Rename tests for consistency (#3592)
54ca474db [ci] Enable asan and tsan in CI for tests that pass (#3591)
1ca383b23 Add Debouncer (#3590)
179fde3a7 [build] Update to 2022 native utils and gradle 7 (#3588)
50198ffcf [examples] Add Mechanism2d visualization to Elevator Sim (#3587)
a446c2559 [examples] Synchronize C++ and Java Mechanism2d examples (#3589)
a7fb83103 [ci] clang-tidy: Generate compilation commands DB with Gradle (#3585)
4f5e0c9f8 [examples] Update ArmSimulation example to use Mechanism2d (#3572)
8164b91dc [CI] Print CMake test output on failure (#3583)
4d5fca27e [wpilib] Impove Mechanism2D documentation (NFC) (#3584)
fe59e4b9f Make C++ test names more consistent (#3586)
5c8868549 [wpilibc] Fix C++ MechanisimRoot2D to use same NT entries as Java/Glass (#3582)
9359431ba [wpimath] Clean up Eigen usage
72716f51c [wpimath] Upgrade to Eigen 3.4
382deef75 [wpimath] Explicitly export wpimath symbols
161e21173 [ntcore] Match standard handle layout, only allow 16 instances (#3577)
263a24811 [wpimath] Use jinja for codegen (#3574)
725251d29 [wpilib] Increase tolerances of DifferentialDriveSimTest (#3581)
4dff87301 [wpimath] Make LinearFilter::Factorial() constexpr (#3579)
60ede67ab [hal, wpilib] Switch PCM to be a single object that is allowed to be duplicated (#3475)
906bfc846 [build] Add CMake build support for sanitizers (#3576)
0d4f08ad9 [hal] Simplify string copy of joystick name (#3575)
a52bf87b7 [wpiutil] Add Java function package (#3570)
40c7645d6 [wpiutil] UidVector: Return old object from erase() (#3571)
5b886a23f [wpiutil] jni_util: Add size, operator[] to JArrayRef (#3569)
65797caa7 [sim] Fix halsim_ds_socket stringop overflow warning from GCC 10 (#3568)
66abb3988 [hal] Update runtime enum to allow selecting roborio 2 (#3565)
95a12e0ee [hal] UidSetter: Don't revert euid if its already current value (#3566)
27951442b [wpimath] Use external Eigen headers only (#3564)
c42e053ae [docs] Update to doxygen 1.9.2 (#3562)
e7048c8c8 [docs] Disable doxygen linking for common words that are also classes (#3563)
d8e0b6c97 [wpilibj] Fix java async interrupts (#3559)
5e6c34c61 Update to 2022 roborio image (#3537)
828f073eb [wpiutil] Fix uv::Buffer memory leaks caught by asan (#3555)
2dd5701ac [cscore] Fix mutex use-after-free in cscore test (#3557)
531439198 [ntcore] Fix NetworkTables memory leaks caught by asan (#3556)
3d9a4d585 [wpilibc] Fix AnalogTriggerOutput memory leak reported by asan (#3554)
54eda5928 [wpiutil] Ignore ubsan vptr upcast warning in SendableHelper moves (#3553)
5a4f75c9f [wpilib] Replace Speed controller comments with motor controller (NFC) (#3551)
7810f665f [wpiutil] Fix bug in uleb128 (#3540)
697e2dd33 [wpilib] Fix errant jaguar reference in comments (NFC) (#3550)
936c64ff5 [docs] Enable -linksource for javadocs (#3549)
1ea654954 [build] Upgrade CMake build to googletest 1.11.0 (#3548)
32d9949e4 [wpimath] Move controller tests to wpimath (#3541)
01ba56a8a [hal] Replace strncpy with memcpy (#3539)
e109c4251 [build] Rename makeSim flag to forceGazebo to better describe what it does (#3535)
e4c709164 [docs] Use a doxygen theme and add logo (#3533)
960b6e589 [wpimath] Fix Javadoc warning (#3532)
82eef8d5e [hal] Remove over current fault HAL functions from REV PDH (#3526)
aa3848b2c [wpimath] Move RobotDriveBase::ApplyDeadband() to MathUtil (#3529)
3b5d0d141 [wpimath] Add LinearFilter::BackwardFiniteDifference() (#3528)
c8fc715fe [wpimath] Upgrade drake files to v0.33.0 (#3531)
e5fe3a8e1 [build] Treat javadoc warnings as errors in CI and fix warnings (#3530)
e0c6cd3dc [wpimath] Add an operator for composing two Transform2ds (#3527)
2edd510ab [sim] Add sim wrappers for sensors that use SimDevice (#3517)
2b3e2ebc1 [hal] Fix HAL Notifier thread priority setting (#3522)
ab4cb5932 [gitignore] Update gitignore to ignore bazel / clion files (#3524)
57c8615af [build] Generate spotless patch on failure (#3523)
b90317321 Replace std::cout and std::cerr with fmt::print() (#3519)
10cc8b89c [hal] [wpilib] Add initial support for the REV PDH (#3503)
5d9ae3cdb [hal] Set HAL Notifier thread as RT by default (#3482)
192d251ee [wpilibcIntegrationTests] Properly disable DMA integration tests (#3514)
031962608 [wpilib] Add PS4Controller, remove Hand from GenericHID/XboxController (#3345)
25f6f478a [wpilib] Rename DriverStation::IsOperatorControl() to IsTeleop() (#3505)
e80f09f84 [wpilibj] Add unit tests (#3501)
c159f91f0 [wpilib] Only read DS control word once in IterativeRobotBase (#3504)
eb790a74d Add rio development docs documenting myRobot deploy tasks (#3508)
e47451f5a [wpimath] Replace auto with Eigen types (#3511)
252b8c83b Remove Java formatting from build task in CI (#3507)
09666ff29 Shorten Gazebo CI build (#3506)
baf2e501d Update myRobot to use 2021 java (#3509)
5ac60f0a2 [wpilib] Remove IterativeRobotBase mode init prints (#3500)
fb2ee8ec3 [wpilib] Add TimedRobot functions for running code on mode exit (#3499)
94e0db796 [wpilibc] Add more unit tests (#3494)
b25324695 [wpilibj] Add units to parameter names (NFC) (#3497)
1ac73a247 [hal] Rename PowerDistributionPanel to PowerDistribution (#3466)
2014115bc [examples] frisbeebot: Fix typo and reflow comments (NFC) (#3498)
4a944dc39 [examples] Consistently use 0 for controller port (#3496)
3838cc4ec Use unicode characters in docs equations (#3487)
85748f2e6 [examples] Add C++ TankDrive example (#3493)
d7b8aa56d [wpilibj] Rename DriverStation In[Mode] functions to follow style guide (#3488)
16e096cf8 [build] Fix CMake Windows CI (#3490)
50af74c38 [wpimath] Clean up NumericalIntegration and add Discretization tests (#3489)
bfc209b12 Automate fmt update (#3486)
e7f9331e4 [build] Update to Doxygen 1.9.1 (#3008)
ab8e8aa2a [wpimath] Update drake with upstream (#3484)
1ef826d1d [wpimath] Fix IOException path in WPIMath JNI (#3485)
52bddaa97 [wpimath] Disable iostream support for units and enable fmtlib (#3481)
e4dc3908b [wpiutil] Upgrade to fmtlib 8.0.1 (#3483)
1daadb812 [wpimath] Implement Dormand-Prince integration method (#3476)
9c2723391 [cscore] Add [[nodiscard]] to GrabFrame functions (#3479)
7a8796414 [wpilib] Add Notifier integration tests (#3480)
f8f13c536 [wpilibcExamples] Prefix decimal numbers with 0 (#3478)
1adb69c0f [ntcore] Use "NetworkTables" instead of "Network Tables" in NT specs (#3477)
5f5830b96 Upload wpiformat diff if one exists (#3474)
9fb4f35bb [wpimath] Add tests for DARE overload with Q, R, and N matrices (#3472)
c002e6f92 Run wpiformat (#3473)
c154e5262 [wpilib] Make solenoids exclusive use, PCM act like old sendable compressor (#3464)
6ddef1cca [hal] JNI setDIO: use a boolean and not a short (#3469)
9d68d9582 Remove extra newlines after open curly braces (NFC) (#3471)
a4233e1a1 [wpimath] Add script for updating Drake (#3470)
39373c6d2 Update README.md for new GCC version requirement (#3467)
d29acc90a [wpigui] Add option to reset UI on exit (#3463)
a371235b0 [ntcore] Fix dangling pointer in logger (#3465)
53b4891a5 [wpilibcintegrationtests] Fix deprecated Preferences usage (#3461)
646ded912 [wpimath] Remove incorrect discretization in pose estimators (#3460)
ea0b8f48e Fix some deprecation warnings due to fmtlib upgrade (#3459)
2067d7e30 [wpilibjexamples] Add wpimathjni, wpiutiljni to library path (#3455)
866571ab4 [wpiutil] Upgrade to fmtlib 8.0.0 (#3457)
4e1fa0308 [build] Skip PDB copy on windows build servers (#3458)
b45572167 [build] Change CI back to 18.04 docker images (#3456)
57a160f1b [wpilibc] Fix LiveWindow deprecation warning in RobotBase skeleton template (#3454)
29ae8640d [HLT] Implement duty cycle cross connect tests (#3453)
ee6377e54 [HLT] Add relay and analog cross connects (#3452)
b0f1ae7ea [build] CMake: Build the HAL even if WITH_CSCORE=OFF (#3449)
7aae2b72d Replace std::to_string() with fmt::format() (#3451)
73fcbbd74 [HLT] Add relay digital cross connect tests (#3450)
e7bedde83 [HLT] Add PWM tests that use DMA as the back end (#3447)
7253edb1e [wpilibc] Timer: Fix deprecated warning (#3446)
efa28125c [wpilibc] Add message to RobotBase on how to read stacktrace (#3444)
9832fcfe1 [hal] Fix DIO direction getter (#3445)
49c71f9f2 [wpilibj] Clarify robot quit message (#3364)
791770cf6 [wpimath] Move controller from wpilibj to wpimath (#3439)
9ce9188ff [wpimath] Add ReportWarning to MathShared (#3441)
362066a9b [wpilib] Deprecate getInstance() in favor of static functions (#3440)
26ff9371d Initial commit of cross connect integration test project (#3434)
4a36f86c8 [hal] Add support for DMA to Java (#3158)
85144e47f [commands] Unbreak build (#3438)
b417d961e Split Sendable into NT and non-NT portions (#3432)
ef4ea84cb [commands] Change grouping decorator impl to flatten nested group structures (#3335)
b422665a3 [examples] Invert right side of drive subsystems (#3437)
186dadf14 [hal] Error if attempting to set DIO output on an input port (#3436)
04e64db94 Remove redundant C++ lambda parentheses (NFC) (#3433)
f60994ad2 [wpiutil] Rename Java package to edu.wpi.first.util (#3431)
cfa1ca96f [wpilibc] Make ShuffleboardValue non-copyable (#3430)
4d9ff7643 Fix documentation warnings generated by JavaDoc (NFC) (#3428)
9e1b7e046 [build] Fix clang-tidy and clang-format (#3429)
a77c6ff3a [build] Upgrade clang-format and clang-tidy (NFC) (#3422)
099fde97d [wpilib] Improve PDP comments (NFC) (#3427)
f8fc2463e [wpilibc, wpiutil] Clean up includes (NFC) (#3426)
e246b7884 [wpimath] Clean up member initialization in feedforward classes (#3425)
c1e128bd5 Disable frivolous PMD warnings and enable PMD in ntcore (#3419)
8284075ee Run "Lint and Format" CI job on push as well as pull request (#3412)
f7db09a12 [wpimath] Move C++ filters into filter folder to match Java (#3417)
f9c3d54bd [wpimath] Reset error covariance in pose estimator ResetPosition() (#3418)
0773f4033 [hal] Ensure HAL status variables are initialized to zero (#3421)
d068fb321 [build] Upgrade CI to use 20.04 docker images (#3420)
8d054c940 [wpiutil] Remove STLExtras.h
80f1d7921 [wpiutil] Split function_ref to a separate header
64f541325 Use wpi::span instead of wpi::ArrayRef across all libraries (#3414)
2abbbd9e7 [build] clang-tidy: Remove bugprone-exception-escape (#3415)
a5c471af7 [wpimath] Add LQR template specialization for 2x2 system
edd2f0232 [wpimath] Add DARE solver for Q, R, and N with LQR ctor overloads
b2c3b2dd8 Use std::string_view and fmtlib across all libraries (#3402)
4f1cecb8e [wpiutil] Remove Path.h (#3413)
b336eac34 [build] Publish halsim_ws_core to Maven
2a09f6fa4 [build] Also build sim modules as static libraries
0e702eb79 [hal] Add a unified PCM object (#3331)
dea841103 [wpimath] Add fmtlib formatter overloads for Eigen::Matrix and units (#3409)
82856cf81 [wpiutil] Improve wpi::circular_buffer iterators (#3410)
8aecda03e [wpilib] Fix a documentation typo (#3408)
5c817082a [wpilib] Remove InterruptableSensorBase and replace with interrupt classes (#2410)
15c521a7f [wpimath] Fix drivetrain system identification (#3406)
989de4a1b [build] Force all linker warnings to be fatal for rio builds (#3407)
d9eeb45b0 [wpilibc] Add units to Ultrasonic class API (#3403)
fe570e000 [wpiutil] Replace llvm filesystem with C++17 filesystem (#3401)
01dc0249d [wpimath] Move SlewRateLimiter from wpilib to wpimath (#3399)
93523d572 [wpilibc] Clean up integration tests (#3400)
4f7a4464d [wpiutil] Rewrite StringExtras for std::string_view (#3394)
e09293a15 [wpilibc] Transition C++ classes to units::second_t (#3396)
827b17a52 [build] Create run tasks for Glass and OutlineViewer (#3397)
a61037996 [wpiutil] Avoid MSVC warning on span include (#3393)
4e2c3051b [wpilibc] Use std::string_view instead of Twine (#3380)
50915cb7e [wpilibc] MotorSafety::GetDescription(): Return std::string (#3390)
f4e2d26d5 [wpilibc] Move NullDeleter from frc/Base.h to wpi/NullDeleter.h (#3387)
cb0051ae6 [wpilibc] SimDeviceSim: use fmtlib (#3389)
a238cec12 [wpiutil] Deprecate wpi::math constants in favor of wpi::numbers (#3383)
393bf23c0 [ntcore, cscore, wpiutil] Standardize template impl files on .inc extension (NFC) (#3124)
e7d9ba135 [sim] Disable flaky web server integration tests (#3388)
0a0003c11 [wpilibjExamples] Fix name of Java swerve drive pose estimator example (#3382)
7e1b27554 [wpilibc] Use default copies and moves when possible (#3381)
fb2a56e2d [wpilibc] Remove START_ROBOT_CLASS macro (#3384)
84218bfb4 [wpilibc] Remove frc namespace shim (#3385)
dd7824340 [wpilibc] Remove C++ compiler version static asserts (#3386)
484cf9c0e [wpimath] Suppress the -Wmaybe-uninitialized warning in Eigen (#3378)
a04d1b4f9 [wpilibc] DriverStation: Remove ReportError and ReportWarning
831c10bdf [wpilibc] Errors: Use fmtlib
87603e400 [wpiutil] Import fmtlib (#3375)
442621672 [wpiutil] Add ArrayRef/std::span/wpi::span implicit conversions
bc15b953b [wpiutil] Add std::span implementation
6d20b1204 [wpiutil] StringRef, Twine, raw_ostream: Add std::string_view support (#3373)
2385c2a43 [wpilibc] Remove Utility.h (#3376)
87384ea68 [wpilib] Fix PIDController continuous range error calculations (#3170)
04dae799a [wpimath] Add SimpleMotorFeedforward::Calculate(velocity, nextVelocity) overload (#3183)
0768c3903 [wpilib] DifferentialDrive: Remove right side inversion (#3340)
8dd8d4d2d [wpimath] Fix redundant nested math package introduced by #3316 (#3368)
49b06beed [examples] Add Field2d to RamseteController example (#3371)
4c562a445 [wpimath] Fix typo in comment of update_eigen.py (#3369)
fdbbf1188 [wpimath] Add script for updating Eigen
f1e64b349 [wpimath] Move Eigen unsupported folder into eigeninclude
224f3a05c [sim] Fix build error when building with GCC 11.1 (#3361)
ff56d6861 [wpilibj] Fix SpeedController deprecated warnings (#3360)
1873fbefb [examples] Fix Swerve and Mecanum examples (#3359)
80b479e50 [examples] Fix SwerveBot example to use unique encoder ports (#3358)
1f7c9adee [wpilibjExamples] Fix pose estimator examples (#3356)
9ebc3b058 [outlineviewer] Change default size to 600x400 (#3353)
e21b443a4 [build] Gradle: Make C++ examples runnable (#3348)
da590120c [wpilibj] Add MotorController.setVoltage default (#3347)
561d53885 [build] Update opencv to 4.5.2, imgui/implot to latest (#3344)
44ad67ca8 [wpilibj] Preferences: Add missing Deprecated annotation (#3343)
3fe8fc75a [wpilibc] Revert "Return reference from GetInstance" (#3342)
3cc2da332 Merge branch '2022'
a3cd90dd7 [wpimath] Fix classpath used by generate_numbers.py (#3339)
d6cfdd3ba [wpilib] Preferences: Deprecate Put* in favor of Set* (#3337)
ba08baabb [wpimath] Update Drake DARE solver to v0.29.0 (#3336)
497b712f6 [wpilib] Make IterativeRobotBase::m_period private with getter
f00dfed7a [wpilib] Remove IterativeRobot base class
3c0846168 [hal] Use last error reporting instead of PARAMETER_OUT_OF_RANGE (#3328)
5ef2b4fdc [wpilibj] Fix @deprecated warning for SerialPort constructor (#3329)
23d2326d1 [hal] Report previous allocation location for indexed resource duplicates (#3322)
e338f9f19 [build] Fix wpilibc runCpp task (#3327)
c8ff626fe [wpimath] Move Java classes to edu.wpi.first.math (#3316)
4e424d51f [wpilibj] DifferentialDrivetrainSim: Rename constants to match the style guide (#3312)
6b50323b0 [cscore] Use Lock2DSize if possible for Windows USB cameras (#3326)
65c148536 [wpilibc] Fix "control reaches end of non-void function" warning (#3324)
f99f62bee [wpiutil] uv Handle: Use malloc/free instead of new/delete (#3325)
365f5449c [wpimath] Fix MecanumDriveKinematics (#3266)
ff52f207c [glass, wpilib] Rewrite Mechanism2d (#3281)
ee0eed143 [wpimath] Add DCMotor factory function for Romi motors (#3319)
512738072 [hal] Add HAL_GetLastError to enable better error messages from HAL calls (#3320)
ced654880 [glass, outlineviewer] Update Mac icons to macOS 11 style (#3313)
936d3b9f8 [templates] Add Java template for educational robot (#3309)
6e31230ad [examples] Fix odometry update in SwerveControllerCommand example (#3310)
05ebe9318 Merge branch 'main' into 2022
aaf24e255 [wpilib] Fix initial heading behavior in HolonomicDriveController (#3290)
8d961dfd2 [wpilibc] Remove ErrorBase (#3306)
659b37ef9 [wpiutil] StackTrace: Include offset on Linux (#3305)
0abf6c904 [wpilib] Move motor controllers to motorcontrol package (#3302)
4630191fa [wpiutil] circular_buffer: Use value initialization instead of passing zero (#3303)
b7b178f49 [wpilib] Remove Potentiometer interface
687066af3 [wpilib] Remove GyroBase
6b168ab0c [wpilib] Remove PIDController, PIDOutput, PIDSource
948625de9 [wpimath] Document conversion from filter cutoff frequency to time constant (#3299)
3848eb8b1 [wpilibc] Fix flywhel -> flywheel typo in FlywheelSim (#3298)
3abe0b9d4 [cscore] Move java package to edu.wpi.first.cscore (#3294)
d7fabe81f [wpilib] Remove RobotDrive (#3295)
1dc81669c [wpilib] Remove GearTooth (#3293)
01d0e1260 [wpilib] Revert move of RomiGyro into main wpilibc/j (#3296)
397e569aa [ntcore] Remove "using wpi" from nt namespace
79267f9e6 [ntcore] Remove NetworkTable -> nt::NetworkTable shim
48ebe5736 [ntcore] Remove deprecated Java interfaces and classes
c2064c78b [ntcore] Remove deprecated ITable interfaces
36608a283 [ntcore] Remove deprecated C++ APIs
a1c87e1e1 [glass] LogView: Add "copy to clipboard" button (#3274)
fa7240a50 [wpimath] Fix typo in quintic spline basis matrix
ffb4d38e2 [wpimath] Add derivation for spline basis matrices
f57c188f2 [wpilib] Add AnalogEncoder(int) ctor (#3273)
8471c4fb2 [wpilib] FieldObject2d: Add setTrajectory() method (#3277)
c97acd18e [glass] Field2d enhancements (#3234)
ffb590bfc [wpilib] Fix Compressor sendable properties (#3269)
6137f98eb [hal] Rename SimValueCallback2 to SimValueCallback (#3212)
a6f653969 [hal] Move registerSimPeriodic functions to HAL package (#3211)
10c038d9b [glass] Plot: Fix window creation after removal (#3264)
2d2eaa3ef [wpigui] Ensure window will be initially visible (#3256)
4d28b1f0c [wpimath] Use JNI for trajectory serialization (#3257)
3de800a60 [wpimath] TrajectoryUtil.h: Comment formatting (NFC) (#3262)
eff592377 [glass] Plot: Don't overwrite series ID (#3260)
a79faace1 [wpilibc] Return reference from GetInstance (#3247)
9550777b9 [wpilib] PWMSpeedController: Use PWM by composition (#3248)
c8521a3c3 [glass] Plot: Set reasonable default window size (#3261)
d71eb2cf3 [glass] Plot: Show full source name as tooltip and in popup (#3255)
160fb740f [hal] Use std::lround() instead of adding 0.5 and truncating (#3012)
48e9f3951 [wpilibj] Remove wpilibj package CameraServer (#3213)
8afa596fd [wpilib] Remove deprecated Sendable functions and SendableBase (#3210)
d3e45c297 [wpimath] Make C++ geometry classes immutable (#3249)
2c98939c1 [glass] StringChooser: Don't call SameLine() at end
a18a7409f [glass] NTStringChooser: Clear value of deleted entries
2f19cf452 [glass] NetworkTablesHelper: listen to delete events
da96707dc Merge branch 'main' into 2022
c3a8bdc24 [build] Fix clang-tidy action (#3246)
21624ef27 Add ImGui OutlineViewer (#3220)
1032c9b91 [wpiutil] Unbreak wpi::Format on Windows (#3242)
2e07902d7 [glass] NTField2D: Fix name lookup (#3233)
6e23e1840 [wpilibc] Remove WPILib.h (#3235)
3e22e4506 [wpilib] Make KoP drivetrain simulation weight 60 lbs (#3228)
79d1bd6c8 [glass] NetworkTablesSetting: Allow disable of server option (#3227)
fe341a16f [examples] Use more logical elevator setpoints in GearsBot (#3198)
62abf46b3 [glass] NetworkTablesSettings: Don't block GUI (#3226)
a95a5e0d9 [glass] Move NetworkTablesSettings to libglassnt (#3224)
d6f6ceaba [build] Run Spotless formatter (NFC) (#3221)
0922f8af5 [commands] CommandScheduler.requiring(): Note return can be null (NFC) (#2934)
6812302ff [examples] Make DriveDistanceOffboard example work in sim (#3199)
f3f86b8e7 [wpimath] Add pose estimator overload for vision + std dev measurement (#3200)
1a2680b9e [wpilibj] Change CommandBase.withName() to return CommandBase (#3209)
435bbb6a8 [command] RamseteCommand: Output 0 if interrupted (#3216)
3cf44e0a5 [hal] Add function for changing HAL Notifier thread priority (#3218)
40b367513 [wpimath] Units.java: Add kg-lb conversions (#3203)
9f563d584 [glass] NT: Fix return value in StringToDoubleArray (#3208)
af4adf537 [glass] Auto-size plots to fit window (#3193)
2560146da [sim] GUI: Add option to show prefix in Other Devices (#3186)
eae3a6397 gitignore: Ignore .cache directory (#3196)
959611420 [wpilib] Require non-zero positive value for PIDController.period (#3175)
9522f2e8c [wpimath] Add methods to concatenate trajectories (#3139)
e42a0b6cf [wpimath] Rotation2d comment formatting (NFC) (#3162)
d1c7032de [wpimath] Fix order of setting gyro offset in pose estimators (#3176)
d241bc81a [sim] Add DoubleSolenoidSim and SolenoidSim classes (#3177)
cb7f39afa [wpilibc] Add RobotController::GetBatteryVoltage() to C++ (#3179)
99b5ad9eb [wpilibj] Fix warnings that are not unused variables or deprecation (#3161)
c14b23775 [build] Fixup doxygen generated include dirs to match what users would need (#3154)
d447c7dc3 [sim] Add SimDeviceSim ctor overloads (#3134)
247420c9c [build] Remove jcenter repo (#3157)
04b112e00 [build] Include debug info in plugin published artifacts (#3149)
be0ce9900 [examples] Use PWMSparkMax instead of PWMVictorSPX (#3156)
69e8d0b65 [wpilib] Move RomiGyro into main wpilibc/j (#3143)
94e685e1b [wpimath] Add custom residual support to EKF (#3148)
5899f3dd2 [sim] GUI: Make keyboard settings loading more robust (#3167)
f82aa1d56 [wpilib] Fix HolonomicDriveController atReference() behavior (#3163)
fe5c2cf4b [wpimath] Remove ControllerUtil.java (#3169)
43d40c6e9 [wpiutil] Suppress unchecked cast in CombinedRuntimeLoader (#3155)
3d44d8f79 [wpimath] Fix argument order in UKF docs (NFC) (#3147)
ba6fe8ff2 [cscore] Add USB camera change event (#3123)
533725888 [build] Tweak OpenCV cmake search paths to work better on Linux (#3144)
29bf9d6ef [cscore] Add polled support to listener
483beb636 [ntcore] Move CallbackManager to wpiutil
fdaec7759 [examples] Instantiate m_ramseteController in example (#3142)
8494a5761 Rename default branch to main (#3140)
45590eea2 [wpigui] Hardcode window scale to 1 on macOS (#3135)
834a64920 [build] Publish libglass and libglassnt to Maven (#3127)
2c2ccb361 [wpimath] Fix Rotation2d equality operator (#3128)
fb5c8c39a [wpigui] clang-tidy: readability-braces-around-statements
f7d39193a [wpigui] Fix copyright in pfd and wpigui_metal.mm
aec796b21 [ntcore] Fix conditional jump on uninitialized value (#3125)
fb13bb239 [sim] GUI: Add right click popup for keyboard joystick settings (#3119)
c517ec677 [build] Update thirdparty-imgui to 1.79-2 (#3118)
e8cbf2a71 [wpimath] Fix typo in SwerveDrivePoseEstimator doc (NFC) (#3112)
e9c86df46 [wpimath] Add tests for swerve module optimization (#3100)
6ba8c289c [examples] Remove negative of ArcadeDrive(fwd, ..) in the C++ Getting Started Example (#3102)
3f1672e89 [hal] Add SimDevice createInt() and createLong() (#3110)
15be5cbf1 [examples] Fix segfault in GearsBot C++ example (#3111)
4cf0e5e6d Add quick links to API documentation in README (#3082)
6b1898f12 Fix RT priority docs (NFC) (#3098)
b3426e9c0 [wpimath] Fix missing whitespace in pose estimator doc (#3097)
38c1a1f3e [examples] Fix feildRelative -> fieldRelative typo in XControllerCommand examples (#3104)
4488e25f1 [glass] Shorten SmartDashboard window names (#3096)
cfdb3058e [wpilibj] Update SimDeviceSimTest (#3095)
64adff5fe [examples] Fix typo in ArcadeDrive constructor parameter name (#3092)
6efc58e3d [build] Fix issues with build on windows, deprecations, and native utils (#3090)
f393989a5 [wpimath, wpiutil] Add wpi::array for compile time size checking (#3087)
d6ed20c1e [build] Set macOS deployment target to 10.14 (#3088)
7c524014c [hal] Add [[nodiscard]] to HAL_WaitForNotifierAlarm() (#3085)
406d055f0 [wpilib] Fixup wouldHitLowerLimit in elevator and arm simulation classes. (#3076)
04a90b5dd [examples] Don't continually set setpoint in PotentiometerPID Examples (#3084)
8c5bfa013 [sim] GUI: Add max value setting for keyboard joysticks (#3083)
bc80c5535 [hal] Add SimValue reset() function (#3064)
9c3b51ca0 [wpilib] Document simulation APIs (#3079)
26584ff14 [wpimath] Add model description to LinearSystemId Javadocs (#3080)
42c3d5286 [examples] Sync Java and C++ trajectories in sim example (#3081)
64e72f710 [wpilibc] Add missing function RoboRioSim::ResetData (#3073)
e95503798 [wpimath] Add optimize() to SwerveModuleState (#3065)
fb99910c2 [hal] Add SimInt and SimLong wrappers for int/long SimValue (#3066)
e620bd4d3 [doc] Add machine-readable websocket specification (#3059)
a44e761d9 [glass] Add support for plot Y axis labels
ea1974d57 [wpigui] Update imgui and implot to latest
85a0bd43c [wpimath] Add RKF45 integration (#3047)
278e0f126 [glass] Use .controllable to set widgets' read-only state (#3035)
d8652cfd4 [wpimath] Make Java DCMotor API consistent with C++ and fix motor calcs (#3046)
377b7065a [build] Add toggleOffOn to Java spotless (#3053)
1e9c79c58 [sim] Use plant output to retrieve simulated position (#3043)
78147aa34 [sim] GUI: Fix Keyboard Joystick (#3052)
cd4a2265b [ntcore] Fix NetworkTableEntry::GetRaw() (#3051)
767ac1de1 [build] Use deploy key for doc publish (#3048)
d762215d1 [build] Add publish documentation script (#3040)
1fd09593c [examples] Add missing TestInit method to GettingStarted Example (#3039)
e45a0f6ce [examples] Add RomiGyro to the Romi Reference example (#3037)
94f852572 Update imaging link and fix typo (#3038)
d73cf64e5 [examples] Update RomiReference to match motor directions (#3036)
f945462ba Bump copyright year to 2021 (#3033)
b05946175 [wpimath] Catch Drake JNI exceptions and rethrow them (#3032)
62f0f8190 [wpimath] Deduplicate angle modulus functions (#2998)
bf8c0da4b [glass] Add "About" popup with version number (#3031)
dfdd6b389 [build] Increase Gradle heap size in Gazebo build (#3028)
f5e0fc3e9 Finish clang-tidy cleanups (#3003)
d741101fe [sim] Revert accidental commit of WSProvider_PDP.h (#3027)
e1620799c [examples] Add C++ RomiReference example (#2969)
749c7adb1 [command] Fix use-after-free in CommandScheduler (#3024)
921a73391 [sim] Add WS providers for AddressableLED, PCM, and Solenoid (#3026)
26d0004fe [build] Split Actions into different yml files (#3025)
948af6d5b [wpilib] PWMSpeedController.get(): Apply Inversion (#3016)
670a187a3 [wpilibc] SuppliedValueWidget.h: Forward declare ShuffleboardContainer (#3021)
be9f72502 [ntcore] NetworkTableValue: Use std::forward instead of std::move (#3022)
daf3f4cb1 [cscore] cscore_raw_cv.h: Fix error in PutFrame() (#3019)
5acda4cc7 [wpimath] ElevatorFeedforward.h: Add time.h include
8452af606 [wpimath] units/radiation.h: Add mass.h include
630d44952 [hal] ErrorsInternal.h: Add stdint.h include
7372cf7d9 [cscore] Windows NetworkUtil.cpp: Add missing include
b7e46c558 Include .h from .inc/.inl files (NFC) (#3017)
bf8f8710e [examples] Update Romi template and example (#2996)
6ffe5b775 [glass] Ensure NetworkTableTree parent context menu has an id (#3015)
be0805b85 [build] Update to WPILibVersioningPlugin 4.1.0 (#3014)
65b2359b2 [build] Add spotless for other files (#3007)
8651aa73e [examples] Enable NT Flush in Field2d examples (#3013)
78b542737 [build] Add Gazebo build to Actions CI (#3004)
fccf86532 [sim] DriverStationGui: Fix two bugs (#3010)
185741760 [sim] WSProvider_Joystick: Fix off-by-1 in incoming buttons (#3011)
ee7114a58 [glass] Add drive class widgets (#2975)
00fa91d0d [glass] Use ImGui style for gyro widget colors (#3009)
b7a25bfc3 ThirdPartyNotices: Add portable file dialogs license (#3005)
a2e46b9a1 [glass] modernize-use-nullptr (NFC) (#3006)
a751fa22d [build] Apply spotless for java formatting (#1768)
e563a0b7d [wpimath] Make LinearSystemLoop move-constructible and move-assignable (#2967)
49085ca94 [glass] Add context menus to remove and add NetworkTables values (#2979)
560a850a2 [glass] Add NetworkTables Log window (#2997)
66782e231 [sim] Create Left/Right drivetrain current accessors (#3001)
b60eb1544 clang-tidy: bugprone-virtual-near-miss
cbe59fa3b clang-tidy: google-explicit-constructor
c97c6dc06 clang-tidy: google-readability-casting (NFC)
32fa97d68 clang-tidy: modernize-use-nullptr (NFC)
aee460326 clang-tidy: modernize-pass-by-value
29c7da5f1 clang-tidy: modernize-make-unique
6131f4e32 clang-tidy: modernize-concat-nested-namespaces (NFC)
67e03e625 clang-tidy: modernize-use-equals-default
b124f9101 clang-tidy: modernize-use-default-member-init
d11a3a638 clang-tidy: modernize-use-override (NFC)
4cc0706b0 clang-tidy: modernize-use-using (NFC)
885f5a978 [wpilibc] Speed up ScopedTracerTest (#2999)
60b596457 [wpilibj] Fix typos (NFC) (#3000)
6e1919414 [build] Bring naming checkstyle rules up to date with Google Style guide (#1781)
8c8ec5e63 [wpilibj] Suppress unchecked cast warnings (#2995)
b8413ddd5 [wpiutil] Add noexcept to timestamp static functions (#2994)
5d976b6e1 [glass] Load NetworkTableView settings on first draw (#2993)
2b4317452 Replace NOLINT(runtime/explicit) comments with NOLINT (NFC) (#2992)
1c3011ba4 [glass] Fix handling of "/" NetworkTables key (#2991)
574a42f3b [hal] Fix UnsafeManipulateDIO status check (#2987)
9005cd59e [wpilib] Clamp input voltage in sim classes (#2955)
dd494d4ab [glass] NetworkTablesModel::Update(): Avoid use-after-move (#2988)
7cca469a1 [wpimath] NormalizeAngle: Make inline, remove unnamed namespace (#2986)
2aed432b4 Add braces to C++ single-line loops and conditionals (NFC) (#2973)
0291a3ff5 [wpiutil] StringRef: Add noexcept to several constructors (#2984)
5d7315280 [wpimath] Update UnitsTest.cpp copyright (#2985)
254931b9a [wpimath] Remove LinearSystem from LinearSystemLoop (#2968)
aa89744c9 Update OtherVersions.md to include wpimath info (#2983)
1cda3f5ad [glass] Fix styleguide (#2976)
8f1f64ffb Remove year from file copyright message (NFC) (#2972)
2bc0a7795 [examples] Fix wpiformat warning about utility include (#2971)
4204da6ad [glass] Add application icon
7ac39b10f [wpigui] Add icon support
6b567e006 [wpimath] Add support for varying vision standard deviations in pose estimators (#2956)
df299d6ed [wpimath] Add UnscentedKalmanFilter::Correct() overload (#2966)
4e34f0523 [examples] Use ADXRS450_GyroSim class in simulation example (#2964)
9962f6fd7 [wpilib] Give Field2d a default Sendable name (#2953)
f9d492f4b [sim] GUI: Show "Other Devices" window by default (#2961)
a8bb2ef1c [sim] Fix ADXRS450_GyroSim and DutyCycleEncoderSim (#2963)
240c629cd [sim] Try to guess "Map Gamepad" setting (#2960)
952567dd3 [wpilibc] Add missing move constructors and assignment operators (#2959)
10b396b4c [sim] Various WebSockets fixes and enhancements (#2952)
699bbe21a [examples] Fix comments in Gearsbot to match implementation (NFC) (#2957)
27b67deca [glass] Add more widgets (#2947)
581b7ec55 [wpilib] Add option to flush NetworkTables every iterative loop
acfbb1a44 [ntcore] DispatcherBase::Flush: Use wpi::Now()
d85a6d8fe [ntcore] Reduce limit on flush and update rate to 5 ms
20fbb5c63 [sim] Fix stringop truncation warning from GCC 10 (#2945)
1051a06a7 [glass] Show NT timestamps in seconds (#2944)
98dfc2620 [glass] Fix plots (#2943)
1ba0a2ced [sim] GUI: Add keyboard virtual joystick support (#2940)
4afb13f98 [examples] Replace M_PI with wpi::math::pi (#2938)
b27d33675 [examples] Enhance Romi templates (#2931)
00b9ae77f [sim] Change default WS port number to 3300 (#2932)
65219f309 [examples] Update Field2d position in periodic() (#2928)
f78d1d434 [sim] Process WS Encoder reset internally (#2927)
941edca59 [hal] Add Java SimDeviceDataJNI.getSimDeviceName (#2924)
a699435ed [wpilibj] Fix FlywheelSim argument order in constructor (#2922)
66d641718 [examples] Add tasks to run Java examples (#2920)
558e37c41 [examples] Add simple differential drive simulation example (#2918)
4f40d991e [glass] Switch name of Glass back to glass (#2919)
549af9900 [build] Update native-utils to 2021.0.6 (#2914)
b33693009 [glass] Change basename of glass to Glass (#2915)
c9a0edfb8 [glass] Package macOS application bundle
2c5668af4 [wpigui] Add platform-specific preferences save
751dea32a [wpilibc] Try to work around ABI break introduced in #2901 (#2917)
cd8f4bfb1 [build] Package up msvc runtime into maven artifact (#2913)
a6cfcc686 [wpilibc] Move SendableChooser Doxygen comments to header (NFC) (#2911)
b8c4f603d [wpimath] Upgrade to Eigen 3.3.9 (#2910)
0075e4b39 [wpilibj] Fix NPE in Field2d (#2909)
125af556c [simulation] Fix halsim_gui ntcore and wpiutil deps (#2908)
963ad5c25 [wpilib] Add noise to Differential Drive simulator (#2903)
387f56cb7 [examples] Add Romi reference Java example and templates (#2905)
b3deda38c [examples] Zero motors on disabledInit() in sim physics examples (#2906)
2a5ca7745 [glass] Add glass: an application for display of robot data
727940d84 [wpilib] Move Field2d to SmartDashboard
8cd42478e [wpilib] SendableBuilder: Make GetTable() visible
c11d34b26 [command] Use addCommands in command group templates (#2900)
339d7445b [sim] Add HAL hooks for simulationPeriodic (#2881)
d16f05f2c [wpilib] Fix SmartDashboard update order (#2896)
5427b32a4 [wpiutil] unique_function: Restrict implicit conversion (#2899)
f73701239 [ntcore] Add missing SetDefault initializer_list functions (#2898)
f5a6fc070 [sim] Add initialized flag for all solenoids on a PCM (#2897)
bdf5ba91a [wpilibj] Fix typo in ElevatorSim (#2895)
bc8f33877 [wpilib] Add pose estimators (#2867)
3413bfc06 [wpilib] PIDController: Recompute the error in AtSetpoint() (#2822)
2056f0ce0 [wpilib] Fix bugs in Hatchbot examples (#2893)
5eb8cfd69 [wpilibc] Fix MatchDataSender (#2892)
e6a425448 [build] Delete test folders after tests execute (#2891)
d478ad00d [imgui] Allow usage of imgui_stdlib (#2889)
53eda861d [build] Add unit-testing infrastructure to examples (#2863)
cc1d86ba6 [sim] Add title to simulator GUI window (#2888)
f0528f00e [build] CMake: Use project-specific binary and source dirs (#2886)
5cd2ad124 [wpilibc] Add Color::operator!= (#2887)
6c00e7a90 [build] CI CMake: build with GUI enabled (#2884)
53170bbb5 Update roboRIO toolchain installation instructions (#2883)
467258e05 [sim] GUI: Add option to not zero disconnected joysticks (#2876)
129be23c9 Clarify JDK installation instructions in readme (#2882)
8e9290e86 [build] Add separate CMake setting for wpimath (#2885)
7cf5bebf8 [wpilibj] Cache NT writes from DriverStation (#2780)
f7f9087fb [command] Fix timing issue in RamseteCommand (#2871)
256e7904f [wpilibj] SimDeviceSim: Fix sim value changed callback (#2880)
c8ea1b6c3 [wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)
2816b06c0 [sim] HAL_GetControlWord: Fully zero output (#2873)
4c695ea08 Add toolchain installation instructions to README (#2875)
a14d51806 [wpimath] DCMotor: fix doc typo (NFC) (#2868)
017097791 [build] CMake: build sim extensions as shared libs (#2866)
f61726b5a [build] Fix cmake-config files (#2865)
fc27fdac5 [wpilibc] Cache NT values from driver station (#2768)
47c59859e [sim] Make SimDevice callbacks synchronous (#2861)
6e76ab9c0 [build] Turn on WITH_GUI for Windows cmake CI
5f78b7670 [build] Set GLFW_INSTALL to OFF
5e0808c84 [wpigui] Fix Windows cmake build
508f05a47 [imgui] Fix typo in Windows CMake target sources
Change-Id: I1737b45965f31803a96676bedc7dc40e337aa321
git-subtree-dir: third_party/allwpilib
git-subtree-split: e473a00f9785f9949e5ced30901baeaf426d2fc9
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index 2065e26..c1786c3 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -1,57 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 "Eigen/Core"
#include "Eigen/LU"
#include "gtest/gtest.h"
-TEST(EigenTest, MultiplicationTest) {
- Eigen::Matrix<double, 2, 2> m1;
- m1 << 2, 1, 0, 1;
-
- Eigen::Matrix<double, 2, 2> m2;
- m2 << 3, 0, 0, 2.5;
+TEST(EigenTest, Multiplication) {
+ Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
+ Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
const auto result = m1 * m2;
- Eigen::Matrix<double, 2, 2> expectedResult;
- expectedResult << 6.0, 2.5, 0.0, 2.5;
+ Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
EXPECT_TRUE(expectedResult.isApprox(result));
- Eigen::Matrix<double, 2, 3> m3;
- m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
-
- Eigen::Matrix<double, 3, 4> m4;
- m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
+ Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
+ Eigen::Matrix<double, 3, 4> m4{
+ {3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
const auto result2 = m3 * m4;
- Eigen::Matrix<double, 2, 4> expectedResult2;
- expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
+ Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
+ {22.13, 9.82, 13.28, 23.53}};
EXPECT_TRUE(expectedResult2.isApprox(result2));
}
-TEST(EigenTest, TransposeTest) {
- Eigen::Matrix<double, 3, 1> vec;
- vec << 1, 2, 3;
+TEST(EigenTest, Transpose) {
+ Eigen::Vector<double, 3> vec{1, 2, 3};
const auto transpose = vec.transpose();
- Eigen::Matrix<double, 1, 3> expectedTranspose;
- expectedTranspose << 1, 2, 3;
+ Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
EXPECT_TRUE(expectedTranspose.isApprox(transpose));
}
-TEST(EigenTest, InverseTest) {
- Eigen::Matrix<double, 3, 3> mat;
- mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
+TEST(EigenTest, Inverse) {
+ Eigen::Matrix<double, 3, 3> 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
new file mode 100644
index 0000000..cd7ef5c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -0,0 +1,23 @@
+// 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 <fmt/format.h>
+
+#include "frc/fmt/Eigen.h"
+#include "frc/fmt/Units.h"
+#include "gtest/gtest.h"
+#include "units/velocity.h"
+
+TEST(FormatterTest, Eigen) {
+ Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
+ EXPECT_EQ(
+ " 1.000000 2.000000\n"
+ " 3.000000 4.000000\n"
+ " 5.000000 6.000000",
+ fmt::format("{}", A));
+}
+
+TEST(FormatterTest, Units) {
+ EXPECT_EQ("4 mps", fmt::format("{}", 4_mps));
+}
diff --git a/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp
deleted file mode 100644
index 934e14b..0000000
--- a/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h" // NOLINT(build/include_order)
-
-#include <cmath>
-#include <memory>
-#include <random>
-
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-#include "units/time.h"
-
-// Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr int32_t kMovAvgTaps = 6;
-
-enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
-
-std::ostream& operator<<(std::ostream& os,
- const LinearFilterNoiseTestType& type) {
- switch (type) {
- case TEST_SINGLE_POLE_IIR:
- os << "LinearFilter SinglePoleIIR";
- break;
- case TEST_MOVAVG:
- os << "LinearFilter MovingAverage";
- break;
- }
-
- return os;
-}
-
-static double GetData(double t) {
- return 100.0 * std::sin(2.0 * wpi::math::pi * t);
-}
-
-class LinearFilterNoiseTest
- : public testing::TestWithParam<LinearFilterNoiseTestType> {
- protected:
- std::unique_ptr<frc::LinearFilter<double>> m_filter;
-
- void SetUp() override {
- switch (GetParam()) {
- case TEST_SINGLE_POLE_IIR: {
- m_filter = std::make_unique<frc::LinearFilter<double>>(
- frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
- kFilterStep));
- break;
- }
-
- case TEST_MOVAVG: {
- m_filter = std::make_unique<frc::LinearFilter<double>>(
- frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
- break;
- }
- }
- }
-};
-
-/**
- * Test if the filter reduces the noise produced by a signal generator
- */
-TEST_P(LinearFilterNoiseTest, NoiseReduce) {
- double noiseGenError = 0.0;
- double filterError = 0.0;
-
- std::random_device rd;
- std::mt19937 gen{rd()};
- std::normal_distribution<double> distr{0.0, 10.0};
-
- for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
- double theory = GetData(t.to<double>());
- double noise = distr(gen);
- filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
- noiseGenError += std::abs(noise - theory);
- }
-
- RecordProperty("FilterError", filterError);
-
- // The filter should have produced values closer to the theory
- EXPECT_GT(noiseGenError, filterError)
- << "Filter should have reduced noise accumulation but failed";
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
- testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));
diff --git a/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp
deleted file mode 100644
index d321518..0000000
--- a/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h" // NOLINT(build/include_order)
-
-#include <cmath>
-#include <functional>
-#include <memory>
-#include <random>
-
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-#include "units/time.h"
-
-// Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
-static constexpr double kHighPassTimeConstant = 0.006631;
-static constexpr double kHighPassExpectedOutput = 10.074717;
-static constexpr int32_t kMovAvgTaps = 6;
-static constexpr double kMovAvgExpectedOutput = -10.191644;
-
-enum LinearFilterOutputTestType {
- TEST_SINGLE_POLE_IIR,
- TEST_HIGH_PASS,
- TEST_MOVAVG,
- TEST_PULSE
-};
-
-std::ostream& operator<<(std::ostream& os,
- const LinearFilterOutputTestType& type) {
- switch (type) {
- case TEST_SINGLE_POLE_IIR:
- os << "LinearFilter SinglePoleIIR";
- break;
- case TEST_HIGH_PASS:
- os << "LinearFilter HighPass";
- break;
- case TEST_MOVAVG:
- os << "LinearFilter MovingAverage";
- break;
- case TEST_PULSE:
- os << "LinearFilter Pulse";
- break;
- }
-
- return os;
-}
-
-static double GetData(double t) {
- return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
- 20.0 * std::cos(50.0 * wpi::math::pi * t);
-}
-
-static double GetPulseData(double t) {
- if (std::abs(t - 1.0) < 0.001) {
- return 1.0;
- } else {
- return 0.0;
- }
-}
-
-/**
- * A fixture that includes a consistent data source wrapped in a filter
- */
-class LinearFilterOutputTest
- : public testing::TestWithParam<LinearFilterOutputTestType> {
- protected:
- std::unique_ptr<frc::LinearFilter<double>> m_filter;
- std::function<double(double)> m_data;
- double m_expectedOutput = 0.0;
-
- void SetUp() override {
- switch (GetParam()) {
- case TEST_SINGLE_POLE_IIR: {
- m_filter = std::make_unique<frc::LinearFilter<double>>(
- frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
- kFilterStep));
- m_data = GetData;
- m_expectedOutput = kSinglePoleIIRExpectedOutput;
- break;
- }
-
- case TEST_HIGH_PASS: {
- m_filter = std::make_unique<frc::LinearFilter<double>>(
- frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
- kFilterStep));
- m_data = GetData;
- m_expectedOutput = kHighPassExpectedOutput;
- break;
- }
-
- case TEST_MOVAVG: {
- m_filter = std::make_unique<frc::LinearFilter<double>>(
- frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
- m_data = GetData;
- m_expectedOutput = kMovAvgExpectedOutput;
- break;
- }
-
- case TEST_PULSE: {
- m_filter = std::make_unique<frc::LinearFilter<double>>(
- frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
- m_data = GetPulseData;
- m_expectedOutput = 0.0;
- break;
- }
- }
- }
-};
-
-/**
- * Test if the linear filters produce consistent output for a given data set.
- */
-TEST_P(LinearFilterOutputTest, Output) {
- double filterOutput = 0.0;
- for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
- filterOutput = m_filter->Calculate(m_data(t.to<double>()));
- }
-
- RecordProperty("LinearFilterOutput", filterOutput);
-
- EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
- << "Filter output didn't match expected value";
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
- testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
- TEST_MOVAVG, TEST_PULSE));
diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
new file mode 100644
index 0000000..6b5af2b
--- /dev/null
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -0,0 +1,90 @@
+// 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/MathUtil.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+
+#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
+
+#define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c)
+
+TEST(MathUtilTest, ApplyDeadband) {
+ // < 0
+ EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
+ EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
+ frc::ApplyDeadband(-0.03, 0.02));
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02));
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02));
+
+ // == 0
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02));
+
+ // > 0
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02));
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02));
+ EXPECT_DOUBLE_EQ((0.03 - 0.02) / (1.0 - 0.02),
+ frc::ApplyDeadband(0.03, 0.02));
+ EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
+}
+
+TEST(MathUtilTest, InputModulus) {
+ // These tests check error wrapping. That is, the result of wrapping the
+ // result of an angle reference minus the measurement.
+
+ // Test symmetric range
+ EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
+ EXPECT_DOUBLE_EQ(-20.0,
+ frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
+ EXPECT_DOUBLE_EQ(-20.0,
+ frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
+ EXPECT_DOUBLE_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
+ EXPECT_DOUBLE_EQ(20.0,
+ frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
+ EXPECT_DOUBLE_EQ(20.0,
+ frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
+
+ // Test range starting at zero
+ EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
+ EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
+ EXPECT_DOUBLE_EQ(340.0,
+ frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
+
+ // Test asymmetric range that doesn't start at zero
+ EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
+
+ // Test range with both positive endpoints
+ EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
+ EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
+ EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
+ EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
+ EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
+
+ // Test all supported types
+ EXPECT_DOUBLE_EQ(-20.0,
+ frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
+ EXPECT_EQ(-20, frc::InputModulus<int>(170 - (-170), -170, 190));
+ EXPECT_EQ(-20_deg, frc::InputModulus<units::degree_t>(170_deg - (-170_deg),
+ -170_deg, 190_deg));
+}
+
+TEST(MathUtilTest, AngleModulus) {
+ EXPECT_UNITS_NEAR(
+ frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}),
+ units::radian_t{160 * wpi::numbers::pi / 180}, 1e-10);
+ EXPECT_UNITS_NEAR(
+ frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}),
+ units::radian_t{-2 * wpi::numbers::pi / 180}, 1e-10);
+ EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
+ 0_rad, 1e-10);
+
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
+ units::radian_t(wpi::numbers::pi));
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::numbers::pi)),
+ units::radian_t(wpi::numbers::pi));
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::numbers::pi / 2)),
+ units::radian_t(wpi::numbers::pi / 2));
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::numbers::pi / 2)),
+ units::radian_t(-wpi::numbers::pi / 2));
+}
diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index a1600fc..0dc3978 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -25,7 +22,7 @@
constexpr double kPositionStddev = 0.0001;
constexpr auto kDt = 0.00505_s;
-class StateSpace : public testing::Test {
+class StateSpaceTest : public testing::Test {
public:
LinearSystem<2, 1, 1> plant = [] {
auto motors = DCMotor::Vex775Pro(2);
@@ -46,24 +43,23 @@
LinearSystemLoop<2, 1, 1> loop{plant, controller, observer, 12_V, kDt};
};
-void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) {
- Eigen::Matrix<double, 1, 1> y =
- loop.Plant().CalculateY(loop.Xhat(), loop.U()) +
- Eigen::Matrix<double, 1, 1>(noise);
+void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
+ double noise) {
+ Eigen::Vector<double, 1> y =
+ plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
loop.Correct(y);
loop.Predict(kDt);
}
-TEST_F(StateSpace, CorrectPredictLoop) {
+TEST_F(StateSpaceTest, CorrectPredictLoop) {
std::default_random_engine generator;
std::normal_distribution<double> dist{0.0, kPositionStddev};
- Eigen::Matrix<double, 2, 1> references;
- references << 2.0, 0.0;
+ Eigen::Vector<double, 2> references{2.0, 0.0};
loop.SetNextR(references);
for (int i = 0; i < 1000; i++) {
- Update(loop, dist(generator));
+ Update(plant, loop, dist(generator));
EXPECT_PRED_FORMAT2(testing::DoubleLE, -12.0, loop.U(0));
EXPECT_PRED_FORMAT2(testing::DoubleLE, loop.U(0), 12.0);
}
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 24e9cf2..57b93bb 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -11,16 +8,16 @@
#include "Eigen/Core"
#include "frc/StateSpaceUtil.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
TEST(StateSpaceUtilTest, MakeMatrix) {
// Column vector
- Eigen::Matrix<double, 2, 1> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
+ Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
EXPECT_NEAR(mat1(0), 1.0, 1e-3);
EXPECT_NEAR(mat1(1), 2.0, 1e-3);
// Row vector
- Eigen::Matrix<double, 1, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
+ Eigen::RowVector<double, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
EXPECT_NEAR(mat2(0), 1.0, 1e-3);
EXPECT_NEAR(mat2(1), 2.0, 1e-3);
@@ -105,44 +102,59 @@
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
- Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
+ Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
- Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
+ Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, IsStabilizable) {
- Eigen::Matrix<double, 2, 2> A;
- Eigen::Matrix<double, 2, 1> B;
- B << 0, 1;
-
- // We separate the result of IsStabilizable from the assertion because
- // templates break gtest.
+ Eigen::Matrix<double, 2, 1> B{0, 1};
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
- A << 1.2, 0, 0, 0.5;
- bool ret = frc::IsStabilizable<2, 1>(A, B);
- EXPECT_FALSE(ret);
+ EXPECT_FALSE((frc::IsStabilizable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
- A << 1, 0, 0, 0.5;
- ret = frc::IsStabilizable<2, 1>(A, B);
- EXPECT_FALSE(ret);
+ EXPECT_FALSE((frc::IsStabilizable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
- A << 0.2, 0, 0, 0.5;
- ret = frc::IsStabilizable<2, 1>(A, B);
- EXPECT_TRUE(ret);
+ EXPECT_TRUE((frc::IsStabilizable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
- A << 0.2, 0, 0, 1.2;
- ret = frc::IsStabilizable<2, 1>(A, B);
- EXPECT_TRUE(ret);
+ EXPECT_TRUE((frc::IsStabilizable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
+}
+
+TEST(StateSpaceUtilTest, IsDetectable) {
+ Eigen::Matrix<double, 1, 2> C{0, 1};
+
+ // First eigenvalue is unobservable and unstable.
+ // Second eigenvalue is observable and stable.
+ EXPECT_FALSE((frc::IsDetectable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
+
+ // First eigenvalue is unobservable and marginally stable.
+ // Second eigenvalue is observable and stable.
+ EXPECT_FALSE((frc::IsDetectable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
+
+ // First eigenvalue is unobservable and stable.
+ // Second eigenvalue is observable and stable.
+ EXPECT_TRUE((frc::IsDetectable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
+
+ // First eigenvalue is unobservable and stable.
+ // Second eigenvalue is observable and unstable.
+ EXPECT_TRUE((frc::IsDetectable<2, 1>(
+ Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
}
diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index 54e8195..ec38a47 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <array>
#include <chrono>
@@ -96,66 +93,66 @@
class TypeTraits : public ::testing::Test {
protected:
- TypeTraits() {}
- virtual ~TypeTraits() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ TypeTraits() = default;
+ ~TypeTraits() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class UnitManipulators : public ::testing::Test {
protected:
- UnitManipulators() {}
- virtual ~UnitManipulators() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ UnitManipulators() = default;
+ ~UnitManipulators() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class UnitContainer : public ::testing::Test {
protected:
- UnitContainer() {}
- virtual ~UnitContainer() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ UnitContainer() = default;
+ ~UnitContainer() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class UnitConversion : public ::testing::Test {
protected:
- UnitConversion() {}
- virtual ~UnitConversion() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ UnitConversion() = default;
+ ~UnitConversion() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class UnitMath : public ::testing::Test {
protected:
- UnitMath() {}
- virtual ~UnitMath() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ UnitMath() = default;
+ ~UnitMath() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class CompileTimeArithmetic : public ::testing::Test {
protected:
- CompileTimeArithmetic() {}
- virtual ~CompileTimeArithmetic() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ CompileTimeArithmetic() = default;
+ ~CompileTimeArithmetic() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class Constexpr : public ::testing::Test {
protected:
- Constexpr() {}
- virtual ~Constexpr() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ Constexpr() = default;
+ ~Constexpr() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
};
class CaseStudies : public ::testing::Test {
protected:
- CaseStudies() {}
- virtual ~CaseStudies() {}
- virtual void SetUp() {}
- virtual void TearDown() {}
+ CaseStudies() = default;
+ ~CaseStudies() override = default;
+ void SetUp() override {}
+ void TearDown() override {}
struct RightTriangle {
using a = unit_value_t<meters, 3>;
@@ -1327,7 +1324,7 @@
}
TEST_F(UnitContainer, valueMethod) {
- double test = meter_t(3.0).to<double>();
+ double test = meter_t(3.0).value();
EXPECT_DOUBLE_EQ(3.0, test);
auto test2 = meter_t(4.0).value();
@@ -1336,11 +1333,11 @@
}
TEST_F(UnitContainer, convertMethod) {
- double test = meter_t(3.0).convert<feet>().to<double>();
+ double test = meter_t(3.0).convert<feet>().value();
EXPECT_NEAR(9.84252, test, 5.0e-6);
}
-#ifndef UNIT_LIB_DISABLE_IOSTREAM
+#ifdef UNIT_LIB_ENABLE_IOSTREAM
TEST_F(UnitContainer, cout) {
testing::internal::CaptureStdout();
std::cout << degree_t(349.87);
@@ -1421,6 +1418,88 @@
EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
#endif
}
+#endif
+
+TEST_F(UnitContainer, fmtlib) {
+ testing::internal::CaptureStdout();
+ fmt::print("{}", degree_t(349.87));
+ std::string output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("349.87 deg", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", meter_t(1.0));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("1 m", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", dB_t(31.0));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("31 dB", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", volt_t(21.79));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("21.79 V", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", dBW_t(12.0));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("12 dBW", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", dBm_t(120.0));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("120 dBm", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", miles_per_hour_t(72.1));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("72.1 mph", output.c_str());
+
+ // undefined unit
+ testing::internal::CaptureStdout();
+ fmt::print("{}", units::math::cpow<4>(meter_t(2)));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("16 m^4", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{}", units::math::cpow<3>(foot_t(2)));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("8 cu_ft", output.c_str());
+
+ testing::internal::CaptureStdout();
+ fmt::print("{:.9}", units::math::cpow<4>(foot_t(2)));
+ output = testing::internal::GetCapturedStdout();
+ EXPECT_STREQ("0.138095597 m^4", output.c_str());
+
+ // constants
+ testing::internal::CaptureStdout();
+ fmt::print("{:.8}", constants::k_B);
+ output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+ EXPECT_STREQ("1.3806485e-023 m^2 kg s^-2 K^-1", output.c_str());
+#else
+ EXPECT_STREQ("1.3806485e-23 m^2 kg s^-2 K^-1", output.c_str());
+#endif
+
+ testing::internal::CaptureStdout();
+ fmt::print("{:.9}", constants::mu_B);
+ output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+ EXPECT_STREQ("9.27400999e-024 m^2 A", output.c_str());
+#else
+ EXPECT_STREQ("9.27400999e-24 m^2 A", output.c_str());
+#endif
+
+ testing::internal::CaptureStdout();
+ fmt::print("{:.7}", constants::sigma);
+ output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+ EXPECT_STREQ("5.670367e-008 kg s^-3 K^-4", output.c_str());
+#else
+ EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
+#endif
+}
TEST_F(UnitContainer, to_string) {
foot_t a(3.5);
@@ -1479,18 +1558,17 @@
EXPECT_STREQ("m", b.abbreviation());
EXPECT_STREQ("meter", b.name());
}
-#endif
TEST_F(UnitContainer, negative) {
meter_t a(5.3);
meter_t b(-5.3);
- EXPECT_NEAR(a.to<double>(), -b.to<double>(), 5.0e-320);
- EXPECT_NEAR(b.to<double>(), -a.to<double>(), 5.0e-320);
+ EXPECT_NEAR(a.value(), -b.value(), 5.0e-320);
+ EXPECT_NEAR(b.value(), -a.value(), 5.0e-320);
dB_t c(2.87);
dB_t d(-2.87);
- EXPECT_NEAR(c.to<double>(), -d.to<double>(), 5.0e-320);
- EXPECT_NEAR(d.to<double>(), -c.to<double>(), 5.0e-320);
+ EXPECT_NEAR(c.value(), -d.value(), 5.0e-320);
+ EXPECT_NEAR(d.value(), -c.value(), 5.0e-320);
ppm_t e = -1 * ppm_t(10);
EXPECT_EQ(e, -ppm_t(10));
@@ -1501,7 +1579,7 @@
ppb_t a(ppm_t(1));
EXPECT_EQ(ppb_t(1000), a);
EXPECT_EQ(0.000001, a);
- EXPECT_EQ(0.000001, a.to<double>());
+ EXPECT_EQ(0.000001, a.value());
scalar_t b(ppm_t(1));
EXPECT_EQ(0.000001, b);
@@ -1711,7 +1789,7 @@
year_t twoYears(2.0);
week_t twoYearsInWeeks = twoYears;
- EXPECT_NEAR(week_t(104.286).to<double>(), twoYearsInWeeks.to<double>(),
+ EXPECT_NEAR(week_t(104.286).value(), twoYearsInWeeks.value(),
5.0e-4);
double test;
@@ -1748,8 +1826,8 @@
TEST_F(UnitConversion, angle) {
angle::degree_t quarterCircleDeg(90.0);
angle::radian_t quarterCircleRad = quarterCircleDeg;
- EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).to<double>(),
- quarterCircleRad.to<double>(), 5.0e-12);
+ EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).value(),
+ quarterCircleRad.value(), 5.0e-12);
double test;
@@ -2553,7 +2631,7 @@
EXPECT_TRUE(constants::pi < 4.0);
// explicit conversion
- EXPECT_NEAR(3.14159, constants::pi.to<double>(), 5.0e-6);
+ EXPECT_NEAR(3.14159, constants::pi.value(), 5.0e-6);
// auto multiplication
EXPECT_TRUE(
@@ -2562,16 +2640,16 @@
(std::is_same<meter_t, decltype(meter_t(1) * constants::pi)>::value));
EXPECT_NEAR(constants::detail::PI_VAL,
- (constants::pi * meter_t(1)).to<double>(), 5.0e-10);
+ (constants::pi * meter_t(1)).value(), 5.0e-10);
EXPECT_NEAR(constants::detail::PI_VAL,
- (meter_t(1) * constants::pi).to<double>(), 5.0e-10);
+ (meter_t(1) * constants::pi).value(), 5.0e-10);
// explicit multiplication
meter_t a = constants::pi * meter_t(1);
meter_t b = meter_t(1) * constants::pi;
- EXPECT_NEAR(constants::detail::PI_VAL, a.to<double>(), 5.0e-10);
- EXPECT_NEAR(constants::detail::PI_VAL, b.to<double>(), 5.0e-10);
+ EXPECT_NEAR(constants::detail::PI_VAL, a.value(), 5.0e-10);
+ EXPECT_NEAR(constants::detail::PI_VAL, b.value(), 5.0e-10);
// auto division
EXPECT_TRUE(
@@ -2580,16 +2658,16 @@
(std::is_same<second_t, decltype(second_t(1) / constants::pi)>::value));
EXPECT_NEAR(constants::detail::PI_VAL,
- (constants::pi / second_t(1)).to<double>(), 5.0e-10);
+ (constants::pi / second_t(1)).value(), 5.0e-10);
EXPECT_NEAR(1.0 / constants::detail::PI_VAL,
- (second_t(1) / constants::pi).to<double>(), 5.0e-10);
+ (second_t(1) / constants::pi).value(), 5.0e-10);
// explicit
hertz_t c = constants::pi / second_t(1);
second_t d = second_t(1) / constants::pi;
- EXPECT_NEAR(constants::detail::PI_VAL, c.to<double>(), 5.0e-10);
- EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.to<double>(), 5.0e-10);
+ EXPECT_NEAR(constants::detail::PI_VAL, c.value(), 5.0e-10);
+ EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.value(), 5.0e-10);
}
TEST_F(UnitConversion, constants) {
@@ -2696,12 +2774,12 @@
(std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(acos(scalar_t(0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(2).to<double>(),
- acos(scalar_t(-0.41614683654)).to<double>(), 5.0e-11);
+ EXPECT_NEAR(angle::radian_t(2).value(),
+ acos(scalar_t(-0.41614683654)).value(), 5.0e-11);
EXPECT_NEAR(
- angle::degree_t(135).to<double>(),
+ angle::degree_t(135).value(),
angle::degree_t(acos(scalar_t(-0.70710678118654752440084436210485)))
- .to<double>(),
+ .value(),
5.0e-12);
}
@@ -2710,12 +2788,12 @@
(std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(asin(scalar_t(0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(1.14159265).to<double>(),
- asin(scalar_t(0.90929742682)).to<double>(), 5.0e-9);
+ EXPECT_NEAR(angle::radian_t(1.14159265).value(),
+ asin(scalar_t(0.90929742682)).value(), 5.0e-9);
EXPECT_NEAR(
- angle::degree_t(45).to<double>(),
+ angle::degree_t(45).value(),
angle::degree_t(asin(scalar_t(0.70710678118654752440084436210485)))
- .to<double>(),
+ .value(),
5.0e-12);
}
@@ -2724,32 +2802,32 @@
(std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(atan(scalar_t(0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(-1.14159265).to<double>(),
- atan(scalar_t(-2.18503986326)).to<double>(), 5.0e-9);
- EXPECT_NEAR(angle::degree_t(-45).to<double>(),
- angle::degree_t(atan(scalar_t(-1.0))).to<double>(), 5.0e-12);
+ EXPECT_NEAR(angle::radian_t(-1.14159265).value(),
+ atan(scalar_t(-2.18503986326)).value(), 5.0e-9);
+ EXPECT_NEAR(angle::degree_t(-45).value(),
+ angle::degree_t(atan(scalar_t(-1.0))).value(), 5.0e-12);
}
TEST_F(UnitMath, atan2) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(atan2(
scalar_t(1), scalar_t(1)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).to<double>(),
- atan2(scalar_t(2), scalar_t(2)).to<double>(), 5.0e-12);
+ EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).value(),
+ atan2(scalar_t(2), scalar_t(2)).value(), 5.0e-12);
EXPECT_NEAR(
- angle::degree_t(45).to<double>(),
- angle::degree_t(atan2(scalar_t(2), scalar_t(2))).to<double>(),
+ angle::degree_t(45).value(),
+ angle::degree_t(atan2(scalar_t(2), scalar_t(2))).value(),
5.0e-12);
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(atan2(
scalar_t(1), scalar_t(1)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).to<double>(),
- atan2(scalar_t(1), scalar_t(std::sqrt(3))).to<double>(),
+ EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).value(),
+ atan2(scalar_t(1), scalar_t(std::sqrt(3))).value(),
5.0e-12);
- EXPECT_NEAR(angle::degree_t(30).to<double>(),
+ EXPECT_NEAR(angle::degree_t(30).value(),
angle::degree_t(atan2(scalar_t(1), scalar_t(std::sqrt(3))))
- .to<double>(),
+ .value(),
5.0e-12);
}
@@ -2781,30 +2859,30 @@
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
acosh(scalar_t(0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(1.316957896924817).to<double>(),
- acosh(scalar_t(2.0)).to<double>(), 5.0e-11);
- EXPECT_NEAR(angle::degree_t(75.456129290216893).to<double>(),
- angle::degree_t(acosh(scalar_t(2.0))).to<double>(), 5.0e-12);
+ EXPECT_NEAR(angle::radian_t(1.316957896924817).value(),
+ acosh(scalar_t(2.0)).value(), 5.0e-11);
+ EXPECT_NEAR(angle::degree_t(75.456129290216893).value(),
+ angle::degree_t(acosh(scalar_t(2.0))).value(), 5.0e-12);
}
TEST_F(UnitMath, asinh) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
asinh(scalar_t(0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(1.443635475178810).to<double>(),
- asinh(scalar_t(2)).to<double>(), 5.0e-9);
- EXPECT_NEAR(angle::degree_t(82.714219883108939).to<double>(),
- angle::degree_t(asinh(scalar_t(2))).to<double>(), 5.0e-12);
+ EXPECT_NEAR(angle::radian_t(1.443635475178810).value(),
+ asinh(scalar_t(2)).value(), 5.0e-9);
+ EXPECT_NEAR(angle::degree_t(82.714219883108939).value(),
+ angle::degree_t(asinh(scalar_t(2))).value(), 5.0e-12);
}
TEST_F(UnitMath, atanh) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
atanh(scalar_t(0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(0.549306144334055).to<double>(),
- atanh(scalar_t(0.5)).to<double>(), 5.0e-9);
- EXPECT_NEAR(angle::degree_t(31.472923730945389).to<double>(),
- angle::degree_t(atanh(scalar_t(0.5))).to<double>(), 5.0e-12);
+ EXPECT_NEAR(angle::radian_t(0.549306144334055).value(),
+ atanh(scalar_t(0.5)).value(), 5.0e-9);
+ EXPECT_NEAR(angle::degree_t(31.472923730945389).value(),
+ angle::degree_t(atanh(scalar_t(0.5))).value(), 5.0e-12);
}
TEST_F(UnitMath, exp) {
@@ -2876,14 +2954,14 @@
EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
typename std::decay<decltype(sqrt(
square_meter_t(4.0)))>::type>::value));
- EXPECT_NEAR(meter_t(2.0).to<double>(),
- sqrt(square_meter_t(4.0)).to<double>(), 5.0e-9);
+ EXPECT_NEAR(meter_t(2.0).value(),
+ sqrt(square_meter_t(4.0)).value(), 5.0e-9);
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
sqrt(steradian_t(16.0)))>::type>::value));
- EXPECT_NEAR(angle::radian_t(4.0).to<double>(),
- sqrt(steradian_t(16.0)).to<double>(), 5.0e-9);
+ EXPECT_NEAR(angle::radian_t(4.0).value(),
+ sqrt(steradian_t(16.0)).value(), 5.0e-9);
EXPECT_TRUE((std::is_convertible<typename std::decay<foot_t>::type,
typename std::decay<decltype(sqrt(
@@ -2892,9 +2970,9 @@
// for rational conversion (i.e. no integral root) let's check a bunch of
// different ways this could go wrong
foot_t resultFt = sqrt(square_foot_t(10.0));
- EXPECT_NEAR(foot_t(3.16227766017).to<double>(),
- sqrt(square_foot_t(10.0)).to<double>(), 5.0e-9);
- EXPECT_NEAR(foot_t(3.16227766017).to<double>(), resultFt.to<double>(),
+ EXPECT_NEAR(foot_t(3.16227766017).value(),
+ sqrt(square_foot_t(10.0)).value(), 5.0e-9);
+ EXPECT_NEAR(foot_t(3.16227766017).value(), resultFt.value(),
5.0e-9);
EXPECT_EQ(resultFt, sqrt(square_foot_t(10.0)));
}
@@ -2903,19 +2981,19 @@
EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
typename std::decay<decltype(hypot(
meter_t(3.0), meter_t(4.0)))>::type>::value));
- EXPECT_NEAR(meter_t(5.0).to<double>(),
- (hypot(meter_t(3.0), meter_t(4.0))).to<double>(), 5.0e-9);
+ EXPECT_NEAR(meter_t(5.0).value(),
+ (hypot(meter_t(3.0), meter_t(4.0))).value(), 5.0e-9);
EXPECT_TRUE((std::is_same<typename std::decay<foot_t>::type,
typename std::decay<decltype(hypot(
foot_t(3.0), meter_t(1.2192)))>::type>::value));
- EXPECT_NEAR(foot_t(5.0).to<double>(),
- (hypot(foot_t(3.0), meter_t(1.2192))).to<double>(), 5.0e-9);
+ EXPECT_NEAR(foot_t(5.0).value(),
+ (hypot(foot_t(3.0), meter_t(1.2192))).value(), 5.0e-9);
}
TEST_F(UnitMath, ceil) {
double val = 101.1;
- EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).to<double>());
+ EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).value());
EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
typename std::decay<decltype(
ceil(meter_t(val)))>::type>::value));
@@ -2928,7 +3006,7 @@
TEST_F(UnitMath, fmod) {
EXPECT_EQ(std::fmod(100.0, 101.2),
- fmod(meter_t(100.0), meter_t(101.2)).to<double>());
+ fmod(meter_t(100.0), meter_t(101.2)).value());
}
TEST_F(UnitMath, trunc) {
@@ -2951,8 +3029,8 @@
TEST_F(UnitMath, fdim) {
EXPECT_EQ(meter_t(0.0), fdim(meter_t(8.0), meter_t(10.0)));
EXPECT_EQ(meter_t(2.0), fdim(meter_t(10.0), meter_t(8.0)));
- EXPECT_NEAR(meter_t(9.3904).to<double>(),
- fdim(meter_t(10.0), foot_t(2.0)).to<double>(),
+ EXPECT_NEAR(meter_t(9.3904).value(),
+ fdim(meter_t(10.0), foot_t(2.0)).value(),
5.0e-320); // not sure why they aren't comparing exactly equal,
// but clearly they are.
}
@@ -2979,17 +3057,6 @@
EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0)));
}
-TEST_F(UnitMath, normalize) {
- EXPECT_EQ(NormalizeAngle(radian_t(5 * wpi::math::pi)),
- radian_t(wpi::math::pi));
- EXPECT_EQ(NormalizeAngle(radian_t(-5 * wpi::math::pi)),
- radian_t(wpi::math::pi));
- EXPECT_EQ(NormalizeAngle(radian_t(wpi::math::pi / 2)),
- radian_t(wpi::math::pi / 2));
- EXPECT_EQ(NormalizeAngle(radian_t(-wpi::math::pi / 2)),
- radian_t(-wpi::math::pi / 2));
-}
-
// Constexpr
#if !defined(_MSC_VER) || _MSC_VER > 1800
TEST_F(Constexpr, construction) {
@@ -3096,7 +3163,7 @@
}
TEST_F(CompileTimeArithmetic, is_unit_value_t) {
- typedef unit_value_t<meters, 3, 2> mRatio;
+ using mRatio = unit_value_t<meters, 3, 2>;
EXPECT_TRUE((traits::is_unit_value_t<mRatio>::value));
EXPECT_FALSE((traits::is_unit_value_t<meter_t>::value));
@@ -3108,7 +3175,7 @@
}
TEST_F(CompileTimeArithmetic, is_unit_value_t_category) {
- typedef unit_value_t<feet, 3, 2> mRatio;
+ using mRatio = unit_value_t<feet, 3, 2>;
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, mRatio>::value));
EXPECT_FALSE(
@@ -3120,90 +3187,90 @@
}
TEST_F(CompileTimeArithmetic, unit_value_add) {
- typedef unit_value_t<meters, 3, 2> mRatio;
+ using mRatio = unit_value_t<meters, 3, 2>;
using sum = unit_value_add<mRatio, mRatio>;
EXPECT_EQ(meter_t(3.0), sum::value());
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, sum>::value));
- typedef unit_value_t<feet, 1> ftRatio;
+ using ftRatio = unit_value_t<feet, 1>;
using sumf = unit_value_add<ftRatio, mRatio>;
EXPECT_TRUE((
std::is_same<typename std::decay<foot_t>::type,
typename std::decay<decltype(sumf::value())>::type>::value));
- EXPECT_NEAR(5.92125984, sumf::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(5.92125984, sumf::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, sumf>::value));
- typedef unit_value_t<celsius, 1> cRatio;
- typedef unit_value_t<fahrenheit, 2> fRatio;
+ using cRatio = unit_value_t<celsius, 1>;
+ using fRatio = unit_value_t<fahrenheit, 2>;
using sumc = unit_value_add<cRatio, fRatio>;
EXPECT_TRUE((
std::is_same<typename std::decay<celsius_t>::type,
typename std::decay<decltype(sumc::value())>::type>::value));
- EXPECT_NEAR(2.11111111111, sumc::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(2.11111111111, sumc::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
sumc>::value));
- typedef unit_value_t<angle::radian, 1> rRatio;
- typedef unit_value_t<angle::degree, 3> dRatio;
+ using rRatio = unit_value_t<angle::radian, 1>;
+ using dRatio = unit_value_t<angle::degree, 3>;
using sumr = unit_value_add<rRatio, dRatio>;
EXPECT_TRUE((
std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(sumr::value())>::type>::value));
- EXPECT_NEAR(1.05235988, sumr::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(1.05235988, sumr::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::angle_unit, sumr>::value));
}
TEST_F(CompileTimeArithmetic, unit_value_subtract) {
- typedef unit_value_t<meters, 3, 2> mRatio;
+ using mRatio = unit_value_t<meters, 3, 2>;
using diff = unit_value_subtract<mRatio, mRatio>;
EXPECT_EQ(meter_t(0), diff::value());
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, diff>::value));
- typedef unit_value_t<feet, 1> ftRatio;
+ using ftRatio = unit_value_t<feet, 1>;
using difff = unit_value_subtract<ftRatio, mRatio>;
EXPECT_TRUE((std::is_same<
typename std::decay<foot_t>::type,
typename std::decay<decltype(difff::value())>::type>::value));
- EXPECT_NEAR(-3.92125984, difff::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(-3.92125984, difff::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, difff>::value));
- typedef unit_value_t<celsius, 1> cRatio;
- typedef unit_value_t<fahrenheit, 2> fRatio;
+ using cRatio = unit_value_t<celsius, 1>;
+ using fRatio = unit_value_t<fahrenheit, 2>;
using diffc = unit_value_subtract<cRatio, fRatio>;
EXPECT_TRUE((std::is_same<
typename std::decay<celsius_t>::type,
typename std::decay<decltype(diffc::value())>::type>::value));
- EXPECT_NEAR(-0.11111111111, diffc::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(-0.11111111111, diffc::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
diffc>::value));
- typedef unit_value_t<angle::radian, 1> rRatio;
- typedef unit_value_t<angle::degree, 3> dRatio;
+ using rRatio = unit_value_t<angle::radian, 1>;
+ using dRatio = unit_value_t<angle::degree, 3>;
using diffr = unit_value_subtract<rRatio, dRatio>;
EXPECT_TRUE((std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(diffr::value())>::type>::value));
- EXPECT_NEAR(0.947640122, diffr::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(0.947640122, diffr::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::angle_unit, diffr>::value));
}
TEST_F(CompileTimeArithmetic, unit_value_multiply) {
- typedef unit_value_t<meters, 2> mRatio;
- typedef unit_value_t<feet, 656168, 100000> ftRatio; // 2 meter
+ using mRatio = unit_value_t<meters, 2>;
+ using ftRatio = unit_value_t<feet, 656168, 100000>; // 2 meter
using product = unit_value_multiply<mRatio, mRatio>;
EXPECT_EQ(square_meter_t(4), product::value());
@@ -3215,7 +3282,7 @@
EXPECT_TRUE((std::is_same<
typename std::decay<square_meter_t>::type,
typename std::decay<decltype(productM::value())>::type>::value));
- EXPECT_NEAR(4.0, productM::value().to<double>(), 5.0e-7);
+ EXPECT_NEAR(4.0, productM::value().value(), 5.0e-7);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::area_unit, productM>::value));
@@ -3223,7 +3290,7 @@
EXPECT_TRUE((std::is_same<
typename std::decay<square_foot_t>::type,
typename std::decay<decltype(productF::value())>::type>::value));
- EXPECT_NEAR(43.0556444224, productF::value().to<double>(), 5.0e-6);
+ EXPECT_NEAR(43.0556444224, productF::value().value(), 5.0e-6);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::area_unit, productF>::value));
@@ -3232,11 +3299,11 @@
(std::is_same<
typename std::decay<square_foot_t>::type,
typename std::decay<decltype(productF2::value())>::type>::value));
- EXPECT_NEAR(43.0556444224, productF2::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(43.0556444224, productF2::value().value(), 5.0e-8);
EXPECT_TRUE((
traits::is_unit_value_t_category<category::area_unit, productF2>::value));
- typedef unit_value_t<units::force::newton, 5> nRatio;
+ using nRatio = unit_value_t<units::force::newton, 5>;
using productN = unit_value_multiply<nRatio, ftRatio>;
EXPECT_FALSE(
@@ -3246,30 +3313,30 @@
EXPECT_TRUE((std::is_convertible<
typename std::decay<torque::newton_meter_t>::type,
typename std::decay<decltype(productN::value())>::type>::value));
- EXPECT_NEAR(32.8084, productN::value().to<double>(), 5.0e-8);
- EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().to<double>()),
+ EXPECT_NEAR(32.8084, productN::value().value(), 5.0e-8);
+ EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().value()),
5.0e-7);
EXPECT_TRUE((traits::is_unit_value_t_category<category::torque_unit,
productN>::value));
- typedef unit_value_t<angle::radian, 11, 10> r1Ratio;
- typedef unit_value_t<angle::radian, 22, 10> r2Ratio;
+ using r1Ratio = unit_value_t<angle::radian, 11, 10>;
+ using r2Ratio = unit_value_t<angle::radian, 22, 10>;
using productR = unit_value_multiply<r1Ratio, r2Ratio>;
EXPECT_TRUE((std::is_same<
typename std::decay<steradian_t>::type,
typename std::decay<decltype(productR::value())>::type>::value));
- EXPECT_NEAR(2.42, productR::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(2.42, productR::value().value(), 5.0e-8);
EXPECT_NEAR(7944.39137,
- (productR::value().convert<degrees_squared>().to<double>()),
+ (productR::value().convert<degrees_squared>().value()),
5.0e-6);
EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
productR>::value));
}
TEST_F(CompileTimeArithmetic, unit_value_divide) {
- typedef unit_value_t<meters, 2> mRatio;
- typedef unit_value_t<feet, 656168, 100000> ftRatio; // 2 meter
+ using mRatio = unit_value_t<meters, 2>;
+ using ftRatio = unit_value_t<feet, 656168, 100000>; // 2 meter
using product = unit_value_divide<mRatio, mRatio>;
EXPECT_EQ(scalar_t(1), product::value());
@@ -3281,7 +3348,7 @@
EXPECT_TRUE((std::is_same<
typename std::decay<scalar_t>::type,
typename std::decay<decltype(productM::value())>::type>::value));
- EXPECT_NEAR(1, productM::value().to<double>(), 5.0e-7);
+ EXPECT_NEAR(1, productM::value().value(), 5.0e-7);
EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
productM>::value));
@@ -3289,7 +3356,7 @@
EXPECT_TRUE((std::is_same<
typename std::decay<scalar_t>::type,
typename std::decay<decltype(productF::value())>::type>::value));
- EXPECT_NEAR(1.0, productF::value().to<double>(), 5.0e-6);
+ EXPECT_NEAR(1.0, productF::value().value(), 5.0e-6);
EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
productF>::value));
@@ -3298,90 +3365,90 @@
(std::is_same<
typename std::decay<scalar_t>::type,
typename std::decay<decltype(productF2::value())>::type>::value));
- EXPECT_NEAR(1.0, productF2::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(1.0, productF2::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
productF2>::value));
- typedef unit_value_t<seconds, 10> sRatio;
+ using sRatio = unit_value_t<seconds, 10>;
using productMS = unit_value_divide<mRatio, sRatio>;
EXPECT_TRUE(
(std::is_same<
typename std::decay<meters_per_second_t>::type,
typename std::decay<decltype(productMS::value())>::type>::value));
- EXPECT_NEAR(0.2, productMS::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(0.2, productMS::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::velocity_unit,
productMS>::value));
- typedef unit_value_t<angle::radian, 20> rRatio;
+ using rRatio = unit_value_t<angle::radian, 20>;
using productRS = unit_value_divide<rRatio, sRatio>;
EXPECT_TRUE(
(std::is_same<
typename std::decay<radians_per_second_t>::type,
typename std::decay<decltype(productRS::value())>::type>::value));
- EXPECT_NEAR(2, productRS::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(2, productRS::value().value(), 5.0e-8);
EXPECT_NEAR(114.592,
- (productRS::value().convert<degrees_per_second>().to<double>()),
+ (productRS::value().convert<degrees_per_second>().value()),
5.0e-4);
EXPECT_TRUE((traits::is_unit_value_t_category<category::angular_velocity_unit,
productRS>::value));
}
TEST_F(CompileTimeArithmetic, unit_value_power) {
- typedef unit_value_t<meters, 2> mRatio;
+ using mRatio = unit_value_t<meters, 2>;
using sq = unit_value_power<mRatio, 2>;
EXPECT_TRUE((std::is_convertible<
typename std::decay<square_meter_t>::type,
typename std::decay<decltype(sq::value())>::type>::value));
- EXPECT_NEAR(4, sq::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(4, sq::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::area_unit, sq>::value));
- typedef unit_value_t<angle::radian, 18, 10> rRatio;
+ using rRatio = unit_value_t<angle::radian, 18, 10>;
using sqr = unit_value_power<rRatio, 2>;
EXPECT_TRUE((std::is_convertible<
typename std::decay<steradian_t>::type,
typename std::decay<decltype(sqr::value())>::type>::value));
- EXPECT_NEAR(3.24, sqr::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(3.24, sqr::value().value(), 5.0e-8);
EXPECT_NEAR(10636.292574038049895092690529904,
- (sqr::value().convert<degrees_squared>().to<double>()), 5.0e-10);
+ (sqr::value().convert<degrees_squared>().value()), 5.0e-10);
EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
sqr>::value));
}
TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
- typedef unit_value_t<square_meters, 10> mRatio;
+ using mRatio = unit_value_t<square_meters, 10>;
using root = unit_value_sqrt<mRatio>;
EXPECT_TRUE((std::is_convertible<
typename std::decay<meter_t>::type,
typename std::decay<decltype(root::value())>::type>::value));
- EXPECT_NEAR(3.16227766017, root::value().to<double>(), 5.0e-9);
+ EXPECT_NEAR(3.16227766017, root::value().value(), 5.0e-9);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, root>::value));
- typedef unit_value_t<hectare, 51, 7> hRatio;
+ using hRatio = unit_value_t<hectare, 51, 7>;
using rooth = unit_value_sqrt<hRatio, 100000000>;
EXPECT_TRUE((std::is_convertible<
typename std::decay<mile_t>::type,
typename std::decay<decltype(rooth::value())>::type>::value));
- EXPECT_NEAR(2.69920623253, rooth::value().to<double>(), 5.0e-8);
- EXPECT_NEAR(269.920623, rooth::value().convert<meters>().to<double>(),
+ EXPECT_NEAR(2.69920623253, rooth::value().value(), 5.0e-8);
+ EXPECT_NEAR(269.920623, rooth::value().convert<meters>().value(),
5.0e-6);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, rooth>::value));
- typedef unit_value_t<steradian, 18, 10> rRatio;
+ using rRatio = unit_value_t<steradian, 18, 10>;
using rootr = unit_value_sqrt<rRatio>;
EXPECT_TRUE((traits::is_angle_unit<decltype(rootr::value())>::value));
- EXPECT_NEAR(1.3416407865, rootr::value().to<double>(), 5.0e-8);
+ EXPECT_NEAR(1.3416407865, rootr::value().value(), 5.0e-8);
EXPECT_NEAR(76.870352574,
- rootr::value().convert<angle::degrees>().to<double>(), 5.0e-6);
+ rootr::value().convert<angle::degrees>().value(), 5.0e-6);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::angle_unit, rootr>::value));
}
diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index 79f1a6d..354ed18 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -1,70 +1,52 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <cmath>
#include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
-Eigen::Matrix<double, 2, 1> Dynamics(const Eigen::Matrix<double, 2, 1>& x,
- const Eigen::Matrix<double, 1, 1>& u) {
- Eigen::Matrix<double, 2, 1> result;
-
- result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) +
- (frc::MakeMatrix<2, 1>(0.0, 1.0) * u);
-
- return result;
+Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
+ const Eigen::Vector<double, 1>& u) {
+ return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
+ Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
}
-Eigen::Matrix<double, 2, 1> StateDynamics(
- const Eigen::Matrix<double, 2, 1>& x) {
- Eigen::Matrix<double, 2, 1> result;
-
- result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x);
-
- return result;
+Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
+ return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
- std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
- const Eigen::Matrix<double, 1, 1>&)>
+ std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
+ const Eigen::Vector<double, 1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
- modelDynamics, units::second_t(0.02)};
+ modelDynamics, units::second_t{0.02}};
- Eigen::Matrix<double, 2, 1> r;
- r << 2, 2;
- Eigen::Matrix<double, 2, 1> nextR;
- nextR << 3, 3;
+ Eigen::Vector<double, 2> r{2, 2};
+ Eigen::Vector<double, 2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
- std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&)>
+ std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
modelDynamics = [](auto& x) { return StateDynamics(x); };
- Eigen::Matrix<double, 2, 1> B;
- B << 0, 1;
+ Eigen::Matrix<double, 2, 1> B{0, 1};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, B, units::second_t(0.02)};
- Eigen::Matrix<double, 2, 1> r;
- r << 2, 2;
- Eigen::Matrix<double, 2, 1> nextR;
- nextR << 3, 3;
+ Eigen::Vector<double, 2> r{2, 2};
+ Eigen::Vector<double, 2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
new file mode 100644
index 0000000..c6b669c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -0,0 +1,66 @@
+// 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 <wpi/numbers>
+
+#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"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+ 180.0};
+
+TEST(HolonomicDriveControllerTest, ReachesReference) {
+ frc::HolonomicDriveController controller{
+ frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+ frc::ProfiledPIDController<units::radian>{
+ 1.0, 0.0, 0.0,
+ frc::TrapezoidProfile<units::radian>::Constraints{
+ units::radians_per_second_t{2.0 * wpi::numbers::pi},
+ units::radians_per_second_squared_t{wpi::numbers::pi}}}};
+
+ frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+ auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+ frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.0_mps, 4.0_mps_sq});
+
+ constexpr auto kDt = 0.02_s;
+ auto totalTime = trajectory.TotalTime();
+ for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+ auto state = trajectory.Sample(kDt * i);
+ auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
+
+ robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
+ }
+
+ auto& endPose = trajectory.States().back().pose;
+ EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+ EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+ EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
+ kAngularTolerance);
+}
+
+TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
+ frc::HolonomicDriveController controller{
+ frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+ frc::ProfiledPIDController<units::radian>{
+ 1, 0, 0,
+ frc::TrapezoidProfile<units::radian>::Constraints{
+ 4_rad_per_s, 2_rad_per_s / 1_s}}};
+
+ frc::ChassisSpeeds speeds = controller.Calculate(
+ frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+
+ EXPECT_EQ(0, speeds.omega.value());
+}
diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index abc22d9..6e61706 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -16,19 +13,14 @@
namespace frc {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
- Eigen::Matrix<double, 2, 2> A;
- A << 1, 0, 0, 1;
-
- Eigen::Matrix<double, 2, 1> B;
- B << 0, 1;
+ Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
+ Eigen::Matrix<double, 2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
units::second_t(0.02)};
- Eigen::Matrix<double, 2, 1> r;
- r << 2, 2;
- Eigen::Matrix<double, 2, 1> nextR;
- nextR << 3, 3;
+ Eigen::Vector<double, 2> r{2, 2};
+ Eigen::Vector<double, 2> nextR{3, 3};
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index eecaf34..8c52cd0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -85,4 +82,27 @@
EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
}
+TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
+ LinearSystem<2, 1, 1> plant = [] {
+ auto motors = DCMotor::Vex775Pro(4);
+
+ // Carriage mass
+ constexpr auto m = 8_kg;
+
+ // Radius of pulley
+ constexpr auto r = 0.75_in;
+
+ // Gear ratio
+ constexpr double G = 14.67;
+
+ return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+ }();
+ LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};
+
+ controller.LatencyCompensate(plant, 0.02_s, 0.01_s);
+
+ EXPECT_NEAR(8.97115941, controller.K(0, 0), 1e-3);
+ EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
+}
+
} // namespace frc
diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
new file mode 100644
index 0000000..379db9e
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -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.
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+class PIDInputOutputTest : public testing::Test {
+ protected:
+ frc2::PIDController* controller;
+
+ void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+
+ void TearDown() override { delete controller; }
+};
+
+TEST_F(PIDInputOutputTest, ContinuousInput) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(-180, 180);
+ EXPECT_DOUBLE_EQ(controller->Calculate(-179, 179), -2);
+
+ controller->EnableContinuousInput(0, 360);
+ EXPECT_DOUBLE_EQ(controller->Calculate(1, 359), -2);
+}
+
+TEST_F(PIDInputOutputTest, ProportionalGainOutput) {
+ controller->SetP(4);
+
+ EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
+}
+
+TEST_F(PIDInputOutputTest, IntegralGainOutput) {
+ controller->SetI(4);
+
+ double out = 0;
+
+ for (int i = 0; i < 5; i++) {
+ out = controller->Calculate(0.025, 0);
+ }
+
+ EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out);
+}
+
+TEST_F(PIDInputOutputTest, DerivativeGainOutput) {
+ controller->SetD(4);
+
+ controller->Calculate(0, 0);
+
+ EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+ controller->Calculate(0.0025, 0));
+}
diff --git a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
new file mode 100644
index 0000000..0aec438
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -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.
+
+#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};
+ controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+ EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST(PIDToleranceTest, AbsoluteTolerance) {
+ frc2::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();
+
+ controller.SetTolerance(kTolerance);
+ controller.SetSetpoint(kSetpoint);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(0.0);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(kSetpoint + kTolerance / 2);
+
+ EXPECT_TRUE(controller.AtSetpoint())
+ << "Error was not in tolerance when it should have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(kSetpoint + 10 * kTolerance);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+}
diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
new file mode 100644
index 0000000..da402ae
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -0,0 +1,117 @@
+// 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 <wpi/numbers>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+
+class ProfiledPIDInputOutputTest : public testing::Test {
+ protected:
+ frc::ProfiledPIDController<units::degrees>* controller;
+
+ void SetUp() override {
+ controller = new frc::ProfiledPIDController<units::degrees>(
+ 0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq});
+ }
+
+ void TearDown() override { delete controller; }
+};
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInput1) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(-180_deg, 180_deg);
+
+ static constexpr units::degree_t kSetpoint{-179.0};
+ static constexpr units::degree_t kMeasurement{-179.0};
+ static constexpr units::degree_t kGoal{179.0};
+
+ controller->Reset(kSetpoint);
+ EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+ // Error must be less than half the input range at all times
+ EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+ 180_deg);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInput2) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
+ units::radian_t{wpi::numbers::pi});
+
+ static constexpr units::radian_t kSetpoint{-3.4826633343199735};
+ static constexpr units::radian_t kMeasurement{-3.1352207333939606};
+ static constexpr units::radian_t kGoal{-3.534162788601621};
+
+ controller->Reset(kSetpoint);
+ EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+ // Error must be less than half the input range at all times
+ EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+ units::radian_t{wpi::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInput3) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
+ units::radian_t{wpi::numbers::pi});
+
+ static constexpr units::radian_t kSetpoint{-3.5176604690006377};
+ static constexpr units::radian_t kMeasurement{3.1191729343822456};
+ static constexpr units::radian_t kGoal{2.709680418117445};
+
+ controller->Reset(kSetpoint);
+ EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+ // Error must be less than half the input range at all times
+ EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+ units::radian_t{wpi::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInput4) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(0_rad,
+ units::radian_t{2.0 * wpi::numbers::pi});
+
+ static constexpr units::radian_t kSetpoint{2.78};
+ static constexpr units::radian_t kMeasurement{3.12};
+ static constexpr units::radian_t kGoal{2.71};
+
+ controller->Reset(kSetpoint);
+ EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+ // Error must be less than half the input range at all times
+ EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+ units::radian_t{wpi::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
+ controller->SetP(4);
+
+ EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutput) {
+ controller->SetI(4);
+
+ double out = 0;
+
+ for (int i = 0; i < 5; i++) {
+ out = controller->Calculate(0.025_deg, 0_deg);
+ }
+
+ EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutput) {
+ controller->SetD(4);
+
+ controller->Calculate(0_deg, 0_deg);
+
+ EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+ controller->Calculate(0.0025_deg, 0_deg));
+}
diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
new file mode 100644
index 0000000..5e297f4
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -0,0 +1,43 @@
+// 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/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) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+ 180.0};
+
+TEST(RamseteControllerTest, ReachesReference) {
+ frc::RamseteController controller{2.0, 0.7};
+ frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+ auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+ frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ constexpr auto kDt = 0.02_s;
+ auto totalTime = trajectory.TotalTime();
+ for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+ auto state = trajectory.Sample(kDt * i);
+ auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+ static_cast<void>(vy);
+
+ robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
+ }
+
+ auto& endPose = trajectory.States().back().pose;
+ EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+ EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+ EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+ robotPose.Rotation().Radians()),
+ 0_rad, kAngularTolerance);
+}
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
new file mode 100644
index 0000000..3cf944e
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -0,0 +1,46 @@
+// 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 <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(SimpleMotorFeedforwardTest, Calculate) {
+ double Ks = 0.5;
+ double Kv = 3.0;
+ double Ka = 0.6;
+ auto dt = 0.02_s;
+
+ Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
+ Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
+
+ frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+ frc::SimpleMotorFeedforward<units::meter> simpleMotor{
+ units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
+ units::volt_t{Ka} / 1_mps_sq};
+
+ Eigen::Vector<double, 1> r{2.0};
+ Eigen::Vector<double, 1> nextR{3.0};
+
+ EXPECT_NEAR(37.524995834325161 + Ks,
+ simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+ EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
+ simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+
+ // These won't match exactly. It's just an approximation to make sure they're
+ // in the same ballpark.
+ EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
+ simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0);
+}
+
+} // namespace frc
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
new file mode 100644
index 0000000..8631d6f
--- /dev/null
+++ b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
@@ -0,0 +1,124 @@
+#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/drake/math/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp
deleted file mode 100644
index edcb772..0000000
--- a/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Core>
-#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.
- MatrixXd Y = A.transpose() * X * A - X -
- A.transpose() * X * B * (B.transpose() * X * B + R).inverse() *
- B.transpose() * X * A +
- Q;
- 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);
- // 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);
- // 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);
- // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
- int n4 = 2, m4 = 2;
- MatrixXd A4(n4, n4), B4(n4, m4), Q4(n4, n4), R4(m4, m4);
- A4 << 1, 0, 0, 1;
- B4 << 1, 0, 0, 1;
- Q4 << 1, 0, 0, 1;
- R4 << 1, 0, 0, 1;
- SolveDAREandVerify(A4, B4, Q4, R4);
-}
-} // 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
new file mode 100644
index 0000000..ee1da7f
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include <wpi/numbers>
+
+#include "Eigen/Core"
+#include "frc/estimator/AngleStatistics.h"
+
+TEST(AngleStatisticsTest, Mean) {
+ Eigen::Matrix<double, 3, 3> sigmas{
+ {1, 1.2, 0},
+ {359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
+ {1, 2, 0}};
+ // Weights need to produce the mean of the sigmas
+ Eigen::Vector3d weights;
+ weights.fill(1.0 / sigmas.cols());
+
+ EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
+ .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
+}
+
+TEST(AngleStatisticsTest, Residual) {
+ Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
+ Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+
+ EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
+ Eigen::Vector3d{0, 2 * wpi::numbers::pi / 180, 1}));
+}
+
+TEST(AngleStatisticsTest, Add) {
+ Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
+ Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+
+ EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
+}
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
new file mode 100644
index 0000000..4a854fd
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -0,0 +1,98 @@
+// 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 <limits>
+#include <random>
+
+#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/kinematics/DifferentialDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+
+TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
+ frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
+ frc::Pose2d(),
+ {0.02, 0.02, 0.01, 0.02, 0.02},
+ {0.01, 0.01, 0.001},
+ {0.1, 0.1, 0.01}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+ frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+ frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
+
+ frc::DifferentialDriveKinematics kinematics{1.0_m};
+ frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0.0_s;
+
+ units::meter_t leftDistance = 0_m;
+ units::meter_t rightDistance = 0_m;
+
+ units::second_t kVisionUpdateRate = 0.1_s;
+ frc::Pose2d lastVisionPose;
+ units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t <= trajectory.TotalTime()) {
+ auto groundTruthState = trajectory.Sample(t);
+ auto input = kinematics.ToWheelSpeeds(
+ {groundTruthState.velocity, 0_mps,
+ groundTruthState.velocity * groundTruthState.curvature});
+
+ if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+ if (lastVisionPose != frc::Pose2d()) {
+ estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+ }
+ lastVisionPose =
+ groundTruthState.pose +
+ frc::Transform2d(
+ frc::Translation2d(distribution(generator) * 0.1 * 1_m,
+ distribution(generator) * 0.1 * 1_m),
+ frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
+
+ lastVisionUpdateTime = t;
+ }
+
+ leftDistance += input.left * distribution(generator) * 0.01 * dt;
+ rightDistance += input.right * distribution(generator) * 0.01 * dt;
+
+ auto xhat = estimator.UpdateWithTime(
+ t,
+ groundTruthState.pose.Rotation() +
+ frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
+ input, leftDistance, rightDistance);
+
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
+ 0.2);
+ EXPECT_NEAR(0.0, maxError, 0.4);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 387593b..6d51185 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -21,8 +18,8 @@
namespace {
-Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
+ const Eigen::Vector<double, 2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -43,36 +40,26 @@
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
- Eigen::Matrix<double, 5, 1> result;
auto v = 0.5 * (vl + vr);
- result(0) = v.to<double>() * std::cos(x(2));
- result(1) = v.to<double>() * std::sin(x(2));
- result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
- result(3) =
- k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
- k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
- result(4) =
- k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
- k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
- return result;
+ return Eigen::Vector<double, 5>{
+ v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
+ ((vr - vl) / (2.0 * rb)).value(),
+ k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+ k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
+ k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+ k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
-Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
- const Eigen::Matrix<double, 5, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> LocalMeasurementModel(
+ const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
- Eigen::Matrix<double, 3, 1> y;
- y << x(2), x(3), x(4);
- return y;
+ return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
}
-Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
- const Eigen::Matrix<double, 5, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> GlobalMeasurementModel(
+ const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
- Eigen::Matrix<double, 5, 1> y;
- y << x(0), x(1), x(2), x(3), x(4);
- return y;
+ return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -84,8 +71,7 @@
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
- Eigen::Matrix<double, 2, 1> u;
- u << 12.0, 12.0;
+ Eigen::Vector<double, 2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -112,41 +98,37 @@
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
- Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+ Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
+ Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
- Eigen::Matrix<double, 5, 1> nextR;
- Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+ auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
+ Eigen::Vector<double, 5>::Zero(),
+ Eigen::Vector<double, 2>::Zero());
- auto B = frc::NumericalJacobianU<5, 5, 2>(
- Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
- Eigen::Matrix<double, 2, 1>::Zero());
-
- observer.SetXhat(frc::MakeMatrix<5, 1>(
- trajectory.InitialPose().Translation().X().to<double>(),
- trajectory.InitialPose().Translation().Y().to<double>(),
- trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+ observer.SetXhat(Eigen::Vector<double, 5>{
+ trajectory.InitialPose().Translation().X().value(),
+ trajectory.InitialPose().Translation().Y().value(),
+ trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
auto totalTime = trajectory.TotalTime();
- for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
+ for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
- ref.velocity * (1 - (ref.curvature * rb).to<double>());
+ ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
- ref.velocity * (1 + (ref.curvature * rb).to<double>());
+ ref.velocity * (1 + (ref.curvature * rb).value());
- nextR(0) = ref.pose.Translation().X().to<double>();
- nextR(1) = ref.pose.Translation().Y().to<double>();
- nextR(2) = ref.pose.Rotation().Radians().to<double>();
- nextR(3) = vl.to<double>();
- nextR(4) = vr.to<double>();
+ Eigen::Vector<double, 5> nextR{
+ ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
+ ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY =
- LocalMeasurementModel(nextR, Eigen::Matrix<double, 2, 1>::Zero());
+ LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
- Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
- u = B.householderQr().solve(
- rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+ Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
+ u = B.householderQr().solve(rdot -
+ Dynamics(r, Eigen::Vector<double, 2>::Zero()));
observer.Predict(u, dt);
@@ -161,12 +143,12 @@
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
- ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
- observer.Xhat(0), 1.0);
- ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
- observer.Xhat(1), 1.0);
- ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
- observer.Xhat(2), 1.0);
+ ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+ 1.0);
+ ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+ 1.0);
+ ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+ 1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
}
diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
index 53c54ef..fc373ca 100644
--- a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
new file mode 100644
index 0000000..881d4e8
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -0,0 +1,89 @@
+// 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 <limits>
+#include <random>
+
+#include "frc/estimator/MecanumDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
+ 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{
+ frc::Rotation2d(), frc::Pose2d(), kinematics,
+ {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
+
+ frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+ frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+ frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0_s;
+
+ units::second_t kVisionUpdateRate = 0.1_s;
+ frc::Pose2d lastVisionPose;
+ units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+ std::vector<frc::Pose2d> visionPoses;
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t < trajectory.TotalTime()) {
+ frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+ if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+ if (lastVisionPose != frc::Pose2d()) {
+ estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+ }
+ lastVisionPose =
+ groundTruthState.pose +
+ frc::Transform2d(
+ frc::Translation2d(distribution(generator) * 0.1_m,
+ distribution(generator) * 0.1_m),
+ frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
+ visionPoses.push_back(lastVisionPose);
+ lastVisionUpdateTime = t;
+ }
+
+ auto wheelSpeeds = kinematics.ToWheelSpeeds(
+ {groundTruthState.velocity, 0_mps,
+ groundTruthState.velocity * groundTruthState.curvature});
+
+ auto xhat = estimator.UpdateWithTime(
+ t,
+ groundTruthState.pose.Rotation() +
+ frc::Rotation2d(distribution(generator) * 0.05_rad),
+ wheelSpeeds);
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
+ EXPECT_LT(maxError, 0.4);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index ace5e79..c012435 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -1,41 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
-namespace drake {
-namespace math {
+namespace drake::math {
namespace {
-TEST(MerweScaledSigmaPointsTest, TestZeroMean) {
+TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
- auto points =
- sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0),
- frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0));
+ auto points = sigmaPoints.SigmaPoints(
+ Eigen::Vector<double, 2>{0.0, 0.0},
+ Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
- (points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0,
- 0.0, 0.0, 0.00173205, 0.0, -0.00173205))
+ (points -
+ Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
+ {0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
-TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) {
+TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
- auto points =
- sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0),
- frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0));
+ auto points = sigmaPoints.SigmaPoints(
+ Eigen::Vector<double, 2>{1.0, 2.0},
+ Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
EXPECT_TRUE(
- (points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0,
- 2.0, 2.00548, 2.0, 1.99452))
+ (points -
+ Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
+ {2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}
} // namespace
-} // namespace math
-} // namespace drake
+} // namespace drake::math
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
new file mode 100644
index 0000000..ee01f6f
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -0,0 +1,89 @@
+// 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 <limits>
+#include <random>
+
+#include "frc/estimator/SwerveDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
+ 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::SwerveDrivePoseEstimator<4> estimator{
+ frc::Rotation2d(), frc::Pose2d(), kinematics,
+ {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
+
+ frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+ frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+ frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0_s;
+
+ units::second_t kVisionUpdateRate = 0.1_s;
+ frc::Pose2d lastVisionPose;
+ units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+ std::vector<frc::Pose2d> visionPoses;
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t < trajectory.TotalTime()) {
+ frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+ if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+ if (lastVisionPose != frc::Pose2d()) {
+ estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+ }
+ lastVisionPose =
+ groundTruthState.pose +
+ frc::Transform2d(
+ frc::Translation2d(distribution(generator) * 0.1_m,
+ distribution(generator) * 0.1_m),
+ frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
+ visionPoses.push_back(lastVisionPose);
+ lastVisionUpdateTime = t;
+ }
+
+ auto moduleStates = kinematics.ToSwerveModuleStates(
+ {groundTruthState.velocity, 0_mps,
+ groundTruthState.velocity * groundTruthState.curvature});
+
+ auto xhat = estimator.UpdateWithTime(
+ t,
+ groundTruthState.pose.Rotation() +
+ frc::Rotation2d(distribution(generator) * 0.05_rad),
+ moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
+ EXPECT_LT(maxError, 0.4);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 0017b42..9665442 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -13,17 +10,18 @@
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
namespace {
-Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
+ const Eigen::Vector<double, 2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -44,49 +42,38 @@
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
- Eigen::Matrix<double, 5, 1> result;
auto v = 0.5 * (vl + vr);
- result(0) = v.to<double>() * std::cos(x(2));
- result(1) = v.to<double>() * std::sin(x(2));
- result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
- result(3) =
- k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
- k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
- result(4) =
- k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
- k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
- return result;
+ return Eigen::Vector<double, 5>{
+ v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
+ ((vr - vl) / (2.0 * rb)).value(),
+ k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+ k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
+ k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+ k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
-Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
- const Eigen::Matrix<double, 5, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> LocalMeasurementModel(
+ const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
- Eigen::Matrix<double, 3, 1> y;
- y << x(2), x(3), x(4);
- return y;
+ return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
}
-Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
- const Eigen::Matrix<double, 5, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> GlobalMeasurementModel(
+ const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
- Eigen::Matrix<double, 5, 1> y;
- y << x(0), x(1), x(2), x(3), x(4);
- return y;
+ return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
TEST(UnscentedKalmanFilterTest, Init) {
- constexpr auto dt = 0.00505_s;
+ constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
- Eigen::Matrix<double, 2, 1> u;
- u << 12.0, 12.0;
+ Eigen::Vector<double, 2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -94,11 +81,13 @@
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
- observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+ observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+ frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
+ frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(UnscentedKalmanFilterTest, Convergence) {
- constexpr auto dt = 0.00505_s;
+ constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
@@ -113,48 +102,44 @@
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
- Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+ Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
+ Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
- Eigen::Matrix<double, 5, 1> nextR;
- Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+ auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
+ Eigen::Vector<double, 5>::Zero(),
+ Eigen::Vector<double, 2>::Zero());
- auto B = frc::NumericalJacobianU<5, 5, 2>(
- Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
- Eigen::Matrix<double, 2, 1>::Zero());
-
- observer.SetXhat(frc::MakeMatrix<5, 1>(
- trajectory.InitialPose().Translation().X().to<double>(),
- trajectory.InitialPose().Translation().Y().to<double>(),
- trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+ observer.SetXhat(Eigen::Vector<double, 5>{
+ trajectory.InitialPose().Translation().X().value(),
+ trajectory.InitialPose().Translation().Y().value(),
+ trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
auto trueXhat = observer.Xhat();
auto totalTime = trajectory.TotalTime();
- for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
+ for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
- ref.velocity * (1 - (ref.curvature * rb).to<double>());
+ ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
- ref.velocity * (1 + (ref.curvature * rb).to<double>());
+ ref.velocity * (1 + (ref.curvature * rb).value());
- nextR(0) = ref.pose.Translation().X().to<double>();
- nextR(1) = ref.pose.Translation().Y().to<double>();
- nextR(2) = ref.pose.Rotation().Radians().to<double>();
- nextR(3) = vl.to<double>();
- nextR(4) = vr.to<double>();
+ Eigen::Vector<double, 5> nextR{
+ ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
+ ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY =
- LocalMeasurementModel(trueXhat, Eigen::Matrix<double, 2, 1>::Zero());
+ LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
- Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
- u = B.householderQr().solve(
- rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+ Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
+ u = B.householderQr().solve(rdot -
+ Dynamics(r, Eigen::Vector<double, 2>::Zero()));
observer.Predict(u, dt);
r = nextR;
- trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt);
+ trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
}
auto localY = LocalMeasurementModel(trueXhat, u);
@@ -162,15 +147,19 @@
auto globalY = GlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
- observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+ observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+ frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
+ frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
+
+ );
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
- ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
- observer.Xhat(0), 1.0);
- ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
- observer.Xhat(1), 1.0);
- ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
- observer.Xhat(2), 1.0);
+ ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+ 1.0);
+ ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+ 1.0);
+ ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+ 1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
}
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
new file mode 100644
index 0000000..5ccd829
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
@@ -0,0 +1,69 @@
+// 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/filter/LinearFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <random>
+
+#include <wpi/numbers>
+
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+// Filter constants
+static constexpr auto kFilterStep = 5_ms;
+static constexpr auto kFilterTime = 2_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr int32_t kMovAvgTaps = 6;
+
+enum LinearFilterNoiseTestType { kTestSinglePoleIIR, kTestMovAvg };
+
+static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * wpi::numbers::pi * t);
+}
+
+class LinearFilterNoiseTest
+ : public testing::TestWithParam<LinearFilterNoiseTestType> {
+ protected:
+ frc::LinearFilter<double> m_filter = [=] {
+ switch (GetParam()) {
+ case kTestSinglePoleIIR:
+ return frc::LinearFilter<double>::SinglePoleIIR(
+ kSinglePoleIIRTimeConstant, kFilterStep);
+ break;
+ default:
+ return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+ break;
+ }
+ }();
+};
+
+/**
+ * Test if the filter reduces the noise produced by a signal generator
+ */
+TEST_P(LinearFilterNoiseTest, NoiseReduce) {
+ double noiseGenError = 0.0;
+ double filterError = 0.0;
+
+ std::random_device rd;
+ std::mt19937 gen{rd()};
+ std::normal_distribution<double> distr{0.0, 10.0};
+
+ for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+ double theory = GetData(t.value());
+ double noise = distr(gen);
+ filterError += std::abs(m_filter.Calculate(theory + noise) - theory);
+ noiseGenError += std::abs(noise - theory);
+ }
+
+ RecordProperty("FilterError", filterError);
+
+ // The filter should have produced values closer to the theory
+ EXPECT_GT(noiseGenError, filterError)
+ << "Filter should have reduced noise accumulation but failed";
+}
+
+INSTANTIATE_TEST_SUITE_P(Tests, LinearFilterNoiseTest,
+ testing::Values(kTestSinglePoleIIR, kTestMovAvg));
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
new file mode 100644
index 0000000..bca3f9d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -0,0 +1,214 @@
+// 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/filter/LinearFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+
+#include <wpi/numbers>
+
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+// Filter constants
+static constexpr auto kFilterStep = 5_ms;
+static constexpr auto kFilterTime = 2_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
+static constexpr double kHighPassTimeConstant = 0.006631;
+static constexpr double kHighPassExpectedOutput = 10.074717;
+static constexpr int32_t kMovAvgTaps = 6;
+static constexpr double kMovAvgExpectedOutput = -10.191644;
+
+enum LinearFilterOutputTestType {
+ kTestSinglePoleIIR,
+ kTestHighPass,
+ kTestMovAvg,
+ kTestPulse
+};
+
+static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * wpi::numbers::pi * t) +
+ 20.0 * std::cos(50.0 * wpi::numbers::pi * t);
+}
+
+static double GetPulseData(double t) {
+ if (std::abs(t - 1.0) < 0.001) {
+ return 1.0;
+ } else {
+ return 0.0;
+ }
+}
+
+/**
+ * A fixture that includes a consistent data source wrapped in a filter
+ */
+class LinearFilterOutputTest
+ : public testing::TestWithParam<LinearFilterOutputTestType> {
+ protected:
+ frc::LinearFilter<double> m_filter = [=] {
+ switch (GetParam()) {
+ case kTestSinglePoleIIR:
+ return frc::LinearFilter<double>::SinglePoleIIR(
+ kSinglePoleIIRTimeConstant, kFilterStep);
+ break;
+ case kTestHighPass:
+ return frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+ kFilterStep);
+ break;
+ case kTestMovAvg:
+ return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+ break;
+ default:
+ return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+ break;
+ }
+ }();
+ std::function<double(double)> m_data;
+ double m_expectedOutput = 0.0;
+
+ LinearFilterOutputTest() {
+ switch (GetParam()) {
+ case kTestSinglePoleIIR: {
+ m_data = GetData;
+ m_expectedOutput = kSinglePoleIIRExpectedOutput;
+ break;
+ }
+
+ case kTestHighPass: {
+ m_data = GetData;
+ m_expectedOutput = kHighPassExpectedOutput;
+ break;
+ }
+
+ case kTestMovAvg: {
+ m_data = GetData;
+ m_expectedOutput = kMovAvgExpectedOutput;
+ break;
+ }
+
+ case kTestPulse: {
+ m_data = GetPulseData;
+ m_expectedOutput = 0.0;
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the linear filters produce consistent output for a given data set.
+ */
+TEST_P(LinearFilterOutputTest, Output) {
+ double filterOutput = 0.0;
+ for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+ filterOutput = m_filter.Calculate(m_data(t.value()));
+ }
+
+ RecordProperty("LinearFilterOutput", filterOutput);
+
+ EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+ << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_SUITE_P(Tests, LinearFilterOutputTest,
+ testing::Values(kTestSinglePoleIIR, kTestHighPass,
+ kTestMovAvg, kTestPulse));
+
+template <int Derivative, int Samples, typename F, typename DfDx>
+void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+ double max) {
+ auto filter =
+ frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
+ h);
+
+ for (int i = min / h.value(); i < max / h.value(); ++i) {
+ // Let filter initialize
+ if (i < static_cast<int>(min / h.value()) + Samples) {
+ filter.Calculate(f(i * h.value()));
+ continue;
+ }
+
+ // The order of accuracy is O(h^(N - d)) where N is number of stencil
+ // points and d is order of derivative
+ EXPECT_NEAR(dfdx(i * h.value()), filter.Calculate(f(i * h.value())),
+ 10.0 * std::pow(h.value(), Samples - Derivative));
+ }
+}
+
+/**
+ * Test backward finite difference.
+ */
+TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
+ constexpr auto h = 5_ms;
+
+ AssertResults<1, 2>(
+ [](double x) {
+ // f(x) = x²
+ return x * x;
+ },
+ [](double x) {
+ // df/dx = 2x
+ return 2.0 * x;
+ },
+ h, -20.0, 20.0);
+
+ AssertResults<1, 2>(
+ [](double x) {
+ // f(x) = std::sin(x)
+ return std::sin(x);
+ },
+ [](double x) {
+ // df/dx = std::cos(x)
+ return std::cos(x);
+ },
+ h, -20.0, 20.0);
+
+ AssertResults<1, 2>(
+ [](double x) {
+ // f(x) = ln(x)
+ return std::log(x);
+ },
+ [](double x) {
+ // df/dx = 1 / x
+ return 1.0 / x;
+ },
+ h, 1.0, 20.0);
+
+ AssertResults<2, 4>(
+ [](double x) {
+ // f(x) = x^2
+ return x * x;
+ },
+ [](double x) {
+ // d²f/dx² = 2
+ return 2.0;
+ },
+ h, -20.0, 20.0);
+
+ AssertResults<2, 4>(
+ [](double x) {
+ // f(x) = std::sin(x)
+ return std::sin(x);
+ },
+ [](double x) {
+ // d²f/dx² = -std::sin(x)
+ return -std::sin(x);
+ },
+ h, -20.0, 20.0);
+
+ AssertResults<2, 4>(
+ [](double x) {
+ // f(x) = ln(x)
+ return std::log(x);
+ },
+ [](double x) {
+ // d²f/dx² = -1 / x²
+ return -1.0 / (x * x);
+ },
+ h, 1.0, 20.0);
+}
diff --git a/wpimath/src/test/native/cpp/MedianFilterTest.cpp b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
similarity index 65%
rename from wpimath/src/test/native/cpp/MedianFilterTest.cpp
rename to wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
index 2a02e1c..8151a45 100644
--- a/wpimath/src/test/native/cpp/MedianFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/MedianFilter.h"
+#include "frc/filter/MedianFilter.h"
#include "gtest/gtest.h"
TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
new file mode 100644
index 0000000..d2c0bae
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -0,0 +1,33 @@
+// 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 <wpi/timestamp.h>
+
+#include "frc/filter/SlewRateLimiter.h"
+#include "gtest/gtest.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+static units::second_t now = 0_s;
+
+TEST(SlewRateLimiterTest, SlewRateLimit) {
+ WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ now += 1_s;
+
+ EXPECT_LT(limiter.Calculate(2_m), 2_m);
+}
+
+TEST(SlewRateLimiterTest, SlewRateNoLimit) {
+ WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ now += 1_s;
+
+ EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index 620c9d3..cd5b127 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -20,9 +17,9 @@
const auto transformed = initial + transform;
- EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
- EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
- EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
+ EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
}
TEST(Pose2dTest, RelativeTo) {
@@ -31,10 +28,10 @@
const auto finalRelativeToInitial = final.RelativeTo(initial);
- EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
+ EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
kEpsilon);
- EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
+ EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
kEpsilon);
}
@@ -56,7 +53,7 @@
const auto transform = final - initial;
- EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
- EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index a29371f..ed3b6b5 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <wpi/math>
+#include <wpi/numbers>
#include "frc/geometry/Rotation2d.h"
#include "gtest/gtest.h"
@@ -17,51 +14,55 @@
static constexpr double kEpsilon = 1E-9;
TEST(Rotation2dTest, RadiansToDegrees) {
- const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
- const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
+ const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
+ const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
- EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
- EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
+ EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
+ EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
}
TEST(Rotation2dTest, DegreesToRadians) {
- const auto one = Rotation2d(45.0_deg);
- const auto two = Rotation2d(30.0_deg);
+ const auto rot1 = Rotation2d(45.0_deg);
+ const auto rot2 = Rotation2d(30.0_deg);
- EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
- EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
+ EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
+ EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
auto sum = zero + Rotation2d(90.0_deg);
- EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
- EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
+ EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByNonZero) {
auto rot = Rotation2d(90.0_deg);
- rot += Rotation2d(30.0_deg);
+ rot = rot + Rotation2d(30.0_deg);
- EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
+ EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
}
TEST(Rotation2dTest, Minus) {
- const auto one = Rotation2d(70.0_deg);
- const auto two = Rotation2d(30.0_deg);
+ const auto rot1 = Rotation2d(70.0_deg);
+ const auto rot2 = Rotation2d(30.0_deg);
- EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
+ EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
}
TEST(Rotation2dTest, Equality) {
- const auto one = Rotation2d(43_deg);
- const auto two = Rotation2d(43_deg);
- EXPECT_TRUE(one == two);
+ const auto rot1 = Rotation2d(43_deg);
+ const auto rot2 = Rotation2d(43_deg);
+ EXPECT_EQ(rot1, rot2);
+
+ const auto rot3 = Rotation2d(-180_deg);
+ const auto rot4 = Rotation2d(180_deg);
+ EXPECT_EQ(rot3, rot4);
}
TEST(Rotation2dTest, Inequality) {
- const auto one = Rotation2d(43_deg);
- const auto two = Rotation2d(43.5_deg);
- EXPECT_TRUE(one != two);
+ const auto rot1 = Rotation2d(43_deg);
+ const auto rot2 = Rotation2d(43.5_deg);
+ EXPECT_NE(rot1, rot2);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index b302fad..968ab29 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -18,16 +15,30 @@
static constexpr double kEpsilon = 1E-9;
TEST(Transform2dTest, Inverse) {
- const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
- const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+ const Pose2d initial{1_m, 2_m, 45_deg};
+ const Transform2d transform{{5_m, 0_m}, 5_deg};
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
- EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
+ EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
+ EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
+ EXPECT_NEAR(initial.Rotation().Degrees().value(),
+ untransformed.Rotation().Degrees().value(), kEpsilon);
+}
+
+TEST(Transform2dTest, Composition) {
+ const Pose2d initial{1_m, 2_m, 45_deg};
+ const Transform2d transform1{{5_m, 0_m}, 5_deg};
+ const Transform2d transform2{{0_m, 2_m}, 5_deg};
+
+ auto transformedSeparate = initial + transform1 + transform2;
+ auto transformedCombined = initial + (transform1 + transform2);
+
+ EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
kEpsilon);
- EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
+ EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
kEpsilon);
- EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
- untransformed.Rotation().Degrees().to<double>(), kEpsilon);
+ EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
+ transformedCombined.Rotation().Degrees().value(), kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 8e487f3..efdcace 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -20,8 +17,8 @@
const auto sum = one + two;
- EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
- EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
+ EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
+ EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
}
TEST(Translation2dTest, Difference) {
@@ -30,51 +27,51 @@
const auto difference = one - two;
- EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
- EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
+ EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
+ EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
}
TEST(Translation2dTest, RotateBy) {
const Translation2d another{3.0_m, 0.0_m};
const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
- EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
+ EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon);
}
TEST(Translation2dTest, Multiplication) {
const Translation2d original{3.0_m, 5.0_m};
const auto mult = original * 3;
- EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
- EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
+ EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
+ EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
}
-TEST(Translation2d, Division) {
+TEST(Translation2dTest, Division) {
const Translation2d original{3.0_m, 5.0_m};
const auto div = original / 2;
- EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
- EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
+ EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
+ EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
}
TEST(Translation2dTest, Norm) {
const Translation2d one{3.0_m, 5.0_m};
- EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
+ EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon);
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
- EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
+ EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon);
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
- EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
- EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
+ EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
+ EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
}
TEST(Translation2dTest, Equality) {
@@ -91,10 +88,10 @@
TEST(Translation2dTest, PolarConstructor) {
Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
- EXPECT_NEAR(one.X().to<double>(), 1.0, kEpsilon);
- EXPECT_NEAR(one.Y().to<double>(), 1.0, kEpsilon);
+ EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
Translation2d two{2_m, Rotation2d(60_deg)};
- EXPECT_NEAR(two.X().to<double>(), 1.0, kEpsilon);
- EXPECT_NEAR(two.Y().to<double>(), std::sqrt(3.0), kEpsilon);
+ EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index 4766bd4..fa9eecc 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <wpi/math>
+#include <wpi/numbers>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
@@ -20,29 +17,28 @@
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const auto straightPose = Pose2d().Exp(straight);
- EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
+ EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
}
TEST(Twist2dTest, QuarterCircle) {
- const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
- units::radian_t(wpi::math::pi / 2.0)};
+ const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
+ units::radian_t(wpi::numbers::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
- EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
- kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
- EXPECT_NEAR(diagonalPose.X().to<double>(), 2.0, kEpsilon);
- EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
- EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
}
TEST(Twist2dTest, Equality) {
@@ -63,7 +59,7 @@
const auto twist = start.Log(end);
- EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
- EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
+ EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
+ EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 864860a..7665a97 100644
--- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/ChassisSpeeds.h"
#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
-TEST(ChassisSpeeds, FieldRelativeConstruction) {
+TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
- EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
- EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
- EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
+ EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
+ EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 7c1a28d..224e231 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <wpi/math>
+#include <wpi/numbers>
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
@@ -18,62 +15,62 @@
static constexpr double kEpsilon = 1E-9;
-TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
+TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds;
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
- EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
- EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon);
}
-TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
+TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds;
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
}
-TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
+TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
- EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
- EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon);
}
-TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
+TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
}
-TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
+TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
- const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
- units::radians_per_second_t{wpi::math::pi}};
+ const ChassisSpeeds chassisSpeeds{
+ 0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
- EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
- EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon);
}
-TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
+TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
- units::meters_per_second_t(+0.381 * wpi::math::pi),
- units::meters_per_second_t(-0.381 * wpi::math::pi)};
+ units::meters_per_second_t(+0.381 * wpi::numbers::pi),
+ units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index 89d65ea..da16b28 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <wpi/math>
+#include <wpi/numbers>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
@@ -15,13 +12,13 @@
using namespace frc;
-TEST(DifferentialDriveOdometry, EncoderDistances) {
+TEST(DifferentialDriveOdometryTest, EncoderDistances) {
DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
- units::meter_t(5 * wpi::math::pi));
+ units::meter_t(5 * wpi::numbers::pi));
- EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
+ EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
+ EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index cb03d97..18b72de 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <wpi/math>
+#include <wpi/numbers>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
@@ -28,113 +25,69 @@
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
- /*
- By equation (13.12) of the state-space-guide, the wheel speeds should
- be as follows:
- velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
- */
-
- EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
- EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
- EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
- EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
+ EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1);
+ EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1);
+ EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1);
+ EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
- MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
- 3.536_mps};
+ MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- /*
- By equation (13.13) of the state-space-guide, the chassis motion from wheel
- velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
- [[5][0][0]]
- */
-
- EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
- EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
- EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+ EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
- /*
- By equation (13.12) of the state-space-guide, the wheel speeds should
- be as follows:
- velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
- */
-
- EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
- EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
- EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
- EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
+ EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1);
+ EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1);
+ EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1);
+ EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
- MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
- -2.828427_mps};
+ MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- /*
- By equation (13.13) of the state-space-guide, the chassis motion from wheel
- velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
- [[5][0][0]]
- */
-
- EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
- EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
- EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
+ EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
- units::radians_per_second_t(2 * wpi::math::pi)};
+ units::radians_per_second_t(2 * wpi::numbers::pi)};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
- /*
- By equation (13.12) of the state-space-guide, the wheel speeds should
- be as follows:
- velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
- */
-
- EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
- EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
- EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
- EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
+ EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
+ EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1);
+ EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1);
+ EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
- MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
- -106.62919_mps, 106.62919_mps};
+ MecanumDriveWheelSpeeds wheelSpeeds{-150.79644737_mps, 150.79644737_mps,
+ -150.79644737_mps, 150.79644737_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- /*
- By equation (13.13) of the state-space-guide, the chassis motion from wheel
- velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
- be [[0][0][2pi]]
- */
-
- EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
- EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
- EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
+ EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
- /*
- By equation (13.12) of the state-space-guide, the wheel speeds should
- be as follows:
- velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
- */
-
- EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
- EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
- EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
- EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
+ EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1);
+ EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1);
+ EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1);
+ EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
@@ -143,31 +96,19 @@
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- /*
- By equation (13.13) of the state-space-guide, the chassis motion from wheel
- velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
- [[2][3][1]]
- */
-
- EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
- EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
- EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+ EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1);
+ EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1);
+ EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
- /*
- By equation (13.12) of the state-space-guide, the wheel speeds should
- be as follows:
- velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
- */
-
- EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
- EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
- EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
- EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
+ EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1);
+ EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1);
+ EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1);
+ EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
@@ -175,14 +116,9 @@
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- /*
- By equation (13.13) of the state-space-guide, the chassis motion from the
- wheel velocities should be [[12][-12][1]]
- */
-
- EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
- EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
- EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+ EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1);
+ EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1);
+ EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -190,15 +126,10 @@
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
- /*
- By equation (13.12) of the state-space-guide, the wheel speeds should
- be as follows:
- velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
- */
- EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
- EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
- EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
- EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
+ EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1);
+ EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1);
+ EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1);
+ EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -207,24 +138,19 @@
36.06_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
- /*
- By equation (13.13) of the state-space-guide, the chassis motion from the
- wheel velocities should be [[17][-10][1]]
- */
-
- EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
- EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
- EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+ EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1);
+ EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1);
+ EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
-TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
+TEST_F(MecanumDriveKinematicsTest, Normalize) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Normalize(5.5_mps);
double kFactor = 5.5 / 7.0;
- EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
- EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
- EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
- EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index cb85ec7..152506d 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/MecanumDriveOdometry.h"
#include "gtest/gtest.h"
@@ -29,9 +26,9 @@
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
- EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
- EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
- EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.Rotation().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
@@ -41,21 +38,21 @@
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
- EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
- EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
- EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
+ EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
+ EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}
-TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
+TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
39.986_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
- EXPECT_NEAR(pose.X().to<double>(), 12, 0.01);
- EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
- EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
+ EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
+ EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
+ EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
@@ -66,7 +63,7 @@
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
- EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
- EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
- EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
+ EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
+ EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 368dbaf..9384d89 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <wpi/math>
+#include <wpi/numbers>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
@@ -31,15 +28,15 @@
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
- EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
- EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Radians().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Radians().value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
@@ -47,49 +44,49 @@
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
- EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
- EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
- EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
- EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
- EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().value(), 90.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().value(), 90.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, Rotation2d(90_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
- units::radians_per_second_t(2 * wpi::math::pi)};
+ units::radians_per_second_t(2 * wpi::numbers::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
- EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
- EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
- EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
- EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
+ EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon);
+ EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon);
+ EXPECT_NEAR(br.speed.value(), 106.63, kEpsilon);
- EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
- EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
- EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
- EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+ EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
@@ -100,25 +97,25 @@
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
- units::radians_per_second_t(2 * wpi::math::pi)};
+ units::radians_per_second_t(2 * wpi::numbers::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
- EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
- EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
- EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
+ EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon);
+ EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon);
+ EXPECT_NEAR(br.speed.value(), 213.258, kEpsilon);
- EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
- EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+ EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().value(), -90.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
@@ -129,9 +126,9 @@
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -140,15 +137,15 @@
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
- EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
- EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
- EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
- EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
+ EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
+ EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
+ EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon);
+ EXPECT_NEAR(br.speed.value(), 54.08, kEpsilon);
- EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
- EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
- EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
- EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
+ EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().value(), -109.44, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().value(), -70.56, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -160,24 +157,24 @@
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
- EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
}
-TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
+TEST_F(SwerveDriveKinematicsTest, Normalize) {
SwerveModuleState state1{5.0_mps, Rotation2d()};
SwerveModuleState state2{6.0_mps, Rotation2d()};
SwerveModuleState state3{4.0_mps, Rotation2d()};
SwerveModuleState state4{7.0_mps, Rotation2d()};
- std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+ wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
- EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
- EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
- EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
- EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 40207a1..27d2a6e 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
@@ -34,9 +31,9 @@
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
state, state);
- EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
- EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
- EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
@@ -52,9 +49,9 @@
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
- EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
- EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
- EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+ EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
+ EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
+ EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
@@ -68,7 +65,7 @@
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
state, state);
- EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
- EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
- EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
new file mode 100644
index 0000000..4880bef
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -0,0 +1,41 @@
+// 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/Rotation2d.h"
+#include "frc/kinematics/SwerveModuleState.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(SwerveModuleStateTest, Optimize) {
+ frc::Rotation2d angleA{45_deg};
+ frc::SwerveModuleState refA{-2_mps, 180_deg};
+ auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
+
+ EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
+ EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
+
+ frc::Rotation2d angleB{-50_deg};
+ frc::SwerveModuleState refB{4.7_mps, 41_deg};
+ auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
+
+ EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon);
+ EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
+}
+
+TEST(SwerveModuleStateTest, NoOptimize) {
+ frc::Rotation2d angleA{0_deg};
+ frc::SwerveModuleState refA{2_mps, 89_deg};
+ auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
+
+ EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
+ EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
+
+ frc::Rotation2d angleB{0_deg};
+ frc::SwerveModuleState refB{-2_mps, -2_deg};
+ auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
+
+ EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
+ EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/main.cpp b/wpimath/src/test/native/cpp/main.cpp
index e2126f2..09072ee 100644
--- a/wpimath/src/test/native/cpp/main.cpp
+++ b/wpimath/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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"
diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index fe084e0..69e202f 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <chrono>
#include <iostream>
@@ -56,27 +53,26 @@
// Make sure the twist is under the tolerance defined by the Spline class.
auto twist = p0.first.Log(p1.first);
- EXPECT_LT(std::abs(twist.dx.to<double>()),
- SplineParameterizer::kMaxDx.to<double>());
- EXPECT_LT(std::abs(twist.dy.to<double>()),
- SplineParameterizer::kMaxDy.to<double>());
- EXPECT_LT(std::abs(twist.dtheta.to<double>()),
- SplineParameterizer::kMaxDtheta.to<double>());
+ EXPECT_LT(std::abs(twist.dx.value()),
+ SplineParameterizer::kMaxDx.value());
+ EXPECT_LT(std::abs(twist.dy.value()),
+ SplineParameterizer::kMaxDy.value());
+ EXPECT_LT(std::abs(twist.dtheta.value()),
+ SplineParameterizer::kMaxDtheta.value());
}
// Check first point.
- EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
- EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
- EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
- a.Rotation().Radians().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Rotation().Radians().value(),
+ a.Rotation().Radians().value(), 1E-9);
// Check interior waypoints
bool interiorsGood = true;
for (auto& waypoint : waypoints) {
bool found = false;
for (auto& state : poses) {
- if (std::abs(
- waypoint.Distance(state.first.Translation()).to<double>()) <
+ if (std::abs(waypoint.Distance(state.first.Translation()).value()) <
1E-9) {
found = true;
}
@@ -87,10 +83,10 @@
EXPECT_TRUE(interiorsGood);
// Check last point.
- EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
- EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
- EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
- b.Rotation().Radians().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
+ b.Rotation().Radians().value(), 1E-9);
static_cast<void>(duration);
}
diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index 30b5b31..25449fb 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <chrono>
#include <iostream>
@@ -43,25 +40,25 @@
// Make sure the twist is under the tolerance defined by the Spline class.
auto twist = p0.first.Log(p1.first);
- EXPECT_LT(std::abs(twist.dx.to<double>()),
- SplineParameterizer::kMaxDx.to<double>());
- EXPECT_LT(std::abs(twist.dy.to<double>()),
- SplineParameterizer::kMaxDy.to<double>());
- EXPECT_LT(std::abs(twist.dtheta.to<double>()),
- SplineParameterizer::kMaxDtheta.to<double>());
+ EXPECT_LT(std::abs(twist.dx.value()),
+ SplineParameterizer::kMaxDx.value());
+ EXPECT_LT(std::abs(twist.dy.value()),
+ SplineParameterizer::kMaxDy.value());
+ EXPECT_LT(std::abs(twist.dtheta.value()),
+ SplineParameterizer::kMaxDtheta.value());
}
// Check first point.
- EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
- EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
- EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
- a.Rotation().Radians().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Rotation().Radians().value(),
+ a.Rotation().Radians().value(), 1E-9);
// Check last point.
- EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
- EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
- EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
- b.Rotation().Radians().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
+ b.Rotation().Radians().value(), 1E-9);
static_cast<void>(duration);
}
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index dbeb518..b5a0fdc 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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>
@@ -12,25 +9,23 @@
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "frc/system/Discretization.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/RungeKuttaTimeVarying.h"
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeA) {
- Eigen::Matrix<double, 2, 2> contA;
- contA << 0, 1, 0, 0;
+ Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
- Eigen::Matrix<double, 2, 1> x0;
- x0 << 1, 1;
+ Eigen::Vector<double, 2> x0{1, 1};
Eigen::Matrix<double, 2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
- Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0;
+ Eigen::Vector<double, 2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
- Eigen::Matrix<double, 2, 1> x1Truth;
- x1Truth(1) = x0(1);
- x1Truth(0) = x0(0) + 1.0 * x0(1);
+ Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
+ 0.0 * x0(0) + 1.0 * x0(1)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -38,38 +33,30 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeAB) {
- Eigen::Matrix<double, 2, 2> contA;
- contA << 0, 1, 0, 0;
+ Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+ Eigen::Matrix<double, 2, 1> contB{0, 1};
- Eigen::Matrix<double, 2, 1> contB;
- contB << 0, 1;
-
- Eigen::Matrix<double, 2, 1> x0;
- x0 << 1, 1;
- Eigen::Matrix<double, 1, 1> u;
- u << 1;
+ Eigen::Vector<double, 2> x0{1, 1};
+ Eigen::Vector<double, 1> u{1};
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 1> discB;
frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
- Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0 + discB * u;
+ Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
// We now have pos = vel = accel = 1, which should give us:
- Eigen::Matrix<double, 2, 1> x1Truth;
- x1Truth(1) = x0(1) + 1.0 * u(0);
- x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
+ Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
+ 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
EXPECT_EQ(x1Truth, x1Discrete);
}
-// Test that the discrete approximation of Q is roughly equal to
-// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+// dt
+// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aáµ€τ) dτ
+// 0
TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
- Eigen::Matrix<double, 2, 2> contA;
- contA << 0, 1, 0, 0;
-
- Eigen::Matrix<double, 2, 2> contQ;
- contQ << 1, 0, 0, 1;
+ Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+ Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
@@ -79,10 +66,10 @@
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
- (contA * t.to<double>()).exp() * contQ *
- (contA.transpose() * t.to<double>()).exp());
+ (contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+ 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
@@ -94,16 +81,14 @@
<< discQIntegrated;
}
-// Test that the discrete approximation of Q is roughly equal to
-// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+// dt
+// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aáµ€τ) dτ
+// 0
TEST(DiscretizationTest, DiscretizeFastModelAQ) {
- Eigen::Matrix<double, 2, 2> contA;
- contA << 0, 1, 0, -1406.29;
+ Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
+ Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
- Eigen::Matrix<double, 2, 2> contQ;
- contQ << 0.0025, 0, 0, 1;
-
- constexpr auto dt = 5.05_ms;
+ constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
@@ -111,10 +96,10 @@
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
- (contA * t.to<double>()).exp() * contQ *
- (contA.transpose() * t.to<double>()).exp());
+ (contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+ 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
@@ -128,26 +113,19 @@
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
- Eigen::Matrix<double, 2, 2> contA;
- contA << 0, 1, 0, 0;
-
- Eigen::Matrix<double, 2, 1> contB;
- contB << 0, 1;
-
- Eigen::Matrix<double, 2, 2> contQ;
- contQ << 1, 0, 0, 1;
+ Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+ Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
- Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
- for (int i = 0; i < contQ.rows(); i++) {
- EXPECT_GT(esCont.eigenvalues()[i], 0);
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
+ for (int i = 0; i < contQ.rows(); ++i) {
+ EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -156,12 +134,12 @@
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
- (contA * t.to<double>()).exp() * contQ *
- (contA.transpose() * t.to<double>()).exp());
+ (contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+ 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
- frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+ frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
@@ -171,34 +149,27 @@
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_GT(esDisc.eigenvalues()[i], 0);
+ 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) {
- Eigen::Matrix<double, 2, 2> contA;
- contA << 0, 1, 0, -1500;
+ Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
+ Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
- Eigen::Matrix<double, 2, 1> contB;
- contB << 0, 1;
-
- Eigen::Matrix<double, 2, 2> contQ;
- contQ << 0.0025, 0, 0, 1;
-
- constexpr auto dt = 5.05_ms;
+ constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
- Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
- for (int i = 0; i < contQ.rows(); i++) {
- EXPECT_GT(esCont.eigenvalues()[i], 0);
+ for (int i = 0; i < contQ.rows(); ++i) {
+ EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -207,12 +178,12 @@
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
- (contA * t.to<double>()).exp() * contQ *
- (contA.transpose() * t.to<double>()).exp());
+ (contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+ 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
- frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+ frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
@@ -223,18 +194,15 @@
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
- for (int i = 0; i < discQTaylor.rows(); i++) {
- EXPECT_GT(esDisc.eigenvalues()[i], 0);
+ for (int i = 0; i < discQTaylor.rows(); ++i) {
+ EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
- Eigen::Matrix<double, 2, 2> contR;
- contR << 2.0, 0.0, 0.0, 1.0;
-
- Eigen::Matrix<double, 2, 2> discRTruth;
- discRTruth << 4.0, 0.0, 0.0, 2.0;
+ Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
+ Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index eedf8c0..1fa12c2 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <gtest/gtest.h>
-#include "frc/StateSpaceUtil.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/length.h"
#include "units/mass.h"
@@ -20,32 +16,37 @@
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
ASSERT_TRUE(model.A().isApprox(
- frc::MakeMatrix<2, 2>(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
+ Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
+ 0.001));
ASSERT_TRUE(model.B().isApprox(
- frc::MakeMatrix<2, 2>(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
- ASSERT_TRUE(
- model.C().isApprox(frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0), 0.001));
- ASSERT_TRUE(
- model.D().isApprox(frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0), 0.001));
+ Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
+ 0.001));
+ ASSERT_TRUE(model.C().isApprox(
+ Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(
+ Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
}
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
0.05_m, 12);
ASSERT_TRUE(model.A().isApprox(
- frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -99.05473), 0.001));
- ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 20.8), 0.001));
- ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 2>(1.0, 0.0), 0.001));
- ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+ Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
+ ASSERT_TRUE(
+ model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
+ ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, FlywheelSystem) {
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
- ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-26.87032), 0.001));
- ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1354.166667), 0.001));
- ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 1>(1.0), 0.001));
- ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+ ASSERT_TRUE(
+ model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
+ ASSERT_TRUE(
+ model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
+ ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -56,9 +57,10 @@
auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
- ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka),
- 0.001));
- ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 1.0 / ka), 0.001));
+ ASSERT_TRUE(model.A().isApprox(
+ Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
+ ASSERT_TRUE(
+ model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -70,6 +72,6 @@
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
- ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001));
- ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));
+ ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
+ ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
}
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
new file mode 100644
index 0000000..fd9c039
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -0,0 +1,57 @@
+// 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 <cmath>
+
+#include "frc/system/NumericalIntegration.h"
+
+// Tests that integrating dx/dt = e^x works.
+TEST(NumericalIntegrationTest, Exponential) {
+ Eigen::Vector<double, 1> y0{0.0};
+
+ Eigen::Vector<double, 1> y1 = frc::RK4(
+ [](const Eigen::Vector<double, 1>& x) {
+ return Eigen::Vector<double, 1>{std::exp(x(0))};
+ },
+ y0, 0.1_s);
+ EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works when we provide a U.
+TEST(NumericalIntegrationTest, ExponentialWithU) {
+ Eigen::Vector<double, 1> y0{0.0};
+
+ Eigen::Vector<double, 1> y1 = frc::RK4(
+ [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+ return Eigen::Vector<double, 1>{std::exp(u(0) * x(0))};
+ },
+ y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
+ EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works with RKF45.
+TEST(NumericalIntegrationTest, ExponentialRKF45) {
+ Eigen::Vector<double, 1> y0{0.0};
+
+ Eigen::Vector<double, 1> y1 = frc::RKF45(
+ [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+ return Eigen::Vector<double, 1>{std::exp(x(0))};
+ },
+ y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+ EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works with RKDP
+TEST(NumericalIntegrationTest, ExponentialRKDP) {
+ Eigen::Vector<double, 1> y0{0.0};
+
+ Eigen::Vector<double, 1> y1 = frc::RKDP(
+ [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+ return Eigen::Vector<double, 1>{std::exp(x(0))};
+ },
+ y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+ EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
index ddc3f68..4e64825 100644
--- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
@@ -1,68 +1,58 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/system/NumericalJacobian.h"
-Eigen::Matrix<double, 4, 4> A = (Eigen::Matrix<double, 4, 4>() << 1, 2, 4, 1, 5,
- 2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7)
- .finished();
-
-Eigen::Matrix<double, 4, 2> B =
- (Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
+Eigen::Matrix<double, 4, 4> A{
+ {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
+Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
// Function from which to recover A and B
-Eigen::Matrix<double, 4, 1> AxBuFn(const Eigen::Matrix<double, 4, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
+ const Eigen::Vector<double, 2>& u) {
return A * x + B * u;
}
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax) {
- Eigen::Matrix<double, 4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
- AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
- Eigen::Matrix<double, 2, 1>::Zero());
+ Eigen::Matrix<double, 4, 4> newA =
+ frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
+ Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu) {
- Eigen::Matrix<double, 4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
- AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
- Eigen::Matrix<double, 2, 1>::Zero());
+ Eigen::Matrix<double, 4, 2> newB =
+ frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
+ Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
-Eigen::Matrix<double, 3, 4> C =
- (Eigen::Matrix<double, 3, 4>() << 1, 2, 4, 1, 5, 2, 3, 4, 5, 1, 3, 2)
- .finished();
-
-Eigen::Matrix<double, 3, 2> D =
- (Eigen::Matrix<double, 3, 2>() << 1, 1, 2, 1, 3, 2).finished();
+Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
+Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
// Function from which to recover C and D
-Eigen::Matrix<double, 3, 1> CxDuFn(const Eigen::Matrix<double, 4, 1>& x,
- const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
+ const Eigen::Vector<double, 2>& u) {
return C * x + D * u;
}
// Test that we can recover C from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Cx) {
- Eigen::Matrix<double, 3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
- CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
- Eigen::Matrix<double, 2, 1>::Zero());
+ Eigen::Matrix<double, 3, 4> newC =
+ frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
+ Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newC.isApprox(C));
}
// Test that we can recover D from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Du) {
- Eigen::Matrix<double, 3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
- CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
- Eigen::Matrix<double, 2, 1>::Zero());
+ Eigen::Matrix<double, 3, 2> newD =
+ frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
+ Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
deleted file mode 100644
index a12c1b7..0000000
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include <gtest/gtest.h>
-
-#include <cmath>
-
-#include "frc/system/RungeKutta.h"
-
-// Tests that integrating dx/dt = e^x works.
-TEST(RungeKuttaTest, Exponential) {
- Eigen::Matrix<double, 1, 1> y0;
- y0(0) = 0.0;
-
- Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
- [](Eigen::Matrix<double, 1, 1> x) {
- Eigen::Matrix<double, 1, 1> y;
- y(0) = std::exp(x(0));
- return y;
- },
- y0, 0.1_s);
- EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
-}
-
-// Tests that integrating dx/dt = e^x works when we provide a U.
-TEST(RungeKuttaTest, ExponentialWithU) {
- Eigen::Matrix<double, 1, 1> y0;
- y0(0) = 0.0;
-
- Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
- [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
- Eigen::Matrix<double, 1, 1> y;
- y(0) = std::exp(u(0) * x(0));
- return y;
- },
- y0, (Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1_s);
- EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
-}
-
-namespace {
-Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
- return (Eigen::Matrix<double, 1, 1>()
- << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
- .finished();
-}
-} // namespace
-
-// Tests RungeKutta with a time varying solution.
-// Now, lets test RK4 with a time varying solution. From
-// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
-// x' = x (2 / (e^t + 1) - 1)
-//
-// The true (analytical) solution is:
-//
-// x(t) = 12 * e^t / ((e^t + 1)^2)
-TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
- Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
-
- Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
- [](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
- return (Eigen::Matrix<double, 1, 1>()
- << x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
- .finished();
- },
- y0, 5_s, 1_s);
- EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
-}
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
new file mode 100644
index 0000000..f1be861
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -0,0 +1,35 @@
+// 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 <cmath>
+
+#include "frc/system/RungeKuttaTimeVarying.h"
+
+namespace {
+Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
+ return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
+ (std::pow(std::exp(t) + 1.0, 2.0))};
+}
+} // namespace
+
+// Tests RK4 with a time varying solution. From
+// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+// x' = x (2 / (e^t + 1) - 1)
+//
+// The true (analytical) solution is:
+//
+// x(t) = 12 * e^t / ((e^t + 1)^2)
+TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
+ Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+ Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
+ [](units::second_t t, const Eigen::Vector<double, 1>& x) {
+ return Eigen::Vector<double, 1>{
+ x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+ },
+ 5_s, y0, 1_s);
+ EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
+}
diff --git a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
index 42e9fe2..e2f7112 100644
--- a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <memory>
#include <vector>
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
index 636b002..e3723b5 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <memory>
#include <vector>
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index 6cd8075..b21ef7b 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <iostream>
#include <memory>
diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 44c4222..88dc2b8 100644
--- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <vector>
diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 33f753f..77310ae 100644
--- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <vector>
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
new file mode 100644
index 0000000..2b733a8
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.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/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(TrajectoryConcatenateTest, States) {
+ auto t1 = frc::TrajectoryGenerator::GenerateTrajectory(
+ {}, {}, {1_m, 1_m, 0_deg}, {2_mps, 2_mps_sq});
+ auto t2 = frc::TrajectoryGenerator::GenerateTrajectory(
+ {1_m, 1_m, 0_deg}, {}, {2_m, 2_m, 45_deg}, {2_mps, 2_mps_sq});
+
+ auto t = t1 + t2;
+
+ double time = -1.0;
+ for (size_t i = 0; i < t.States().size(); ++i) {
+ const auto& state = t.States()[i];
+
+ // Make sure that the timestamps are strictly increasing.
+ EXPECT_GT(state.t.value(), time);
+ time = state.t.value();
+
+ // Ensure that the states in t are the same as those in t1 and t2.
+ if (i < t1.States().size()) {
+ EXPECT_EQ(state, t1.States()[i]);
+ } else {
+ auto st = t2.States()[i - t1.States().size() + 1];
+ st.t += t1.TotalTime();
+ EXPECT_EQ(state, st);
+ }
+ }
+}
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 378aff7..175becf 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <vector>
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
index c18a3e9..90c6dc0 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryUtil.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 349ff5c..0c6e07a 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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 <vector>
@@ -24,14 +21,14 @@
auto a = a2.RelativeTo(a1);
auto b = b2.RelativeTo(b1);
- EXPECT_NEAR(a.X().to<double>(), b.X().to<double>(), 1E-9);
- EXPECT_NEAR(a.Y().to<double>(), b.Y().to<double>(), 1E-9);
- EXPECT_NEAR(a.Rotation().Radians().to<double>(),
- b.Rotation().Radians().to<double>(), 1E-9);
+ EXPECT_NEAR(a.X().value(), b.X().value(), 1E-9);
+ EXPECT_NEAR(a.Y().value(), b.Y().value(), 1E-9);
+ EXPECT_NEAR(a.Rotation().Radians().value(), b.Rotation().Radians().value(),
+ 1E-9);
}
}
-TEST(TrajectoryTransforms, TransformBy) {
+TEST(TrajectoryTransformsTest, TransformBy) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
@@ -42,14 +39,14 @@
auto firstPose = transformedTrajectory.Sample(0_s).pose;
- EXPECT_NEAR(firstPose.X().to<double>(), 1.0, 1E-9);
- EXPECT_NEAR(firstPose.Y().to<double>(), 2.0, 1E-9);
- EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
+ EXPECT_NEAR(firstPose.X().value(), 1.0, 1E-9);
+ EXPECT_NEAR(firstPose.Y().value(), 2.0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 30.0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
}
-TEST(TrajectoryTransforms, RelativeTo) {
+TEST(TrajectoryTransformsTest, RelativeTo) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
@@ -60,9 +57,9 @@
auto firstPose = transformedTrajectory.Sample(0_s).pose;
- EXPECT_NEAR(firstPose.X().to<double>(), 0, 1E-9);
- EXPECT_NEAR(firstPose.Y().to<double>(), 0, 1E-9);
- EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.X().value(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Y().value(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
}
diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index 63ea916..6a35261 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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/TrapezoidProfile.h" // NOLINT(build/include_order)
diff --git a/wpimath/src/test/native/include/drake/common/autodiff.h b/wpimath/src/test/native/include/drake/common/autodiff.h
deleted file mode 100644
index 66fd88a..0000000
--- a/wpimath/src/test/native/include/drake/common/autodiff.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#pragma once
-/// @file This header provides a single inclusion point for autodiff-related
-/// header files in the `drake/common` directory. Users should include this
-/// file. Including other individual headers such as `drake/common/autodiffxd.h`
-/// will generate a compile-time warning.
-
-// In each header included below, it asserts that this macro
-// `DRAKE_COMMON_AUTODIFF_HEADER` is defined. If the macro is not defined, it
-// generates diagnostic warning messages.
-#define DRAKE_COMMON_AUTODIFF_HEADER
-
-#include <Eigen/Core>
-#include <unsupported/Eigen/AutoDiff>
-
-static_assert(EIGEN_VERSION_AT_LEAST(3, 3, 3),
- "Drake requires Eigen >= v3.3.3.");
-
-// Do not alpha-sort the following block of hard-coded #includes, which is
-// protected by `clang-format on/off`.
-//
-// Rationale: We want to maximize the use of this header, `autodiff.h`, even
-// inside of the autodiff-related files to avoid any mistakes which might not be
-// detected. By centralizing the list here, we make sure that everyone will see
-// the correct order which respects the inter-dependencies of the autodiff
-// headers. This shields us from triggering undefined behaviors due to
-// order-of-specialization-includes-changed mistakes.
-//
-// clang-format off
-#include "drake/common/eigen_autodiff_limits.h"
-#include "drake/common/eigen_autodiff_types.h"
-#include "drake/common/autodiffxd.h"
-#include "drake/common/autodiff_overloads.h"
-// clang-format on
-#undef DRAKE_COMMON_AUTODIFF_HEADER
diff --git a/wpimath/src/test/native/include/drake/common/autodiff_overloads.h b/wpimath/src/test/native/include/drake/common/autodiff_overloads.h
deleted file mode 100644
index 7eaeb3f..0000000
--- a/wpimath/src/test/native/include/drake/common/autodiff_overloads.h
+++ /dev/null
@@ -1,203 +0,0 @@
-/// @file
-/// Overloads for STL mathematical operations on AutoDiffScalar.
-///
-/// Used via argument-dependent lookup (ADL). These functions appear
-/// in the Eigen namespace so that ADL can automatically choose between
-/// the STL version and the overloaded version to match the type of the
-/// arguments. The proper use would be e.g.
-///
-/// \code{.cc}
-/// void mymethod() {
-/// using std::isinf;
-/// isinf(myval);
-/// }
-/// \endcode{}
-///
-/// @note The if_then_else and cond functions for AutoDiffScalar are in
-/// namespace drake because cond is defined in namespace drake in
-/// "drake/common/cond.h" file.
-
-#pragma once
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <cmath>
-#include <limits>
-
-#include "drake/common/cond.h"
-#include "drake/common/drake_assert.h"
-#include "drake/common/dummy_value.h"
-
-namespace Eigen {
-
-/// Overloads nexttoward to mimic std::nexttoward from <cmath>.
-template <typename DerType>
-double nexttoward(const Eigen::AutoDiffScalar<DerType>& from, long double to) {
- using std::nexttoward;
- return nexttoward(from.value(), to);
-}
-
-/// Overloads round to mimic std::round from <cmath>.
-template <typename DerType>
-double round(const Eigen::AutoDiffScalar<DerType>& x) {
- using std::round;
- return round(x.value());
-}
-
-/// Overloads isinf to mimic std::isinf from <cmath>.
-template <typename DerType>
-bool isinf(const Eigen::AutoDiffScalar<DerType>& x) {
- using std::isinf;
- return isinf(x.value());
-}
-
-/// Overloads isfinite to mimic std::isfinite from <cmath>.
-template <typename DerType>
-bool isfinite(const Eigen::AutoDiffScalar<DerType>& x) {
- using std::isfinite;
- return isfinite(x.value());
-}
-
-/// Overloads isnan to mimic std::isnan from <cmath>.
-template <typename DerType>
-bool isnan(const Eigen::AutoDiffScalar<DerType>& x) {
- using std::isnan;
- return isnan(x.value());
-}
-
-/// Overloads floor to mimic std::floor from <cmath>.
-template <typename DerType>
-double floor(const Eigen::AutoDiffScalar<DerType>& x) {
- using std::floor;
- return floor(x.value());
-}
-
-/// Overloads ceil to mimic std::ceil from <cmath>.
-template <typename DerType>
-double ceil(const Eigen::AutoDiffScalar<DerType>& x) {
- using std::ceil;
- return ceil(x.value());
-}
-
-/// Overloads copysign from <cmath>.
-template <typename DerType, typename T>
-Eigen::AutoDiffScalar<DerType> copysign(const Eigen::AutoDiffScalar<DerType>& x,
- const T& y) {
- using std::isnan;
- if (isnan(x)) return (y >= 0) ? NAN : -NAN;
- if ((x < 0 && y >= 0) || (x >= 0 && y < 0))
- return -x;
- else
- return x;
-}
-
-/// Overloads copysign from <cmath>.
-template <typename DerType>
-double copysign(double x, const Eigen::AutoDiffScalar<DerType>& y) {
- using std::isnan;
- if (isnan(x)) return (y >= 0) ? NAN : -NAN;
- if ((x < 0 && y >= 0) || (x >= 0 && y < 0))
- return -x;
- else
- return x;
-}
-
-/// Overloads pow for an AutoDiffScalar base and exponent, implementing the
-/// chain rule.
-template <typename DerTypeA, typename DerTypeB>
-Eigen::AutoDiffScalar<
- typename internal::remove_all<DerTypeA>::type::PlainObject>
-pow(const Eigen::AutoDiffScalar<DerTypeA>& base,
- const Eigen::AutoDiffScalar<DerTypeB>& exponent) {
- // The two AutoDiffScalars being exponentiated must have the same matrix
- // type. This includes, but is not limited to, the same scalar type and
- // the same dimension.
- static_assert(
- std::is_same<
- typename internal::remove_all<DerTypeA>::type::PlainObject,
- typename internal::remove_all<DerTypeB>::type::PlainObject>::value,
- "The derivative types must match.");
-
- internal::make_coherent(base.derivatives(), exponent.derivatives());
-
- const auto& x = base.value();
- const auto& xgrad = base.derivatives();
- const auto& y = exponent.value();
- const auto& ygrad = exponent.derivatives();
-
- using std::pow;
- using std::log;
- const auto x_to_the_y = pow(x, y);
- if (ygrad.isZero(std::numeric_limits<double>::epsilon()) ||
- ygrad.size() == 0) {
- // The derivative only depends on ∂(x^y)/∂x -- this prevents undefined
- // behavior in the corner case where ∂(x^y)/∂y is infinite when x = 0,
- // despite ∂y/∂v being 0.
- return Eigen::MakeAutoDiffScalar(x_to_the_y, y * pow(x, y - 1) * xgrad);
- }
- return Eigen::MakeAutoDiffScalar(
- // The value is x ^ y.
- x_to_the_y,
- // The multivariable chain rule states:
- // df/dv_i = (∂f/∂x * dx/dv_i) + (∂f/∂y * dy/dv_i)
- // ∂f/∂x is y*x^(y-1)
- y * pow(x, y - 1) * xgrad +
- // ∂f/∂y is (x^y)*ln(x)
- x_to_the_y * log(x) * ygrad);
-}
-
-} // namespace Eigen
-
-namespace drake {
-
-/// Returns the autodiff scalar's value() as a double. Never throws.
-/// Overloads ExtractDoubleOrThrow from common/extract_double.h.
-template <typename DerType>
-double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar<DerType>& scalar) {
- return static_cast<double>(scalar.value());
-}
-
-/// Specializes common/dummy_value.h.
-template <typename DerType>
-struct dummy_value<Eigen::AutoDiffScalar<DerType>> {
- static constexpr Eigen::AutoDiffScalar<DerType> get() {
- constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
- DerType derivatives;
- derivatives.fill(kNaN);
- return Eigen::AutoDiffScalar<DerType>(kNaN, derivatives);
- }
-};
-
-/// Provides if-then-else expression for Eigen::AutoDiffScalar type. To support
-/// Eigen's generic expressions, we use casting to the plain object after
-/// applying Eigen::internal::remove_all. It is based on the Eigen's
-/// implementation of min/max function for AutoDiffScalar type
-/// (https://bitbucket.org/eigen/eigen/src/10a1de58614569c9250df88bdfc6402024687bc6/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h?at=default&fileviewer=file-view-default#AutoDiffScalar.h-546).
-template <typename DerType1, typename DerType2>
-inline Eigen::AutoDiffScalar<
- typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
-if_then_else(bool f_cond, const Eigen::AutoDiffScalar<DerType1>& x,
- const Eigen::AutoDiffScalar<DerType2>& y) {
- typedef Eigen::AutoDiffScalar<
- typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
- ADS1;
- typedef Eigen::AutoDiffScalar<
- typename Eigen::internal::remove_all<DerType2>::type::PlainObject>
- ADS2;
- static_assert(std::is_same<ADS1, ADS2>::value,
- "The derivative types must match.");
- return f_cond ? ADS1(x) : ADS2(y);
-}
-
-/// Provides special case of cond expression for Eigen::AutoDiffScalar type.
-template <typename DerType, typename... Rest>
-Eigen::AutoDiffScalar<
- typename Eigen::internal::remove_all<DerType>::type::PlainObject>
-cond(bool f_cond, const Eigen::AutoDiffScalar<DerType>& e_then, Rest... rest) {
- return if_then_else(f_cond, e_then, cond(rest...));
-}
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/common/autodiffxd.h b/wpimath/src/test/native/include/drake/common/autodiffxd.h
deleted file mode 100644
index a99b2f0..0000000
--- a/wpimath/src/test/native/include/drake/common/autodiffxd.h
+++ /dev/null
@@ -1,423 +0,0 @@
-#pragma once
-
-// This file is a modification of Eigen-3.3.3's AutoDiffScalar.h file which is
-// available at
-// https://bitbucket.org/eigen/eigen/raw/67e894c6cd8f5f1f604b27d37ed47fdf012674ff/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2017 Drake Authors
-//
-// 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 DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <cmath>
-#include <ostream>
-
-#include <Eigen/Core>
-
-namespace Eigen {
-
-#if !defined(DRAKE_DOXYGEN_CXX)
-// Explicit template specializations of Eigen::AutoDiffScalar for VectorXd.
-//
-// AutoDiffScalar tries to call internal::make_coherent to promote empty
-// derivatives. However, it fails to do the promotion when an operand is an
-// expression tree (i.e. CwiseBinaryOp). Our solution is to provide special
-// overloading for VectorXd and change the return types of its operators. With
-// this change, the operators evaluate terms immediately and return an
-// AutoDiffScalar<VectorXd> instead of expression trees (such as CwiseBinaryOp).
-// Eigen's implementation of internal::make_coherent makes use of const_cast in
-// order to promote zero sized derivatives. This however interferes badly with
-// our caching system and produces unexpected behaviors. See #10971 for details.
-// Therefore our implementation stops using internal::make_coherent and treats
-// scalars with zero sized derivatives as constants, as it should.
-//
-// We also provide overloading of math functions for AutoDiffScalar<VectorXd>
-// which return AutoDiffScalar<VectorXd> instead of an expression tree.
-//
-// See https://github.com/RobotLocomotion/drake/issues/6944 for more
-// information. See also drake/common/autodiff_overloads.h.
-//
-// TODO(soonho-tri): Next time when we upgrade Eigen, please check if we still
-// need these specializations.
-template <>
-class AutoDiffScalar<VectorXd>
- : public internal::auto_diff_special_op<VectorXd, false> {
- public:
- typedef internal::auto_diff_special_op<VectorXd, false> Base;
- typedef typename internal::remove_all<VectorXd>::type DerType;
- typedef typename internal::traits<DerType>::Scalar Scalar;
- typedef typename NumTraits<Scalar>::Real Real;
-
- using Base::operator+;
- using Base::operator*;
-
- AutoDiffScalar() {}
-
- AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
- : m_value(value), m_derivatives(DerType::Zero(nbDer)) {
- m_derivatives.coeffRef(derNumber) = Scalar(1);
- }
-
- // NOLINTNEXTLINE(runtime/explicit): Code from Eigen.
- AutoDiffScalar(const Real& value) : m_value(value) {
- if (m_derivatives.size() > 0) m_derivatives.setZero();
- }
-
- AutoDiffScalar(const Scalar& value, const DerType& der)
- : m_value(value), m_derivatives(der) {}
-
- template <typename OtherDerType>
- AutoDiffScalar(
- const AutoDiffScalar<OtherDerType>& other
-#ifndef EIGEN_PARSED_BY_DOXYGEN
- ,
- typename internal::enable_if<
- internal::is_same<
- Scalar, typename internal::traits<typename internal::remove_all<
- OtherDerType>::type>::Scalar>::value,
- void*>::type = 0
-#endif
- )
- : m_value(other.value()), m_derivatives(other.derivatives()) {
- }
-
- friend std::ostream& operator<<(std::ostream& s, const AutoDiffScalar& a) {
- return s << a.value();
- }
-
- AutoDiffScalar(const AutoDiffScalar& other)
- : m_value(other.value()), m_derivatives(other.derivatives()) {}
-
- template <typename OtherDerType>
- inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other) {
- m_value = other.value();
- m_derivatives = other.derivatives();
- return *this;
- }
-
- inline AutoDiffScalar& operator=(const AutoDiffScalar& other) {
- m_value = other.value();
- m_derivatives = other.derivatives();
- return *this;
- }
-
- inline AutoDiffScalar& operator=(const Scalar& other) {
- m_value = other;
- if (m_derivatives.size() > 0) m_derivatives.setZero();
- return *this;
- }
-
- inline const Scalar& value() const { return m_value; }
- inline Scalar& value() { return m_value; }
-
- inline const DerType& derivatives() const { return m_derivatives; }
- inline DerType& derivatives() { return m_derivatives; }
-
- inline bool operator<(const Scalar& other) const { return m_value < other; }
- inline bool operator<=(const Scalar& other) const { return m_value <= other; }
- inline bool operator>(const Scalar& other) const { return m_value > other; }
- inline bool operator>=(const Scalar& other) const { return m_value >= other; }
- inline bool operator==(const Scalar& other) const { return m_value == other; }
- inline bool operator!=(const Scalar& other) const { return m_value != other; }
-
- friend inline bool operator<(const Scalar& a, const AutoDiffScalar& b) {
- return a < b.value();
- }
- friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) {
- return a <= b.value();
- }
- friend inline bool operator>(const Scalar& a, const AutoDiffScalar& b) {
- return a > b.value();
- }
- friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) {
- return a >= b.value();
- }
- friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) {
- return a == b.value();
- }
- friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) {
- return a != b.value();
- }
-
- template <typename OtherDerType>
- inline bool operator<(const AutoDiffScalar<OtherDerType>& b) const {
- return m_value < b.value();
- }
- template <typename OtherDerType>
- inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const {
- return m_value <= b.value();
- }
- template <typename OtherDerType>
- inline bool operator>(const AutoDiffScalar<OtherDerType>& b) const {
- return m_value > b.value();
- }
- template <typename OtherDerType>
- inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const {
- return m_value >= b.value();
- }
- template <typename OtherDerType>
- inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const {
- return m_value == b.value();
- }
- template <typename OtherDerType>
- inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const {
- return m_value != b.value();
- }
-
- inline const AutoDiffScalar<DerType> operator+(const Scalar& other) const {
- return AutoDiffScalar<DerType>(m_value + other, m_derivatives);
- }
-
- friend inline const AutoDiffScalar<DerType> operator+(
- const Scalar& a, const AutoDiffScalar& b) {
- return AutoDiffScalar<DerType>(a + b.value(), b.derivatives());
- }
-
- inline AutoDiffScalar& operator+=(const Scalar& other) {
- value() += other;
- return *this;
- }
-
- template <typename OtherDerType>
- inline const AutoDiffScalar<DerType> operator+(
- const AutoDiffScalar<OtherDerType>& other) const {
- const bool has_this_der = m_derivatives.size() > 0;
- const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
- return MakeAutoDiffScalar(
- m_value + other.value(),
- has_both_der
- ? VectorXd(m_derivatives + other.derivatives())
- : has_this_der ? m_derivatives : VectorXd(other.derivatives()));
- }
-
- template <typename OtherDerType>
- inline AutoDiffScalar& operator+=(const AutoDiffScalar<OtherDerType>& other) {
- (*this) = (*this) + other;
- return *this;
- }
-
- inline const AutoDiffScalar<DerType> operator-(const Scalar& b) const {
- return AutoDiffScalar<DerType>(m_value - b, m_derivatives);
- }
-
- friend inline const AutoDiffScalar<DerType> operator-(
- const Scalar& a, const AutoDiffScalar& b) {
- return AutoDiffScalar<DerType>(a - b.value(), -b.derivatives());
- }
-
- inline AutoDiffScalar& operator-=(const Scalar& other) {
- value() -= other;
- return *this;
- }
-
- template <typename OtherDerType>
- inline const AutoDiffScalar<DerType> operator-(
- const AutoDiffScalar<OtherDerType>& other) const {
- const bool has_this_der = m_derivatives.size() > 0;
- const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
- return MakeAutoDiffScalar(
- m_value - other.value(),
- has_both_der
- ? VectorXd(m_derivatives - other.derivatives())
- : has_this_der ? m_derivatives : VectorXd(-other.derivatives()));
- }
-
- template <typename OtherDerType>
- inline AutoDiffScalar& operator-=(const AutoDiffScalar<OtherDerType>& other) {
- *this = *this - other;
- return *this;
- }
-
- inline const AutoDiffScalar<DerType> operator-() const {
- return AutoDiffScalar<DerType>(-m_value, -m_derivatives);
- }
-
- inline const AutoDiffScalar<DerType> operator*(const Scalar& other) const {
- return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
- }
-
- friend inline const AutoDiffScalar<DerType> operator*(
- const Scalar& other, const AutoDiffScalar& a) {
- return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
- }
-
- inline const AutoDiffScalar<DerType> operator/(const Scalar& other) const {
- return MakeAutoDiffScalar(m_value / other,
- (m_derivatives * (Scalar(1) / other)));
- }
-
- friend inline const AutoDiffScalar<DerType> operator/(
- const Scalar& other, const AutoDiffScalar& a) {
- return MakeAutoDiffScalar(
- other / a.value(),
- a.derivatives() * (Scalar(-other) / (a.value() * a.value())));
- }
-
- template <typename OtherDerType>
- inline const AutoDiffScalar<DerType> operator/(
- const AutoDiffScalar<OtherDerType>& other) const {
- const auto& this_der = m_derivatives;
- const auto& other_der = other.derivatives();
- const bool has_this_der = m_derivatives.size() > 0;
- const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
- const double scale = 1. / (other.value() * other.value());
- return MakeAutoDiffScalar(
- m_value / other.value(),
- has_both_der ?
- VectorXd(this_der * other.value() - other_der * m_value) * scale :
- has_this_der ?
- VectorXd(this_der * other.value()) * scale :
- // has_other_der || has_neither
- VectorXd(other_der * -m_value) * scale);
- }
-
- template <typename OtherDerType>
- inline const AutoDiffScalar<DerType> operator*(
- const AutoDiffScalar<OtherDerType>& other) const {
- const bool has_this_der = m_derivatives.size() > 0;
- const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
- return MakeAutoDiffScalar(
- m_value * other.value(),
- has_both_der ? VectorXd(m_derivatives * other.value() +
- other.derivatives() * m_value)
- : has_this_der ? VectorXd(m_derivatives * other.value())
- : VectorXd(other.derivatives() * m_value));
- }
-
- inline AutoDiffScalar& operator*=(const Scalar& other) {
- *this = *this * other;
- return *this;
- }
-
- template <typename OtherDerType>
- inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) {
- *this = *this * other;
- return *this;
- }
-
- inline AutoDiffScalar& operator/=(const Scalar& other) {
- *this = *this / other;
- return *this;
- }
-
- template <typename OtherDerType>
- inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other) {
- *this = *this / other;
- return *this;
- }
-
- protected:
- Scalar m_value;
- DerType m_derivatives;
-};
-
-#define DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(FUNC, CODE) \
- inline const AutoDiffScalar<VectorXd> FUNC( \
- const AutoDiffScalar<VectorXd>& x) { \
- EIGEN_UNUSED typedef double Scalar; \
- CODE; \
- }
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- abs, using std::abs; return Eigen::MakeAutoDiffScalar(
- abs(x.value()), x.derivatives() * (x.value() < 0 ? -1 : 1));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- abs2, using numext::abs2; return Eigen::MakeAutoDiffScalar(
- abs2(x.value()), x.derivatives() * (Scalar(2) * x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- sqrt, using std::sqrt; Scalar sqrtx = sqrt(x.value());
- return Eigen::MakeAutoDiffScalar(sqrtx,
- x.derivatives() * (Scalar(0.5) / sqrtx));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- cos, using std::cos; using std::sin;
- return Eigen::MakeAutoDiffScalar(cos(x.value()),
- x.derivatives() * (-sin(x.value())));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- sin, using std::sin; using std::cos;
- return Eigen::MakeAutoDiffScalar(sin(x.value()),
- x.derivatives() * cos(x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- exp, using std::exp; Scalar expx = exp(x.value());
- return Eigen::MakeAutoDiffScalar(expx, x.derivatives() * expx);)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- log, using std::log; return Eigen::MakeAutoDiffScalar(
- log(x.value()), x.derivatives() * (Scalar(1) / x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- tan, using std::tan; using std::cos; return Eigen::MakeAutoDiffScalar(
- tan(x.value()),
- x.derivatives() * (Scalar(1) / numext::abs2(cos(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- asin, using std::sqrt; using std::asin; return Eigen::MakeAutoDiffScalar(
- asin(x.value()),
- x.derivatives() * (Scalar(1) / sqrt(1 - numext::abs2(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- acos, using std::sqrt; using std::acos; return Eigen::MakeAutoDiffScalar(
- acos(x.value()),
- x.derivatives() * (Scalar(-1) / sqrt(1 - numext::abs2(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- atan, using std::atan; return Eigen::MakeAutoDiffScalar(
- atan(x.value()),
- x.derivatives() * (Scalar(1) / (1 + x.value() * x.value())));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- tanh, using std::cosh; using std::tanh; return Eigen::MakeAutoDiffScalar(
- tanh(x.value()),
- x.derivatives() * (Scalar(1) / numext::abs2(cosh(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- sinh, using std::sinh; using std::cosh;
- return Eigen::MakeAutoDiffScalar(sinh(x.value()),
- x.derivatives() * cosh(x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
- cosh, using std::sinh; using std::cosh;
- return Eigen::MakeAutoDiffScalar(cosh(x.value()),
- x.derivatives() * sinh(x.value()));)
-
-#undef DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY
-
-// We have this specialization here because the Eigen-3.3.3's atan2
-// implementation for AutoDiffScalar does not make a return with properly sized
-// derivatives.
-inline const AutoDiffScalar<VectorXd> atan2(const AutoDiffScalar<VectorXd>& a,
- const AutoDiffScalar<VectorXd>& b) {
- const bool has_a_der = a.derivatives().size() > 0;
- const bool has_both_der = has_a_der && (b.derivatives().size() > 0);
- const double squared_hypot = a.value() * a.value() + b.value() * b.value();
- return MakeAutoDiffScalar(
- std::atan2(a.value(), b.value()),
- VectorXd((has_both_der
- ? VectorXd(a.derivatives() * b.value() -
- a.value() * b.derivatives())
- : has_a_der ? VectorXd(a.derivatives() * b.value())
- : VectorXd(-a.value() * b.derivatives())) /
- squared_hypot));
-}
-
-inline const AutoDiffScalar<VectorXd> pow(const AutoDiffScalar<VectorXd>& a,
- double b) {
- using std::pow;
- return MakeAutoDiffScalar(pow(a.value(), b),
- a.derivatives() * (b * pow(a.value(), b - 1)));
-}
-
-#endif
-
-} // namespace Eigen
diff --git a/wpimath/src/test/native/include/drake/common/cond.h b/wpimath/src/test/native/include/drake/common/cond.h
deleted file mode 100644
index 16dd21e..0000000
--- a/wpimath/src/test/native/include/drake/common/cond.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include <functional>
-#include <type_traits>
-
-#include "drake/common/double_overloads.h"
-
-namespace drake {
-/** @name cond
- Constructs conditional expression (similar to Lisp's cond).
-
- @verbatim
- cond(cond_1, expr_1,
- cond_2, expr_2,
- ..., ...,
- cond_n, expr_n,
- expr_{n+1})
- @endverbatim
-
- The value returned by the above cond expression is @c expr_1 if @c cond_1 is
- true; else if @c cond_2 is true then @c expr_2; ... ; else if @c cond_n is
- true then @c expr_n. If none of the conditions are true, it returns @c
- expr_{n+1}.
-
- @note This functions assumes that @p ScalarType provides @c operator< and the
- type of @c f_cond is the type of the return type of <tt>operator<(ScalarType,
- ScalarType)</tt>. For example, @c symbolic::Expression can be used as a @p
- ScalarType because it provides <tt>symbolic::Formula
- operator<(symbolic::Expression, symbolic::Expression)</tt>.
-
-
- @{
- */
-template <typename ScalarType>
-ScalarType cond(const ScalarType& e) {
- return e;
-}
-template <typename ScalarType, typename... Rest>
-ScalarType cond(const decltype(ScalarType() < ScalarType()) & f_cond,
- const ScalarType& e_then, Rest... rest) {
- return if_then_else(f_cond, e_then, cond(rest...));
-}
-///@}
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/common/constants.h b/wpimath/src/test/native/include/drake/common/constants.h
deleted file mode 100644
index 0ccddca..0000000
--- a/wpimath/src/test/native/include/drake/common/constants.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#pragma once
-
-namespace drake {
-
-constexpr int kQuaternionSize = 4;
-
-constexpr int kSpaceDimension = 3;
-
-constexpr int kRpySize = 3;
-
-/// https://en.wikipedia.org/wiki/Screw_theory#Twist
-constexpr int kTwistSize = 6;
-
-/// http://www.euclideanspace.com/maths/geometry/affine/matrix4x4/
-constexpr int kHomogeneousTransformSize = 16;
-
-const int kRotmatSize = kSpaceDimension * kSpaceDimension;
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/common/double_overloads.h b/wpimath/src/test/native/include/drake/common/double_overloads.h
deleted file mode 100644
index 125f113..0000000
--- a/wpimath/src/test/native/include/drake/common/double_overloads.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/// @file
-/// Provides necessary operations on double to have it as a ScalarType in drake.
-
-#pragma once
-
-namespace drake {
-/// Provides if-then-else expression for double. The value returned by the
-/// if-then-else expression is @p v_then if @p f_cond is @c true. Otherwise, it
-/// returns @p v_else.
-
-/// The semantics is similar but not exactly the same as C++'s conditional
-/// expression constructed by its ternary operator, @c ?:. In
-/// <tt>if_then_else(f_cond, v_then, v_else)</tt>, both of @p v_then and @p
-/// v_else are evaluated regardless of the evaluation of @p f_cond. In contrast,
-/// only one of @p v_then or @p v_else is evaluated in C++'s conditional
-/// expression <tt>f_cond ? v_then : v_else</tt>.
-inline double if_then_else(bool f_cond, double v_then, double v_else) {
- return f_cond ? v_then : v_else;
-}
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/common/drake_deprecated.h b/wpimath/src/test/native/include/drake/common/drake_deprecated.h
deleted file mode 100644
index 5ce6328..0000000
--- a/wpimath/src/test/native/include/drake/common/drake_deprecated.h
+++ /dev/null
@@ -1,65 +0,0 @@
-#pragma once
-
-/** @file
-Provides a portable macro for use in generating compile-time warnings for
-use of code that is permitted but discouraged. */
-
-#ifdef DRAKE_DOXYGEN_CXX
-/** Use `DRAKE_DEPRECATED("removal_date", "message")` to discourage use of
-certain APIs. It can be used on classes, typedefs, variables, non-static data
-members, functions, arguments, enumerations, and template specializations. When
-code refers to the deprecated item, a compile time warning will be issued
-displaying the given message, preceded by "DRAKE DEPRECATED: ". The Doxygen API
-reference will show that the API is deprecated, along with the message.
-
-This is typically used for constructs that have been replaced by something
-better and it is good practice to suggest the appropriate replacement in the
-deprecation message. Deprecation warnings are conventionally used to convey to
-users that a feature they are depending on will be removed in a future release.
-
-Every deprecation notice must include a date (as YYYY-MM-DD string) where the
-deprecated item is planned for removal. Future commits may change the date
-(e.g., delaying the removal) but should generally always reflect the best
-current expectation for removal.
-
-Absent any other particular need, we prefer to use a deprecation period of
-three months by default, often rounded up to the next first of the month. So
-for code announced as deprecated on 2018-01-22 the removal_date would nominally
-be set to 2018-05-01.
-
-Try to keep the date string immediately after the DRAKE_DEPRECATED macro name,
-even if the message itself must be wrapped to a new line:
-@code
- DRAKE_DEPRECATED("2038-01-19",
- "foo is being replaced with a safer, const-method named bar().")
- int foo();
-@endcode
-
-Sample uses: @code
- // Attribute comes *before* declaration of a deprecated function or variable;
- // no semicolon is allowed.
- DRAKE_DEPRECATED("2038-01-19", "f() is slow; use g() instead.")
- int f(int arg);
-
- // Attribute comes *after* struct, class, enum keyword.
- class DRAKE_DEPRECATED("2038-01-19", "Use MyNewClass instead.")
- MyClass {
- };
-
- // Type alias goes before the '='.
- using NewType
- DRAKE_DEPRECATED("2038-01-19", "Use NewType instead.")
- = OldType;
-@endcode
-*/
-#define DRAKE_DEPRECATED(removal_date, message)
-
-#else // DRAKE_DOXYGEN_CXX
-
-#define DRAKE_DEPRECATED(removal_date, message) \
- [[deprecated( \
- "\nDRAKE DEPRECATED: " message \
- "\nThe deprecated code will be removed from Drake" \
- " on or after " removal_date ".")]]
-
-#endif // DRAKE_DOXYGEN_CXX
diff --git a/wpimath/src/test/native/include/drake/common/drake_nodiscard.h b/wpimath/src/test/native/include/drake/common/drake_nodiscard.h
deleted file mode 100644
index 29f078d..0000000
--- a/wpimath/src/test/native/include/drake/common/drake_nodiscard.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#pragma once
-
-// TODO(jwnimmer-tri) Once we are in --std=c++17 mode as our minimum version,
-// we can remove this file and just say [[nodiscard]] directly everywhere.
-
-#if defined(DRAKE_DOXYGEN_CXX) || __has_cpp_attribute(nodiscard)
-/** Synonym for [[nodiscard]], iff the current compiler supports it;
-see https://en.cppreference.com/w/cpp/language/attributes/nodiscard. */
-// NOLINTNEXTLINE(whitespace/braces)
-#define DRAKE_NODISCARD [[nodiscard]]
-#else
-#define DRAKE_NODISCARD
-#endif
diff --git a/wpimath/src/test/native/include/drake/common/dummy_value.h b/wpimath/src/test/native/include/drake/common/dummy_value.h
deleted file mode 100644
index b9c616a..0000000
--- a/wpimath/src/test/native/include/drake/common/dummy_value.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <limits>
-
-namespace drake {
-
-/// Provides a "dummy" value for a ScalarType -- a value that is unlikely to be
-/// mistaken for a purposefully-computed value, useful for initializing a value
-/// before the true result is available.
-///
-/// Defaults to using std::numeric_limits::quiet_NaN when available; it is a
-/// compile-time error to call the unspecialized dummy_value::get() when
-/// quiet_NaN is unavailable.
-///
-/// See autodiff_overloads.h to use this with Eigen's AutoDiffScalar.
-template <typename T>
-struct dummy_value {
- static constexpr T get() {
- static_assert(std::numeric_limits<T>::has_quiet_NaN,
- "Custom scalar types should specialize this struct");
- return std::numeric_limits<T>::quiet_NaN();
- }
-};
-
-template <>
-struct dummy_value<int> {
- static constexpr int get() {
- // D is for "Dummy". We assume as least 32 bits (per cppguide) -- if `int`
- // is larger than 32 bits, this will leave some fraction of the bytes zero
- // instead of 0xDD, but that's okay.
- return 0xDDDDDDDD;
- }
-};
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h
deleted file mode 100644
index 49175ce..0000000
--- a/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#pragma once
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <limits>
-
-// Eigen provides `numeric_limits<AutoDiffScalar<T>>` starting with v3.3.4.
-#if !EIGEN_VERSION_AT_LEAST(3, 3, 4) // Eigen Version < v3.3.4
-
-namespace std {
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T>>
- : public numeric_limits<typename T::Scalar> {};
-
-} // namespace std
-
-#endif // Eigen Version < v3.3.4
diff --git a/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h
deleted file mode 100644
index 10ffec6..0000000
--- a/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#pragma once
-
-/// @file
-/// This file contains abbreviated definitions for certain uses of
-/// AutoDiffScalar that are commonly used in Drake.
-/// @see also eigen_types.h
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <type_traits>
-
-#include <Eigen/Core>
-
-#include "drake/common/eigen_types.h"
-
-namespace drake {
-
-/// An autodiff variable with a dynamic number of partials.
-using AutoDiffXd = Eigen::AutoDiffScalar<Eigen::VectorXd>;
-
-// TODO(hongkai-dai): Recursive template to get arbitrary gradient order.
-
-/// An autodiff variable with `num_vars` partials.
-template <int num_vars>
-using AutoDiffd = Eigen::AutoDiffScalar<Eigen::Matrix<double, num_vars, 1> >;
-
-/// A vector of `rows` autodiff variables, each with `num_vars` partials.
-template <int num_vars, int rows>
-using AutoDiffVecd = Eigen::Matrix<AutoDiffd<num_vars>, rows, 1>;
-
-/// A dynamic-sized vector of autodiff variables, each with a dynamic-sized
-/// vector of partials.
-typedef AutoDiffVecd<Eigen::Dynamic, Eigen::Dynamic> AutoDiffVecXd;
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/common/eigen_types.h b/wpimath/src/test/native/include/drake/common/eigen_types.h
deleted file mode 100644
index abe3e0b..0000000
--- a/wpimath/src/test/native/include/drake/common/eigen_types.h
+++ /dev/null
@@ -1,461 +0,0 @@
-#pragma once
-
-/// @file
-/// This file contains abbreviated definitions for certain specializations of
-/// Eigen::Matrix that are commonly used in Drake.
-/// These convenient definitions are templated on the scalar type of the Eigen
-/// object. While Drake uses `<T>` for scalar types across the entire code base
-/// we decided in this file to use `<Scalar>` to be more consistent with the
-/// usage of `<Scalar>` in Eigen's code base.
-/// @see also eigen_autodiff_types.h
-
-#include <utility>
-
-#include <Eigen/Core>
-
-#include "drake/common/constants.h"
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_copyable.h"
-#include "drake/common/drake_deprecated.h"
-
-namespace drake {
-
-/// The empty column vector (zero rows, one column), templated on scalar type.
-template <typename Scalar>
-using Vector0 = Eigen::Matrix<Scalar, 0, 1>;
-
-/// A column vector of size 1 (that is, a scalar), templated on scalar type.
-template <typename Scalar>
-using Vector1 = Eigen::Matrix<Scalar, 1, 1>;
-
-/// A column vector of size 1 of doubles.
-using Vector1d = Eigen::Matrix<double, 1, 1>;
-
-/// A column vector of size 2, templated on scalar type.
-template <typename Scalar>
-using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
-
-/// A column vector of size 3, templated on scalar type.
-template <typename Scalar>
-using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
-
-/// A column vector of size 4, templated on scalar type.
-template <typename Scalar>
-using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
-
-/// A column vector of size 6.
-template <typename Scalar>
-using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
-
-/// A column vector templated on the number of rows.
-template <typename Scalar, int Rows>
-using Vector = Eigen::Matrix<Scalar, Rows, 1>;
-
-/// A column vector of any size, templated on scalar type.
-template <typename Scalar>
-using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
-
-/// A vector of dynamic size templated on scalar type, up to a maximum of 6
-/// elements.
-template <typename Scalar>
-using VectorUpTo6 = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, 6, 1>;
-
-/// A row vector of size 2, templated on scalar type.
-template <typename Scalar>
-using RowVector2 = Eigen::Matrix<Scalar, 1, 2>;
-
-/// A row vector of size 3, templated on scalar type.
-template <typename Scalar>
-using RowVector3 = Eigen::Matrix<Scalar, 1, 3>;
-
-/// A row vector of size 4, templated on scalar type.
-template <typename Scalar>
-using RowVector4 = Eigen::Matrix<Scalar, 1, 4>;
-
-/// A row vector of size 6.
-template <typename Scalar>
-using RowVector6 = Eigen::Matrix<Scalar, 1, 6>;
-
-/// A row vector templated on the number of columns.
-template <typename Scalar, int Cols>
-using RowVector = Eigen::Matrix<Scalar, 1, Cols>;
-
-/// A row vector of any size, templated on scalar type.
-template <typename Scalar>
-using RowVectorX = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
-
-
-/// A matrix of 2 rows and 2 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
-
-/// A matrix of 3 rows and 3 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
-
-/// A matrix of 4 rows and 4 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-
-/// A matrix of 6 rows and 6 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
-
-/// A matrix of 2 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
-
-/// A matrix of 3 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
-
-/// A matrix of 4 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
-
-/// A matrix of 6 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
-
-/// A matrix of dynamic size, templated on scalar type.
-template <typename Scalar>
-using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
-
-/// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows
-/// and 6 columns. Rectangular matrices, with different number of rows and
-/// columns, are allowed.
-template <typename Scalar>
-using MatrixUpTo6 =
-Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
-
-/// A quaternion templated on scalar type.
-template <typename Scalar>
-using Quaternion = Eigen::Quaternion<Scalar>;
-
-/// An AngleAxis templated on scalar type.
-template <typename Scalar>
-using AngleAxis = Eigen::AngleAxis<Scalar>;
-
-/// An Isometry templated on scalar type.
-template <typename Scalar>
-using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
-
-/// A translation in 3D templated on scalar type.
-template <typename Scalar>
-using Translation3 = Eigen::Translation<Scalar, 3>;
-
-/// A column vector consisting of one twist.
-template <typename Scalar>
-using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
-
-/// A matrix with one twist per column, and dynamically many columns.
-template <typename Scalar>
-using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
-
-/// A six-by-six matrix.
-template <typename Scalar>
-using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
-
-/// A column vector consisting of one wrench (spatial force) = `[r X f; f]`,
-/// where f is a force (translational force) applied at a point `P` and `r` is
-/// the position vector from a point `O` (called the "moment center") to point
-/// `P`.
-template <typename Scalar>
-using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
-
-/// A column vector consisting of a concatenated rotational and translational
-/// force. The wrench is a special case of a SpatialForce. For a general
-/// SpatialForce the rotational force can be a pure torque or the accumulation
-/// of moments and need not necessarily be a function of the force term.
-template <typename Scalar>
-using SpatialForce
-DRAKE_DEPRECATED("2019-10-15", "Please use Vector6<> instead.")
- = Eigen::Matrix<Scalar, 6, 1>;
-
-/// EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time
-/// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by
-/// Dynamic, followed by other finite values.
-///
-/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC
-/// macro in "Eigen/Core/util/Macros.h".
-template <int a, int b>
-struct EigenSizeMinPreferDynamic {
- // clang-format off
- static constexpr int value = (a == 0 || b == 0) ? 0 :
- (a == 1 || b == 1) ? 1 :
- (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic :
- a <= b ? a : b;
- // clang-format on
-};
-
-/// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The
-/// difference is that finite values now have priority over Dynamic, so that
-/// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3.
-///
-/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro
-/// in "Eigen/Core/util/Macros.h".
-template <int a, int b>
-struct EigenSizeMinPreferFixed {
- // clang-format off
- static constexpr int value = (a == 0 || b == 0) ? 0 :
- (a == 1 || b == 1) ? 1 :
- (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic :
- (a == Eigen::Dynamic) ? b :
- (b == Eigen::Dynamic) ? a :
- a <= b ? a : b;
- // clang-format on
-};
-
-/// MultiplyEigenSizes<a, b> gives a * b if both of a and b are fixed
-/// sizes. Otherwise it gives Eigen::Dynamic.
-template <int a, int b>
-struct MultiplyEigenSizes {
- static constexpr int value =
- (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b;
-};
-
-/*
- * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>,
- * MatrixBase<>).
- */
-template <typename Derived>
-struct is_eigen_type : std::is_base_of<Eigen::EigenBase<Derived>, Derived> {};
-
-/*
- * Determines if an EigenBase<> has a specific scalar type.
- */
-template <typename Derived, typename Scalar>
-struct is_eigen_scalar_same
- : std::integral_constant<
- bool, is_eigen_type<Derived>::value &&
- std::is_same<typename Derived::Scalar, Scalar>::value> {};
-
-/*
- * Determines if an EigenBase<> type is a compile-time (column) vector.
- * This will not check for run-time size.
- */
-template <typename Derived>
-struct is_eigen_vector
- : std::integral_constant<bool, is_eigen_type<Derived>::value &&
- Derived::ColsAtCompileTime == 1> {};
-
-/*
- * Determines if an EigenBase<> type is a compile-time (column) vector of a
- * scalar type. This will not check for run-time size.
- */
-template <typename Derived, typename Scalar>
-struct is_eigen_vector_of
- : std::integral_constant<
- bool, is_eigen_scalar_same<Derived, Scalar>::value &&
- is_eigen_vector<Derived>::value> {};
-
-// TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and
-// this logic will qualify it as a vector. Address the downstream logic if this
-// becomes an issue.
-/*
- * Determines if a EigenBase<> type is a compile-time non-column-vector matrix
- * of a scalar type. This will not check for run-time size.
- * @note For an EigenBase<> of the correct Scalar type, this logic is
- * exclusive to is_eigen_vector_of<> such that distinct specializations are not
- * ambiguous.
- */
-template <typename Derived, typename Scalar>
-struct is_eigen_nonvector_of
- : std::integral_constant<
- bool, is_eigen_scalar_same<Derived, Scalar>::value &&
- !is_eigen_vector<Derived>::value> {};
-
-// TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if
-// appropriate.
-
-/// This wrapper class provides a way to write non-template functions taking raw
-/// pointers to Eigen objects as parameters while limiting the number of copies,
-/// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref<T>`
-/// and provides access to it via `operator*` and `operator->`.
-///
-/// The motivation of this class is to follow <a
-/// href="https://google.github.io/styleguide/cppguide.html#Reference_Arguments">GSG's
-/// "output arguments should be pointers" rule</a> while taking advantage of
-/// using `Eigen::Ref`. Here is an example.
-///
-/// @code
-/// // This function is taking an Eigen::Ref of a matrix and modifies it in
-/// // the body. This violates GSG's rule on output parameters.
-/// void foo(Eigen::Ref<Eigen::MatrixXd> M) {
-/// M(0, 0) = 0;
-/// }
-/// // At Call-site, we have:
-/// foo(M);
-/// foo(M.block(0, 0, 2, 2));
-///
-/// // We can rewrite the above function into the following using EigenPtr.
-/// void foo(EigenPtr<Eigen::MatrixXd> M) {
-/// (*M)(0, 0) = 0;
-/// }
-/// // Note that, call sites should be changed to:
-/// foo(&M);
-///
-/// // We need tmp to avoid taking the address of a temporary object such as the
-/// // return value of .block().
-/// auto tmp = M.block(0, 0, 2, 2);
-/// foo(&tmp);
-/// @endcode
-///
-/// Notice that methods taking an EigenPtr can mutate the entries of a matrix as
-/// in method `foo()` in the example code above, but cannot change its size.
-/// This is because `operator*` and `operator->` return an `Eigen::Ref<T>`
-/// object and only plain matrices/arrays can be resized and not expressions.
-/// This **is** the desired behavior, since resizing the block of a matrix or
-/// even a more general expression should not be allowed. If you do want to be
-/// able to resize a mutable matrix argument, then you must pass it as a
-/// `Matrix<T>*`, like so:
-/// @code
-/// void bar(Eigen::MatrixXd* M) {
-/// DRAKE_THROW_UNLESS(M != nullptr);
-/// // In this case this method only works with 4x3 matrices.
-/// if (M->rows() != 4 && M->cols() != 3) {
-/// M->resize(4, 3);
-/// }
-/// (*M)(0, 0) = 0;
-/// }
-/// @endcode
-///
-/// @note This class provides a way to avoid the `const_cast` hack introduced in
-/// <a
-/// href="https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#TopicPlainFunctionsFailing">Eigen's
-/// documentation</a>.
-template <typename PlainObjectType>
-class EigenPtr {
- public:
- typedef Eigen::Ref<PlainObjectType> RefType;
-
- EigenPtr() : EigenPtr(nullptr) {}
-
- /// Overload for `nullptr`.
- // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
- EigenPtr(std::nullptr_t) {}
-
- /// Constructs with a reference to the given matrix type.
- // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
- EigenPtr(const EigenPtr& other) { assign(other); }
-
- /// Constructs with a reference to another matrix type.
- /// May be `nullptr`.
- template <typename PlainObjectTypeIn>
- // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
- EigenPtr(PlainObjectTypeIn* m) {
- if (m) {
- m_.set_value(m);
- }
- }
-
- /// Constructs from another EigenPtr.
- template <typename PlainObjectTypeIn>
- // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
- EigenPtr(const EigenPtr<PlainObjectTypeIn>& other) {
- // Cannot directly construct `m_` from `other.m_`.
- assign(other);
- }
-
- EigenPtr& operator=(const EigenPtr& other) {
- // We must explicitly override this version of operator=.
- // The template below will not take precedence over this one.
- return assign(other);
- }
-
- template <typename PlainObjectTypeIn>
- EigenPtr& operator=(const EigenPtr<PlainObjectTypeIn>& other) {
- return assign(other);
- }
-
- /// @throws std::runtime_error if this is a null dereference.
- RefType& operator*() const { return get_reference(); }
-
- /// @throws std::runtime_error if this is a null dereference.
- RefType* operator->() const { return &get_reference(); }
-
- /// Returns whether or not this contains a valid reference.
- operator bool() const { return is_valid(); }
-
- bool operator==(std::nullptr_t) const { return !is_valid(); }
-
- bool operator!=(std::nullptr_t) const { return is_valid(); }
-
- private:
- // Simple reassignable container without requirement of heap allocation.
- // This is used because `drake::optional<>` does not work with `Eigen::Ref<>`
- // because `Ref` deletes the necessary `operator=` overload for
- // `std::is_copy_assignable`.
- class ReassignableRef {
- public:
- DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ReassignableRef)
- ReassignableRef() {}
- ~ReassignableRef() {
- reset();
- }
-
- // Reset value to null.
- void reset() {
- if (has_value_) {
- raw_value().~RefType();
- has_value_ = false;
- }
- }
-
- // Set value.
- template <typename PlainObjectTypeIn>
- void set_value(PlainObjectTypeIn* value_in) {
- if (has_value_) {
- raw_value().~RefType();
- }
- new (&raw_value()) RefType(*value_in);
- has_value_ = true;
- }
-
- // Access to value.
- RefType& value() {
- DRAKE_ASSERT(has_value());
- return raw_value();
- }
-
- // Indicates if it has a value.
- bool has_value() const { return has_value_; }
-
- private:
- // Unsafe access to value.
- RefType& raw_value() { return reinterpret_cast<RefType&>(storage_); }
-
- bool has_value_{};
- typename std::aligned_storage<sizeof(RefType), alignof(RefType)>::type
- storage_;
- };
-
- // Use mutable, reassignable ref to permit pointer-like semantics (with
- // ownership) on the stack.
- mutable ReassignableRef m_;
-
- // Consolidate assignment here, so that both the copy constructor and the
- // construction from another type may be used.
- template <typename PlainObjectTypeIn>
- EigenPtr& assign(const EigenPtr<PlainObjectTypeIn>& other) {
- if (other) {
- m_.set_value(&(*other));
- } else {
- m_.reset();
- }
- return *this;
- }
-
- // Consolidate getting a reference here.
- RefType& get_reference() const {
- if (!m_.has_value())
- throw std::runtime_error("EigenPtr: nullptr dereference");
- return m_.value();
- }
-
- bool is_valid() const {
- return m_.has_value();
- }
-};
-
-} // namespace drake
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
index 8847d87..d6bcbb8 100644
--- 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
@@ -7,7 +7,7 @@
#include <Eigen/Core>
#include <gtest/gtest.h>
-#include "drake/common/drake_nodiscard.h"
+// #include "drake/common/text_logging.h"
namespace drake {
@@ -28,7 +28,7 @@
* @return true if the two matrices are equal based on the specified tolerance.
*/
template <typename DerivedA, typename DerivedB>
-DRAKE_NODISCARD ::testing::AssertionResult CompareMatrices(
+[[nodiscard]] ::testing::AssertionResult CompareMatrices(
const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
MatrixCompareType compare_type = MatrixCompareType::absolute) {
diff --git a/wpimath/src/test/native/include/drake/common/unused.h b/wpimath/src/test/native/include/drake/common/unused.h
deleted file mode 100644
index 5a28b01..0000000
--- a/wpimath/src/test/native/include/drake/common/unused.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#pragma once
-
-namespace drake {
-
-/// Documents the argument(s) as unused, placating GCC's -Wunused-parameter
-/// warning. This can be called within function bodies to mark that certain
-/// parameters are unused.
-///
-/// When possible, removing the unused parameter is better than placating the
-/// warning. However, in some cases the parameter is part of a virtual API or
-/// template concept that is used elsewhere, so we can't remove it. In those
-/// cases, this function might be an appropriate work-around.
-///
-/// Here's rough advice on how to fix Wunused-parameter warnings:
-///
-/// (1) If the parameter can be removed entirely, prefer that as the first
-/// choice. (This may not be possible if, e.g., a method must match some
-/// virtual API or template concept.)
-///
-/// (2) Unless the parameter name has acute value, prefer to omit the name of
-/// the parameter, leaving only the type, e.g.
-/// @code
-/// void Print(const State& state) override { /* No state to print. */ }
-/// @endcode
-/// changes to
-/// @code
-/// void Print(const State&) override { /* No state to print. */}
-/// @endcode
-/// This no longer triggers the warning and further makes it clear that a
-/// parameter required by the API is definitively unused in the function.
-///
-/// This is an especially good solution in the context of method
-/// definitions (vs declarations); the parameter name used in a definition
-/// is entirely irrelevant to Doxygen and most readers.
-///
-/// (3) When leaving the parameter name intact has acute value, it is
-/// acceptable to keep the name and mark it `unused`. For example, when
-/// the name appears as part of a virtual method's base class declaration,
-/// the name is used by Doxygen to document the method, e.g.,
-/// @code
-/// /** Sets the default State of a System. This default implementation is to
-/// set all zeros. Subclasses may override to use non-zero defaults. The
-/// custom defaults may be based on the given @p context, when relevant. */
-/// virtual void SetDefault(const Context<T>& context, State<T>* state) const {
-/// unused(context);
-/// state->SetZero();
-/// }
-/// @endcode
-///
-template <typename ... Args>
-void unused(const Args& ...) {}
-
-} // namespace drake
diff --git a/wpimath/src/test/native/include/drake/math/autodiff.h b/wpimath/src/test/native/include/drake/math/autodiff.h
deleted file mode 100644
index 52edb11..0000000
--- a/wpimath/src/test/native/include/drake/math/autodiff.h
+++ /dev/null
@@ -1,332 +0,0 @@
-/// @file
-/// Utilities for arithmetic on AutoDiffScalar.
-
-// TODO(russt): rename methods to be GSG compliant.
-
-#pragma once
-
-#include <cmath>
-#include <tuple>
-
-#include <Eigen/Core>
-
-#include "drake/common/autodiff.h"
-#include "drake/common/unused.h"
-
-namespace drake {
-namespace math {
-
-template <typename Derived>
-struct AutoDiffToValueMatrix {
- typedef typename Eigen::Matrix<typename Derived::Scalar::Scalar,
- Derived::RowsAtCompileTime,
- Derived::ColsAtCompileTime> type;
-};
-
-template <typename Derived>
-typename AutoDiffToValueMatrix<Derived>::type autoDiffToValueMatrix(
- const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
- typename AutoDiffToValueMatrix<Derived>::type ret(auto_diff_matrix.rows(),
- auto_diff_matrix.cols());
- for (int i = 0; i < auto_diff_matrix.rows(); i++) {
- for (int j = 0; j < auto_diff_matrix.cols(); ++j) {
- ret(i, j) = auto_diff_matrix(i, j).value();
- }
- }
- return ret;
-}
-
-/** `B = DiscardGradient(A)` enables casting from a matrix of AutoDiffScalars
- * to AutoDiffScalar::Scalar type, explicitly throwing away any gradient
- * information. For a matrix of type, e.g. `MatrixX<AutoDiffXd> A`, the
- * comparable operation
- * `B = A.cast<double>()`
- * should (and does) fail to compile. Use `DiscardGradient(A)` if you want to
- * force the cast (and explicitly declare that information is lost).
- *
- * This method is overloaded to permit the user to call it for double types and
- * AutoDiffScalar types (to avoid the calling function having to handle the
- * two cases differently).
- *
- * @see DiscardZeroGradient
- */
-template <typename Derived>
-typename std::enable_if<
- !std::is_same<typename Derived::Scalar, double>::value,
- Eigen::Matrix<typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime,
- Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime,
- Derived::MaxColsAtCompileTime>>::type
-DiscardGradient(const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
- return autoDiffToValueMatrix(auto_diff_matrix);
-}
-
-/// @see DiscardGradient().
-template <typename Derived>
-typename std::enable_if<
- std::is_same<typename Derived::Scalar, double>::value,
- const Eigen::MatrixBase<Derived>&>::type
-DiscardGradient(const Eigen::MatrixBase<Derived>& matrix) {
- return matrix;
-}
-
-/// @see DiscardGradient().
-template <typename _Scalar, int _Dim, int _Mode, int _Options>
-typename std::enable_if<
- !std::is_same<_Scalar, double>::value,
- Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>>::type
-DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&
- auto_diff_transform) {
- return Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>(
- autoDiffToValueMatrix(auto_diff_transform.matrix()));
-}
-
-/// @see DiscardGradient().
-template <typename _Scalar, int _Dim, int _Mode, int _Options>
-typename std::enable_if<std::is_same<_Scalar, double>::value,
- const Eigen::Transform<_Scalar, _Dim, _Mode,
- _Options>&>::type
-DiscardGradient(
- const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform) {
- return transform;
-}
-
-
-/** \brief Initialize a single autodiff matrix given the corresponding value
- *matrix.
- *
- * Set the values of \p auto_diff_matrix to be equal to \p val, and for each
- *element i of \p auto_diff_matrix,
- * resize the derivatives vector to \p num_derivatives, and set derivative
- *number \p deriv_num_start + i to one (all other elements of the derivative
- *vector set to zero).
- *
- * \param[in] mat 'regular' matrix of values
- * \param[out] ret AutoDiff matrix
- * \param[in] num_derivatives the size of the derivatives vector @default the
- *size of mat
- * \param[in] deriv_num_start starting index into derivative vector (i.e.
- *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
- *@default 0
- */
-template <typename Derived, typename DerivedAutoDiff>
-void initializeAutoDiff(const Eigen::MatrixBase<Derived>& val,
- // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
- Eigen::MatrixBase<DerivedAutoDiff>& auto_diff_matrix,
- Eigen::DenseIndex num_derivatives = Eigen::Dynamic,
- Eigen::DenseIndex deriv_num_start = 0) {
- using ADScalar = typename DerivedAutoDiff::Scalar;
- static_assert(static_cast<int>(Derived::RowsAtCompileTime) ==
- static_cast<int>(DerivedAutoDiff::RowsAtCompileTime),
- "auto diff matrix has wrong number of rows at compile time");
- static_assert(static_cast<int>(Derived::ColsAtCompileTime) ==
- static_cast<int>(DerivedAutoDiff::ColsAtCompileTime),
- "auto diff matrix has wrong number of columns at compile time");
-
- if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size();
-
- auto_diff_matrix.resize(val.rows(), val.cols());
- Eigen::DenseIndex deriv_num = deriv_num_start;
- for (Eigen::DenseIndex i = 0; i < val.size(); i++) {
- auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++);
- }
-}
-
-/** \brief The appropriate AutoDiffScalar gradient type given the value type and
- * the number of derivatives at compile time
- */
-template <typename Derived, int Nq>
-using AutoDiffMatrixType = Eigen::Matrix<
- Eigen::AutoDiffScalar<Eigen::Matrix<typename Derived::Scalar, Nq, 1>>,
- Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0,
- Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>;
-
-/** \brief Initialize a single autodiff matrix given the corresponding value
- *matrix.
- *
- * Create autodiff matrix that matches \p mat in size with derivatives of
- *compile time size \p Nq and runtime size \p num_derivatives.
- * Set its values to be equal to \p val, and for each element i of \p
- *auto_diff_matrix, set derivative number \p deriv_num_start + i to one (all
- *other derivatives set to zero).
- *
- * \param[in] mat 'regular' matrix of values
- * \param[in] num_derivatives the size of the derivatives vector @default the
- *size of mat
- * \param[in] deriv_num_start starting index into derivative vector (i.e.
- *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
- *@default 0
- * \return AutoDiff matrix
- */
-template <int Nq = Eigen::Dynamic, typename Derived>
-AutoDiffMatrixType<Derived, Nq> initializeAutoDiff(
- const Eigen::MatrixBase<Derived>& mat,
- Eigen::DenseIndex num_derivatives = -1,
- Eigen::DenseIndex deriv_num_start = 0) {
- if (num_derivatives == -1) num_derivatives = mat.size();
-
- AutoDiffMatrixType<Derived, Nq> ret(mat.rows(), mat.cols());
- initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start);
- return ret;
-}
-
-namespace internal {
-template <typename Derived, typename Scalar>
-struct ResizeDerivativesToMatchScalarImpl {
- // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
- static void run(Eigen::MatrixBase<Derived>&, const Scalar&) {}
-};
-
-template <typename Derived, typename DerivType>
-struct ResizeDerivativesToMatchScalarImpl<Derived,
- Eigen::AutoDiffScalar<DerivType>> {
- using Scalar = Eigen::AutoDiffScalar<DerivType>;
- // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
- static void run(Eigen::MatrixBase<Derived>& mat, const Scalar& scalar) {
- for (int i = 0; i < mat.size(); i++) {
- auto& derivs = mat(i).derivatives();
- if (derivs.size() == 0) {
- derivs.resize(scalar.derivatives().size());
- derivs.setZero();
- }
- }
- }
-};
-} // namespace internal
-
-/** Resize derivatives vector of each element of a matrix to to match the size
- * of the derivatives vector of a given scalar.
- * \brief If the mat and scalar inputs are AutoDiffScalars, resize the
- * derivatives vector of each element of the matrix mat to match
- * the number of derivatives of the scalar. This is useful in functions that
- * return matrices that do not depend on an AutoDiffScalar
- * argument (e.g. a function with a constant output), while it is desired that
- * information about the number of derivatives is preserved.
- * \param mat matrix, for which the derivative vectors of the elements will be
- * resized
- * \param scalar scalar to match the derivative size vector against.
- */
-template <typename Derived>
-// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
-void resizeDerivativesToMatchScalar(Eigen::MatrixBase<Derived>& mat,
- const typename Derived::Scalar& scalar) {
- internal::ResizeDerivativesToMatchScalarImpl<
- Derived, typename Derived::Scalar>::run(mat, scalar);
-}
-
-namespace internal {
-/** \brief Helper for totalSizeAtCompileTime function (recursive)
- */
-template <typename Head, typename... Tail>
-struct TotalSizeAtCompileTime {
- static constexpr int eval() {
- return Head::SizeAtCompileTime == Eigen::Dynamic ||
- TotalSizeAtCompileTime<Tail...>::eval() == Eigen::Dynamic
- ? Eigen::Dynamic
- : Head::SizeAtCompileTime +
- TotalSizeAtCompileTime<Tail...>::eval();
- }
-};
-
-/** \brief Helper for totalSizeAtCompileTime function (base case)
- */
-template <typename Head>
-struct TotalSizeAtCompileTime<Head> {
- static constexpr int eval() { return Head::SizeAtCompileTime; }
-};
-
-/** \brief Determine the total size at compile time of a number of arguments
- * based on their SizeAtCompileTime static members
- */
-template <typename... Args>
-constexpr int totalSizeAtCompileTime() {
- return TotalSizeAtCompileTime<Args...>::eval();
-}
-
-/** \brief Determine the total size at runtime of a number of arguments using
- * their size() methods (base case).
- */
-constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; }
-
-/** \brief Determine the total size at runtime of a number of arguments using
- * their size() methods (recursive)
- */
-template <typename Head, typename... Tail>
-Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase<Head>& head,
- const Tail&... tail) {
- return head.size() + totalSizeAtRunTime(tail...);
-}
-
-/** \brief Helper for initializeAutoDiffTuple function (recursive)
- */
-template <size_t Index>
-struct InitializeAutoDiffTupleHelper {
- template <typename... ValueTypes, typename... AutoDiffTypes>
- static void run(const std::tuple<ValueTypes...>& values,
- // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
- std::tuple<AutoDiffTypes...>& auto_diffs,
- Eigen::DenseIndex num_derivatives,
- Eigen::DenseIndex deriv_num_start) {
- constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index;
- const auto& value = std::get<tuple_index>(values);
- auto& auto_diff = std::get<tuple_index>(auto_diffs);
- auto_diff.resize(value.rows(), value.cols());
- initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start);
- InitializeAutoDiffTupleHelper<Index - 1>::run(
- values, auto_diffs, num_derivatives, deriv_num_start + value.size());
- }
-};
-
-/** \brief Helper for initializeAutoDiffTuple function (base case)
- */
-template <>
-struct InitializeAutoDiffTupleHelper<0> {
- template <typename... ValueTypes, typename... AutoDiffTypes>
- static void run(const std::tuple<ValueTypes...>& values,
- const std::tuple<AutoDiffTypes...>& auto_diffs,
- Eigen::DenseIndex num_derivatives,
- Eigen::DenseIndex deriv_num_start) {
- unused(values, auto_diffs, num_derivatives, deriv_num_start);
- }
-};
-} // namespace internal
-
-/** \brief Given a series of Eigen matrices, create a tuple of corresponding
- *AutoDiff matrices with values equal to the input matrices and properly
- *initialized derivative vectors.
- *
- * The size of the derivative vector of each element of the matrices in the
- *output tuple will be the same, and will equal the sum of the number of
- *elements of the matrices in \p args.
- * If all of the matrices in \p args have fixed size, then the derivative
- *vectors will also have fixed size (being the sum of the sizes at compile time
- *of all of the input arguments),
- * otherwise the derivative vectors will have dynamic size.
- * The 0th element of the derivative vectors will correspond to the derivative
- *with respect to the 0th element of the first argument.
- * Subsequent derivative vector elements correspond first to subsequent elements
- *of the first input argument (traversed first by row, then by column), and so
- *on for subsequent arguments.
- *
- * \param args a series of Eigen matrices
- * \return a tuple of properly initialized AutoDiff matrices corresponding to \p
- *args
- *
- */
-template <typename... Deriveds>
-std::tuple<AutoDiffMatrixType<
- Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
-initializeAutoDiffTuple(const Eigen::MatrixBase<Deriveds>&... args) {
- Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...);
- std::tuple<AutoDiffMatrixType<
- Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
- ret(AutoDiffMatrixType<Deriveds,
- internal::totalSizeAtCompileTime<Deriveds...>()>(
- args.rows(), args.cols())...);
- auto values = std::forward_as_tuple(args...);
- internal::InitializeAutoDiffTupleHelper<sizeof...(args)>::run(
- values, ret, dynamic_num_derivs, 0);
- return ret;
-}
-
-} // namespace math
-} // namespace drake
diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
new file mode 100644
index 0000000..23a20e8
--- /dev/null
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.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 <array>
+
+#include "Eigen/Core"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Performs 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt.
+ *
+ * @param f The function to integrate. It must take two arguments t and y.
+ * @param t The initial value of t.
+ * @param y The initial value of y.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RungeKuttaTimeVarying(F&& f, units::second_t t, T y, units::second_t dt) {
+ const auto h = dt.value();
+
+ T k1 = f(t, y);
+ T k2 = f(t + dt * 0.5, y + h * k1 * 0.5);
+ T k3 = f(t + dt * 0.5, y + h * k2 * 0.5);
+ T k4 = f(t + dt, y + h * k3);
+
+ return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+} // namespace frc
diff --git a/wpimath/src/test/native/include/trajectory/TestTrajectory.h b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
index 1cac87f..de7b8b8 100644
--- a/wpimath/src/test/native/include/trajectory/TestTrajectory.h
+++ b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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