diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 75d5ceb..248007e 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -36,7 +36,14 @@ 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 { + Animater, + 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' @@ -71,7 +78,8 @@ [ModesEnum.Calibration]: new CalibrationState(), [ModesEnum.Rest]: new RestState(), [ModesEnum.Stand]: new StandState(), - [ModesEnum.Walk]: new BezierState() + [ModesEnum.Walk]: new BezierState(), + [ModesEnum.Animate]: new Animater() } let lastTick = performance.now() diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index 9c2e48f..e07f512 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -504,440 +504,432 @@ Units: meters, radians, seconds / beats // } interface Frame { - time: number; - position: number[]; - orientation: number[]; - feet?: number[][]; + time: number + position: number[] + orientation: number[] + feet?: number[][] } type Parameter = { - // name: string; - min: number; - max: number; - default: number; -}; + // name: string; + min: number + max: number + default: number +} -type Parameters = Record; +type Parameters = Record interface Animation { - // options: Options = {}; - parameters: Parameters; - frames: Frame[]; + // options: Options = {}; + parameters: Parameters + frames: Frame[] } const generateCircleAnimation = ( - radius: number, - y: number, - duration: number, - segments: number + radius: number, + y: number, + duration: number, + segments: number ): Animation => { - const frames: Frame[] = []; - const deltaTime = duration / segments; + const frames: Frame[] = [] + const deltaTime = duration / segments - for (let i = 0; i <= segments; i++) { - const angle = (2 * Math.PI * i) / segments; // Angle in radians - const x = radius * Math.cos(angle); - const z = radius * Math.sin(angle); + for (let i = 0; i <= segments; i++) { + const angle = (2 * Math.PI * i) / segments // Angle in radians + const x = radius * Math.cos(angle) + const z = radius * Math.sin(angle) - frames.push({ - time: i * deltaTime, - position: [x, y, z], - orientation: [0, 0, 0] - }); - } + frames.push({ + time: i * deltaTime, + position: [x, y, z], + orientation: [0, 0, 0] + }) + } - return { - parameters: { - speed: { min: 0.1, max: 2, default: 1 }, - x_offset: { min: -0.1, max: 0.1, default: 0 } - }, - frames - }; -}; + return { + parameters: { + speed: { min: 0.1, max: 2, default: 1 }, + x_offset: { min: -0.1, max: 0.1, default: 0 } + }, + frames + } +} -const kinematicShowCaseGen = generateCircleAnimation(0.5, 0.7, 4000, 32); +const kinematicShowCaseGen = generateCircleAnimation(0.5, 0.7, 4000, 32) const kinematicShowCase: Animation = { - parameters: { - speed: { min: 0.1, max: 2, default: 1 }, - x_offset: { min: -0.1, max: 0.1, default: 0 } + parameters: { + speed: { min: 0.1, max: 2, default: 1 }, + x_offset: { min: -0.1, max: 0.1, default: 0 } + }, + frames: [ + { + time: 0, + position: [0.5, 0.7, 0], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] }, - frames: [ - { - time: 0, - position: [0.5, 0.7, 0], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 500, - position: [0.3, 0.7, 0.3], - orientation: [0, 0, 0] - }, - { - time: 1000, - position: [0, 0.7, 0.5], - orientation: [0, 0, 0] - }, - { - time: 1500, - position: [-0.3, 0.7, 0.3], - orientation: [0, 0, 0] - }, - { - time: 2000, - position: [-0.5, 0.7, 0], - orientation: [0, 0, 0] - }, - { - time: 2500, - position: [-0.3, 0.7, -0.3], - orientation: [0, 0, 0] - }, - { - time: 3000, - position: [0, 0.7, -0.5], - orientation: [0, 0, 0] - }, - { - time: 3500, - position: [0.3, 0.7, -0.3], - orientation: [0, 0, 0] - }, - { - time: 4000, - position: [0.5, 0.7, 0], - orientation: [0, 0, 0] - } - ] -}; + { + time: 500, + position: [0.3, 0.7, 0.3], + orientation: [0, 0, 0] + }, + { + time: 1000, + position: [0, 0.7, 0.5], + orientation: [0, 0, 0] + }, + { + time: 1500, + position: [-0.3, 0.7, 0.3], + orientation: [0, 0, 0] + }, + { + time: 2000, + position: [-0.5, 0.7, 0], + orientation: [0, 0, 0] + }, + { + time: 2500, + position: [-0.3, 0.7, -0.3], + orientation: [0, 0, 0] + }, + { + time: 3000, + position: [0, 0.7, -0.5], + orientation: [0, 0, 0] + }, + { + time: 3500, + position: [0.3, 0.7, -0.3], + orientation: [0, 0, 0] + }, + { + time: 4000, + position: [0.5, 0.7, 0], + orientation: [0, 0, 0] + } + ] +} const stretch: Animation = { - parameters: { - speed: { min: 0.1, max: 2, default: 1 }, - x_offset: { min: -0.1, max: 0.1, default: 0 } + parameters: { + speed: { min: 0.1, max: 2, default: 1 }, + x_offset: { min: -0.1, max: 0.1, default: 0 } + }, + frames: [ + // Step forward + { + time: 0, + position: [0, 0.7, 0], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] }, - frames: [ - // Step forward - { - time: 0, - position: [0, 0.7, 0], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 250, - position: [0, 0.7, -0.2], - orientation: [0, 0, 0], - feet: [ - [1.5, -0.5, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 500, - position: [0, 0.7, -0.2], - orientation: [0, 0, 0], - feet: [ - [2, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 750, - position: [0, 0.7, 0.2], - orientation: [0, 0, 0], - feet: [ - [2, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 1000, - position: [0, 0.7, 0.2], - orientation: [0, 0, 0], - feet: [ - [2, -1, 1, 1], - [1.5, -0.5, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 1250, - position: [0, 0.7, 0.2], - orientation: [0, 0, 0], - feet: [ - [2, -1, 1, 1], - [2, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 2500, - position: [0.5, 0.7, 0], - orientation: [0, 0, 25] - }, - { - time: 4000, - position: [-0.7, 0.7, 0], - orientation: [0, 0, -20] - }, - { - time: 5000, - position: [-0.7, 0.7, 0], - orientation: [0, 0, -20] - }, - { - time: 6000, - position: [0, 0.7, 0], - orientation: [0, 0, 0] - }, - { - time: 6000, - position: [-0.2, 0.7, -0.2], - orientation: [0, 0, 0], - feet: [ - [2, -1, 1, 1], - [2, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 6500, - position: [-0.2, 0.7, -0.2], - orientation: [0, 0, 0], - feet: [ - [0.5, -0.5, 1, 1], - [2, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 7000, - position: [-0.2, 0.7, 0.2], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [2, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 7500, - position: [-0.2, 0.7, 0.2], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [0.5, -0.5, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 8000, - position: [0, 0.7, 0], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - } - ] -}; + { + time: 250, + position: [0, 0.7, -0.2], + orientation: [0, 0, 0], + feet: [ + [1.5, -0.5, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 500, + position: [0, 0.7, -0.2], + orientation: [0, 0, 0], + feet: [ + [2, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 750, + position: [0, 0.7, 0.2], + orientation: [0, 0, 0], + feet: [ + [2, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 1000, + position: [0, 0.7, 0.2], + orientation: [0, 0, 0], + feet: [ + [2, -1, 1, 1], + [1.5, -0.5, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 1250, + position: [0, 0.7, 0.2], + orientation: [0, 0, 0], + feet: [ + [2, -1, 1, 1], + [2, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 2500, + position: [0.5, 0.7, 0], + orientation: [0, 0, 25] + }, + { + time: 4000, + position: [-0.7, 0.7, 0], + orientation: [0, 0, -20] + }, + { + time: 5000, + position: [-0.7, 0.7, 0], + orientation: [0, 0, -20] + }, + { + time: 6000, + position: [0, 0.7, 0], + orientation: [0, 0, 0] + }, + { + time: 6000, + position: [-0.2, 0.7, -0.2], + orientation: [0, 0, 0], + feet: [ + [2, -1, 1, 1], + [2, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 6500, + position: [-0.2, 0.7, -0.2], + orientation: [0, 0, 0], + feet: [ + [0.5, -0.5, 1, 1], + [2, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 7000, + position: [-0.2, 0.7, 0.2], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [2, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 7500, + position: [-0.2, 0.7, 0.2], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [0.5, -0.5, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 8000, + position: [0, 0.7, 0], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + } + ] +} const pee: Animation = { - parameters: { - speed: { min: 0.1, max: 2, default: 1 }, - x_offset: { min: -0.1, max: 0.1, default: 0 } + parameters: { + speed: { min: 0.1, max: 2, default: 1 }, + x_offset: { min: -0.1, max: 0.1, default: 0 } + }, + frames: [ + { + time: 0, + position: [0, 0.7, 0], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] }, - frames: [ - { - time: 0, - position: [0, 0.7, 0], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 1000, - position: [0, 0.7, 0], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 2000, - position: [0.2, 0.7, 0.45], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 3000, - position: [0.2, 0.7, 0.45], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 4000, - position: [0.2, 0.7, 0.45], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, 0, -1, 1] - ] - }, - { - time: 5000, - position: [0.2, 0.7, 0.45], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - }, - { - time: 6000, - position: [0, 0.7, 0], - orientation: [0, 0, 0], - feet: [ - [1, -1, 1, 1], - [1, -1, -1, 1], - [-1, -1, 1, 1], - [-1, -1, -1, 1] - ] - } - ] -}; + { + time: 1000, + position: [0, 0.7, 0], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 2000, + position: [0.2, 0.7, 0.45], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 3000, + position: [0.2, 0.7, 0.45], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 4000, + position: [0.2, 0.7, 0.45], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, 0, -1, 1] + ] + }, + { + time: 5000, + position: [0.2, 0.7, 0.45], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + }, + { + time: 6000, + position: [0, 0.7, 0], + orientation: [0, 0, 0], + feet: [ + [1, -1, 1, 1], + [1, -1, -1, 1], + [-1, -1, 1, 1], + [-1, -1, -1, 1] + ] + } + ] +} export class Animater extends GaitState { - protected name = 'Bezier'; - time = 0; - animation = pee; // stretch; + protected name = 'Bezier' + time = 0 + animation = stretch //pee; - begin() { - this.time = 0; - super.begin(); + begin() { + this.time = 0 + super.begin() + } + + end() { + this.time = 0 + super.end() + } + + step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { + return this.step_animation(body_state, dt) + } + + step_animation(body_state: body_state_t, dt: number = 0.02) { + this.dt = dt / 1000 + this.time += dt + + const duration = this.animation.frames[this.animation.frames.length - 1].time + if (this.time > duration) { + this.time = this.time % duration } - end() { - this.time = 0; - super.end(); + const { prevFrame, nextFrame } = this.getBoundingFrames() + + const t = this.getInterpolationFactor(prevFrame, nextFrame) + const position = this.interpolatePosition(prevFrame.position, nextFrame.position, t) + const orientation = this.interpolatePosition(prevFrame.orientation, nextFrame.orientation, t) + + // Apply x_offset + // position[0] += this.xOffset; + + body_state.xm = position[0] + body_state.ym = position[1] + body_state.zm = position[2] + body_state.omega = orientation[0] + body_state.phi = orientation[1] + body_state.psi = orientation[2] + + if (prevFrame.feet && nextFrame.feet) { + for (let i = 0; i < 4; i++) { + body_state.feet[i] = this.interpolatePosition(prevFrame.feet[i], nextFrame.feet[i], t) + } } - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { - return this.step_animation(body_state, dt); + return body_state + } + + private getBoundingFrames(): { prevFrame: Frame; nextFrame: Frame } { + const frames = this.animation.frames + + for (let i = 0; i < frames.length - 1; i++) { + const prevFrame = frames[i] + const nextFrame = frames[i + 1] + + if (this.time >= prevFrame.time && this.time <= nextFrame.time) { + return { prevFrame, nextFrame } + } } - step_animation(body_state: body_state_t, dt: number = 0.02) { - this.dt = dt / 1000; - this.time += dt; + // Fallback (should not be reached if looping correctly) + return { prevFrame: frames[frames.length - 1], nextFrame: frames[0] } + } - const duration = this.animation.frames[this.animation.frames.length - 1].time; - if (this.time > duration) { - this.time = this.time % duration; - } + private getInterpolationFactor(prevFrame: Frame, nextFrame: Frame): number { + const timeRange = nextFrame.time - prevFrame.time + const elapsed = this.time - prevFrame.time + return elapsed / timeRange + } - const { prevFrame, nextFrame } = this.getBoundingFrames(); - - const t = this.getInterpolationFactor(prevFrame, nextFrame); - const position = this.interpolatePosition(prevFrame.position, nextFrame.position, t); - const orientation = this.interpolatePosition( - prevFrame.orientation, - nextFrame.orientation, - t - ); - - // Apply x_offset - // position[0] += this.xOffset; - - body_state.xm = position[0]; - body_state.ym = position[1]; - body_state.zm = position[2]; - body_state.omega = orientation[0]; - body_state.phi = orientation[1]; - body_state.psi = orientation[2]; - - if (prevFrame.feet && nextFrame.feet) { - for (let i = 0; i < 4; i++) { - body_state.feet[i] = this.interpolatePosition( - prevFrame.feet[i], - nextFrame.feet[i], - t - ); - } - } - - return body_state; - } - - private getBoundingFrames(): { prevFrame: Frame; nextFrame: Frame } { - const frames = this.animation.frames; - - for (let i = 0; i < frames.length - 1; i++) { - const prevFrame = frames[i]; - const nextFrame = frames[i + 1]; - - if (this.time >= prevFrame.time && this.time <= nextFrame.time) { - return { prevFrame, nextFrame }; - } - } - - // Fallback (should not be reached if looping correctly) - return { prevFrame: frames[frames.length - 1], nextFrame: frames[0] }; - } - - private getInterpolationFactor(prevFrame: Frame, nextFrame: Frame): number { - const timeRange = nextFrame.time - prevFrame.time; - const elapsed = this.time - prevFrame.time; - return elapsed / timeRange; - } - - private interpolatePosition(pos1: number[], pos2: number[], t: number): number[] { - return pos1.map((val, index) => val + t * (pos2[index] - val)); - } + private interpolatePosition(pos1: number[], pos2: number[], t: number): number[] { + return pos1.map((val, index) => val + t * (pos2[index] - val)) + } } diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index c8f3cd7..da8e71d 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -8,17 +8,26 @@ export const jointNames = persistentStore('joint_names', []) export const model = writable() -export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'walk'] as const +export const modes = [ + 'deactivated', + 'idle', + 'calibration', + 'rest', + 'stand', + 'walk', + 'animate' +] as const export type Modes = (typeof modes)[number] export enum ModesEnum { - Deactivated = 0, - Idle = 1, - Calibration = 2, - Rest = 3, - Stand = 4, - Walk = 5 + Deactivated = 0, + Idle = 1, + Calibration = 2, + Rest = 3, + Stand = 4, + Walk = 5, + Animate = 6 } export enum WalkGaits {