Add single wheel tire model
This lets us play with the tire math and see how it works in various
situations to see if we got it right.
Change-Id: I7b6f3add85d4be6c894532b71249df59281a8db3
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
new file mode 100644
index 0000000..e411fc9
--- /dev/null
+++ b/frc971/control_loops/swerve/BUILD
@@ -0,0 +1,13 @@
+py_binary(
+ name = "simulation",
+ srcs = [
+ "simulation.py",
+ ],
+ deps = [
+ "//frc971/control_loops/python:controls",
+ "@pip//matplotlib",
+ "@pip//numpy",
+ "@pip//pygobject",
+ "@pip//sympy",
+ ],
+)
diff --git a/frc971/control_loops/swerve/simulation.py b/frc971/control_loops/swerve/simulation.py
new file mode 100644
index 0000000..4eed97b
--- /dev/null
+++ b/frc971/control_loops/swerve/simulation.py
@@ -0,0 +1,103 @@
+#!/usr/bin/python3
+
+import numpy
+import sympy
+import scipy.integrate
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+
+from matplotlib import pylab
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+
+class SwerveSimulation(object):
+
+ def __init__(self):
+ self.motor = control_loop.KrakenFOC()
+
+ vx, vy, omega = sympy.symbols('vx vy omega')
+ fx, fy = sympy.symbols('fx fy')
+ t = sympy.symbols('t')
+
+ # 5kg of force got us a slip angle of 0.05 radians with 4 tires.
+ self.C = 5 * 9.8 / 0.05 / 4.0
+
+ self.r = 2 * 0.0254
+
+ # Base is 20kg without battery.
+ self.m = 25.0
+ self.G = 1.0 / 6.75
+
+ I = sympy.symbols('I')
+
+ # Absolute linear velocity in x and y of the robot.
+ self.dvx = (self.C * (self.r * omega - vx) / vx + fx) / self.m
+ self.dvy = (-self.C * sympy.atan2(vy, vx) + fy) / self.m
+ # Angular velocity of the wheel.
+ self.domega = (-self.G * self.C * (self.r * omega - vx) / vx * self.r +
+ self.motor.Kt * I) * self.G / self.motor.motor_inertia
+
+ self.x0 = sympy.lambdify((vx, vy, omega, fx, fy, I), self.dvx)
+ self.x1 = sympy.lambdify((vx, vy, omega, fx, fy, I), self.dvy)
+ self.x2 = sympy.lambdify((vx, vy, omega, fx, fy, I), self.domega)
+
+ self.f = lambda X, fx, fy, I: numpy.array([
+ self.x0(X[0], X[1], X[2], fx, fy, I),
+ self.x1(X[0], X[1], X[2], fx, fy, I),
+ self.x2(X[0], X[1], X[2], fx, fy, I)
+ ])
+
+ print(self.f)
+ print(
+ 'f',
+ self.f(numpy.matrix([[1.0], [0.0], [1.0 / self.r]]), 0.0, 0.0,
+ 0.0))
+
+ print(self.dvx)
+ print(self.dvy)
+ print(self.domega)
+
+ def run(self, X_initial):
+ print(X_initial)
+
+ fx = -9.8
+ fy = 0.0
+ I = -(fx * self.r * self.G / self.motor.Kt)
+ print(f"Fx: {fx}, Fy: {fy}, I: {I}")
+
+ def f_const(t, X):
+ return self.f(
+ X=X,
+ fx=fx,
+ fy=fy,
+ I=I,
+ )
+
+ result = scipy.integrate.solve_ivp(
+ f_const, (0, 2.0),
+ numpy.squeeze(numpy.array(X_initial.transpose())),
+ max_step=0.01)
+
+ pylab.plot(result.t, result.y[0, :], label="y0")
+ pylab.plot(result.t, result.y[1, :], label="y1")
+ pylab.plot(result.t, result.y[2, :], label="y2")
+
+ pylab.legend()
+ pylab.show()
+
+
+def main(argv):
+ s = SwerveSimulation()
+ s.run(numpy.matrix([[1.0], [0.0], [1.0 / s.r]]))
+
+ return 0
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/tools/python/README.md b/tools/python/README.md
index 625cb24..25ef34f 100644
--- a/tools/python/README.md
+++ b/tools/python/README.md
@@ -50,7 +50,7 @@
1. Follow the above procedure for adding new pip packages if not already done.
2. Run the mirroring script.
- bazel run //tools/python:mirror_pip_packages --config=k8_upstream_python -- --ssh_host <software>
+ bazel run //tools/python:mirror_pip_packages -- --ssh_host <software>
where `<software>` is the `ssh(1)` target for reaching the server that hosts
the FRC971 mirror.
diff --git a/tools/python/requirements.lock.txt b/tools/python/requirements.lock.txt
index a8a8f4d..c265e77 100644
--- a/tools/python/requirements.lock.txt
+++ b/tools/python/requirements.lock.txt
@@ -286,6 +286,10 @@
--hash=sha256:8947af423a6d0facf41ea1195b8e1e8c85ad94ac95ae307fe11232e0424b11c5 \
--hash=sha256:c8856a832c1e56702577023cd64cc5f84948280c1c0fcc6af4cd39006ea6aa8c
# via -r tools/python/requirements.txt
+mpmath==1.3.0 \
+ --hash=sha256:7a28eb2a9774d00c7bc92411c19a89209d5da7c4c9a9e227be8330a23a25b91f \
+ --hash=sha256:a0b2b9fe80bbcd81a6647ff13108738cfb482d481d826cc0e02f5b35e5c88d2c
+ # via sympy
numpy==1.23.5 \
--hash=sha256:01dd17cbb340bf0fc23981e52e1d18a9d4050792e8fb8363cecbf066a84b827d \
--hash=sha256:06005a2ef6014e9956c09ba07654f9837d9e26696a0470e42beedadb78c11b07 \
@@ -608,6 +612,10 @@
# via
# glog
# python-dateutil
+sympy==1.12 \
+ --hash=sha256:c3588cd4295d0c0f603d0f2ae780587e64e2efeedb3521e46b9bb1d08d184fa5 \
+ --hash=sha256:ebf595c8dac3e0fdc4152c51878b498396ec7f30e7a914d6071e674d49420fb8
+ # via -r tools/python/requirements.txt
urllib3==1.26.13 \
--hash=sha256:47cc05d99aaa09c9e72ed5809b60e7ba354e64b59c9c173ac3018642d8bb41fc \
--hash=sha256:c083dd0dce68dbfbe1129d5271cb90f9447dea7d52097c6e0126120c521ddea8
diff --git a/tools/python/requirements.txt b/tools/python/requirements.txt
index 0a5f24a..e758cf5 100644
--- a/tools/python/requirements.txt
+++ b/tools/python/requirements.txt
@@ -14,6 +14,7 @@
shapely
validators
yapf
+sympy
# TODO(phil): Migrate to absl-py. These are abandoned as far as I can tell.
python-gflags
diff --git a/tools/python/whl_overrides.json b/tools/python/whl_overrides.json
index 6cb9998..a109969 100644
--- a/tools/python/whl_overrides.json
+++ b/tools/python/whl_overrides.json
@@ -67,6 +67,10 @@
"sha256": "c8856a832c1e56702577023cd64cc5f84948280c1c0fcc6af4cd39006ea6aa8c",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/mkdocs-1.4.2-py3-none-any.whl"
},
+ "mpmath==1.3.0": {
+ "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"
@@ -139,6 +143,10 @@
"sha256": "8abb2f1d86890a2dfb989f9a77cfcfd3e47c2a354b01111771326f8aa26e0254",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/six-1.16.0-py2.py3-none-any.whl"
},
+ "sympy==1.12": {
+ "sha256": "c3588cd4295d0c0f603d0f2ae780587e64e2efeedb3521e46b9bb1d08d184fa5",
+ "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/sympy-1.12-py3-none-any.whl"
+ },
"urllib3==1.26.13": {
"sha256": "47cc05d99aaa09c9e72ed5809b60e7ba354e64b59c9c173ac3018642d8bb41fc",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/urllib3-1.26.13-py2.py3-none-any.whl"