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 { lerp, degToRad } from 'three/src/math/MathUtils'
import { GUI } from 'three/addons/libs/lil-gui.module.min.js' import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
import { type body_state_t } from '$lib/kinematic' 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 { radToDeg } from 'three/src/math/MathUtils.js'
import type { URDFRobot } from 'urdf-loader' import type { URDFRobot } from 'urdf-loader'
import { get } from 'svelte/store' 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 { interface Props {
defaultColor?: string | null defaultColor?: string | null
@@ -51,8 +51,9 @@
let sceneManager = $state(new SceneBuilder()) let sceneManager = $state(new SceneBuilder())
let canvas: HTMLCanvasElement let canvas: HTMLCanvasElement
let currentModelAngles: 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 modelTargetAngles: number[] = new Array(12).fill(0) 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 let gui_panel: GUI
const SMOOTH_AMOUNT = 0.2 const SMOOTH_AMOUNT = 0.2
@@ -62,13 +63,15 @@
let kinematic = get(currentKinematic) let kinematic = get(currentKinematic)
let planners = { // 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)
[ModesEnum.Deactivated]: new IdleState(), const planners: Record<ModesEnum, IdleState | CalibrationState | RestState | StandState | BezierState> = {
[ModesEnum.Idle]: new IdleState(), [ModesEnum.DEACTIVATED]: new IdleState(),
[ModesEnum.Calibration]: new CalibrationState(), [ModesEnum.IDLE]: new IdleState(),
[ModesEnum.Rest]: new RestState(), [ModesEnum.CALIBRATION]: new CalibrationState(),
[ModesEnum.Stand]: new StandState(), [ModesEnum.REST]: new RestState(),
[ModesEnum.Walk]: new BezierState() [ModesEnum.STAND]: new StandState(),
[ModesEnum.WALK]: new BezierState(),
[ModesEnum.UNRECOGNIZED]: new IdleState()
} }
let lastTick = performance.now() let lastTick = performance.now()
let lastRobotPosition = new Vector3() let lastRobotPosition = new Vector3()
@@ -113,7 +116,13 @@
await populateModelCache() await populateModelCache()
await createScene() await createScene()
servoAngles.subscribe(updateAnglesFromStore) 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() if (panel) createPanel()
}) })
@@ -121,7 +130,7 @@
gui_panel?.destroy() gui_panel?.destroy()
}) })
const updateAnglesFromStore = (angles: number[]) => { const updateAnglesFromStore = (angles: AnglesData) => {
if (sceneManager.isDragging) return if (sceneManager.isDragging) return
if (settings['Internal kinematic']) return if (settings['Internal kinematic']) return
modelTargetAngles = angles modelTargetAngles = angles
@@ -167,8 +176,10 @@
const setSceneBackground = (c: string | null) => (sceneManager.scene.background = new Color(c!)) const setSceneBackground = (c: string | null) => (sceneManager.scene.background = new Color(c!))
const updateAngles = (name: string, angle: number) => { const updateAngles = (name: string, angle: number) => {
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI) modelTargetAngles.angles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))) servoAnglesOut.set(
AnglesData.create({ angles: modelTargetAngles.angles.map(num => Math.round(num)) })
)
} }
const createScene = async () => { const createScene = async () => {
@@ -221,7 +232,7 @@
} }
let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i])) 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[]) => { const orient_robot = (robot: URDFRobot, toes: Vector3[]) => {
@@ -271,22 +282,13 @@
const update_gait = () => { const update_gait = () => {
if (sceneManager.isDragging || !settings['Internal kinematic']) return if (sceneManager.isDragging || !settings['Internal kinematic']) return
const controlData = get(input) const controlData = get(outControllerData)
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
}
let planner = planners[get(mode)] let planner = planners[get(mode).mode]
const delta = performance.now() - lastTick const delta = performance.now() - lastTick
lastTick = performance.now() 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.omega = body_state.omega
settings.phi = body_state.phi settings.phi = body_state.phi
@@ -327,12 +329,12 @@
sceneManager.transformControl.showZ = settings['Robot transform controls'] sceneManager.transformControl.showZ = settings['Robot transform controls']
for (let i = 0; i < $jointNames.length; i++) { 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), (robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
modelTargetAngles[i], modelTargetAngles.angles[i],
SMOOTH_AMOUNT SMOOTH_AMOUNT
) )
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i])) robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles.angles[i]))
} }
orient_robot(robot, toes) orient_robot(robot, toes)
app/src/lib/gait.ts
+22 -32
View File
@@ -1,6 +1,7 @@
import { get } from 'svelte/store' import { get } from 'svelte/store'
import type { body_state_t } from './kinematic' import type { body_state_t } from './kinematic'
import { currentKinematic } from './stores/featureFlags' import { currentKinematic } from './stores/featureFlags'
import { HumanInputData, WalkGaits } from './platform_shared/websocket_message'
export interface gait_state_t { export interface gait_state_t {
step_height: number step_height: number
@@ -11,16 +12,6 @@ export interface gait_state_t {
step_depth: number step_depth: number
} }
export interface ControllerCommand {
lx: number
ly: number
rx: number
ry: number
h: number
s: number
s1: number
}
export abstract class GaitState { export abstract class GaitState {
protected abstract name: string protected abstract name: string
@@ -62,7 +53,7 @@ export abstract class GaitState {
end() { end() {
console.log('Ending', this.name) 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.map_command(command)
this.body_state = body_state this.body_state = body_state
this.dt = dt / 1000 this.dt = dt / 1000
@@ -79,14 +70,14 @@ export abstract class GaitState {
return body_state return body_state
} }
map_command(command: ControllerCommand) { map_command(command: HumanInputData) {
const kin = this.kinematic const kin = this.kinematic
this.gait_state = { this.gait_state = {
step_height: command.s1 * kin.max_step_height, step_height: command.s1 * kin.max_step_height,
step_x: command.ly * kin.max_step_length, step_x: command.left!.y * kin.max_step_length,
step_z: -command.lx * kin.max_step_length, step_z: -command.left!.x * kin.max_step_length,
step_velocity: command.s, step_velocity: command.speed,
step_angle: command.rx, step_angle: command.right!.x,
step_depth: kin.default_step_depth step_depth: kin.default_step_depth
} }
} }
@@ -94,8 +85,7 @@ export abstract class GaitState {
export class IdleState extends GaitState { export class IdleState extends GaitState {
protected name = 'Idle' protected name = 'Idle'
step(body_state: body_state_t, command: HumanInputData) {
step(body_state: body_state_t, command: ControllerCommand) {
super.step(body_state, command) super.step(body_state, command)
return body_state return body_state
} }
@@ -104,7 +94,7 @@ export class IdleState extends GaitState {
export class CalibrationState extends GaitState { export class CalibrationState extends GaitState {
protected name = 'Calibration' protected name = 'Calibration'
step(body_state: body_state_t, _command: ControllerCommand) { step(body_state: body_state_t, _command: HumanInputData) {
super.step(body_state, _command) super.step(body_state, _command)
body_state.omega = 0 body_state.omega = 0
body_state.phi = 0 body_state.phi = 0
@@ -120,7 +110,7 @@ export class CalibrationState extends GaitState {
export class RestState extends GaitState { export class RestState extends GaitState {
protected name = 'Rest' protected name = 'Rest'
step(body_state: body_state_t, _command: ControllerCommand) { step(body_state: body_state_t, _command: HumanInputData) {
super.step(body_state, _command) super.step(body_state, _command)
body_state.omega = 0 body_state.omega = 0
body_state.phi = 0 body_state.phi = 0
@@ -136,15 +126,15 @@ export class RestState extends GaitState {
export class StandState extends GaitState { export class StandState extends GaitState {
protected name = 'Stand' protected name = 'Stand'
step(body_state: body_state_t, command: ControllerCommand) { step(body_state: body_state_t, command: HumanInputData) {
super.step(body_state, command) super.step(body_state, command)
const kin = this.kinematic const kin = this.kinematic
body_state.omega = 0 body_state.omega = 0
body_state.ym = kin.min_body_height + command.h * kin.body_height_range body_state.ym = kin.min_body_height + command.height * kin.body_height_range
body_state.psi = command.ry * kin.max_pitch body_state.psi = command.right!.y * kin.max_pitch
body_state.phi = command.rx * kin.max_roll body_state.phi = command.right!.x * kin.max_roll
body_state.xm = command.ly * kin.max_body_shift_x body_state.xm = command.left!.y * kin.max_body_shift_x
body_state.zm = command.lx * kin.max_body_shift_z body_state.zm = command.left!.x * kin.max_body_shift_z
body_state.feet = this.default_feet_pos body_state.feet = this.default_feet_pos
return body_state return body_state
} }
@@ -156,7 +146,7 @@ export class BezierState extends GaitState {
protected phase_num = 0 protected phase_num = 0
protected step_length = 0 protected step_length = 0
protected stand_offset = 0.75 protected stand_offset = 0.75
protected mode: 'crawl' | 'trot' = 'trot' protected mode: WalkGaits = WalkGaits.TROT
protected speed_factor = 1 protected speed_factor = 1
offset = [0, 0.5, 0.75, 0.25] offset = [0, 0.5, 0.75, 0.25]
@@ -178,11 +168,11 @@ export class BezierState extends GaitState {
super.begin() 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) console.log('BezierState set_mode', mode)
this.mode = mode this.mode = mode
if (mode === 'crawl') { if (mode === WalkGaits.CRAWL) {
this.speed_factor = 0.5 this.speed_factor = 0.5
this.stand_offset = duty ?? 0.85 this.stand_offset = duty ?? 0.85
const o = order ?? [3, 0, 2, 1] const o = order ?? [3, 0, 2, 1]
@@ -201,10 +191,10 @@ export class BezierState extends GaitState {
super.end() 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) super.step(body_state, command, dt)
const kin = this.kinematic 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) 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_phase()
@@ -232,7 +222,7 @@ export class BezierState extends GaitState {
const moving = m.step_x !== 0 || m.step_z !== 0 || m.step_angle !== 0 const moving = m.step_x !== 0 || m.step_z !== 0 || m.step_angle !== 0
if (!moving) return 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() 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(RSSIData, (data) => telemetry.setRSSI(data)),
socket.on(ModeData, (data) => mode.set(data)), socket.on(ModeData, (data) => mode.set(data)),
socket.on(AnalyticsData, (data) => {analytics.addData(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 => { features.subscribe(data => {
if (data?.download_firmware) eventListeners.push( socket.on(DownloadOTAData, (data) => telemetry.setDownloadOTA(data)) ) if (data?.download_firmware) eventListeners.push( socket.on(DownloadOTAData, (data) => telemetry.setDownloadOTA(data)) )