Pathfinder - Fast Motion Profiling for Java and C++, Tank and Swerve Drive

[Jaci]

Project Link

Pathfinder is an API for Motion Profiling. Give it a few waypoints and some information about your Robot and it will generate a nice smooth trajectory for your robot to follow to avoid jerkiness and keep your robot's autonomous movements accurate.

Pathfinder can generate paths for both Tank and Swerve drive systems, as shown in the images below:



(red = front wheels, blue = back wheels. Drivebase constantly facing forward)

Pathfinder's actual generation code is all written in C, so it's very fast, meaning profiles can be generated on the fly. Pathfinder supports C, C++ and Java currently, and doesn't have any dependencies! Because it doesn't have any dependencies, you can even use it in your non-frc projects if you want to.

Pathfinder can use any type of encoder as a feedback device to keep your robot on track. All the parameters are exposed, so you can run the control loop at any speed you like.

The wiki contains instructions for setup and some demo code for generation and following in both Java and C++.

If you use Toast, it will also work in simulation if you're running Windows or Mac. If you're on linux, you can build and install the library manually with the instructions in the readme.

Special thanks to Austin Schuh, Jared Russel and Tom Bottiglieri for pioneering motion profiling code in FRC, and for giving this wonderful talk. Some of the generation code was inspired by Team 254's 'TrajectoryLib', although heavily modified to be more efficient and to work in C.

[Ben Wolsieffer]

This looks really cool! I was just working on integrating 254's TrajectoryLib into our code, but I might try this instead.

I know 254 did trajectory generation off of the robot because it was too slow. I assume that the combination of the faster roboRIO processor and the more efficient C implementation made it feasible to move the generation on to the robot. How long does it usually take to generate the trajectories?

[Jaci]

Originally Posted by lopsided98

This looks really cool! I was just working on integrating 254's TrajectoryLib into our code, but I might try this instead.

I know 254 did trajectory generation off of the robot because it was too slow. I assume that the combination of the faster roboRIO processor and the more efficient C implementation made it feasible to move the generation on to the robot. How long does it usually take to generate the trajectories?

I've tested with a rate of 50Hz for the profile update, and with about ~200 generated points it takes roughly 3 seconds to generate on the RoboRIO. This year, I used this to generate the profiles before the match had begun so they were ready at the beginning of auton, by providing it with the defense layout of the field and where my robot is on the field as soon as my driver station had connected. Once the robot had received word of where it was, it would generate the trajectory and hold on to it until Autonomous

[Ben Wolsieffer]

What advantages does this have over 254's TrajectoryLib? The trajectory following code seems very similar to 254's, but did you make any changes to the generation algorithm other than rewriting it in C? The main reason I am asking this is that TrajectoryLib seems to fit more easily into my robot code than your library, and I'm trying to decide whether I should switch right now.

[Oromus]

What's the advantage to using this over, say, a well-tuned PID loop? I find this quite interesting and I'm considering using this on our robot, but I'm wondering what there is to gain from using it.

[Ether]

Originally Posted by Oromus

What's the advantage to using this over, say, a well-tuned PID loop?

Explained here, starting at 14:50

[Jaci]

Originally Posted by lopsided98

What advantages does this have over 254's TrajectoryLib? The trajectory following code seems very similar to 254's, but did you make any changes to the generation algorithm other than rewriting it in C? The main reason I am asking this is that TrajectoryLib seems to fit more easily into my robot code than your library, and I'm trying to decide whether I should switch right now.

The most major modification is C rewrite, however the structure has been changed, and some of the more mathematical functions of the library have been optimized.

The main advantages are the speed and portability. Pathfinder will run on either Java or C++ robots, and also has the added support for Swerve Drive. As for integrating it into you robot code, it should be a fairly similar experience to TrajectoryLib

[Ben Wolsieffer]

Originally Posted by Jaci

The most major modification is C rewrite, however the structure has been changed, and some of the more mathematical functions of the library have been optimized.

The main advantages are the speed and portability. Pathfinder will run on either Java or C++ robots, and also has the added support for Swerve Drive. As for integrating it into you robot code, it should be a fairly similar experience to TrajectoryLib

