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

✨ Adds animation state

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-04 21:02:41 +02:00
parent 59f6089335
commit 7bc11bf94b
3 changed files with 420 additions and 411 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+10 -2
View File
@@ -36,7 +36,14 @@
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 {
Animater,
BezierState,
CalibrationState,
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'
@@ -71,7 +78,8 @@
[ModesEnum.Calibration]: new CalibrationState(), [ModesEnum.Calibration]: new CalibrationState(),
[ModesEnum.Rest]: new RestState(), [ModesEnum.Rest]: new RestState(),
[ModesEnum.Stand]: new StandState(), [ModesEnum.Stand]: new StandState(),
[ModesEnum.Walk]: new BezierState() [ModesEnum.Walk]: new BezierState(),
[ModesEnum.Animate]: new Animater()
} }
let lastTick = performance.now() let lastTick = performance.now()
app/src/lib/gait.ts
+394 -402
View File
@@ -504,440 +504,432 @@ Units: meters, radians, seconds / beats
// } // }
interface Frame { interface Frame {
time: number; time: number
position: number[]; position: number[]
orientation: number[]; orientation: number[]
feet?: number[][]; feet?: number[][]
} }
type Parameter = { type Parameter = {
// name: string; // name: string;
min: number; min: number
max: number; max: number
default: number; default: number
}; }
type Parameters = Record<string, Parameter>; type Parameters = Record<string, Parameter>
interface Animation { interface Animation {
// options: Options = {}; // options: Options = {};
parameters: Parameters; parameters: Parameters
frames: Frame[]; frames: Frame[]
} }
const generateCircleAnimation = ( const generateCircleAnimation = (
radius: number, radius: number,
y: number, y: number,
duration: number, duration: number,
segments: number segments: number
): Animation => { ): Animation => {
const frames: Frame[] = []; const frames: Frame[] = []
const deltaTime = duration / segments; const deltaTime = duration / segments
for (let i = 0; i <= segments; i++) { for (let i = 0; i <= segments; i++) {
const angle = (2 * Math.PI * i) / segments; // Angle in radians const angle = (2 * Math.PI * i) / segments // Angle in radians
const x = radius * Math.cos(angle); const x = radius * Math.cos(angle)
const z = radius * Math.sin(angle); const z = radius * Math.sin(angle)
frames.push({ frames.push({
time: i * deltaTime, time: i * deltaTime,
position: [x, y, z], position: [x, y, z],
orientation: [0, 0, 0] orientation: [0, 0, 0]
}); })
} }
return { return {
parameters: { parameters: {
speed: { min: 0.1, max: 2, default: 1 }, speed: { min: 0.1, max: 2, default: 1 },
x_offset: { min: -0.1, max: 0.1, default: 0 } x_offset: { min: -0.1, max: 0.1, default: 0 }
}, },
frames frames
}; }
}; }
const kinematicShowCaseGen = generateCircleAnimation(0.5, 0.7, 4000, 32); const kinematicShowCaseGen = generateCircleAnimation(0.5, 0.7, 4000, 32)
const kinematicShowCase: Animation = { const kinematicShowCase: Animation = {
parameters: { parameters: {
speed: { min: 0.1, max: 2, default: 1 }, speed: { min: 0.1, max: 2, default: 1 },
x_offset: { min: -0.1, max: 0.1, default: 0 } 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: 500,
time: 0, position: [0.3, 0.7, 0.3],
position: [0.5, 0.7, 0], orientation: [0, 0, 0]
orientation: [0, 0, 0], },
feet: [ {
[1, -1, 1, 1], time: 1000,
[1, -1, -1, 1], position: [0, 0.7, 0.5],
[-1, -1, 1, 1], orientation: [0, 0, 0]
[-1, -1, -1, 1] },
] {
}, time: 1500,
{ position: [-0.3, 0.7, 0.3],
time: 500, orientation: [0, 0, 0]
position: [0.3, 0.7, 0.3], },
orientation: [0, 0, 0] {
}, time: 2000,
{ position: [-0.5, 0.7, 0],
time: 1000, orientation: [0, 0, 0]
position: [0, 0.7, 0.5], },
orientation: [0, 0, 0] {
}, time: 2500,
{ position: [-0.3, 0.7, -0.3],
time: 1500, orientation: [0, 0, 0]
position: [-0.3, 0.7, 0.3], },
orientation: [0, 0, 0] {
}, time: 3000,
{ position: [0, 0.7, -0.5],
time: 2000, orientation: [0, 0, 0]
position: [-0.5, 0.7, 0], },
orientation: [0, 0, 0] {
}, time: 3500,
{ position: [0.3, 0.7, -0.3],
time: 2500, orientation: [0, 0, 0]
position: [-0.3, 0.7, -0.3], },
orientation: [0, 0, 0] {
}, time: 4000,
{ position: [0.5, 0.7, 0],
time: 3000, orientation: [0, 0, 0]
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 = { const stretch: Animation = {
parameters: { parameters: {
speed: { min: 0.1, max: 2, default: 1 }, speed: { min: 0.1, max: 2, default: 1 },
x_offset: { min: -0.1, max: 0.1, default: 0 } 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: 250,
{ position: [0, 0.7, -0.2],
time: 0, orientation: [0, 0, 0],
position: [0, 0.7, 0], feet: [
orientation: [0, 0, 0], [1.5, -0.5, 1, 1],
feet: [ [1, -1, -1, 1],
[1, -1, 1, 1], [-1, -1, 1, 1],
[1, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 500,
{ position: [0, 0.7, -0.2],
time: 250, orientation: [0, 0, 0],
position: [0, 0.7, -0.2], feet: [
orientation: [0, 0, 0], [2, -1, 1, 1],
feet: [ [1, -1, -1, 1],
[1.5, -0.5, 1, 1], [-1, -1, 1, 1],
[1, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 750,
{ position: [0, 0.7, 0.2],
time: 500, orientation: [0, 0, 0],
position: [0, 0.7, -0.2], feet: [
orientation: [0, 0, 0], [2, -1, 1, 1],
feet: [ [1, -1, -1, 1],
[2, -1, 1, 1], [-1, -1, 1, 1],
[1, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 1000,
{ position: [0, 0.7, 0.2],
time: 750, orientation: [0, 0, 0],
position: [0, 0.7, 0.2], feet: [
orientation: [0, 0, 0], [2, -1, 1, 1],
feet: [ [1.5, -0.5, -1, 1],
[2, -1, 1, 1], [-1, -1, 1, 1],
[1, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 1250,
{ position: [0, 0.7, 0.2],
time: 1000, orientation: [0, 0, 0],
position: [0, 0.7, 0.2], feet: [
orientation: [0, 0, 0], [2, -1, 1, 1],
feet: [ [2, -1, -1, 1],
[2, -1, 1, 1], [-1, -1, 1, 1],
[1.5, -0.5, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 2500,
{ position: [0.5, 0.7, 0],
time: 1250, orientation: [0, 0, 25]
position: [0, 0.7, 0.2], },
orientation: [0, 0, 0], {
feet: [ time: 4000,
[2, -1, 1, 1], position: [-0.7, 0.7, 0],
[2, -1, -1, 1], orientation: [0, 0, -20]
[-1, -1, 1, 1], },
[-1, -1, -1, 1] {
] time: 5000,
}, position: [-0.7, 0.7, 0],
{ orientation: [0, 0, -20]
time: 2500, },
position: [0.5, 0.7, 0], {
orientation: [0, 0, 25] time: 6000,
}, position: [0, 0.7, 0],
{ orientation: [0, 0, 0]
time: 4000, },
position: [-0.7, 0.7, 0], {
orientation: [0, 0, -20] time: 6000,
}, position: [-0.2, 0.7, -0.2],
{ orientation: [0, 0, 0],
time: 5000, feet: [
position: [-0.7, 0.7, 0], [2, -1, 1, 1],
orientation: [0, 0, -20] [2, -1, -1, 1],
}, [-1, -1, 1, 1],
{ [-1, -1, -1, 1]
time: 6000, ]
position: [0, 0.7, 0], },
orientation: [0, 0, 0] {
}, time: 6500,
{ position: [-0.2, 0.7, -0.2],
time: 6000, orientation: [0, 0, 0],
position: [-0.2, 0.7, -0.2], feet: [
orientation: [0, 0, 0], [0.5, -0.5, 1, 1],
feet: [ [2, -1, -1, 1],
[2, -1, 1, 1], [-1, -1, 1, 1],
[2, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 7000,
{ position: [-0.2, 0.7, 0.2],
time: 6500, orientation: [0, 0, 0],
position: [-0.2, 0.7, -0.2], feet: [
orientation: [0, 0, 0], [1, -1, 1, 1],
feet: [ [2, -1, -1, 1],
[0.5, -0.5, 1, 1], [-1, -1, 1, 1],
[2, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 7500,
{ position: [-0.2, 0.7, 0.2],
time: 7000, orientation: [0, 0, 0],
position: [-0.2, 0.7, 0.2], feet: [
orientation: [0, 0, 0], [1, -1, 1, 1],
feet: [ [0.5, -0.5, -1, 1],
[1, -1, 1, 1], [-1, -1, 1, 1],
[2, -1, -1, 1], [-1, -1, -1, 1]
[-1, -1, 1, 1], ]
[-1, -1, -1, 1] },
] {
}, time: 8000,
{ position: [0, 0.7, 0],
time: 7500, orientation: [0, 0, 0],
position: [-0.2, 0.7, 0.2], feet: [
orientation: [0, 0, 0], [1, -1, 1, 1],
feet: [ [1, -1, -1, 1],
[1, -1, 1, 1], [-1, -1, 1, 1],
[0.5, -0.5, -1, 1], [-1, -1, -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 = { const pee: Animation = {
parameters: { parameters: {
speed: { min: 0.1, max: 2, default: 1 }, speed: { min: 0.1, max: 2, default: 1 },
x_offset: { min: -0.1, max: 0.1, default: 0 } 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: 1000,
time: 0, position: [0, 0.7, 0],
position: [0, 0.7, 0], orientation: [0, 0, 0],
orientation: [0, 0, 0], feet: [
feet: [ [1, -1, 1, 1],
[1, -1, 1, 1], [1, -1, -1, 1],
[1, -1, -1, 1], [-1, -1, 1, 1],
[-1, -1, 1, 1], [-1, -1, -1, 1]
[-1, -1, -1, 1] ]
] },
}, {
{ time: 2000,
time: 1000, position: [0.2, 0.7, 0.45],
position: [0, 0.7, 0], orientation: [0, 0, 0],
orientation: [0, 0, 0], feet: [
feet: [ [1, -1, 1, 1],
[1, -1, 1, 1], [1, -1, -1, 1],
[1, -1, -1, 1], [-1, -1, 1, 1],
[-1, -1, 1, 1], [-1, -1, -1, 1]
[-1, -1, -1, 1] ]
] },
}, {
{ time: 3000,
time: 2000, position: [0.2, 0.7, 0.45],
position: [0.2, 0.7, 0.45], orientation: [0, 0, 0],
orientation: [0, 0, 0], feet: [
feet: [ [1, -1, 1, 1],
[1, -1, 1, 1], [1, -1, -1, 1],
[1, -1, -1, 1], [-1, -1, 1, 1],
[-1, -1, 1, 1], [-1, -1, -1, 1]
[-1, -1, -1, 1] ]
] },
}, {
{ time: 4000,
time: 3000, position: [0.2, 0.7, 0.45],
position: [0.2, 0.7, 0.45], orientation: [0, 0, 0],
orientation: [0, 0, 0], feet: [
feet: [ [1, -1, 1, 1],
[1, -1, 1, 1], [1, -1, -1, 1],
[1, -1, -1, 1], [-1, -1, 1, 1],
[-1, -1, 1, 1], [-1, 0, -1, 1]
[-1, -1, -1, 1] ]
] },
}, {
{ time: 5000,
time: 4000, position: [0.2, 0.7, 0.45],
position: [0.2, 0.7, 0.45], orientation: [0, 0, 0],
orientation: [0, 0, 0], feet: [
feet: [ [1, -1, 1, 1],
[1, -1, 1, 1], [1, -1, -1, 1],
[1, -1, -1, 1], [-1, -1, 1, 1],
[-1, -1, 1, 1], [-1, -1, -1, 1]
[-1, 0, -1, 1] ]
] },
}, {
{ time: 6000,
time: 5000, position: [0, 0.7, 0],
position: [0.2, 0.7, 0.45], orientation: [0, 0, 0],
orientation: [0, 0, 0], feet: [
feet: [ [1, -1, 1, 1],
[1, -1, 1, 1], [1, -1, -1, 1],
[1, -1, -1, 1], [-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 { export class Animater extends GaitState {
protected name = 'Bezier'; protected name = 'Bezier'
time = 0; time = 0
animation = pee; // stretch; animation = stretch //pee;
begin() { begin() {
this.time = 0; this.time = 0
super.begin(); 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() { const { prevFrame, nextFrame } = this.getBoundingFrames()
this.time = 0;
super.end(); 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 body_state
return this.step_animation(body_state, dt); }
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) { // Fallback (should not be reached if looping correctly)
this.dt = dt / 1000; return { prevFrame: frames[frames.length - 1], nextFrame: frames[0] }
this.time += dt; }
const duration = this.animation.frames[this.animation.frames.length - 1].time; private getInterpolationFactor(prevFrame: Frame, nextFrame: Frame): number {
if (this.time > duration) { const timeRange = nextFrame.time - prevFrame.time
this.time = this.time % duration; const elapsed = this.time - prevFrame.time
} return elapsed / timeRange
}
const { prevFrame, nextFrame } = this.getBoundingFrames(); private interpolatePosition(pos1: number[], pos2: number[], t: number): number[] {
return pos1.map((val, index) => val + t * (pos2[index] - val))
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));
}
} }
app/src/lib/stores/model-store.ts
+16 -7
View File
@@ -8,17 +8,26 @@ export const jointNames = persistentStore('joint_names', <string[]>[])
export const model = writable() 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 type Modes = (typeof modes)[number]
export enum ModesEnum { export enum ModesEnum {
Deactivated = 0, Deactivated = 0,
Idle = 1, Idle = 1,
Calibration = 2, Calibration = 2,
Rest = 3, Rest = 3,
Stand = 4, Stand = 4,
Walk = 5 Walk = 5,
Animate = 6
} }
export enum WalkGaits { export enum WalkGaits {