Penalize velocity perpendicular to the goal vector differently
We care more about going in the right direction than getting up to
speed. Penalize motion in the right direction differently than motion
perpendicular to the direction we care about.
Change-Id: Iab3f4ad3d50aa6510c2c5f48f24ab31afbf4b59c
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 94ec70a..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
@@ -208,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))