Fix indexing of unpack_{x,u} in casadi_velocity_mpc
They didn't get updated when the states shuffled... Whops.
Change-Id: I13a88330f0427b40d63d477a4d5d560f26b8e5f8
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 864843a..0d3e177 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -230,13 +230,14 @@
p=casadi.DM(p))
def unpack_u(self, sol, i):
- return sol['x'].full().flatten()[8 * i:8 * (i + 1)]
+ return sol['x'].full().flatten()[
+ (8 + dynamics.NUM_VELOCITY_STATES) *
+ i:((8 + dynamics.NUM_VELOCITY_STATES) * i + 8)]
def unpack_x(self, sol, i):
- return sol['x'].full().flatten()[8 * self.N +
- dynamics.NUM_VELOCITY_STATES *
- (i - 1):8 * self.N +
- dynamics.NUM_VELOCITY_STATES * (i)]
+ return sol['x'].full().flatten(
+ )[8 + (8 + dynamics.NUM_VELOCITY_STATES) *
+ (i - 1):(8 + dynamics.NUM_VELOCITY_STATES) * i]
mpc = MPC(solver='fatrop')