diff --git a/esp32/include/gait/kinematic_constraints.h b/esp32/include/gait/kinematic_constraints.h
index a9277c4..10f5969 100644
--- a/esp32/include/gait/kinematic_constraints.h
+++ b/esp32/include/gait/kinematic_constraints.h
@@ -52,7 +52,8 @@ class KinConfig {
     static constexpr float max_body_height = max_leg_reach * 0.9;
     static constexpr float body_height_range = max_body_height - min_body_height;
 
-    static constexpr float max_step_length = 1;
+    static constexpr float max_step_length = max_leg_reach * 0.8;
+    static constexpr float max_step_height = max_leg_reach / 2;
 
     // Default constant
     static constexpr float default_step_depth = 0.002;
diff --git a/esp32/include/motion.h b/esp32/include/motion.h
index 1f3af76..946ef13 100644
--- a/esp32/include/motion.h
+++ b/esp32/include/motion.h
@@ -76,7 +76,7 @@ class MotionService {
                 break;
             }
             case MOTION_STATE::WALK: {
-                gait_state.step_height = KinConfig::default_step_height + command.s1;
+                gait_state.step_height = command.s1 * KinConfig::max_step_height;
                 gait_state.step_x = command.ly * KinConfig::max_step_length;
                 gait_state.step_z = -command.lx * KinConfig::max_step_length;
                 gait_state.step_velocity = command.s;