ongoing work on docs for pathfollowing. (still more to come)

notes/PathFollowing.md

@@ -247,18 +247,55 @@ Trajectory<TimedState<Pose2dWithCurvature>> generateTrajectory(
             double max_voltage);
 ```
 
-Thus, `MotionPlanner` converts a small set of poses into a larger set of
+Thus, `MotionPlanner` converts a small set of waypoints into a larger set of
 timed poses with augmented curvature information.  In order to generate
 the trajectory, it takes into account the provided list of `TimingContraints`
 and the maximum allowances for velocity, acceleration and voltage.
 
-To accomplish this daunting task it first invokes [this method](#trajectoryfromsplinewaypoints):
+To accomplish this daunting task it first invokes this method:
 
 ```java
-trajectory = TrajectoryUtil.trajectoryFromSplineWaypoints(waypoints,
-                                            kMaxDx, kMaxDy, kMaxDTheta);
+/* from TrajectoryUtil.java */
+public static Trajectory<Pose2dWithCurvature>
+trajectoryFromSplineWaypoints(final List<Pose2d> waypoints, double maxDx,
+                                double maxDy, double maxDTheta)
+{
+    List<QuinticHermiteSpline> splines = new ArrayList<>(waypoints.size() - 1);
+    for (int i = 1; i < waypoints.size(); ++i)
+    {
+        splines.add(new QuinticHermiteSpline(waypoints.get(i - 1), waypoints.get(i)));
+    }
+    QuinticHermiteSpline.optimizeSpline(splines);
+    return trajectoryFromSplines(splines, maxDx, maxDy, maxDTheta);
+}
+```
+
+The first step is to simply allocate our spline segments.  Note N waypoints
+produce N-1 segments. After populating our list of spline segments, each
+segment has our target values for position and orientation as provided
+by the Pose2d waypoints.  We haven't provided acceleration (curvature) data
+so at this point our splines still need some work.  This is the job of
+`optimizeSpline`.  Here's how it describes itself.
+
+```java
+/**
+ * optimizeSpline:
+ *  finds the optimal second derivative values for a set of splines to reduce
+ *  the sum of the change in curvature squared over the path.
+ */
 ```
 
+So this is how it goes about computing acceleration/curvature values.
+The optimization step is interative and implements a form of gradient
+descent to achieve its ends.  After a nontrivial numerical struggle,
+we now assign values for second-derivative terms for our spline.  These
+are still not useable as velocity and acceleration targets since the
+initial poses only include orientation (tangent) information but not
+_speed_.
+
+
+
+
 Note that at this point, we haven't applied TimingConstraints and this
 means that the Trajectory has a list of poses that have been _geometrically
 constrained_ by provided max values, but these poses haven't been assigned
@@ -367,6 +404,7 @@ Our feedforward generation techniques used a dynamic model 2 of a skid-steer dri
 ## References
 
 ### splines
+* [cornell lecture on splines](http://www.cs.cornell.edu/courses/cs4620/2013fa/lectures/16spline-curves.pdf)
 * [cmu lecture slide on splines](http://www.cs.cmu.edu/afs/cs/academic/class/15462-s10/www/lec-slides/lec06.pdf)
 * [rose-hulman quintic hermite notes](https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf)
 * [online cubic hermite editor](https://www.desmos.com/calculator/v8hozhn35m)

src/main/java/com/spartronics4915/lib/spline/QuinticHermiteSpline.java

@@ -147,24 +147,36 @@ public double getVelocity(double t)
         return Math.hypot(dx(t), dy(t));
     }
 
+    /*
+     * curvature formulae from mathworld:
+     *      http://mathworld.wolfram.com/Curvature.html,
+     *
+     * eq 13 - the curvature of two parameteric splines relates their
+     *        respective 1st and 2nd derivs.
+     */
     @Override
     public double getCurvature(double t)
     {
-        return (dx(t) * ddy(t) - ddx(t) * dy(t)) / ((dx(t) * dx(t) + dy(t) * dy(t)) * Math.sqrt((dx(t) * dx(t) + dy(t) * dy(t))));
+        return (dx(t)*ddy(t) - ddx(t)*dy(t)) /
+            ((dx(t)*dx(t) + dy(t)*dy(t)) * Math.sqrt((dx(t)*dx(t) + dy(t)*dy(t))));
     }
 
+    /* this is a magnitude of the derivative of getCurvature.. */
     @Override
     public double getDCurvature(double t)
     {
         double dx2dy2 = (dx(t) * dx(t) + dy(t) * dy(t));
-        double num = (dx(t) * dddy(t) - dddx(t) * dy(t)) * dx2dy2 - 3 * (dx(t) * ddy(t) - ddx(t) * dy(t)) * (dx(t) * ddx(t) + dy(t) * ddy(t));
+        double num = (dx(t)*dddy(t) - dddx(t)*dy(t))*dx2dy2 -
+            3 * (dx(t)*ddy(t) - ddx(t)*dy(t)) * (dx(t)*ddx(t) + dy(t)*ddy(t));
         return num / (dx2dy2 * dx2dy2 * Math.sqrt(dx2dy2));
     }
 
-    private double dCurvature2(double t)
+    /* this is just square(getDCurvature) */
+    private double dCurvatureSq(double t)
     {
         double dx2dy2 = (dx(t) * dx(t) + dy(t) * dy(t));
-        double num = (dx(t) * dddy(t) - dddx(t) * dy(t)) * dx2dy2 - 3 * (dx(t) * ddy(t) - ddx(t) * dy(t)) * (dx(t) * ddx(t) + dy(t) * ddy(t));
+        double num = (dx(t)*dddy(t) - dddx(t)*dy(t)) * dx2dy2 -
+            3 * (dx(t)*ddy(t) - ddx(t)*dy(t)) * (dx(t)*ddx(t) + dy(t)*ddy(t));
         return num * num / (dx2dy2 * dx2dy2 * dx2dy2 * dx2dy2 * dx2dy2);
     }
 
@@ -177,26 +189,26 @@ public Rotation2d getHeading(double t)
     /**
      * @return integral of dCurvature^2 over the length of the spline
      */
-    private double sumDCurvature2()
+    private double sumDCurvatureSq()
     {
         double dt = 1.0 / kSamples;
         double sum = 0;
         for (double t = 0; t < 1.0; t += dt)
         {
-            sum += (dt * dCurvature2(t));
+            sum += (dt * dCurvatureSq(t));
         }
         return sum;
     }
 
     /**
      * @return integral of dCurvature^2 over the length of multiple splines
      */
-    public static double sumDCurvature2(List<QuinticHermiteSpline> splines)
+    public static double sumDCurvatureSq(List<QuinticHermiteSpline> splines)
     {
         double sum = 0;
         for (QuinticHermiteSpline s : splines)
         {
-            sum += s.sumDCurvature2();
+            sum += s.sumDCurvatureSq();
         }
         return sum;
     }
@@ -221,11 +233,11 @@ private static class ControlPoint
     public static double optimizeSpline(List<QuinticHermiteSpline> splines)
     {
         int count = 0;
-        double prev = sumDCurvature2(splines);
+        double prev = sumDCurvatureSq(splines);
         while (count < kMaxIterations)
         {
             runOptimizationIteration(splines);
-            double current = sumDCurvature2(splines);
+            double current = sumDCurvatureSq(splines);
             if (prev - current < kMinDelta)
                 return current;
             prev = current;
@@ -256,7 +268,7 @@ private static void runOptimizationIteration(List<QuinticHermiteSpline> splines)
             {
                 continue;
             }
-            double original = sumDCurvature2(splines);
+            double original = sumDCurvatureSq(splines);
             QuinticHermiteSpline temp, temp1;
 
             temp = splines.get(i);
@@ -268,12 +280,12 @@ private static void runOptimizationIteration(List<QuinticHermiteSpline> splines)
                     kEpsilon, temp.y0, temp.y1, temp.dy0, temp.dy1, temp.ddy0, temp.ddy1));
             splines.set(i + 1, new QuinticHermiteSpline(temp1.x0, temp1.x1, temp1.dx0, temp1.dx1, temp1.ddx0 +
                     kEpsilon, temp1.ddx1, temp1.y0, temp1.y1, temp1.dy0, temp1.dy1, temp1.ddy0, temp1.ddy1));
-            controlPoints[i].ddx = (sumDCurvature2(splines) - original) / kEpsilon;
+            controlPoints[i].ddx = (sumDCurvatureSq(splines) - original) / kEpsilon;
             splines.set(i, new QuinticHermiteSpline(temp.x0, temp.x1, temp.dx0, temp.dx1, temp.ddx0, temp.ddx1, temp.y0, temp.y1, temp.dy0, temp.dy1,
                     temp.ddy0, temp.ddy1 + kEpsilon));
             splines.set(i + 1, new QuinticHermiteSpline(temp1.x0, temp1.x1, temp1.dx0, temp1.dx1, temp1.ddx0,
                     temp1.ddx1, temp1.y0, temp1.y1, temp1.dy0, temp1.dy1, temp1.ddy0 + kEpsilon, temp1.ddy1));
-            controlPoints[i].ddy = (sumDCurvature2(splines) - original) / kEpsilon;
+            controlPoints[i].ddy = (sumDCurvatureSq(splines) - original) / kEpsilon;
 
             splines.set(i, temp);
             splines.set(i + 1, temp1);
@@ -285,7 +297,7 @@ private static void runOptimizationIteration(List<QuinticHermiteSpline> splines)
         //minimize along the direction of the gradient
         //first calculate 3 points along the direction of the gradient
         Translation2d p1, p2, p3;
-        p2 = new Translation2d(0, sumDCurvature2(splines)); //middle point is at the current location
+        p2 = new Translation2d(0, sumDCurvatureSq(splines)); //middle point is at the current location
 
         for (int i = 0; i < splines.size() - 1; ++i)
         { //first point is offset from the middle location by -stepSize
@@ -308,7 +320,7 @@ private static void runOptimizationIteration(List<QuinticHermiteSpline> splines)
             splines.get(i).computeCoefficients();
             splines.get(i + 1).computeCoefficients();
         }
-        p1 = new Translation2d(-kStepSize, sumDCurvature2(splines));
+        p1 = new Translation2d(-kStepSize, sumDCurvatureSq(splines));
 
         for (int i = 0; i < splines.size() - 1; ++i)
         { //last point is offset from the middle location by +stepSize
@@ -329,9 +341,9 @@ private static void runOptimizationIteration(List<QuinticHermiteSpline> splines)
             splines.get(i + 1).computeCoefficients();
         }
 
-        p3 = new Translation2d(kStepSize, sumDCurvature2(splines));
+        p3 = new Translation2d(kStepSize, sumDCurvatureSq(splines));
 
-        double stepSize = fitParabola(p1, p2, p3); //approximate step size to minimize sumDCurvature2 along the gradient
+        double stepSize = fitParabola(p1, p2, p3); //approximate step size to minimize sumDCurvatureSq along the gradient
 
         for (int i = 0; i < splines.size() - 1; ++i)
         {