🎨 Cleans up gait handling code

app/src/lib/components/Visualization.svelte

@@ -22,7 +22,10 @@
     servoAngles,
     mpu,
     jointNames,
-    currentKinematic
+    currentKinematic,
+    walkGait,
+    walkGaits,
+    walkGaitToMode
   } from '$lib/stores'
   import {
     extractFootColor,
@@ -34,14 +37,7 @@
   import { lerp, degToRad } from 'three/src/math/MathUtils'
   import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
   import { type body_state_t } from '$lib/kinematic'
-  import {
+  import { BezierState, CalibrationState, IdleState, RestState, StandState } from '$lib/gait'
-    BezierState,
-    CalibrationState,
-    EightPhaseWalkState,
-    IdleState,
-    RestState,
-    StandState
-  } from '$lib/gait'
   import { radToDeg } from 'three/src/math/MathUtils.js'
   import type { URDFRobot } from 'urdf-loader'
   import { get } from 'svelte/store'
@@ -78,7 +74,6 @@
     [ModesEnum.Calibration]: new CalibrationState(),
     [ModesEnum.Rest]: new RestState(),
     [ModesEnum.Stand]: new StandState(),
-    [ModesEnum.Crawl]: new EightPhaseWalkState(),
     [ModesEnum.Walk]: new BezierState()
   }
   let lastTick = performance.now()
@@ -117,6 +112,7 @@
     await populateModelCache()
     await createScene()
     servoAngles.subscribe(updateAnglesFromStore)
+    walkGait.subscribe(gait => planners[ModesEnum.Walk].set_mode(walkGaitToMode(gait)))
     if (panel) createPanel()
   })
 

app/src/lib/gait.ts

@@ -2,8 +2,6 @@ import { get } from 'svelte/store'
 import type { body_state_t } from './kinematic'
 import { currentKinematic } from './stores/featureFlags'
 
