hexapod-robot/2026-current/sim/tests/sim_smoke.cpp

// Headless check of the robot firmware running in the simulation - no Unity,
// no remote. Drives it with CmdPackets the way the remote would and checks
// the observable behavior. Exit code = number of failed checks.
//
//   sim/build/sim_smoke.exe
#include "HexapodSimApi.h"
#include <HexapodProtocol.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <initializer_list>

static int g_failures = 0;
static void check(bool ok, const char* what, const char* detail = "") {
  printf("[%s] %s%s%s\n", ok ? "PASS" : "FAIL", what, detail[0] ? " - " : "", detail);
  if (!ok) g_failures++;
}

static const char* stateName(int s) {
  static const char* names[] = { "DISARMED", "SIT", "STAND", "WALKING" };
  return (s >= 0 && s < 4) ? names[s] : "?";
}

// Body yaw as actually achieved by the feet, integrated over every step:
// the lowest feet are grounded, and the 2D rotation that best maps their
// previous body-frame positions onto the new ones is minus the body's turn.
// (Same method as the Unity simulation's kinematic body.)
static float g_actual_yaw_deg = 0;
// Set HXSIM_VERBOSE=1 to print the mode B timeline and a per-tick trace of
// its first half second (stance/grounded feet, foot heights, yaw steps).
static bool g_verbose = getenv("HXSIM_VERBOSE") != nullptr;
static bool g_trace = false;

static void groundedMask(const float* feet, bool* grounded) {
  float min_z = 1e9f;
  for (int i = 0; i < 6; i++) min_z = fminf(min_z, feet[i * 3 + 2]);
  for (int i = 0; i < 6; i++) grounded[i] = feet[i * 3 + 2] <= min_z + 5.0f;
}

static void trackYaw(const float* before, const float* after) {
  bool g0[6], g1[6];
  groundedMask(before, g0);
  groundedMask(after, g1);
  float c0x = 0, c0y = 0, c1x = 0, c1y = 0; int m = 0;
  for (int i = 0; i < 6; i++)
    if (g0[i] && g1[i]) { c0x += before[i*3]; c0y += before[i*3+1]; c1x += after[i*3]; c1y += after[i*3+1]; m++; }
  if (m < 2) return;
  c0x /= m; c0y /= m; c1x /= m; c1y /= m;
  float s_cross = 0, s_dot = 0;
  for (int i = 0; i < 6; i++) {
    if (!(g0[i] && g1[i])) continue;
    float ax = before[i*3] - c0x, ay = before[i*3+1] - c0y, bx = after[i*3] - c1x, by = after[i*3+1] - c1y;
    s_cross += ax * by - ay * bx;
    s_dot += ax * bx + ay * by;
  }
  g_actual_yaw_deg -= atan2f(s_cross, s_dot) * 57.29578f;
}

// ---- Physical-plausibility monitor, enabled per test section ----
// Leg clearance is measured independently of the firmware: joint positions
// by forward kinematics from the decoded servo angles, then the closest
// approach between points sampled along neighbouring legs' segments.
static HxGeometry g_geo;
static bool g_monitor = false;
static float g_min_clearance, g_max_rigid_err, g_min_pose_scale, g_max_pose_scale;
static float g_min_roll, g_max_roll, g_min_pitch, g_max_pitch, g_max_body_step_deg;
static void resetMonitor() {
  g_min_clearance = 1e9f; g_max_rigid_err = 0; g_min_pose_scale = 1e9f; g_max_pose_scale = -1e9f;
  g_min_roll = g_min_pitch = 1e9f; g_max_roll = g_max_pitch = -1e9f; g_max_body_step_deg = 0;
}

// Kinematic body (what Unity shows): world roll/pitch in degrees.
static void bodyRollPitch(float& roll, float& pitch, float q_out[4] = nullptr) {
  float q[4]; hxsim_get_body(nullptr, q, nullptr, nullptr);
  float w = q[0], x = q[1], y = q[2], z = q[3];
  roll = atan2f(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)) * 57.29578f;
  pitch = asinf(fmaxf(-1.0f, fminf(1.0f, 2 * (w * y - z * x)))) * 57.29578f;
  if (q_out) for (int k = 0; k < 4; k++) q_out[k] = q[k];
}

