diff --git a/notes/PathFollowing.md b/notes/PathFollowing.md index d62bfa5..9ae2697 100644 --- a/notes/PathFollowing.md +++ b/notes/PathFollowing.md @@ -247,18 +247,55 @@ Trajectory> 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 +trajectoryFromSplineWaypoints(final List waypoints, double maxDx, + double maxDy, double maxDTheta) +{ + List 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) \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/spline/QuinticHermiteSpline.java b/src/main/java/com/spartronics4915/lib/spline/QuinticHermiteSpline.java index c8b738e..8853936 100755 --- a/src/main/java/com/spartronics4915/lib/spline/QuinticHermiteSpline.java +++ b/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,13 +189,13 @@ 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; } @@ -191,12 +203,12 @@ private double sumDCurvature2() /** * @return integral of dCurvature^2 over the length of multiple splines */ - public static double sumDCurvature2(List splines) + public static double sumDCurvatureSq(List 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 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 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 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 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 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 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) {