Maple Simulator integration v0

[aschokking]

Context

maple-sim is a frc physics engine built on top of dyn4j: https://shenzhen-robotics-alliance.github.io/maple-sim/
Out of the box it has things like:

• collision with field elements
• simulated sensor drift (like gyro not being 100% right or drive wheels slipping when they hit something)

This PR upgrades our very basic drive simulator with one based on maple-sim.

I abstracted our Simulator into a very basic interface BaseSimulator so we could swap between different implementations at the dagger module level without having to change other code.

Testing

The solid robot is the physics sim ground truth and the red ghost is our PoseSubsystem's perception of where the robot is (currently based only on wheel odometry).

maple-sim-demo.mp4

Future work

Position setting

We need a way for commands like an auto program to reset the current position in the physics sim, probably with a new interface method that someone calls into the sim.

Vision

There's currently no fake vision stuff plugged in but we should add that in next to get april tags and things

Swerve integration

There are multiple ways to integrate with maple-sim, I opted for the 'stupidest' where they provide a fully working simulated swerve drive train and just sort of plugged in our requested swerve states in the top and get back out swerve module positions and gyro angle on the end, effectively skipping our own swerve module logic. https://shenzhen-robotics-alliance.github.io/maple-sim/swerve-sim-easy/

Doing the deeper integration at the motor level would be a good idea but is a much bigger ticket so I didn't opt to start there. https://shenzhen-robotics-alliance.github.io/maple-sim/swerve-sim-hardware-abstraction/

[aschokking]

These shouldn't be committed into source control