struct P3 { float x, y, z; };
static void legJoints(const float* deg, int i, P3 out[4]) {
  float yaw = (g_geo.mount_angle_deg[i] + deg[i*3]) * 0.0174533f;
  float f = deg[i*3+1] * 0.0174533f, t = (deg[i*3+1] + deg[i*3+2]) * 0.0174533f;
  float r1 = g_geo.coxa_mm, r2 = r1 + g_geo.femur_mm * cosf(f), r3 = r2 + g_geo.tibia_mm * cosf(t);
  float z2 = g_geo.femur_mm * sinf(f), z3 = z2 + g_geo.tibia_mm * sinf(t);
  float mx = g_geo.mount_x_mm[i], my = g_geo.mount_y_mm[i], c = cosf(yaw), s = sinf(yaw);
  out[0] = { mx, my, 0 };
  out[1] = { mx + c * r1, my + s * r1, 0 };
  out[2] = { mx + c * r2, my + s * r2, z2 };
  out[3] = { mx + c * r3, my + s * r3, z3 };
}
static float clearanceNow() {
  static const int pairs[][2] = { {0,1}, {1,2}, {3,4}, {4,5}, {0,5}, {2,3} };
  float deg[18]; hxsim_get_joints_deg(deg);
  P3 j[6][4];
  for (int i = 0; i < 6; i++) legJoints(deg, i, j[i]);
  float best = 1e9f;
  constexpr int N = 16;
  for (auto& pr : pairs)
    for (int sa = 0; sa < 3; sa++)
      for (int sb = 0; sb < 3; sb++)
        for (int u = 0; u <= N; u++)
          for (int v = 0; v <= N; v++) {
            const P3 &a0 = j[pr[0]][sa], &a1 = j[pr[0]][sa+1], &b0 = j[pr[1]][sb], &b1 = j[pr[1]][sb+1];
            float ua = (float)u / N, vb = (float)v / N;
            float dx = a0.x + (a1.x - a0.x) * ua - b0.x - (b1.x - b0.x) * vb;
            float dy = a0.y + (a1.y - a0.y) * ua - b0.y - (b1.y - b0.y) * vb;
            float dz = a0.z + (a1.z - a0.z) * ua - b0.z - (b1.z - b0.z) * vb;
            best = fminf(best, sqrtf(dx*dx + dy*dy + dz*dz));
          }
  return best;
}

static float footDist(const float* f, int a, int b) {
  return hypotf(hypotf(f[a*3] - f[b*3], f[a*3+1] - f[b*3+1]), f[a*3+2] - f[b*3+2]);
}

// Sends `p` at 100 Hz (like the remote) for `seconds` of simulated time.
static void run(CmdPacket p, float seconds) {
  int steps = (int)(seconds * 100.0f + 0.5f);
  for (int n = 0; n < steps; n++) {
    float before[18]; hxsim_get_feet_mm(before);
    HxStatus s0; hxsim_get_status(&s0);
    float r0, p0, q0[4]; bodyRollPitch(r0, p0, q0);
    hxsim_radio_send(reinterpret_cast<const uint8_t*>(&p), sizeof(p));
    p.seq++;
    hxsim_step(10000);
    float after[18]; hxsim_get_feet_mm(after);
    float yaw_before = g_actual_yaw_deg;
    trackYaw(before, after);
    if (g_monitor) {
      HxStatus s1; hxsim_get_status(&s1);
      g_min_clearance = fminf(g_min_clearance, clearanceNow());
      g_min_pose_scale = fminf(g_min_pose_scale, s1.pose_reach_scale);
      g_max_pose_scale = fmaxf(g_max_pose_scale, s1.pose_reach_scale);
      float r1, p1, q1[4]; bodyRollPitch(r1, p1, q1);
      g_min_roll = fminf(g_min_roll, r1); g_max_roll = fmaxf(g_max_roll, r1);
      g_min_pitch = fminf(g_min_pitch, p1); g_max_pitch = fmaxf(g_max_pitch, p1);
      // How far the body rotated in this 10 ms step (a flip or snap shows up here).
      float qd = fabsf(q0[0]*q1[0] + q0[1]*q1[1] + q0[2]*q1[2] + q0[3]*q1[3]);
      g_max_body_step_deg = fmaxf(g_max_body_step_deg, 2 * acosf(fminf(1.0f, qd)) * 57.29578f);
      // Feet planted through this step must keep their distances (the body
      // moves rigidly over them); a clamp bending one foot shows up here.
      int both = s0.stance_mask & s1.stance_mask;
      if (s1.robot_state != 3) both = 0x3F;   // standing: all six planted
      for (int a = 0; a < 6; a++)
        for (int b = a + 1; b < 6; b++)
          if ((both >> a & 1) && (both >> b & 1))
            g_max_rigid_err = fmaxf(g_max_rigid_err, fabsf(footDist(after, a, b) - footDist(before, a, b)));
    }
    if (g_trace) {
      bool g1[6]; groundedMask(after, g1);
      HxStatus s; hxsim_get_status(&s);
      printf("      %s stance ", stateName(s.robot_state));
      for (int i = 0; i < 6; i++) putchar((s.stance_mask >> i) & 1 ? '#' : '.');
      printf(" grounded ");
      for (int i = 0; i < 6; i++) putchar(g1[i] ? '#' : '.');
      printf(" z");
      for (int i = 0; i < 6; i++) printf(" %6.1f", after[i * 3 + 2]);
      printf("  dyaw %+.2f est %.1f\n", g_actual_yaw_deg - yaw_before, s.heading_deg);
    }
  }
}