-const { sin } = Math
-
 export interface gait_state_t {
   step_height: number
   step_x: number
@@ -79,7 +77,8 @@ export class IdleState extends GaitState {
 export class CalibrationState extends GaitState {
   protected name = 'Calibration'
 
-  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+  // eslint-disable-next-line @typescript-eslint/no-unused-vars
+  step(body_state: body_state_t, _command: ControllerCommand) {
     body_state.omega = 0
     body_state.phi = 0
     body_state.psi = 0
@@ -94,7 +93,8 @@ export class CalibrationState extends GaitState {
 export class RestState extends GaitState {
   protected name = 'Rest'
 
-  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+  // eslint-disable-next-line @typescript-eslint/no-unused-vars
+  step(body_state: body_state_t, _command: ControllerCommand) {
     body_state.omega = 0
     body_state.phi = 0
     body_state.psi = 0
@@ -109,7 +109,7 @@ export class RestState extends GaitState {
 export class StandState extends GaitState {
   protected name = 'Stand'
 
-  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+  step(body_state: body_state_t, command: ControllerCommand) {
     body_state.omega = 0
     body_state.phi = command.rx * 10 * (Math.PI / 2)
     body_state.psi = command.ry * 10 * (Math.PI / 2)
@@ -120,157 +120,41 @@ export class StandState extends GaitState {
   }
 }
 
-abstract class PhaseGaitState extends GaitState {
-  protected tick = 0
-  protected phase = 0
-  protected phase_time = 0
-  protected abstract num_phases: number
-  protected abstract phase_speed_factor: number
-  protected abstract swing_stand_ratio: number
-
-  protected contact_phases!: number[][]
-  protected shifts!: number[][]
-
-  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-    super.step(body_state, command, dt)
-    this.update_phase()
-    this.update_body_position()
-    this.update_feet_positions()
-    return this.body_state
-  }
-
-  update_phase() {
-    this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity
-
-    if (this.phase_time >= 1) {
-      this.phase += 1
-      if (this.phase == this.num_phases) this.phase = 0
-      this.phase_time = 0
-    }
-  }
-
-  update_body_position() {
-    if (this.num_phases === 4) return
-
-    const shift = this.shifts[Math.floor(this.phase / 2)]
-
-    this.body_state.xm += (shift[0] - this.body_state.xm) * this.dt * 4
-    this.body_state.zm += (shift[2] - this.body_state.zm) * this.dt * 4
-  }
-
-  update_feet_positions() {
-    for (let i = 0; i < 4; i++) {
-      this.body_state.feet[i] = this.update_foot_position(i)
-    }
-  }
-
-  update_foot_position(index: number): number[] {
-    const contact = this.contact_phases[index][this.phase]
-    return contact ? this.stand(index) : this.swing(index)
-  }
-
-  stand(index: number): number[] {
-    const delta_pos = [
-      -this.gait_state.step_x * this.dt * this.swing_stand_ratio,
-      0,
-      -this.gait_state.step_z * this.dt * this.swing_stand_ratio
-    ]
-
-    this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0]
-    this.body_state.feet[index][1] = this.default_feet_pos[index][1]
-    this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2]
-    return this.body_state.feet[index]
-  }
-
-  swing(index: number): number[] {
-    const delta_pos = [this.gait_state.step_x * this.dt, 0, this.gait_state.step_z * this.dt]
-
-    if (this.gait_state.step_x == 0) {
-      delta_pos[0] =
-        (this.default_feet_pos[index][0] - this.body_state.feet[index][0]) * this.dt * 8
-    }
-
-    if (this.gait_state.step_z == 0) {
-      delta_pos[2] =
-        (this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * this.dt * 8
-    }
-
-    this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0]
-    this.body_state.feet[index][1] =
-      this.default_feet_pos[index][1] + sin(this.phase_time * Math.PI) * this.gait_state.step_height
-    this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2]
-    return this.body_state.feet[index]
-  }
-}
-
-export class FourPhaseWalkState extends PhaseGaitState {
-  protected name = 'Four phase walk'
-  protected num_phases = 4
-  protected phase_speed_factor = 6
-  protected contact_phases = [
-    [1, 0, 1, 1],
-    [1, 1, 1, 0],
-    [1, 1, 1, 0],
-    [1, 0, 1, 1]
-  ]
-  protected swing_stand_ratio = 1 / (this.num_phases - 1)
-
-  begin() {
-    super.begin()
-  }
-
-  end() {
-    super.end()
-  }
-
-  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-    return super.step(body_state, command, dt)
-  }
-}
-
-export class EightPhaseWalkState extends PhaseGaitState {
-  protected name = 'Eight phase walk'
-  protected num_phases = 8
-  protected phase_speed_factor = 4
-  protected contact_phases = [
-    [1, 0, 1, 1, 1, 1, 1, 1],
-    [1, 1, 1, 1, 1, 0, 1, 1],
-    [1, 1, 1, 1, 1, 1, 1, 0],
-    [1, 1, 1, 0, 1, 1, 1, 1]
-  ]
-  protected shifts = [
-    [-0.05, 0, -0.2],
-    [0.3, 0, 0.2],
-    [-0.05, 0, 0.2],
-    [0.3, 0, -0.2]
-  ]
-  protected swing_stand_ratio = 1 / (this.num_phases - 1)
-
-  begin() {
-    super.begin()
-  }
-
-  end() {
-    super.end()
-  }
-
-  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-    return super.step(body_state, command, dt)
-  }
-}
-
 export class BezierState extends GaitState {
   protected name = 'Bezier'
   protected phase = 0
   protected phase_num = 0
-  protected step_length: number = 0
+  protected step_length = 0
-  protected stand_offset = 0.75
+  protected stand_offset = 0.85
-  offset = [0, 0.5, 0.5, 0]
+  protected mode: 'crawl' | 'trot' = 'trot'
+  offset = [0, 0.5, 0.75, 0.25]
+
+  constructor() {
+    super()
+    this.set_mode(this.mode)
+  }
 
   begin() {
     super.begin()
   }
 
+  set_mode(mode: 'crawl' | 'trot', duty?: number, order?: [number, number, number, number]) {
+    console.log('BezierState set_mode', mode)
+
+    this.mode = mode
+    if (mode === 'crawl') {
+      this.stand_offset = duty ?? 0.85
+      const o = order ?? [0, 3, 1, 2]
+      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.stand_offset = duty ?? 0.6
+      this.offset = order ? (order.map(v => v % 1) as number[]) : [0, 0.5, 0.5, 0]
+    }
+  }
+
   end() {
     super.end()
   }
@@ -278,34 +162,78 @@ export class BezierState extends GaitState {
   step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
     super.step(body_state, command, dt)
     this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2)
-    if (this.gait_state.step_x < 0) {
+    if (this.gait_state.step_x < 0) this.step_length = -this.step_length
-      this.step_length = -this.step_length
-    }
     this.update_phase()
+    this.update_body_position()
     this.update_feet_positions()
     return this.body_state
   }
 
   update_phase() {
-    this.phase += this.dt * this.gait_state.step_velocity * 2
+    const m = this.gait_state
+    if (m.step_x === 0 && m.step_z === 0 && m.step_angle === 0) return
+    this.phase += this.dt * m.step_velocity * 2
     if (this.phase >= 1) {
-      this.phase_num += 1
+      this.phase_num = (this.phase_num + 1) % 2
-      this.phase_num %= 2
       this.phase = 0
     }
   }
 
-  update_feet_positions() {
+  protected phase_lead = 0.08
-    for (let i = 0; i < 4; i++) {
+  protected feather = 0.05
-      this.body_state.feet[i] = this.update_foot_position(i)
+
+  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
+    const c = this.dynamic_stance_centroid()
+    const k = this.mode === 'crawl' ? 16 : 10
+    const a = 1 - Math.exp(-k * this.dt)
+    this.body_state.xm += (c[0] - this.body_state.xm) * a
+    this.body_state.zm += (c[2] - this.body_state.zm) * a
   }
+
+  protected dynamic_stance_centroid(): number[] {
+    let sx = 0,
+      sz = 0,
+      sw = 0
+    for (let i = 0; i < 4; i++) {
+      const w = this.stance_weight(i)
+      sx += this.body_state.feet[i][0] * w
+      sz += this.body_state.feet[i][2] * w
+      sw += w
+    }
+    if (sw === 0) return [this.body_state.xm, 0, this.body_state.zm]
+    return [sx / sw, 0, sz / sw]
+  }
+
+  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)
+  }
+
+  update_feet_positions() {
+    for (let i = 0; i < 4; i++) this.body_state.feet[i] = this.update_foot_position(i)
   }
 
   update_foot_position(index: number): number[] {
     let phase = this.phase + this.offset[index]
-    if (phase >= 1) {
+    if (phase >= 1) phase -= 1
-      phase -= 1
-    }
     this.body_state.feet[index][0] = this.default_feet_pos[index][0]
     this.body_state.feet[index][1] = this.default_feet_pos[index][1]
     this.body_state.feet[index][2] = this.default_feet_pos[index][2]
@@ -356,10 +284,7 @@ const stance_curve = (length: number, angle: number, depth: number, phase: numbe
   const X = step * X_POLAR
   const Z = step * Y_POLAR
   let Y = 0
-
+  if (length !== 0) Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length))
-  if (length !== 0) {
-    Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length))
-  }
   return [X, Y, Z]
 }
 
