// 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 #include #include #include #include #include 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(&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 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; }