✨ Adds animation state
This commit is contained in:
Rune Harlyk
@@ -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()
|
||||
|
||||
|
||||
+394
-402
@@ -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<string, Parameter>;
|
||||
type Parameters = Record<string, Parameter>
|
||||
|
||||
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))
|
||||
}
|
||||
}
|
||||
|
||||
@@ -8,17 +8,26 @@ export const jointNames = persistentStore('joint_names', <string[]>[])
|
||||
|
||||
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 {
|
||||
|
||||
Reference in New Issue
Block a user