Add the catapult simulation and design code
This is mostly shamelessly stolen from rotory system. It is different
enough because we have different control goals to not warrent being
lumped with the rotory system.
This lets us compute final speeds given the gear ratio and masses,
letting us optimize the gear ratio.
This gives us a model to start using when writing the optimization
problem for the fixed horizon MPC for shooting.
Change-Id: I03ac3448aa6ef717965fef7f41ef0a1197ea77a5
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index 4049acc..54bce63 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -48,6 +48,33 @@
],
)
+py_library(
+ name = "catapult_lib",
+ srcs = [
+ "catapult_lib.py",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ "//frc971/control_loops/python:controls",
+ "@matplotlib_repo//:matplotlib3",
+ ],
+)
+
+py_binary(
+ name = "catapult",
+ srcs = [
+ "catapult.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":catapult_lib",
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ ],
+)
+
py_binary(
name = "intake",
srcs = [
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
new file mode 100755
index 0000000..bb8d74d
--- /dev/null
+++ b/y2022/control_loops/python/catapult.py
@@ -0,0 +1,84 @@
+#!/usr/bin/python3
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import math
+import sys
+import math
+from y2022.control_loops.python import catapult_lib
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', True, 'If true, plot the loop response.')
+
+ball_mass = 0.25
+ball_diameter = 9.5 * 0.0254
+lever = 17.5 * 0.0254
+
+G = (14.0 / 72.0) * (12.0 / 33.0)
+
+
+def AddResistance(motor, resistance):
+ motor.resistance += resistance
+ return motor
+
+J_ball = 1.5 * ball_mass * lever * lever
+# Assuming carbon fiber, calculate the mass of the bar.
+M_bar = (1750 * lever * 0.0254 * 0.0254 * (1.0 - (1 - 0.07)**2.0))
+# And the moment of inertia.
+J_bar = 1.0 / 3.0 * M_bar * lever**2.0
+
+# Do the same for a theoretical cup. Assume a 40 thou thick carbon cup.
+M_cup = (1750 * 0.0254 * 0.04 * 2 * math.pi * (ball_diameter / 2.)**2.0)
+J_cup = M_cup * lever**2.0 + M_cup * (ball_diameter / 2.)**2.0
+
+print("J ball", ball_mass * lever * lever)
+print("J bar", J_bar)
+print("bar mass", M_bar)
+print("J cup", J_cup)
+print("cup mass", M_cup)
+
+J = (J_ball + J_bar + J_cup * 1.5)
+print("J", J)
+
+kFinisher = catapult_lib.CatapultParams(
+ name='Finisher',
+ motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
+ G=G,
+ J=J,
+ lever=lever,
+ q_pos=0.01,
+ q_vel=10.0,
+ q_voltage=4.0,
+ r_pos=0.01,
+ controller_poles=[.93],
+ dt=0.0005)
+
+
+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=kFinisher, U=U, final_position = math.pi / 2.0))
+
+ if FLAGS.plot:
+ catapult_lib.PlotShot(kFinisher, U, final_position = math.pi / 4.0)
+
+ gs = []
+ speed = []
+ for i in numpy.linspace(0.01, 0.15, 150):
+ kFinisher.G = i
+ gs.append(kFinisher.G)
+ speed.append(catapult_lib.MaxSpeed(params=kFinisher, U=U, final_position = math.pi / 2.0))
+ pylab.plot(gs, speed, label = "max_speed")
+ pylab.show()
+ return 0
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
new file mode 100644
index 0000000..256793a
--- /dev/null
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -0,0 +1,308 @@
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+from matplotlib import pylab
+
+import gflags
+import glog
+
+
+class CatapultParams(object):
+ def __init__(self,
+ name,
+ motor,
+ G,
+ J,
+ lever,
+ q_pos,
+ q_vel,
+ q_voltage,
+ r_pos,
+ controller_poles,
+ dt=0.00505):
+ self.name = name
+ self.motor = motor
+ self.G = G
+ self.J = J
+ self.lever = lever
+ self.q_pos = q_pos
+ self.q_vel = q_vel
+ self.q_voltage = q_voltage
+ self.r_pos = r_pos
+ self.dt = dt
+ self.controller_poles = controller_poles
+
+
+class VelocityCatapult(control_loop.HybridControlLoop):
+ def __init__(self, params, name="Catapult"):
+ super(VelocityCatapult, self).__init__(name=name)
+ self.params = params
+ # Set Motor
+ self.motor = self.params.motor
+ # Gear ratio
+ self.G = self.params.G
+ # Moment of inertia of the catapult wheel in kg m^2
+ self.J = self.params.J + self.motor.motor_inertia / (self.G**2.0)
+ # Control loop time step
+ self.dt = self.params.dt
+
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix([[
+ -self.motor.Kt / self.motor.Kv /
+ (self.J * self.G * self.G * self.motor.resistance)
+ ]])
+ self.B_continuous = numpy.matrix(
+ [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles(self.params.controller_poles)
+
+ # Generated controller not used.
+ self.PlaceObserverPoles([0.3])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+ glog.debug('K: %s', str(self.K))
+ glog.debug('Poles: %s',
+ str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+
+class Catapult(VelocityCatapult):
+ def __init__(self, params, name="Catapult"):
+ super(Catapult, self).__init__(params, name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
+
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ rpl = 0.45
+ ipl = 0.07
+ self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+class IntegralCatapult(Catapult):
+ def __init__(self, params, name="IntegralCatapult"):
+ super(IntegralCatapult, self).__init__(params, name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+ # states
+ # [position, velocity, voltage_error]
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
+
+ glog.debug('A_continuous %s' % str(self.A_continuous))
+ glog.debug('B_continuous %s' % str(self.B_continuous))
+ glog.debug('C %s' % str(self.C))
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ glog.debug('A %s' % str(self.A))
+ glog.debug('B %s' % str(self.B))
+
+ q_pos = self.params.q_pos
+ q_vel = self.params.q_vel
+ q_voltage = self.params.q_voltage
+ self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0,
+ 0.0], [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
+
+ r_pos = self.params.r_pos
+ self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
+
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ glog.debug('Q_discrete %s' % (str(self.Q)))
+ glog.debug('R_discrete %s' % (str(self.R)))
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+def MaxSpeed(params, U, final_position):
+ """Runs the catapult plant with an initial condition and goal.
+
+ Args:
+ catapult: Catapult object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_catapult: Catapult object to get K from, or None if we should
+ use catapult.
+ observer_catapult: Catapult object to use for the observer, or None if we
+ should use the actual state.
+ """
+
+ # Various lists for graphing things.
+ catapult = Catapult(params, params.name)
+ controller_catapult = IntegralCatapult(params, params.name)
+ observer_catapult = IntegralCatapult(params, params.name)
+ vbat = 12.0
+
+ while True:
+ X_hat = catapult.X
+ if catapult.X[0, 0] > final_position:
+ return catapult.X[1, 0] * params.lever
+
+ if observer_catapult is not None:
+ X_hat = observer_catapult.X_hat
+
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+
+ if observer_catapult is not None:
+ observer_catapult.Y = catapult.Y
+ observer_catapult.CorrectHybridObserver(U)
+
+ applied_U = U.copy()
+ catapult.Update(applied_U)
+
+ if observer_catapult is not None:
+ observer_catapult.PredictHybridObserver(U, catapult.dt)
+
+
+def PlotShot(params, U, final_position):
+ """Runs the catapult plant with an initial condition and goal.
+
+ Args:
+ catapult: Catapult object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_catapult: Catapult object to get K from, or None if we should
+ use catapult.
+ observer_catapult: Catapult object to use for the observer, or None if we
+ should use the actual state.
+ """
+
+ # Various lists for graphing things.
+ t = []
+ x = []
+ x_hat = []
+ v = []
+ w_hat = []
+ v_hat = []
+ a = []
+ u = []
+ offset = []
+
+ catapult = Catapult(params, params.name)
+ controller_catapult = IntegralCatapult(params, params.name)
+ observer_catapult = IntegralCatapult(params, params.name)
+ vbat = 12.0
+
+ if t:
+ initial_t = t[-1] + catapult.dt
+ else:
+ initial_t = 0
+
+ for i in range(10000):
+ X_hat = catapult.X
+ if catapult.X[0, 0] > final_position:
+ break
+
+ if observer_catapult is not None:
+ X_hat = observer_catapult.X_hat
+ x_hat.append(observer_catapult.X_hat[0, 0])
+ w_hat.append(observer_catapult.X_hat[1, 0])
+ v_hat.append(observer_catapult.X_hat[1, 0] * params.lever)
+
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ x.append(catapult.X[0, 0])
+
+ if v:
+ last_v = v[-1]
+ else:
+ last_v = 0
+
+ v.append(catapult.X[1, 0])
+ a.append((v[-1] - last_v) / catapult.dt)
+
+ if observer_catapult is not None:
+ observer_catapult.Y = catapult.Y
+ observer_catapult.CorrectHybridObserver(U)
+ offset.append(observer_catapult.X_hat[2, 0])
+
+ catapult.Update(U)
+
+ if observer_catapult is not None:
+ observer_catapult.PredictHybridObserver(U, catapult.dt)
+
+ t.append(initial_t + i * catapult.dt)
+ u.append(U[0, 0])
+
+ pylab.subplot(3, 1, 1)
+ pylab.plot(t, v, label='v')
+ pylab.plot(t, x_hat, label='x_hat')
+ pylab.plot(t, v, label='v')
+ pylab.plot(t, v_hat, label='v_hat')
+ pylab.plot(t, w_hat, label='w_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(t, u, label='u')
+ pylab.plot(t, offset, label='voltage_offset')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(t, a, label='a')
+ pylab.legend()
+
+ pylab.show()