static float wrap180(float d) { while (d > 180) d -= 360; while (d < -180) d += 360; return d; }

// Mean foot z over a set of legs (body frame).
static float meanZ(const float* feet, std::initializer_list<int> legs) {
  float s = 0; for (int i : legs) s += feet[i * 3 + 2]; return s / legs.size();
}
static void runSilent(float seconds) { hxsim_step((uint32_t)(seconds * 1e6f)); }

static HxStatus status() { HxStatus s; hxsim_get_status(&s); return s; }

static uint32_t totalWrites() {
  uint32_t w[18]; hxsim_get_servos(nullptr, nullptr, w);
  uint32_t sum = 0; for (uint32_t v : w) sum += v;
  return sum;
}

static void printLegs(const char* label) {
  float deg[18], feet[18];
  hxsim_get_joints_deg(deg);
  hxsim_get_feet_mm(feet);
  printf("  %s\n", label);
  for (int i = 0; i < 6; i++)
    printf("    leg %d  coxa %7.1f femur %7.1f tibia %7.1f  | foot %7.1f %7.1f %7.1f mm\n", i,
           deg[i * 3], deg[i * 3 + 1], deg[i * 3 + 2], feet[i * 3], feet[i * 3 + 1], feet[i * 3 + 2]);
}

int main() {
  char buf[512];
  check(hxsim_api_version() == HXSIM_API_VERSION, "API version matches header");
  snprintf(buf, sizeof(buf), "%d bytes", hxsim_packet_size());
  check(hxsim_packet_size() == (int)sizeof(CmdPacket), "CmdPacket size", buf);

  HxGeometry g; hxsim_get_geometry(&g);
  g_geo = g;
  printf("  geometry: coxa %.0f femur %.0f tibia %.0f mm, stance r %.0f h %.0f mm, %.0f Hz\n",
         g.coxa_mm, g.femur_mm, g.tibia_mm, g.stance_radius_mm, g.stance_height_mm, g.control_hz);

  hxsim_start();

  // ---- Power-on, no packets: disarmed ----
  runSilent(0.5f);
  check(status().robot_state == 0, "starts DISARMED with no link");

  // ---- Arm -> SIT ----
  CmdPacket p;
  p.flags = FLAG_ARMED;
  run(p, 0.5f);
  HxStatus s = status();
  check(s.link_ok, "link comes up from radio packets");
  snprintf(buf, sizeof(buf), "state %s", stateName(s.robot_state));
  check(s.robot_state == 1, "arming lands in SIT", buf);

  // ---- Stand ----
  uint32_t writes_before = totalWrites();
  p.flags = FLAG_ARMED | FLAG_STAND;
  run(p, 1.5f);
  s = status();
  snprintf(buf, sizeof(buf), "state %s, blend %.2f", stateName(s.robot_state), s.sit_stand_blend);
  check(s.robot_state == 2 && s.sit_stand_blend > 0.99f, "stand request reaches STAND", buf);
  uint32_t writes = totalWrites() - writes_before;
  snprintf(buf, sizeof(buf), "%u servo writes in 1.5 s (0 = every IK solution was rejected as invalid)", writes);
  check(writes > 0, "IK produces valid joint solutions at the stance pose", buf);
  printLegs("standing:");

  float feet[18]; hxsim_get_feet_mm(feet);
  float worst = 0;
  for (int i = 0; i < 6; i++) worst = fmaxf(worst, fabsf(feet[i * 3 + 2] + g.stance_height_mm));
  snprintf(buf, sizeof(buf), "worst foot height error %.1f mm vs -%.0f", worst, g.stance_height_mm);
  check(worst < 10.0f, "standing feet are at STANCE_HEIGHT below the hips (FK of servo output)", buf);

  // ---- Walk forward: grounded feet must move backward relative to the body ----
  p.vx = 600;
  p.speed_scale = 255;
  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK;
  run(p, 0.5f);
  s = status();
  snprintf(buf, sizeof(buf), "state %s", stateName(s.robot_state));
  check(s.robot_state == 3, "FLAG_WALK while standing -> WALKING", buf);

  float dx_sum = 0; int samples = 0;
  for (int n = 0; n < 200; n++) {
    float before[18]; hxsim_get_feet_mm(before);
    int mask = status().stance_mask;
    run(p, 0.01f);
    float after[18]; hxsim_get_feet_mm(after);
    for (int i = 0; i < 6; i++)
      if ((mask & (1 << i)) && (status().stance_mask & (1 << i))) { dx_sum += after[i * 3] - before[i * 3]; samples++; }
  }
  snprintf(buf, sizeof(buf), "mean stance-foot dx %.3f mm/step over %d samples (must be < 0)", samples ? dx_sum / samples : 0.0f, samples);
  check(samples > 0 && dx_sum < 0, "+vx pushes the body forward (stance feet sweep backward)", buf);

  // ---- Turn left: grounded feet must rotate clockwise around the body ----
  p.vx = 0;
  p.yaw_rate = 600;
  run(p, 0.5f);
  float turn_sum = 0; samples = 0;
  for (int n = 0; n < 200; n++) {
    float before[18]; hxsim_get_feet_mm(before);
    int mask = status().stance_mask;
    run(p, 0.01f);
    float after[18]; hxsim_get_feet_mm(after);
    for (int i = 0; i < 6; i++)
      if ((mask & (1 << i)) && (status().stance_mask & (1 << i))) {
        // z of (position x displacement): > 0 means CCW about the body center.
        float dx = after[i * 3] - before[i * 3], dy = after[i * 3 + 1] - before[i * 3 + 1];
        turn_sum += before[i * 3] * dy - before[i * 3 + 1] * dx;
        samples++;
      }
  }
  snprintf(buf, sizeof(buf), "mean stance-foot angular sweep %.1f (must be < 0 = clockwise)", samples ? turn_sum / samples : 0.0f);
  check(samples > 0 && turn_sum < 0, "+yaw_rate turns the body left (stance feet sweep clockwise)", buf);

  // ---- Mode A: the body actually turns at the commanded rate ----
  // (robot.ino's heading estimate integrates the *commanded* omega; if the
  // gait turns slower or faster, mode B's heading hold is off by that ratio.)
  p.yaw_rate = 1000;   // = MAX_TURN_RATE_DEG_S
  float est0 = status().heading_deg, act0 = g_actual_yaw_deg;
  resetMonitor(); g_monitor = true;
  run(p, 3.0f);
  g_monitor = false;
  float est_turn = status().heading_deg - est0, act_turn = g_actual_yaw_deg - act0;
  snprintf(buf, sizeof(buf), "estimated %.0f deg (walk scaled to %.0f%%), body actually turned %.0f deg in 3 s",
           est_turn, status().twist_scale * 100, act_turn);
  check(est_turn > 0 && act_turn / est_turn > 0.8f && act_turn / est_turn < 1.25f,
        "mode A: actual turn rate matches the heading estimate (within 20%)", buf);
  snprintf(buf, sizeof(buf), "closest neighbouring legs %.1f mm (limit %.0f)", g_min_clearance, g.leg_clearance_mm);
  check(g_min_clearance >= g.leg_clearance_mm - 2, "turning at full rate keeps neighbouring legs apart", buf);
  p.yaw_rate = 0;

  // ---- Heading estimate doesn't run on while not walking ----
  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_MODE_B;
  p.vy = 1000;                                    // stick left: target heading +90
  run(p, 0.05f);
  p.vx = 0; p.vy = 0;
  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_MODE_B; // no FLAG_WALK -> STAND, feet don't move
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  run(p, 1.0f);
  snprintf(buf, sizeof(buf), "estimate moved %.1f deg, body moved %.1f deg while standing",
           status().heading_deg - est0, g_actual_yaw_deg - act0);
  check(fabsf(status().heading_deg - est0) < 1.0f, "heading estimate holds while standing still", buf);

  // ---- Mode B: steering, relative to where the body faces right now ----
  auto bodyPlanar = [](float& x, float& y) { float pos[3]; hxsim_get_body(pos, nullptr, nullptr, nullptr); x = pos[0]; y = pos[1]; };
  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK | FLAG_MODE_B;

  // Forward (W): straight on, no turning.
  p.vx = 1000; p.vy = 0;
  run(p, 1.0f);
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  run(p, 2.0f);
  snprintf(buf, sizeof(buf), "estimate changed %.1f deg, body turned %.1f deg",
           wrap180(status().heading_deg - est0), g_actual_yaw_deg - act0);
  check(fabsf(wrap180(status().heading_deg - est0)) < 3.0f && fabsf(g_actual_yaw_deg - act0) < 5.0f,
        "mode B: forward walks straight on", buf);

  // Forward + left (W+A), held: keeps arcing left while still moving.
  p.vx = 707; p.vy = 707;
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  float bx0, by0, bx1, by1; bodyPlanar(bx0, by0);
  run(p, 2.0f);
  bodyPlanar(bx1, by1);
  float moved = hypotf(bx1 - bx0, by1 - by0);
  snprintf(buf, sizeof(buf), "turned %.0f deg (estimate %.0f) and moved %.0f mm in 2 s",
           g_actual_yaw_deg - act0, status().heading_deg - est0, moved);
  check(g_actual_yaw_deg - act0 > 30.0f && moved > 30.0f, "mode B: forward+left held steers left while walking", buf);

  // Left alone (A), held: keeps turning on the spot - not stopping at 90.
  p.vx = 0; p.vy = 1000;
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  for (int k = 0; k < 6; k++) {
    g_trace = g_verbose && k == 0;
    run(p, 0.5f);
    g_trace = false;
    if (g_verbose)
      printf("    t %.1f  state %s  estimate %+6.1f  actual %+6.1f\n", (k + 1) * 0.5f,
             stateName(status().robot_state), status().heading_deg - est0, g_actual_yaw_deg - act0);
  }
  float est_turn_b = status().heading_deg - est0, act_turn_b = g_actual_yaw_deg - act0;
  snprintf(buf, sizeof(buf), "3 s held: estimate %.0f deg, body actually turned %.0f deg", est_turn_b, act_turn_b);
  check(act_turn_b > 120.0f && act_turn_b / est_turn_b > 0.8f && act_turn_b / est_turn_b < 1.25f,
        "mode B: left held keeps turning left (estimate tracks the body)", buf);

  // Let go: the heading holds.
  p.vx = 0; p.vy = 0;
  run(p, 0.2f);
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  run(p, 1.0f);
  snprintf(buf, sizeof(buf), "estimate moved %.1f deg, body %.1f deg after release",
           wrap180(status().heading_deg - est0), g_actual_yaw_deg - act0);
  check(fabsf(g_actual_yaw_deg - act0) < 3.0f, "mode B: releasing the stick holds the heading", buf);

  // Back (S): walks straight backward - no turning around.
  p.vx = -1000; p.vy = 0;
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  float yaw_b; hxsim_get_body(nullptr, nullptr, &yaw_b, nullptr);
  bodyPlanar(bx0, by0);
  run(p, 2.0f);
  bodyPlanar(bx1, by1);
  float along = (bx1 - bx0) * cosf(yaw_b * 0.0174533f) + (by1 - by0) * sinf(yaw_b * 0.0174533f);
  snprintf(buf, sizeof(buf), "moved %.0f mm along its facing (must be < 0), turned %.1f deg", along, g_actual_yaw_deg - act0);
  check(along < -30.0f && fabsf(g_actual_yaw_deg - act0) < 5.0f, "mode B: back walks straight backward", buf);

  // Forward again: straight on in the new facing.
  p.vx = 1000;
  est0 = status().heading_deg; act0 = g_actual_yaw_deg;
  run(p, 3.0f);
  snprintf(buf, sizeof(buf), "estimate changed %.1f deg, body turned %.1f deg while walking forward",
           wrap180(status().heading_deg - est0), g_actual_yaw_deg - act0);
  check(fabsf(wrap180(status().heading_deg - est0)) < 3.0f && fabsf(g_actual_yaw_deg - act0) < 5.0f,
        "mode B: forward after a turn walks straight on in the body's new facing", buf);
  p.vx = 0; p.vy = 0;

  // ---- Body pose from the right stick, standing still ----
  // Protocol: +roll = right side down, +pitch = nose down, +yaw = CCW, and
  // translation +X = body forward. In body frame that means: roll -> right
  // feet higher than left, pitch -> front feet higher than rear, yaw -> feet
  // rotate clockwise, +X offset -> feet move backward.
  p.flags = FLAG_ARMED | FLAG_STAND;
  run(p, 1.0f);
  float neutral[18]; hxsim_get_feet_mm(neutral);
  float f[18];
  const auto right = { 0, 1, 2 }, left = { 3, 4, 5 };

  p.rx = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.rx = 0;
  snprintf(buf, sizeof(buf), "right feet z %.1f, left feet z %.1f mm", meanZ(f, right), meanZ(f, left));
  check(meanZ(f, right) > meanZ(f, left) + 5, "right stick +roll lowers the body's right side", buf);

  p.ry = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.ry = 0;
  snprintf(buf, sizeof(buf), "front feet z %.1f, rear feet z %.1f mm", meanZ(f, { 0, 5 }), meanZ(f, { 2, 3 }));
  check(meanZ(f, { 0, 5 }) > meanZ(f, { 2, 3 }) + 5, "right stick +pitch lowers the nose", buf);

  p.rz = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.rz = 0;
  float sweep = 0;
  for (int i = 0; i < 6; i++)
    sweep += neutral[i*3] * (f[i*3+1] - neutral[i*3+1]) - neutral[i*3+1] * (f[i*3] - neutral[i*3]);
  snprintf(buf, sizeof(buf), "feet angular sweep %.0f (must be < 0 = clockwise)", sweep);
  check(sweep < 0, "right stick +yaw turns the body left (feet rotate clockwise under it)", buf);

  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_RSTICK_XLATE;
  p.rx = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.rx = 0;
  float dx = 0; for (int i = 0; i < 6; i++) dx += (f[i*3] - neutral[i*3]) / 6;
  snprintf(buf, sizeof(buf), "mean foot dx %.1f mm (must be < 0)", dx);
  check(dx < -5, "right stick +X translation moves the body forward", buf);
  // ---- Height slider: standing height across its whole range ----
  for (int16_t h : { (int16_t)1000, (int16_t)-1000 }) {
    p.flags = FLAG_ARMED | FLAG_STAND;
    p.body_height = h;
    run(p, 3.0f);   // rate-limited: range/rate is < 1 s, plus margin
    hxsim_get_feet_mm(f);
    float want = -(g.stance_height_mm + h / 1000.0f * g.stance_height_range_mm);
    float worst_h = 0;
    for (int i = 0; i < 6; i++) worst_h = fmaxf(worst_h, fabsf(f[i*3+2] - want));
    uint32_t w0[18]; hxsim_get_servos(nullptr, nullptr, w0);
    run(p, 0.2f);
    uint32_t w1[18]; hxsim_get_servos(nullptr, nullptr, w1);
    int frozen = 0; for (int k = 0; k < 18; k++) if (w1[k] == w0[k]) frozen++;
    snprintf(buf, sizeof(buf), "body_height %d: feet %.1f mm off the %.0f mm target, %d/18 servos frozen",
             h, worst_h, -want, frozen);
    check(worst_h < 2.0f && frozen == 0, "height slider sets the standing height", buf);
  }
  p.body_height = 0;
  run(p, 3.0f);
  hxsim_get_feet_mm(neutral);   // back at default height for the pose tests

  // ---- Combined full-deflection poses stay physically possible ----
  // All six feet are planted while standing, so whatever the body does, the
  // feet must keep their distances to each other (a rigid shape on the
  // ground) and every leg must keep getting valid servo commands.
  struct Combo { const char* name; uint8_t flags; int16_t rx, ry, rz; };
  const Combo combos[] = {
    { "roll+pitch+yaw", FLAG_ARMED | FLAG_STAND, 1000, 1000, 1000 },
    { "roll-pitch+yaw", FLAG_ARMED | FLAG_STAND, -1000, 1000, 1000 },
    { "x+y+z offset",   FLAG_ARMED | FLAG_STAND | FLAG_RSTICK_XLATE, 1000, 1000, 1000 },
    { "x-y-z offset",   FLAG_ARMED | FLAG_STAND | FLAG_RSTICK_XLATE, 1000, -1000, -1000 },
  };
  for (const Combo& c : combos) {
    p.flags = c.flags; p.rx = c.rx; p.ry = c.ry; p.rz = c.rz;
    run(p, 2.0f);   // let the pose smoothers get all the way there
    uint32_t w0[18]; hxsim_get_servos(nullptr, nullptr, w0);
    run(p, 0.2f);
    uint32_t w1[18]; hxsim_get_servos(nullptr, nullptr, w1);
    int frozen = 0; for (int k = 0; k < 18; k++) if (w1[k] == w0[k]) frozen++;
    hxsim_get_feet_mm(f);
    float worst = 0;
    for (int a = 0; a < 6; a++)
      for (int b = a + 1; b < 6; b++) {
        float d0 = hypotf(hypotf(neutral[a*3] - neutral[b*3], neutral[a*3+1] - neutral[b*3+1]), neutral[a*3+2] - neutral[b*3+2]);
        float d1 = hypotf(hypotf(f[a*3] - f[b*3], f[a*3+1] - f[b*3+1]), f[a*3+2] - f[b*3+2]);
        worst = fmaxf(worst, fabsf(d1 - d0));
      }
    float clear = clearanceNow();
    snprintf(buf, sizeof(buf), "%s: feet distorted by up to %.1f mm, %d/18 servos frozen, legs %.0f mm apart",
             c.name, worst, frozen, clear);
    check(worst < 3.0f && frozen == 0 && clear >= g.leg_clearance_mm - 2,
          "full-deflection pose keeps the planted feet rigid and the legs apart", buf);
  }
  p.rx = p.ry = p.rz = 0;

  // ---- Walking (WASD) while posing (UIOJKL) ----
  // Must stay physical (planted feet rigid, legs apart) and steady: the pose
  // scale must not saw up and down with the step cycle.
  const Combo walk_combos[] = {
    { "walk fwd + roll/pitch/yaw", FLAG_ARMED | FLAG_STAND | FLAG_WALK, 1000, 1000, 1000 },
    { "walk fwd + x/y/z offset",   FLAG_ARMED | FLAG_STAND | FLAG_WALK | FLAG_RSTICK_XLATE, 1000, -1000, 1000 },
  };
  for (const Combo& c : walk_combos) {
    p.flags = c.flags; p.rx = c.rx; p.ry = c.ry; p.rz = c.rz;
    p.vx = 1000; p.vy = 500; p.yaw_rate = 500;
    run(p, 3.0f);   // pose smoothers settle
    resetMonitor(); g_monitor = true;
    run(p, 2.0f);
    g_monitor = false;
    snprintf(buf, sizeof(buf), "%s: planted feet drift %.1f mm, legs >= %.0f mm apart, pose %.0f-%.0f%%, walk %.0f%%, "
             "body roll %.1f..%.1f pitch %.1f..%.1f, max step %.1f deg",
             c.name, g_max_rigid_err, g_min_clearance, g_min_pose_scale * 100, g_max_pose_scale * 100,
             status().twist_scale * 100, g_min_roll, g_max_roll, g_min_pitch, g_max_pitch, g_max_body_step_deg);
    check(g_max_rigid_err < 1.0f && g_min_clearance >= g.leg_clearance_mm - 2 &&
          g_max_pose_scale - g_min_pose_scale < 0.05f &&
          g_max_roll - g_min_roll < 3.0f && g_max_pitch - g_min_pitch < 3.0f && g_max_body_step_deg < 3.0f,
          "walking while posing stays physical and steady (feet, legs, body)", buf);
  }

  // ---- Like a person on the keyboard: walk and rotate keys going on/off ----
  // Sticks ramp like SimRemote's keyboard sticks (4/s), keys change every
  // 0.3-0.7 s. The body must never snap or tip beyond the tilt limit.
  {
    p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK;
    resetMonitor(); g_monitor = true;
    float sx = 0, sr = 0, sp = 0, sy = 0;
    for (int n = 0; n < 500; n++) {                       // 5 s at 100 Hz
      float t = n * 0.01f;
      float tx = fmodf(t, 1.4f) < 0.7f ? 1.0f : 0.0f;    // W on/off
      float tr = fmodf(t, 0.6f) < 0.3f ? 1.0f : -1.0f;   // L / J alternating
      float tp = fmodf(t, 1.0f) < 0.5f ? 1.0f : 0.0f;    // I on/off
      float ty = fmodf(t, 0.8f) < 0.4f ? 1.0f : 0.0f;    // U on/off
      auto ramp = [](float v, float target) { float d = target - v; return v + fmaxf(-0.04f, fminf(0.04f, d)); };
      sx = ramp(sx, tx); sr = ramp(sr, tr); sp = ramp(sp, tp); sy = ramp(sy, ty);
      p.vx = (int16_t)(sx * 1000); p.rx = (int16_t)(sr * 1000); p.ry = (int16_t)(sp * 1000); p.rz = (int16_t)(sy * 1000);
      run(p, 0.01f);
    }
    g_monitor = false;
    float tilt = fmaxf(fmaxf(fabsf(g_min_roll), fabsf(g_max_roll)), fmaxf(fabsf(g_min_pitch), fabsf(g_max_pitch)));
    snprintf(buf, sizeof(buf), "planted feet drift %.1f mm, legs >= %.0f mm apart, body tilt up to %.1f deg, "
             "max body step %.1f deg / 10 ms", g_max_rigid_err, g_min_clearance, tilt, g_max_body_step_deg);
    check(g_max_rigid_err < 1.5f && g_min_clearance >= g.leg_clearance_mm - 2 && tilt < 23.0f &&
          g_max_body_step_deg < 3.0f, "walking while toggling rotate keys stays smooth", buf);
  }
  p.vx = p.vy = p.yaw_rate = 0;
  p.rx = p.ry = p.rz = 0;

  // ---- Standing: the body really tilts as commanded ----
  p.flags = FLAG_ARMED | FLAG_STAND;
  p.rx = 500;   // +10 deg roll = right side down
  run(p, 2.0f);
  float br, bp; bodyRollPitch(br, bp);
  snprintf(buf, sizeof(buf), "body roll %.1f deg (want +10), pitch %.1f", br, bp);
  check(fabsf(br - 10.0f) < 1.0f && fabsf(bp) < 1.0f, "standing +roll tilts the simulated body right side down", buf);
  p.rx = 0;
  run(p, 2.0f);

  // ---- Robot gyro: on tilted ground the body levels itself ----
  // (Total tilt, since the robot's heading turns a ground roll into body pitch.)
  auto tiltNow = []() {
    float q[4]; hxsim_get_body(nullptr, q, nullptr, nullptr);
    return acosf(fmaxf(-1.0f, fminf(1.0f, 1 - 2 * (q[1] * q[1] + q[2] * q[2])))) * 57.29578f;
  };
  hxsim_set_ground_tilt(8.0f, 0.0f);
  p.flags = FLAG_ARMED | FLAG_STAND;
  run(p, 2.0f);
  float uncorrected = tiltNow();
  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_ROBOT_GYRO;
  run(p, 2.0f);
  float t_min = 1e9f, t_max = -1e9f;
  for (int n = 0; n < 200; n++) { run(p, 0.01f); float t = tiltNow(); t_min = fminf(t_min, t); t_max = fmaxf(t_max, t); }
  snprintf(buf, sizeof(buf), "ground tilted 8 deg: body %.1f deg without, %.1f..%.1f deg with robot gyro",
           uncorrected, t_min, t_max);
  check(t_max < 1.5f, "robot gyro levels the body on tilted ground, without oscillating", buf);
  hxsim_set_ground_tilt(0, 0);
  p.flags = FLAG_ARMED | FLAG_STAND;
  run(p, 2.0f);

  p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK;
  run(p, 1.0f);

  // ---- Link loss -> disarm ----
  runSilent(0.5f);
  s = status();
  check(!s.link_ok && s.robot_state == 0, "link loss (>0.3 s silence) disarms the robot");

  // ---- Protocol mismatch fails closed ----
  uint8_t short_payload[16] = {};
  short_payload[10] = FLAG_ARMED;
  for (int n = 0; n < 20; n++) { hxsim_radio_send(short_payload, sizeof(short_payload)); hxsim_step(10000); }
  check(!status().link_ok, "a wrong-size (old protocol) packet is never received");

  // ---- Serial test harness path ----
  hxsim_serial_input("a", 1);
  runSilent(0.1f);
  check(status().robot_state == 1, "serial harness 'a' arms the robot (TEST_SERIAL_INPUT)");

  int n = hxsim_serial_output(buf, sizeof(buf) - 1);
  buf[n] = 0;
  if (n) printf("  robot serial output:\n%s\n", buf);

  printf("%d check(s) failed\n", g_failures);
  return g_failures;
}