Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

📈 Fixed TargetAngles, Gaits and more angles

Browse Source
This commit is contained in:
Niklas Jensen
2026-01-02 12:44:37 +01:00
committed by nikguin04
parent cdaa60d0e1
commit d6e281d6a5
3 changed files with 56 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+33 -31
View File
@@ -26,11 +26,11 @@
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, IdleState, RestState, StandState } from '$lib/gait'
import { BezierState, CalibrationState, GaitState, 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'
import { KinematicData } from '$lib/platform_shared/websocket_message'
import { AnglesData, KinematicData, ModesEnum } from '$lib/platform_shared/websocket_message'
interface Props {
defaultColor?: string | null
@@ -51,8 +51,9 @@
let sceneManager = $state(new SceneBuilder())
let canvas: HTMLCanvasElement
let currentModelAngles: number[] = new Array(12).fill(0)
let modelTargetAngles: number[] = new Array(12).fill(0)
// TODO: This assumes that we have 12 angles (valid for the spot robot) but this should not be a static number defined in each individual data set
let currentModelAngles: AnglesData = AnglesData.create({ angles: new Array(12).fill(0) })
let modelTargetAngles: AnglesData = AnglesData.create({ angles: new Array(12).fill(0) })
let gui_panel: GUI
const SMOOTH_AMOUNT = 0.2
@@ -62,13 +63,15 @@
let kinematic = get(currentKinematic)
let planners = {
[ModesEnum.Deactivated]: new IdleState(),
[ModesEnum.Idle]: new IdleState(),
[ModesEnum.Calibration]: new CalibrationState(),
[ModesEnum.Rest]: new RestState(),
[ModesEnum.Stand]: new StandState(),
[ModesEnum.Walk]: new BezierState()
// Incredibly ugly but cant be bothered to fix this or statement right now, we cant key on GaitState objects, only the class extensions themselves (which we dont use here)
const planners: Record<ModesEnum, IdleState | CalibrationState | RestState | StandState | BezierState> = {
[ModesEnum.DEACTIVATED]: new IdleState(),
[ModesEnum.IDLE]: new IdleState(),
[ModesEnum.CALIBRATION]: new CalibrationState(),
[ModesEnum.REST]: new RestState(),
[ModesEnum.STAND]: new StandState(),
[ModesEnum.WALK]: new BezierState(),
[ModesEnum.UNRECOGNIZED]: new IdleState()
}
let lastTick = performance.now()
let lastRobotPosition = new Vector3()
@@ -113,7 +116,13 @@
await populateModelCache()
await createScene()
servoAngles.subscribe(updateAnglesFromStore)
walkGait.subscribe(gait => planners[ModesEnum.Walk].set_mode(walkGaitToMode(gait)))
walkGait.subscribe(gait => {
const walkPlanner = planners[ModesEnum.WALK]
if (!(walkPlanner instanceof BezierState)) {
throw new Error(`Expected BezierState for WALK mode, got ${walkPlanner.constructor.name}`)
}
walkPlanner.set_mode(gait.gait)
})
if (panel) createPanel()
})
@@ -121,7 +130,7 @@
gui_panel?.destroy()
})
const updateAnglesFromStore = (angles: number[]) => {
const updateAnglesFromStore = (angles: AnglesData) => {
if (sceneManager.isDragging) return
if (settings['Internal kinematic']) return
modelTargetAngles = angles
@@ -167,8 +176,10 @@
const setSceneBackground = (c: string | null) => (sceneManager.scene.background = new Color(c!))
const updateAngles = (name: string, angle: number) => {
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num)))
modelTargetAngles.angles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
servoAnglesOut.set(
AnglesData.create({ angles: modelTargetAngles.angles.map(num => Math.round(num)) })
)
}
const createScene = async () => {
@@ -221,7 +232,7 @@
}
let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]))
modelTargetAngles = new_angles
modelTargetAngles.angles = new_angles
}
const orient_robot = (robot: URDFRobot, toes: Vector3[]) => {
@@ -271,22 +282,13 @@
const update_gait = () => {
if (sceneManager.isDragging || !settings['Internal kinematic']) return
const controlData = get(input)
const data = {
lx: controlData.left.x,
ly: controlData.left.y,
rx: controlData.right.x,
ry: controlData.right.y,
h: controlData.height,
s: controlData.speed,
s1: controlData.s1
}
const controlData = get(outControllerData)
let planner = planners[get(mode)]
let planner = planners[get(mode).mode]
const delta = performance.now() - lastTick
lastTick = performance.now()
body_state = planner.step(body_state, data, delta)
body_state = planner.step(body_state, controlData, delta)
settings.omega = body_state.omega
settings.phi = body_state.phi
@@ -327,12 +329,12 @@
sceneManager.transformControl.showZ = settings['Robot transform controls']
for (let i = 0; i < $jointNames.length; i++) {
currentModelAngles[i] = smooth(
currentModelAngles.angles[i] = smooth(
(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
modelTargetAngles[i],
modelTargetAngles.angles[i],
SMOOTH_AMOUNT
)
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]))
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles.angles[i]))
}
orient_robot(robot, toes)
app/src/lib/gait.ts
+22 -32
View File
@@ -1,6 +1,7 @@
import { get } from 'svelte/store'
import type { body_state_t } from './kinematic'
import { currentKinematic } from './stores/featureFlags'
import { HumanInputData, WalkGaits } from './platform_shared/websocket_message'
export interface gait_state_t {
step_height: number
@@ -11,16 +12,6 @@ export interface gait_state_t {
step_depth: number
}
export interface ControllerCommand {
lx: number
ly: number
rx: number
ry: number
h: number
s: number
s1: number
}
export abstract class GaitState {
protected abstract name: string
@@ -62,7 +53,7 @@ export abstract class GaitState {
end() {
console.log('Ending', this.name)
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
step(body_state: body_state_t, command: HumanInputData, dt: number = 0.02) {
this.map_command(command)
this.body_state = body_state
this.dt = dt / 1000
@@ -79,14 +70,14 @@ export abstract class GaitState {
return body_state
}
map_command(command: ControllerCommand) {
map_command(command: HumanInputData) {
const kin = this.kinematic
this.gait_state = {
step_height: command.s1 * kin.max_step_height,
step_x: command.ly * kin.max_step_length,
step_z: -command.lx * kin.max_step_length,
step_velocity: command.s,
step_angle: command.rx,
step_x: command.left!.y * kin.max_step_length,
step_z: -command.left!.x * kin.max_step_length,
step_velocity: command.speed,
step_angle: command.right!.x,
step_depth: kin.default_step_depth
}
}
@@ -94,8 +85,7 @@ export abstract class GaitState {
export class IdleState extends GaitState {
protected name = 'Idle'
step(body_state: body_state_t, command: ControllerCommand) {
step(body_state: body_state_t, command: HumanInputData) {
super.step(body_state, command)
return body_state
}
@@ -104,7 +94,7 @@ export class IdleState extends GaitState {
export class CalibrationState extends GaitState {
protected name = 'Calibration'
step(body_state: body_state_t, _command: ControllerCommand) {
step(body_state: body_state_t, _command: HumanInputData) {
super.step(body_state, _command)
body_state.omega = 0
body_state.phi = 0
@@ -120,7 +110,7 @@ export class CalibrationState extends GaitState {
export class RestState extends GaitState {
protected name = 'Rest'
step(body_state: body_state_t, _command: ControllerCommand) {
step(body_state: body_state_t, _command: HumanInputData) {
super.step(body_state, _command)
body_state.omega = 0
body_state.phi = 0
@@ -136,15 +126,15 @@ export class RestState extends GaitState {
export class StandState extends GaitState {
protected name = 'Stand'
step(body_state: body_state_t, command: ControllerCommand) {
step(body_state: body_state_t, command: HumanInputData) {
super.step(body_state, command)
const kin = this.kinematic
body_state.omega = 0
body_state.ym = kin.min_body_height + command.h * kin.body_height_range
body_state.psi = command.ry * kin.max_pitch
body_state.phi = command.rx * kin.max_roll
body_state.xm = command.ly * kin.max_body_shift_x
body_state.zm = command.lx * kin.max_body_shift_z
body_state.ym = kin.min_body_height + command.height * kin.body_height_range
body_state.psi = command.right!.y * kin.max_pitch
body_state.phi = command.right!.x * kin.max_roll
body_state.xm = command.left!.y * kin.max_body_shift_x
body_state.zm = command.left!.x * kin.max_body_shift_z
body_state.feet = this.default_feet_pos
return body_state
}
@@ -156,7 +146,7 @@ export class BezierState extends GaitState {
protected phase_num = 0
protected step_length = 0
protected stand_offset = 0.75
protected mode: 'crawl' | 'trot' = 'trot'
protected mode: WalkGaits = WalkGaits.TROT
protected speed_factor = 1
offset = [0, 0.5, 0.75, 0.25]
@@ -178,11 +168,11 @@ export class BezierState extends GaitState {
super.begin()
}
set_mode(mode: 'crawl' | 'trot', duty?: number, order?: [number, number, number, number]) {
set_mode(mode: WalkGaits, duty?: number, order?: [number, number, number, number]) {
console.log('BezierState set_mode', mode)
this.mode = mode
if (mode === 'crawl') {
if (mode === WalkGaits.CRAWL) {
this.speed_factor = 0.5
this.stand_offset = duty ?? 0.85
const o = order ?? [3, 0, 2, 1]
@@ -201,10 +191,10 @@ export class BezierState extends GaitState {
super.end()
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
step(body_state: body_state_t, command: HumanInputData, dt: number = 0.02) {
super.step(body_state, command, dt)
const kin = this.kinematic
this.body_state.ym = kin.min_body_height + command.h * kin.body_height_range
this.body_state.ym = kin.min_body_height + command.height * kin.body_height_range
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
this.update_phase()
@@ -232,7 +222,7 @@ export class BezierState extends GaitState {
const moving = m.step_x !== 0 || m.step_z !== 0 || m.step_angle !== 0
if (!moving) return
if (this.mode !== 'crawl') return
if (this.mode !== WalkGaits.CRAWL) return
const { stance, swing, next_swing, time_to_lift } = this.get_leg_states()
app/src/routes/+layout.svelte
+1 -1
View File
@@ -59,7 +59,7 @@
socket.on(RSSIData, (data) => telemetry.setRSSI(data)),
socket.on(ModeData, (data) => mode.set(data)),
socket.on(AnalyticsData, (data) => {analytics.addData(data)}),
socket.on(AnglesData, (data) => {servoAngles.set(data.angles)})
socket.on(AnglesData, (data) => {servoAngles.set(data)})
])
features.subscribe(data => {
if (data?.download_firmware) eventListeners.push( socket.on(DownloadOTAData, (data) => telemetry.setDownloadOTA(data)) )