Swerve path and motor trajectory generator?

Does a path planner exist for swerve given way points x y and heading?

I think you could benefit greatly from Ether’s swerve kinematics and programming documents. Look for the excel spreadsheet. All you need to give is a Joystick Y(up/down), X(left/right strafe), Z (Rotation) and Gyro Angle and it visualizes the angle and speed of 4 swerve wheels.

There is also a paperon how to write these algorithms into the programming language of your choice.

Hope this helps.

I think what he’s looking for is something that will generate a sequence of angles and powers to do over time not just a single setting to choose. Interestingly, I have written such a thing for some restricted cases but was more of an exploratory exercise and didn’t actually make it onto a robot.

To illustrate the difference between these two things, suppose what you want to do is go left. Ether’s spreadsheet will tell you to turn all the wheels should face left and the power should be 100%. If you want to actually make trajectories what you would have is the sequence of “first turn wheels so they face left, second then turn power on for 1 ft.”. Incidentally, this is not the fastest way to get to a given x,y coordinate unless you’re already facing that direction.

I assume that you have a functioning swerve chassis. Before you start on figuring out path planning, can your robot drive straight. If you command it to drive 20 ft straight ahead can your current platform do it? With perfect alignment and say 10 or more test can your robot go perfectly straight every time? What is the error spot. If your swerve can’t drive straight then your not ready to tackle path planning. We have found that for autonomous navigation with our swerve required the minimum of adding the kit of parts gyro and a PID control loop to drive straight. This year we used the Navx MXP. We have encoders on all wheels to measure wheel rotation (distance). Are you to this point yet? If not you have allot of work ahead.

I have a functioning swerve chassis that I can program to do anything I want really with great accuracy with encoders (for wheel angle and wheel rotations), as well as a gyro. I can set it to go x feet forward and end at a certain angle and it will do that in the amount of time that I specified thanks to @faust1706’s neural network library. I’m looking for a swerve implementation of 254’s waypoint navigation using splines:


I am looking for the same thing, except with swerve, that takes waypoints x y and heading and will traverse the path in a specified t time.

:smiley: It’s been a pleasure working with you.

I actually have exactly what you are looking for, verbatim actually…It is entirely untested, however.


I should have just asked you, it seems that you always have exactly what I am looking for or know exactly where to find it. I’ll try to get this on the robot as soon as I can.

I’m sorry but what exactly is the implementation of this? As far as I understood, the original point of 254’s spline based navigation was to give a robot that couldn’t strafe an option for getting to a very specific point and orientation. With a swerve you can just give it points to get to and (as long as you have done due diligence and implemented dynamic acceleration limiting in your code) it should drive smoothly, at least it did for us. What kind of situations are you foreseeing where a holonomic robot would actually need to follow a defined curve rather than just a set of waypoints?

I don’t know about the OP, but the reason I have considered doing this is for avoiding objects rather than trying to drive into them (i.e. the cans for a 20pt autonomous, or even other robots if you have a way to detect them and can run the path generator in real-time).

One quick implementation of this is picking up objects on the fly. If you know where an object is with respect to the field, then you can drive to it, while driving to it, orientate your robot to quickly pick it up, have a passive grabber grab it, have your robot then proceed to go to a scoring position, then score, all in one fluid motion.

For a truly holonomic robot, you can generate three separate 1D trajectories for x, y, and heading and have the robot follow them. Many existing pieces of public code, including the 254 libraries, would be appropriate for this task.

Here’s a video of a “robot” “simulation” that a programmer on team 2046 did of the program.

I guess I should post an update: @faust1706’s code works perfectly on a robot if you have the ability to go n encoder counts in t seconds, which I do with his neural network program that I trained to do just that.

1 Like

How do I run a simulation using the code on my laptop like the video above?

You can get the latest code from here: https://github.com/faust1706/Smooth-Swerve

The simulation is what this code displays as of right now, but that can be changed. To alter the simulation, change the waypoints to whatever and however many you want. Make sure the heading variable is of the same size as the waypoints array size, otherwise the program will get an outofbounds error.

I have not rigorously tested this code, so feel free to try to break it. If you do break it, either by making it generate an incorrect path or something, then leave a comment here or make it an issue on the repo’s page.

The simulation is also completely smooth now thanks to the programmer on 2046.

This is likely far, far more complicated than you would ever, ever want to put on an FRC bot, but it’s definitely an interesting read. It’s kind of the de facto algorithm in the RoboCup collegiate robotics competition. I guess in college, the programmers more or less get free reign to do whatever they want. It helps that they have a bit more than 6 weeks, too :wink:

Anyway, the paper: http://msl.cs.uiuc.edu/~lavalle/papers/LavKuf01b.pdf

Reading that hurt my eyes…Nonetheless, a very interesting. My current research pertains to differential topology with path planning in embedded systems, so this topic is very dear to me.

I found another pdf of the paper posted above that is easier on the eyes to read: http://msl.cs.uiuc.edu/~lavalle/papers/LavKuf99.pdf

Has a team in the past implemented state space based machine control? If so, could you point me to where I could read about what they did?

This is absolutely amazing. Is there any LabView Implementation of this?

There doesn’t need to be, as this output can be fed into a robot written in labview. You generate your paths off board ahead of time, save off the velocities and headings to a file, then have labview read through that file. If you have good enough control of your robot (go x encoder counts at h heading), your robot will follow the path generated from your way-points.

I think the best game for this is 2011 when doing a multi ubertube autonomous. Maybe something this advanced will be needed in the future.

Let me know if you have any questions.