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

👔 Update visualization to better align with robot

Browse Source
This commit is contained in:
Rune Harlyk
2025-07-02 22:55:00 +02:00
parent 632f603fda
commit 7c8c5b40a1
1 changed files with 311 additions and 319 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+161 -169
View File
@@ -1,5 +1,5 @@
<script lang="ts"> <script lang="ts">
import { onDestroy, onMount } from 'svelte'; import { onDestroy, onMount } from 'svelte'
import { import {
BufferGeometry, BufferGeometry,
Line, Line,
@@ -11,7 +11,7 @@
Vector3, Vector3,
type NormalBufferAttributes, type NormalBufferAttributes,
type Object3DEventMap type Object3DEventMap
} from 'three'; } from 'three'
import { import {
ModesEnum, ModesEnum,
kinematicData, kinematicData,
@@ -22,12 +22,17 @@
servoAngles, servoAngles,
mpu, mpu,
jointNames jointNames
} from '$lib/stores'; } from '$lib/stores'
import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities'; import {
import SceneBuilder from '$lib/sceneBuilder'; extractFootColor,
import { lerp, degToRad } from 'three/src/math/MathUtils'; populateModelCache,
import { GUI } from 'three/addons/libs/lil-gui.module.min.js'; throttler,
import Kinematic, { type body_state_t } from '$lib/kinematic'; getToeWorldPositions
} from '$lib/utilities'
import SceneBuilder from '$lib/sceneBuilder'
import { lerp, degToRad } from 'three/src/math/MathUtils'
import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
import Kinematic, { type body_state_t } from '$lib/kinematic'
import { import {
BezierState, BezierState,
CalibrationState, CalibrationState,
@@ -36,42 +41,36 @@
IdleState, IdleState,
RestState, RestState,
StandState StandState
} from '$lib/gait'; } 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'
interface Props { interface Props {
sky?: boolean; sky?: boolean
orbit?: boolean; orbit?: boolean
panel?: boolean; panel?: boolean
debug?: boolean; debug?: boolean
ground?: boolean; ground?: boolean
} }
let { let { sky = true, orbit = false, panel = true, debug = false, ground = true }: Props = $props()
sky = true,
orbit = false,
panel = true,
debug = false,
ground = true
}: Props = $props();
let sceneManager = $state(new SceneBuilder()); let sceneManager = $state(new SceneBuilder())
let canvas: HTMLCanvasElement = $state(); let canvas: HTMLCanvasElement = $state()
let currentModelAngles: number[] = new Array(12).fill(0); let currentModelAngles: number[] = new Array(12).fill(0)
let modelTargetAngles: number[] = new Array(12).fill(0); let modelTargetAngles: number[] = new Array(12).fill(0)
let gui_panel: GUI; let gui_panel: GUI
let Throttler = new throttler(); let Throttler = new throttler()
let feet_trace = new Array(4).fill([]); let feet_trace = new Array(4).fill([])
let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []; let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
let target: Object3D<Object3DEventMap>; let target: Object3D<Object3DEventMap>
let target_position = { x: 0, z: 0, yaw: 0 }; let target_position = { x: 0, z: 0, yaw: 0 }
let kinematic = new Kinematic(); let kinematic = new Kinematic()
let planners = { let planners = {
[ModesEnum.Deactivated]: new IdleState(), [ModesEnum.Deactivated]: new IdleState(),
@@ -81,10 +80,10 @@
[ModesEnum.Stand]: new StandState(), [ModesEnum.Stand]: new StandState(),
[ModesEnum.Crawl]: new EightPhaseWalkState(), [ModesEnum.Crawl]: new EightPhaseWalkState(),
[ModesEnum.Walk]: new BezierState() [ModesEnum.Walk]: new BezierState()
}; }
let lastTick = performance.now(); let lastTick = performance.now()
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]; const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
let body_state = { let body_state = {
omega: 0, omega: 0,
@@ -93,8 +92,8 @@
xm: 0, xm: 0,
ym: 0.5, ym: 0.5,
zm: 0, zm: 0,
feet: planners[ModesEnum.Idle].default_feet_pos feet: kinematic.getDefaultFeetPos()
}; }
let settings = { let settings = {
'Internal kinematic': true, 'Internal kinematic': true,
@@ -112,51 +111,51 @@
ym: 0.7, ym: 0.7,
zm: 0, zm: 0,
Background: 'black' Background: 'black'
}; }
onMount(async () => { onMount(async () => {
await populateModelCache(); await populateModelCache()
await createScene(); await createScene()
servoAngles.subscribe(updateAnglesFromStore); servoAngles.subscribe(updateAnglesFromStore)
if (panel) createPanel(); if (panel) createPanel()
}); })
onDestroy(() => { onDestroy(() => {
canvas.remove(); canvas.remove()
gui_panel?.destroy(); gui_panel?.destroy()
}); })
const updateAnglesFromStore = (angles: number[]) => { const updateAnglesFromStore = (angles: number[]) => {
if (sceneManager.isDragging) return; if (sceneManager.isDragging) return
if (settings['Internal kinematic']) return; if (settings['Internal kinematic']) return
modelTargetAngles = angles; modelTargetAngles = angles
}; }
const createPanel = () => { const createPanel = () => {
gui_panel = new GUI({ width: 310 }); gui_panel = new GUI({ width: 310 })
gui_panel.close(); gui_panel.close()
gui_panel.domElement.id = 'three-gui-panel'; gui_panel.domElement.id = 'three-gui-panel'
const general = gui_panel.addFolder('General'); const general = gui_panel.addFolder('General')
general.add(settings, 'Internal kinematic'); general.add(settings, 'Internal kinematic')
general.add(settings, 'Robot transform controls'); general.add(settings, 'Robot transform controls')
general.add(settings, 'Auto orient robot'); general.add(settings, 'Auto orient robot')
const kinematic = gui_panel.addFolder('Kinematics'); const kinematic = gui_panel.addFolder('Kinematics')
kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen(); kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen()
const visibility = gui_panel.addFolder('Visualization'); const visibility = gui_panel.addFolder('Visualization')
visibility.add(settings, 'Trace feet'); visibility.add(settings, 'Trace feet')
visibility.add(settings, 'Trace points', 1, 1000, 1); visibility.add(settings, 'Trace points', 1, 1000, 1)
visibility.add(settings, 'Target position'); visibility.add(settings, 'Target position')
visibility.add(settings, 'Smooth motion'); visibility.add(settings, 'Smooth motion')
visibility.addColor(settings, 'Background'); visibility.addColor(settings, 'Background')
}; }
const updateKinematicPosition = () => { const updateKinematicPosition = () => {
kinematicData.set([ kinematicData.set([
@@ -166,16 +165,13 @@
settings.xm, settings.xm,
settings.ym, settings.ym,
settings.zm settings.zm
]); ])
}; }
const updateAngles = (name: string, angle: number) => { const updateAngles = (name: string, angle: number) => {
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI); modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
Throttler.throttle( Throttler.throttle(() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))), 100)
() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))), }
100
);
};
const createScene = async () => { const createScene = async () => {
sceneManager sceneManager
@@ -189,46 +185,46 @@
.addTransformControls(sceneManager.model) .addTransformControls(sceneManager.model)
.fillParent() .fillParent()
.addRenderCb(render) .addRenderCb(render)
.startRenderLoop(); .startRenderLoop()
if (ground) sceneManager.addGroundPlane(); if (ground) sceneManager.addGroundPlane()
const geometry = new SphereGeometry(0.1, 32, 16); const geometry = new SphereGeometry(0.1, 32, 16)
const material = new MeshBasicMaterial({ color: 0xffff00 }); const material = new MeshBasicMaterial({ color: 0xffff00 })
target = new Mesh(geometry, material); target = new Mesh(geometry, material)
sceneManager.scene.add(target); sceneManager.scene.add(target)
if (debug) { if (debug) {
sceneManager.addDragControl(updateAngles); sceneManager.addDragControl(updateAngles)
} }
if (sky) sceneManager.addSky(); if (sky) sceneManager.addSky()
for (let i = 0; i < 4; i++) { for (let i = 0; i < 4; i++) {
const geometry = new BufferGeometry(); const geometry = new BufferGeometry()
const material = new LineBasicMaterial({ color: footColor() }); const material = new LineBasicMaterial({ color: extractFootColor() })
const line = new Line(geometry, material); const line = new Line(geometry, material)
trace_lines.push(geometry); trace_lines.push(geometry)
sceneManager.scene.add(line); sceneManager.scene.add(line)
}
} }
};
const renderTraceLines = (foot_positions: Vector3[]) => { const renderTraceLines = (foot_positions: Vector3[]) => {
if (!settings['Trace feet']) { if (!settings['Trace feet']) {
if (!feet_trace.length) return; if (!feet_trace.length) return
trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1))); trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1)))
feet_trace = new Array(4).fill([]); feet_trace = new Array(4).fill([])
return; return
} }
trace_lines.forEach((line, i) => { trace_lines.forEach((line, i) => {
feet_trace[i].push(foot_positions[i]); feet_trace[i].push(foot_positions[i])
feet_trace[i] = feet_trace[i].slice(-settings['Trace points']); feet_trace[i] = feet_trace[i].slice(-settings['Trace points'])
line.setFromPoints(feet_trace[i]); line.setFromPoints(feet_trace[i])
}); })
}; }
const calculate_kinematics = () => { const calculate_kinematics = () => {
if (sceneManager.isDragging || !settings['Internal kinematic']) return; if (sceneManager.isDragging || !settings['Internal kinematic']) return
const position: body_state_t = { const position: body_state_t = {
omega: settings.omega, omega: settings.omega,
phi: settings.phi, phi: settings.phi,
@@ -237,40 +233,36 @@
ym: settings.ym, ym: settings.ym,
zm: settings.zm, zm: settings.zm,
feet: body_state.feet feet: body_state.feet
}; }
let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i])); let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]))
modelTargetAngles = new_angles; modelTargetAngles = new_angles
}; }
const orient_robot = (robot: URDFRobot, toes: Vector3[]) => { const orient_robot = (robot: URDFRobot, toes: Vector3[]) => {
if (settings['Robot transform controls'] || !settings['Auto orient robot']) return; if (settings['Robot transform controls'] || !settings['Auto orient robot']) return
robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y)); robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y))
robot.position.z = smooth(robot.position.z, -settings.xm, 0.1); robot.position.z = smooth(robot.position.z, -settings.xm, 0.1)
robot.position.x = smooth(robot.position.x, -settings.zm, 0.1); robot.position.x = smooth(robot.position.x, -settings.zm, 0.1)
robot.rotation.z = smooth( robot.rotation.z = smooth(robot.rotation.z, degToRad(-settings.phi + $mpu.heading + 90), 0.1)
robot.rotation.z, robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1)
degToRad(-settings.phi + $mpu.heading + 90), robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1)
0.1 }
);
robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1);
robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
};
const update_camera = (robot: URDFRobot) => { const update_camera = (robot: URDFRobot) => {
if (!settings['Fix camera on robot']) return; if (!settings['Fix camera on robot']) return
sceneManager.orbit.target = robot.position.clone(); sceneManager.orbit.target = robot.position.clone()
}; }
const smooth = (start: number, end: number, amount: number) => { const smooth = (start: number, end: number, amount: number) => {
return settings['Smooth motion'] ? lerp(start, end, amount) : end; return settings['Smooth motion'] ? lerp(start, end, amount) : end
}; }
const update_gait = () => { const update_gait = () => {
if (sceneManager.isDragging || !settings['Internal kinematic']) return; if (sceneManager.isDragging || !settings['Internal kinematic']) return
const controlData = get(outControllerData); const controlData = get(outControllerData)
const data = { const data = {
stop: controlData[0], stop: controlData[0],
lx: controlData[1], lx: controlData[1],
@@ -280,66 +272,66 @@
h: controlData[5], h: controlData[5],
s: controlData[6], s: controlData[6],
s1: controlData[7] s1: controlData[7]
}; }
body_state.ym = ((data.h + 127) * 0.75) / 100; body_state.ym = ((data.h + 127) * 0.35) / 100
let planner = planners[get(mode)]; let planner = planners[get(mode)]
const delta = performance.now() - lastTick; const delta = performance.now() - lastTick
lastTick = performance.now(); lastTick = performance.now()
body_state = planner.step(body_state, data, delta); body_state = planner.step(body_state, data, delta)
settings.omega = body_state.omega; settings.omega = body_state.omega
settings.phi = body_state.phi; settings.phi = body_state.phi
settings.psi = body_state.psi; settings.psi = body_state.psi
settings.xm = body_state.xm; settings.xm = body_state.xm
settings.ym = body_state.ym; settings.ym = body_state.ym
settings.zm = body_state.zm; settings.zm = body_state.zm
}; }
const update_robot_position = (robot: URDFRobot) => { const update_robot_position = (robot: URDFRobot) => {
if (!settings['Robot transform controls']) return; if (!settings['Robot transform controls']) return
settings.omega = radToDeg(robot.rotation.y); settings.omega = radToDeg(robot.rotation.y)
settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90; settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90
settings.psi = radToDeg(robot.rotation.x) + 90; settings.psi = radToDeg(robot.rotation.x) + 90
settings.xm = robot.position.z * 100; settings.xm = robot.position.z * 100
settings.zm = -robot.position.x * 100; settings.zm = -robot.position.x * 100
}; }
const updateTargetPosition = () => { const updateTargetPosition = () => {
target.visible = settings['Target position']; target.visible = settings['Target position']
target.position.x = smooth(target.position.x, target_position.x, 0.5); target.position.x = smooth(target.position.x, target_position.x, 0.5)
target.position.z = smooth(target.position.z, target_position.z, 0.5); target.position.z = smooth(target.position.z, target_position.z, 0.5)
}; }
const render = () => { const render = () => {
const robot = sceneManager.model; const robot = sceneManager.model
if (!robot) return; if (!robot) return
const toes = toeWorldPositions(robot); const toes = getToeWorldPositions(robot)
renderTraceLines(toes); renderTraceLines(toes)
update_camera(robot); update_camera(robot)
update_gait(); update_gait()
calculate_kinematics(); calculate_kinematics()
update_robot_position(robot); update_robot_position(robot)
sceneManager.transformControl.showX = settings['Robot transform controls']; sceneManager.transformControl.showX = settings['Robot transform controls']
sceneManager.transformControl.showY = settings['Robot transform controls']; sceneManager.transformControl.showY = settings['Robot transform controls']
sceneManager.transformControl.showZ = settings['Robot transform controls']; sceneManager.transformControl.showZ = settings['Robot transform controls']
for (let i = 0; i < $jointNames.length; i++) { for (let i = 0; i < $jointNames.length; i++) {
currentModelAngles[i] = smooth( currentModelAngles[i] = smooth(
(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI), (robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
modelTargetAngles[i], modelTargetAngles[i],
0.1 0.1
); )
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i])); robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]))
} }
orient_robot(robot, toes); orient_robot(robot, toes)
updateTargetPosition(); updateTargetPosition()
}; }
</script> </script>
<svelte:window onresize={sceneManager.fillParent} /> <svelte:window onresize={sceneManager.fillParent} />