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/main/native/cpp/MathShared.cpp b/wpimath/src/main/native/cpp/MathShared.cpp
index 8a64f2e..5252e87 100644
--- a/wpimath/src/main/native/cpp/MathShared.cpp
+++ b/wpimath/src/main/native/cpp/MathShared.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 "wpimath/MathShared.h"
@@ -14,7 +11,9 @@
namespace {
class DefaultMathShared : public MathShared {
public:
- void ReportError(const wpi::Twine& error) override {}
+ void ReportErrorV(fmt::string_view format, fmt::format_args args) override {}
+ void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
+ }
void ReportUsage(MathUsageId id, int count) override {}
};
} // namespace
@@ -24,7 +23,9 @@
MathShared& MathSharedStore::GetMathShared() {
std::scoped_lock lock(setLock);
- if (!mathShared) mathShared = std::make_unique<DefaultMathShared>();
+ if (!mathShared) {
+ mathShared = std::make_unique<DefaultMathShared>();
+ }
return *mathShared;
}
diff --git a/wpimath/src/main/native/cpp/MathUtil.cpp b/wpimath/src/main/native/cpp/MathUtil.cpp
new file mode 100644
index 0000000..19cd214
--- /dev/null
+++ b/wpimath/src/main/native/cpp/MathUtil.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 "frc/MathUtil.h"
+
+#include <cmath>
+
+namespace frc {
+
+double ApplyDeadband(double value, double deadband) {
+ if (std::abs(value) > deadband) {
+ if (value > 0.0) {
+ return (value - deadband) / (1.0 - deadband);
+ } else {
+ return (value + deadband) / (1.0 - deadband);
+ }
+ } else {
+ return 0.0;
+ }
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index d828f30..8f1145d 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -1,14 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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/StateSpaceUtil.h"
namespace frc {
+Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
+ return Eigen::Vector<double, 3>{pose.Translation().X().value(),
+ pose.Translation().Y().value(),
+ pose.Rotation().Radians().value()};
+}
+
+Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
+ return Eigen::Vector<double, 4>{pose.Translation().X().value(),
+ pose.Translation().Y().value(),
+ pose.Rotation().Cos(), pose.Rotation().Sin()};
+}
+
template <>
bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B) {
@@ -21,9 +30,9 @@
return detail::IsStabilizableImpl<2, 1>(A, B);
}
-Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose) {
- return frc::MakeMatrix<3, 1>(pose.X().to<double>(), pose.Y().to<double>(),
- pose.Rotation().Radians().to<double>());
+Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
+ return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
+ pose.Rotation().Radians().value()};
}
} // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
new file mode 100644
index 0000000..23b22cd
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -0,0 +1,78 @@
+// 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/HolonomicDriveController.h"
+
+#include <utility>
+
+#include "units/angular_velocity.h"
+
+using namespace frc;
+
+HolonomicDriveController::HolonomicDriveController(
+ frc2::PIDController xController, frc2::PIDController yController,
+ ProfiledPIDController<units::radian> thetaController)
+ : m_xController(std::move(xController)),
+ m_yController(std::move(yController)),
+ m_thetaController(std::move(thetaController)) {}
+
+bool HolonomicDriveController::AtReference() const {
+ const auto& eTranslate = m_poseError.Translation();
+ const auto& eRotate = m_rotationError;
+ const auto& tolTranslate = m_poseTolerance.Translation();
+ const auto& tolRotate = m_poseTolerance.Rotation();
+ return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+ units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+ units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
+ m_poseTolerance = tolerance;
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+ const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+ // If this is the first run, then we need to reset the theta controller to the
+ // current pose's heading.
+ if (m_firstRun) {
+ m_thetaController.Reset(currentPose.Rotation().Radians());
+ m_firstRun = false;
+ }
+
+ // Calculate feedforward velocities (field-relative)
+ auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
+ auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
+ auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate(
+ currentPose.Rotation().Radians(), angleRef.Radians()));
+
+ m_poseError = poseRef.RelativeTo(currentPose);
+ m_rotationError = angleRef - currentPose.Rotation();
+
+ if (!m_enabled) {
+ return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
+ currentPose.Rotation());
+ }
+
+ // Calculate feedback velocities (based on position error).
+ auto xFeedback = units::meters_per_second_t(
+ m_xController.Calculate(currentPose.X().value(), poseRef.X().value()));
+ auto yFeedback = units::meters_per_second_t(
+ m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value()));
+
+ // Return next output.
+ return ChassisSpeeds::FromFieldRelativeSpeeds(
+ xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+ const Pose2d& currentPose, const Trajectory::State& desiredState,
+ const Rotation2d& angleRef) {
+ return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+ angleRef);
+}
+
+void HolonomicDriveController::SetEnabled(bool enabled) {
+ m_enabled = enabled;
+}
diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
index ae58440..4d2fbe9 100644
--- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
+++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.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 "frc/controller/LinearQuadraticRegulator.h"
@@ -11,7 +8,7 @@
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
- const std::array<double, 1>& Qelems, const std::array<double, 1>& Relems,
+ const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
@@ -22,9 +19,15 @@
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+ const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+ const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
+ const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
+ : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
+
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
- const std::array<double, 2>& Qelems, const std::array<double, 1>& Relems,
+ const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
@@ -35,4 +38,29 @@
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+ const Eigen::Matrix<double, 2, 1>& N, units::second_t dt)
+ : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
+
+LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
+ units::second_t dt)
+ : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+ MakeCostMatrix(Relems), dt) {}
+
+LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+ units::second_t dt)
+ : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
+
+LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
+ const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+ const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+ const Eigen::Matrix<double, 2, 2>& N, units::second_t dt)
+ : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
+
} // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
new file mode 100644
index 0000000..34af2aa
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -0,0 +1,175 @@
+// 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 <algorithm>
+#include <cmath>
+
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/MathUtil.h"
+#include "wpimath/MathShared.h"
+
+using namespace frc2;
+
+PIDController::PIDController(double Kp, double Ki, double Kd,
+ units::second_t period)
+ : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+ if (period <= 0_s) {
+ wpi::math::MathSharedStore::ReportError(
+ "Controller period must be a non-zero positive number, got {}!",
+ period.value());
+ m_period = 20_ms;
+ wpi::math::MathSharedStore::ReportWarning(
+ "{}", "Controller period defaulted to 20ms.");
+ }
+ static int instances = 0;
+ instances++;
+
+ wpi::math::MathSharedStore::ReportUsage(
+ wpi::math::MathUsageId::kController_PIDController2, instances);
+ wpi::SendableRegistry::Add(this, "PIDController", instances);
+}
+
+void PIDController::SetPID(double Kp, double Ki, double Kd) {
+ m_Kp = Kp;
+ m_Ki = Ki;
+ m_Kd = Kd;
+}
+
+void PIDController::SetP(double Kp) {
+ m_Kp = Kp;
+}
+
+void PIDController::SetI(double Ki) {
+ m_Ki = Ki;
+}
+
+void PIDController::SetD(double Kd) {
+ m_Kd = Kd;
+}
+
+double PIDController::GetP() const {
+ return m_Kp;
+}
+
+double PIDController::GetI() const {
+ return m_Ki;
+}
+
+double PIDController::GetD() const {
+ return m_Kd;
+}
+
+units::second_t PIDController::GetPeriod() const {
+ return units::second_t(m_period);
+}
+
+void PIDController::SetSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+}
+
+double PIDController::GetSetpoint() const {
+ return m_setpoint;
+}
+
+bool PIDController::AtSetpoint() const {
+ double positionError;
+ if (m_continuous) {
+ double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+ positionError =
+ frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+ } else {
+ positionError = m_setpoint - m_measurement;
+ }
+
+ double velocityError = (positionError - m_prevError) / m_period.value();
+
+ return std::abs(positionError) < m_positionTolerance &&
+ std::abs(velocityError) < m_velocityTolerance;
+}
+
+void PIDController::EnableContinuousInput(double minimumInput,
+ double maximumInput) {
+ m_continuous = true;
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+}
+
+void PIDController::DisableContinuousInput() {
+ m_continuous = false;
+}
+
+bool PIDController::IsContinuousInputEnabled() const {
+ return m_continuous;
+}
+
+void PIDController::SetIntegratorRange(double minimumIntegral,
+ double maximumIntegral) {
+ m_minimumIntegral = minimumIntegral;
+ m_maximumIntegral = maximumIntegral;
+}
+
+void PIDController::SetTolerance(double positionTolerance,
+ double velocityTolerance) {
+ m_positionTolerance = positionTolerance;
+ m_velocityTolerance = velocityTolerance;
+}
+
+double PIDController::GetPositionError() const {
+ return m_positionError;
+}
+
+double PIDController::GetVelocityError() const {
+ return m_velocityError;
+}
+
+double PIDController::Calculate(double measurement) {
+ m_measurement = measurement;
+ m_prevError = m_positionError;
+
+ if (m_continuous) {
+ double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+ m_positionError =
+ frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+ } else {
+ m_positionError = m_setpoint - measurement;
+ }
+
+ m_velocityError = (m_positionError - m_prevError) / m_period.value();
+
+ if (m_Ki != 0) {
+ m_totalError =
+ std::clamp(m_totalError + m_positionError * m_period.value(),
+ m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+ }
+
+ return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+}
+
+double PIDController::Calculate(double measurement, double setpoint) {
+ // Set setpoint to provided value
+ SetSetpoint(setpoint);
+ return Calculate(measurement);
+}
+
+void PIDController::Reset() {
+ m_prevError = 0;
+ m_totalError = 0;
+}
+
+void PIDController::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("PIDController");
+ builder.AddDoubleProperty(
+ "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+ builder.AddDoubleProperty(
+ "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+ builder.AddDoubleProperty(
+ "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
+ builder.AddDoubleProperty(
+ "setpoint", [this] { return GetSetpoint(); },
+ [this](double value) { SetSetpoint(value); });
+}
diff --git a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
new file mode 100644
index 0000000..fa58427
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -0,0 +1,12 @@
+// 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/ProfiledPIDController.h"
+
+void frc::detail::ReportProfiledPIDController() {
+ static int instances = 0;
+ ++instances;
+ wpi::math::MathSharedStore::ReportUsage(
+ wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+}
diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
new file mode 100644
index 0000000..8aa16d8
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
@@ -0,0 +1,77 @@
+// 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/RamseteController.h"
+
+#include <cmath>
+
+#include "units/math.h"
+
+using namespace frc;
+
+/**
+ * Returns sin(x) / x.
+ *
+ * @param x Value of which to take sinc(x).
+ */
+static double Sinc(double x) {
+ if (std::abs(x) < 1e-9) {
+ return 1.0 - 1.0 / 6.0 * x * x;
+ } else {
+ return std::sin(x) / x;
+ }
+}
+
+RamseteController::RamseteController(double b, double zeta)
+ : m_b{b}, m_zeta{zeta} {}
+
+bool RamseteController::AtReference() const {
+ const auto& eTranslate = m_poseError.Translation();
+ const auto& eRotate = m_poseError.Rotation();
+ const auto& tolTranslate = m_poseTolerance.Translation();
+ const auto& tolRotate = m_poseTolerance.Rotation();
+ return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+ units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+ units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void RamseteController::SetTolerance(const Pose2d& poseTolerance) {
+ m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds RamseteController::Calculate(
+ const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef,
+ units::radians_per_second_t angularVelocityRef) {
+ if (!m_enabled) {
+ return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+ }
+
+ m_poseError = poseRef.RelativeTo(currentPose);
+
+ // Aliases for equation readability
+ double eX = m_poseError.X().value();
+ double eY = m_poseError.Y().value();
+ double eTheta = m_poseError.Rotation().Radians().value();
+ double vRef = linearVelocityRef.value();
+ double omegaRef = angularVelocityRef.value();
+
+ double k =
+ 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
+
+ units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
+ units::radians_per_second_t omega{omegaRef + k * eTheta +
+ m_b * vRef * Sinc(eTheta) * eY};
+ return ChassisSpeeds{v, 0_mps, omega};
+}
+
+ChassisSpeeds RamseteController::Calculate(
+ const Pose2d& currentPose, const Trajectory::State& desiredState) {
+ return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+ desiredState.velocity * desiredState.curvature);
+}
+
+void RamseteController::SetEnabled(bool enabled) {
+ m_enabled = enabled;
+}
diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
index e80cadc..20ea2b7 100644
--- a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
+++ b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
@@ -1,14 +1,11 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-#include "drake/common/is_approx_equal_abstol.h"
-
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
-// This code has https://github.com/RobotLocomotion/drake/pull/11118 applied to
-// fix an infinite loop in reorder_eigen().
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_throw.h"
+#include "drake/common/is_approx_equal_abstol.h"
namespace drake {
namespace math {
@@ -385,12 +382,11 @@
* DiscreteAlgebraicRiccatiEquation function
* computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
- * \f[
- * A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
- * \f]
*
- * @throws std::runtime_error if Q is not positive semi-definite.
- * @throws std::runtime_error if R is not positive definite.
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @throws std::exception if Q is not positive semi-definite.
+ * @throws std::exception if R is not positive definite.
*
* Based on the Schur Vector approach outlined in this paper:
* "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
@@ -399,9 +395,9 @@
*
* Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
* R_half are sampled from standard normal distributions, where
- * Q = Q_half'*Q_half and similar for R, the absolute error of the solution
- * is 10^{-6}, while the absolute error of the solution computed by Matlab is
- * 10^{-8}.
+ * Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution
+ * is 10⁻⁶, while the absolute error of the solution computed by Matlab is
+ * 10⁻⁸.
*
* TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
* accuracy, together with more thorough tests.
@@ -459,5 +455,21 @@
return X;
}
+Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+ const Eigen::Ref<const Eigen::MatrixXd>& A,
+ const Eigen::Ref<const Eigen::MatrixXd>& B,
+ const Eigen::Ref<const Eigen::MatrixXd>& Q,
+ const Eigen::Ref<const Eigen::MatrixXd>& R,
+ const Eigen::Ref<const Eigen::MatrixXd>& N) {
+ DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
+
+ // This is a change of variables to make the DARE that includes Q, R, and N
+ // cost matrices fit the form of the DARE that includes only Q and R cost
+ // matrices.
+ Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
+ Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
+ return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
+}
+
} // namespace math
} // namespace drake
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
new file mode 100644
index 0000000..c5ed7a1
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -0,0 +1,145 @@
+// 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/estimator/DifferentialDrivePoseEstimator.h"
+
+#include <wpi/timestamp.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+
+using namespace frc;
+
+DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
+ const Rotation2d& gyroAngle, const Pose2d& initialPose,
+ const wpi::array<double, 5>& stateStdDevs,
+ const wpi::array<double, 3>& localMeasurementStdDevs,
+ const wpi::array<double, 3>& visionMeasurmentStdDevs,
+ units::second_t nominalDt)
+ : m_observer(
+ &DifferentialDrivePoseEstimator::F,
+ [](const Eigen::Vector<double, 5>& x,
+ const Eigen::Vector<double, 3>& u) {
+ return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
+ },
+ stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
+ frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
+ frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
+ m_nominalDt(nominalDt) {
+ SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
+
+ // Create correction mechanism for vision measurements.
+ m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+ const Eigen::Vector<double, 3>& y) {
+ m_observer.Correct<3>(
+ u, y,
+ [](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
+ return x.block<3, 1>(0, 0);
+ },
+ m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
+ frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
+ };
+
+ m_gyroOffset = initialPose.Rotation() - gyroAngle;
+ m_previousAngle = initialPose.Rotation();
+ m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
+}
+
+void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
+ const wpi::array<double, 3>& visionMeasurmentStdDevs) {
+ // Create R (covariances) for vision measurements.
+ m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+}
+
+void DifferentialDrivePoseEstimator::ResetPosition(
+ const Pose2d& pose, const Rotation2d& gyroAngle) {
+ // Reset state estimate and error covariance
+ m_observer.Reset();
+ m_latencyCompensator.Reset();
+
+ m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
+
+ m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
+ m_previousAngle = pose.Rotation();
+}
+
+Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
+ return Pose2d(units::meter_t(m_observer.Xhat(0)),
+ units::meter_t(m_observer.Xhat(1)),
+ Rotation2d(units::radian_t(m_observer.Xhat(2))));
+}
+
+void DifferentialDrivePoseEstimator::AddVisionMeasurement(
+ const Pose2d& visionRobotPose, units::second_t timestamp) {
+ m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+ &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+ m_visionCorrect, timestamp);
+}
+
+Pose2d DifferentialDrivePoseEstimator::Update(
+ const Rotation2d& gyroAngle,
+ const DifferentialDriveWheelSpeeds& wheelSpeeds,
+ units::meter_t leftDistance, units::meter_t rightDistance) {
+ return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+ wheelSpeeds, leftDistance, rightDistance);
+}
+
+Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& gyroAngle,
+ const DifferentialDriveWheelSpeeds& wheelSpeeds,
+ units::meter_t leftDistance, units::meter_t rightDistance) {
+ auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+ m_prevTime = currentTime;
+
+ auto angle = gyroAngle + m_gyroOffset;
+ auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
+
+ auto u = Eigen::Vector<double, 3>{
+ (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
+
+ m_previousAngle = angle;
+
+ auto localY = Eigen::Vector<double, 3>{
+ leftDistance.value(), rightDistance.value(), angle.Radians().value()};
+
+ m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+ m_observer.Predict(u, dt);
+ m_observer.Correct(u, localY);
+
+ return GetEstimatedPosition();
+}
+
+Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
+ const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
+ // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
+ // that for us.
+ auto& theta = x(2);
+ Eigen::Matrix<double, 5, 5> toFieldRotation{
+ {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
+ {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
+ {0.0, 0.0, 1.0, 0.0, 0.0},
+ {0.0, 0.0, 0.0, 1.0, 0.0},
+ {0.0, 0.0, 0.0, 0.0, 1.0}};
+ return toFieldRotation *
+ Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
+}
+
+template <int Dim>
+wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
+ const Eigen::Vector<double, Dim>& stdDevs) {
+ wpi::array<double, Dim> array;
+ for (size_t i = 0; i < Dim; ++i) {
+ array[i] = stdDevs(i);
+ }
+ return array;
+}
+
+Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
+ const Pose2d& pose, units::meter_t leftDistance,
+ units::meter_t rightDistance) {
+ return Eigen::Vector<double, 5>{pose.Translation().X().value(),
+ pose.Translation().Y().value(),
+ pose.Rotation().Radians().value(),
+ leftDistance.value(), rightDistance.value()};
+}
diff --git a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
index a1747ab..1209eae 100644
--- a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
+++ b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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/estimator/KalmanFilter.h"
namespace frc {
KalmanFilter<1, 1, 1>::KalmanFilter(
- LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
- const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+ LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
+ const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
: detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
dt} {}
KalmanFilter<2, 1, 1>::KalmanFilter(
- LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
- const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+ LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
+ const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
: detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
dt} {}
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
new file mode 100644
index 0000000..9d93647
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -0,0 +1,116 @@
+// 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/estimator/MecanumDrivePoseEstimator.h"
+
+#include <wpi/timestamp.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+
+using namespace frc;
+
+frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
+ const Rotation2d& gyroAngle, const Pose2d& initialPose,
+ MecanumDriveKinematics kinematics,
+ const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 1>& localMeasurementStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs,
+ units::second_t nominalDt)
+ : m_observer(
+ [](const Eigen::Vector<double, 3>& x,
+ const Eigen::Vector<double, 3>& u) { return u; },
+ [](const Eigen::Vector<double, 3>& x,
+ const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
+ stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
+ frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
+ frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
+ m_kinematics(kinematics),
+ m_nominalDt(nominalDt) {
+ SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+ // Create vision correction mechanism.
+ m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+ const Eigen::Vector<double, 3>& y) {
+ m_observer.Correct<3>(
+ u, y,
+ [](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
+ return x;
+ },
+ m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
+ frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
+ };
+
+ // Set initial state.
+ m_observer.SetXhat(PoseTo3dVector(initialPose));
+
+ // Calculate offsets.
+ m_gyroOffset = initialPose.Rotation() - gyroAngle;
+ m_previousAngle = initialPose.Rotation();
+}
+
+void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
+ const wpi::array<double, 3>& visionMeasurmentStdDevs) {
+ // Create R (covariances) for vision measurements.
+ m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+}
+
+void frc::MecanumDrivePoseEstimator::ResetPosition(
+ const Pose2d& pose, const Rotation2d& gyroAngle) {
+ // Reset state estimate and error covariance
+ m_observer.Reset();
+ m_latencyCompensator.Reset();
+
+ m_observer.SetXhat(PoseTo3dVector(pose));
+
+ m_gyroOffset = pose.Rotation() - gyroAngle;
+ m_previousAngle = pose.Rotation();
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
+ return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
+ Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+}
+
+void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
+ const Pose2d& visionRobotPose, units::second_t timestamp) {
+ m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+ &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+ m_visionCorrect, timestamp);
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::Update(
+ const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
+ return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+ wheelSpeeds);
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelSpeeds& wheelSpeeds) {
+ auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+ m_prevTime = currentTime;
+
+ auto angle = gyroAngle + m_gyroOffset;
+ auto omega = (angle - m_previousAngle).Radians() / dt;
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+ auto fieldRelativeVelocities =
+ Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
+ .RotateBy(angle);
+
+ Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
+ fieldRelativeVelocities.Y().value(),
+ omega.value()};
+
+ Eigen::Vector<double, 1> localY{angle.Radians().value()};
+ m_previousAngle = angle;
+
+ m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+
+ m_observer.Predict(u, dt);
+ m_observer.Correct(u, localY);
+
+ return GetEstimatedPosition();
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 57dad44..b7176cd 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.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/geometry/Pose2d.h"
@@ -23,12 +20,6 @@
return TransformBy(other);
}
-Pose2d& Pose2d::operator+=(const Transform2d& other) {
- m_translation += other.Translation().RotateBy(m_rotation);
- m_rotation += other.Rotation();
- return *this;
-}
-
Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
return Transform2d(pose.Translation(), pose.Rotation());
@@ -55,7 +46,7 @@
Pose2d Pose2d::Exp(const Twist2d& twist) const {
const auto dx = twist.dx;
const auto dy = twist.dy;
- const auto dtheta = twist.dtheta.to<double>();
+ const auto dtheta = twist.dtheta.value();
const auto sinTheta = std::sin(dtheta);
const auto cosTheta = std::cos(dtheta);
@@ -77,7 +68,7 @@
Twist2d Pose2d::Log(const Pose2d& end) const {
const auto transform = end.RelativeTo(*this);
- const auto dtheta = transform.Rotation().Radians().to<double>();
+ const auto dtheta = transform.Rotation().Radians().value();
const auto halfDtheta = dtheta / 2.0;
const auto cosMinusOne = transform.Rotation().Cos() - 1;
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 32a9b40..27af5ed 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.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/geometry/Rotation2d.h"
@@ -41,32 +38,20 @@
return RotateBy(other);
}
-Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
- double cos = Cos() * other.Cos() - Sin() * other.Sin();
- double sin = Cos() * other.Sin() + Sin() * other.Cos();
- m_cos = cos;
- m_sin = sin;
- m_value = units::radian_t(std::atan2(m_sin, m_cos));
- return *this;
-}
-
Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
-Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
- *this += -other;
- return *this;
+Rotation2d Rotation2d::operator-() const {
+ return Rotation2d(-m_value);
}
-Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
-
Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d(m_value * scalar);
}
bool Rotation2d::operator==(const Rotation2d& other) const {
- return units::math::abs(m_value - other.m_value) < 1E-9_rad;
+ return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
}
bool Rotation2d::operator!=(const Rotation2d& other) const {
@@ -79,7 +64,7 @@
}
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
- json = wpi::json{{"radians", rotation.Radians().to<double>()}};
+ json = wpi::json{{"radians", rotation.Radians().value()}};
}
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index eb5e2ec..0808f35 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.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/geometry/Transform2d.h"
@@ -31,6 +28,10 @@
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
+Transform2d Transform2d::operator+(const Transform2d& other) const {
+ return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
+}
+
bool Transform2d::operator==(const Transform2d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 6f4551c..5a30ec2 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.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/geometry/Translation2d.h"
@@ -36,33 +33,18 @@
return {X() + other.X(), Y() + other.Y()};
}
-Translation2d& Translation2d::operator+=(const Translation2d& other) {
- m_x += other.m_x;
- m_y += other.m_y;
- return *this;
-}
-
Translation2d Translation2d::operator-(const Translation2d& other) const {
return *this + -other;
}
-Translation2d& Translation2d::operator-=(const Translation2d& other) {
- *this += -other;
- return *this;
+Translation2d Translation2d::operator-() const {
+ return {-m_x, -m_y};
}
-Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
-
Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
-Translation2d& Translation2d::operator*=(double scalar) {
- m_x *= scalar;
- m_y *= scalar;
- return *this;
-}
-
Translation2d Translation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
@@ -76,14 +58,9 @@
return !operator==(other);
}
-Translation2d& Translation2d::operator/=(double scalar) {
- *this *= (1.0 / scalar);
- return *this;
-}
-
void frc::to_json(wpi::json& json, const Translation2d& translation) {
- json = wpi::json{{"x", translation.X().to<double>()},
- {"y", translation.Y().to<double>()}};
+ json =
+ wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
}
void frc::from_json(const wpi::json& json, Translation2d& translation) {
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index 26d57e2..b036833 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <jni.h>
+#include <exception>
+
#include <wpi/jni_util.h>
#include "Eigen/Core"
@@ -14,28 +13,38 @@
#include "Eigen/QR"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
#include "unsupported/Eigen/MatrixFunctions"
using namespace wpi::java;
+/**
+ * Returns true if (A, B) is a stabilizable pair.
+ *
+ * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+ * any, have absolute values less than one, where an eigenvalue is
+ * uncontrollable if rank(λI - A, B) < n where n is the number of states.
+ *
+ * @param A System matrix.
+ * @param B Input matrix.
+ */
bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
- // This function checks if (A,B) is a stabilizable pair.
- // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
- // A, if any, have absolute values less than one, where an eigenvalue is
- // uncontrollable if Rank[lambda * I - A, B] < n.
- int n = B.rows(), m = B.cols();
- Eigen::EigenSolver<Eigen::MatrixXd> es(A);
- for (int i = 0; i < n; i++) {
+ int states = B.rows();
+ int inputs = B.cols();
+ Eigen::EigenSolver<Eigen::MatrixXd> es{A};
+ for (int i = 0; i < states; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
- 1)
+ 1) {
continue;
+ }
- Eigen::MatrixXcd E(n, n + m);
- E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
- Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
- if (qr.rank() != n) {
+ Eigen::MatrixXcd E{states, states + inputs};
+ E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
+ B;
+ Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+ if (qr.rank() < states) {
return false;
}
}
@@ -43,6 +52,46 @@
return true;
}
+std::vector<double> GetElementsFromTrajectory(
+ const frc::Trajectory& trajectory) {
+ std::vector<double> elements;
+ elements.reserve(trajectory.States().size() * 7);
+
+ for (auto&& state : trajectory.States()) {
+ elements.push_back(state.t.value());
+ elements.push_back(state.velocity.value());
+ elements.push_back(state.acceleration.value());
+ elements.push_back(state.pose.X().value());
+ elements.push_back(state.pose.Y().value());
+ elements.push_back(state.pose.Rotation().Radians().value());
+ elements.push_back(state.curvature.value());
+ }
+
+ return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(wpi::span<const double> elements) {
+ // Make sure that the elements have the correct length.
+ assert(elements.size() % 7 == 0);
+
+ // Create a vector of states from the elements.
+ std::vector<frc::Trajectory::State> states;
+ states.reserve(elements.size() / 7);
+
+ for (size_t i = 0; i < elements.size(); i += 7) {
+ states.emplace_back(frc::Trajectory::State{
+ units::second_t{elements[i]},
+ units::meters_per_second_t{elements[i + 1]},
+ units::meters_per_second_squared_t{elements[i + 2]},
+ frc::Pose2d{units::meter_t{elements[i + 3]},
+ units::meter_t{elements[i + 4]},
+ units::radian_t{elements[i + 5]}},
+ units::curvature_t{elements[i + 6]}});
+ }
+
+ return frc::Trajectory(states);
+}
+
extern "C" {
/*
@@ -73,15 +122,22 @@
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Rmat{nativeR, inputs, inputs};
- Eigen::MatrixXd result =
- drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
+ try {
+ Eigen::MatrixXd result =
+ drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
- env->ReleaseDoubleArrayElements(A, nativeA, 0);
- env->ReleaseDoubleArrayElements(B, nativeB, 0);
- env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
- env->ReleaseDoubleArrayElements(R, nativeR, 0);
+ env->ReleaseDoubleArrayElements(A, nativeA, 0);
+ env->ReleaseDoubleArrayElements(B, nativeB, 0);
+ env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
+ env->ReleaseDoubleArrayElements(R, nativeR, 0);
- env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+ } catch (const std::runtime_error& e) {
+ jclass cls = env->FindClass("java/lang/RuntimeException");
+ if (cls) {
+ env->ThrowNew(cls, e.what());
+ }
+ }
}
/*
@@ -107,6 +163,28 @@
/*
* Class: edu_wpi_first_math_WPIMathJNI
+ * Method: pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+ (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+ jdoubleArray dst)
+{
+ jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
+
+ Eigen::Map<
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+ Amat{arrayBody, rows, rows}; // NOLINT
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+ Amat.pow(exponent);
+
+ env->ReleaseDoubleArrayElements(src, arrayBody, 0);
+ env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
* Method: isStabilizable
* Signature: (II[D[D)Z
*/
@@ -134,4 +212,99 @@
return isStabilizable;
}
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+ (JNIEnv* env, jclass, jstring path)
+{
+ try {
+ auto trajectory =
+ frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+ std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+ return MakeJDoubleArray(env, elements);
+ } catch (std::exception& e) {
+ jclass cls = env->FindClass("java/io/IOException");
+ if (cls) {
+ env->ThrowNew(cls, e.what());
+ }
+ return nullptr;
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+ (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+ try {
+ auto trajectory =
+ CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
+ frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+ JStringRef{env, path}.c_str());
+ } catch (std::exception& e) {
+ jclass cls = env->FindClass("java/io/IOException");
+ if (cls) {
+ env->ThrowNew(cls, e.what());
+ }
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+ (JNIEnv* env, jclass, jstring json)
+{
+ try {
+ auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+ JStringRef{env, json}.c_str());
+ std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+ return MakeJDoubleArray(env, elements);
+ } catch (std::exception& e) {
+ jclass cls = env->FindClass(
+ "edu/wpi/first/math/trajectory/TrajectoryUtil$"
+ "TrajectorySerializationException");
+ if (cls) {
+ env->ThrowNew(cls, e.what());
+ }
+ return nullptr;
+ }
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+ (JNIEnv* env, jclass, jdoubleArray elements)
+{
+ try {
+ auto trajectory =
+ CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
+ return MakeJString(env,
+ frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+ } catch (std::exception& e) {
+ jclass cls = env->FindClass(
+ "edu/wpi/first/math/trajectory/TrajectoryUtil$"
+ "TrajectorySerializationException");
+ if (cls) {
+ env->ThrowNew(cls, e.what());
+ }
+ return nullptr;
+ }
+}
+
} // extern "C"
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 25b6c51..c4a4311 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.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/DifferentialDriveOdometry.h"
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
index 36a4952..42d3018 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.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/DifferentialDriveWheelSpeeds.h"
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index de1b2d0..c4a71fd 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.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/MecanumDriveKinematics.h"
@@ -24,33 +21,31 @@
m_previousCoR = centerOfRotation;
}
- Eigen::Vector3d chassisSpeedsVector;
- chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
- chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+ Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
+ chassisSpeeds.vy.value(),
+ chassisSpeeds.omega.value()};
- Eigen::Matrix<double, 4, 1> wheelsMatrix =
+ Eigen::Vector<double, 4> wheelsVector =
m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
- wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
- wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
- wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
- wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
+ wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
+ wheelSpeeds.frontRight = units::meters_per_second_t{wheelsVector(1)};
+ wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsVector(2)};
+ wheelSpeeds.rearRight = units::meters_per_second_t{wheelsVector(3)};
return wheelSpeeds;
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
- Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
- // clang-format off
- wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
- wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
- // clang-format on
+ Eigen::Vector<double, 4> wheelSpeedsVector{
+ wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
+ wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
Eigen::Vector3d chassisSpeedsVector =
- m_forwardKinematics.solve(wheelSpeedsMatrix);
+ m_forwardKinematics.solve(wheelSpeedsVector);
- return {units::meters_per_second_t{chassisSpeedsVector(0)},
+ return {units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
@@ -59,11 +54,9 @@
Translation2d fr,
Translation2d rl,
Translation2d rr) const {
- // clang-format off
- m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
- 1, 1, (fr.X() - fr.Y()).template to<double>(),
- 1, 1, (rl.X() - rl.Y()).template to<double>(),
- 1, -1, (-(rr.X() + rr.Y())).template to<double>();
- // clang-format on
- m_inverseKinematics /= std::sqrt(2);
+ m_inverseKinematics =
+ Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
+ {1, 1, (fr.X() - fr.Y()).value()},
+ {1, 1, (rl.X() - rl.Y()).value()},
+ {1, -1, (-(rr.X() + rr.Y())).value()}};
}
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 7534fc1..bbeee58 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.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"
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
index b20dddf..fc47461 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.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/MecanumDriveWheelSpeeds.h"
diff --git a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index c00a362..b643849 100644
--- a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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/spline/CubicHermiteSpline.h"
using namespace frc;
CubicHermiteSpline::CubicHermiteSpline(
- std::array<double, 2> xInitialControlVector,
- std::array<double, 2> xFinalControlVector,
- std::array<double, 2> yInitialControlVector,
- std::array<double, 2> yFinalControlVector) {
+ wpi::array<double, 2> xInitialControlVector,
+ wpi::array<double, 2> xFinalControlVector,
+ wpi::array<double, 2> yInitialControlVector,
+ wpi::array<double, 2> yFinalControlVector) {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index 5b34cdb..5362b7c 100644
--- a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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/spline/QuinticHermiteSpline.h"
using namespace frc;
QuinticHermiteSpline::QuinticHermiteSpline(
- std::array<double, 3> xInitialControlVector,
- std::array<double, 3> xFinalControlVector,
- std::array<double, 3> yInitialControlVector,
- std::array<double, 3> yFinalControlVector) {
+ wpi::array<double, 3> xInitialControlVector,
+ wpi::array<double, 3> xFinalControlVector,
+ wpi::array<double, 3> yInitialControlVector,
+ wpi::array<double, 3> yFinalControlVector) {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
index 58f7537..cec620c 100644
--- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineHelper.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/spline/SplineHelper.h"
@@ -16,10 +13,10 @@
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;
- std::array<double, 2> xInitial = start.x;
- std::array<double, 2> yInitial = start.y;
- std::array<double, 2> xFinal = end.x;
- std::array<double, 2> yFinal = end.y;
+ wpi::array<double, 2> xInitial = start.x;
+ wpi::array<double, 2> yInitial = start.y;
+ wpi::array<double, 2> xFinal = end.x;
+ wpi::array<double, 2> yFinal = end.y;
if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
@@ -55,29 +52,27 @@
c.emplace_back(0);
// populate rhs vectors
- dx.emplace_back(
- 3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
- xInitial[1]);
- dy.emplace_back(
- 3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
- yInitial[1]);
+ dx.emplace_back(3 * (waypoints[2].X().value() - waypoints[0].X().value()) -
+ xInitial[1]);
+ dy.emplace_back(3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) -
+ yInitial[1]);
if (waypoints.size() > 4) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
// dx and dy represent the derivatives of the internal waypoints. The
// derivative of the second internal waypoint should involve the third
// and first internal waypoint, which have indices of 1 and 3 in the
// waypoints list (which contains ALL waypoints).
- dx.emplace_back(3 * (waypoints[i + 2].X().to<double>() -
- waypoints[i].X().to<double>()));
- dy.emplace_back(3 * (waypoints[i + 2].Y().to<double>() -
- waypoints[i].Y().to<double>()));
+ dx.emplace_back(
+ 3 * (waypoints[i + 2].X().value() - waypoints[i].X().value()));
+ dy.emplace_back(
+ 3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value()));
}
}
- dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
- waypoints[waypoints.size() - 3].X().to<double>()) -
+ dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() -
+ waypoints[waypoints.size() - 3].X().value()) -
xFinal[1]);
- dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
- waypoints[waypoints.size() - 3].Y().to<double>()) -
+ dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() -
+ waypoints[waypoints.size() - 3].Y().value()) -
yFinal[1]);
// Compute solution to tridiagonal system
@@ -92,10 +87,10 @@
for (size_t i = 0; i < fx.size() - 1; ++i) {
// Create the spline.
const CubicHermiteSpline spline{
- {waypoints[i].X().to<double>(), fx[i]},
- {waypoints[i + 1].X().to<double>(), fx[i + 1]},
- {waypoints[i].Y().to<double>(), fy[i]},
- {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
+ {waypoints[i].X().value(), fx[i]},
+ {waypoints[i + 1].X().value(), fx[i + 1]},
+ {waypoints[i].Y().value(), fy[i]},
+ {waypoints[i + 1].Y().value(), fy[i + 1]}};
splines.push_back(spline);
}
@@ -105,10 +100,8 @@
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
- std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
- xDeriv};
- std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
- yDeriv};
+ wpi::array<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
+ wpi::array<double, 2> midYControlVector{waypoints[0].Y().value(), yDeriv};
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
@@ -137,22 +130,20 @@
return splines;
}
-std::array<Spline<3>::ControlVector, 2>
+wpi::array<Spline<3>::ControlVector, 2>
SplineHelper::CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
- scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
+ scalar = 1.2 * start.Translation().Distance(end.Translation()).value();
} else {
scalar =
- 1.2 *
- start.Translation().Distance(interiorWaypoints.front()).to<double>();
+ 1.2 * start.Translation().Distance(interiorWaypoints.front()).value();
}
const auto initialCV = CubicControlVector(scalar, start);
if (!interiorWaypoints.empty()) {
- scalar =
- 1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
+ scalar = 1.2 * end.Translation().Distance(interiorWaypoints.back()).value();
}
const auto finalCV = CubicControlVector(scalar, end);
return {initialCV, finalCV};
@@ -168,7 +159,7 @@
// This just makes the splines look better.
const auto scalar =
- 1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
+ 1.2 * p0.Translation().Distance(p1.Translation()).value();
auto controlVectorA = QuinticControlVector(scalar, p0);
auto controlVectorB = QuinticControlVector(scalar, p1);
diff --git a/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
index b7e7f9e..73c475b 100644
--- a/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineParameterizer.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/spline/SplineParameterizer.h"
diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index 067b8de..db419f7 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.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/Trajectory.h"
@@ -34,7 +31,9 @@
const auto deltaT = newT - t;
// If delta time is negative, flip the order of interpolation.
- if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
+ if (deltaT < 0_s) {
+ return endValue.Interpolate(*this, 1.0 - i);
+ }
// Check whether the robot is reversing at this stage.
const auto reversing =
@@ -68,8 +67,12 @@
}
Trajectory::State Trajectory::Sample(units::second_t t) const {
- if (t <= m_states.front().t) return m_states.front();
- if (t >= m_totalTime) return m_states.back();
+ if (t <= m_states.front().t) {
+ return m_states.front();
+ }
+ if (t >= m_totalTime) {
+ return m_states.back();
+ }
// Use binary search to get the element with a timestamp no less than the
// requested timestamp. This starts at 1 because we use the previous state
@@ -121,12 +124,33 @@
return Trajectory(newStates);
}
+Trajectory Trajectory::operator+(const Trajectory& other) const {
+ // If this is a default constructed trajectory with no states, then we can
+ // simply return the rhs trajectory.
+ if (m_states.empty()) {
+ return other;
+ }
+
+ auto states = m_states;
+ auto otherStates = other.States();
+ for (auto& otherState : otherStates) {
+ otherState.t += m_totalTime;
+ }
+
+ // Here we omit the first state of the other trajectory because we don't want
+ // two time points with different states. Sample() will automatically
+ // interpolate between the end of this trajectory and the second state of the
+ // other trajectory.
+ states.insert(states.end(), otherStates.begin() + 1, otherStates.end());
+ return Trajectory(states);
+}
+
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
- json = wpi::json{{"time", state.t.to<double>()},
- {"velocity", state.velocity.to<double>()},
- {"acceleration", state.acceleration.to<double>()},
+ json = wpi::json{{"time", state.t.value()},
+ {"velocity", state.velocity.value()},
+ {"acceleration", state.acceleration.value()},
{"pose", state.pose},
- {"curvature", state.curvature.to<double>()}};
+ {"curvature", state.curvature.value()}};
}
void frc::from_json(const wpi::json& json, Trajectory::State& state) {
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 6cb3f7a..2e2771d 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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/TrajectoryGenerator.h"
#include <utility>
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
#include "frc/spline/SplineHelper.h"
#include "frc/spline/SplineParameterizer.h"
@@ -22,10 +19,11 @@
std::function<void(const char*)> TrajectoryGenerator::s_errorFunc;
void TrajectoryGenerator::ReportError(const char* error) {
- if (s_errorFunc)
+ if (s_errorFunc) {
s_errorFunc(error);
- else
- wpi::errs() << "TrajectoryGenerator error: " << error << "\n";
+ } else {
+ fmt::print(stderr, "TrajectoryGenerator error: {}\n", error);
+ }
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
@@ -115,8 +113,11 @@
const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
auto newWaypoints = waypoints;
const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
- if (config.IsReversed())
- for (auto& waypoint : newWaypoints) waypoint += flip;
+ if (config.IsReversed()) {
+ for (auto& waypoint : newWaypoints) {
+ waypoint = waypoint + flip;
+ }
+ }
std::vector<SplineParameterizer::PoseWithCurvature> points;
try {
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index 0e78a15..d397d0c 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.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.
/*
* MIT License
@@ -31,7 +28,7 @@
#include "frc/trajectory/TrajectoryParameterizer.h"
-#include <string>
+#include <fmt/format.h>
#include "units/math.h"
@@ -88,7 +85,9 @@
// Now enforce all acceleration limits.
EnforceAccelerationLimits(reversed, constraints, &constrainedState);
- if (ds.to<double>() < kEpsilon) break;
+ if (ds.value() < kEpsilon) {
+ break;
+ }
// If the actual acceleration for this state is higher than the max
// acceleration that we applied, then we need to reduce the max
@@ -133,14 +132,18 @@
successor.minAcceleration * ds * 2.0);
// No more limits to impose! This state can be finalized.
- if (newMaxVelocity >= constrainedState.maxVelocity) break;
+ if (newMaxVelocity >= constrainedState.maxVelocity) {
+ break;
+ }
constrainedState.maxVelocity = newMaxVelocity;
// Check all acceleration constraints with the new max velocity.
EnforceAccelerationLimits(reversed, constraints, &constrainedState);
- if (ds.to<double>() > -kEpsilon) break;
+ if (ds.value() > -kEpsilon) {
+ break;
+ }
// If the actual acceleration for this state is lower than the min
// acceleration, then we need to lower the min acceleration of the
@@ -190,9 +193,9 @@
// delta_x = v * t
dt = ds / v;
} else {
- throw std::runtime_error("Something went wrong at iteration " +
- std::to_string(i) +
- " of time parameterization.");
+ throw std::runtime_error(fmt::format(
+ "Something went wrong at iteration {} of time parameterization.",
+ i));
}
}
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index 0ae43f5..169b642 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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/TrajectoryUtil.h"
#include <system_error>
+#include <fmt/format.h>
#include <wpi/SmallString.h>
#include <wpi/json.h>
#include <wpi/raw_istream.h>
@@ -17,13 +15,12 @@
using namespace frc;
void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
- const wpi::Twine& path) {
+ std::string_view path) {
std::error_code error_code;
- wpi::SmallString<128> buf;
- wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+ wpi::raw_fd_ostream output{path, error_code};
if (error_code) {
- throw std::runtime_error(("Cannot open file: " + path).str());
+ throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
wpi::json json = trajectory.States();
@@ -31,13 +28,12 @@
output.flush();
}
-Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+Trajectory TrajectoryUtil::FromPathweaverJson(std::string_view path) {
std::error_code error_code;
- wpi::SmallString<128> buf;
- wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+ wpi::raw_fd_istream input{path, error_code};
if (error_code) {
- throw std::runtime_error(("Cannot open file: " + path).str());
+ throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
wpi::json json;
@@ -51,8 +47,7 @@
return json.dump();
}
-Trajectory TrajectoryUtil::DeserializeTrajectory(
- const wpi::StringRef json_str) {
- wpi::json json = wpi::json::parse(json_str);
+Trajectory TrajectoryUtil::DeserializeTrajectory(std::string_view jsonStr) {
+ wpi::json json = wpi::json::parse(jsonStr);
return Trajectory{json.get<std::vector<Trajectory::State>>()};
}
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
index f04b9d6..738d243 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.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/constraint/CentripetalAccelerationConstraint.h"
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index c3380e5..d1c2f6f 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.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/constraint/DifferentialDriveKinematicsConstraint.h"
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index f12ce75..7c10201 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.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/constraint/DifferentialDriveVoltageConstraint.h"
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp
new file mode 100644
index 0000000..9e6e712
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.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 "frc/trajectory/constraint/MaxVelocityConstraint.h"
+
+using namespace frc;
+
+MaxVelocityConstraint::MaxVelocityConstraint(
+ units::meters_per_second_t maxVelocity)
+ : m_maxVelocity(units::math::abs(maxVelocity)) {}
+
+units::meters_per_second_t MaxVelocityConstraint::MaxVelocity(
+ const Pose2d& pose, units::curvature_t curvature,
+ units::meters_per_second_t velocity) const {
+ return m_maxVelocity;
+}
+
+TrajectoryConstraint::MinMax MaxVelocityConstraint::MinMaxAcceleration(
+ const Pose2d& pose, units::curvature_t curvature,
+ units::meters_per_second_t speed) const {
+ return {};
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
index 7d95803..418a904 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.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/constraint/MecanumDriveKinematicsConstraint.h"