search

wpilibsuite / allwpilib

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

LTVDifferentialDriveController constructor does not check max velocity before LUT creation #5027

Closed DeltaDizzy closed 1 year ago

DeltaDizzy commented 1 year ago

Describe the bug When using the Java version LTVDifferentialDriveController, the constructor takes a very long time to complete. On our RIO 1 it ran for a little over 6 and a half minutes before the JVM crashes with an out of memory error. In sim, it ran for over 11 and a half minutes before i stopped it, still without finishing the constructor.

To Reproduce Steps to reproduce the behavior:

  1. Create a new WPILib Java TimedRobot project with desktop support
  2. Edit Robot.java to contain a single instantiation of an LTVDifferentialDriveController and print statements before and after it (such as here). The LinearSystem constants and trackwidth used were determined via SysId (albeit a java logger impl) on a real drivetrain.
  3. Simulate the code and note that the first print statement prints but the second one does not.

Expected behavior The two print statements should both print within a few seconds of each other, indicating that the LTVDifferentialDriveController object was successfully constructed and is ready for use.

Desktop (please complete the following information):

calcmogul commented 1 year ago

A linear Kv of 0.00052047 m/s means that 12V results in a steady-state velocity of 23,056 m/s. The LUT is generated in 0.01 m/s increments, so there's 2.3056 million entries. On my machine, the LTV LQR instance takes 30 ms to construct each, so that's a total LUT generation time of 19 hours (and 7 minutes, 40 seconds in C++ because it takes 0.2 ms per instance).

A Kv in the correct size range should address the issue, altho we should emit a warning if the max velocity calculated from the drivetrain model is too high.

calcmogul commented 1 year ago

We should also look into smarter interpolation schemes so we don't need so many LUT entries. Based on the shape of the controller gain manifold from my experience trying to memoize the LTV controller gains before, a second-order or third-order Taylor series expansion may be sufficient.