🎨 Expand animation with looping

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

@@ -490,4 +490,465 @@ const comb = (n: number, k: number): number => {
     let c = 1
     for (let i = 0; i < k; i++) c = (c * (n - i)) / (i + 1)
     return c
+};
+
+/*
+Units:  meters, radians, seconds / beats
+*/
+
+// interface Options {
+//     controls: 'body' | 'legs' | 'both';
+
+//     extendable?: boolean; // if true, the animation can loop
+//     description?: string; // a description of the animation
+// }
+
+interface Frame {
+  time: number
+  position: number[]
+  orientation: number[]
+  feet?: number[][]
+}
+
+type Parameter = {
+  // name: string;
+  min: number
+  max: number
+  default: number
+}
+
+type Parameters = Record<string, Parameter>
+
+interface Animation {
+  // options: Options = {};
+  parameters: Parameters
+  frames: Frame[]
+  loop?: boolean
+}
+
+const generateCircleAnimation = (
+  radius: number,
+  y: number,
+  duration: number,
+  segments: number
+): Animation => {
+  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)
+
+    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
+  }
+}
+
+const kinematicShowCaseGen = generateCircleAnimation(0.5, 0.7, 4000, 32)
+
+const kinematicShowCase: Animation = {
+  loop: true,
+  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]
+      ]
+    },
+    {
+      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 = {
+  loop: false,
+  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]
+      ]
+    },
+    {
+      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 = {
+  loop: false,
+
+  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]
+      ]
+    },
+    {
+      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 = [stretch, pee, kinematicShowCase][0]
+  speed = 1
+  xOffset = 0
+
+  begin() {
+    this.reset()
+    super.begin()
+  }
+
+  end() {
+    this.reset()
+    super.end()
+  }
+
+  reset() {
+    this.time = 0
+  }
+
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    return this.step_animation(body_state, dt)
+  }
+
+  setAnimation(animation: Animation) {
+    this.animation = animation
+    this.reset()
+  }
+
+  step_animation(body_state: body_state_t, dt: number = 0.02) {
+    this.dt = dt / 1000
+    const frames = this.animation.frames
+    const duration = frames[frames.length - 1].time
+
+    this.time += dt * this.speed
+    if (this.animation.loop !== false && this.time > duration) {
+      this.time = this.time % duration
+    } else if (this.time > duration) {
+      this.time = duration
+    }
+
+    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))
+  }
 }

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 {