@@ -390,6 +315,7 @@ const bezier_curve = (length: number, angle: number, height: number, phase: numb
   }
   return point
 }
+
 const get_control_points = (length: number, angle: number, height: number): number[][] => {
   const X_POLAR = Math.cos(angle)
   const Z_POLAR = Math.sin(angle)
@@ -440,8 +366,6 @@ const comb = (n: number, k: number): number => {
   if (k === 0 || k === n) return 1
   k = Math.min(k, n - k)
   let c = 1
-  for (let i = 0; i < k; i++) {
+  for (let i = 0; i < k; i++) c = (c * (n - i)) / (i + 1)
-    c = (c * (n - i)) / (i + 1)
-  }
   return c
 }

app/src/lib/stores/model-store.ts

@@ -8,30 +8,39 @@ export const jointNames = persistentStore('joint_names', <string[]>[])
 
 export const model = writable()
 
-export const modes = [
+export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'walk'] as const
-  'deactivated',
-  'idle',
-  'calibration',
-  'rest',
-  'stand',
-  'crawl',
-  'walk'
-] as const
 
 export type Modes = (typeof modes)[number]
 
 export enum ModesEnum {
-  Deactivated,
+  Deactivated = 0,
-  Idle,
+  Idle = 1,
-  Calibration,
+  Calibration = 2,
-  Rest,
+  Rest = 3,
-  Stand,
+  Stand = 4,
-  Crawl,
+  Walk = 5
-  Walk
+}
+
+export enum WalkGaits {
+  Trot = 0,
+  Crawl = 1
+}
+
+export const walkGaits = ['trot', 'crawl'] as const
+
+export const walkGaitLabels: Record<WalkGaits, string> = {
+  [WalkGaits.Trot]: 'Trot',
+  [WalkGaits.Crawl]: 'Crawl'
+}
+
+export const walkGaitToMode = (gait: WalkGaits): 'trot' | 'crawl' => {
+  return gait === WalkGaits.Trot ? 'trot' : 'crawl'
 }
 
 export const mode: Writable<ModesEnum> = writable(ModesEnum.Deactivated)
 
