diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index ed66c8e8c585263176af9bbd4fd8167b717818c5..e2ff4b5afd65fe9e157d47f9cf5233aafafde271 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -32,6 +32,7 @@ class MPC {
   Eigen::Matrix<int, Eigen::Dynamic, 4> gait;
   Eigen::Matrix<int, Eigen::Dynamic, 4> inv_gait;
   Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
+  Eigen::Matrix<double, 3, 1> offset_CoM = Eigen::Matrix<double, 3, 1>::Zero();
 
   Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Identity();
   Eigen::Matrix<double, 12, 12> B = Eigen::Matrix<double, 12, 12>::Zero();
diff --git a/src/MPC.cpp b/src/MPC.cpp
index e49c39347b5d11efe2ad405736952feeb2b7f37e..c3cbf6d6bd93273dc9c910832b240714bd18554a 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -18,6 +18,7 @@ MPC::MPC(double dt_in, int n_steps_in, double T_gait_in, int N_gait) {
   mu = 0.9f;
   cpt_ML = 0;
   cpt_P = 0;
+  offset_CoM(2, 0) = - 0.03;  // Approximation of the vertical offset between center of base and CoM
 
   // Predefined matrices
   footholds << 0.19, 0.19, -0.19, -0.19, 0.15005, -0.15005, 0.15005, -0.15005, 0.0, 0.0, 0.0, 0.0;
@@ -433,7 +434,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
       // footholds = footholds_tmp.reshaped(3, 4);
       Eigen::Map<Eigen::MatrixXd> footholds_bis(footholds_tmp.data(), 3, 4);
 
-      lever_arms = footholds_bis - (xref.block(0, k, 3, 1)).replicate<1, 4>();
+      lever_arms = footholds_bis - (xref.block(0, k, 3, 1) + offset_CoM).replicate<1, 4>();
       for (int i = 0; i < 4; i++) {
         B.block(9, 3 * i, 3, 3) = dt * (I_inv * getSkew(lever_arms.col(i)));
       }