🎨 Cleans up gait handling code
This commit is contained in:
Rune Harlyk
@@ -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 {
|
||||
BezierState,
|
||||
CalibrationState,
|
||||
EightPhaseWalkState,
|
||||
IdleState,
|
||||
RestState,
|
||||
StandState
|
||||
} from '$lib/gait'
|
||||
import { BezierState, CalibrationState, 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()
|
||||
})
|
||||
|
||||
|
||||
+89
-165
@@ -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 stand_offset = 0.75
|
||||
offset = [0, 0.5, 0.5, 0]
|
||||
protected step_length = 0
|
||||
protected stand_offset = 0.85
|
||||
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) {
|
||||
this.step_length = -this.step_length
|
||||
}
|
||||
if (this.gait_state.step_x < 0) 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 %= 2
|
||||
this.phase_num = (this.phase_num + 1) % 2
|
||||
this.phase = 0
|
||||
}
|
||||
}
|
||||
|
||||
update_feet_positions() {
|
||||
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
|
||||
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++) {
|
||||
this.body_state.feet[i] = this.update_foot_position(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) {
|
||||
phase -= 1
|
||||
}
|
||||
if (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++) {
|
||||
c = (c * (n - i)) / (i + 1)
|
||||
}
|
||||
for (let i = 0; i < k; i++) c = (c * (n - i)) / (i + 1)
|
||||
return c
|
||||
}
|
||||
|
||||
@@ -8,30 +8,39 @@ export const jointNames = persistentStore('joint_names', <string[]>[])
|
||||
|
||||
export const model = writable()
|
||||
|
||||
export const modes = [
|
||||
'deactivated',
|
||||
'idle',
|
||||
'calibration',
|
||||
'rest',
|
||||
'stand',
|
||||
'crawl',
|
||||
'walk'
|
||||
] as const
|
||||
export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'walk'] as const
|
||||
|
||||
export type Modes = (typeof modes)[number]
|
||||
|
||||
export enum ModesEnum {
|
||||
Deactivated,
|
||||
Idle,
|
||||
Calibration,
|
||||
Rest,
|
||||
Stand,
|
||||
Crawl,
|
||||
Walk
|
||||
Deactivated = 0,
|
||||
Idle = 1,
|
||||
Calibration = 2,
|
||||
Rest = 3,
|
||||
Stand = 4,
|
||||
Walk = 5
|
||||
}
|
||||
|
||||
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])
|
||||
|
||||
@@ -8,6 +8,7 @@ export enum MessageTopic {
|
||||
i2cScan = 'i2cScan',
|
||||
peripheralSettings = 'peripheralSettings',
|
||||
otastatus = 'otastatus',
|
||||
gait = 'walk_gait',
|
||||
servoState = 'servoState',
|
||||
servoPWM = 'servoPWM',
|
||||
WiFiSettings = 'WiFiSettings',
|
||||
|
||||
@@ -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 => {
|
||||
console.log(data)
|
||||
|
||||
socket.sendEvent(MessageTopic.input, data)
|
||||
})
|
||||
outControllerData.subscribe(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))
|
||||
})
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
};
|
||||
@@ -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);
|
||||
}
|
||||
};
|
||||
@@ -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];
|
||||
}
|
||||
};
|
||||
@@ -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};
|
||||
static constexpr float STAND_OFFSET = 0.75f;
|
||||
static constexpr uint8_t BEZIER_POINTS = 12;
|
||||
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;
|
||||
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) {
|
||||
const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f;
|
||||
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) {
|
||||
updateFootPosition(body_state, 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 bool contact = leg_phase <= STAND_OFFSET;
|
||||
contact ? standController(body_state, index, leg_phase / STAND_OFFSET)
|
||||
: swingController(body_state, index, (leg_phase - STAND_OFFSET) / (1 - STAND_OFFSET));
|
||||
const float leg_phase = std::fmod(phase_time + phase_offset[index], 1.0f);
|
||||
const bool contact = leg_phase <= stand_offset;
|
||||
if (contact)
|
||||
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;
|
||||
}
|
||||
};
|
||||
+17
-12
@@ -8,8 +8,7 @@
|
||||
#include <utils/math_utils.h>
|
||||
|
||||
#include <gait/state.h>
|
||||
#include <gait/crawl_state.h>
|
||||
#include <gait/bezier_state.h>
|
||||
#include <gait/walk_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;
|
||||
|
||||
|
||||
@@ -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};
|
||||
|
||||
Reference in New Issue
Block a user