+export const walkGait: Writable<WalkGaits> = writable(WalkGaits.Trot)
+
 export const outControllerData = writable([0, 0, 0, 0, 0, 1, 0])
 
 export const kinematicData = writable([0, 0, 0, 0, 1, 0])

app/src/lib/types/models.ts

@@ -8,6 +8,7 @@ export enum MessageTopic {
   i2cScan = 'i2cScan',
   peripheralSettings = 'peripheralSettings',
   otastatus = 'otastatus',
+  gait = 'walk_gait',
   servoState = 'servoState',
   servoPWM = 'servoPWM',
   WiFiSettings = 'WiFiSettings',

app/src/routes/+layout.svelte

@@ -19,7 +19,8 @@
     servoAnglesOut,
     socket,
     location,
-    useFeatureFlags
+    useFeatureFlags,
+    walkGait
   } from '$lib/stores'
   import { type Analytics, type DownloadOTA } from '$lib/types/models'
   import { MessageTopic } from '$lib/types/models'
@@ -38,12 +39,9 @@
 
     addEventListeners()
 
-    outControllerData.subscribe(data => {
+    outControllerData.subscribe(data => socket.sendEvent(MessageTopic.input, data))
-      console.log(data)
-
-      socket.sendEvent(MessageTopic.input, data)
-    })
     mode.subscribe(data => socket.sendEvent(MessageTopic.mode, data))
+    walkGait.subscribe(data => socket.sendEvent(MessageTopic.gait, data))
     servoAnglesOut.subscribe(data => socket.sendEvent(MessageTopic.angles, data))
     kinematicData.subscribe(data => socket.sendEvent(MessageTopic.position, data))
   })

app/src/routes/controller/Controls.svelte

@@ -2,7 +2,18 @@
   import nipplejs from 'nipplejs'
   import { onMount } from 'svelte'
   import { capitalize, throttler } from '$lib/utilities'
-  import { input, outControllerData, mode, modes, type Modes, ModesEnum } from '$lib/stores'
+  import {
+    input,
+    outControllerData,
+    mode,
+    modes,
+    type Modes,
+    ModesEnum,
+    walkGaits,
+    WalkGaits,
+    walkGait,
+    walkGaitLabels
+  } from '$lib/stores'
   import type { vector } from '$lib/types/models'
   import { VerticalSlider } from '$lib/components/input'
   import { gamepadAxes, hasGamepad } from '$lib/stores/gamepad'
