Using Inverse Kinematics to become a Master-Swerver
Intro
Hello and welcome to a tutorial on how to program the infamous swerve drive! Why so infamous? Well … it’s better shown than told. This video by FRC team 1690 shows off the incredible power that a swerve drive can bring. Observe the craziness of motion and its incredible speed (yes I’m fangirling :))
Cool right! So, how do we even begin to control something like this. Well, let’s get an understanding for how a swerve drive works.
Understanding the Swerve Drive
The key difference between swerve and normal drive bases is how the wheels are driven. For normal drive bases, wheels are fixed to each side of the robot, therefore only providing a forward or backwards force on the drive base. However, swerve drives have the ability to independently drive and steer each one of the wheels from each other. This is because each wheel is driven by two motors. One controls the steering direction of the wheel, and the other controls the speed of the wheel. This allows it a greater degree of freedom that a normal drive base simply doesn’t have. For example, a normal drive can’t move sideways or translate at an angle like a swerve drive can. Here are some basic configurations that allow for basic movements on the swerve drive.
So, assuming we can control the orientation and speed of each wheel on a swerve drive, we can do some basic movements. However, this isn’t what 1690 was doing with their swerve drive. They were going crazy, translating and rotating at the same time. How do we do that? First, lets talk about notation.
Notation
Now, this will probably be the lamest part of the tutorial, but it is so crucial to maintaining a properly functioning swerve drive. There were a countless number of times where our team didn’t communicate the terminology properly, and it hurt our ability to debug issues.
For each wheel (I’ll refer to it as a pod from now on, because each wheel is a package of the 2 motors, gearbox, and wheel), we need to represent what direction it is facing (the steering angle), and we need to represent the speed of the wheel (the drive speed). Likewise, the motor that controls the direction of the wheel will be called the steering motor, and the motor that controls the drive speed of the wheel will be called the drive motor.
For the entire drive base, we need to characterize its motion in two dimensions. Any object in a plane has three degrees of freedom. It can move up/down, right/left, and rotate. To simplify the math, we will put this all into a 3 element vector [vx, vy, w] where vx is the velocity in the x direction, vy is the velocity in the y direction, and w is the angular velocity. We also need to define a naming scheme for each pod, because it is easy to confuse them during debugging without a form of identification. This is often done by noting down where they are located on the robot (we used the convention left-front, left-back …). We also used a predetermined order for these pods whenever using a bundle of pod data in order to keep all of our code organized.
The Actual Inverse Kinematics
So, now that we have a good understanding of how to characterize the swerve drive, how do we actually make it do fancy stuff? In a more rigorous sense, what are the pod setpoints that generate the desired translational and rotational velocity? To find these setpoints, we are going to use the equations that define the movement of rigid bodies(Derivation can be found here). However, before we go into the equations, I’d like to motivate them a bit.
So lets take two points in a rigid body named A and B. Because the body is rigid, the distance must not change between the points. Therefore, if A is translating at some velocity, B must also translate at this velocity. Now take the case where B is rotating around A at some rotational velocity omega with a vector r that represents the displacement between the points. Since point B is moving in a circle, it will follow uniform circular motion at a speed proportional to the angular velocity and the magnitude of the radius. Therefore, the velocity vector of point B induced by rotation will equal w x r where w = [0, 0, w] and r = [rx, ry, 0]. By taking a cross product of these vectors, we get a product that is perpendicular to the radius in the direction of rotation of the rigid body. Combining these two cases, we can add the velocity vectors induced from translation and rotation, giving us the general equation:
This will describe the motion of any point relative to another that has a defined angular and translational velocity. Going back to the swerve drive, we can use this equation to model the pod setpoints given swerve setpoints. Given a desired translational and rotational velocity of the center of the drive base (Point A) and the location of the pod relative to the center (Point B), we can use the equation above to find the exact velocity vector that the pod needs to actuate. Doing this for all 4 pods, we can generate each setpoint for each pod. Here is a demo of our swerve inverse kinematics visualizer that implements this equation to find the desired pod setpoints given the drive base setpoint.
The green arrows are all of the pod setpoints, the blue arrows are the translation setpoint, and the tan vectors are the velocities induced by rotation. Notice how the vectors add up together to make the final velocity. Here is the underlying code in LabVIEW that does the math for inverse kinematics.
Additional Details and Optimizations
Now that we have the velocity vectors for each pod, we need to convert the vectors to a steering angle and drive speed setpoint which can be controlled through PID. The naïve solution would be to set the magnitude of the vector as the drive speed and the angle of the vector as the steering angle. Although this is correct, it isn’t the optimal setpoint for each pod. This is because there are actually two motor setpoints that will give the same velocity vector. Given a setpoint (v, θ), the pod can send a drive speed of v at an angle of θ, or a drive speed of -v at an angle of θ + 180. Therefore, the software needs to select a solution that is easier to reach. In our case, it is easier to invert the drive speed instead of the steering, so we prioritize the solution that is the closest to the current orientation of the swerve pod. This can be seen in the video of the inverse kinematics visualizer whenever the arrow for a pod turns red and dashed. This indicates that the pod drive speed is negated. Doing this will yield an optimal inverse kinematics program.
The final detail to add is the ability to control the swerve drive in field centric instead of robot centric. This means that the reference frame for translation is shifted from the robot to a global frame.
In the picture, notice how what the robot perceives as forwards is different from what the an observer on the field perceives as forward. This will be true for translation in general, which will be offset by θ, the orientation of the robot, between robot and field coordinate frames. This is important, because the formulation for swerve control assumes that commands will be sent in robot centric. However, most drivers will prefer a field centric form of control. Therefore, we need to convert field centric commands sent by the driver into robot centric which can then be used by inverse kinematics. This can be done by rotating the field centric vectors to create equivalent vectors in robot centric.
Conclusion
And there it is! Now you know the art of swerve, and you can make robots float all over the place. One caveat though. 1690 shows off a lot of autonomous swerve control for executing special trajectories. Although it still uses the same underlying math to make the drive base move, it requires a path planner to generate trajectory setpoints. For those of you who want to go down this path, there are plenty of resources I will link below. Otherwise, happy swerving!