diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 598f9033372a162a23776f64f2c20d84e80c4093..7d79cf445f84dcc6df96c94dc75096118f4e1a56 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -154,7 +154,7 @@ class Controller:
                             m["x_m"][: self.pd.nq],
                             self.mpc_result.xs[0][: self.pd.nq],
                         ),
-                        m["x_m"][self.pd.nq] - self.mpc_result.xs[0][self.pd.nq :],
+                        m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
                     ]
                 ),
             )