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

[Jared]

This looks really cool. I've worked on a very similar project (https://github.com/dicarlo236/AutonomousPlanner) that has a similar end result to yours- you draw in some points and headings, and it generates a path for the left and right wheels.

I have a few questions/comments about the paths that this generates.

How exactly are you generating the curves? I see from the examples that you don't need to have headings at individual waypoints, and that the path needs to only come close to the waypoints, rather than cross. I'd like to figure out how to do this for my code, as picking headings for each waypoint can be frustrating.

If you take the inverse tangent of the derivative of y position with respect to x, you get the heading of the robot. If you take the derivative of this function with respect to time, you get how fast the robot is rotating. One of my biggest challenges was keeping this function continuous, so that the path doesn't change from a sharp left turn to a sharp right instantly (think of an "S" where the top and bottom sections are half-circles). From looking at your paths, it seems that your curves do not have these issues. How did you solve this?


I don't follow your code for generating the velocity profile very well, but I don't know how it would deal with sharp turns. From my tests, I needed to create a "maximum velocity" function for my path which told me what the fastest possible speed the robot could go at any point. I took into account the centripetal acceleration required to turn a 150 lb robot, the actual maximum speed of the robot, and the maximum speed of the wheel on the outside of the turn. Then, I generated a smooth velocity function that was always less than the maximum velocity function. It seems like you do have ways to correct your smooth velocity to be within limits, but I don't see how it works.

[faust1706]

Ether posted a math quiz on here a little while ago that basically required someone to lay out the mathematics of how to calculate the left and right path trajectory of a robot given it's route.

http://www.chiefdelphi.com/forums/sh...d.php?t=126639

