diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index 670689a..b6fb846 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -149,15 +149,15 @@ export class BezierState extends GaitState { this.mode = mode if (mode === 'crawl') { - this.speed_factor = 0.1 + this.speed_factor = 0.5 this.stand_offset = duty ?? 0.85 - const o = order ?? [0, 3, 1, 2] + const o = order ?? [3, 0, 2, 1] const base = [0, 0.25, 0.5, 0.75] const offsets = new Array(4).fill(0) for (let i = 0; i < 4; i++) offsets[o[i]] = base[i] this.offset = offsets } else { - this.speed_factor = 1 + this.speed_factor = 2 this.stand_offset = duty ?? 0.6 this.offset = order ? (order.map(v => v % 1) as number[]) : [0, 0.5, 0.5, 0] } @@ -179,7 +179,10 @@ export class BezierState extends GaitState { update_phase() { const m = this.gait_state - if (m.step_x === 0 && m.step_z === 0 && m.step_angle === 0) return + if (m.step_x === 0 && m.step_z === 0 && m.step_angle === 0) { + this.phase = 0 + return + } this.phase += this.dt * m.step_velocity * this.speed_factor if (this.phase >= 1) { this.phase_num = (this.phase_num + 1) % 2 @@ -187,19 +190,16 @@ export class BezierState extends GaitState { } } - protected phase_lead = 0.08 - protected feather = 0.05 - update_body_position() { const m = this.gait_state const moving = m.step_x !== 0 || m.step_z !== 0 || m.step_angle !== 0 if (!moving) return + if (this.mode !== 'crawl') return + const { stance, swing, next_swing, time_to_lift } = this.get_leg_states() - // Only shift when all legs are down and we know which leg lifts next if (stance.length >= 3 && swing.length === 0 && next_swing !== -1) { - // Check if we're starting a new shift if (this.current_shift_leg !== next_swing) { this.current_shift_leg = next_swing this.shift_start_pos.x = this.body_state.xm @@ -213,12 +213,10 @@ export class BezierState extends GaitState { this.shift_start_time = time_to_lift } - // Calculate smooth progress using smoothstep const total_time = this.shift_start_time const progress = total_time > 0 ? 1 - time_to_lift / total_time : 1 const smooth_progress = this.smoothstep01(Math.max(0, Math.min(1, progress))) - // Smoothly interpolate to target this.body_state.xm = this.lerp( this.shift_start_pos.x, this.shift_target_pos.x, @@ -278,21 +276,6 @@ export class BezierState extends GaitState { return { stance, swing, next_swing, time_to_lift: min_time_to_swing } } - protected stance_weight(i: number): number { - const s = this.stand_offset - const e = this.feather - let p = this.phase + this.offset[i] + this.phase_lead - p -= Math.floor(p) - if (p < s - e) return 1 - if (p > s + e && p < 1 - e) return 0 - if (p <= s + e) { - const t = (p - (s - e)) / (2 * e) - return 1 - this.smoothstep01(t) - } - const q = p >= 1 - e ? (p - (1 - e)) / e : (e - p) / e - return this.smoothstep01(q) - } - protected smoothstep01(t: number): number { const x = Math.max(0, Math.min(1, t)) return x * x * (3 - 2 * x) diff --git a/esp32/include/gait/walk_state.h b/esp32/include/gait/walk_state.h index 164f1a7..e3c9246 100644 --- a/esp32/include/gait/walk_state.h +++ b/esp32/include/gait/walk_state.h @@ -14,40 +14,25 @@ class WalkState : public GaitState { float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f}; float stand_offset = 0.6f; float step_length = 0.0f; - float phase_lead = 0.08f; - float feather = 0.05f; float speed_factor = 1; - float com_shift_gain = 0.35f; - float com_shift_limit = 0.06f; - float com_tau = 0.12f; - alignas(16) float crawl_target_xz[4][2] = {{0}}; - int crawl_order[4] = {0, 1, 2, 3}; + struct ShiftState { + float start_x = 0.0f; + float start_z = 0.0f; + float target_x = 0.0f; + float target_z = 0.0f; + float start_time = 0.0f; + int current_shift_leg = -1; + } shift_state; - static float d2(float ax, float az, float bx, float bz) { - float dx = ax - bx, dz = az - bz; - return std::sqrt(dx * dx + dz * dz); - } - - void buildCrawlTargetsIncenter() { - for (int s = 0; s < 4; ++s) { - int a = (s + 1) & 3, b = (s + 2) & 3, c = (s + 3) & 3; - float Ax = default_feet_pos[a][0], Az = default_feet_pos[a][2]; - float Bx = default_feet_pos[b][0], Bz = default_feet_pos[b][2]; - float Cx = default_feet_pos[c][0], Cz = default_feet_pos[c][2]; - float la = d2(Bx, Bz, Cx, Cz); - float lb = d2(Ax, Az, Cx, Cz); - float lc = d2(Ax, Az, Bx, Bz); - float L = la + lb + lc; - if (L <= 1e-6f) { - crawl_target_xz[s][0] = (Ax + Bx + Cx) / 3.f; - crawl_target_xz[s][1] = (Az + Bz + Cz) / 3.f; - } else { - crawl_target_xz[s][0] = (la * Ax + lb * Bx + lc * Cx) / L; - crawl_target_xz[s][1] = (la * Az + lb * Bz + lc * Cz) / L; - } - } - } + struct LegStates { + std::array stance; + std::array swing; + int stance_count = 0; + int swing_count = 0; + int next_swing = -1; + float time_to_lift = INFINITY; + }; static constexpr uint8_t BEZIER_POINTS = 12; static constexpr std::array COMBINATORIAL_VALUES = { @@ -72,12 +57,12 @@ class WalkState : public GaitState { 0.9f, 1.1f, 1.1f, 1.1f, 0.0f, 0.0f}; public: - WalkState() { buildCrawlTargetsIncenter(); } + WalkState() = default; const char *name() const override { return "Bezier"; } - void set_mode_crawl(float duty = 0.85f, std::array order = {0, 1, 2, 3}) { + void set_mode_crawl(float duty = 0.85f, std::array order = {3, 0, 2, 1}) { mode = WALK_GAIT::CRAWL; - speed_factor = 0.1; + speed_factor = 0.5; stand_offset = duty; const float base[4] = {0.f, 0.25f, 0.5f, 0.75f}; for (int i = 0; i < 4; ++i) phase_offset[order[i]] = base[i]; @@ -85,7 +70,7 @@ class WalkState : public GaitState { void set_mode_trot(float duty = 0.6f, std::array offsets = {0.f, 0.5f, 0.5f, 0.f}) { mode = WALK_GAIT::TROT; - speed_factor = 1; + speed_factor = 2; stand_offset = duty; for (int i = 0; i < 4; ++i) phase_offset[i] = std::fmod(std::fabs(offsets[i]), 1.f); } @@ -101,33 +86,90 @@ class WalkState : public GaitState { protected: void updatePhase(float dt) { + if (gait_state.step_x == 0 && gait_state.step_z == 0 && gait_state.step_angle == 0) { + phase_time = 0; + return; + } phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * speed_factor, 1.0f); } + LegStates getLegStates() { + LegStates states; + float min_time_to_swing = INFINITY; + + for (int i = 0; i < 4; i++) { + float phase = std::fmod(phase_time + phase_offset[i], 1.0f); + + if (phase <= stand_offset) { + states.stance[states.stance_count++] = i; + float time_to_swing = stand_offset - phase; + if (time_to_swing < min_time_to_swing) { + min_time_to_swing = time_to_swing; + states.next_swing = i; + } + } else { + states.swing[states.swing_count++] = i; + } + } + + states.time_to_lift = min_time_to_swing; + return states; + } + + std::array stanceCentroid(const LegStates &states) { + if (states.stance_count == 0) { + return {0.0f, 0.0f, 0.0f}; + } + + float sx = 0.0f, sz = 0.0f; + int remaining_count = 0; + + for (int i = 0; i < states.stance_count; i++) { + int leg = states.stance[i]; + if (leg != states.next_swing) { + sx += default_feet_pos[leg][0]; + sz += default_feet_pos[leg][2]; + remaining_count++; + } + } + + if (remaining_count > 0) { + return {sx / remaining_count, 0.0f, sz / remaining_count}; + } + + return {0.0f, 0.0f, 0.0f}; + } + + static float lerp(float a, float b, float t) { return a + (b - a) * t; } + void updateBodyPosition(body_state_t &body_state, float dt) { if (mode != WALK_GAIT::CRAWL) return; + const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f; - const float a = dt / (com_tau + dt); - if (!moving) { - body_state.xm += (0.f - body_state.xm) * a; - body_state.zm += (0.f - body_state.zm) * a; - return; + if (!moving) return; + + LegStates leg_states = getLegStates(); + + if (leg_states.stance_count >= 3 && leg_states.swing_count == 0 && leg_states.next_swing != -1) { + if (shift_state.current_shift_leg != leg_states.next_swing) { + shift_state.current_shift_leg = leg_states.next_swing; + shift_state.start_x = body_state.xm; + shift_state.start_z = body_state.zm; + + auto target = stanceCentroid(leg_states); + shift_state.target_x = target[0]; + shift_state.target_z = target[2]; + + shift_state.start_time = leg_states.time_to_lift; + } + + float total_time = shift_state.start_time; + float progress = total_time > 0 ? 1.0f - (leg_states.time_to_lift / total_time) : 1.0f; + float smooth_progress = smoothstep01(std::clamp(progress, 0.0f, 1.0f)); + + body_state.xm = lerp(shift_state.start_x, shift_state.target_x, smooth_progress); + body_state.zm = lerp(shift_state.start_z, shift_state.target_z, smooth_progress); } - - int k = (int)std::floor(std::fmod(phase_time, 1.f) * 8.f) & 7; - int leg = crawl_order[k >> 1]; - - float tx = crawl_target_xz[leg][0] * com_shift_gain; - float tz = crawl_target_xz[leg][1] * com_shift_gain; - float m = std::hypot(tx, tz); - if (m > com_shift_limit) { - float s = com_shift_limit / m; - tx *= s; - tz *= s; - } - - body_state.xm += (tx - body_state.xm) * a; - body_state.zm += (tz - body_state.zm) * a; } static float smoothstep01(float t) {