✨ Adds animation state

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()
 

app/src/lib/gait.ts

@@ -504,25 +504,25 @@ Units:  meters, radians, seconds / beats
 // }
 
 interface Frame {
-    time: number;
+  time: number
-    position: number[];
+  position: number[]
-    orientation: number[];
+  orientation: number[]
-    feet?: number[][];
+  feet?: number[][]
 }
 
 type Parameter = {
   // 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 {
   // options: Options = {};
-    parameters: Parameters;
+  parameters: Parameters
-    frames: Frame[];
+  frames: Frame[]
 }
 
 const generateCircleAnimation = (
@@ -531,19 +531,19 @@ const generateCircleAnimation = (
   duration: number,
   segments: number
 ): Animation => {
-    const frames: Frame[] = [];
+  const frames: Frame[] = []
-    const deltaTime = duration / segments;
+  const deltaTime = duration / segments
 
   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({
       time: i * deltaTime,
       position: [x, y, z],
       orientation: [0, 0, 0]
-        });
+    })
   }
 
   return {
@@ -552,10 +552,10 @@ const generateCircleAnimation = (
       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: {
@@ -615,7 +615,7 @@ const kinematicShowCase: Animation = {
       orientation: [0, 0, 0]
     }
   ]
-};
+}
 
 const stretch: Animation = {
   parameters: {
@@ -766,7 +766,7 @@ const stretch: Animation = {
       ]
     }
   ]
-};
+}
 
 const pee: Animation = {
   parameters: {
@@ -852,92 +852,84 @@ const pee: Animation = {
       ]
     }
   ]
-};
+}
 
 export class Animater extends GaitState {
-    protected name = 'Bezier';
+  protected name = 'Bezier'
-    time = 0;
+  time = 0
-    animation = pee; // stretch;
+  animation = stretch //pee;
 
   begin() {
-        this.time = 0;
+    this.time = 0
-        super.begin();
+    super.begin()
   }
 
   end() {
-        this.time = 0;
+    this.time = 0
-        super.end();
+    super.end()
   }
 
   step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        return this.step_animation(body_state, dt);
+    return this.step_animation(body_state, dt)
   }
 
   step_animation(body_state: body_state_t, dt: number = 0.02) {
-        this.dt = dt / 1000;
+    this.dt = dt / 1000
-        this.time += dt;
+    this.time += dt
 
-        const duration = this.animation.frames[this.animation.frames.length - 1].time;
+    const duration = this.animation.frames[this.animation.frames.length - 1].time
     if (this.time > duration) {
-            this.time = this.time % duration;
+      this.time = this.time % duration
     }
 
-        const { prevFrame, nextFrame } = this.getBoundingFrames();
+    const { prevFrame, nextFrame } = this.getBoundingFrames()
 
-        const t = this.getInterpolationFactor(prevFrame, nextFrame);
+    const t = this.getInterpolationFactor(prevFrame, nextFrame)
-        const position = this.interpolatePosition(prevFrame.position, nextFrame.position, t);
+    const position = this.interpolatePosition(prevFrame.position, nextFrame.position, t)
-        const orientation = this.interpolatePosition(
+    const orientation = this.interpolatePosition(prevFrame.orientation, nextFrame.orientation, t)
-            prevFrame.orientation,
-            nextFrame.orientation,
-            t
-        );
 
     // Apply x_offset
     // position[0] += this.xOffset;
 
-        body_state.xm = position[0];
+    body_state.xm = position[0]
-        body_state.ym = position[1];
+    body_state.ym = position[1]
-        body_state.zm = position[2];
+    body_state.zm = position[2]
-        body_state.omega = orientation[0];
+    body_state.omega = orientation[0]
-        body_state.phi = orientation[1];
+    body_state.phi = orientation[1]
-        body_state.psi = orientation[2];
+    body_state.psi = orientation[2]
 
     if (prevFrame.feet && nextFrame.feet) {
       for (let i = 0; i < 4; i++) {
-                body_state.feet[i] = this.interpolatePosition(
+        body_state.feet[i] = this.interpolatePosition(prevFrame.feet[i], nextFrame.feet[i], t)
-                    prevFrame.feet[i],
-                    nextFrame.feet[i],
-                    t
-                );
       }
     }
 
-        return body_state;
+    return body_state
   }
 
   private getBoundingFrames(): { prevFrame: Frame; nextFrame: Frame } {
-        const frames = this.animation.frames;
+    const frames = this.animation.frames
 
     for (let i = 0; i < frames.length - 1; i++) {
-            const prevFrame = frames[i];
+      const prevFrame = frames[i]
-            const nextFrame = frames[i + 1];
+      const nextFrame = frames[i + 1]
 
       if (this.time >= prevFrame.time && this.time <= nextFrame.time) {
-                return { prevFrame, nextFrame };
+        return { prevFrame, nextFrame }
       }
     }
 
     // Fallback (should not be reached if looping correctly)
-        return { prevFrame: frames[frames.length - 1], nextFrame: frames[0] };
+    return { prevFrame: frames[frames.length - 1], nextFrame: frames[0] }
   }
 
   private getInterpolationFactor(prevFrame: Frame, nextFrame: Frame): number {
-        const timeRange = nextFrame.time - prevFrame.time;
+    const timeRange = nextFrame.time - prevFrame.time
-        const elapsed = this.time - prevFrame.time;
+    const elapsed = this.time - prevFrame.time
-        return elapsed / timeRange;
+    return elapsed / timeRange
   }
 
   private interpolatePosition(pos1: number[], pos2: number[], t: number): number[] {
-        return pos1.map((val, index) => val + t * (pos2[index] - val));
+    return pos1.map((val, index) => val + t * (pos2[index] - val))
   }
 }

app/src/lib/stores/model-store.ts

@@ -8,7 +8,15 @@ 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]
 
@@ -18,7 +26,8 @@ export enum ModesEnum {
   Calibration = 2,
   Rest = 3,
   Stand = 4,
-    Walk = 5
+  Walk = 5,
+  Animate = 6
 }
 
 export enum WalkGaits {