Firmware simulation, Unity project, and sim-verified robot fixes

Simulation
- sim/: the unchanged robot firmware compiled into hexapod_sim.dll
  against PC stand-ins (Arduino, Servo, Wire + register-level MPU6050,
  RF24 with matching-config radio), a kinematic body model (support
  plane + planted-foot odometry) that also drives the simulated IMU,
  and a headless smoke test (sim_smoke). build.ps1 builds and tests.
- Hexapod_Simulation_Unity/ folded in (history kept in a bundle
  outside the repo). Assets/HexapodSim loads the DLL, drives the
  existing leg rig, and offers joystick / keyboard / robot-serial
  input plus a test panel.

Robot (all found or confirmed in the simulation)
- Config.h geometry taken from the Unity model.
- IK femur elevation sign, gait stance direction, body pose
  rotation direction, yaw lever arm.
- Stance feet integrated from the current command; gait restarts at
  mid-stance; heading integrates the applied turn rate, only while
  walking.
- Leg clearance and reach checks (Limits::feetFeasible); walk command
  and body pose scaled as a whole, pose scale rate-limited, planted
  feet guarded so the body never snaps or legs collide.
- Robot-gyro self-leveling as an integrator.
- Heading mode: steering relative to the body; S walks backward.
- Standing height from the remote (STANCE_HEIGHT_RANGE_MM).

Protocol v3 (reflash both boards)
- CmdPacket gains body_height (22 bytes).

Remote
- Height slider (Nextion hHeight/nHeight, serial [ ]).
- Fixed unspecified I2C read order in RemoteIMU.

LEGACY/: owner's old Nextion HMI and remote hardware assets.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

2026-current/robot/BodyPose.cpp

@@ -1,4 +1,5 @@
 #include "BodyPose.h"
+#include "Limits.h"
 
 void BodyPose::begin() {
   s_roll_.reset(0); s_pitch_.reset(0); s_yaw_.reset(0);
@@ -25,10 +26,17 @@ void BodyPose::update(float dt) {
 // whether offsetting +X moves the feet the way you expect) depends on your
 // leg numbering and mount angles - bench-check it the same way as the IK
 // elbow direction, and flip signs if it goes the wrong way.
-FootTarget BodyPose::apply(const FootTarget& in) const {
-  float roll  = radians(s_roll_.current + correction_roll_);
-  float pitch = radians(s_pitch_.current + correction_pitch_);
-  float yaw   = radians(s_yaw_.current);
+FootTarget BodyPose::apply(const FootTarget& in) const { return applyScaled(in, reach_scale_); }
+
+FootTarget BodyPose::applyScaled(const FootTarget& in, float s) const {
+  // The feet stay put in the world, so to rotate the body by (roll, pitch,
+  // yaw) the body-frame foot targets rotate by the inverse: negated angles,
+  // applied yaw -> pitch -> roll. Result: +roll = right side down,
+  // +pitch = nose down, +yaw = CCW (HexapodProtocol.h), checked in sim_smoke.
+  // `s` scales the whole pose (see fitToReach).
+  float roll  = -radians(s * (s_roll_.current + correction_roll_));
+  float pitch = -radians(s * (s_pitch_.current + correction_pitch_));
+  float yaw   = -radians(s * s_yaw_.current);
 
   float x = in.x, y = in.y, z = in.z;
 
@@ -43,8 +51,47 @@ FootTarget BodyPose::apply(const FootTarget& in) const {
   float z3 = y1 * sin(roll) + z2 * cos(roll);
 
   FootTarget out;
-  out.x = x2 - s_x_.current;
-  out.y = y3 - s_y_.current;
-  out.z = z3 - s_z_.current;
+  out.x = x2 - s * s_x_.current;
+  out.y = y3 - s * s_y_.current;
+  out.z = z3 - s * s_z_.current;
   return out;
 }
+
+bool BodyPose::reachable(const FootTarget sets[][6], int count, float s) const {
+  for (int k = 0; k < count; k++) {
+    FootTarget posed[6];
+    for (int i = 0; i < 6; i++) posed[i] = applyScaled(sets[k][i], s);
+    if (!Limits::feetFeasible(posed)) return false;
+  }
+  return true;
+}
+
+bool BodyPose::feasible(const FootTarget feet[6]) const {
+  FootTarget posed[6];
+  for (int i = 0; i < 6; i++) posed[i] = apply(feet[i]);
+  return Limits::feetFeasible(posed);
+}
+
+// Clamping each foot separately would bend the six feet out of their rigid
+// shape on the ground (feet dragging, legs fighting each other) as soon as
+// a combined pose asks for more than a leg can reach, and nothing would stop
+// neighbouring legs from swinging into each other. Instead the whole pose
+// is scaled down by one factor until every foot set is physically possible,
+// so the body does the most of what was asked that it really can.
+//
+// The scale itself moves at a bounded rate: a sudden drop would snap the
+// whole body, faster than the servos can follow together. While walking,
+// the gait slows down against the current pose in the meantime (see
+// GaitEngine::limitTwist), so it stays physical during the handover.
+void BodyPose::fitToReach(const FootTarget sets[][6], int count, float dt) {
+  float fit = 1.0f;
+  if (!reachable(sets, count, 1.0f)) {
+    float lo = 0.0f, hi = 1.0f;   // lo stays feasible (or 0 if even no pose isn't - leave that to Limits)
+    for (int n = 0; n < 8; n++) {
+      float mid = 0.5f * (lo + hi);
+      if (reachable(sets, count, mid)) lo = mid; else hi = mid;
+    }
+    fit = lo;
+  }
+  reach_scale_ = constrain(fit, reach_scale_ - REACH_DROP_PER_SEC * dt, reach_scale_ + REACH_RECOVER_PER_SEC * dt);
+}

2026-current/robot/BodyPose.h

@@ -20,12 +20,35 @@ public:
 
   void update(float dt);
 
-  // Applies the current (smoothed) pose transform to a body-frame foot target.
+  // Scales the whole pose down (one factor for all six legs) until every set
+  // of six feet in `sets` is still physically possible with the pose applied
+  // (reachable, no colliding legs). Pass the stride envelope while walking,
+  // so the pose fits the whole stride rather than this tick's feet, or the
+  // stand targets otherwise. Call after update() and before apply().
+  void fitToReach(const FootTarget sets[][6], int count, float dt);
+  float reachScale() const { return reach_scale_; }   // 1 = full commanded pose
+
+  // The operator's tilt as actually applied (smoothed and reach-scaled),
+  // without the gyro correction - what self-leveling should keep.
+  float commandRollDeg() const  { return s_roll_.current * reach_scale_; }
+  float commandPitchDeg() const { return s_pitch_.current * reach_scale_; }
+
+  // Applies the current (smoothed, reach-scaled) pose transform to a body-frame foot target.
   FootTarget apply(const FootTarget& in) const;
 
+  // True if these (un-posed) feet are physically possible with the current
+  // pose applied - see Limits::feetFeasible.
+  bool feasible(const FootTarget feet[6]) const;
+
 private:
   PoseCommand command_;
   float correction_roll_ = 0, correction_pitch_ = 0;
+  float reach_scale_ = 1.0f;
+  static constexpr float REACH_RECOVER_PER_SEC = 1.0f;
+  static constexpr float REACH_DROP_PER_SEC = 3.0f;
+
+  FootTarget applyScaled(const FootTarget& in, float scale) const;
+  bool reachable(const FootTarget sets[][6], int count, float scale) const;
 
   SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG};
   SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG};

2026-current/robot/Config.h

@@ -7,16 +7,20 @@
 // =====================================================================
 
 // ---- Leg geometry ----
+// Taken from the Unity model (Hexapod_Simulation_Unity/Assets/Main.unity,
+// 1 Unity unit = 10 mm): hip pivots at x = +-10 / 0, z = +-5 / +-6.5;
+// coxa 5.5, femur 10, tibia 15. BENCH: confirm against the real chassis.
 constexpr int NUM_LEGS = 6;
 // Leg order: 0=front-right, 1=mid-right, 2=rear-right, 3=rear-left, 4=mid-left, 5=front-left
 // Angles measured counterclockwise from the body's +X (forward) axis, viewed from above.
-constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -45, -90, -135, 135, 90, 45 };
-constexpr float LEG_MOUNT_X_MM[NUM_LEGS]      = {  80,   0,  -80,  -80,   0,  80 };
-constexpr float LEG_MOUNT_Y_MM[NUM_LEGS]      = { -60, -90,  -60,   60,  90,  60 };
+// This is the direction the coxa points at servo center: straight sideways on this chassis.
+constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -90, -90, -90,  90,  90,  90 };
+constexpr float LEG_MOUNT_X_MM[NUM_LEGS]      = { 100,   0, -100, -100,  0, 100 };
+constexpr float LEG_MOUNT_Y_MM[NUM_LEGS]      = { -50, -65,  -50,   50,  65,  50 };
 
