diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index 315c2c6caa893e1c24e8fa8a3fcbd2da845b12d9..b7c8fb4072b0ee7b0de0a3db7f0ba54c2b5c94bd 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -209,7 +209,7 @@ class QPWBC {
   Eigen::Matrix<double, 12, 12> Q2 = Eigen::Matrix<double, 12, 12>::Identity();
 
   // Friction coefficient
-  const double mu = 0.9;
+  const double mu = 0.5;
 
   // Minimum and maximum normal contact force
   double Fz_max = 0.0;
diff --git a/include/qrw/WbcWrapper.hpp b/include/qrw/WbcWrapper.hpp
index eccf0e3635b99cb2b93b25105d276ec740eee0c1..bce86d02589f0df99ccd656522d7d52707dbb831 100644
--- a/include/qrw/WbcWrapper.hpp
+++ b/include/qrw/WbcWrapper.hpp
@@ -60,6 +60,8 @@ class WbcWrapper {
   VectorN get_vdes() { return vdes_; }
   VectorN get_tau_ff() { return tau_ff_; }
   VectorN get_ddq_cmd() { return ddq_cmd_; }
+  VectorN get_dq_cmd() { return dq_cmd_; }
+  VectorN get_q_cmd() { return q_cmd_; }
   VectorN get_f_with_delta() { return f_with_delta_; }
   VectorN get_ddq_with_delta() { return ddq_with_delta_; }
   VectorN get_nle() { return nle_; }
@@ -97,7 +99,9 @@ class WbcWrapper {
 
   Vector19 q_wbc_;           // Configuration vector for the whole body control
   Vector18 dq_wbc_;          // Velocity vector for the whole body control
-  Vector18 ddq_cmd_;         // Actuator accelerations computed by Inverse Kinematics
+  Vector18 ddq_cmd_;         // Command ccelerations computed by Inverse Kinematics
+  Vector18 dq_cmd_;          // Command velocities computed by Inverse Kinematics
+  Vector19 q_cmd_;           // Command positions computed by Inverse Kinematics
   Vector12 f_with_delta_;    // Contact forces with deltas found by QP solver
   Vector18 ddq_with_delta_;  // Actuator accelerations with deltas found by QP solver
 
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index b3af67d80a50dfe7db71b93e204ed8c685b51aa8..2b9fa0d5acc53b67240b3fdb6f23d9da84574de5 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -417,8 +417,16 @@ int QPWBC::run(const MatrixN &M, const MatrixN &Jc, const MatrixN &ddq_cmd, cons
   Eigen::Matrix<double, 20, 1> Gf = G * f_cmd;
 
   for (int i = 0; i < G.rows(); i++) {
-    v_NK_low[i] = -Gf(i, 0) + params_->Fz_min;
-    v_NK_up[i] = -Gf(i, 0) + params_->Fz_max;
+    v_NK_low[i] = -Gf(i, 0);
+    v_NK_up[i] = std::numeric_limits<double>::infinity();
+  }
+  for (int i = 0; i < 4; i++) {
+    if (k_contact(0, i) > 0) {
+      v_NK_low[5 * i + 4] += params_->Fz_min;
+      v_NK_up[5 * i + 4] = -Gf(5 * i + 4, 0) + params_->Fz_max;
+    } else {
+      v_NK_up[5 * i + 4] = -Gf(5 * i + 4, 0) + 0.0;
+    }
   }
 
   // Limit max force when contact is activated
diff --git a/src/WbcWrapper.cpp b/src/WbcWrapper.cpp
index c618b1adc174ffa7c4e31ef55a145c02f559531a..e5a1bc96c5d6611422f2418712922580f5a0d9ca 100644
--- a/src/WbcWrapper.cpp
+++ b/src/WbcWrapper.cpp
@@ -18,6 +18,8 @@ WbcWrapper::WbcWrapper()
       q_wbc_(Vector19::Zero()),
       dq_wbc_(Vector18::Zero()),
       ddq_cmd_(Vector18::Zero()),
+      dq_cmd_(Vector18::Zero()),
+      q_cmd_(Vector19::Zero()),
       f_with_delta_(Vector12::Zero()),
       ddq_with_delta_(Vector18::Zero()),
       nle_(Vector6::Zero()),
@@ -103,6 +105,8 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
   // Compute Inverse Kinematics
   invkin_->run(q_wbc_, dq_wbc_, contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose(), xgoals);
   ddq_cmd_ = invkin_->get_ddq_cmd();
+  dq_cmd_ = invkin_->get_dq_cmd();
+  q_cmd_ = invkin_->get_q_cmd();
 
   // TODO: Check if we can save time by switching MatrixXd to defined sized vector since they are
   // not called from python anymore