How I solved the problem was some simple calculus. I download a symbolic-numeric library that allowed me to take the derivative of my original function (the path), and then I found the function that at point x gives the slope of the normal line of my derivative function. Then I solve a system of linear equations. I want to "march out" n units from my original function, so I have the equation of the normal line and a circle around my point x (x^2 + y^2 = n^2). Then I solve the system. I do that for every point, then compile the points and fit a function to it. I feel as though there is a much better way of doing this though. (This method has proven to be quick enough for it's purpose. It takes about 10ms to go through a function with precision of 1,000 points to solve for) Could either of you explain your mathematics?

[Jared]

Originally Posted by faust1706

Ether posted a math quiz on here a little while ago that basically required someone to lay out the mathematics of how to calculate the left and right path trajectory of a robot given it's route.

http://www.chiefdelphi.com/forums/sh...d.php?t=126639

How I solved the problem was some simple calculus. I download a symbolic-numeric library that allowed me to take the derivative of my original function (the path), and then I found the function that at point x gives the slope of the normal line of my derivative function. Then I solve a system of linear equations. I want to "march out" n units from my original function, so I have the equation of the normal line and a circle around my point x (x^2 + y^2 = n^2). Then I solve the system. I do that for every point, then compile the points and fit a function to it. I feel as though there is a much better way of doing this though. (This method has proven to be quick enough for it's purpose. It takes about 10ms to go through a function with precision of 1,000 points to solve for) Could either of you explain your mathematics?

My approach is similar. There may be a faster approach, but this was simple to write, and isn't the slowest part of my program. I have a big list of about 3,000 or so points that I iterate through. I have a method to calculate dy/dx already for another part of the process, so I have that value already.

leftSegments.x = segment.x - r * sin(atan(s.dydx));
leftSegment.y = segment.y + r*sin(atan(dydx));

After this, I recalculate the displacement during the step

double ds = Math.sqrt((l.get(i).x - l.get(i-1).x) * (l.get(i).x - l.get(i-1).x) + (l.get(i).y - l.get(i-1).y) * (l.get(i).y - l.get(i-1).y))

The distance travelled (for encoder following loop)

l.get(i).distance = l.get(i-1).distance + dp;

Velocity (s.dt is the amount of time that should pass between the current segment and the previous one.)

l.get(i).vel = dp/s.dt;

Acceleration

l.get(i).acc = (l.get(i).vel - l.get(i - 1).vel) / s.dt;

and Jerk

l.get(i).jerk = (l.get(i).acc - l.get(i - 1).acc) / s.dt;

The heading is the same as the original path.

[NotInControl]

Originally Posted by Jared

How exactly are you generating the curves?

Please take a read of the ReadMe on github, i try to explain the overall workings of the algorithm. But the general idea is this:
1. The original path is a collection of straight lines (the waypoint path)
2. We can interject any number of intermediate points in a straight line without changing the line equation itself (we inject the correct number of points so that there is one sample per robot timeStep)
3. Now that we have all of these injected points, we can push and pull on them to coerce them into a smooth path. The result is a bunch of straight lines, which appears smooth globally, and has smooth transitions.
4. We use gradient descent which is a first order optimization algorithm to push or pull each point just the right amount, I have "tuned" the parameters to converge very quickly so as to be useful for near real-time applications. If tuned incorrectly, the algorithm may never converge. The default parameters, should always converge, let me know if they do not. Take a look at the ReadMe for more info.

Originally Posted by Jared

If you take the inverse tangent of the derivative of y position with respect to x, you get the heading of the robot. If you take the derivative of this function with respect to time, you get how fast the robot is rotating. One of my biggest challenges was keeping this function continuous, so that the path doesn't change from a sharp left turn to a sharp right instantly (think of an "S" where the top and bottom sections are half-circles). From looking at your paths, it seems that your curves do not have these issues. How did you solve this?

The overall approach to solving motion planning is different, and by nature, the method of finding the optimal path does not need to be told a pre-determined heading for each point. The heading at each point is determined by where the previous and next point lie, in order to globally create a smooth path.

Originally Posted by Jared

I don't follow your code for generating the velocity profile very well, but I don't know how it would deal with sharp turns. From my tests, I needed to create a "maximum velocity" function for my path which told me what the fastest possible speed the robot could go at any point. I took into account the centripetal acceleration required to turn a 150 lb robot, the actual maximum speed of the robot, and the maximum speed of the wheel on the outside of the turn. Then, I generated a smooth velocity function that was always less than the maximum velocity function. It seems like you do have ways to correct your smooth velocity to be within limits, but I don't see how it works.

The velocity for each path is determined by the derivative of the smooth path. However, it is then modified.The final velocity path is also smoothed by the same algorithm the original waypoint path undergoes, each while making sure that the over all distance traveled is equal to the original positional displacement. I also inject a 0 initial velocity and final velocity, then recalculate the velocity to make up the positional loss. Simply taking the derivative of position without fixing it, will yield an assumption which assumes instantaneous acceleration, which for our robots in unachievable. For this reason you can see velocity is always 0 at the beginning and end of each path, and there is always a slow start and stop on the velocity profile, allowing the robot to accelerate and decelerate as needed. If you wish for the transitions to be slower/faster, or more/less smooth, the smoothing parameters can be modified. See the readme on github, but most users won't need to change this.

It is up to the user to determine if the robot can sustain the max velocoity produced by the path, or if the speed controller on the robot can keep up with the velocity transitions (most important), The velocity can be reduced by increasing the time requirement to the calculate method. I did this on purpose, because it is important to understand when the path will end for autonomous routines, so by forcing the user to specify that parameter, they too will be able to see that they may or may not be able to achieve what they want in auto because the velocity profile is too high.

Originally Posted by faust1706

Could either of you explain your mathematics?

The difference is that you are trying to solve for a continuously smooth path. Which requires a lot of computation. The path algorithm I supplied uses the ideas behind integration to generate a smooth path from smaller straight lines. Each line has a deterministic slope for the entire line duration, You can calculate left and right paths based on a little trig and determining the point +/- 90 degrees from your original point and slope. You do not need thousands of points to travel a few feet in a match. So you can safely reduce the number of points needed to speed up computation time.

Please take a look at the readme entirely, it should answer a lot of your questions. I also left my pseudo code in each algorithm to help others understand the theory better. I will try to put together some slides on how it works, but in the mean time, try it out and let me know how its working for you. I may be out of touch for a while, We have an off-season competition next sat, we are also a 2015 alpha/beta test team, and we also have a bunch of off season work we are doing.

Regards,
Kevin

[faust1706]

Originally Posted by Jared

I have a big list of about 3,000 or so points that I iterate through.

You probably already know this (and implemented it) but it doesn't hurt to say this for those reading along. (If you take a data science class, you'll learn how to deal with massive amounts of data efficiently.) Anyways, say you have 2xn matrix of points. Instead of stepping through each point in the matrix, that is, line by line, make it a linear algebra problem. Compute on the entire matrix instead of one row at a time. It greatly reduces runtime.

Kevin, +1 for using gradient descent. I love that algorithm. Noob question: why not use the normal equation: (XX')^-1 X'y to solve for you thetas?

I am too lazy to add in your quote the clean way, but...

"The result is a bunch of straight lines, which appears smooth globally, and has smooth transitions."

I don't think I understand how mathematically this produces a smooth transition. I guess this doesn't matter if your outputted paths are smooth. That's a really clever approach to the problem.

You mentioned this was part of you PhD thesis (first of all, that's really impressive. Some of my lab mates are working on theirs and it looks incredibly stressful). For your project of "developing a controller for an Autonomous Car," are you given a path already? I'm curious because I wrote a pathfinding algorithm (really just an adaption of a*) over the summer and I'm taking the path generated from that and putting it into my program that generates velocity profiles of the left and right side for the robot. I am just wondering if you're doing a similar thing.

"If the algorithm does not converge, the program will simply never finish, so it is pretty easy to identify." Luckily for an autonomous period, everything will be calculated long before the match starts, unless you develop a new path moments before the match, so you'll have ample time investigate why it fails to converge.

[Ether]

Accuracy and Stability of Numerical Algorithms
Second Edition
Nicholas J. Higham

QA297 .H53 2002

[faust1706]

I am no expert in data science, or computational mathematics, it was an amateur question. Since his program is iterative, there does exist a point when taking the inverse of a matrix is faster than n interations, I believe.

[GuyM142]

Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?

If so, what is the best way to use PID for velocity control?

[artK]

Kevin, can you talk a little more about the convergence of Alpha and Beta combinations? How did you determine whether or not certain parameters converge? And would convergence be related with the original path (Like would a path with an right or acute angle between segments have different convergence properties than a closer to straight path)?

[Ether]

Originally Posted by faust1706

Since his program is iterative, there does exist a point when taking the inverse of a matrix is faster than n interations, I believe.

Please provide a more detailed explanation what you mean. I'd rather not guess.

[Jared Russell]

Originally Posted by GuyM142

Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?

If so, what is the best way to use PID for velocity control?

As Art said there are many ways to implement this. The easiest velocity loops (IMO) heavily utilize feedforward terms.

// For each side of the drive...
while (following) {
  // desired_position, desired_velocity, and desired_acceleration are all generated by your profile.
  position_error =  desired_position[i] - actual_position;
  command[i] = Kv * desired_velocity[i] + Ka * desired_acceleration[i] + Kp * position_error;
}

You will find that it is very easy to tune with the following procedure. Before doing this, it is REALLY helpful to have graphs of position vs. time and/or velocity vs. time in Smart Dashboard (or an equivalent plotting tool).

1) Generate an impulse response. From a stop, slam the sticks forward until you max out at your full robot speed. You may require a long distance to get up to speed, so plan accordingly. Stop the robot once it hits full speed (...or the back wall of your shop).

2) You can now analyze your charts to obtain a couple useful quantities:
max_speed: The maximum speed (maximum slope) of the position vs. time plot.
accel_time: How long from when you started until you hit the maximum speed (it is a bit of a judgement call since the curve is smooth, but try to get it in the right ballpark). max_accel ~= max_speed / accel_time

4) Set Kp = Ka = 0, and Kv = 1/max_speed.

