Currently, I'm working on implementing something similar to this while trying to reduce computing time and I had a few more questions about how yours worked. My implementation has a graphical interface for dragging points on a spline to see how it moves and constraining these points as well as a path player that lets you play through the path in real time. I started working on this a while ago, before you released the software, after talking to someone (it might have been Jared Russell?) in St. Louis about how the software worked, but now that yours is out, I've started work on it again.
If you're interested, you can check out the project here.
https://github.com/dicarlo236/PathIn...nterpolate/src The cool parts are in Spline, Robot, SplineEditor, and Path. If it turns out that it generates good paths, I'm planning to post a paper explaining it later.
I originally started with the parametric x(t), y(t) splines, using two quintic hermite splines generated from basis functions here
https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf (page 4). One spline was x(t), and the other y(t). I knew the desired dy/dx, so I made up values for dy/dt and dx/dt that were proportional to the desired dy/dx. The issue was that there were many solutions for dy/dt and dx/dt given a known dy/dx, and using values for dy/dt and dx/dt that were too big or too small resulted in really tight curves.
My solution was to use two short nonparametric quintic hermite splines at the beginning and end of the path and fill the middle with up to 6 cubic Bezier splines. As you guys found, you must use a higher order polynomial to be able to specify first derivative, second derivative, and position. Instead, I specified position and second derivative, which is zero at the ends of the cubic portion and equal at the knots, and let the first derivative be whatever it happened to be. This way, there's no fiddling with heading to generate a reasonable path.
The other thing I've worked on is increasing speed. I'm kind of hoping the roboRIO can do this fast enough to make updates, or to automate a short but complex driving maneuver, like our turn to line up to climb in 2013. I've gotten the calculation for a 50 ft long path down to under 200ms on my laptop (i5 @ 2.6GHz), but going from an ArrayList of pathPoints to a String for exporting takes over a second. I've also worked on simplifying the math on the splines, but have been mostly unsuccessful. I've found it a little faster, and still very accurate, to spit out 25,000 points and calculate the distance between each one, then add them up.
My other thought for speed increase is in reducing resolution. Right now, my "low-res" spline of 2,500 points updates fast enough to drag and drop individual points with a mouse and have it update very fast. Adding more points slows it down quickly. I'd like to eventually do some tests to see how small I can make the data while still generating valid paths.
Now for my questions- how does your acceleration/velocity/jerk calculation work? I've read through it a bunch of times, and I still have no idea. You have something called impulse, but I'm not sure what it is.
My current solution is way more complicated than it needs to be. Basically, it starts with the spline data (uneven spacing in distance and time, time is not known yet), and assigns max velocity to each point. Next, it calculates the radius of curvature at each point, and limits the velocity using the given max acceleration. Then, it starts at the end, sets that point to zero, then backs up, setting each one's velocity based on vf^2 = v0^2 + 2a*dx until it gets to the point where it's equal to the "unadjusted" velocity. Finally, it starts at the beginning calculating acceleration between each point given the distance between and the change in velocity, limiting the acceleration and adjusting the velocity based on the previous velocity, the distance between points, and the maximum acceleration. Interestingly enough, the jerk (da/dt) limit can be implemented by limiting dv/dx, where x is position instead. When the robot is going slow, it covers ground slowly, so the change in velocity, which is proportional to it's change in distance, happens slowly with respect to time. It the robot's going fast, the dx is being covered really fast, so the dv can happen quickly too. Next, it goes back and calculates the change in time between each point based off of the change in velocity and the acceleration at that point. At the end, it splits the trajectory into even time spaced segments, interpolating between data points where dt > 0.01.
This method has a flaw- deceleration while entering turns. If the robot's going really fast, then suddenly gets to a turn, it won't be able to slow down in time because of the acceleration limits. Right now, I either choose between ignoring the max speed through the curve or ignoring max deceleration, while still having acceleration limits coming out of the curve.
Looking through your paths, you ignored max deceleration, with accelerations less than -60, but the robot still behaved perfectly. Was this present in your final paths? If I wanted to implement a system which "looked ahead" to make sure it didn't get going too fast to be able to slow down in time(ie have a smooth function for velocity with a maximum value for dv/dt that was always below another function), how would I do this? Is this what yours does?