wpilibsuite / allwpilib

Official Repository of WPILibJ and WPILibC
https://wpilib.org/
Other
1.06k stars 614 forks source link

[wpimath] Add and use kinematics.copyInto() #6789

Closed KangarooKoala closed 2 months ago

KangarooKoala commented 3 months ago

This saves on a deep copy of the wheel positions in Odometry.resetPosition() and Odometry.update(), and by extension PoseEstimator.resetPosition() and PoseEstimator.update(). Notably, for a 4 module swerve that's 5 object allocations (1 for the array and 1 per module), which given that update() is typically called every single robot loop can add up relatively quickly. It also doesn't effect what few custom kinematics implementations there are, since you could use the defaulted implementation.