5) Generate a motion profile with a maximum velocity of about 80% of your max speed and a maximum acceleration of about 80% of your max acceleration. The 80% is to be conservative and robust to batteries, etc.

6) Attempt to follow the profile with the gains from step 4. Watch your plots vs. the profile (yep...also plot the commanded positions/velocities). It is likely that you lag the profile initially, and lead it (or overshoot) at the end due to the robot's inertia.

7) Next, we will tune Ka to have the robot compensate for this lag/lead. The idea is that when you are accelerating heavily, you can add or subtract from your command to compensate for the robot's inertia. Tune Ka by adjusting and repeating the experiment in 6 until you are following the profile as close as possible. The right value of Ka will be between 0 and 1/max_accel. 1/(2*max_accel) is a reasonable first guess.

8) At this point, you should be following profiles reasonably well, but it is not perfectly repeatable, probably doesn't drive straight, etc. Let's add some feedback. The units of Kp are in (% of power per unit of error). If you have tuned Kv and Ka pretty well, you can get away with a Kp on the order of 1 or 2 if your units are meters (1/3 to 2/3 if using feet, etc).

9) You should follow the trajectories really, really well now. They should be somewhat straighter and quite repeatable.

At this point, things are probably working decently well. Here are a few more options if you want to keep tweaking:

1) Add an integral term based on the sum of position error to get even more precision in final distance. We found this totally unnecessary last year, but YMMV.

2) Add a gyro to help stay straight. Each side of the drive is totally independent, so you can add a simple gyro controller that adds or subtracts from the velocity command being fed to each in order to compensate for accumulated errors.

3) Add trajectory replanning so that if you get bumped to the side, you instantly regenerate a new trajectory to smoothly get you back on track. If necessary for our strategy, 254 might do this in 2015

4) Better characterize your drive train. The calculated left and right velocities are actually assuming a two wheeled vehicle with no slip...4, 6, and 8 wheel drives always have some scrub that results in the vehicle turning less than commanded. You can attempt to measure this and adjust your trajectories accordingly.

[yash101]

This software is quite similar to what I have been planning to implement. I think I will try to decompose this code and rewrite it so I can learn how it works. I have been working on a way to generate a path on the field. The problem with my approach is that no algorithm is perfect. Even though I am using greedy's algorithm because it seems like it will work perfectly for an FRC setup, it every once in a while will generate some sudden turn, often for no purpose. This type of algorithm would help smoothen it out so It can be transformed into motor powers to control the robot.