-constexpr float COXA_LEN_MM  = 30.0f;
-constexpr float FEMUR_LEN_MM = 60.0f;
-constexpr float TIBIA_LEN_MM = 100.0f;
+constexpr float COXA_LEN_MM  = 55.0f;
+constexpr float FEMUR_LEN_MM = 100.0f;
+constexpr float TIBIA_LEN_MM = 150.0f;
 
 // ---- Joint limits, degrees ----
 constexpr float COXA_MIN_DEG  = -60,  COXA_MAX_DEG  = 60;
@@ -24,8 +28,21 @@ constexpr float FEMUR_MIN_DEG = -80,  FEMUR_MAX_DEG = 80;
 constexpr float TIBIA_MIN_DEG = -150, TIBIA_MAX_DEG = -20;
 
 // ---- Neutral stance, relative to each leg's own coxa pivot ----
-constexpr float STANCE_RADIUS_MM = 140.0f;  // how far out the foot rests
-constexpr float STANCE_HEIGHT_MM = 90.0f;   // how far below the coxa pivot the foot rests
+// From the Unity model's rest foot point: 130 mm out from the coxa pivot
+// (= COXA_LEN_MM + 75), 150 mm below it.
+constexpr float STANCE_RADIUS_MM = 75.0f;   // how far out the foot rests, beyond the coxa
+constexpr float STANCE_HEIGHT_MM = 150.0f;  // how far below the coxa pivot the foot rests (default height)
+// The remote's height slider (CmdPacket.body_height) moves the standing
+// height by up to this much either way (100..200 mm here - both ends checked
+// reachable in the simulation), at no more than STANCE_HEIGHT_RATE_MM_S.
+constexpr float STANCE_HEIGHT_RANGE_MM  = 50.0f;
+constexpr float STANCE_HEIGHT_RATE_MM_S = 60.0f;
+
+// ---- Leg collision ----
+// Minimum distance between the center lines of neighbouring legs' segments
+// (coxa, femur, tibia). Gait strides and body poses are scaled down to keep
+// it. BENCH: set to roughly the width of a leg (servo body + bracket).
+constexpr float LEG_CLEARANCE_MM = 30.0f;
 
 // ---- Servo pins (Teensy 4.0). Verify these are PWM-capable on your board. ----
 // Order per leg: coxa, femur, tibia.
@@ -55,6 +72,12 @@ constexpr uint8_t RADIO_CE_PIN     = 33;
 constexpr uint8_t RADIO_CSN_PIN    = 34;
 constexpr float   LINK_TIMEOUT_SEC = 0.3f;
 
+// ---- Robot-gyro self-leveling ----
+// Integral gain (1/s): a leftover tilt of 1 deg changes the correction by
+// this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot.
+constexpr float ROBOT_GYRO_KI           = 3.0f;
+constexpr float ROBOT_GYRO_MAX_CORR_DEG = 15.0f;   // steepest slope it will level out
+
 // ---- Setpoint smoothing rates ----
 constexpr float POSE_TILT_MAX_RATE_DEG  = 120.0f;  // roll/pitch/yaw offset, deg/s
 constexpr float POSE_OFFSET_MAX_RATE_MM = 150.0f;  // X/Y/Z offset, mm/s

2026-current/robot/GaitEngine.cpp

@@ -1,4 +1,5 @@
 #include "GaitEngine.h"