The main reasons I feel it is harder to integrate are the lack of the ability to load trajectories from files and that it assumes that the encoder values are not already scaled to physical units. This second limitation can be easily worked around with some math.

I am curious, though, why you chose to do the tick to distance conversion within Pathfinder. Both the Talon SRX and the WPILib Encoder object will do distance scaling for you, so why didn't you just use that?

My original plan was to have a bunch of pre-generated trajectories and just choose to load the one for our selected autonomous. I have decided to use your library after all, and generate multiple trajectories concurrently in the background. This makes the whole process simpler to manage.

[GBilletdeaux930]

Originally Posted by lopsided98

I am curious, though, why you chose to do the tick to distance conversion within Pathfinder. Both the Talon SRX and the WPILib Encoder object will do distance scaling for you, so why didn't you just use that?

Pathfinder supports C, C++ and Java currently, and doesn't have any dependencies! Because it doesn't have any dependencies, you can even use it in your non-frc projects if you want to.

Probably for that reason right there.

[Ben Wolsieffer]

Originally Posted by GBilletdeaux930

Probably for that reason right there.

Dependencies have nothing to do with this. If you are using this library outside of FRC, you can very easily do the distance conversion yourself, but when using it with WPILib, the built in conversion makes things more complicated.

[GBilletdeaux930]

Originally Posted by lopsided98

Dependencies have nothing to do with this. If you are using this library outside of FRC, you can very easily do the distance conversion yourself, but when using it with WPILib, the built in conversion makes things more complicated.

I understand what you are saying, but that doesn't make the dependency on WPILib disappear.

[artK]

Originally Posted by Jaci

I've tested with a rate of 50Hz for the profile update, and with about ~200 generated points it takes roughly 3 seconds to generate on the RoboRIO. This year, I used this to generate the profiles before the match had begun so they were ready at the beginning of auton, by providing it with the defense layout of the field and where my robot is on the field as soon as my driver station had connected. Once the robot had received word of where it was, it would generate the trajectory and hold on to it until Autonomous

This is awesome! Do you have any video of this running on your robot during auto?

My one question is why are you generating these 200 points? Because that seems like a lot. Each spline covers about 2", and you have 199 points where the quintic constraints have you driving straight locally (Since the second derivative is zero when it approaches the waypoint in the spline configuration we developed). I suspect this would make the robot drive a little twitchy, especially when trying to turn.

[Jaci]

Originally Posted by lopsided98

Dependencies have nothing to do with this. If you are using this library outside of FRC, you can very easily do the distance conversion yourself, but when using it with WPILib, the built in conversion makes things more complicated.

This has been fixed in the latest commit. Now we have a Distance Follower, which ditches the encoder conversion, assuming you are giving it values in meters (or whatever unit you're using everywhere else). The EncoderFollower still exists for people who want to feed it ticks, though.

I've also added File Serialization. It will export as a binary format to save space, or as a CSV file. In C++ this will be the functions in `io.h`. In Java, these functions are under the `Pathfinder` class.

[Jaci]

Originally Posted by artK

This is awesome! Do you have any video of this running on your robot during auto?

My one question is why are you generating these 200 points? Because that seems like a lot. Each spline covers about 2", and you have 199 points where the quintic constraints have you driving straight locally (Since the second derivative is zero when it approaches the waypoint in the spline configuration we developed). I suspect this would make the robot drive a little twitchy, especially when trying to turn.

We don't have a video running auto, in my initial code repo all this was done on a coprocessor (and worked flawlessly on a practice field), however we had some issues with FMS not forwarding ports at the regional I went to, so I turned Motion Profiling off in favor for just a 'go forward for x encoder ticks'.

The reason so many points are generated is because of the nature of the control loop. Each point has a dt value (i.e. how long this control loop iteration lasts before moving onto the next) - in our case, it was 1 / 50. Each iteration (which is timed with the Notifier class from WPIlib) will fetch the next segment and process it.

It is possible to look at the second derivs in order to reduce the amount of points, but that would (at least in the current algorithm) add more time to the actual generation sequence. I can modify the algorithm to correct for this and suffer next to no time overhead. Maybe something to put on my todo list

The path didn't jitter at all, in fact it was very smooth. Turning was very smooth and didn't seem to have any issues

[Jared Russell]

Cool!