Compute spline derivatives as a function of distance
We want to do all our math as a function of the distance along the path,
not the bezier spline parameter. So, take all the derivatives
accordingly. This also requires us to be able to compute the path
length accurately and convert from path length to the spline parameter.
Next step is to start planning the max velocity we can follow the spline
at to adhere to our constraints.
Change-Id: I7c6d9ddd2ce92aa22e334eaa02594b91ac11066a
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index 2f8c836..665ab3c 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -2,19 +2,25 @@
from __future__ import print_function
-import numpy
-import sys
from matplotlib import pylab
-import glog
import gflags
+import glog
+import numpy
+import scipy
+import scipy.integrate
+import sys
-"""This file is my playground for implementing spline following."""
+"""This file is my playground for implementing spline following.
+
+All splines here are cubic bezier splines. See
+ https://en.wikipedia.org/wiki/B%C3%A9zier_curve for more details.
+"""
FLAGS = gflags.FLAGS
def spline(alpha, control_points):
- """Computes a Bezier curve.
+ """Computes a Cubic Bezier curve.
Args:
alpha: scalar or list of spline parameters to calculate the curve at.
@@ -34,7 +40,7 @@
def dspline(alpha, control_points):
- """Computes the derivitive of a Bezier curve wrt alpha.
+ """Computes the derivative of a Cubic Bezier curve wrt alpha.
Args:
alpha: scalar or list of spline parameters to calculate the curve at.
@@ -56,7 +62,7 @@
def ddspline(alpha, control_points):
- """Computes the second derivitive of a Bezier curve wrt alpha.
+ """Computes the second derivative of a Cubic Bezier curve wrt alpha.
Args:
alpha: scalar or list of spline parameters to calculate the curve at.
@@ -80,7 +86,7 @@
def dddspline(alpha, control_points):
- """Computes the third derivitive of a Bezier curve wrt alpha.
+ """Computes the third derivative of a Cubic Bezier curve wrt alpha.
Args:
alpha: scalar or list of spline parameters to calculate the curve at.
@@ -104,7 +110,7 @@
def spline_theta(alpha, control_points, dspline_points=None):
- """Computes the heading of a robot following a Bezier curve at alpha.
+ """Computes the heading of a robot following a Cubic Bezier curve at alpha.
Args:
alpha: scalar or list of spline parameters to calculate the heading at.
@@ -115,19 +121,19 @@
m array of spline point headings. m is the number of points in 'alpha'.
"""
if dspline_points is None:
- dspline_points = dspline(alphas, control_points)
+ dspline_points = dspline(alpha, control_points)
return numpy.arctan2(
numpy.array(dspline_points)[1, :], numpy.array(dspline_points)[0, :])
-def dspline_theta(alphas,
+def dspline_theta(alpha,
control_points,
dspline_points=None,
ddspline_points=None):
- """Computes the derivitive of the heading at alpha.
+ """Computes the derivative of the heading at alpha.
- This is the derivitive of spline_theta wrt alpha.
+ This is the derivative of spline_theta wrt alpha.
Args:
alpha: scalar or list of spline parameters to calculate the derivative
@@ -140,10 +146,10 @@
in 'alpha'.
"""
if dspline_points is None:
- dspline_points = dspline(alphas, control_points)
+ dspline_points = dspline(alpha, control_points)
if ddspline_points is None:
- ddspline_points = ddspline(alphas, control_points)
+ ddspline_points = ddspline(alpha, control_points)
dx = numpy.array(dspline_points)[0, :]
dy = numpy.array(dspline_points)[1, :]
@@ -154,14 +160,14 @@
return 1.0 / (dx**2.0 + dy**2.0) * (dx * ddy - dy * ddx)
-def ddspline_theta(alphas,
+def ddspline_theta(alpha,
control_points,
dspline_points=None,
ddspline_points=None,
dddspline_points=None):
- """Computes the second derivitive of the heading at alpha.
+ """Computes the second derivative of the heading at alpha.
- This is the second derivitive of spline_theta wrt alpha.
+ This is the second derivative of spline_theta wrt alpha.
Args:
alpha: scalar or list of spline parameters to calculate the second
@@ -174,15 +180,15 @@
points in 'alpha'.
"""
if dspline_points is None:
- dspline_points = dspline(alphas, control_points)
+ dspline_points = dspline(alpha, control_points)
if ddspline_points is None:
- ddspline_points = ddspline(alphas, control_points)
+ ddspline_points = ddspline(alpha, control_points)
if dddspline_points is None:
- dddspline_points = dddspline(alphas, control_points)
+ dddspline_points = dddspline(alpha, control_points)
- dddspline_points = dddspline(alphas, control_points)
+ dddspline_points = dddspline(alpha, control_points)
dx = numpy.array(dspline_points)[0, :]
dy = numpy.array(dspline_points)[1, :]
@@ -198,6 +204,142 @@
dddx)
+class Path(object):
+ """Represents a path to follow."""
+ def __init__(self, control_points):
+ """Constructs a path given the control points."""
+ self._control_points = control_points
+
+ def spline_velocity(alpha):
+ return numpy.linalg.norm(dspline(alpha, self._control_points), axis=0)
+
+ self._point_distances = [0.0]
+ num_alpha = 100
+ # Integrate the xy velocity as a function of alpha for each step in the
+ # table to get an alpha -> distance calculation. Gaussian Quadrature
+ # is quite accurate, so we can get away with fewer points here than we
+ # might think.
+ for alpha in numpy.linspace(0.0, 1.0, num_alpha)[:-1]:
+ self._point_distances.append(
+ scipy.integrate.fixed_quad(spline_velocity, alpha, alpha + 1.0
+ / (num_alpha - 1.0))[0] +
+ self._point_distances[-1])
+
+ def distance_to_alpha(self, distance):
+ """Converts distances along the spline to alphas.
+
+ Args:
+ distance: A scalar or array of distances to convert
+
+ Returns:
+ An array of distances, (1 big if the input was a scalar)
+ """
+ if numpy.isscalar(distance):
+ return numpy.array([self._distance_to_alpha_scalar(distance)])
+ else:
+ return numpy.array([self._distance_to_alpha_scalar(d) for d in distance])
+
+ def _distance_to_alpha_scalar(self, distance):
+ """Helper to compute alpha for a distance for a single scalar."""
+ if distance <= 0.0:
+ return 0.0
+ elif distance >= self.length():
+ return 1.0
+ after_index = numpy.searchsorted(
+ self._point_distances, distance, side='right')
+ before_index = after_index - 1
+
+ # Linearly interpolate alpha from our (sorted) distance table.
+ return (distance - self._point_distances[before_index]) / (
+ self._point_distances[after_index] -
+ self._point_distances[before_index]) * (1.0 / (
+ len(self._point_distances) - 1.0)) + float(before_index) / (
+ len(self._point_distances) - 1.0)
+
+ def length(self):
+ """Returns the length of the spline (in meters)"""
+ return self._point_distances[-1]
+
+ # TODO(austin): need a better name...
+ def xy(self, distance):
+ """Returns the xy position as a function of distance."""
+ return spline(self.distance_to_alpha(distance), self._control_points)
+
+ # TODO(austin): need a better name...
+ def dxy(self, distance):
+ """Returns the xy velocity as a function of distance."""
+ dspline_point = dspline(
+ self.distance_to_alpha(distance), self._control_points)
+ return dspline_point / numpy.linalg.norm(dspline_point, axis=0)
+
+ # TODO(austin): need a better name...
+ def ddxy(self, distance):
+ """Returns the xy acceleration as a function of distance."""
+ alpha = self.distance_to_alpha(distance)
+ dspline_points = dspline(alpha, self._control_points)
+ ddspline_points = ddspline(alpha, self._control_points)
+
+ norm = numpy.linalg.norm(
+ dspline_points, axis=0)**2.0
+
+ return ddspline_points / norm - numpy.multiply(
+ dspline_points, (numpy.array(dspline_points)[0, :] *
+ numpy.array(ddspline_points)[0, :] +
+ numpy.array(dspline_points)[1, :] *
+ numpy.array(ddspline_points)[1, :]) / (norm**2.0))
+
+ def theta(self, distance, dspline_points=None):
+ """Returns the heading as a function of distance."""
+ return spline_theta(
+ self.distance_to_alpha(distance),
+ self._control_points,
+ dspline_points=dspline_points)
+
+ def dtheta(self, distance, dspline_points=None, ddspline_points=None):
+ """Returns the angular velocity as a function of distance."""
+ alpha = self.distance_to_alpha(distance)
+ if dspline_points is None:
+ dspline_points = dspline(alpha, self._control_points)
+ if ddspline_points is None:
+ ddspline_points = ddspline(alpha, self._control_points)
+
+ dtheta_points = dspline_theta(alpha, self._control_points,
+ dspline_points, ddspline_points)
+
+ return dtheta_points / numpy.linalg.norm(dspline_points, axis=0)
+
+ def ddtheta(self,
+ distance,
+ dspline_points=None,
+ ddspline_points=None,
+ dddspline_points=None):
+ """Returns the angular acceleration as a function of distance."""
+ alpha = self.distance_to_alpha(distance)
+ if dspline_points is None:
+ dspline_points = dspline(alpha, self._control_points)
+ if ddspline_points is None:
+ ddspline_points = ddspline(alpha, self._control_points)
+ if dddspline_points is None:
+ dddspline_points = dddspline(alpha, self._control_points)
+
+ dtheta_points = dspline_theta(alpha, self._control_points,
+ dspline_points, ddspline_points)
+ ddtheta_points = ddspline_theta(alpha, self._control_points,
+ dspline_points, ddspline_points,
+ dddspline_points)
+
+ # TODO(austin): Factor out the d^alpha/dd^2.
+ return ddtheta_points / numpy.linalg.norm(
+ dspline_points, axis=0)**2.0 - numpy.multiply(
+ dtheta_points, (numpy.array(dspline_points)[0, :] *
+ numpy.array(ddspline_points)[0, :] +
+ numpy.array(dspline_points)[1, :] *
+ numpy.array(ddspline_points)[1, :]) /
+ ((numpy.array(dspline_points)[0, :]**2.0 +
+ numpy.array(dspline_points)[1, :]**2.0)**2.0))
+
+
+
def main(argv):
# Build up the control point matrix
start = numpy.matrix([[0.0, 0.0]]).T
@@ -217,7 +359,8 @@
# Compute theta and the two derivatives
theta = spline_theta(alphas, control_points, dspline_points=dspline_points)
- dtheta = dspline_theta(alphas, control_points, dspline_points=dspline_points)
+ dtheta = dspline_theta(
+ alphas, control_points, dspline_points=dspline_points)
ddtheta = ddspline_theta(
alphas,
control_points,
@@ -307,11 +450,69 @@
pylab.plot(alphas, ddtheta, label='ddtheta')
pylab.plot(alphas, thetaint_plot, label='thetai')
pylab.plot(alphas, dthetaint_plot, label='dthetai')
+ pylab.plot(
+ alphas,
+ numpy.linalg.norm(
+ numpy.array(dspline_points), axis=0),
+ label='velocity')
+
+ # Now, repeat as a function of path length as opposed to alpha
+ path = Path(control_points)
+ distance_count = 1000
+ position = path.xy(0.0)
+ velocity = path.dxy(0.0)
+ theta = path.theta(0.0)
+ omega = path.dtheta(0.0)
+
+ iposition_plot = numpy.matrix(numpy.zeros((2, distance_count)))
+ ivelocity_plot = numpy.matrix(numpy.zeros((2, distance_count)))
+ iposition_plot[:, 0] = position.copy()
+ ivelocity_plot[:, 0] = velocity.copy()
+ itheta_plot = numpy.zeros((distance_count,))
+ iomega_plot = numpy.zeros((distance_count,))
+ itheta_plot[0] = theta
+ iomega_plot[0] = omega
+
+ distances = numpy.linspace(0.0, path.length(), distance_count)
+
+ for i in xrange(len(distances) - 1):
+ position += velocity * (distances[i + 1] - distances[i])
+ velocity += path.ddxy(distances[i]) * (distances[i + 1] - distances[i])
+ iposition_plot[:, i + 1] = position
+ ivelocity_plot[:, i + 1] = velocity
+
+ theta += omega * (distances[i + 1] - distances[i])
+ omega += path.ddtheta(distances[i]) * (distances[i + 1] - distances[i])
+ itheta_plot[i + 1] = theta
+ iomega_plot[i + 1] = omega
+
+ pylab.figure()
+ pylab.plot(distances, numpy.array(path.xy(distances))[0, :], label='x')
+ pylab.plot(distances, numpy.array(iposition_plot)[0, :], label='ix')
+ pylab.plot(distances, numpy.array(path.dxy(distances))[0, :], label='dx')
+ pylab.plot(distances, numpy.array(ivelocity_plot)[0, :], label='idx')
+ pylab.plot(distances, numpy.array(path.ddxy(distances))[0, :], label='ddx')
+ pylab.legend()
+
+ pylab.figure()
+ pylab.plot(distances, numpy.array(path.xy(distances))[1, :], label='y')
+ pylab.plot(distances, numpy.array(iposition_plot)[1, :], label='iy')
+ pylab.plot(distances, numpy.array(path.dxy(distances))[1, :], label='dy')
+ pylab.plot(distances, numpy.array(ivelocity_plot)[1, :], label='idy')
+ pylab.plot(distances, numpy.array(path.ddxy(distances))[1, :], label='ddy')
+ pylab.legend()
+
+ pylab.figure()
+ pylab.plot(distances, path.theta(distances), label='theta')
+ pylab.plot(distances, itheta_plot, label='itheta')
+ pylab.plot(distances, path.dtheta(distances), label='omega')
+ pylab.plot(distances, iomega_plot, label='iomega')
+ pylab.plot(distances, path.ddtheta(distances), label='alpha')
+ pylab.legend()
# TODO(austin): Start creating a velocity plan now that we have all the
# derivitives of our spline.
- pylab.legend()
pylab.show()