@@ -13,7 +24,7 @@
   let right: nipplejs.JoystickManager
 
   let throttle_timing = 40
-  let data = new Array(8)
+  let data = new Array(7)
 
   $effect(() => {
     if ($hasGamepad) {
@@ -100,6 +111,10 @@
   const changeMode = (modeValue: Modes) => {
     mode.set(modes.indexOf(modeValue))
   }
+
+  const changeWalkGait = (walkGaitValue: WalkGaits) => {
+    walkGait.set(walkGaitValue)
+  }
 </script>
 
 <div class="absolute top-0 left-0 w-screen h-screen">
@@ -141,7 +156,20 @@
         {/each}
       </div>
 
-      {#if $mode === ModesEnum.Walk || $mode === ModesEnum.Crawl}
+      {#if $mode === ModesEnum.Walk}
+        <div class="join">
+          {#each Object.values(WalkGaits) as gaitValue}
+            {#if typeof gaitValue === 'number'}
+              <button
+                class="btn join-item btn-sm"
+                class:btn-secondary={$walkGait === gaitValue}
+                onclick={() => changeWalkGait(gaitValue)}>
+                {walkGaitLabels[gaitValue]}
+              </button>
+            {/if}
+          {/each}
+        </div>
+
         <div class="flex gap-4">
           <div>
             <label for="s1">S1</label>

esp32/include/gait/crawl_state.h

@@ -1,33 +0,0 @@
-#pragma once
-
-#include <gait/phase_state_base.h>
-
-class EightPhaseWalkState : public PhaseGaitState {
-  protected:
-    const char *name() const override { return "Eight phase walk"; }
-
-    int num_phases() const override { return 8; }
-
-    float phase_speed_factor() const override { return 4; }
-
-    float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
-
-  public:
-    EightPhaseWalkState() {
-        uint8_t contact[4][8] = {
-            {1, 0, 1, 1, 1, 1, 1, 1}, {1, 1, 1, 1, 1, 0, 1, 1}, {1, 1, 1, 1, 1, 1, 1, 0}, {1, 1, 1, 0, 1, 1, 1, 1}};
-        float shift_values[4][3] = {{-0.05f, 0, -0.2f}, {0.25f, 0, 0.2f}, {-0.05f, 0, 0.2f}, {0.25f, 0, -0.2f}};
-        for (uint8_t i = 0; i < 4; ++i) {
-            for (uint8_t j = 0; j < 8; ++j) {
-                contact_phases[i][j] = contact[i][j];
-            }
-            for (uint8_t j = 0; j < 3; ++j) {
-                shifts[i][j] = shift_values[i][j];
-            }
-        }
-    }
-
-    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
-        return PhaseGaitState::step(body_state, command, dt);
-    }
-};

esp32/include/gait/four_phase_trot_state.h

@@ -1,28 +0,0 @@
-#pragma once
-
-#include <gait/phase_state_base.h>
-
-class FourPhaseWalkState : public PhaseGaitState {
-  protected:
-    const char *name() const override { return "Four phase walk"; }
-
-    int num_phases() const override { return 4; }
-
-    float phase_speed_factor() const override { return 6; }
-
-    float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
-
-  public:
-    FourPhaseWalkState() {
-        uint8_t contact[4][4] = {{1, 0, 1, 1}, {1, 1, 1, 0}, {1, 1, 1, 0}, {1, 0, 1, 1}};
-        for (int i = 0; i < 4; ++i) {
-            for (int j = 0; j < 4; ++j) {
-                contact_phases[i][j] = contact[i][j];
-            }
-        }
-    }
-
-    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
-        return PhaseGaitState::step(body_state, command, dt);
-    }
-};

esp32/include/gait/phase_state_base.h

@@ -1,78 +0,0 @@
-#pragma once
-
-#include <gait/state.h>
-
-class PhaseGaitState : public GaitState {
-  protected:
-    int phase = 0;
-    float phase_time = 0;
-    virtual int num_phases() const = 0;
-    virtual float phase_speed_factor() const = 0;
-    virtual float swing_stand_ratio() const = 0;
-    float dt = 0.02f;
-
-    uint8_t contact_phases[4][8];
-    float shifts[4][3];
-
-    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
-        mapCommand(command);
-        this->dt = dt;
-        updatePhase();
-        updateBodyPosition(body_state);
-        updateFeetPositions(body_state);
-    }
-
-    void updatePhase() {
-        phase_time += dt * phase_speed_factor() * gait_state.step_velocity;
-
-        if (phase_time >= 1.0f) {
-            phase += 1;
-            if (phase == num_phases()) phase = 0;
-            phase_time = 0;
-        }
-    }
-
-    void updateBodyPosition(body_state_t &body_state) {
-        if (num_phases() == 4) return;
-
-        const auto &shift = shifts[phase / 2];
-        body_state.xm += (shift[0] - body_state.xm) * dt * 4;
-        body_state.zm += (shift[2] - body_state.zm) * dt * 4;
-    }
-
-    void updateFeetPositions(body_state_t &body_state) {
-        for (int i = 0; i < 4; ++i) {
-            updateFootPosition(body_state, i);
-        }
-    }
-
-    void updateFootPosition(body_state_t &body_state, int index) {
-        bool contact = contact_phases[index][phase];
-        contact ? stand(body_state, index) : swing(body_state, index);
-    }
-
-    void stand(body_state_t &body_state, int index) {
-        float delta_pos[3] = {-gait_state.step_x * dt * swing_stand_ratio(), 0,
-                              -gait_state.step_z * dt * swing_stand_ratio()};
-
-        body_state.feet[index][0] += delta_pos[0];
-        body_state.feet[index][1] = default_feet_pos[index][1];
-        body_state.feet[index][2] += delta_pos[2];
-    }
-
-    void swing(body_state_t &body_state, int index) {
-        float delta_pos[3] = {gait_state.step_x * dt, 0, gait_state.step_z * dt};
-
-        if (std::fabs(gait_state.step_x) < 0.01) {
-            delta_pos[0] = (default_feet_pos[index][0] - body_state.feet[index][0]) * dt * 8;
-        }
-
-        if (std::fabs(gait_state.step_z) < 0.01) {
-            delta_pos[2] = (default_feet_pos[index][2] - body_state.feet[index][2]) * dt * 8;
-        }
-
-        body_state.feet[index][0] += delta_pos[0];
-        body_state.feet[index][1] = default_feet_pos[index][1] + std::sin(phase_time * M_PI) * gait_state.step_height;
-        body_state.feet[index][2] += delta_pos[2];
-    }
-};

esp32/include/gait/walk_state.h

@@ -5,13 +5,18 @@
 #include <array>
 #include <functional>
 
-class BezierState : public GaitState {
+enum class WALK_GAIT { TROT, CRAWL };
+
+class WalkState : public GaitState {
   private:
+    WALK_GAIT mode = WALK_GAIT::TROT;
     float phase_time = 0.0f;
-    static constexpr float PHASE_OFFSET[4] = {0.f, 0.5f, 0.5f, 0.f};
+    float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f};
-    static constexpr float STAND_OFFSET = 0.75f;
+    float stand_offset = 0.6f;
-    static constexpr uint8_t BEZIER_POINTS = 12;
     float step_length = 0.0f;
+    float phase_lead = 0.08f;
+    float feather = 0.05f;
+    static constexpr uint8_t BEZIER_POINTS = 12;
     static constexpr std::array<float, BEZIER_POINTS> COMBINATORIAL_VALUES = {
         combinatorial_constexpr(11, 0),  // 1
         combinatorial_constexpr(11, 1),  // 11
@@ -36,6 +41,19 @@ class BezierState : public GaitState {
   public:
     const char *name() const override { return "Bezier"; }
 
+    void set_mode_crawl(float duty = 0.85f, std::array<int, 4> order = {0, 3, 1, 2}) {
+        mode = WALK_GAIT::CRAWL;
+        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];
+    }
+
+    void set_mode_trot(float duty = 0.6f, std::array<float, 4> offsets = {0.f, 0.5f, 0.5f, 0.f}) {
+        mode = WALK_GAIT::TROT;
+        stand_offset = duty;
+        for (int i = 0; i < 4; ++i) phase_offset[i] = std::fmod(std::fabs(offsets[i]), 1.f);
+    }
+
     void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         this->mapCommand(command);
         step_length = std::hypot(gait_state.step_x, gait_state.step_z);
@@ -43,26 +61,69 @@ class BezierState : public GaitState {
             step_length = -step_length;
         }
         updatePhase(dt);
+        updateBodyPosition(body_state, dt);
         updateFeetPositions(body_state);
     }
 
   protected:
     void updatePhase(float dt) { phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * 2, 1.0f); }
 
-    void updateFeetPositions(body_state_t &body_state) {
+    void updateBodyPosition(body_state_t &body_state, float dt) {
-        for (int i = 0; i < 4; ++i) {
+        const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f;
-            updateFootPosition(body_state, i);
+        if (!moving) return;
+        const auto c = dynamicStanceCentroid(body_state);
+        const float k = mode == WALK_GAIT::CRAWL ? 16.f : 10.f;
+        const float a = 1.f - std::exp(-k * dt);
+        body_state.xm += (c[0] - body_state.xm) * a;
+        body_state.zm += (c[2] - body_state.zm) * a;
     }
+
+    std::array<float, 3> dynamicStanceCentroid(const body_state_t &body_state) const {
+        float sx = 0.f, sz = 0.f, sw = 0.f;
+        for (int i = 0; i < 4; ++i) {
+            const float w = stanceWeight(i);
+            sx += body_state.feet[i][0] * w;
+            sz += body_state.feet[i][2] * w;
+            sw += w;
+        }
+        if (sw == 0.f) return {body_state.xm, 0.f, body_state.zm};
+        return {sx / sw, 0.f, sz / sw};
+    }
+
+    static float smoothstep01(float t) {
+        const float x = std::clamp(t, 0.f, 1.f);
+        return x * x * (3.f - 2.f * x);
+    }
+
+    float stanceWeight(int i) const {
+        const float s = stand_offset;
+        const float e = feather;
+        float p = std::fmod(phase_time + phase_offset[i] + phase_lead, 1.f);
+        if (p < 0.f) p += 1.f;
+        if (p < s - e) return 1.f;
+        if (p > s + e && p < 1.f - e) return 0.f;
+        if (p <= s + e) {
+            const float t = (p - (s - e)) / (2.f * e);
+            return 1.f - smoothstep01(t);
+        }
+        const float q = p >= 1.f - e ? (p - (1.f - e)) / e : (e - p) / e;
+        return smoothstep01(q);
+    }
+
+    void updateFeetPositions(body_state_t &body_state) {
+        for (int i = 0; i < 4; ++i) updateFootPosition(body_state, i);
     }
 
     void updateFootPosition(body_state_t &body_state, const int index) {
         body_state.feet[index][0] = this->default_feet_pos[index][0];
         body_state.feet[index][1] = this->default_feet_pos[index][1];
         body_state.feet[index][2] = this->default_feet_pos[index][2];
-        const float leg_phase = std::fmod(phase_time + PHASE_OFFSET[index], 1.0f);
+        const float leg_phase = std::fmod(phase_time + phase_offset[index], 1.0f);
-        const bool contact = leg_phase <= STAND_OFFSET;
+        const bool contact = leg_phase <= stand_offset;
-        contact ? standController(body_state, index, leg_phase / STAND_OFFSET)
+        if (contact)
-                : swingController(body_state, index, (leg_phase - STAND_OFFSET) / (1 - STAND_OFFSET));
+            standController(body_state, index, leg_phase / stand_offset);
+        else
+            swingController(body_state, index, (leg_phase - stand_offset) / (1.f - stand_offset));
     }
 
     void standController(body_state_t &body_state, const int index, const float phase) {
@@ -131,6 +192,6 @@ class BezierState : public GaitState {
         const float offset_mag = std::hypot(offsets[0], offsets[2]);
         const float offset_mod = std::atan2(offset_mag, foot_mag);
 
-        return M_PI_2 + foot_dir + offset_mod;
+        return (float)M_PI_2 + foot_dir + offset_mod;
     }
 };

esp32/include/motion.h

@@ -8,8 +8,7 @@
 #include <utils/math_utils.h>
 
 #include <gait/state.h>
-#include <gait/crawl_state.h>
+#include <gait/walk_state.h>
-#include <gait/bezier_state.h>
 #include <message_types.h>
 
 #define DEFAULT_STATE false
@@ -17,8 +16,9 @@
 #define INPUT_EVENT "input"
 #define POSITION_EVENT "position"
 #define MODE_EVENT "mode"
+#define WALK_GAIT_EVENT "walk_gait"
 
-enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WALK };
+enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
 
 class MotionService {
   public:
@@ -29,6 +29,8 @@ class MotionService {
 
         socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) { handleMode(root, originId); });
 
+        socket.onEvent(WALK_GAIT_EVENT, [&](JsonVariant &root, int originId) { handleWalkGait(root, originId); });
+
         socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { anglesEvent(root, originId); });
 
         socket.onEvent(POSITION_EVENT, [&](JsonVariant &root, int originId) { positionEvent(root, originId); });
@@ -71,7 +73,6 @@ class MotionService {
                 body_state.updateFeet(kinematics.default_feet_positions);
                 break;
             }
-            case MOTION_STATE::CRAWL:
             case MOTION_STATE::WALK: {
                 gait_state.step_height = 0.4 + (command.s1 + 1) / 2;
                 gait_state.step_x = command.ly;
@@ -84,6 +85,16 @@ class MotionService {
         }
     }
 
+    void handleWalkGait(JsonVariant &root, int originId) {
+        ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
+
+        WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
+        if (walkGait == WALK_GAIT::TROT)
+            this->walkGait.set_mode_trot();
+        else
+            this->walkGait.set_mode_crawl();
+    }
+
     void handleMode(JsonVariant &root, int originId) {
         motionState = static_cast<MOTION_STATE>(root.as<int>());
         ESP_LOGV("MotionService", "Mode %d", motionState);
@@ -114,12 +125,8 @@ class MotionService {
             case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles, false); break;
             case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break;
             case MOTION_STATE::STAND: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
-            case MOTION_STATE::CRAWL:
-                crawlGait->step(body_state, command);
-                kinematics.calculate_inverse_kinematics(body_state, new_angles);
-                break;
             case MOTION_STATE::WALK:
-                walkGait->step(body_state, command);
+                walkGait.step(body_state, command);
                 kinematics.calculate_inverse_kinematics(body_state, new_angles);
                 break;
         }
@@ -143,13 +150,11 @@ class MotionService {
   private:
     ServoController *_servoController;
     Kinematics kinematics;
+    WalkState walkGait;
     CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
 
     friend class GaitState;
 
-    std::unique_ptr<GaitState> crawlGait = std::make_unique<EightPhaseWalkState>();
-    std::unique_ptr<GaitState> walkGait = std::make_unique<BezierState>();
-
     MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
     unsigned long _lastUpdate;
 

esp32/test/gait_performance.cpp

@@ -4,7 +4,7 @@
 #include "gait/bezier_state.h"
 
 void test_gaitPlanner_calculateStep_time() {
-    BezierState gaitPlanner;
+    WalkState gaitPlanner;
     body_state_t body_state = {
         128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
     CommandMsg command = {0, 0, 0, 0, 0, 0, 0};