Smooth Path Generator for RoboRio 2015

[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