Smooth Path Generator for RoboRio 2015

[NotInControl]

After the Cheesy Poofs (FRC 254) put on an amazing display of robot path following during the 2014 FRC championship finals, a lot of buzz has been generated by other teams wishing to implement a similar feature on their robot. The poofs released their code and the community learned they used 5th order polynomial splines in order to find the best fit smooth curve from Point A to Point B.

Path planning has been the primary focus in many other fields outside of FRC. For example in Game AI, generating smooth paths for the main character to follow when going around obstacles needs to be calculated in real-time. In other robotics fields such as autonomous car navigation, many smooth paths need to be generated simultaneously, and the "best" path chosen.

These fields have spent many years adopting and optimizing the field of motion planning for their specific application. However, what they have in common is that the calculations for generating the path needs to be performed in real-time, and typically are performed on inexpensive embedded devices, such as gaming consoles or car computers. As part of my PhD thesis (developing a controller for an Autonomous Car) I had the opportunity to create a simplified path planner for real-time mobile robot applications.

This class is a port of the code I developed for my PhD thesis, to perform the function of path calculation on a mobile, skid-steer robot very quickly (near real-time). The algorithms provided here rely on algorithms used in optimal control theory to converge to a solution very quickly. The code here uses the "gradient descent" optimization technique to coerce a simple waypoint path, into a smooth trajectory.

Once the smooth trajectory is determined, parallel paths for the left and right wheels are calculated based on the robots trackwidth distance, and corresponding velocities for each of the wheels are determined. As long as the robot has independent speed controllers for the left and right side of the drivetrain, the user can "play" through the velocity profile as if they are setpoints and the robot should drive the corresponding path.

The function takes in just 3 parameters to get started and boost a very simple user interface. I started writing documentation which lives on the ReadMe in Github, check it out to get started.

You can check it out here: https://github.com/KHEngineering/SmoothPathPlanner

The program is written in Java, but outputs arrays which can support C++ and Labview teams as well. It does not require any external library and can run on any desktop, or even the RoboRio. I have even included a plotting function modeled off the matlab plot function, to help visualize the path. (You can't run the plotter on the RoboRio because it is headless, and the JVM is configure to throw an error if you try to display anything).

This code can run directly on the RoboRio to calculate new paths on the fly and the response times have been under 30ms for a complete calculation of all points with paths of about 50 waypoints (more than generous for most cases). I currently have this running on the Beta 2015 system. I will be updating the documentation, and streamlining the code as we approach closer to the 2015 season, along with ironing out any bugs, but if I didn't release this now, I probably would never do it.

So please use it, provide any feedback, and if you run into any errors, bugs, or crashes, please let me know by creating a issue on github. Here are some screen shots of what this can produce.

THIS CODE IS NOT ASSOCIATED WITH MY TEAM, I DEVELOPED IT AND AM RELEASING IT BASED ON MY PERSONAL RESEARCH AS I BELIEVE IT WILL BE USEFUL FOR OTHER TEAMS COMPETING IN FRC.

Regards,
Kevin