Kevin,
Thank you for sharing this with me as now I have an almost identical piece of software that works, so I can have an example to learn off of.

[NotInControl]

Originally Posted by faust1706

Kevin, +1 for using gradient descent. I love that algorithm. Noob question: why not use the normal equation: (XX')^-1 X'y to solve for you thetas?

The algorithm is modified for the purpose of what we are trying to do here. Its actually a combination of 2 separate gradient functions playing tug of war against each other, one pushing each point and the other pulling each point. Alpha and Beta control who is "stronger" in the tug. One pulling the point away from its origin point, the other pushing it back. A happy medium generates smooth transitions. If we didn't modify the approach and used a single gradient method to try to minimize the distance between all points, the solution will always be a straight line from the first node to the last node. You can achieve this result, by making Alpha zero, which essentially takes one half of the algorithm out of the tug of war and thus with only Beta active, will pull the solution to a straight line. This doesn't work for us. Another reason why I avoid the format you posted, is based on my own best practice that I try to live by, is avoid having to calculate the inverse of a variable because can lead to problems dealing with singularizes and crashes when you do not control the dataset, as in most algorithm cases.

Originally Posted by faust1706

You mentioned this was part of you PhD thesis (first of all, that's really impressive. Some of my lab mates are working on theirs and it looks incredibly stressful). For your project of "developing a controller for an Autonomous Car," are you given a path already? I'm curious because I wrote a pathfinding algorithm (really just an adaption of a*) over the summer and I'm taking the path generated from that and putting it into my program that generates velocity profiles of the left and right side for the robot. I am just wondering if you're doing a similar thing.

We have no apriori knowledge other than map definition data (gps location of streets, 1-way, or 2-way, sidewalks, stores, etc) and a destination. We also have speed limit definitions for each roads.

Originally Posted by faust1706

"If the algorithm does not converge, the program will simply never finish, so it is pretty easy to identify." Luckily for an autonomous period, everything will be calculated long before the match starts, unless you develop a new path moments before the match, so you'll have ample time investigate why it fails to converge.

This is true for if you only plan to calculate the path once before the match. The reason I made that statement is subtle, but it alludes to a teams desire to calculate new paths in real-time. This would be useful for example if you got nudged in a match, and wanted to course correct. The primary reason for this approach to the problem was to support real-time or near real-time path re-planning on an embedded device. However, this is where you need to pay very close attention to the parameters, and do a lot of testing for convergence and other problems. If I have a chance, I will post some test results, I can't post anything related to my PhD study, because that is proprietary.

Originally Posted by artK

Kevin, can you talk a little more about the convergence of Alpha and Beta combinations? How did you determine whether or not certain parameters converge? And would convergence be related with the original path (Like would a path with an right or acute angle between segments have different convergence properties than a closer to straight path)?

Majority of these values were determined imperially during my previous study. I provide them here, because they still make sense, and are still valid. The default parameters have the best convergence ratio based on the algorithm structure itself. While I can not guarantee convergence for anything, I am fairly confident you will not have issues if you stay within the bounds of the table. It doesn't mean values outside of it won't work for your particular test set. If anyone runs into a convergence issue, I would like to know, because it helps everyone.

Let me think more about your second question. My hunch right now is that valid path data regardless of its actual shape should have no affect on the convergence rate of a set of parameters. But let me think about it some more, and possibly do some calcs on it and get back to you. I would assume we are talking about valid, realistic paths only, and nothing which is intentionally malformed.

Originally Posted by GuyM142

Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?

If so, what is the best way to use PID for velocity control?

Yes that is correct. Jared, posted some good points in the previous post. I have also had a good conversation with Austin from Team 971 on this exact topic. We share how we approach controls on each respective team, and I think a lot of useful information is in that thread. If you have a chance, give it a read. http://www.chiefdelphi.com/forums/sh...d.php?t=129574

Originally Posted by yash101

Kevin,
Thank you for sharing this with me as now I have an almost identical piece of software that works, so I can have an example to learn off of.

No problem, let me know how it works for you. I wish I could release this under a beerware license, but this is a HS community lol.

Regards,
Kevin

[faust1706]

Sorry to revive an rather old thread, but since this is a continuation of @notincontrol's work, I felt it needed to be tied with it.

Here is my attempt at converting his code to work with swerve: https://github.com/faust1706/Smooth-Swerve

I based the calculations off Ether's materials, but that doesn't mean I miss typed or something.

How to use: define set of waypoints, x and y, and then a set of angles you want the robot to be facing at each way point.

A known thing that is broken is the output velocity per motor.