+#include "Limits.h"
 
 const GaitPattern GAIT_TRIPOD = {
   0.5f,
@@ -11,16 +12,79 @@ const GaitPattern GAIT_WAVE = {
 };
 
 void GaitEngine::begin() {
+  resetFeet();
+}
+
+// Every foot starts at neutral, which is the *middle* of a stride. So the
+// cycle restarts with the pattern shifted to put leg 0 at mid-stance (for
+// the tripod that puts every grounded leg at mid-stance); a grounded leg
+// that started at the beginning of its stance would instead run a full
+// stride past neutral into the workspace clamp and stop pushing. Legs that
+// land in their swing phase lift off from neutral over the remaining swing.
+void GaitEngine::resetFeet() {
   for (int i = 0; i < 6; i++) {
     legs_pattern_[i] = requested_pattern_;
-    leg_phase_[i] = legs_pattern_[i]->leg_phase_offset[i];
+    const GaitPattern* p = legs_pattern_[i];
+    float phase = p->leg_phase_offset[i] + p->duty_factor * 0.5f;
+    leg_phase_[i] = phase - floorf(phase);
+    foot_[i] = Kinematics::neutralStance(i);
+    liftoff_[i] = foot_[i];
+    swing_start_[i] = leg_phase_[i] < p->duty_factor ? p->duty_factor : leg_phase_[i];
   }
 }
 
 static float smoothstep(float t) { return t * t * (3.0f - 2.0f * t); }
 
-void GaitEngine::update(float dt, const Twist& twist, FootTarget out[6]) {
+void GaitEngine::strideEnvelope(const Twist& twist, FootTarget sets[ENVELOPE_SETS][6]) const {
+  float half = cycle_sec_ * requested_pattern_->duty_factor * 0.5f;
   for (int i = 0; i < 6; i++) {
+    FootTarget n = Kinematics::neutralStance(i);
+    float dx = (twist.vx - twist.omega * n.y) * half;   // half-stride: body velocity at the foot
+    float dy = (twist.vy + twist.omega * n.x) * half;
+    float sign = (i % 2 == 0) ? 1.0f : -1.0f;
+    sets[0][i] = { n.x + sign * dx, n.y + sign * dy, n.z };
+    sets[1][i] = { n.x - sign * dx, n.y - sign * dy, n.z };
+    sets[2][i] = { n.x, n.y, n.z + SWING_LIFT_MM };
+  }
+}
+
+const BodyPose GaitEngine::NO_POSE{};
+
+bool GaitEngine::envelopeFeasible(const Twist& twist, const BodyPose& pose) const {
+  FootTarget sets[ENVELOPE_SETS][6];
+  strideEnvelope(twist, sets);
+  for (int k = 0; k < ENVELOPE_SETS; k++)
+    if (!pose.feasible(sets[k])) return false;
+  return true;
+}
+
+// Neighbouring legs step in opposite phases, so each one's stride eats into
+// the gap between them from both sides; turning on the spot at full rate
+// asks for ~60 mm half-strides at the outer feet, enough to make the legs
+// meet. Rather than letting single feet clamp (or collide), the whole
+// command is slowed until the stride fits.
+Twist GaitEngine::limitTwist(const Twist& twist, const BodyPose& pose, bool chained) {
+  float s = 1.0f;
+  if (!envelopeFeasible(twist, pose)) {
+    float lo = 0.0f, hi = 1.0f;
+    for (int n = 0; n < 8; n++) {
+      float mid = 0.5f * (lo + hi);
+      Twist t = { twist.vx * mid, twist.vy * mid, twist.omega * mid };
+      if (envelopeFeasible(t, pose)) lo = mid; else hi = mid;
+    }
+    s = lo;
+  }
+  twist_scale_ = chained ? twist_scale_ * s : s;
+  return { twist.vx * s, twist.vy * s, twist.omega * s };
+}
+
+void GaitEngine::update(float dt, const Twist& twist, const BodyPose& pose, FootTarget out[6]) {
+  // Pass 1: advance phases, place swing feet, and work out how far each
+  // planted foot would move this tick.
+  bool stance[6];
+  FootTarget step[6] = {};
+  for (int i = 0; i < 6; i++) {
+    bool was_stance = leg_phase_[i] < legs_pattern_[i]->duty_factor;
     leg_phase_[i] += dt / cycle_sec_;
 
     // Touchdown: this leg's cycle wraps back to 0 - the one moment it's safe
@@ -33,33 +97,68 @@ void GaitEngine::update(float dt, const Twist& twist, FootTarget out[6]) {
 
     const GaitPattern* p = legs_pattern_[i];
     bool in_stance = leg_phase_[i] < p->duty_factor;
+    stance[i] = in_stance;
 
     FootTarget neutral = Kinematics::neutralStance(i);
     float stride_time = cycle_sec_ * p->duty_factor;
+    FootTarget& foot = foot_[i];
 
-    // Half-stride displacement from the commanded twist.
-    float half_len_x = twist.vx * stride_time * 0.5f;
-    float half_len_y = twist.vy * stride_time * 0.5f;
-    // Rough yaw contribution using this leg's mount position as the moment arm.
-    float yaw_dx = -twist.omega * LEG_MOUNT_Y_MM[i] * stride_time * 0.5f;
-    float yaw_dy =  twist.omega * LEG_MOUNT_X_MM[i] * stride_time * 0.5f;
-
-    FootTarget start = { neutral.x - half_len_x - yaw_dx, neutral.y - half_len_y - yaw_dy, neutral.z };
-    FootTarget end    = { neutral.x + half_len_x + yaw_dx, neutral.y + half_len_y + yaw_dy, neutral.z };
-
-    FootTarget foot;
     if (in_stance) {
-      float t = leg_phase_[i] / p->duty_factor;
-      foot.x = start.x + (end.x - start.x) * t;
-      foot.y = start.y + (end.y - start.y) * t;
+      // Planted foot: stays put in the world, so in the body frame it moves
+      // opposite to the body's velocity at that point (v + omega x r).
+      // Integrated every tick, so the body moves exactly as commanded even
+      // when the command changes mid-stride.
+      step[i].x = -(twist.vx - twist.omega * foot.y) * dt;
+      step[i].y = -(twist.vy + twist.omega * foot.x) * dt;
       foot.z = neutral.z;
     } else {
-      float t = (leg_phase_[i] - p->duty_factor) / (1.0f - p->duty_factor);
+      if (was_stance) {                      // just lifted off
+        liftoff_[i] = foot;
+        swing_start_[i] = p->duty_factor;
+      }
+
+      // Swing toward the leading end of the next stride: the point that a
+      // full stance at the current command carries back through neutral.
+      float half = stride_time * 0.5f;
+      FootTarget touchdown = {
+        neutral.x + (twist.vx - twist.omega * neutral.y) * half,
+        neutral.y + (twist.vy + twist.omega * neutral.x) * half,
+        neutral.z
+      };
+      float t = (leg_phase_[i] - swing_start_[i]) / (1.0f - swing_start_[i]);
       float st = smoothstep(t);
-      foot.x = end.x + (start.x - end.x) * st;
-      foot.y = end.y + (start.y - end.y) * st;
+      foot.x = liftoff_[i].x + (touchdown.x - liftoff_[i].x) * st;
+      foot.y = liftoff_[i].y + (touchdown.y - liftoff_[i].y) * st;
       foot.z = neutral.z + SWING_LIFT_MM * sin(PI * t);
     }
-    out[i] = foot;
+  }
+
+  // Pass 2: the planted feet move together - they're what carries the body.
+  // limitTwist() sizes the command for a steady stride, but when the command
+  // changes mid-step (a key pressed while a foot has just landed) a planted
+  // foot can be asked to travel further than its leg reaches, or into its
+  // neighbour. Then all planted feet move less, by one common factor: the
+  // body slows down rather than one foot being clamped out of formation.
+  float s = 1.0f;
+  FootTarget proposed[6];
+  auto propose = [&](float k) {
+    for (int i = 0; i < 6; i++)
+      proposed[i] = stance[i] ? FootTarget{ foot_[i].x + step[i].x * k, foot_[i].y + step[i].y * k, foot_[i].z }
+                              : foot_[i];
+    return pose.feasible(proposed);
+  };
+  if (!propose(1.0f)) {
+    float lo = 0.0f, hi = 1.0f;
+    for (int n = 0; n < 6; n++) {
+      float mid = 0.5f * (lo + hi);
+      if (propose(mid)) lo = mid; else hi = mid;
+    }
+    s = lo;
+    propose(s);
+  }
+  applied_twist_ = { twist.vx * s, twist.vy * s, twist.omega * s };
+  for (int i = 0; i < 6; i++) {
+    foot_[i] = proposed[i];
+    out[i] = foot_[i];
   }
 }

2026-current/robot/GaitEngine.h

@@ -1,5 +1,6 @@
 #pragma once
 #include "Kinematics.h"
+#include "BodyPose.h"
 #include "Config.h"
 
 struct GaitPattern {
@@ -22,14 +23,53 @@ public:
   void setCycleDuration(float sec) { cycle_sec_ = sec; }
 
   // Advances every leg by dt under the current twist and pattern, writing each
-  // leg's target foot position (body frame) into out[6].
-  void update(float dt, const Twist& twist, FootTarget out[6]);
+  // leg's target foot position (body frame, before the body pose) into out[6].
+  // `pose` is the body pose that will be applied on top: planted feet only
+  // move as far as stays physically possible with it.
+  void update(float dt, const Twist& twist, const BodyPose& pose, FootTarget out[6]);
+
+  // Puts every foot back at its neutral stance position and restarts the
+  // cycle to match. Call when walking starts, since the stand pose holds the
+  // feet at neutral in between.
+  void resetFeet();
 
   bool legInStance(int i) const { return leg_phase_[i] < legs_pattern_[i]->duty_factor; }
 
+  // The extreme foot positions a stride at this twist reaches:
+  // [0] even legs at the leading end of their stride, odd legs at the trailing end,
+  // [1] the other way round (tripod legs alternate even/odd, so neighbours are
+  //     always in opposite phases - this is when they come closest),
+  // [2] every foot at neutral, lifted by SWING_LIFT_MM (mid-swing).
+  static constexpr int ENVELOPE_SETS = 3;
+  void strideEnvelope(const Twist& twist, FootTarget sets[ENVELOPE_SETS][6]) const;
+
+  // Scales the twist down (all components together) until that envelope,
+  // with `pose` applied, is physically possible: every foot reachable and no
+  // legs colliding. Pass GaitEngine::NO_POSE for the stride alone.
+  // twistScale() reports the factor applied (1 = as commanded); with
+  // `chained`, this call's factor multiplies onto the previous call's, for a
+  // second limit applied to an already-limited twist.
+  Twist limitTwist(const Twist& twist, const BodyPose& pose, bool chained = false);
+  float twistScale() const { return twist_scale_; }
+  static const BodyPose NO_POSE;
+
+  // The twist the planted feet actually carried out in the last update()
+  // (it can be less than asked when a stride change runs a foot out of reach).
+  const Twist& appliedTwist() const { return applied_twist_; }
+
+  // Current foot targets (body frame, before the body pose).
+  const FootTarget* feet() const { return foot_; }
+
 private:
+  Twist applied_twist_;
+  bool envelopeFeasible(const Twist& twist, const BodyPose& pose) const;
+  float twist_scale_ = 1.0f;
+
   float leg_phase_[6];
   const GaitPattern* legs_pattern_[6];
+  FootTarget foot_[6];      // current foot target, body frame (stance feet are integrated)
+  FootTarget liftoff_[6];   // where each swing started
+  float swing_start_[6];    // phase at which each swing started (duty factor, except right after resetFeet)
   const GaitPattern* requested_pattern_ = &GAIT_TRIPOD;
   float cycle_sec_ = DEFAULT_CYCLE_SEC;
 };

2026-current/robot/InputMapper.cpp

@@ -30,17 +30,33 @@ MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
     out.twist.vx = raw_vx * MAX_LINEAR_MM_S;
     out.twist.vy = raw_vy * MAX_LINEAR_MM_S;
     out.twist.omega = radians(raw_yaw_in * MAX_TURN_RATE_DEG_S);
+    // Hold the current heading, so switching to mode B doesn't turn the
+    // robot toward some old target.
+    heading_target_deg_ = heading_deg;
+    stick_active_ = false;
   } else {
-    // Mode B: heading-hold. Stick angle becomes a target heading; forward
-    // speed comes from stick magnitude. yaw_rate is unused here.
+    // Mode B: steering, relative to where the body faces right now. The
+    // stick's forward/back component sets the walking direction, its sideways
+    // component steers: forward walks straight on, back walks straight back,
+    // forward+left or back+left walk while turning left, left alone turns on
+    // the spot (for as long as it's held). Let go and the robot holds the
+    // heading it has at that moment. yaw_rate is unused here.
     float mag = sqrt(raw_vx * raw_vx + raw_vy * raw_vy);
+    float direction = raw_vx < 0 ? -1.0f : 1.0f;
     if (mag > 0.05f) {
-      heading_target_deg_ = degrees(atan2(raw_vy, raw_vx));
+      stick_active_ = true;
+      heading_target_deg_ = wrapDeg(heading_deg + degrees(atan2(raw_vy, fabs(raw_vx))));
+    } else if (stick_active_) {
+      stick_active_ = false;
+      heading_target_deg_ = heading_deg;   // released: hold this heading
     }
     float heading_error = wrapDeg(heading_target_deg_ - heading_deg);
     float omega_deg_s = constrain(HEADING_KP * heading_error, -MAX_TURN_RATE_DEG_S, MAX_TURN_RATE_DEG_S);
     out.twist.omega = radians(omega_deg_s);
-    out.twist.vx = mag * MAX_LINEAR_MM_S;   // forward-only in the robot's current facing
+    // Along the robot's current facing (forward or back), scaled by how well
+    // it already faces the target: a hard turn happens mostly on the spot.
+    float facing = max(0.0f, cos(radians(heading_error)));
+    out.twist.vx = direction * mag * facing * MAX_LINEAR_MM_S;
     out.twist.vy = 0;
   }
 
@@ -66,5 +82,7 @@ MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
                                    -MAX_TILT_DEG, MAX_TILT_DEG);
   }
 
