Remove the old spline code
We stopped updating it when we wrote the C++.
Change-Id: Ied82faa16ecbade0d1c4e474159d0180f1d21c0a
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 880624e..a893ce1 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -123,22 +123,6 @@
deps = ["//frc971/control_loops:python_init"],
)
-py_binary(
- name = "spline",
- srcs = [
- "spline.py",
- ],
- legacy_create_init = False,
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:controls",
- "//y2016/control_loops/python:polydrivetrain_lib",
- "@matplotlib_repo//:matplotlib3",
- ],
-)
-
py_library(
name = "linear_system",
srcs = ["linear_system.py"],
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
deleted file mode 100644
index 44f9be6..0000000
--- a/frc971/control_loops/python/spline.py
+++ /dev/null
@@ -1,998 +0,0 @@
-#!/usr/bin/python3
-
-from __future__ import print_function
-
-from matplotlib import pylab
-import gflags
-import glog
-import numpy
-import scipy
-import scipy.integrate
-import scipy.optimize
-import sys
-
-from frc971.control_loops.python import polydrivetrain
-from frc971.control_loops.python import drivetrain
-from frc971.control_loops.python import controls
-import y2016.control_loops.python.drivetrain
-"""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 RungeKutta(f, y0, t, h, count=1):
- """4th order RungeKutta integration of dy/dt = f(t, y) starting at X."""
- y1 = y0
- dh = h / float(count)
- for x in range(count):
- k1 = dh * f(t + dh * x, y1)
- k2 = dh * f(t + dh * x + dh / 2.0, y1 + k1 / 2.0)
- k3 = dh * f(t + dh * x + dh / 2.0, y1 + k2 / 2.0)
- k4 = dh * f(t + dh * x + dh, y1 + k3)
- y1 += (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0
- return y1
-
-
-def spline(alpha, control_points):
- """Computes a Cubic Bezier curve.
-
- Args:
- alpha: scalar or list of spline parameters to calculate the curve at.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- n x m matrix of spline points. n is the dimension of the control
- points, and m is the number of points in 'alpha'.
- """
- if numpy.isscalar(alpha):
- alpha = [alpha]
- alpha_matrix = [[(1.0 - a)**3.0, 3.0 * (1.0 - a)**2.0 * a,
- 3.0 * (1.0 - a) * a**2.0, a**3.0] for a in alpha]
-
- return control_points * numpy.matrix(alpha_matrix).T
-
-
-def dspline(alpha, control_points):
- """Computes the derivative of a Cubic Bezier curve wrt alpha.
-
- Args:
- alpha: scalar or list of spline parameters to calculate the curve at.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- n x m matrix of spline point derivatives. n is the dimension of the
- control points, and m is the number of points in 'alpha'.
- """
- if numpy.isscalar(alpha):
- alpha = [alpha]
- dalpha_matrix = [[
- -3.0 * (1.0 - a)**2.0,
- 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 * (1.0 - a) * a,
- -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
- ] for a in alpha]
-
- return control_points * numpy.matrix(dalpha_matrix).T
-
-
-def ddspline(alpha, control_points):
- """Computes the second derivative of a Cubic Bezier curve wrt alpha.
-
- Args:
- alpha: scalar or list of spline parameters to calculate the curve at.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- n x m matrix of spline point second derivatives. n is the dimension of
- the control points, and m is the number of points in 'alpha'.
- """
- if numpy.isscalar(alpha):
- alpha = [alpha]
- ddalpha_matrix = [[
- 2.0 * 3.0 * (1.0 - a),
- -2.0 * 3.0 * (1.0 - a) + -2.0 * 3.0 * (1.0 - a) + 2.0 * 3.0 * a,
- -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a, 2.0 * 3.0 * a
- ] for a in alpha]
-
- return control_points * numpy.matrix(ddalpha_matrix).T
-
-
-def dddspline(alpha, control_points):
- """Computes the third derivative of a Cubic Bezier curve wrt alpha.
-
- Args:
- alpha: scalar or list of spline parameters to calculate the curve at.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- n x m matrix of spline point second derivatives. n is the dimension of
- the control points, and m is the number of points in 'alpha'.
- """
- if numpy.isscalar(alpha):
- alpha = [alpha]
- ddalpha_matrix = [[
- -2.0 * 3.0, 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
- -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0
- ] for a in alpha]
-
- return control_points * numpy.matrix(ddalpha_matrix).T
-
-
-def spline_theta(alpha, control_points, dspline_points=None):
- """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.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- m array of spline point headings. m is the number of points in 'alpha'.
- """
- if dspline_points is None:
- dspline_points = dspline(alpha, control_points)
-
- return numpy.arctan2(
- numpy.array(dspline_points)[1, :],
- numpy.array(dspline_points)[0, :])
-
-
-def dspline_theta(alpha,
- control_points,
- dspline_points=None,
- ddspline_points=None):
- """Computes the derivative of the heading at alpha.
-
- This is the derivative of spline_theta wrt alpha.
-
- Args:
- alpha: scalar or list of spline parameters to calculate the derivative
- of the heading at.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- m array of spline point heading derivatives. m is the number of points
- in 'alpha'.
- """
- if dspline_points is None:
- dspline_points = dspline(alpha, control_points)
-
- if ddspline_points is None:
- ddspline_points = ddspline(alpha, control_points)
-
- dx = numpy.array(dspline_points)[0, :]
- dy = numpy.array(dspline_points)[1, :]
-
- ddx = numpy.array(ddspline_points)[0, :]
- ddy = numpy.array(ddspline_points)[1, :]
-
- return 1.0 / (dx**2.0 + dy**2.0) * (dx * ddy - dy * ddx)
-
-
-def ddspline_theta(alpha,
- control_points,
- dspline_points=None,
- ddspline_points=None,
- dddspline_points=None):
- """Computes the second derivative of the heading at alpha.
-
- This is the second derivative of spline_theta wrt alpha.
-
- Args:
- alpha: scalar or list of spline parameters to calculate the second
- derivative of the heading at.
- control_points: n x 4 matrix of control points. n[:, 0] is the
- starting point, and n[:, 3] is the ending point.
-
- Returns:
- m array of spline point heading second derivatives. m is the number of
- points in 'alpha'.
- """
- if dspline_points is None:
- dspline_points = dspline(alpha, control_points)
-
- if ddspline_points is None:
- ddspline_points = ddspline(alpha, control_points)
-
- if dddspline_points is None:
- dddspline_points = dddspline(alpha, control_points)
-
- dddspline_points = dddspline(alpha, control_points)
-
- dx = numpy.array(dspline_points)[0, :]
- dy = numpy.array(dspline_points)[1, :]
-
- ddx = numpy.array(ddspline_points)[0, :]
- ddy = numpy.array(ddspline_points)[1, :]
-
- dddx = numpy.array(dddspline_points)[0, :]
- dddy = numpy.array(dddspline_points)[1, :]
-
- return -1.0 / ((dx**2.0 + dy**2.0)**2.0) * (dx * ddy - dy * ddx) * 2.0 * (
- dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (
- dx * dddy - dy * 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 dtheta_dt(self,
- distance,
- velocity,
- dspline_points=None,
- ddspline_points=None):
- """Returns the angular velocity as a function of time."""
- return self.dtheta(distance, dspline_points,
- ddspline_points) * velocity
-
- 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 integrate_accel_for_distance(f, v, x, dx):
- # Use a trick from
- # https://www.johndcook.com/blog/2012/02/21/care-and-treatment-of-singularities/
- #
- # We want to calculate:
- # v0 + (integral of dv/dt = f(x, v) from x to x + dx); noting that dv/dt
- # is expressed in t, not distance, so we want to do the integral of
- # dv/dx = f(x, v) / v.
- #
- # Because v can be near zero at the start of the integral (but because f is
- # nonnegative, v will never go to zero), but the integral should still be
- # valid, we follow the suggestion and instead calculate
- # v0 + integral((f(x, v) - f(x0, v0)) / v) + integral(f(x0, v0) / v).
- #
- # Using a0 = f(x0, v0), we get the second term as
- # integral((f(x, v) - a0) / v)
- # where when v is zero we will also be at x0/v0 (because v can only start
- # at zero, not go to zero).
- #
- # The second term, integral(a0 / v) requires an approximation.--in
- # this case, that dv/dt is constant. Thus, we have
- # integral(a0 / sqrt(v0^2 + 2*a0*x)) = sqrt(2*a0*dx + v0^2) - sqrt(v0^2)
- # = sqrt(2 * a0 * dx * v0^2) - v0.
- #
- # Because the RungeKutta function returns v0 + the integral, this
- # gives the statements below.
-
- a0 = f(x, v)
-
- def integrablef(t, y):
- # Since we know that a0 == a(0) and that they are asymptotically the
- # same at 0, we know that the limit is 0 at 0. This is true because
- # when starting from a stop, under sane accelerations, we can assume
- # that we will start with a constant acceleration. So, hard-code it.
- if numpy.abs(y) < 1e-6:
- return 0.0
- return (f(t, y) - a0) / y
-
- return (RungeKutta(integrablef, v, x, dx) - v) + numpy.sqrt(2.0 * a0 * dx +
- v * v)
-
-
-class Trajectory(object):
- def __init__(self, path, drivetrain, longitudal_accel, lateral_accel,
- distance_count):
- self._path = path
- self._drivetrain = drivetrain
- self.distances = numpy.linspace(0.0, self._path.length(),
- distance_count)
- self._longitudal_accel = longitudal_accel
- self._lateral_accel = lateral_accel
-
- self._B_inverse = numpy.linalg.inv(self._drivetrain.B_continuous)
-
- def create_plan(self, vmax):
- vmax = 10.0
- plan = numpy.array(numpy.zeros((len(self.distances), )))
- plan.fill(vmax)
- return plan
-
- def lateral_velocity_curvature(self, distance):
- return numpy.sqrt(
- self._lateral_accel / numpy.linalg.norm(self._path.ddxy(distance)))
-
- def lateral_accel_pass(self, plan):
- plan = plan.copy()
- # TODO(austin): This appears to be doing nothing.
- for i, distance in enumerate(self.distances):
- plan[i] = min(plan[i], self.lateral_velocity_curvature(distance))
- return plan
-
- def compute_K345(self, current_dtheta, current_ddtheta):
- # We've now got the equation:
- # K2 * d^2x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- K1 = numpy.matrix(
- [[-self._drivetrain.robot_radius_l * current_ddtheta],
- [self._drivetrain.robot_radius_r * current_ddtheta]])
- K2 = numpy.matrix(
- [[1.0 - self._drivetrain.robot_radius_l * current_dtheta],
- [1.0 + self._drivetrain.robot_radius_r * current_dtheta]])
-
- # Now, rephrase it as K5 a + K3 v^2 + K4 v = U
- K3 = self._B_inverse * K1
- K4 = -self._B_inverse * self._drivetrain.A_continuous * K2
- K5 = self._B_inverse * K2
- return K3, K4, K5
-
- def forward_acceleration(self, x, v):
- current_ddtheta = self._path.ddtheta(x)[0]
- current_dtheta = self._path.dtheta(x)[0]
- # We've now got the equation:
- # K2 * d^2x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- # Now, rephrase it as K5 a + K3 v^2 + K4 v = U
- K3, K4, K5 = self.compute_K345(current_dtheta, current_ddtheta)
-
- C = K3 * v * v + K4 * v
- # Note: K345 are not quite constant over the step, but we are going
- # to assume they are for now.
- accelerations = [
- (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
- (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
- ]
- maxa = -float('inf')
- for a in accelerations:
- U = K5 * a + K3 * v * v + K4 * v
- if not (numpy.abs(U) > 12.0 + 1e-6).any():
- maxa = max(maxa, a)
-
- lateral_accel = v * v * numpy.linalg.norm(self._path.ddxy(x))
- # Constrain the longitudinal acceleration to keep in a pseudo friction
- # circle. This will make it so we don't floor it while in a turn and
- # cause extra wheel slip.
- long_accel = numpy.sqrt(1.0 - (
- lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
- return min(long_accel, maxa)
-
- def forward_pass(self, plan):
- plan = plan.copy()
- for i, distance in enumerate(self.distances):
- if i == len(self.distances) - 1:
- break
-
- plan[i + 1] = min(
- plan[i + 1],
- integrate_accel_for_distance(
- self.forward_acceleration, plan[i], self.distances[i],
- self.distances[i + 1] - self.distances[i]))
- return plan
-
- def backward_acceleration(self, x, v):
- # TODO(austin): Forwards and backwards are quite similar. Can we
- # factor this out?
- current_ddtheta = self._path.ddtheta(x)[0]
- current_dtheta = self._path.dtheta(x)[0]
- # We've now got the equation:
- # K2 * d^2x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- # Now, rephrase it as K5 a + K3 v^2 + K4 v = U
- K3, K4, K5 = self.compute_K345(current_dtheta, current_ddtheta)
-
- C = K3 * v * v + K4 * v
- # Note: K345 are not quite constant over the step, but we are going
- # to assume they are for now.
- accelerations = [
- (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
- (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
- ]
- mina = float('inf')
- for a in accelerations:
- U = K5 * a + K3 * v * v + K4 * v
- if not (numpy.abs(U) > 12.0 + 1e-6).any():
- mina = min(mina, a)
-
- lateral_accel = v * v * numpy.linalg.norm(self._path.ddxy(x))
- # Constrain the longitudinal acceleration to keep in a pseudo friction
- # circle. This will make it so we don't floor it while in a turn and
- # cause extra wheel slip.
- long_accel = -numpy.sqrt(1.0 - (
- lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
- return max(long_accel, mina)
-
- def backward_pass(self, plan):
- plan = plan.copy()
- for i, distance in reversed(list(enumerate(self.distances))):
- if i == 0:
- break
-
- plan[i - 1] = min(
- plan[i - 1],
- integrate_accel_for_distance(
- self.backward_acceleration, plan[i], self.distances[i],
- self.distances[i - 1] - self.distances[i]))
- return plan
-
- # TODO(austin): The plan should probably not be passed in...
- def ff_accel(self, plan, distance):
- if distance < self.distances[1]:
- after_index = 1
- before_index = after_index - 1
- if distance < self.distances[0]:
- distance = 0.0
- elif distance > self.distances[-2]:
- after_index = len(self.distances) - 1
- before_index = after_index - 1
- if distance > self.distances[-1]:
- distance = self.distances[-1]
- else:
- after_index = numpy.searchsorted(
- self.distances, distance, side='right')
- before_index = after_index - 1
-
- vforwards = integrate_accel_for_distance(
- self.forward_acceleration, plan[before_index],
- self.distances[before_index],
- distance - self.distances[before_index])
- vbackward = integrate_accel_for_distance(
- self.backward_acceleration, plan[after_index],
- self.distances[after_index],
- distance - self.distances[after_index])
-
- vcurvature = self.lateral_velocity_curvature(distance)
-
- if vcurvature < vforwards and vcurvature < vbackward:
- accel = 0
- velocity = vcurvature
- elif vforwards < vbackward:
- velocity = vforwards
- accel = self.forward_acceleration(distance, velocity)
- else:
- velocity = vbackward
- accel = self.backward_acceleration(distance, velocity)
- return (distance, velocity, accel)
-
- def ff_voltage(self, plan, distance):
- _, velocity, accel = self.ff_accel(plan, distance)
- current_ddtheta = self._path.ddtheta(distance)[0]
- current_dtheta = self._path.dtheta(distance)[0]
- # TODO(austin): Factor these out.
- # We've now got the equation:
- # K2 * d^2x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- # Now, rephrase it as K5 a + K3 v^2 + K4 v = U
- K3, K4, K5 = self.compute_K345(current_dtheta, current_ddtheta)
-
- U = K5 * accel + K3 * velocity * velocity + K4 * velocity
- return U
-
- def goal_state(self, distance, velocity):
- width = (
- self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
- goal = numpy.matrix(numpy.zeros((5, 1)))
-
- goal[0:2, :] = self._path.xy(distance)
- goal[2, :] = self._path.theta(distance)
-
- Ter = numpy.linalg.inv(
- numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
- goal[3:5, :] = Ter * numpy.matrix(
- [[velocity], [self._path.dtheta_dt(distance, velocity)]])
- return goal
-
-
-def main(argv):
- # Build up the control point matrix
- start = numpy.matrix([[0.0, 0.0]]).T
- c1 = numpy.matrix([[0.5, 0.0]]).T
- c2 = numpy.matrix([[0.5, 1.0]]).T
- end = numpy.matrix([[1.0, 1.0]]).T
- control_points = numpy.hstack((start, c1, c2, end))
-
- # The alphas to plot
- alphas = numpy.linspace(0.0, 1.0, 1000)
-
- # Compute x, y and the 3 derivatives
- spline_points = spline(alphas, control_points)
- dspline_points = dspline(alphas, control_points)
- ddspline_points = ddspline(alphas, control_points)
- dddspline_points = dddspline(alphas, control_points)
-
- # 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)
- ddtheta = ddspline_theta(
- alphas,
- control_points,
- dspline_points=dspline_points,
- dddspline_points=dddspline_points)
-
- # Plot the control points and the spline.
- pylab.figure()
- pylab.plot(
- numpy.array(control_points)[0, :],
- numpy.array(control_points)[1, :],
- '-o',
- label='control')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
- pylab.legend()
-
- # For grins, confirm that the double integral of the acceleration (with
- # respect to the spline parameter) matches the position. This lets us
- # confirm that the derivatives are consistent.
- xint_plot = numpy.matrix(numpy.zeros((2, len(alphas))))
- dxint_plot = xint_plot.copy()
- xint = spline_points[:, 0].copy()
- dxint = dspline_points[:, 0].copy()
- xint_plot[:, 0] = xint
- dxint_plot[:, 0] = dxint
- for i in range(len(alphas) - 1):
- xint += (alphas[i + 1] - alphas[i]) * dxint
- dxint += (alphas[i + 1] - alphas[i]) * ddspline_points[:, i]
- xint_plot[:, i + 1] = xint
- dxint_plot[:, i + 1] = dxint
-
- # Integrate up the spline velocity and heading to confirm that given a
- # velocity (as a function of the spline parameter) and angle, we will move
- # from the starting point to the ending point.
- thetaint_plot = numpy.zeros((len(alphas), ))
- thetaint = theta[0]
- dthetaint_plot = numpy.zeros((len(alphas), ))
- dthetaint = dtheta[0]
- thetaint_plot[0] = thetaint
- dthetaint_plot[0] = dthetaint
-
- txint_plot = numpy.matrix(numpy.zeros((2, len(alphas))))
- txint = spline_points[:, 0].copy()
- txint_plot[:, 0] = txint
- for i in range(len(alphas) - 1):
- dalpha = alphas[i + 1] - alphas[i]
- txint += dalpha * numpy.linalg.norm(
- dspline_points[:, i]) * numpy.matrix([[numpy.cos(theta[i])],
- [numpy.sin(theta[i])]])
- txint_plot[:, i + 1] = txint
- thetaint += dalpha * dtheta[i]
- dthetaint += dalpha * ddtheta[i]
- thetaint_plot[i + 1] = thetaint
- dthetaint_plot[i + 1] = dthetaint
-
- # Now plot x, dx/dalpha, ddx/ddalpha, dddx/dddalpha, and integrals thereof
- # to perform consistency checks.
- pylab.figure()
- pylab.plot(alphas, numpy.array(spline_points)[0, :], label='x')
- pylab.plot(alphas, numpy.array(xint_plot)[0, :], label='ix')
- pylab.plot(alphas, numpy.array(dspline_points)[0, :], label='dx')
- pylab.plot(alphas, numpy.array(dxint_plot)[0, :], label='idx')
- pylab.plot(alphas, numpy.array(txint_plot)[0, :], label='tix')
- pylab.plot(alphas, numpy.array(ddspline_points)[0, :], label='ddx')
- pylab.plot(alphas, numpy.array(dddspline_points)[0, :], label='dddx')
- pylab.legend()
-
- # Now do the same for y.
- pylab.figure()
- pylab.plot(alphas, numpy.array(spline_points)[1, :], label='y')
- pylab.plot(alphas, numpy.array(xint_plot)[1, :], label='iy')
- pylab.plot(alphas, numpy.array(dspline_points)[1, :], label='dy')
- pylab.plot(alphas, numpy.array(dxint_plot)[1, :], label='idy')
- pylab.plot(alphas, numpy.array(txint_plot)[1, :], label='tiy')
- pylab.plot(alphas, numpy.array(ddspline_points)[1, :], label='ddy')
- pylab.plot(alphas, numpy.array(dddspline_points)[1, :], label='dddy')
- pylab.legend()
-
- # And for theta.
- pylab.figure()
- pylab.plot(alphas, theta, label='theta')
- pylab.plot(alphas, dtheta, label='dtheta')
- 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 range(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.
-
- velocity_drivetrain = polydrivetrain.VelocityDrivetrainModel(
- y2016.control_loops.python.drivetrain.kDrivetrain)
- position_drivetrain = drivetrain.Drivetrain(
- y2016.control_loops.python.drivetrain.kDrivetrain)
-
- longitudal_accel = 3.0
- lateral_accel = 2.0
-
- trajectory = Trajectory(
- path,
- drivetrain=velocity_drivetrain,
- longitudal_accel=longitudal_accel,
- lateral_accel=lateral_accel,
- distance_count=500)
-
- vmax = numpy.inf
- vmax = 10.0
- lateral_accel_plan = trajectory.lateral_accel_pass(
- trajectory.create_plan(vmax))
-
- forward_accel_plan = lateral_accel_plan.copy()
- # Start and end the path stopped.
- forward_accel_plan[0] = 0.0
- forward_accel_plan[-1] = 0.0
-
- forward_accel_plan = trajectory.forward_pass(forward_accel_plan)
-
- backward_accel_plan = trajectory.backward_pass(forward_accel_plan)
-
- # And now, calculate the left, right voltage as a function of distance.
-
- # TODO(austin): Factor out the accel and decel functions so we can use them
- # to calculate voltage as a function of distance.
-
- pylab.figure()
- pylab.plot(trajectory.distances, lateral_accel_plan, label='accel pass')
- pylab.plot(trajectory.distances, forward_accel_plan, label='forward pass')
- pylab.plot(trajectory.distances, backward_accel_plan, label='forward pass')
- pylab.xlabel("distance along spline (m)")
- pylab.ylabel("velocity (m/s)")
- pylab.legend()
-
- dt = 0.005
- # Now, let's integrate up the path centric coordinates to get a distance,
- # velocity, and acceleration as a function of time to follow the path.
-
- length_plan_t = [0.0]
- length_plan_x = [numpy.matrix(numpy.zeros((2, 1)))]
- length_plan_v = [0.0]
- length_plan_a = [trajectory.ff_accel(backward_accel_plan, 0.0)[2]]
- t = 0.0
- spline_state = length_plan_x[-1][0:2, :]
-
- while spline_state[0, 0] < path.length():
- t += dt
-
- def along_spline_diffeq(t, x):
- _, v, a = trajectory.ff_accel(backward_accel_plan, x[0, 0])
- return numpy.matrix([[x[1, 0]], [a]])
-
- spline_state = RungeKutta(along_spline_diffeq, spline_state.copy(), t,
- dt)
-
- d, v, a = trajectory.ff_accel(backward_accel_plan,
- length_plan_x[-1][0, 0])
-
- length_plan_v.append(v)
- length_plan_a.append(a)
- length_plan_t.append(t)
- length_plan_x.append(spline_state.copy())
- spline_state[1, 0] = v
-
- xva_plan = numpy.matrix(numpy.zeros((3, len(length_plan_t))))
- u_plan = numpy.matrix(numpy.zeros((2, len(length_plan_t))))
- state_plan = numpy.matrix(numpy.zeros((5, len(length_plan_t))))
-
- state = numpy.matrix(numpy.zeros((5, 1)))
- state[3, 0] = 0.1
- state[4, 0] = 0.1
- states = numpy.matrix(numpy.zeros((5, len(length_plan_t))))
- full_us = numpy.matrix(numpy.zeros((2, len(length_plan_t))))
- x_es = []
- y_es = []
- theta_es = []
- vel_es = []
- omega_es = []
- omega_rs = []
- omega_cs = []
-
- width = (velocity_drivetrain.robot_radius_l +
- velocity_drivetrain.robot_radius_r)
- Ter = numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]])
-
- for i in range(len(length_plan_t)):
- xva_plan[0, i] = length_plan_x[i][0, 0]
- xva_plan[1, i] = length_plan_v[i]
- xva_plan[2, i] = length_plan_a[i]
-
- xy_r = path.xy(xva_plan[0, i])
- x_r = xy_r[0, 0]
- y_r = xy_r[1, 0]
- theta_r = path.theta(xva_plan[0, i])[0]
- vel_omega_r = numpy.matrix(
- [[xva_plan[1, i]],
- [path.dtheta_dt(xva_plan[0, i], xva_plan[1, i])[0]]])
- vel_lr = numpy.linalg.inv(Ter) * vel_omega_r
-
- state_plan[:, i] = numpy.matrix([[x_r], [y_r], [theta_r],
- [vel_lr[0, 0]], [vel_lr[1, 0]]])
- u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan,
- xva_plan[0, i])
-
- Q = numpy.matrix(
- numpy.diag([
- 1.0 / (0.05**2), 1.0 / (0.05**2), 1.0 / (0.2**2), 1.0 / (0.5**2),
- 1.0 / (0.5**2)
- ]))
- R = numpy.matrix(numpy.diag([1.0 / (12.0**2), 1.0 / (12.0**2)]))
- kMinVelocity = 0.1
-
- for i in range(len(length_plan_t)):
- states[:, i] = state
-
- theta = state[2, 0]
- sintheta = numpy.sin(theta)
- costheta = numpy.cos(theta)
- linear_velocity = (state[3, 0] + state[4, 0]) / 2.0
- if abs(linear_velocity) < kMinVelocity / 100.0:
- linear_velocity = 0.1
- elif abs(linear_velocity) > kMinVelocity:
- pass
- elif linear_velocity > 0:
- linear_velocity = kMinVelocity
- elif linear_velocity < 0:
- linear_velocity = -kMinVelocity
-
- width = (velocity_drivetrain.robot_radius_l +
- velocity_drivetrain.robot_radius_r)
- A_linearized_continuous = numpy.matrix(
- [[
- 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta,
- 0.5 * costheta
- ],
- [
- 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta,
- 0.5 * sintheta
- ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
- [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]])
- A_linearized_continuous[3:5, 3:5] = velocity_drivetrain.A_continuous
- B_linearized_continuous = numpy.matrix(numpy.zeros((5, 2)))
- B_linearized_continuous[3:5, :] = velocity_drivetrain.B_continuous
-
- A, B = controls.c2d(A_linearized_continuous, B_linearized_continuous,
- dt)
-
- if i >= 0:
- K = controls.dlqr(A, B, Q, R)
- print("K", K)
- print("eig", numpy.linalg.eig(A - B * K)[0])
- goal_state = trajectory.goal_state(xva_plan[0, i], xva_plan[1, i])
- state_error = goal_state - state
-
- U = (trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i]) +
- K * (state_error))
-
- def spline_diffeq(U, t, x):
- velocity = x[3:5, :]
- theta = x[2, 0]
- linear_velocity = (velocity[0, 0] + velocity[1, 0]) / 2.0
- angular_velocity = (velocity[1, 0] - velocity[0, 0]) / (
- velocity_drivetrain.robot_radius_l +
- velocity_drivetrain.robot_radius_r)
- accel = (velocity_drivetrain.A_continuous * velocity +
- velocity_drivetrain.B_continuous * U)
- return numpy.matrix([[numpy.cos(theta) * linear_velocity],
- [numpy.sin(theta) * linear_velocity],
- [angular_velocity], [accel[0, 0]],
- [accel[1, 0]]])
-
- full_us[:, i] = U
-
- state = RungeKutta(lambda t, x: spline_diffeq(U, t, x), state, i * dt,
- dt)
-
- pylab.figure()
- pylab.plot(length_plan_t, numpy.array(xva_plan)[0, :], label='x')
- pylab.plot(length_plan_t, [x[1, 0] for x in length_plan_x], label='v')
- pylab.plot(length_plan_t, numpy.array(xva_plan)[1, :], label='planv')
- pylab.plot(length_plan_t, numpy.array(xva_plan)[2, :], label='a')
-
- pylab.plot(length_plan_t, numpy.array(full_us)[0, :], label='vl')
- pylab.plot(length_plan_t, numpy.array(full_us)[1, :], label='vr')
- pylab.legend()
-
- pylab.figure()
- pylab.plot(
- numpy.array(states)[0, :], numpy.array(states)[1, :], label='robot')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
- pylab.legend()
-
- def a(_, x):
- return 2.0
- return 2.0 + 0.0001 * x
-
- v = 0.0
- for _ in range(10):
- dx = 4.0 / 10.0
- v = integrate_accel_for_distance(a, v, 0.0, dx)
- print('v', v)
-
- pylab.show()
-
-
-if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))