[examples] There is no example code for using DrivetrainVelocitySystem for anything except simulation

[ewpratten]

I am currently looking to use LinearSystemId.createDrivetrainVelocitySystem with a KalmanFilter, LinearQuadraticRegulator and LinearSystemLoop to control a drivetrain's velocity, but I have noticed that there is no example code for this, whereas flywheel, elevator, and single-jointed-arm all have example code.

[calcmogul]

We originally intended to add drivetrain examples that used globally convergent controllers (Ramsete, linear time-varying LQR) rather than velocity-only control. That part of the state-space effort has been temporarily stalled because we wanted to get the pose estimator with vision correction done first.

I can't find the authoritative version of that code right now, so I'll mention what my FRC team did; we'll add wrappers for this at some point. We used a 5-state model (x, y, heading, left velocity, right velocity) for control. We calculated the velocity model from linear and angular constants from frc-characterization, then turned it into a model using frc::IdentifyDrivetrainVelocitySystem(). We stuck that in the 5-state model's f(x, u) dynamics function, took the Jacobian so we had a linearized model, then ran LQR on it. We had some extra logic to make sure the velocity never hit zero, because a degree of freedom becomes uncontrollable at v = 0 and the DARE solver throws an exception.

https://github.com/frc3512/Robot-2020/blob/master/src/main/cpp/controllers/DrivetrainController.cpp#L210-L301 https://github.com/frc3512/Robot-2020/blob/master/src/main/include/controllers/DrivetrainController.hpp

We used a 10-state model for the EKF so predictions were more accurate. The matrix block() calls can go away if you have just a 5-state model. Section 8.6 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf has a derivation for it, which might be easier to read than our code. In particular, we implemented theorem 8.6.4.

[ewpratten]

@calcmogul I'll take a look