+  out.stance_height_mm = STANCE_HEIGHT_MM + normalize(p.body_height) * STANCE_HEIGHT_RANGE_MM;
+
   return out;
 }

2026-current/robot/InputMapper.h

@@ -13,6 +13,7 @@ struct MapperOutput {
   bool gait_wave = false;
   bool remote_gyro_enabled = false;
   bool robot_gyro_enabled = false;
+  float stance_height_mm = STANCE_HEIGHT_MM;   // requested standing height (feet below hips)
 };
 
 class InputMapper {
@@ -22,6 +23,7 @@ public:
 
 private:
   float heading_target_deg_ = 0;
+  bool stick_active_ = false;          // mode B: stick currently pushed
   static constexpr float MAX_TURN_RATE_DEG_S = 90.0f;
   static constexpr float HEADING_KP = 2.0f;
   static constexpr float MAX_LINEAR_MM_S = 150.0f;   // tune to your leg workspace / servo speed

2026-current/robot/Kinematics.cpp

@@ -39,8 +39,9 @@ JointAngles Kinematics::solve(int i, const FootTarget& target) {
   cos_tibia = constrain(cos_tibia, -1.0f, 1.0f);
   float tibia_rad = -(PI - acos(cos_tibia));   // knee-down convention; flip sign if yours mounts the other way
 
-  // Femur angle: elevation to the foot, plus the triangle's femur-side angle.
-  float elevation = atan2(-local_z, horiz);
+  // Femur angle: elevation to the foot (Z up, so a foot below the hip gives a
+  // negative elevation), plus the triangle's femur-side angle.
+  float elevation = atan2(local_z, horiz);
   float cos_femur_tri = (FEMUR_LEN_MM * FEMUR_LEN_MM + dist * dist - TIBIA_LEN_MM * TIBIA_LEN_MM)
                          / (2.0f * FEMUR_LEN_MM * dist);
   cos_femur_tri = constrain(cos_femur_tri, -1.0f, 1.0f);
@@ -57,11 +58,32 @@ JointAngles Kinematics::solve(int i, const FootTarget& target) {
   return out;
 }
 
+void Kinematics::jointPositions(int i, const JointAngles& a, FootTarget out[4]) {
+  float yaw = radians(LEG_MOUNT_ANGLE_DEG[i] + a.coxa_deg);
+  float femur = radians(a.femur_deg);
+  float tibia = radians(a.femur_deg + a.tibia_deg);   // tibia angle is relative to the femur
+  float cx = cos(yaw), cy = sin(yaw);
+  float r_knee = COXA_LEN_MM + FEMUR_LEN_MM * cos(femur);
+  float z_knee = FEMUR_LEN_MM * sin(femur);
+  float r_foot = r_knee + TIBIA_LEN_MM * cos(tibia);
+  float z_foot = z_knee + TIBIA_LEN_MM * sin(tibia);
+  float mx = LEG_MOUNT_X_MM[i], my = LEG_MOUNT_Y_MM[i];
+  out[0] = { mx, my, 0 };
+  out[1] = { mx + cx * COXA_LEN_MM, my + cy * COXA_LEN_MM, 0 };
+  out[2] = { mx + cx * r_knee, my + cy * r_knee, z_knee };
+  out[3] = { mx + cx * r_foot, my + cy * r_foot, z_foot };
+}
+
+static float g_stance_height_mm = STANCE_HEIGHT_MM;
+
+void Kinematics::setStanceHeight(float mm) { g_stance_height_mm = mm; }
+float Kinematics::stanceHeight() { return g_stance_height_mm; }
+
 FootTarget Kinematics::neutralStance(int i) {
   float a = radians(LEG_MOUNT_ANGLE_DEG[i]);
   FootTarget t;
   t.x = LEG_MOUNT_X_MM[i] + cos(a) * (COXA_LEN_MM + STANCE_RADIUS_MM);
   t.y = LEG_MOUNT_Y_MM[i] + sin(a) * (COXA_LEN_MM + STANCE_RADIUS_MM);
-  t.z = -STANCE_HEIGHT_MM;
+  t.z = -g_stance_height_mm;
   return t;
 }

2026-current/robot/Kinematics.h

@@ -17,6 +17,17 @@ public:
   // Solves leg i's joint angles for a body-frame foot target.
   static JointAngles solve(int leg_index, const FootTarget& target);
 
-  // The leg's resting/neutral foot target, in body frame.
+  // The leg's resting/neutral foot target, in body frame, at the current
+  // stance height.
   static FootTarget neutralStance(int leg_index);
+
+  // Standing height: how far below the hips the neutral feet sit (starts at
+  // STANCE_HEIGHT_MM). Everything built on neutralStance() - gait, sit/stand,
+  // workspace limits - follows it.
+  static void setStanceHeight(float mm);
+  static float stanceHeight();
+
+  // Forward kinematics: the leg's joint positions for the given angles, body
+  // frame - [0] coxa pivot (hip), [1] femur pivot, [2] knee, [3] foot.
+  static void jointPositions(int leg_index, const JointAngles& a, FootTarget out[4]);
 };

2026-current/robot/Limits.cpp

@@ -1,16 +1,25 @@
 #include "Limits.h"
+#include "Config.h"
 #include <Arduino.h>
 #include <math.h>
 
+static constexpr float MAX_HORIZ_MM = 70.0f;   // tune to your real leg reach, with margin
+static constexpr float MAX_VERT_MM  = 60.0f;
+
+bool Limits::inWorkspace(int i, const FootTarget& target) {
+  FootTarget neutral = Kinematics::neutralStance(i);
+  float dx = target.x - neutral.x;
+  float dy = target.y - neutral.y;
+  float dz = target.z - neutral.z;
+  return dx * dx + dy * dy <= MAX_HORIZ_MM * MAX_HORIZ_MM && fabs(dz) <= MAX_VERT_MM;
+}
+
 FootTarget Limits::clampWorkspace(int i, const FootTarget& target) {
   FootTarget neutral = Kinematics::neutralStance(i);
   float dx = target.x - neutral.x;
   float dy = target.y - neutral.y;
   float dz = target.z - neutral.z;
 
-  constexpr float MAX_HORIZ_MM = 70.0f;   // tune to your real leg reach, with margin
-  constexpr float MAX_VERT_MM  = 60.0f;
-
   float horiz = sqrt(dx * dx + dy * dy);
   if (horiz > MAX_HORIZ_MM) {
     float scale = MAX_HORIZ_MM / horiz;
@@ -22,6 +31,74 @@ FootTarget Limits::clampWorkspace(int i, const FootTarget& target) {
   return { neutral.x + dx, neutral.y + dy, neutral.z + dz };
 }
 
+// Neighbouring legs: along each side, plus the front and rear pairs.
+static const uint8_t NEIGHBOURS[][2] = { {0, 1}, {1, 2}, {3, 4}, {4, 5}, {0, 5}, {2, 3} };
+
+static FootTarget sub(const FootTarget& a, const FootTarget& b) { return { a.x - b.x, a.y - b.y, a.z - b.z }; }
+static float dot(const FootTarget& a, const FootTarget& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
+
+// Shortest distance between segments p1-q1 and p2-q2 (Ericson, Real-Time
+// Collision Detection, 5.1.9).
+static float segmentDistance(const FootTarget& p1, const FootTarget& q1,
+                             const FootTarget& p2, const FootTarget& q2) {
+  FootTarget d1 = sub(q1, p1), d2 = sub(q2, p2), r = sub(p1, p2);
+  float a = dot(d1, d1), e = dot(d2, d2), f = dot(d2, r);
+  float s = 0, t = 0;
+  constexpr float EPS = 1e-6f;
+  if (a > EPS || e > EPS) {
+    if (a <= EPS) {
+      t = constrain(f / e, 0.0f, 1.0f);
+    } else {
+      float c = dot(d1, r);
+      if (e <= EPS) {
+        s = constrain(-c / a, 0.0f, 1.0f);
+      } else {
+        float b = dot(d1, d2);
+        float denom = a * e - b * b;
+        s = denom > EPS ? constrain((b * f - c * e) / denom, 0.0f, 1.0f) : 0.0f;
+        t = (b * s + f) / e;
+        if (t < 0)      { t = 0; s = constrain(-c / a, 0.0f, 1.0f); }
+        else if (t > 1) { t = 1; s = constrain((b - c) / a, 0.0f, 1.0f); }
+      }
+    }
+  }
+  FootTarget c1 = { p1.x + d1.x * s, p1.y + d1.y * s, p1.z + d1.z * s };
+  FootTarget c2 = { p2.x + d2.x * t, p2.y + d2.y * t, p2.z + d2.z * t };
+  FootTarget d = sub(c1, c2);
+  return sqrt(dot(d, d));
+}
+
+// Joint positions of all legs for these feet; false if any leg can't reach.
+static bool solveAll(const FootTarget feet[6], FootTarget joints[6][4], bool check_workspace) {
+  for (int i = 0; i < 6; i++) {
+    if (check_workspace && !Limits::inWorkspace(i, feet[i])) return false;
+    JointAngles a = Kinematics::solve(i, feet[i]);
+    if (!a.valid) return false;
+    Kinematics::jointPositions(i, a, joints[i]);
+  }
+  return true;
+}
+
+static float minClearance(const FootTarget joints[6][4]) {
+  float best = 1e9f;
+  for (const auto& n : NEIGHBOURS)
+    for (int s1 = 0; s1 < 3; s1++)        // segments: coxa, femur, tibia
+      for (int s2 = 0; s2 < 3; s2++)
+        best = min(best, segmentDistance(joints[n[0]][s1], joints[n[0]][s1 + 1],
+                                         joints[n[1]][s2], joints[n[1]][s2 + 1]));
+  return best;
+}
+
+bool Limits::feetFeasible(const FootTarget feet[6]) {
+  FootTarget joints[6][4];
+  return solveAll(feet, joints, true) && minClearance(joints) >= LEG_CLEARANCE_MM;
+}
+
+float Limits::legClearance(const FootTarget feet[6]) {
+  FootTarget joints[6][4];
+  return solveAll(feet, joints, false) ? minClearance(joints) : -1e9f;
+}
+
 float Limits::clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt) {
   float max_step = max_deg_per_sec * dt;
   float delta = constrain(target_deg - current_deg, -max_step, max_step);

2026-current/robot/Limits.h

@@ -5,6 +5,17 @@ class Limits {
 public:
   // Clamps a foot target into a conservative safe envelope around the leg's neutral stance.
   static FootTarget clampWorkspace(int leg_index, const FootTarget& target);
+  // True if clampWorkspace() would leave the target unchanged.
+  static bool inWorkspace(int leg_index, const FootTarget& target);
+
+  // True if all six feet can be placed there at once: each target in its
+  // workspace with a valid IK solution, and no two neighbouring legs'
+  // segments closer than LEG_CLEARANCE_MM.
+  static bool feetFeasible(const FootTarget feet[6]);
+
+  // Smallest distance between neighbouring legs' segments for these feet
+  // (diagnostics; a large negative number if some leg has no IK solution).
+  static float legClearance(const FootTarget feet[6]);
 
   // Rate-limits a joint angle change to the servo's real max speed.
   static float clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt);

2026-current/robot/README.md

@@ -18,8 +18,9 @@ sketchbook `libraries/` folder and install **RF24** by TMRh20.
 
 ## Before you trust this on the floor
 
-Everything in `Config.h` is a placeholder for a generic small hexapod. Measure
-your own robot and edit:
+The leg geometry in `Config.h` is taken from the Unity model
+(`Hexapod_Simulation_Unity`, 1 unit = 10 mm). Everything else there is
+still a placeholder. Check all of it against the real robot:
 
 - `LEG_MOUNT_ANGLE_DEG` / `LEG_MOUNT_X_MM` / `LEG_MOUNT_Y_MM` — where each
   coxa pivot actually sits on your chassis, and which way it points
@@ -27,6 +28,9 @@ your own robot and edit:
 - `COXA_MIN_DEG..TIBIA_MAX_DEG` — your servos' real safe range of motion
 - `STANCE_RADIUS_MM` / `STANCE_HEIGHT_MM` — how far out/down you want the
   resting foot position
+- `LEG_CLEARANCE_MM`: how close neighbouring legs may get. Walking speed
+  and body pose are scaled down to keep it, so set it to about one leg's
+  width.
 - `SERVO_PINS` — must be PWM-capable pins on your Teensy 4.0 wiring
 - `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` — calibrate per servo, they vary
   batch to batch
@@ -43,13 +47,16 @@ These are sign conventions that depend on which way your hardware is
 physically mounted, and the code has a best-guess default that may be wrong
 for your build:
 
-1. **IK elbow/tibia direction** (`Kinematics.cpp`) — command one leg to a
-   few known foot positions by hand (e.g. via the serial harness) and watch
-   whether the knee bends the way you expect. Flip the sign noted in the
-   comment on `tibia_rad` if it bends backwards.
-2. **BodyPose roll/pitch sign** (`BodyPose.cpp`) — command a small roll/pitch
-   offset and verify the body leans the direction you asked for, not the
-   opposite.
+1. **IK knee direction** (`Kinematics.cpp`). The femur elevation sign bug
+   (the foot was solved *above* the hip) is fixed and verified in the
+   simulation: the stance pose comes out femur −1.2°, tibia −98.3°. Still
+   command one leg to a few known foot positions on the bench and check the
+   knee bends the way the sim shows.
+2. **BodyPose roll/pitch sign** (`BodyPose.cpp`). Fixed in the simulation:
+   the pose was applied as the forward rotation, which inverted roll, pitch
+   and yaw. On the bench, command a small roll/pitch and verify the body
+   leans the way the protocol says (+roll = right side down, +pitch = nose
+   down).
 3. **RobotIMU register map** (`RobotIMU.cpp`) — written for an MPU6050-style
    chip (I2C addr `0x68`). If your gyro is a different part, its register
    addresses and scale factors will differ; swap them in.
@@ -58,14 +65,22 @@ for your build:
    If your chassis is meant to rest its belly on the ground instead, this
    needs a different, sequenced approach (fold legs up rather than just
    lowering).
-5. **Gait travel direction** (`GaitEngine.cpp`) — the protocol defines
-   `+vx` = forward, `+vy` = left, `+yaw_rate` = turn left (CCW). As written,
-   the stance phase sweeps each foot from `neutral - half_stride` to
-   `neutral + half_stride`, i.e. the foot moves *with* the commanded
-   direction, which pushes the body the opposite way. Verify with `w` on the
-   bench; if the robot backs up, swap `start`/`end` in `GaitEngine::update`
-   (fixes translation and yaw together). The `z`/`c` harness keys were
-   written against the current behavior — recheck them after that fix.
+5. **Gait travel direction** (`GaitEngine.cpp`). Fixed: stance now sweeps
+   each foot opposite to the commanded motion. The smoke test checks that
+   `+vx` moves the body forward and `+yaw_rate` turns it left (CCW). On
+   the bench, confirm `w` walks forward and `z` turns left.
+6. **Per-servo calibration**. `ServoDriver` uses one
+   `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` for all 18 servos, so servo
+   center = joint angle 0 (tibia *straight*). On this build the knee servo
+   center is the tibia at 90° to the femur, and the left side is mirrored
+   (per the old Unity/servo code). That needs a per-joint offset and
+   direction before the real robot will stand.
+
+## Testing in the simulation
+
+This exact code also compiles for the PC and runs in the Unity model. See
+`../sim/README.md`. `sim/build/sim_smoke.exe` is a quick headless check
+after every change.
 
 ## Testing without a remote yet
 
@@ -79,7 +94,7 @@ keystrokes) and use:
 | `s` | toggle sit / stand |
 | `w` / `x` | walk forward / backward (`vx` ±) |
 | `q` / `e` | strafe left / right (`vy` ±) |
-| `z` / `c` | yaw left / right |
+| `z` / `c` | turn left / right (`yaw_rate` ±) |
 | `g` | toggle gait: tripod ↔ wave |
 | `m` | toggle walk mode: direct (A) ↔ heading-hold (B) |
 | space | zero all velocity (stop walking, stay in current state) |

2026-current/robot/robot.ino

@@ -8,6 +8,7 @@
 #include "Limits.h"
 #include "Comms.h"
 #include "ServoDriver.h"
+#include "SmoothedValue.h"
 
 // Uncomment to drive the robot from the Serial Monitor before a remote exists.
 // See README.md for the key map.
@@ -22,6 +23,8 @@ Comms comms;
 ServoDriver servo_driver;
 
 float current_heading_deg = 0;          // open-loop: accumulates commanded omega. See note in loop().
+SmoothedValue stance_height(STANCE_HEIGHT_RATE_MM_S);   // remote height slider, rate-limited
+float gyro_corr_roll_deg = 0, gyro_corr_pitch_deg = 0;  // robot-gyro self-leveling (integrated)
 float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting
 uint32_t last_loop_us = 0;
 RobotState last_state = RobotState::DISARMED;
@@ -41,8 +44,9 @@ void pollSerialTestInput() {
       case 'x': test_packet.vx = max(test_packet.vx - 200, -1000); test_packet.flags |= FLAG_WALK; break;
       case 'q': test_packet.vy = min(test_packet.vy + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
       case 'e': test_packet.vy = max(test_packet.vy - 200, -1000); test_packet.flags |= FLAG_WALK; break;
-      case 'z': test_packet.yaw_rate = max(test_packet.yaw_rate - 200, -1000); test_packet.flags |= FLAG_WALK; break;
-      case 'c': test_packet.yaw_rate = min(test_packet.yaw_rate + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
+      // yaw_rate + = turn left (CCW).
+      case 'z': test_packet.yaw_rate = min(test_packet.yaw_rate + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
+      case 'c': test_packet.yaw_rate = max(test_packet.yaw_rate - 200, -1000); test_packet.flags |= FLAG_WALK; break;
       case 'g': test_packet.flags ^= FLAG_GAIT_WAVE; break;
       case 'm': test_packet.flags ^= FLAG_MODE_B; break;
       case ' ':
@@ -64,6 +68,7 @@ void setup() {
   gait.begin();
   pose.begin();
   state_machine.begin();
+  stance_height.reset(STANCE_HEIGHT_MM);
 
   if (!imu.begin())   Serial.println("robot IMU not responding - stabilization disabled");
   if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only");
@@ -87,15 +92,20 @@ void loop() {
 
   MapperOutput mapped = mapper.map(cmd, current_heading_deg, dt);
 
+  // Standing height: every neutral foot position (gait, sit/stand, limits)
+  // follows this, so it's set before anything below uses them.
+  stance_height.setTarget(mapped.stance_height_mm);
+  Kinematics::setStanceHeight(stance_height.update(dt));
+
+  // Slow the walk command down to a stride the legs can really take (reach,
+  // no neighbouring legs colliding) - the stride alone; the body pose then
+  // makes room for it below.
+  gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
+  mapped.twist = gait.limitTwist(mapped.twist, GaitEngine::NO_POSE);
+
   imu.enabled = mapped.robot_gyro_enabled;
   imu.update(dt);
 
-  // Simple open-loop heading estimate: accumulates the omega we've been
-  // commanding. Good enough for mode B in a normal session; fusing the
-  // robot IMU's yaw rate in as well is the more robust upgrade if legs
-  // ever slip on your surface.
-  current_heading_deg += degrees(mapped.twist.omega) * dt;
-
   state_machine.update(mapped.armed, mapped.stand_request, mapped.walk_active, dt);
   RobotState state = state_machine.state();
 
@@ -105,23 +115,55 @@ void loop() {
   } else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) {
     servo_driver.begin();
   }
+  // The stand pose holds every foot at neutral, so walking starts from there.
+  if (state == RobotState::WALKING && last_state != RobotState::WALKING) gait.resetFeet();
   last_state = state;
 
-  gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
-
   // Stick pose + remote-tilt feedforward (gated on FLAG_REMOTE_GYRO) are
   // already combined in InputMapper.
   pose.setCommand(mapped.pose);
-  pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0,
-                      mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0);
+
+  // Robot-gyro self-leveling. The IMU measures the body's tilt in the world,
+  // which already includes the correction applied so far - so the correction
+  // integrates (a plain "correction = -measured tilt" only halves a slope).
+  // The tilt the operator asked for is subtracted first, so leveling doesn't
+  // cancel the stick. Switched off, the correction bleeds away smoothly.
+  if (mapped.robot_gyro_enabled) {
+    gyro_corr_roll_deg  -= ROBOT_GYRO_KI * (imu.rollDeg()  - pose.commandRollDeg())  * dt;
+    gyro_corr_pitch_deg -= ROBOT_GYRO_KI * (imu.pitchDeg() - pose.commandPitchDeg()) * dt;
+    gyro_corr_roll_deg  = constrain(gyro_corr_roll_deg,  -ROBOT_GYRO_MAX_CORR_DEG, ROBOT_GYRO_MAX_CORR_DEG);
+    gyro_corr_pitch_deg = constrain(gyro_corr_pitch_deg, -ROBOT_GYRO_MAX_CORR_DEG, ROBOT_GYRO_MAX_CORR_DEG);
+  } else {
+    float step = POSE_TILT_MAX_RATE_DEG * dt;
+    gyro_corr_roll_deg  = constrain(0.0f, gyro_corr_roll_deg - step,  gyro_corr_roll_deg + step);
+    gyro_corr_pitch_deg = constrain(0.0f, gyro_corr_pitch_deg - step, gyro_corr_pitch_deg + step);
+  }
+  pose.setCorrection(gyro_corr_roll_deg, gyro_corr_pitch_deg);
   pose.update(dt);
 
   FootTarget gait_targets[NUM_LEGS];
   bool in_stance[NUM_LEGS];
 
   if (state == RobotState::WALKING) {
-    gait.update(dt, mapped.twist, gait_targets);
+    // Walking has priority over the body pose: the pose shrinks until the
+    // stride the walk command wants fits (and where the feet are right now,
+    // since a stride in transition can be outside that envelope). The pose
+    // shrinks at a bounded rate - no snapping - so meanwhile the walk slows
+    // down against the pose as it is, and everything stays physical.
+    FootTarget sets[GaitEngine::ENVELOPE_SETS + 1][NUM_LEGS];
+    gait.strideEnvelope(mapped.twist, sets);
+    for (int i = 0; i < NUM_LEGS; i++) sets[GaitEngine::ENVELOPE_SETS][i] = gait.feet()[i];
+    pose.fitToReach(sets, GaitEngine::ENVELOPE_SETS + 1, dt);
+    Twist twist = gait.limitTwist(mapped.twist, pose, true);
+
+    gait.update(dt, twist, pose, gait_targets);
     for (int i = 0; i < NUM_LEGS; i++) in_stance[i] = gait.legInStance(i);
+
+    // Simple open-loop heading estimate: accumulates the omega the planted
+    // feet actually carried out (only while walking - the body only turns
+    // then). Good enough for mode B in a normal session; fusing the robot
+    // IMU's yaw rate in as well is the more robust upgrade if legs ever slip.
+    current_heading_deg += degrees(gait.appliedTwist().omega) * dt;
   } else {
     // Sit/stand: blend between a crouched and a neutral stance height, no
     // stepping. This assumes "sit" keeps all six feet grounded - if your
@@ -136,6 +178,8 @@ void loop() {
       gait_targets[i].z = sit_target.z + (neutral.z - sit_target.z) * blend;
       in_stance[i] = true;
     }
+    // Shrink the body pose as a whole until it's physically possible here.
+    pose.fitToReach(&gait_targets, 1, dt);
   }
 
   bool stable = Limits::stancePolygonOk(in_stance);