# Swerve FORWARD kinematics

*For those interested in the math. Just added a paper on forward kinematics for swerve:

**

good stuff.

However, your solution should be limited to debugging activities only… You mention this as an application but the front matter can be misleading. This is because solving for the motions is a DYNAMICS (Kinematics AND Kinetics) problem. And with the exception of rare stick-slip frictional paradoxes (not present in tire models) motions are both solvable and UNIQUE. Allowing the tires to slip means that the wheel/ground interface is compliant and requires representation as forces not exact kinematic constraints (pure rolling). Kinematics is then the wrong tool for general motion prediction.

From a kinematics perspective, the system becomes LOCKED (grinds to a halt) as soon as one of the wheels does not track with the motion of the other three. Because you also specify the wheel speeds (instead of torques), inconsistent kinematic variables would instead be visualized as tearing the chassis apart (into 2, 3, or even 4 pieces). The kinematics problem is very much over constrained and as a least squares solve of the same, it is really only valid in the immediate neighborhood of kinematically consistent states (a measure of “scrubbing” as you say). In this case, the slight compliance in the tires and chassis flexure will make-up for “nearly” perfect outputs.

Actual wheel tracking will have much more to do with the normal force under each wheel AND bias will be present for those wheels that are currently in a pure rolling (not skidding) configuration. It is entirely possible that this least squares solve will result in nice smooth, fluid, and straight motions if you miss your target angles and speeds, where the actual system will instead rock, hop, and hunt for a direction of travel (not to mention being torque limited).

Given all of the off season interest here in Swerve Drive, I’ve been thinking about putting out a configurable 3D Swerve Drive simulator. If there really is is interest in physics based virtual system test, programming before you build it, or just having fun with something you’ve no intention of building, then PM me or post a reply in the VIRSYS thread with your request. I can support LabVIEW, C/C++, Java, Python…

There was no suggestion to use forward kinematics (FK) in the code that goes on the robot. The FK solution was presented for its mathematical interest and for possible use to check the correctness of inverse kinematic (IK) calculations. For those purposes it is entirely suitable.

solving for the motions is a DYNAMICS (Kinematics AND Kinetics) problem

That is true only if the residual RSS of the FK solution is non-negligible. If the RSS is negligible, the FK solution provides an estimated prediction of vehicle motion whose accuracy is no worse than the accuracy of IK calculations of wheel angle and speed required to produce the desired vehicle motion. AFAIK, most FRC teams use IK, not a complex dynamic model, to convert driver commands to wheel speeds and angles for swerve.

And with the exception of rare stick-slip frictional paradoxes (not present in tire models) motions are both solvable and UNIQUE.

Two things:

1. It’s not clear what you meant by “paradoxes” in this context, or why stick-slip should be “rare” (more about this below).

2. the non-uniqueness statement in the paper was entirely in the context of FK, and in that context is true as it stands. The FK problem (for swerve) involves an overdetermined system of nonlinear equations. There is no unique solution, in general, for this kind of problem; but a least-squares solution can be found and is useful subject to the caveats discussed here.

Allowing the tires to slip means that the wheel/ground interface is compliant and requires representation as forces not exact kinematic constraints (pure rolling). Kinematics is then the wrong tool for general motion prediction.

You missed the point I think. If the residual RSS of the FK solution is small, then the solution represents a kinematically correct prediction of the vehicle motion. It’s every bit as correct (or incorrect) as using IK to convert driver commands into wheel speed and angle commands. If you want to question whether IK should be used to convert driver commands to wheel speeds and angles, that would be a separate (and interesting) discussion. My guess is that introducing the complexity of dynamics would result in only marginal improvement at great cost in complexity.

From a kinematics perspective, the system becomes LOCKED (grinds to a halt) as soon as one of the wheels does not track with the motion of the other three.

True. But as you mentioned (see below) this is not what happens if the FK residual RSS is small. If the RSS is small, the FK result predicts the vehicle motion every bit as accurately (or inaccurately) as IK determines the correct (or incorrect) wheel speeds and angles required to produce the driver-commanded vehicle motion.

