Add way to reset SwerveDriveKinematics internal module states

[calcmogul]

Is your feature request related to a problem? Please describe.
SwerveDriveKinematics maintains an internal copy of the module states so it can maintain module heading when the chassis velocity is zero. However, it initializes them to zero in the constructor, which may not reflect the true module starting orientation.

Also, it's possible for the user to disable the robot and move the modules, which makes them inconsistent with the internal module states. A reset() function should be provided to reinitialize them (specifically the module headings) when the robot is re-enabled to avoid excessive movement.

Describe the solution you'd like
Add a reset() function for the internal module states and an optional constructor argument for the initial module states.