Simulating a Swerve Drive with the 2021 WPILib Libraries

Since everyone seems to be trying to use swerve these days, our team also attempted to run a swerve bot for the 2021 Infinite Recharge @ Home Skills Challenges. While we ultimately defaulted back to our 2020 robot, we were still able to make a lot of progress on it that we’ll be returning to now that the challenges are done. One of the things I’d like to showcase is some of the stuff we’ve been doing in regards to swerve code.

Some background: In the middle of the 2021 build season, our programming team had mostly finished working on the trajectories we would use for the 2021 At home challenges by simulating using a model of our 2020 robot. At this point, we were still waiting on the assembly of our swerve bot before we could do more extensive testing, so we decided to try to make a swerve simulation since one didn’t exist in the 2021 WPILib libraries.

Since simulation was new and we initially had a limited understanding of it, we thought that ‘cheating’ a solution with what we knew have been easier to do than simulate the actual thing, but would still be representative of a swerve drive’s actual behavior.

The first idea of how we tried to do this was to stack 3 DifferentialDrivetrainSims on top of each other. Because if you think about it, a swerve drive is just three drivetrains on top of each other (One for each degree of movement). This initial concept kind of worked. This is an example of how it looked like in teleop.


While it was able to generally translate controller inputs into the proper movement of a swerve drive chassis and the modules, it ultimately could not replicate the fine movement you would need for it to represent autos effectively. After trying various methods of getting this to work with autos, we ultimately abandoned them due to running into various issues. Around this time, our team also decided to switch back to our 2020 robot for the skills challenges due to not having enough progress done on our swerve bot where we would feel comfortable enough to do the skills challenges successfully. While the rest of our programming team focused on completing the challenges, I kept thinking back on how to make a better swerve simulation model. At this point, I decided to attempt to implement one of my earlier concepts that I thought was too silly to work.

My second idea of how to approach the problem was to simulate swerve using 4 DifferentialDrivetrainSims. Because if you really think about it, a swerve drive is really just 4 differential drives running in tandem. I toyed with this idea for a bit before putting it on pause on it, as I didn’t make much progress at the time. When people started showing off videos of their swerve bots performing the Skills Challenges, it motivated me to look at the problem again (Plus the entire concept sounds like a huge meme so getting it to work would be funny). Looking at it with fresh eyes after a break got me to see how overly complicated I was making it. So after simplifying the code, I got a model of it to work.


I had to ‘fudge’ the numbers a bit for the inputs to the DiffentialDrivetrainSim by putting multipliers to the simulation inputs since I was converting the throttle and turn inputs of the module motors into a left/right differential output, but I deemed this to be an acceptable workaround as it was a very simple modification.

At this point, I had played around with simulation enough that I thought “Why not just do it properly?”. So the third concept I had was to use 8 FlywheelSims. Because if you really think about it, a flywheel is really just a simple motor, so if you had one for each swerve motor, it should represent a swerve drive fairly well.

Skills Challenges:

TeleOp Control:


The result of this was pretty good, looking at the examples above. After the Skills Challenge deadline, I shared the results with my team to play with it for a bit. After playing with it and talking about trajectory following in general, we identified a few improvements with how we were generating trajectories. We switched from making separate trajectories that were merged by modifying the start/end velocities and then following them in succession to making a single trajectory generated by using a list of Pose2d’s.

This ended up removing the last bit of error from the initial results and made the paths more consistent (less pausing between sections). Backporting this to our 2020 robot would have probably improved some of our skills challenge’s times by a few seconds if we had realized this earlier, but oh well.

The only thing this model cannot do well is follow a path with a changing heading (e.g. orbit/strafe a point/target). While I’ve attempted to get this to work, I think this may be the limit of how this implementation works. Otherwise, I think this should pretty much cover 95% of what most teams would need for simulating a swerve drive.

To help teams that don’t have easy access to their robots, we’ll be releasing a public repo containing two examples of this code. The first is a pure WPILib version modified from the original WPILib SwerveControllerCommand example. The second one is based off of what our team modified from the SwerveControllerCommand example that utilizes more vendor specific code, as well as some of the more complex things we’ve implemented that you see in the gifs above.


Very sweet! I’m super enthused to see what sorts of results can come out of the different modeling approaches.

Last fall, there was some discussion on the FRC discord about doing a swerve model with the state-space techniques that are being used under the hood for differential drivetrain and flywheel sims. Sounded like there was some progress, but the final result wasn’t stable enough for general consumption.

In parallel, I had tried a sum of forces + sum of torques approach with a not-very-smart discrete step solver. I can’t claim accuracy with no real robot to compare to, and don’t have any proof of stability, but driving it around the field it looks kinda correct.

With any luck, we’ll have some hardware available this summer to test it out on.


I think I might have not been clear about what I meant by ‘changing heading well’. So in the final examples of the Skills Challenges, the trajectories were fed a constant 0 heading and the simulation held the robot’s heading in place. The issues we’re running into with regards to heading deals with when you attempt to supply a heading that is constantly pointed towards a target as you move, especially if the path starts to get more complex. A better visualization of what I meant:

I’m not sure if this is a defect in how we implemented the simulation, or if its due to the characterization/PID values we’re using. I’d like to compare the simulation to how an actual robot would behave first before attempting to do any significant changes to deal with this single issue. Hopefully with our school starting to open up, we’ll be able to test this in the coming weeks and improve this.


How do you simulate?