In that case, the slight compliance in the tires and chassis flexure will make-up for “nearly” perfect outputs.

Exactly.

Because you also specify the wheel speeds (instead of torques), inconsistent kinematic variables would instead be visualized as tearing the chassis apart (into 2, 3, or even 4 pieces).

I prefer to look at it this way: If the RSS is large, what that’s telling you is that the given wheel speeds and angles are not IK-correct, and that FK cannot reliably predict the vehicle motion.

The kinematics problem is very much over constrained and as a least squares solve of the same, it is really only valid in the immediate neighborhood of kinematically consistent states (a measure of “scrubbing” as you say).

Correct.

Actual wheel tracking will have much more to do with the normal force under each wheel AND bias will be present for those wheels that are currently in a pure rolling (not skidding) configuration.

As mentioned above, the same comment applies to IK as well as FK.

It is entirely possible that this least squares solve will result in nice smooth, fluid, and straight motions if you miss your target angles and speeds…

If the given wheel angles and speeds are not IK-correct, then there will be a large residual RSS in the FK solution which will warn you of that, and you will know that the FK-predicted vehicle motion is not reliable.

…where the actual system will instead rock, hop, and hunt for a direction of travel (not to mention being torque limited).

In other words, stick-slip will not be “rare”. Lots of luck modeling rocking, hopping, and hunting accurately!

**

Have some faith. We all know how to do something, this and leading others in the same would be my thing. Writing clear forum replies apparently is not. I will stick to answering questions specifically about the VIRSYS capability from now on. CHEERS!

In our first season of dynamic simulation, Team 302 is doing a great job with 6 wheel skid steer and used very few actual measurements. We reproduced jumping and sluggishness in our speed based controls and went back to open loop power for less precise but smoother and faster performance. The model showed all of this (but it was late in the game when we finished development).

In the case of swerve drive, the onset of any instability is all we really want to know about. That should not be difficult to capture. Exactly how it goes from there… we don’t really care to capture with lots of accuracy. We just know to avoid it.

Like I said, I volunteer to code it for anybody that wants to play with it and document their findings. No takers yet.

In forward kinematics it is NOT that there is no UNIQUE solution to this problem. It is that is NO solution in general. The kinematic constraints are violated in your “solution”, so by definition it is NOT a solution. The mechanism locking or exploding is just as valid and a much more spectacular solution!!! I can’t remember which, but one software solver used to do this for you.

Stick-slip is not rare. A paradox generated by stick-slip behavior is, for example: http://en.wikipedia.org/wiki/Painlevé_paradox. Furthermore, dynamic models of tires don’t usually apply a stick-slip intermittent “constraint”. It is done with applied forces. Sorry no link, you’ll have to do a bit of research for that one.

The inverse kinematics determines ideal control set points. Your forward kinematics is suggested to verify performance of actual output subject to hardware limitations such as truncations (you specifically say truncations). Agreed, it should be limited to verification of the ideal set points. It can also be useful as a measure of badness but you don’t need least squares for that.

A dynamic model is required for verification of system performance approximating real conditions. The intention is not to put it on the robot for FRC applications. It is used instead of a robot for virtual verification and software integration. You know… best practice systems engineering.

In the literature it is referred to as a least-squares [i]solution.

dynamic models of tires don’t usually apply a stick-slip intermittent “constraint”. It is done with applied forces. Sorry no link, you’ll have to do a bit of research for that one.

I did high-performance antilock brake systems for aircraft in a previous life. I am familiar with tire models.

The inverse kinematics determines ideal control set points. Your forward kinematics is suggested to verify performance of actual output

Let me take one last crack at this: To the extent that correctly-computed IK wheel speeds and angles actually produce the vehicle motion that the driver commanded, FK applied to those same wheel speeds and angles will predict vehicle motion. This is tautological. The FK was presented for its mathematical interest, and for possible use to verify the outputs of IK code.

Dynamically modeling the vehicle’s response is a separate endeavor. I don’t discourage you from doing this. I hope you take the time to publish a detailed paper for the benefit of all here.

**