Add wall clock timing to casadi code
It helps a ton to be able to see exactly how long each solve took.
Change-Id: I571b29c0872e87996ea9e7bca3ccc41b0c74dff3
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 a832c7f..f038ff6 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -248,14 +248,20 @@
seed = [0, 0] * 4 * mpc.N + list(dynamics.to_velocity_state(X)) * (mpc.N - 1)
+overall_time = 0
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)
+ start_time = time.time()
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)),
seed=seed)
+ end_time = time.time()
+ print(f"Took {end_time - start_time} seconds to solve.")
+ overall_time += end_time - start_time
+
X_plot[:, i] = X[:, 0]
U = mpc.unpack_u(sol, 0)
@@ -303,4 +309,6 @@
pyplot.pause(0.0001)
last_time = time.time()
+print(f"Tool {overall_time} seconds overall to solve.")
+
pyplot.pause(-1)