Support a goal velocity of 0 in the swerve mpc
In our normalization code, we were dividing by 0, causing a goal
velocity of 0 to not solve.
Change-Id: I4ffed7554849a9c4e84529682ee6d66092f8a554
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 620d7ec..8b97933 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -200,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