diff --git a/app/src/App.svelte b/app/src/App.svelte index 38c2ec3..ef8ca4d 100644 --- a/app/src/App.svelte +++ b/app/src/App.svelte @@ -9,11 +9,16 @@ import FileCache from './lib/cache'; import { socketLocation } from './lib/location'; import Settings from './routes/Settings.svelte'; + import { jointNames, model } from './lib/store'; + import { loadModelAsync } from './lib/modelLoader'; export let url = window.location.pathname - onMount(() => { + onMount(async () => { connect(socketLocation); registerFetchIntercept() + const [urdf, JOINT_NAME] = await loadModelAsync('/spot_micro.urdf.xacro') + jointNames.set(JOINT_NAME) + model.set(urdf) }); const registerFetchIntercept = () => { diff --git a/app/src/components/Model/ModelView.svelte b/app/src/components/Model/ModelView.svelte index fe0bade..3f91d4e 100644 --- a/app/src/components/Model/ModelView.svelte +++ b/app/src/components/Model/ModelView.svelte @@ -4,7 +4,7 @@ import { CanvasTexture, CircleGeometry, Mesh, MeshBasicMaterial} from 'three'; import {socket, angles, mpu } from '../../lib/socket' import { lerp } from '../../lib/utils'; import uzip from 'uzip'; -import { outControllerData } from '../../lib/store'; +import { model, outControllerData } from '../../lib/store'; import { ForwardKinematics } from '../../lib/kinematic'; import location from '../../lib/location'; import FileCache from '../../lib/cache'; @@ -37,23 +37,11 @@ interface EulerAngle { psi: number; } -interface Point { - x: number; - y: number; - z: number; -} - -interface BodyState { - euler: EulerAngle; - position: Point; - legPositions:[number, number, number, number]; -} - const degToRad = (val:number) => val * (Math.PI / 180) onMount(async () => { await cacheModelFiles() - createScene() + await createScene() outControllerData.subscribe(data => { $socket.send(JSON.stringify({ @@ -84,7 +72,7 @@ const updateAngles = (name:string, angle:number) => { $socket.send(JSON.stringify({type:"kinematic/angle", angle:angle * (180/Math.PI), id:servoNames.indexOf(name)})) } -const createScene = () => { +const createScene = async () => { sceneManager = new SceneBuilder() .addRenderer({ antialias: true, canvas: canvas, alpha: true}) .addPerspectiveCamera({x:-0.5, y:0.5, z:1}) @@ -96,7 +84,7 @@ const createScene = () => { .addDirectionalLight({x:10, y:100, z:10, color:0xffffff, intensity:1}) .addArrowHelper({origin:{x:0, y:0, z:0}, direction:{x:0, y:-2, z:0}}) .addFogExp2(0xcccccc, 0.015) - .loadModel('/spot_micro.urdf.xacro') + .addModel($model) .addDragControl(updateAngles) .handleResize() .addRenderCb(render) diff --git a/app/src/components/Model/sceneBuilder.ts b/app/src/components/Model/sceneBuilder.ts index eb8edc9..b21dfe5 100644 --- a/app/src/components/Model/sceneBuilder.ts +++ b/app/src/components/Model/sceneBuilder.ts @@ -214,7 +214,7 @@ export default class SceneBuilder { isJoint = j => j.isURDFJoint && j.jointType !== 'fixed'; - highlightLinkGeometry = (m, revert:boolean, material) => { + highlightLinkGeometry = (m: URDFMimicJoint, revert:boolean, material: MeshPhongMaterial) => { const traverse = c => { if (c.type === 'Mesh') { if (revert) { @@ -238,26 +238,13 @@ export default class SceneBuilder { traverse(m); }; - public loadModel = (urlXacro:string) => { - const xacroLoader = new XacroLoader(); - xacroLoader.load(urlXacro, xml => { - const urdfLoader = new URDFLoader(); - urdfLoader.workingPath = LoaderUtils.extractUrlBase(urlXacro); - - this.model = urdfLoader.parse(xml); - this.model.rotation.x = -Math.PI / 2; - this.model.rotation.z = Math.PI / 2; - this.model.traverse(c => c.castShadow = true); - this.model.updateMatrixWorld(true); - this.model.scale.setScalar(10); - - this.scene.add(this.model); - - }, (error) => console.log(error)); + public addModel = (model: any) => { + this.model = model + this.scene.add(model) return this } - public addDragControl = (updateAngle) => { + public addDragControl = (updateAngle:any) => { const highlightColor = '#FFFFFF' const highlightMaterial = new MeshPhongMaterial({ diff --git a/app/src/lib/modelLoader.ts b/app/src/lib/modelLoader.ts new file mode 100644 index 0000000..e892ef3 --- /dev/null +++ b/app/src/lib/modelLoader.ts @@ -0,0 +1,31 @@ +import { LoaderUtils } from "three"; +import URDFLoader, { type URDFRobot } from "urdf-loader" +import { XacroLoader } from "xacro-parser" + +export const loadModelAsync = async (url:string):Promise<[URDFRobot, string[]]> => { + return new Promise((resolve, reject) => { + const xacroLoader = new XacroLoader(); + + xacroLoader.load(url, async (xml) => { + const urdfLoader = new URDFLoader(); + + urdfLoader.workingPath = LoaderUtils.extractUrlBase(url); + + try { + const model = urdfLoader.parse(xml); + model.rotation.x = -Math.PI / 2; + model.rotation.z = Math.PI / 2; + model.traverse(c => c.castShadow = true); + model.updateMatrixWorld(true); + model.scale.setScalar(10); + const joints = Object.entries(model.joints) + .filter(joint => joint[1]._jointType !== 'fixed') + .map(joint => joint[0]) + + resolve([model, joints]); + } catch (error) { + reject(error); + } + }, (error) => reject(error)); + }); +} \ No newline at end of file diff --git a/app/src/lib/store.ts b/app/src/lib/store.ts index 49c54fb..26ed9ff 100644 --- a/app/src/lib/store.ts +++ b/app/src/lib/store.ts @@ -1,7 +1,12 @@ import { writable } from 'svelte/store'; +import { persistentStore } from './utils'; export const emulateModel = writable(true); export const input = writable({left:{x:0, y:0}, right:{x:0, y:0}, height:70, speed:0}); export const outControllerData = writable(new Uint8Array([0, 128, 128, 128, 128, 70, 0])); + +export const jointNames = persistentStore("joint_names", []) + +export const model = writable() \ No newline at end of file