diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index cdd57f214589b2898b14f14f88ba3535df1822cb..ec6e3740483c978f3fa86ee95d8b4ef0a5c1e4e1 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -151,13 +151,13 @@ public:
     VectorN get_qdes() { return qdes_; }
     VectorN get_vdes() { return vdes_; }
     VectorN get_tau_ff() { return tau_ff_; }
-    VectorN get_f_with_delta() { box_qp_->get_f_res(); }
-    MatrixN get_feet_pos() { return MatrixN::Zero(4, 3); }
-    MatrixN get_feet_err() { return MatrixN::Zero(4, 3); }
-    MatrixN get_feet_vel() { return MatrixN::Zero(4, 3); }
-    MatrixN get_feet_pos_target() { return MatrixN::Zero(4, 3); }
-    MatrixN get_feet_vel_target() { return MatrixN::Zero(4, 3); }
-    MatrixN get_feet_acc_target() { return MatrixN::Zero(4, 3); }
+    VectorN get_f_with_delta() { return f_with_delta_; }
+    MatrixN get_feet_pos() { return MatrixN::Zero(3, 4); }
+    MatrixN get_feet_err() { return MatrixN::Zero(3, 4); }
+    MatrixN get_feet_vel() { return MatrixN::Zero(3, 4); }
+    MatrixN get_feet_pos_target() { return MatrixN::Zero(3, 4); }
+    MatrixN get_feet_vel_target() { return MatrixN::Zero(3, 4); }
+    MatrixN get_feet_acc_target() { return MatrixN::Zero(3, 4); }
 
 private:
     
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 4023d3bc8c0401a9a943a807b4c9da76fe2fd233..2ccedc2fc988b929295507cacc3e9c777bf975c3 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -158,7 +158,7 @@ class LoggerControl():
         self.wbc_q_des[self.i] = loop.result.q_des
         self.wbc_v_des[self.i] = loop.result.v_des
         self.wbc_tau_ff[self.i] = loop.result.tau_ff
-        self.wbc_f_ctc[self.i] = wbc.f_with_delta[:, 0]
+        self.wbc_f_ctc[self.i] = wbc.f_with_delta
         self.wbc_feet_pos[self.i] = wbc.feet_pos
         self.wbc_feet_pos_target[self.i] = wbc.feet_pos_target
         self.wbc_feet_err[self.i] = wbc.feet_err
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index e22bb5824a66cdf886bfdb52475f32e0dff02a1c..7b57a7b78ce285fbb928a167d71120f397c9c5fd 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -201,7 +201,7 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
             loggerControl.sample(controller.joystick, controller.estimator,
                                  controller, controller.gait, controller.statePlanner,
                                  controller.footstepPlanner, controller.footTrajectoryGenerator,
-                                 controller.myController)
+                                 controller.wbcWrapper)
 
         # Send command to the robot
         for i in range(1):
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 786a89f98047c6968d253442b18963d197cd5317..d3164d51c64d64204b0cc16de7c0c4c6ed39b62e 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -89,8 +89,8 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
 void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals)
 {
     // Update model and data of the robot
-    pinocchio::computeJointJacobians(model_, data_, q);
     pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
+    pinocchio::computeJointJacobians(model_, data_);
     pinocchio::updateFramePlacements(model_, data_);
 
     // Get data required by IK with Pinocchio
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index f8338c3f0f0b70913ac857597800b47b9ba651c2..faaec1b3b6b6f5575a7e86894385daeb1c9fad9f 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -590,6 +590,17 @@ void WbcWrapper::initialize(Params& params)
 
   // Initialize quaternion
   q_default_(6, 0) = 1.0;
+
+  // Initialize joint positions
+  qdes_.tail(12) = Vector12(params_->q_init.data());
+
+  // Compute the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm
+  // Result is stored in data_.M
+  pinocchio::crba(model_, data_, q_default_);
+
+  // Make mass matrix symetric
+  data_.M.triangularView<Eigen::StrictlyLower>() = data_.M.transpose().triangularView<Eigen::StrictlyLower>();
+
 }
 
 void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_cmd, MatrixN const& contacts,
@@ -605,13 +616,6 @@ void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_c
 
   // TODO: Adapt logging of feet_pos, feet_err, feet_vel
 
-  // Compute the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm
-  // Result is stored in data_.M
-  pinocchio::crba(model_, data_, q_default_);
-
-  // Make mass matrix symetric
-  data_.M.triangularView<Eigen::StrictlyLower>() = data_.M.transpose().triangularView<Eigen::StrictlyLower>();
-
   // TODO: Check if needed because crbaMinimal may allow to directly get the jacobian
   // TODO: Check if possible to use the model of InvKin to avoid computations
   pinocchio::computeJointJacobians(model_, data_, q);