Log mechanism poses for Advantagescope Playback/Simulation

[ACat701]

Let's add this after we swap to the rio 2 🙃

• [ ] Ask Mechanical for a CAD model
• [ ] Log all poses as Pose3d's

[ACat701]

Marked this as an optimization because it helps with debugging at comp. I don't know if it really is an optimization though- opinions?