Hello everyone, I hope you are very well, I would like to know if you can explain the inverse and direct kinematics of swer drive, I want to make a robot with two active wheels caster1 ans 2) a third passive wheel (3).
I await your comments, any good help is well received and appreciated.
Hello!
Many years ago, a famous member of this community made this post:
I suspect it may be a helpful example in deriving the equations you are looking for.
Suffice to say, this is a unique combination of wheels that I’ve not yet seen in anyone’s code or robot. I would say this is a bit of unexplored territory.
WPILib’s SwerveDriveKinematics class supports an arbitrary number of wheels >= 2. The WPILib implementation uses a generalized form of the derivation in section 12.7 “Swerve drive kinematics” in https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
I’ll be using the following coordinate axes:
Each module has two actuators and thus two states: velocity of the module along the x axis (v_{1x} for module 1), and velocity of the module along the y axis (v_{1y} for module 1). Since your third wheel is just there to keep the robot from tipping over, your inverse kinematics (skipping over the steps shown in the book) would be:
\begin{bmatrix} v_{1x} \\ v_{1y} \\ v_{2x} \\ v_{2y} \end{bmatrix} = \begin{bmatrix} 1 & 0 & -r_{1y} \\ 0 & 1 & r_{1x} \\ 1 & 0 & -r_{2y} \\ 0 & 1 & r_{2x} \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}
where r_{1x} is the displacement from the center of rotation ^\dagger to the wheel module along the x axis (same logic applies for other module and axis combos), v_x is the chassis x velocity, v_y is the chassis y velocity, and \omega is the chassis angular velocity. You could multiply that out to get four separate equations for the velocity components
\begin{bmatrix} v_{1x} \\ v_{1y} \\ v_{2x} \\ v_{2y} \end{bmatrix} = v_x \begin{bmatrix} 1 \\ 0 \\ 1 \\ 0 \end{bmatrix} + v_y \begin{bmatrix} 0 \\ 1 \\ 0 \\ 1 \end{bmatrix} + \omega \begin{bmatrix} -r_{1y} \\ r_{1x} \\ -r_{2y} \\ r_{2x} \end{bmatrix}
v_{1x} = v_x -\omega r_{1y}
v_{1y} = v_y + \omega r_{1x}
v_{2x} = v_x -\omega r_{2y}
v_{2y} = v_y + \omega r_{2x}
but the matrix notation makes it easier to do the forward kinematics (i.e., going from wheel speeds to chassis speed for use with odometry). You invert the 4x3 matrix (linear algebra libraries have functions that make this easy) then multiply your wheel speed vector by it to get your chassis speed vector.
To convert the swerve module x and y velocity components to a velocity and heading, use the Pythagorean theorem and arctangent respectively. Here’s an example for module 1.
v_1 = \sqrt{v_{1x}^2 + v_{1y}^2}
\theta_1 = \tan^{-1}\left(\frac{v_{1y}}{v_{1x}}\right)
or in Java,
double v1 = Math.hypot(v1x, v1y);
double heading1 = Math.atan2(v1y, v1x);
WPILib’s swerve bot example has some other useful stuff like module angle optimization (negate the wheel velocity when necessary to minimize the change in module rotation). If you decide to implement this, I suggest starting from that example and tweaking the number of modules. The motor and sensor configs are wrong for your drivetrain, but the math and plumbing is there.
^\dagger The driver can usually move the center of rotation around to do pivots around different points.
Touche, edited.
Hi everyone,
I appreciate the time you took to read and even more to give me your help, it is my first robot and the truth is that I do not know many issues, so I will greatly appreciate any advice you have for me 
In the first place I will dedicate myself to understanding inverse and direct kinematics.
@gerthworm @calcmogul
Thanks and regards
If you want to just play with it to understand how things should be moving I added a 3 wheel option to a rudimentary visualizer tool that implements the Ether math.
You’re very welcome! Best of luck as you start out.
As a word of encouragement: I really like your first step.
The math and theory background can be intimidating, and a lot of students I run into are very eager to skip over it (cuz SHINY ROBOTS ZOMG!!!). WPILib and others have done excellent work allowing students to shortcut huge chunks of it and still have success.
Still, when it comes to establishing deep expertise, including the ability to debug complex and arbitrary issues in electro-mechanical-software systems like robots… My opinion is the theory is irreplaceable. It’s part of the secret sauce to make your beard turn grey. Metaphorically, though occasionally literally.
If I have the time and want to really learn something deeply… I start with theory.
This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.