diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index ee5fa8d9ba8ff324b3675ca32153f884dbb168e2..7b8b4898f9d93ee37fa92ff9f0fac19e1ddc1915 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -137,7 +137,8 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
   // Coefficients for z (deterministic)
   double Tz = t_swing[j];
   if (t0s[j] == 0) {
-    stepHeight_[j] = maxHeight_ * (1.0 - 0.5 * targetFootstep_(2, j) / maxHeight_);
+    stepHeight_[j] = maxHeight_ - std::max(0.0, targetFootstep_(2, j)); //* (1.0 - 0.5 * std::max(0.0, targetFootstep_(2, j)) / maxHeight_);
+    // stepHeight_[j] = maxHeight_ * (1.0 - 0.5 * targetFootstep_(2, j) / maxHeight_);
   }
 
   Vector3 Az;  // z trajectory is split in two halfs (before/after apex point)