Onboard path fitting

[qsctr]

Path fitting through waypoints at runtime in robot code. This is useful if we want to adjust the waypoints through vision/lidar inputs as we run the autonomous. Probably will be written in Java along with the rest of the robot code.

[qsctr]

See MATLAB directory for updated MATLAB code.

[qsctr]

Currently using SplineInterpolator from Apache Commons Math. It says that it is a natural cubic spline interpolation.

I tested the spline algorithm on some sample data and compared it to the output of spline in Octave, but the points seem to be off very slightly (average of about 2%).

[qsctr]

cc @bayntun @Kof223

[qsctr]

Since we do not need equal time intervals now, the code can be made simpler.

• [x] Find straight-line distances between waypoints
• [x] Use spline to interpolate x and y values using the approximate distances as parameter
• [x] Calculate angle (including unwrapping)
• [x] Find actual (or close to actual) distance at each point by using linear chordal lengths across the spline fit
• [x] Find velocity at each point from distance and constant acceleration
• [x] Find time at each point (more complicated)

[qsctr]

Calculations are now done, I just need to clean up the code and get it to output the correct things.

[qsctr]

Code is done, now we are testing it on the robot.

Motion profile executes the path successfully when the time duration is not given. However, the robot motion becomes inconsistent with non-zero time duration.

[qsctr]

Future improvements:

• allow custom (non-zero) initial (and maybe final) velocity
• use a spline that will let us specify derivative (angle) at endpoints

[Kof223]

Findings so far: 0 works the same way as before. Difference is slightly faster.

Currently trying to increase the time duration until it doesn't work.

[qsctr]

What do you mean by difference is slightly faster

[Gediminas19]

I will be testing Cubic Hermite Spline using Apache Commons Math's HermiteInterpolator. This is useful in some circumstances in that instead of providing 4 waypoints, you can provide 2 waypoints (endpoints essentially) and provide the first derivative desired at the 2 endpoints (essentially the angle). See this Desmos thing I made for reference.

[Gediminas19]

Hermite spline test successful! How should I upload the code?

[qsctr]

Can you commit to the onboard-pathing branch, swapping out SplineInterpolator for HermiteInterpolator? For the angles you can just take them as parameters to the method.

[Gediminas19]

The way I have it written you can choose between HermiteInterpolator and SplineInterpolator with a boolean flag.

Should I keep only HermiteInterpolator?

[qsctr]

Yes, unless you can think of a good reason to keep the old spline around.

[Gediminas19]

I see, Hermite is probably better indeed. Also, an updated Desmos that shows x and y being splined separately.

[qsctr]

Average execution time of the generatePath method is 48.2 milliseconds.

[qsctr]

Moved hermite spline improvements to #24.

[Gediminas19]

Should a new issue/branch be made specifically to integrate MotionProfile code into the repo?

[qsctr]

Yes

[qsctr]

Ok, created #28