Add osqp based python solver
This gets us in a form we can solve quickly on the roboRIO. Now, all we
need to do is to convert it to C++.
Change-Id: I8416d9f89274d4d288402d3ffd676ed22efc7017
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/WORKSPACE b/WORKSPACE
index 2aeb3a3..855b31a 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -975,6 +975,22 @@
)
http_archive(
+ name = "osqp_amd64",
+ build_file = "@//debian:osqp_python.BUILD",
+ sha256 = "8003fc363f707daa46fef3af548e6a580372154d6cd49a7bf2f569ba5f807d15",
+ type = "zip",
+ url = "https://files.pythonhosted.org/packages/3f/e2/f1c40e890f00f8a566bc2481d0f215e52def3dfe8eea6b8ad4cc2d3cbca2/osqp-0.6.2.post5-cp39-cp39-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_17_x86_64.manylinux2014_x86_64.whl",
+)
+
+http_archive(
+ name = "qdldl_amd64",
+ build_file = "@//debian:qdldl_python.BUILD",
+ sha256 = "2c09f4b1a1c6f3a0579af004443417e084491e7c844ff9fb73170bb5d43f70b5",
+ type = "zip",
+ url = "https://files.pythonhosted.org/packages/9e/26/ccb4f065b40c1e9ff35ee66970d4fa97dd2fe221b846da2110eb8cd6c3f4/qdldl-0.1.5.post0-cp39-cp39-manylinux2014_x86_64.whl",
+)
+
+http_archive(
name = "gstreamer_k8",
build_file = "@//debian:gstreamer.BUILD",
sha256 = "4d74d4a82f7a73dc9fe9463d5fae409b17845eef7cd64ef9c4c4553816c53589",
diff --git a/debian/osqp_python.BUILD b/debian/osqp_python.BUILD
new file mode 100644
index 0000000..9d3260e
--- /dev/null
+++ b/debian/osqp_python.BUILD
@@ -0,0 +1,15 @@
+py_library(
+ name = "python_osqp",
+ srcs = glob(["**/*.py"]),
+ data = glob(
+ include = ["**/*"],
+ exclude = ["**/*.py"],
+ ),
+ imports = ["."],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@python_repo//:numpy",
+ "@python_repo//:scipy",
+ "@qdldl_amd64//:python_qdldl",
+ ],
+)
diff --git a/debian/qdldl_python.BUILD b/debian/qdldl_python.BUILD
new file mode 100644
index 0000000..76d647e
--- /dev/null
+++ b/debian/qdldl_python.BUILD
@@ -0,0 +1,14 @@
+py_library(
+ name = "python_qdldl",
+ srcs = glob(["**/*.py"]),
+ data = glob(
+ include = ["**/*"],
+ exclude = ["**/*.py"],
+ ),
+ imports = ["."],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@python_repo//:numpy",
+ "@python_repo//:scipy",
+ ],
+)
diff --git a/tools/dependency_rewrite b/tools/dependency_rewrite
index f2172cc..d5190dd 100644
--- a/tools/dependency_rewrite
+++ b/tools/dependency_rewrite
@@ -7,6 +7,7 @@
rewrite nodejs.org/(.*) software.frc971.org/Build-Dependencies/nodejs.org/$1
rewrite static.rust-lang.org/(.*) software.frc971.org/Build-Dependencies/static.rust-lang.org/$1
rewrite storage.googleapis.com/(.*) software.frc971.org/Build-Dependencies/storage.googleapis.com/$1
+rewrite files.pythonhosted.org/(.*) software.frc971.org/Build-Dependencies/files.pythonhosted.org/$1
allow golang.org
allow software.frc971.org
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index d3a5a62..286b874 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -72,6 +72,7 @@
":python_init",
"//external:python-gflags",
"//external:python-glog",
+ "@osqp_amd64//:python_osqp",
],
)
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
index ae6079a..491286e 100755
--- a/y2022/control_loops/python/catapult.py
+++ b/y2022/control_loops/python/catapult.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import osqp
import math
import scipy.optimize
import sys
@@ -49,7 +50,7 @@
print("J", J)
kCatapult = catapult_lib.CatapultParams(
- name='Finisher',
+ name='Catapult',
motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
G=G,
J=J,
@@ -61,8 +62,6 @@
controller_poles=[.93],
dt=0.00505)
-catapult = catapult_lib.Catapult(kCatapult)
-
# Ideas for adjusting the cost function:
#
# Penalize battery current?
@@ -76,8 +75,86 @@
# We really want our cost function to be robust so that we can tolerate the
# battery not delivering like we want at the end.
+def quadratic_cost(catapult, X_initial, X_final, horizon):
+ Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
-def mpc_cost(X_initial, X_final, u_matrix):
+ As = numpy.vstack([catapult.A**(n + 1) for n in range(0, horizon)])
+ Af = catapult.A**horizon
+
+ Bs = numpy.matrix(numpy.zeros((2 * horizon, horizon)))
+ for n in range(0, horizon):
+ for m in range(0, n + 1):
+ Bs[n * 2:(n * 2) + 2, m] = catapult.A**(n - m) * catapult.B
+
+ Bf = Bs[horizon * 2 - 2:, :]
+
+ P_final = 2.0 * Bf.transpose() * Q_final * Bf
+ q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
+ Bf).transpose()
+
+ constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
+ Af * X_initial - X_final)
+
+ m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(horizon)])
+ M = Bs[1:horizon * 2:2, :]
+
+ W = numpy.matrix(numpy.identity(horizon) - numpy.eye(horizon, horizon, -1)) / catapult.dt
+ w = -numpy.matrix(numpy.eye(horizon, 1, 0)) / catapult.dt
+
+
+ Pi = numpy.diag([
+ (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (horizon - n)) / 20.0) ** 2.0 for n in range(horizon)
+ ])
+
+ P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
+ q_accel = 2.0 * (((W * m + w) * X_initial[1, 0]).transpose() * Pi * W * M).transpose()
+ constant_accel = ((W * m + w) * X_initial[1, 0]).transpose() * Pi * (
+ (W * m + w) * X_initial[1, 0])
+
+ return ((P_accel + P_final), (q_accel + q_final), (constant_accel + constant_final))
+
+
+def new_cost(catapult, X_initial, X_final, u):
+ u_matrix = numpy.matrix(u).transpose()
+ Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
+
+ As = numpy.vstack([catapult.A**(n + 1) for n in range(0, len(u))])
+ Af = catapult.A**len(u)
+
+ Bs = numpy.matrix(numpy.zeros((2 * len(u), len(u))))
+ for n in range(0, len(u)):
+ for m in range(0, n + 1):
+ Bs[n * 2:(n * 2) + 2, m] = catapult.A**(n - m) * catapult.B
+
+ Bf = Bs[len(u) * 2 - 2:, :]
+
+ P_final = 2.0 * Bf.transpose() * Q_final * Bf
+ q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
+ Bf).transpose()
+
+ constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
+ Af * X_initial - X_final)
+
+ m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(len(u))])
+ M = Bs[1:len(u) * 2:2, :]
+
+ W = numpy.matrix(numpy.identity(len(u)) - numpy.eye(len(u), len(u), -1)) / catapult.dt
+ w = -numpy.matrix(numpy.eye(len(u), 1, 0)) * X_initial[1, 0] / catapult.dt
+
+ accel = W * (M * u_matrix + m * X_initial[1, 0]) + w
+
+ Pi = numpy.diag([
+ (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (len(u) - n)) / 20.0) ** 2.0 for n in range(len(u))
+ ])
+
+ P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
+ q_accel = 2.0 * ((W * m * X_initial[1, 0] + w).transpose() * Pi * W * M).transpose()
+ constant_accel = (W * m * X_initial[1, 0] +
+ w).transpose() * Pi * (W * m * X_initial[1, 0] + w)
+
+
+def mpc_cost(catapult, X_initial, X_final, u_matrix):
+
X = X_initial.copy()
cost = 0.0
last_u = u_matrix[0]
@@ -118,7 +195,7 @@
return cost
-def SolveCatapult(X_initial, X_final, u):
+def SolveCatapult(catapult, X_initial, X_final, u):
""" Solves for the optimal action given a seed, state, and target """
def vbat_constraint(z, i):
return 12.0 - z[i]
@@ -126,10 +203,31 @@
def forward(z, i):
return z[i]
- def mpc_cost2(u_matrix):
- return mpc_cost(X_initial, X_final, u_matrix)
+ P, q, c = quadratic_cost(catapult, X_initial, X_final, len(u))
- constraints = [{
+
+ def mpc_cost2(u_solver):
+ u_matrix = numpy.matrix(u_solver).transpose()
+ cost = mpc_cost(catapult, X_initial, X_final, u_solver)
+ return cost
+
+
+ def mpc_cost3(u_solver):
+ u_matrix = numpy.matrix(u_solver).transpose()
+ return (0.5 * u_matrix.transpose() * P * u_matrix +
+ q.transpose() * u_matrix + c)[0, 0]
+
+ # If we provide scipy with the analytical jacobian and hessian, it solves
+ # more accurately and a *lot* faster.
+ def jacobian(u):
+ u_matrix = numpy.matrix(u).transpose()
+ return numpy.array(P * u_matrix + q)
+
+ def hessian(u):
+ return numpy.array(P)
+
+ constraints = []
+ constraints += [{
'type': 'ineq',
'fun': vbat_constraint,
'args': (i, )
@@ -141,9 +239,12 @@
'args': (i, )
} for i in numpy.arange(len(u))]
- result = scipy.optimize.minimize(mpc_cost2,
+ result = scipy.optimize.minimize(mpc_cost3,
u,
+ jac=jacobian,
+ hess=hessian,
method='SLSQP',
+ tol=1e-12,
constraints=constraints)
print(result)
@@ -171,7 +272,8 @@
# Iteratively solve our MPC and simulate it.
u_samples = []
for i in range(kHorizon):
- u_horizon = SolveCatapult(X, X_final, u)
+ u_horizon = SolveCatapult(c, X, X_final, u)
+
u_samples.append(u_horizon[0])
v_prior = X[1, 0]
X = c.A * X + c.B * numpy.matrix([[u_horizon[0]]])
@@ -200,15 +302,43 @@
def main(argv):
# Do all our math with a lower voltage so we have headroom.
U = numpy.matrix([[9.0]])
- print(
- "For G:", G, " max speed ",
- catapult_lib.MaxSpeed(params=kCatapult,
- U=U,
- final_position=math.pi / 2.0))
- CatapultProblem()
+ prob = osqp.OSQP()
+
+ kHorizon = 40
+ catapult = catapult_lib.Catapult(kCatapult)
+ X_initial = numpy.matrix([[0.0], [0.0]])
+ X_final = numpy.matrix([[2.0], [25.0]])
+ P, q, c = quadratic_cost(catapult, X_initial, X_final, kHorizon)
+ A = numpy.identity(kHorizon)
+ l = numpy.zeros((kHorizon, 1))
+ u = numpy.ones((kHorizon, 1)) * 12.0
+
+ prob.setup(scipy.sparse.csr_matrix(P),
+ q,
+ scipy.sparse.csr_matrix(A),
+ l,
+ u,
+ warm_start=True)
+
+ result = prob.solve()
+ # Check solver status
+ if result.info.status != 'solved':
+ raise ValueError('OSQP did not solve the problem!')
+
+ # Apply first control input to the plant
+ print(result.x)
if FLAGS.plot:
+ print(
+ "For G:", G, " max speed ",
+ catapult_lib.MaxSpeed(params=kCatapult,
+ U=U,
+ final_position=math.pi / 2.0))
+
+ CatapultProblem()
+ return 0
+
catapult_lib.PlotShot(kCatapult, U, final_position=math.pi / 4.0)
gs = []
@@ -222,7 +352,15 @@
final_position=math.pi / 2.0))
pylab.plot(gs, speed, label="max_speed")
pylab.show()
- return 0
+
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the catapult and integral catapult.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'catapult']
+ catapult_lib.WriteCatapult(kCatapult, argv[1:3], argv[3:5], namespaces)
+ return 0
if __name__ == '__main__':
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index 256793a..2606c44 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -306,3 +306,45 @@
pylab.legend()
pylab.show()
+
+def WriteCatapult(params, plant_files, controller_files, year_namespaces):
+ """Writes out the constants for a catapult to a file.
+
+ Args:
+ params: list of CatapultParams or CatapultParams, the
+ parameters defining the system.
+ plant_files: list of strings, the cc and h files for the plant.
+ controller_files: list of strings, the cc and h files for the integral
+ controller.
+ year_namespaces: list of strings, the namespace list to use.
+ """
+ # Write the generated constants out to a file.
+ catapults = []
+ integral_catapults = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ catapults.append(
+ Catapult(param, param.name + str(index)))
+ integral_catapults.append(
+ IntegralCatapult(param,
+ 'Integral' + param.name + str(index)))
+ else:
+ name = params.name
+ catapults.append(Catapult(params, params.name))
+ integral_catapults.append(
+ IntegralCatapult(params, 'Integral' + params.name))
+
+ loop_writer = control_loop.ControlLoopWriter(
+ name, catapults, namespaces=year_namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', catapults[0].G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f',
+ catapults[0].motor.free_speed))
+ loop_writer.Write(plant_files[0], plant_files[1])
+
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, integral_catapults, namespaces=year_namespaces)
+ integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2022/control_loops/superstructure/catapult/mpc.tex b/y2022/control_loops/superstructure/catapult/mpc.tex
new file mode 100644
index 0000000..b7110fe
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/mpc.tex
@@ -0,0 +1,202 @@
+\documentclass[a4paper,12pt]{article}
+\usepackage{amsmath}
+\usepackage{graphicx}
+\begin{document}
+
+TODO(austin): Now that the python matches the original problem and solves, confirm the paper matches what got implemented.
+
+osqp!
+
+\section{Catapult MPC}
+We want to phrase our problem as trying to solve for the set of control inputs
+which get us closest to the destination, but minimizes acceleration.
+Specifically, we want to minimize acceleration close to the end.
+We also have a voltage limit.
+
+Our model is
+
+\begin{equation}
+\label{cost}
+ \begin{bmatrix} x_1 \\ v_1 \end{bmatrix} =
+ \begin{bmatrix} a_{00} & a_{01} \\ 0 & a_{11} \end{bmatrix}
+ \begin{bmatrix} x_0 \\ v_0 \end{bmatrix} +
+ \begin{bmatrix} b_{0} \\ b_{1} \end{bmatrix} \begin{bmatrix} u_0 \end{bmatrix}
+\end{equation}
+
+Our acceleration can be measured as:
+
+\begin{equation}
+\label{accel}
+ \frac{ \left( \boldsymbol{X_1(1)} - \boldsymbol{X_1(0)} \right)}{\Delta t}
+\end{equation}
+
+This simplifies to:
+
+\begin{equation}
+ \frac{a_{11} v_0 + b_1 u_0 - v_0}{\Delta t}
+\end{equation}
+
+and finally
+
+\begin{equation}
+ \frac{(a_{11} - 1) v_0 + b_1 u_0}{\Delta t}
+\end{equation}
+
+
+We can also compute our state matrix as a function of inital state and the control inputs.
+
+\begin{equation}
+\label{all_x}
+ \begin{bmatrix} X_1 \\ X_2 \\ X_3 \\ \vdots \end{bmatrix} =
+ \begin{bmatrix} A \\
+ A^2 \\
+ A^3 \\
+ \vdots \end{bmatrix}
+ X_0 +
+ \begin{bmatrix} B & 0 & 0 & 0 \\
+ A B & B & 0 & 0 \\
+ A^2 B & A B & B & 0 \\
+ \vdots & \ddots & & \hdots \end{bmatrix}
+ \begin{bmatrix} U_0 \\ U_1 \\ U_2 \\ \vdots \end{bmatrix}
+\end{equation}
+
+\section{MPC problem formulation}
+
+We want to penalize both final state and intermediate acceleration.
+
+\begin{equation}
+C = \sum_{n=0}^{39} \frac{\left(v(n + 1) - v(n)\right)^2}{\Delta t} \pi_n + (X_{40} - X_{final})^T Q_{final} (X_{40} - X_{final})
+\end{equation}
+
+where $\pi_n$ is a constant only dependent on $n$, and designed such that it depends on the distance to the end of the sequence, not the distance from the start.
+
+In order to use OSQP, which has a code generator, we need to get this into the form of
+
+\begin{tabular}{ l l }
+minimize & $ \frac{1}{2} X^T P X + q^T X $ \\
+subject to & $ l <= A X <= u $ \\
+\end{tabular}
+
+This is the simplest form of a constrained quadratic program that we can solve efficiently.
+Luckily for us, the problem statement above fits that definition.
+
+\section{Manipulating the formulation}
+
+We need to separate constant factors from things dependent on U (or X in OSQP parlance) so we can create these matrices easier.
+
+\subsection{Terminal cost}
+
+Next step is to compute $X_{40}$ using equation \ref{all_x}.
+We can do this by only computing the final row of the matrix.
+
+\begin{equation}
+\label{x40}
+ X_{40} = \begin{bmatrix} A^{39} B & A^{38} B & \hdots & B \end{bmatrix}
+ \begin{bmatrix} U_0 \\
+ \vdots \\
+ U_{39}
+ \end{bmatrix} + A^{40} X_0 = B_f \boldsymbol{U} + A^{40} X_0
+\end{equation}
+
+We can substitute equation \ref{x40} into equation \ref{cost}.
+
+\begin{equation}
+\label{final_cost}
+\begin{aligned}[t]
+ C_f = & \boldsymbol{U}^T B_f^T Q_{final} B_f \boldsymbol{U} \\
+ & + 2 (A^{40} X_0 - X_{final})^T Q_{final} B_f \boldsymbol{U} \\
+ & + (A^{40} X_0 - X_{final})^T Q_{final} (A^{40} X_0 - X_{final})
+\end{aligned}
+\end{equation}
+
+\subsection{Acceleration costs}
+
+We can compute a velocity matrix for all the times by stripping out the positions from equation \ref{all_x} by using every other row.
+We can use this to then compute the accelerations for each time slice and then penalize them.
+
+\begin{equation}
+ \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_{40} \end{bmatrix} =
+ M \boldsymbol{U} + \begin{bmatrix} a_{11} \\ a^2_{11} \\ \vdots \\ a^{40}_{11} \end{bmatrix} v_0 =
+ M \boldsymbol{U} + m v_0
+\end{equation}
+
+We can then use equation \ref{accel} in matrix form to convert a velocity matrix to an acceleration matrix.
+
+\begin{equation}
+ \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \vdots \\ \alpha_{40} \end{bmatrix} =
+ \frac{1}{\Delta t} \left(
+ \begin{bmatrix} 1 & 0 & 0 & \hdots & 0 \\
+ -1 & 1 & 0 & \hdots & 0 \\
+ 0 & -1 & 1 & \hdots & 0 \\
+ \vdots & & & \ddots & \vdots \\
+ 0 & 0 & \hdots & -1 & 1 \\
+ \end{bmatrix}
+ \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_{40} \end{bmatrix} - \begin{bmatrix} v_0 \\ 0 \\ \vdots \\ 0 \end{bmatrix}
+\right)
+\end{equation}
+
+We can pull some of these terms out to make it easier to work with.
+
+\begin{equation}
+\boldsymbol{\alpha} = W V + w v_0
+\end{equation}
+
+Our acceleration cost function is then:
+
+\begin{equation}
+C_a = \boldsymbol{\alpha}^T
+ \begin{bmatrix} \pi_1 & & 0 \\
+ & \pi_2 & \\
+ 0 & & \ddots \end{bmatrix} \boldsymbol{\alpha} =
+ \boldsymbol{\alpha}^T \Pi \boldsymbol{\alpha}
+\end{equation}
+
+We can substitute everything in to get something as a function of $U$.
+
+\begin{equation}
+C_a = \left(W \left(M \boldsymbol{U} + m v_0 \right) + w v_0 \right)^T \Pi \left(W \left(M \boldsymbol{U} + m v_0 \right) + w v_0 \right)
+\end{equation}
+
+And then simplify this down into the expected form.
+
+\begin{equation}
+ C_a = \left(W M \boldsymbol{U} + (W m + w) v_0 \right)^T \Pi \left(W M \boldsymbol{U} + (W m + w) v_0 \right)
+\end{equation}
+
+\begin{equation}
+\label{accel_cost}
+\begin{aligned}[t]
+C_a = & \boldsymbol{U}^T M^T W^T \Pi W M \boldsymbol{U} \\
+ & + 2 v_0 (W m + w)^T \Pi W M \boldsymbol{U} \\
+& + v_0 (W m + w)^T \Pi \left(W m + w \right) v_0
+\end{aligned}
+\end{equation}
+
+\subsection{Overall cost}
+
+We can combine equations \ref{final_cost} and \ref{accel_cost} to get our overall cost in the correct form.
+
+\begin{equation}
+\begin{aligned}[t]
+C &= \boldsymbol{U}^T \left( M^T W^T \Pi W M + B_f^T Q_{final} B_f \right) \boldsymbol{U} \\
+ &+ \left(2 v_0 (W m + w)^T \Pi W M
+ - 2 X_{final}^T Q_{final} B_f
+\right) \boldsymbol{U} \\
+& + X_{final}^T Q_{final} X_{final}
+ + v_0 (W m + w)^T \Pi \left(W m + w \right) v_0
+\end{aligned}
+\end{equation}
+
+
+\section{Response}
+
+For a reasonable request (11 m/s after 90 degrees), we get the following response
+
+\begin{figure}
+ \centering
+ \includegraphics[width=\linewidth]{response.png}
+\end{figure}
+
+This is well within 1\% error, and avoid saturation and keeps the acceleration down at the end.
+
+\end{document}