Trajectory generation V2

[mjansen4857]

Rewrite the trajectory generation to properly balance rotation and translation speed, and respect individual module limits. This won't be trajectory optimization or time optimal like Choreo, but should be closer to "correct" for on-the-fly generation.

Generation wish list:

• Allow for slowing down translation speed to guarantee a rotation will complete
• Limit overall motion of robot to respect the kinematic limits of each module
• Limit overall motion of robot to account for centripetal force acting on each module to prevent sliding
• Limit module acceleration based on the motor's torque curve
• Maybe account for torque required to overcome friction + how that affects max motor RPM?
• Maintain ability for arbitrary velocity/acceleration constraints if within the module's capabilities
• Fast enough to reasonably generate on-the-fly