Merge changes Id4b9065d,I2e0e54dd
* changes:
Create/support "continuous" control loops
Add motor gear ratios to y2024_swerve wpilib
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 09fc5bd..f9981c9 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -38,7 +38,7 @@
casadi.SX.sym("U", 8, 1)) for i in range(4)
]
- self.N = 10
+ self.N = 25
# TODO(austin): Can we approximate sin/cos/atan for the initial operating point to solve faster if we need it?
@@ -103,7 +103,7 @@
flags = ["-O1"]
jit_options = {"flags": flags, "verbose": True, "compiler": compiler}
options = {
- "jit": True,
+ "jit": False,
"compiler": "shell",
"jit_options": jit_options,
"ipopt.linear_solver": "spral",
@@ -138,12 +138,23 @@
R = casadi.MX.sym('R', 3)
J = 0
- J += 1000 * (R[0] - X[dynamics.VELOCITY_STATE_VX])**2.0
- J += 1000 * (R[1] - X[dynamics.VELOCITY_STATE_VY])**2.0
+ vnorm = casadi.sqrt(R[0]**2.0 + R[1]**2.0)
+ vnormx = R[0] / vnorm
+ vnormy = R[1] / vnorm
+
+ vperpx = -vnormy
+ vperpy = vnormx
+
+ J += 100 * ((R[0] - X[dynamics.VELOCITY_STATE_VX]) * vnormx +
+ (R[1] - X[dynamics.VELOCITY_STATE_VY]) * vnormy)**2.0
+
+ J += 1500 * ((R[0] - X[dynamics.VELOCITY_STATE_VX]) * vperpx +
+ (R[1] - X[dynamics.VELOCITY_STATE_VY]) * vperpy)**2.0
+
J += 1000 * (R[2] - X[dynamics.VELOCITY_STATE_OMEGA])**2.0
kSteerPositionGain = 0
- kSteerVelocityGain = 0.05
+ kSteerVelocityGain = 0.10
J += kSteerPositionGain * (X[dynamics.VELOCITY_STATE_THETAS0])**2.0
J += kSteerVelocityGain * (X[dynamics.VELOCITY_STATE_OMEGAS0])**2.0
@@ -166,12 +177,13 @@
return casadi.Function("Jn", [X, U, R], [J])
def solve(self, p, seed=None):
- w0 = []
+ if seed is None:
+ seed = []
- w0 += [0, 0] * 4 * self.N
- w0 += list(p[:dynamics.NUM_VELOCITY_STATES, 0]) * (self.N - 1)
+ seed += [0, 0] * 4 * self.N
+ seed += list(p[:dynamics.NUM_VELOCITY_STATES, 0]) * (self.N - 1)
- return self.solver(x0=w0,
+ return self.solver(x0=seed,
lbx=self.lbw,
ubx=self.ubw,
lbg=self.lbg,
@@ -179,15 +191,13 @@
p=casadi.DM(p))
def unpack_u(self, sol, i):
- return sol['x'].full().flatten()[
- (8 + 1 * dynamics.NUM_VELOCITY_STATES) *
- i:(8 + 1 * dynamics.NUM_VELOCITY_STATES) * i + 8]
+ return sol['x'].full().flatten()[8 * i:8 * (i + 1)]
def unpack_x(self, sol, i):
- return sol['x'].full().flatten()[
- (8 + 1 * dynamics.NUM_VELOCITY_STATES) * (i - 1) +
- 8:(8 + 1 * dynamics.NUM_VELOCITY_STATES) * (i - 1) + 8 +
- dynamics.NUM_VELOCITY_STATES]
+ return sol['x'].full().flatten()[8 * self.N +
+ dynamics.NUM_VELOCITY_STATES *
+ (i - 1):8 * self.N +
+ dynamics.NUM_VELOCITY_STATES * (i)]
mpc = MPC()
@@ -209,7 +219,7 @@
# No angular velocity
X_initial[21, 0] = 0.0
-iterations = 200
+iterations = 100
X_plot = numpy.zeros((25, iterations))
U_plot = numpy.zeros((8, iterations))
@@ -223,16 +233,23 @@
fig1, axs1 = pylab.subplots(2)
last_time = time.time()
+seed = [0, 0] * 4 * mpc.N + list(dynamics.to_velocity_state(X)) * (mpc.N - 1)
+
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)
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)))
+ p=numpy.vstack((dynamics.to_velocity_state(X), R_goal)),
+ seed=seed)
X_plot[:, i] = X[:, 0]
U = mpc.unpack_u(sol, 0)
+ seed = (list(sol['x'].full().flatten()[8:8 * mpc.N]) +
+ list(sol['x'].full().flatten()[8 * (mpc.N - 1) +
+ dynamics.NUM_VELOCITY_STATES:]) +
+ list(sol['x'].full().flatten()[-dynamics.NUM_VELOCITY_STATES:]))
U_plot[:, i] = U
print('x(0):', X.transpose())