diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 7bba1b8..843c8c9 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -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.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) diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index c6d0fe4..1a6995b 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -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() diff --git a/app/src/routes/+layout.svelte b/app/src/routes/+layout.svelte index 9638bb7..bcd44b8 100644 --- a/app/src/routes/+layout.svelte +++ b/app/src/routes/+layout.svelte @@ -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)) )