Merge "Compensate steer coupling torque from velocity cost" into main
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 5adef05..c7355b5 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -219,6 +219,7 @@
],
deps = [
":dynamics",
+ "@pip//absl_py",
"@pip//casadi",
"@pip//matplotlib",
"@pip//numpy",
@@ -265,3 +266,29 @@
"@pip//scipy",
],
)
+
+py_binary(
+ name = "smooth_function_graph",
+ srcs = [
+ "smooth_function_graph.py",
+ ],
+ deps = [
+ "@pip//matplotlib",
+ "@pip//numpy",
+ "@pip//pygobject",
+ "@pip//scipy",
+ ],
+)
+
+py_binary(
+ name = "multi_casadi_velocity_mpc",
+ srcs = ["multi_casadi_velocity_mpc.py"],
+ data = [":casadi_velocity_mpc"],
+ deps = [
+ ":dynamics",
+ "@pip//absl_py",
+ "@pip//matplotlib",
+ "@pip//numpy",
+ "@pip//pygobject",
+ ],
+)
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 5c1148e..239f49a 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -1,6 +1,7 @@
-#!/usr/bin/python3
+#!/usr/bin/env python3
from frc971.control_loops.swerve import dynamics
+import pickle
import matplotlib.pyplot as pyplot
import matplotlib
from matplotlib import pylab
@@ -9,6 +10,18 @@
import scipy
import casadi
import os, sys
+from absl import flags
+from absl import app
+
+FLAGS = flags.FLAGS
+flags.DEFINE_bool('compileonly', False,
+ 'If true, load casadi, don\'t compile it')
+flags.DEFINE_float('vx', 1.0, 'Goal velocity in m/s in x')
+flags.DEFINE_float('vy', 0.0, 'Goal velocity in m/s in y')
+flags.DEFINE_float('omega', 0.0, 'Goal velocity in m/s in omega')
+flags.DEFINE_float('duration', 0.5, 'Time to simulate in seconds.')
+flags.DEFINE_bool('pickle', False, 'Write optimization results.')
+flags.DEFINE_string('outputdir', None, 'Directory to write problem results to')
matplotlib.use("GTK3Agg")
@@ -45,7 +58,7 @@
casadi.SX.sym("U", 8, 1)) for i in range(4)
]
- self.N = 50
+ self.N = 200
# Start with an empty nonlinear program.
self.w = []
@@ -78,6 +91,7 @@
Xn = casadi.horzcat(*Xn_variables)
U = casadi.horzcat(*U_variables)
+ # printme(number) is the debug.
Xk_begin = casadi.horzcat(self.X0, Xn)
Xk_end = self.next_X.map(self.N, "thread")(Xk_begin, U)
J = casadi.sum2(self.cost.map(self.N, "thread")(Xk_end, U, self.R))
@@ -140,7 +154,6 @@
"debug": True,
"equality": equality,
}
- self.solver = casadi.nlpsol('solver', 'fatrop', prob, options)
else:
options = {
"jit": True,
@@ -154,14 +167,16 @@
options["ipopt"] = {
"print_level": 12,
}
- self.solver = casadi.nlpsol('solver', 'ipopt', prob, options)
+ self.solver = casadi.nlpsol('solver', solver, prob, options)
+
+ # TODO(austin): Vary the number of sub steps to be more short term and fewer long term?
def make_physics(self):
X0 = casadi.MX.sym('X0', dynamics.NUM_VELOCITY_STATES)
U = casadi.MX.sym('U', 8)
X = X0
- M = 4 # RK4 steps per interval
+ M = 2 # RK4 steps per interval
DT = self.dt / M
for j in range(M):
@@ -185,11 +200,14 @@
J = 0
vnorm = casadi.sqrt(R[0]**2.0 + R[1]**2.0)
- vnormx = R[0] / vnorm
- vnormy = R[1] / vnorm
- vperpx = -vnormy
- vperpy = vnormx
+ vnormx = casadi.if_else(vnorm > 0.0001, R[0] / vnorm, 1.0)
+ vnormy = casadi.if_else(vnorm > 0.0001, R[1] / vnorm, 0.0)
+
+ vperpx = casadi.if_else(vnorm > 0.0001, -vnormy, 0.0)
+ vperpy = casadi.if_else(vnorm > 0.0001, vnormx, 1.0)
+
+ # TODO(austin): Do we want to do something more special for 0?
J += 75 * ((R[0] - X[dynamics.VELOCITY_STATE_VX]) * vnormx +
(R[1] - X[dynamics.VELOCITY_STATE_VY]) * vnormy)**2.0
@@ -252,112 +270,210 @@
(i - 1):(8 + dynamics.NUM_VELOCITY_STATES) * i]
-mpc = MPC(solver='fatrop') if not full_debug else MPC(solver='ipopt')
+class Solver(object):
-R_goal = numpy.zeros((3, 1))
-R_goal[0, 0] = 1.0
-R_goal[1, 0] = 1.0
-R_goal[2, 0] = 0.0
+ def __init__(self):
+ self.iterations = int(round(FLAGS.duration / 0.005))
-module_velocity = 0.0
+ self.X_plot = numpy.zeros((25, self.iterations))
+ self.U_plot = numpy.zeros((8, self.iterations))
+ self.t = []
-X_initial = numpy.zeros((25, 1))
-# All the wheels are spinning at the speed needed to hit 1 m/s
-X_initial[dynamics.STATE_THETAS0, 0] = 0.0
-X_initial[dynamics.STATE_OMEGAS0, 0] = module_velocity
+ def solve(self, mpc, X_initial, R_goal, debug=False):
+ X = X_initial.copy()
-X_initial[dynamics.STATE_THETAS1, 0] = 0.0
-X_initial[dynamics.STATE_OMEGAS1, 0] = module_velocity
+ if debug:
+ pyplot.ion()
+ fig0, axs0 = pylab.subplots(2)
+ fig1, axs1 = pylab.subplots(2)
-X_initial[dynamics.STATE_THETAS2, 0] = 0.0
-X_initial[dynamics.STATE_OMEGAS2, 0] = module_velocity
+ last_time = time.time()
-X_initial[dynamics.STATE_THETAS3, 0] = 0.0
-X_initial[dynamics.STATE_OMEGAS3, 0] = module_velocity
+ seed = ([0, 0] * 4 +
+ list(dynamics.to_velocity_state(X))) * (mpc.N - 1) + [0, 0] * 4
-# Robot is moving at 0 m/s
-X_initial[dynamics.STATE_VX, 0] = 0.0
-X_initial[dynamics.STATE_VY, 0] = 0.0
-# No angular velocity
-X_initial[dynamics.STATE_OMEGA, 0] = 0.0
+ overall_time = 0
+ for i in range(self.iterations):
+ self.t.append(i * mpc.dt)
+ print("Current X at", i * mpc.dt, X.transpose())
+ print("Goal R at", i * mpc.dt, R_goal)
+ start_time = time.time()
+ sol = mpc.solve(
+ # TODO(austin): Is this better or worse than constraints on the initial state for convergence?
+ p=numpy.vstack((dynamics.to_velocity_state(X), R_goal)),
+ seed=seed)
+ end_time = time.time()
+ print(f"Took {end_time - start_time} seconds to solve.")
+ overall_time += end_time - start_time
-iterations = 100
+ self.X_plot[:, i] = X[:, 0]
-X_plot = numpy.zeros((25, iterations))
-U_plot = numpy.zeros((8, iterations))
-t = []
+ U = mpc.unpack_u(sol, 0)
+ seed = (list(
+ sol['x'].full().flatten()[8 + dynamics.NUM_VELOCITY_STATES:]) +
+ list(sol['x'].full().flatten()
+ [-(8 + dynamics.NUM_VELOCITY_STATES):]))
+ self.U_plot[:, i] = U
-X = X_initial.copy()
+ print('x(0):', X.transpose())
+ for j in range(mpc.N):
+ print(f'u({j}): ', mpc.unpack_u(sol, j))
+ print(f'x({j+1}): ', mpc.unpack_x(sol, j + 1))
-pyplot.ion()
+ result = scipy.integrate.solve_ivp(
+ lambda t, x: mpc.wrapped_swerve_physics(x, U).flatten(),
+ [0, mpc.dt], X.flatten())
+ X[:, 0] = result.y[:, -1]
-fig0, axs0 = pylab.subplots(2)
-fig1, axs1 = pylab.subplots(2)
-last_time = time.time()
+ if time.time() > last_time + 2 or i == self.iterations - 1:
+ if debug:
+ axs0[0].clear()
+ axs0[1].clear()
-seed = ([0, 0] * 4 + list(dynamics.to_velocity_state(X))) * (mpc.N -
- 1) + [0, 0] * 4
+ axs0[0].plot(self.t,
+ self.X_plot[dynamics.STATE_VX, 0:i + 1],
+ label="vx")
+ axs0[0].plot(self.t,
+ self.X_plot[dynamics.STATE_VY, 0:i + 1],
+ label="vy")
+ axs0[0].legend()
-overall_time = 0
-for i in range(iterations):
- t.append(i * mpc.dt)
- print("Current X at", i * mpc.dt, X.transpose())
- print("Goal R at", i * mpc.dt, R_goal)
- start_time = time.time()
- sol = mpc.solve(
- # TODO(austin): Is this better or worse than constraints on the initial state for convergence?
- p=numpy.vstack((dynamics.to_velocity_state(X), R_goal)),
- seed=seed)
- end_time = time.time()
- print(f"Took {end_time - start_time} seconds to solve.")
- overall_time += end_time - start_time
+ axs0[1].plot(self.t, self.U_plot[0, 0:i + 1], label="Is0")
+ axs0[1].plot(self.t, self.U_plot[1, 0:i + 1], label="Id0")
+ axs0[1].legend()
- X_plot[:, i] = X[:, 0]
+ axs1[0].clear()
+ axs1[1].clear()
- U = mpc.unpack_u(sol, 0)
- seed = (
- list(sol['x'].full().flatten()[8 + dynamics.NUM_VELOCITY_STATES:]) +
- list(sol['x'].full().flatten()[-(8 + dynamics.NUM_VELOCITY_STATES):]))
- U_plot[:, i] = U
+ axs1[0].plot(self.t,
+ self.X_plot[dynamics.STATE_THETAS0, 0:i + 1],
+ label='steer0')
+ axs1[0].plot(self.t,
+ self.X_plot[dynamics.STATE_THETAS1, 0:i + 1],
+ label='steer1')
+ axs1[0].plot(self.t,
+ self.X_plot[dynamics.STATE_THETAS2, 0:i + 1],
+ label='steer2')
+ axs1[0].plot(self.t,
+ self.X_plot[dynamics.STATE_THETAS3, 0:i + 1],
+ label='steer3')
+ axs1[0].legend()
+ axs1[1].plot(self.t,
+ self.X_plot[dynamics.STATE_OMEGAS0, 0:i + 1],
+ label='steer_velocity0')
+ axs1[1].plot(self.t,
+ self.X_plot[dynamics.STATE_OMEGAS1, 0:i + 1],
+ label='steer_velocity1')
+ axs1[1].plot(self.t,
+ self.X_plot[dynamics.STATE_OMEGAS2, 0:i + 1],
+ label='steer_velocity2')
+ axs1[1].plot(self.t,
+ self.X_plot[dynamics.STATE_OMEGAS3, 0:i + 1],
+ label='steer_velocity3')
+ axs1[1].legend()
+ pyplot.pause(0.0001)
- print('x(0):', X.transpose())
- for j in range(mpc.N):
- print(f'u({j}): ', mpc.unpack_u(sol, j))
- print(f'x({j+1}): ', mpc.unpack_x(sol, j + 1))
+ last_time = time.time()
- result = scipy.integrate.solve_ivp(
- lambda t, x: mpc.wrapped_swerve_physics(x, U).flatten(), [0, mpc.dt],
- X.flatten())
- X[:, 0] = result.y[:, -1]
+ print(f"Tool {overall_time} seconds overall to solve.")
- if time.time() > last_time + 2 or i == iterations - 1:
+
+def main(argv):
+ if FLAGS.outputdir:
+ os.chdir(FLAGS.outputdir)
+
+ module_velocity = 0.0
+
+ X_initial = numpy.zeros((25, 1))
+ # All the wheels are spinning at the speed needed to hit 1 m/s
+ X_initial[dynamics.STATE_THETAS0, 0] = 0.0
+ X_initial[dynamics.STATE_OMEGAS0, 0] = module_velocity
+
+ X_initial[dynamics.STATE_THETAS1, 0] = 0.0
+ X_initial[dynamics.STATE_OMEGAS1, 0] = module_velocity
+
+ X_initial[dynamics.STATE_THETAS2, 0] = 0.0
+ X_initial[dynamics.STATE_OMEGAS2, 0] = module_velocity
+
+ X_initial[dynamics.STATE_THETAS3, 0] = 0.0
+ X_initial[dynamics.STATE_OMEGAS3, 0] = module_velocity
+
+ # Robot is moving at 0 m/s
+ X_initial[dynamics.STATE_VX, 0] = 0.0
+ X_initial[dynamics.STATE_VY, 0] = 0.0
+ # No angular velocity
+ X_initial[dynamics.STATE_OMEGA, 0] = 0.0
+
+ R_goal = numpy.zeros((3, 1))
+ R_goal[0, 0] = FLAGS.vx
+ R_goal[1, 0] = FLAGS.vy
+ R_goal[2, 0] = FLAGS.omega
+
+ mpc = MPC(solver='fatrop') if not full_debug else MPC(solver='ipopt')
+ solver = Solver()
+ if not FLAGS.compileonly:
+ results = solver.solve(mpc=mpc,
+ X_initial=X_initial,
+ R_goal=R_goal,
+ debug=(FLAGS.pickle == False))
+ else:
+ return 0
+
+ if FLAGS.pickle:
+ with open('t.pkl', 'wb') as f:
+ pickle.dump(solver.t, f)
+ with open('X_plot.pkl', 'wb') as f:
+ pickle.dump(solver.X_plot, f)
+ with open('U_plot.pkl', 'wb') as f:
+ pickle.dump(solver.U_plot, f)
+
+ fig0, axs0 = pylab.subplots(2)
+ fig1, axs1 = pylab.subplots(2)
+
axs0[0].clear()
axs0[1].clear()
- axs0[0].plot(t, X_plot[dynamics.STATE_VX, 0:i + 1], label="vx")
- axs0[0].plot(t, X_plot[dynamics.STATE_VY, 0:i + 1], label="vy")
+ axs0[0].plot(solver.t, solver.X_plot[dynamics.STATE_VX, :], label="vx")
+ axs0[0].plot(solver.t, solver.X_plot[dynamics.STATE_VY, :], label="vy")
axs0[0].legend()
- axs0[1].plot(t, U_plot[0, 0:i + 1], label="Is0")
- axs0[1].plot(t, U_plot[1, 0:i + 1], label="Id0")
+ axs0[1].plot(solver.t, solver.U_plot[0, :], label="Is0")
+ axs0[1].plot(solver.t, solver.U_plot[1, :], label="Id0")
axs0[1].legend()
axs1[0].clear()
axs1[1].clear()
- axs1[0].plot(t, X_plot[0, 0:i + 1], label='steer0')
- axs1[0].plot(t, X_plot[4, 0:i + 1], label='steer1')
- axs1[0].plot(t, X_plot[8, 0:i + 1], label='steer2')
- axs1[0].plot(t, X_plot[12, 0:i + 1], label='steer3')
+ axs1[0].plot(solver.t,
+ solver.X_plot[dynamics.STATE_THETAS0, :],
+ label='steer0')
+ axs1[0].plot(solver.t,
+ solver.X_plot[dynamics.STATE_THETAS1, :],
+ label='steer1')
+ axs1[0].plot(solver.t,
+ solver.X_plot[dynamics.STATE_THETAS2, :],
+ label='steer2')
+ axs1[0].plot(solver.t,
+ solver.X_plot[dynamics.STATE_THETAS3, :],
+ label='steer3')
axs1[0].legend()
- axs1[1].plot(t, X_plot[2, 0:i + 1], label='steer_velocity0')
- axs1[1].plot(t, X_plot[6, 0:i + 1], label='steer_velocity1')
- axs1[1].plot(t, X_plot[10, 0:i + 1], label='steer_velocity2')
- axs1[1].plot(t, X_plot[14, 0:i + 1], label='steer_velocity3')
+ axs1[1].plot(solver.t,
+ solver.X_plot[dynamics.STATE_OMEGAS0, :],
+ label='steer_velocity0')
+ axs1[1].plot(solver.t,
+ solver.X_plot[dynamics.STATE_OMEGAS1, :],
+ label='steer_velocity1')
+ axs1[1].plot(solver.t,
+ solver.X_plot[dynamics.STATE_OMEGAS2, :],
+ label='steer_velocity2')
+ axs1[1].plot(solver.t,
+ solver.X_plot[dynamics.STATE_OMEGAS3, :],
+ label='steer_velocity3')
axs1[1].legend()
- pyplot.pause(0.0001)
- last_time = time.time()
-print(f"Tool {overall_time} seconds overall to solve.")
+ fig0.savefig('state.svg')
+ fig1.savefig('steer.svg')
-pyplot.pause(-1)
+
+if __name__ == '__main__':
+ app.run(main)
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index 19b4f12..3c4eaf6 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -35,6 +35,7 @@
ABSL_FLAG(double, caster, 0.01, "Caster in meters for the module.");
ABSL_FLAG(bool, symbolic, false, "If true, write everything out symbolically.");
+ABSL_FLAG(bool, function, true, "If true, make soft_atan2 a function.");
using SymEngine::abs;
using SymEngine::add;
@@ -404,7 +405,11 @@
result_py->emplace_back(" sin = casadi.sin");
result_py->emplace_back(" cos = casadi.cos");
result_py->emplace_back(" exp = casadi.exp");
- result_py->emplace_back(" atan2 = soft_atan2");
+ if (absl::GetFlag(FLAGS_function)) {
+ result_py->emplace_back(" atan2 = soft_atan2()");
+ } else {
+ result_py->emplace_back(" atan2 = soft_atan2");
+ }
result_py->emplace_back(" fmax = casadi.fmax");
result_py->emplace_back(" fabs = casadi.fabs");
@@ -448,7 +453,11 @@
result_py->emplace_back(" sin = casadi.sin");
result_py->emplace_back(" exp = casadi.exp");
result_py->emplace_back(" cos = casadi.cos");
- result_py->emplace_back(" atan2 = soft_atan2");
+ if (absl::GetFlag(FLAGS_function)) {
+ result_py->emplace_back(" atan2 = soft_atan2()");
+ } else {
+ result_py->emplace_back(" atan2 = soft_atan2");
+ }
result_py->emplace_back(" fmax = casadi.fmax");
result_py->emplace_back(" fabs = casadi.fabs");
@@ -495,7 +504,7 @@
std::vector<std::string> result_py;
// Write out the header.
- result_py.emplace_back("#!/usr/bin/python3");
+ result_py.emplace_back("#!/usr/bin/env python3");
result_py.emplace_back("");
result_py.emplace_back("import casadi, numpy");
result_py.emplace_back("");
@@ -567,12 +576,46 @@
result_py.emplace_back(" ])");
result_py.emplace_back("");
constexpr double kLogGain = 1.0 / 0.05;
- result_py.emplace_back("def soft_atan2(y, x):");
- result_py.emplace_back(" return casadi.arctan2(");
- result_py.emplace_back(" y,");
- result_py.emplace_back(" casadi.logsumexp(casadi.SX(numpy.array(");
+ constexpr double kAbsGain = 1.0 / 0.01;
+ if (absl::GetFlag(FLAGS_function)) {
+ result_py.emplace_back("def soft_atan2():");
+ result_py.emplace_back(" y = casadi.SX.sym('y')");
+ result_py.emplace_back(" x = casadi.SX.sym('x')");
+ result_py.emplace_back(
+ " return casadi.Function('soft_atan2', [y, x], [");
+ result_py.emplace_back(" casadi.arctan2(");
+ result_py.emplace_back(" y,");
+ result_py.emplace_back(" casadi.logsumexp(");
+ result_py.emplace_back(" casadi.SX(");
+ result_py.emplace_back(" numpy.array([");
+ result_py.emplace_back(" 1.0, x * (1.0 - 2.0 /");
+ result_py.emplace_back(
+ absl::Substitute(" (1 + "
+ "casadi.exp($1.0 * x))) * $0.0",
+ kLogGain, kAbsGain));
+ result_py.emplace_back(
+ absl::Substitute(" ]))) / $0.0)", kLogGain));
+ result_py.emplace_back(" ])");
+ } else {
+ result_py.emplace_back("def soft_atan2(y, x):");
+ result_py.emplace_back(" return casadi.arctan2(");
+ result_py.emplace_back(" y,");
+ result_py.emplace_back(" casadi.logsumexp(casadi.SX(numpy.array(");
+ result_py.emplace_back(
+ absl::Substitute(" [1.0, x * (1.0 - 2.0 / (1 + "
+ "casadi.exp($1.0 * x))) * $0.0]))) / $0.0)",
+ kLogGain, kAbsGain));
+ }
+ result_py.emplace_back("");
+ result_py.emplace_back("# Is = STEER_CURRENT_COUPLING_FACTOR * Id");
result_py.emplace_back(absl::Substitute(
- " [1.0, casadi.fabs(x) * $0.0]))) / $0.0)", kLogGain));
+ "STEER_CURRENT_COUPLING_FACTOR = $0",
+ ccode(*(neg(
+ mul(div(Gs_, Kts_),
+ mul(div(Ktd_, mul(Gd_, rw_)),
+ neg(mul(add(neg(wb_), mul(add(rs_, rp_),
+ sub(integer(1), div(rb1_, rp_)))),
+ div(rw_, rb2_))))))))));
result_py.emplace_back("");
result_py.emplace_back("# Is = STEER_CURRENT_COUPLING_FACTOR * Id");
result_py.emplace_back(absl::Substitute(
diff --git a/frc971/control_loops/swerve/multi_casadi_velocity_mpc.py b/frc971/control_loops/swerve/multi_casadi_velocity_mpc.py
new file mode 100644
index 0000000..8474108
--- /dev/null
+++ b/frc971/control_loops/swerve/multi_casadi_velocity_mpc.py
@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+from absl import app
+from frc971.control_loops.swerve import dynamics
+from absl import flags
+from matplotlib import pylab
+import matplotlib
+import sys, os, pickle
+from multiprocessing.pool import ThreadPool
+import numpy
+import pathlib, collections
+import subprocess
+import itertools
+import threading
+
+matplotlib.use("GTK3Agg")
+
+FLAGS = flags.FLAGS
+flags.DEFINE_string('outdir', '/tmp/swerve', "Directory to write results to.")
+
+workerid_lock = threading.Lock()
+workerid_global = 0
+workerid = threading.local()
+
+
+def set_workerid():
+ global workerid_global
+ with workerid_lock:
+ workerid.v = workerid_global
+ workerid_global += 1
+ print(f'Starting worker {workerid.v}')
+
+
+Object = lambda **kwargs: type("Object", (), kwargs)
+
+
+def solve_mpc(velocity):
+ filename = f'vx{velocity[0]}vy{velocity[1]}omega{velocity[2]}'
+ if FLAGS.outdir:
+ subdir = pathlib.Path(FLAGS.outdir) / filename
+ else:
+ subdir = pathlib.Path(filename)
+ subdir.mkdir(parents=True, exist_ok=True)
+
+ subprocess.check_call(args=[
+ sys.executable,
+ "frc971/control_loops/swerve/casadi_velocity_mpc",
+ f"--vx={velocity[0]}",
+ f"--vy={velocity[1]}",
+ f"--omega={velocity[2]}",
+ f"--outputdir={subdir.resolve()}",
+ "--pickle",
+ ])
+
+ with open(subdir / 't.pkl', 'rb') as f:
+ t = pickle.load(f)
+
+ with open(subdir / 'X_plot.pkl', 'rb') as f:
+ X_plot = pickle.load(f)
+
+ with open(subdir / 'U_plot.pkl', 'rb') as f:
+ U_plot = pickle.load(f)
+
+ return Object(t=t, X_plot=X_plot, U_plot=U_plot)
+
+
+def main(argv):
+ # Load a simple problem first so we compile with less system load. This
+ # makes it faster on a processor with frequency boosting.
+ subprocess.check_call(args=[
+ sys.executable,
+ "frc971/control_loops/swerve/casadi_velocity_mpc",
+ "--compileonly",
+ ])
+
+ # Try a bunch of goals now
+ vxy = numpy.array(
+ numpy.meshgrid(numpy.linspace(-1, 1, 9),
+ numpy.linspace(-1, 1, 9))).T.reshape(-1, 2)
+
+ velocity = numpy.hstack((vxy, numpy.zeros((vxy.shape[0], 1))))
+
+ with ThreadPool(initializer=set_workerid) as pool:
+ results = pool.starmap(solve_mpc, zip(velocity, ))
+
+ fig0, axs0 = pylab.subplots(2)
+ for r in results:
+ axs0[0].plot(r.X_plot[dynamics.STATE_VX, :],
+ r.X_plot[dynamics.STATE_VY, :],
+ label='trajectory')
+ axs0[1].plot(r.t, r.U_plot[0, :], label="Is0")
+ axs0[1].plot(r.t, r.U_plot[1, :], label="Id0")
+
+ axs0[0].legend()
+ axs0[1].legend()
+ pylab.show()
+
+
+if __name__ == '__main__':
+ app.run(main)
diff --git a/frc971/control_loops/swerve/smooth_function_graph.py b/frc971/control_loops/swerve/smooth_function_graph.py
new file mode 100644
index 0000000..570b3a6
--- /dev/null
+++ b/frc971/control_loops/swerve/smooth_function_graph.py
@@ -0,0 +1,111 @@
+#!/usr/bin/python3
+
+import numpy
+import scipy
+import matplotlib.pyplot as plt
+import matplotlib
+from scipy.special import logsumexp
+
+x = numpy.linspace(-10, 10, 1000)
+y0 = numpy.zeros((1000, 1))
+y1 = numpy.zeros((1000, 1))
+
+# add more detail near 0
+X = numpy.sort(
+ numpy.hstack(
+ [numpy.arange(-1, 1, 0.005),
+ numpy.arange(-0.005, 0.005, 0.0001)]))
+Y = numpy.sort(
+ numpy.hstack(
+ [numpy.arange(-1, 1, 0.005),
+ numpy.arange(-0.005, 0.005, 0.0001)]))
+X, Y = numpy.meshgrid(X, Y)
+
+
+def single_slip_force(vy, vx):
+ return numpy.arctan2(vy, numpy.abs(vx))
+
+
+def single_new_slip_force(vy, vx):
+ loggain = 1 / 0.05
+ return numpy.arctan2(vy,
+ logsumexp([1.0, numpy.abs(vx) * loggain]) / loggain)
+
+
+def single_new_new_slip_force(vy, vx):
+ loggain = 1 / 0.05
+ loggain2 = 1 / 0.05
+ return numpy.arctan2(
+ vy,
+ logsumexp(
+ [1.0, vx * (1.0 - 2.0 /
+ (1 + numpy.exp(loggain2 * vx))) * loggain]) / loggain)
+
+
+velocity = 0.1
+
+acc_val = single_new_new_slip_force(velocity, velocity)
+expt_val = single_new_slip_force(velocity, velocity)
+
+print("Percent Error: ", (acc_val - expt_val) / expt_val * 100)
+
+slip_force = scipy.vectorize(single_slip_force)
+new_slip_force = scipy.vectorize(single_new_slip_force)
+new_new_slip_force = scipy.vectorize(single_new_new_slip_force)
+
+Y0 = slip_force(Y, X)
+Y1 = new_slip_force(Y, X)
+Y2 = new_new_slip_force(Y, X)
+Y3 = Y2 - Y1
+
+matplotlib.rcParams['figure.figsize'] = (15, 15)
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y0,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('atan2(y, x)')
+fig.suptitle("Atan2")
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y1,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('atan2(y, x)')
+fig.suptitle("Softened y atan2")
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y2,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('atan2(y, x)')
+fig.suptitle("Softened x and y atan2")
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y3,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('Error')
+fig.suptitle("Error between Soft_atan2 and the new one")
+
+plt.show()
diff --git a/third_party/python/matplotlib/init.patch b/third_party/python/matplotlib/init.patch
index 8911145..6fc460a 100644
--- a/third_party/python/matplotlib/init.patch
+++ b/third_party/python/matplotlib/init.patch
@@ -36,7 +36,7 @@
+ gtk_runtime_base = os.path.join(runfiles_dir, "gtk_runtime")
+
+ # Tell fontconfig where to find matplotlib's sandboxed font files.
-+ os.environ["FONTCONFIG_PATH"] = os.path.join(gtk_runtime_base, "etc/fonts")
++ os.environ["FONTCONFIG_PATH"] = "etc/fonts"
+ os.environ["FONTCONFIG_FILE"] = "fonts.conf"
+ os.environ["FONTCONFIG_SYSROOT"] = gtk_runtime_base
+
diff --git a/third_party/rules_python/0001-Support-overriding-individual-packages.patch b/third_party/rules_python/0001-Support-overriding-individual-packages.patch
index af89ecc..6f01b1b 100644
--- a/third_party/rules_python/0001-Support-overriding-individual-packages.patch
+++ b/third_party/rules_python/0001-Support-overriding-individual-packages.patch
@@ -1,16 +1,16 @@
-From 662f59afaecd7ecff5bd5234c8bbd9c219b7f24f Mon Sep 17 00:00:00 2001
+From b9b8deb69a6c53a0d688988161cd057e6f94881b Mon Sep 17 00:00:00 2001
From: Philipp Schrader <philipp.schrader@gmail.com>
Date: Sun, 11 Sep 2022 22:04:47 -0700
Subject: [PATCH] Support overriding individual packages
---
.../extract_wheels/extract_single_wheel.py | 60 ++++++++++---------
- .../parse_requirements_to_bzl.py | 44 +++++++++++++-
+ .../parse_requirements_to_bzl.py | 51 +++++++++++++++-
python/pip_install/pip_repository.bzl | 38 ++++++++++++
- 3 files changed, 114 insertions(+), 28 deletions(-)
+ 3 files changed, 121 insertions(+), 28 deletions(-)
diff --git a/python/pip_install/extract_wheels/extract_single_wheel.py b/python/pip_install/extract_wheels/extract_single_wheel.py
-index ff64291..8742d25 100644
+index ff642910..8742d250 100644
--- a/python/pip_install/extract_wheels/extract_single_wheel.py
+++ b/python/pip_install/extract_wheels/extract_single_wheel.py
@@ -50,41 +50,47 @@ def main() -> None:
@@ -89,7 +89,7 @@
name, extras_for_pkg = requirements._parse_requirement_for_extra(args.requirement)
extras = {name: extras_for_pkg} if extras_for_pkg and name else dict()
diff --git a/python/pip_install/extract_wheels/parse_requirements_to_bzl.py b/python/pip_install/extract_wheels/parse_requirements_to_bzl.py
-index 686a57d..60936a9 100644
+index 686a57d8..002e6857 100644
--- a/python/pip_install/extract_wheels/parse_requirements_to_bzl.py
+++ b/python/pip_install/extract_wheels/parse_requirements_to_bzl.py
@@ -4,7 +4,7 @@ import shlex
@@ -125,7 +125,7 @@
for name, requirement in _packages:
+ override_entry = requirement.split(" ")[0]
+ override_name, _, version = override_entry.partition("==")
-+ override_key = "%s==%s" % (_clean_name(override_name), version)
++ override_key = "%s==%s" % (_clean_extras(_clean_name(override_name)), version)
+ override = _overrides.get(override_key)
+ if not override:
+ if _require_overrides:
@@ -142,7 +142,7 @@
**whl_config
)
"""
-@@ -154,6 +170,13 @@ def generate_parsed_requirements_contents(
+@@ -154,10 +170,24 @@ def generate_parsed_requirements_contents(
_config = {args}
_annotations = {annotations}
_bzlmod = {bzlmod}
@@ -156,7 +156,18 @@
def _clean_name(name):
return name.replace("-", "_").replace(".", "_").lower()
-@@ -204,6 +227,8 @@ def generate_parsed_requirements_contents(
+
++ def _clean_extras(name):
++ bracket_start = name.find("[")
++ bracket_end = name.find("]")
++ if bracket_start == -1 or bracket_end == -1:
++ return name
++ return name[:bracket_start] + name[bracket_end + 1:]
++
+ def requirement(name):
+ if _bzlmod:
+ return "@@{repo}//:" + _clean_name(name) + "_{py_library_label}"
+@@ -204,6 +234,8 @@ def generate_parsed_requirements_contents(
repo_prefix=repo_prefix,
wheel_file_label=bazel.WHEEL_FILE_LABEL,
bzlmod=bzlmod,
@@ -165,7 +176,7 @@
)
)
-@@ -266,6 +291,16 @@ If set, it will take precedence over python_interpreter.",
+@@ -266,6 +298,16 @@ If set, it will take precedence over python_interpreter.",
default=False,
help="Whether this script is run under bzlmod. Under bzlmod we don't generate the install_deps() macro as it isn't needed.",
)
@@ -182,7 +193,7 @@
arguments.parse_common_args(parser)
args = parser.parse_args()
-@@ -291,6 +326,11 @@ If set, it will take precedence over python_interpreter.",
+@@ -291,6 +333,11 @@ If set, it will take precedence over python_interpreter.",
}
)
@@ -194,7 +205,7 @@
output.write(
textwrap.dedent(
"""\
-@@ -313,6 +353,8 @@ If set, it will take precedence over python_interpreter.",
+@@ -313,6 +360,8 @@ If set, it will take precedence over python_interpreter.",
whl_library_args=whl_library_args,
annotations=annotated_requirements,
bzlmod=args.bzlmod,
@@ -204,7 +215,7 @@
)
diff --git a/python/pip_install/pip_repository.bzl b/python/pip_install/pip_repository.bzl
-index 7fbf503..5af0731 100644
+index 7fbf5039..5af07315 100644
--- a/python/pip_install/pip_repository.bzl
+++ b/python/pip_install/pip_repository.bzl
@@ -322,6 +322,11 @@ def _pip_repository_impl(rctx):
diff --git a/third_party/rules_python/0002-Allow-user-to-patch-wheels.patch b/third_party/rules_python/0002-Allow-user-to-patch-wheels.patch
index 9db2d9e..3669ca5 100644
--- a/third_party/rules_python/0002-Allow-user-to-patch-wheels.patch
+++ b/third_party/rules_python/0002-Allow-user-to-patch-wheels.patch
@@ -1,4 +1,4 @@
-From f828c9aad94b56655b352c4bed9b475d7430865e Mon Sep 17 00:00:00 2001
+From e9002bef44df84370649b995b8c9e6a89d4d37e9 Mon Sep 17 00:00:00 2001
From: Philipp Schrader <philipp.schrader@gmail.com>
Date: Sat, 24 Sep 2022 15:56:33 -0700
Subject: [PATCH] Allow user to patch wheels
@@ -25,7 +25,7 @@
5 files changed, 100 insertions(+), 3 deletions(-)
diff --git a/python/pip_install/extract_wheels/annotation.py b/python/pip_install/extract_wheels/annotation.py
-index 48aaa80..fe8b4dc 100644
+index 48aaa802..fe8b4dc5 100644
--- a/python/pip_install/extract_wheels/annotation.py
+++ b/python/pip_install/extract_wheels/annotation.py
@@ -19,6 +19,7 @@ class Annotation(OrderedDict):
@@ -48,7 +48,7 @@
class AnnotationsMap:
"""A mapping of python package names to [Annotation]"""
diff --git a/python/pip_install/extract_wheels/bazel.py b/python/pip_install/extract_wheels/bazel.py
-index 8f442c9..f4b7f26 100644
+index 8f442c93..f4b7f26a 100644
--- a/python/pip_install/extract_wheels/bazel.py
+++ b/python/pip_install/extract_wheels/bazel.py
@@ -2,6 +2,7 @@
@@ -107,7 +107,7 @@
os.remove(whl.path)
return f"//{directory}"
diff --git a/python/pip_install/extract_wheels/extract_single_wheel.py b/python/pip_install/extract_wheels/extract_single_wheel.py
-index 8742d25..50a1243 100644
+index 8742d250..50a1243e 100644
--- a/python/pip_install/extract_wheels/extract_single_wheel.py
+++ b/python/pip_install/extract_wheels/extract_single_wheel.py
@@ -1,4 +1,5 @@
@@ -166,7 +166,7 @@
diff --git a/python/pip_install/extract_wheels/parse_requirements_to_bzl.py b/python/pip_install/extract_wheels/parse_requirements_to_bzl.py
-index 60936a9..3dd179b 100644
+index 002e6857..fc7fe780 100644
--- a/python/pip_install/extract_wheels/parse_requirements_to_bzl.py
+++ b/python/pip_install/extract_wheels/parse_requirements_to_bzl.py
@@ -88,6 +88,7 @@ def parse_whl_library_args(args: argparse.Namespace) -> Dict[str, Any]:
@@ -214,7 +214,7 @@
_NOP_OVERRIDE = {{
"url": None,
-@@ -229,6 +238,7 @@ def generate_parsed_requirements_contents(
+@@ -236,6 +245,7 @@ def generate_parsed_requirements_contents(
bzlmod=bzlmod,
overrides=overrides or {},
require_overrides=require_overrides,
@@ -222,7 +222,7 @@
)
)
-@@ -301,6 +311,13 @@ If set, it will take precedence over python_interpreter.",
+@@ -308,6 +318,13 @@ If set, it will take precedence over python_interpreter.",
action="store_true",
help="If set, requires that every requirement has a URL override in the --overrides JSON file.",
)
@@ -236,7 +236,7 @@
arguments.parse_common_args(parser)
args = parser.parse_args()
-@@ -331,6 +348,11 @@ If set, it will take precedence over python_interpreter.",
+@@ -338,6 +355,11 @@ If set, it will take precedence over python_interpreter.",
else:
overrides = None
@@ -248,7 +248,7 @@
output.write(
textwrap.dedent(
"""\
-@@ -355,6 +377,7 @@ If set, it will take precedence over python_interpreter.",
+@@ -362,6 +384,7 @@ If set, it will take precedence over python_interpreter.",
bzlmod=args.bzlmod,
overrides=overrides,
require_overrides=args.require_overrides,
@@ -257,7 +257,7 @@
)
diff --git a/python/pip_install/pip_repository.bzl b/python/pip_install/pip_repository.bzl
-index 5af0731..bf7f99a 100644
+index 5af07315..bf7f99a8 100644
--- a/python/pip_install/pip_repository.bzl
+++ b/python/pip_install/pip_repository.bzl
@@ -327,6 +327,9 @@ def _pip_repository_impl(rctx):
diff --git a/tools/python/requirements.lock.txt b/tools/python/requirements.lock.txt
index d991d66..0bb13ca 100644
--- a/tools/python/requirements.lock.txt
+++ b/tools/python/requirements.lock.txt
@@ -4,6 +4,10 @@
#
# bazel run //tools/python:requirements.update
#
+absl-py==2.1.0 \
+ --hash=sha256:526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308 \
+ --hash=sha256:7820790efbb316739cde8b4e19357243fc3608a152024288513dd968d7d959ff
+ # via -r tools/python/requirements.txt
bokeh==3.4.1 \
--hash=sha256:1e3c502a0a8205338fc74dadbfa321f8a0965441b39501e36796a47b4017b642 \
--hash=sha256:d824961e4265367b0750ce58b07e564ad0b83ca64b335521cd3421e9b9f10d89
@@ -323,35 +327,32 @@
--hash=sha256:7a28eb2a9774d00c7bc92411c19a89209d5da7c4c9a9e227be8330a23a25b91f \
--hash=sha256:a0b2b9fe80bbcd81a6647ff13108738cfb482d481d826cc0e02f5b35e5c88d2c
# via sympy
-numpy==1.23.5 \
- --hash=sha256:01dd17cbb340bf0fc23981e52e1d18a9d4050792e8fb8363cecbf066a84b827d \
- --hash=sha256:06005a2ef6014e9956c09ba07654f9837d9e26696a0470e42beedadb78c11b07 \
- --hash=sha256:09b7847f7e83ca37c6e627682f145856de331049013853f344f37b0c9690e3df \
- --hash=sha256:0aaee12d8883552fadfc41e96b4c82ee7d794949e2a7c3b3a7201e968c7ecab9 \
- --hash=sha256:0cbe9848fad08baf71de1a39e12d1b6310f1d5b2d0ea4de051058e6e1076852d \
- --hash=sha256:1b1766d6f397c18153d40015ddfc79ddb715cabadc04d2d228d4e5a8bc4ded1a \
- --hash=sha256:33161613d2269025873025b33e879825ec7b1d831317e68f4f2f0f84ed14c719 \
- --hash=sha256:5039f55555e1eab31124a5768898c9e22c25a65c1e0037f4d7c495a45778c9f2 \
- --hash=sha256:522e26bbf6377e4d76403826ed689c295b0b238f46c28a7251ab94716da0b280 \
- --hash=sha256:56e454c7833e94ec9769fa0f86e6ff8e42ee38ce0ce1fa4cbb747ea7e06d56aa \
- --hash=sha256:58f545efd1108e647604a1b5aa809591ccd2540f468a880bedb97247e72db387 \
- --hash=sha256:5e05b1c973a9f858c74367553e236f287e749465f773328c8ef31abe18f691e1 \
- --hash=sha256:7903ba8ab592b82014713c491f6c5d3a1cde5b4a3bf116404e08f5b52f6daf43 \
- --hash=sha256:8969bfd28e85c81f3f94eb4a66bc2cf1dbdc5c18efc320af34bffc54d6b1e38f \
- --hash=sha256:92c8c1e89a1f5028a4c6d9e3ccbe311b6ba53694811269b992c0b224269e2398 \
- --hash=sha256:9c88793f78fca17da0145455f0d7826bcb9f37da4764af27ac945488116efe63 \
- --hash=sha256:a7ac231a08bb37f852849bbb387a20a57574a97cfc7b6cabb488a4fc8be176de \
- --hash=sha256:abdde9f795cf292fb9651ed48185503a2ff29be87770c3b8e2a14b0cd7aa16f8 \
- --hash=sha256:af1da88f6bc3d2338ebbf0e22fe487821ea4d8e89053e25fa59d1d79786e7481 \
- --hash=sha256:b2a9ab7c279c91974f756c84c365a669a887efa287365a8e2c418f8b3ba73fb0 \
- --hash=sha256:bf837dc63ba5c06dc8797c398db1e223a466c7ece27a1f7b5232ba3466aafe3d \
- --hash=sha256:ca51fcfcc5f9354c45f400059e88bc09215fb71a48d3768fb80e357f3b457e1e \
- --hash=sha256:ce571367b6dfe60af04e04a1834ca2dc5f46004ac1cc756fb95319f64c095a96 \
- --hash=sha256:d208a0f8729f3fb790ed18a003f3a57895b989b40ea4dce4717e9cf4af62c6bb \
- --hash=sha256:dbee87b469018961d1ad79b1a5d50c0ae850000b639bcb1b694e9981083243b6 \
- --hash=sha256:e9f4c4e51567b616be64e05d517c79a8a22f3606499941d97bb76f2ca59f982d \
- --hash=sha256:f063b69b090c9d918f9df0a12116029e274daf0181df392839661c4c7ec9018a \
- --hash=sha256:f9a909a8bae284d46bbfdefbdd4a262ba19d3bc9921b1e76126b1d21c3c34135
+numpy==1.25.2 \
+ --hash=sha256:0d60fbae8e0019865fc4784745814cff1c421df5afee233db6d88ab4f14655a2 \
+ --hash=sha256:1a1329e26f46230bf77b02cc19e900db9b52f398d6722ca853349a782d4cff55 \
+ --hash=sha256:1b9735c27cea5d995496f46a8b1cd7b408b3f34b6d50459d9ac8fe3a20cc17bf \
+ --hash=sha256:2792d23d62ec51e50ce4d4b7d73de8f67a2fd3ea710dcbc8563a51a03fb07b01 \
+ --hash=sha256:3e0746410e73384e70d286f93abf2520035250aad8c5714240b0492a7302fdca \
+ --hash=sha256:4c3abc71e8b6edba80a01a52e66d83c5d14433cbcd26a40c329ec7ed09f37901 \
+ --hash=sha256:5883c06bb92f2e6c8181df7b39971a5fb436288db58b5a1c3967702d4278691d \
+ --hash=sha256:5c97325a0ba6f9d041feb9390924614b60b99209a71a69c876f71052521d42a4 \
+ --hash=sha256:60e7f0f7f6d0eee8364b9a6304c2845b9c491ac706048c7e8cf47b83123b8dbf \
+ --hash=sha256:76b4115d42a7dfc5d485d358728cdd8719be33cc5ec6ec08632a5d6fca2ed380 \
+ --hash=sha256:7dc869c0c75988e1c693d0e2d5b26034644399dd929bc049db55395b1379e044 \
+ --hash=sha256:834b386f2b8210dca38c71a6e0f4fd6922f7d3fcff935dbe3a570945acb1b545 \
+ --hash=sha256:8b77775f4b7df768967a7c8b3567e309f617dd5e99aeb886fa14dc1a0791141f \
+ --hash=sha256:90319e4f002795ccfc9050110bbbaa16c944b1c37c0baeea43c5fb881693ae1f \
+ --hash=sha256:b79e513d7aac42ae918db3ad1341a015488530d0bb2a6abcbdd10a3a829ccfd3 \
+ --hash=sha256:bb33d5a1cf360304754913a350edda36d5b8c5331a8237268c48f91253c3a364 \
+ --hash=sha256:bec1e7213c7cb00d67093247f8c4db156fd03075f49876957dca4711306d39c9 \
+ --hash=sha256:c5462d19336db4560041517dbb7759c21d181a67cb01b36ca109b2ae37d32418 \
+ --hash=sha256:c5652ea24d33585ea39eb6a6a15dac87a1206a692719ff45d53c5282e66d4a8f \
+ --hash=sha256:d7806500e4f5bdd04095e849265e55de20d8cc4b661b038957354327f6d9b295 \
+ --hash=sha256:db3ccc4e37a6873045580d413fe79b68e47a681af8db2e046f1dacfa11f86eb3 \
+ --hash=sha256:dfe4a913e29b418d096e696ddd422d8a5d13ffba4ea91f9f60440a3b759b0187 \
+ --hash=sha256:eb942bfb6f84df5ce05dbf4b46673ffed0d3da59f13635ea9b926af3deb76926 \
+ --hash=sha256:f08f2e037bba04e707eebf4bc934f1972a315c883a9e0ebfa8a7756eabf9e357 \
+ --hash=sha256:fd608e19c8d7c55021dffd43bfe5492fab8cc105cc8986f813f8c3c048b38760
# via
# -r tools/python/requirements.txt
# bokeh
@@ -503,9 +504,9 @@
# via
# bokeh
# matplotlib
-pkginfo==1.8.3 \
- --hash=sha256:848865108ec99d4901b2f7e84058b6e7660aae8ae10164e015a6dcf5b242a594 \
- --hash=sha256:a84da4318dd86f870a9447a8c98340aa06216bfc6f2b7bdc4b8766984ae1867c
+pkginfo==1.11.1 \
+ --hash=sha256:2e0dca1cf4c8e39644eed32408ea9966ee15e0d324c62ba899a393b3c6b467aa \
+ --hash=sha256:bfa76a714fdfc18a045fcd684dbfc3816b603d9d075febef17cb6582bea29573
# via -r tools/python/requirements.txt
pycairo==1.22.0 \
--hash=sha256:007ae728c56b9a0962d8c5513ae967a4fceff03e022940383c20f4f3d4c48dbe \
diff --git a/tools/python/requirements.txt b/tools/python/requirements.txt
index 48b58ff..db0ec26 100644
--- a/tools/python/requirements.txt
+++ b/tools/python/requirements.txt
@@ -15,6 +15,7 @@
validators
yapf
sympy
+absl-py
pyyaml
# TODO(phil): Migrate to absl-py. These are abandoned as far as I can tell.
diff --git a/tools/python/whl_overrides.json b/tools/python/whl_overrides.json
index ccb50a1..4de8223 100644
--- a/tools/python/whl_overrides.json
+++ b/tools/python/whl_overrides.json
@@ -1,4 +1,8 @@
{
+ "absl_py==2.1.0": {
+ "sha256": "526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308",
+ "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/absl_py-2.1.0-py3-none-any.whl"
+ },
"bokeh==3.4.1": {
"sha256": "1e3c502a0a8205338fc74dadbfa321f8a0965441b39501e36796a47b4017b642",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/bokeh-3.4.1-py3-none-any.whl"
@@ -79,9 +83,9 @@
"sha256": "a0b2b9fe80bbcd81a6647ff13108738cfb482d481d826cc0e02f5b35e5c88d2c",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/mpmath-1.3.0-py3-none-any.whl"
},
- "numpy==1.23.5": {
- "sha256": "33161613d2269025873025b33e879825ec7b1d831317e68f4f2f0f84ed14c719",
- "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/numpy-1.23.5-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl"
+ "numpy==1.25.2": {
+ "sha256": "d7806500e4f5bdd04095e849265e55de20d8cc4b661b038957354327f6d9b295",
+ "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/numpy-1.25.2-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl"
},
"opencv_python==4.6.0.66": {
"sha256": "dbdc84a9b4ea2cbae33861652d25093944b9959279200b7ae0badd32439f74de",
@@ -103,9 +107,9 @@
"sha256": "97aabc5c50312afa5e0a2b07c17d4ac5e865b250986f8afe2b02d772567a380c",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/Pillow-9.3.0-cp39-cp39-manylinux_2_28_x86_64.whl"
},
- "pkginfo==1.8.3": {
- "sha256": "848865108ec99d4901b2f7e84058b6e7660aae8ae10164e015a6dcf5b242a594",
- "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/pkginfo-1.8.3-py2.py3-none-any.whl"
+ "pkginfo==1.11.1": {
+ "sha256": "bfa76a714fdfc18a045fcd684dbfc3816b603d9d075febef17cb6582bea29573",
+ "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/pkginfo-1.11.1-py3-none-any.whl"
},
"pycairo==1.22.0": {
"sha256": "451b9f68e45b9f9cae5069cd6eab44ad339ae55cf177be904c0fab6a55228b85",
diff --git a/vm/README.md b/vm/README.md
deleted file mode 100644
index ee9d9aa..0000000
--- a/vm/README.md
+++ /dev/null
@@ -1,180 +0,0 @@
-Setting up a Virtual Machine (VM)
-================================================================================
-This document tries to document three ways in which you can set up a
-development environment. The third is essentially the same as the second so
-this document will just focus on the first and second way.
-
-1. Use the vagrant scripts to automatically create everything.
-2. Create a VM manually and set it up manually.
-3. Install Debian natively and set it up manually.
-
-Using the vagrant scripts requires more setup, but generally is more hands-off.
-Setting up a VM manually can be more rewarding and interesting, but requires
-more manual steps and generally takes longer.
-
-Command line knowledge
---------------------------------------------------------------------------------
-Some basic knowledge of using your terminal is required. On Windows, cmd.exe is
-a good start. I would recommend setting up git-bash because it will resemble
-the environment inside the VM much more closely than cmd.exe.
-
-Whenever you see `$` in a code segment below, please type that into your
-terminal. `$` is just a generic representation of what we call the "command
-prompt". On Windows' cmd.exe it would look something like this:
-
- C:\Users\YourName>
-
-On a UNIX-like Operating System (e.g. Linux, OSX) it may look more like this:
-
- yourname@hostname ~ $
-
-The `$` is just a shortcut to represent any style of prompt. When you see
-something like `$ echo Hello` below please type everything after the `$ ` into
-your terminal. In this case that means typing "echo Hello" and pressing Enter.
-
-Access to the Gerrit repository
---------------------------------------------------------------------------------
-In order to use the setup scripts you'll need access to the 971-Robot-Code
-repository on gerrit:
-<https://robotics.mvla.net/gerrit/#/admin/projects/971-Robot-Code>
-
-Please ask your mentors about getting access. If you already have access,
-great!
-
-In general I recommend setting up an SSH key to pull from and push to the
-repository. You can also use HTTPS-based passwords, but it's a little more
-annoying to use in the long term. See the "How to Generate an SSH key" section
-on [gerrit's SSH page](https://robotics.mvla.net/gerrit/#/settings/ssh-keys)
-for more details.
-
-
-Using Vagrant
-================================================================================
-
-Requirements
---------------------------------------------------------------------------------
-These requirements apply to all Operating Systems. The individual setup steps
-may differ. For example, on Debian you can run `apt-get install virtualbox`
-whereas on Windows and OSX you have to download a dedicated installer.
-
-1. Basic knowledge of the command line. See the "Command line knowledge"
- section above.
-
-2. Install Vagrant <https://www.vagrantup.com/downloads.html>
- - Please install this from the website as anything in, say, an apt repo
- would be quite outdated.
-
-3. Install VirtualBox <https://www.virtualbox.org/wiki/Downloads>
- - On Debian you should install this via `apt-get` since it's integrated
- really well. On another OS please use the downloaded installer.
-
-4. Install git <https://git-scm.com/downloads>
- - On Debian you should install this via `apt-get`. On another OS please use
- the downloaded installer.
- - On Windows, I would recommend installing something called "git-bash" at
- the same time. It will provide you with a better terminal experience than
- `cmd.exe` to perform the majority of the remaining steps.
-
-5. Add `vagrant`, `VBoxManage`, and `git` to your PATH.
- - This is most likely already done by the installation binaries.
- It's added to the system path.
- - To test this, type these commands in a terminal:
-
- $ vagrant --version
- Vagrant 1.8.1
- $ VBoxManage --version
- 5.0.14r105127
- $ git --version
- git version 2.11.0
-
- - You may need to log out and back in for the PATH modifications to take
- effect.
-
-Usage
---------------------------------------------------------------------------------
-1. Check this folder out on your computer somewhere.
-
- $ git clone ssh://USERNAME@robotics.mvla.net:29418/971-Robot-Code
-
- where you replace `USERNAME` with your own username. Keep in mind that you
- need your SVN and Gerrit account set up for this to work. Ask the mentors or
- other students about this if you don't have one yet.
-
-2. Go into the directory and build the VM.
-
- $ cd 971-Robot-Code/vm/
- $ vagrant up
-
- Some errors during the `vagrant up` process can be addressed by
- re-provisioning the vagrant box. This is useful if, for example, an
- `apt-get` invocation timed out and caused the provisioning process to abort.
-
- $ vagrant provision
-
-3. Once built, reboot the VM so it starts the GUI properly.
-
- $ vagrant reload
-
-4. You can then log in and open a terminal. The username and password are both
- `user`.
-
-5. Download the code.
-
- $ git clone https://USERNAME@robotics.mvla.net/gerrit/971-Robot-Code
- $ cd 971-Robot-Code
-
-6. Build the code.
-
- $ bazel build //y2017/...
-
- Replace `//y2017` with the appropriate year's folder. For 2018 the build
- target would be `//y2018` for example.
-
-
-Setting up a VM manually
-================================================================================
-This section is lacking a lot of detail, but that's largely because you can
-find most of the information on other websites in a lot more detail.
-
-Requirements
---------------------------------------------------------------------------------
-1. Basic knowledge of the command line. See the "Command line knowledge"
- section above.
-
-2. Install VirtualBox <https://www.virtualbox.org/wiki/Downloads>
-
- See the details from the "Using Vagrant" section above.
-
-3. Download a Debian 8 ISO. You can find one online. The following link may or
- may not work:
- <https://cdimage.debian.org/cdimage/archive/8.9.0/amd64/iso-cd/debian-8.9.0-amd64-netinst.iso>
-
-Usage
---------------------------------------------------------------------------------
-1. Start VirtualBox and create a new VM. Make sure to mount the ISO in the
- virtual CD/DVD drive of the VM.
-
- There are a lot of guides online for creating a VM and can change between
- VirtualBox versions. If VirtualBox asks for the type of VM, select "Debian
- 64-bit".
-
-2. Boot the VM and go through the guided installation steps to install Debian.
- Once the installation completes, reboot to boot into your newly installed
- system. This will be part of the guided installation.
-
-3. Check this folder out on your computer somewhere.
-
- $ git clone ssh://USERNAME@robotics.mvla.net:29418/971-Robot-Code
-
- where you replace `USERNAME` with your own username. Keep in mind that you
- need your SVN and Gerrit account set up for this to work. Ask the mentors or
- other students about this if you don't have one yet.
-
-4. Run the setup script so you can start building our code.
-
- $ cd 971-Robot-Code/vm/
- $ sudo ./setup_code_building.sh
-
-5. Now you can build code. For example, to build all the 2017 code.
-
- $ bazel build //y2017/...
diff --git a/vm/Vagrantfile b/vm/Vagrantfile
deleted file mode 100644
index 1404131..0000000
--- a/vm/Vagrantfile
+++ /dev/null
@@ -1,57 +0,0 @@
-# Vagrantfile API/syntax version. Don't touch unless you know what you're
-# doing!
-VAGRANTFILE_API_VERSION = "2"
-
-# Install the necessary plugins.
-required_plugins = %w( vagrant-persistent-storage )
-required_plugins.each do |plugin|
- unless Vagrant.has_plugin? plugin || ARGV[0] == 'plugin' then
- exec "vagrant plugin install #{plugin};vagrant #{ARGV.join(" ")}"
- end
-end
-
-Vagrant.configure(VAGRANTFILE_API_VERSION) do |config|
- # All Vagrant configuration is done here. The most common configuration
- # options are documented and commented below. For a complete reference,
- # please see the online documentation at vagrantup.com.
-
- # Every Vagrant virtual environment requires a box to build off of.
- config.vm.box = "debian/jessie64"
-
- config.vm.provider "virtualbox" do |vb|
- # Don't boot with headless mode
- vb.gui = true
- vb.name = "FRC971-Development-2017"
-
- # There are two shortcuts for modifying number of CPUs and amount of
- # memory. Modify them to your liking.
- vb.cpus = 2
- vb.memory = 1024 * 2
- end
-
- # Use rsync to sync the /vagrant folder.
- # NOTE: If you change these settings they will not take effect until you
- # reboot the VM -- i.e. run a "vagrant reload".
- config.vm.synced_folder ".", "/vagrant", type: "rsync",
- rsync__exclude: [".git/", ".svn/", "workspace.vdi"], rsync__verbose: true
-
- # Set up apt and install packages necessary for building the code.
- config.vm.provision :shell, inline: "/vagrant/setup_apt.sh"
- config.vm.provision :shell, inline: "/vagrant/setup_extra_storage.sh"
- config.vm.provision :shell, inline: "/vagrant/setup_code_building.sh"
- config.vm.provision :shell, inline: "/vagrant/setup_scouting.sh"
- config.vm.provision :shell, inline: "/vagrant/setup_desktop.sh"
- config.vm.provision :shell, inline: "/vagrant/setup_misc_packages.sh"
- config.vm.provision :shell, inline: "/vagrant/setup_vbox_guest_additions.sh"
-
- # Add a second disk so we have plenty of space to compile the code.
- config.persistent_storage.enabled = true
- config.persistent_storage.location = "workspace.vdi"
- config.persistent_storage.size = 40000 # MiB
- config.persistent_storage.use_lvm = false
- config.persistent_storage.filesystem = 'ext4'
- config.persistent_storage.mountpoint = '/home/user'
-
- # Forward the scouting app's port.
- config.vm.network :forwarded_port, guest: 5000, host: 5000, auto_correct: true
-end
diff --git a/vm/setup_apt.sh b/vm/setup_apt.sh
deleted file mode 100755
index d31a372..0000000
--- a/vm/setup_apt.sh
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-export DEBIAN_FRONTEND=noninteractive
-
-# Set up contrib and non-free so we can install some more interesting programs.
-cat > /etc/apt/sources.list.d/contrib.list <<EOT
-deb http://ftp.us.debian.org/debian/ jessie contrib non-free
-deb-src http://ftp.us.debian.org/debian/ jessie contrib non-free
-EOT
-
-# Get a list of the latest packages.
-apt-get update
diff --git a/vm/setup_code_building.sh b/vm/setup_code_building.sh
deleted file mode 100755
index cbb8fcd..0000000
--- a/vm/setup_code_building.sh
+++ /dev/null
@@ -1,64 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-export DEBIAN_FRONTEND=noninteractive
-
-readonly PKGS=(
- bazel
- clang-3.6
- clang-format-3.5
- gfortran
- git
- libblas-dev
- liblapack-dev
- libpython3-dev
- libpython-dev
- python3
- python3-matplotlib
- python3-numpy
- python3-scipy
- python-matplotlib
- python-scipy
- resolvconf
- ruby
-)
-
-# Set up the backports repo.
-cat > /etc/apt/sources.list.d/backports.list <<EOT
-deb http://http.debian.net/debian jessie-backports main
-EOT
-
-# Set up the LLVM repo.
-cat > /etc/apt/sources.list.d/llvm-3.6.list <<EOT
-deb http://llvm.org/apt/jessie/ llvm-toolchain-jessie-3.6 main
-deb-src http://llvm.org/apt/jessie/ llvm-toolchain-jessie-3.6 main
-EOT
-
-# Set up the 971-managed bazel repo.
-cat > /etc/apt/sources.list.d/bazel-971.list <<EOT
-deb http://robotics.mvla.net/files/frc971/packages jessie main
-EOT
-
-# Enable user namespace for sandboxing.
-cat > /etc/sysctl.d/99-enable-user-namespaces.conf <<EOT
-kernel.unprivileged_userns_clone = 1
-EOT
-
-# We need to explicitly pull in the java certificates from backports. Otherwise
-# bazel won't install properly.
-cat > /etc/apt/preferences.d/java_certificates <<EOT
-Package: ca-certificates-java
-Pin: release a=jessie-backports
-Pin-Priority: 900
-EOT
-
-# Accept the LLVM GPG key so we can install their packages.
-wget -O - http://llvm.org/apt/llvm-snapshot.gpg.key | apt-key add -
-
-# Install all the packages that we need/want.
-apt-get update
-for pkg in "${PKGS[@]}"; do
- apt-get install -y -f --force-yes "$pkg"
-done
diff --git a/vm/setup_desktop.sh b/vm/setup_desktop.sh
deleted file mode 100755
index d713856..0000000
--- a/vm/setup_desktop.sh
+++ /dev/null
@@ -1,19 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-export DEBIAN_FRONTEND=noninteractive
-
-readonly PKGS=(
- iceweasel
- lightdm
- mousepad
- xfce4
- xfce4-terminal
-)
-
-# Install all the packages that we need/want.
-for pkg in "${PKGS[@]}"; do
- apt-get install -y -f "$pkg"
-done
diff --git a/vm/setup_extra_storage.sh b/vm/setup_extra_storage.sh
deleted file mode 100755
index 5fcf4cb..0000000
--- a/vm/setup_extra_storage.sh
+++ /dev/null
@@ -1,19 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-readonly EXTRA_USER=user
-readonly EXTRA_STORAGE=/home/"${EXTRA_USER}"
-
-if ! grep -q "$EXTRA_STORAGE" /etc/passwd; then
- PASSWORD="$(echo "$EXTRA_USER" | mkpasswd -s)"
- useradd \
- --home="$EXTRA_STORAGE" -M \
- --password="$PASSWORD" \
- --shell=/bin/bash \
- "$EXTRA_USER"
- chown "$EXTRA_USER:$EXTRA_USER" "$EXTRA_STORAGE"
-fi
-
-usermod -a -G sudo "$EXTRA_USER"
diff --git a/vm/setup_misc_packages.sh b/vm/setup_misc_packages.sh
deleted file mode 100755
index 691b2be..0000000
--- a/vm/setup_misc_packages.sh
+++ /dev/null
@@ -1,17 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-export DEBIAN_FRONTEND=noninteractive
-
-readonly PKGS=(
- colordiff
- tmux
- vim
-)
-
-# Install all the packages that we want.
-for pkg in "${PKGS[@]}"; do
- apt-get install -y -f "$pkg"
-done
diff --git a/vm/setup_scouting.sh b/vm/setup_scouting.sh
deleted file mode 100755
index 3360ff5..0000000
--- a/vm/setup_scouting.sh
+++ /dev/null
@@ -1,17 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-export DEBIAN_FRONTEND=noninteractive
-
-readonly PKGS=(
- python3
- python3-flask
-)
-
-# Install all the packages that we need/want.
-apt-get update
-for pkg in "${PKGS[@]}"; do
- apt-get install -y -f --force-yes "$pkg"
-done
diff --git a/vm/setup_vbox_guest_additions.sh b/vm/setup_vbox_guest_additions.sh
deleted file mode 100755
index c9f4b09..0000000
--- a/vm/setup_vbox_guest_additions.sh
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/usr/bin/env bash
-
-set -e
-set -u
-
-export DEBIAN_FRONTEND=noninteractive
-
-# Install the kernel sources before the guest additions to guarantee that
-# we can compile the kernel module.
-apt-get install -q -y linux-headers-amd64
-
-# Now we can install the guest additions.
-apt-get install -q -y \
- virtualbox-guest-dkms \
- virtualbox-guest-x11
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 488d792..94e1227 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -140,8 +140,8 @@
numpy.matrix([[self.A[1, 2]],
[self.A[3, 2]]]),
rcond=None)[0]
- self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] -
- out_x[0]) / lstsq_A[0, 1] + out_x[1]
+ self.K[1, 2] = -lstsq_A[0, 0] * (
+ self.K[0, 2] - out_x[0, 0]) / lstsq_A[0, 1] + out_x[1, 0]
glog.debug('K unaugmented')
glog.debug(str(self.K))