Just out of curiosity, have any teams applied the WPILib LQR controller class on their robots?
IIRC a few of the teams that ran diffy swerve used them. They are also used in the state space examples for wpilib.
2 Likes
What teams ran diffy swerve? Itâs not legal in FRC (unless you are only powering two modules?)
1 Like
In years past, not this year. That would be ninjaneers and then there was another 5000 (I think) team that used an LQR controller for sure. Search CD for them and a few with public code should show up.
5 Likes
We ran state space control with an LQR controller and Kalman filter for differential swerve on our 2023 robot, if you have any questions about it Iâd be happy to help.
1 Like
3512 used LQR and Kalman filters on everything that moved from 2018 offeason to 2020. For context, I was their software mentor and I wrote the C++ state-space library for WPILib 2020. I stopped mentoring 3512 after 2021 (with some transition support for 2022). 3512 stopped using state-space and switched to Java in 2023.
- 2018.1 with model and controller gains generated by Python scripts
- Drivetrain (differential drive model)
- Elevator (elevator model)
- 2019:
- Climber (elevator model)
- Drivetrain (differential drive model)
- Elevator (elevator model)
- Four-bar lift (arm model)
- 2020:
- Drivetrain (differential drive model)
- Turret (arm model with no gravity)
- Shooter (flywheel model)
The main benefit of model-based control like LQR is you can leverage those same models for high-fidelity physics sim. The sim let us write and test new features without a physical robot, catch regressions via unit tests run on GitHub PRs, and separate software troubleshooting from hardware troubleshooting. This was especially important in 2020 since we got the completed robot like the day before competition.
In fact, our sim and testing infrastructure got so good that we often just let the robot sit motionless on the carpet all day during the dedicated âsoftware testing timeâ in offseason. This really confused the head coach who firmly believed a real robot was better. The thing is, we used telemetry to debug issues, and we had plenty of that in sim already; watching the robot drive around didnât tell us much of anything.
We ran our autonomous modes as integration tests; the modes had Google Test assertions and hardware mocking via if constexpr blocks embedded within them. The test driver also asserted the function completed within 15 seconds (the duration of autonomous mode).
The students and I loved this way of working, but I wouldnât recommend it for most teams because it requires a lot of training and discipline. The state-space API is also way easier to use in C++ for ergonomic and performance/determinism reasons, but most teams use Java. The compilation times in C++ are also really long if you donât architect your codebase carefully and use the common system sizes WPILib already explicitly instantiated.
8 Likes
This season, our team experimented with implementing LQR control on our MK4i Swerve modules. We observed that while the LQR controller performed effectively with fixed setpoints (e.g., 1.0 m/s for drive and pi/2 rad for steer), its aggressive nature led to a notable issue when using joystick control. Specifically, rapid transitions between setpoints resulted in loud noises emanating from the swerve gears. We are curious if others have experienced a similar problem.
Our team used Java for our robotâs software this season. We implemented an LQR controller for both our elevator and arm mechanisms. We also developed a Python simulation, and the mechanisms perform well in simulation, with decent performance observed in real-world operation.
However, weâve encountered a persistent issue where the LQR control exhibits a slight error, approximately 1-3 cm for the elevator, regardless of how we tune the Q matrix. Itâs challenging for us to determine whether this inaccuracy stems from an imprecise model or issues with our testing methodology.
We are very interested in understanding how teams specializing in state-space control achieve high speed and precision with their mechanisms. We would greatly appreciate it if you would be willing to share any insights or experiences you might have.
Here is a test video of our elevator (using two Kraken motors with a 1:12 ratio): https://drive.google.com/file/d/1_c5_uJoZtPpi35qZrUHOz6d_9A0bJcn1/view?usp=sharing
Elevators are subject to gravity, so proportional controllers will have steady-state error. A gravity feedforward should fix it. Tracking performance is also greatly improved with a motion profile.
1 Like
We are currently using Trapezoidal Motion Profiles and Gravity Feedforward. We generated our weights based on Brysonâs Rule.
Our primary challenge is the trade-off between precision and overshoot. If we tune the controller to achieve minimal error, it invariably exhibits some degree of overshoot, even if small. Conversely, when we allow for a slightly larger steady-state error, the overshoot is eliminated, but a persistent error remains.
Based on your experience, what level of precision is typically achievable with LQR? Additionally, could you share how you typically conduct your testing?
Thank you for your time and insights.
We were able to eliminate steady-state error and overshoot completely.
Ensure your Kalman filter is accurately estimating velocity from the model and position measurements. Also, minimize measurement and control input latency, since that will limit gain aggressiveness. FYI, LinearQuadraticRegulator.latencyCompensate() reduces the feedback gain to whatâs optimal in the presence of latency rather than actually fixing it. CTRE does fancier stuff like estimating where the system should be based on the most recent position and velocity measurements and expected latency.
3512 used CAN motor controllers, but with quadrature encoders routed directly to the roboRIO DIO. If I recall correctly, Phoenix Pro has a timestamping feature you could potentially use to latency-compensate measurements sent over the CAN bus.
You can also try input error estimation (model-based integral control) from section 6.7.2 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf to estimate the unmodeled disturbances and cancel them out. Section 6.9.2 has an elevator input error estimation example.
We ran motion profiles with everything plotted vs time (references, state estimates, measurements, control inputs). We used a different plot for each unit, and we included plot titles and axis labels (e.g., âVelocity (m/s)â). These plots usually exposed all the issues.
Our most difficult case was gearbox backlash in our 2019 four-bar linkage; mechanical couldnât eliminate it, so we reduced the control input weights to make the controller less aggressive.
2 Likes
Thank you for your advice. We will try thatďź