diff --git a/app/src/components/Model/ModelView.svelte b/app/src/components/Model/ModelView.svelte index 7621309..18446d7 100644 --- a/app/src/components/Model/ModelView.svelte +++ b/app/src/components/Model/ModelView.svelte @@ -14,7 +14,11 @@ import { MathUtils, LoaderUtils, GridHelper, - Camera + Camera, + FogExp2, + MeshBasicMaterial, + CanvasTexture, + CircleGeometry } from 'three'; import { XacroLoader } from 'xacro-parser'; import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js'; @@ -24,9 +28,11 @@ import { lerp } from '../../lib/utils'; import uzip from 'uzip'; import { outControllerData } from '../../lib/store'; import Kinematic from '../../lib/kinematic'; +import location from '../../lib/location'; -let el: HTMLCanvasElement; -let scene: Scene, camera: Camera, renderer: WebGLRenderer, controls: OrbitControls, robot; +let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement, scene: Scene, camera: Camera, renderer: WebGLRenderer, controls: OrbitControls, robot, isLoaded = false; + +let context: CanvasRenderingContext2D, texture: CanvasTexture let modelAngles:number[] | Int8Array = new Array(12).fill(0) let modelTargetAngles:number[] | Int8Array = new Array(12).fill(0) @@ -37,6 +43,11 @@ let modelTargeBodyAngles:EulerAngle = {omega: 0, phi: 0, psi: 0 } let modelBodyPoint:Point = {x: 0, y: 0, z: 0 } let modelTargetBodyPoint:Point = {x: 0, y: 0, z: 0 } +const dir = [ -1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1] +const videoStream = `//${location}/stream`; + +let showModel = true, showStream = false + const servoNames = [ "front_left_shoulder", "front_left_leg", "front_left_foot", "front_right_shoulder", "front_right_leg", "front_right_foot", @@ -83,13 +94,11 @@ const stand = () => { const calculateKinematics = () => { const kinematic = new Kinematic(); const angles: number[] = [degToRad(modelTargeBodyAngles.omega), degToRad(modelTargeBodyAngles.phi), degToRad(modelTargeBodyAngles.psi)]; - const center: number[] = [modelBodyPoint.x, modelBodyPoint.y, modelBodyPoint.z]; + const center: number[] = [modelTargetBodyPoint.x, modelTargetBodyPoint.y, modelTargetBodyPoint.z]; const Lp = [[100,-100,100,1],[100,-100,-100,1],[-100,-100,100,1],[-100,-100,-100,1]] const legs = kinematic.calcIK(Lp, angles, center) - const dir = [ -1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1] - const legsAngles = legs .map(x => x.map(y => radToDeg(y))) .flat() @@ -102,12 +111,16 @@ onMount(async () => { createScene() outControllerData.subscribe(data => { - modelTargeBodyAngles = {omega:(data[1]-128) / 3, phi:(data[2]-128) / 3, psi:(data[3]-128) / 4} + modelTargeBodyAngles = {omega:0, phi:(data[1]-128) / 3, psi:(data[2]-128) / 4} + modelTargetBodyPoint = {x:(data[4]-128) / 2, y:data[5], z:(data[3]-128) / 2} // (data[5]-128) / 4 calculateKinematics() - //modelTargetBodyPoint = data.bodyPoint }) servoBuffer.subscribe(angles => modelTargetAngles = angles) + + modelTargeBodyAngles = {omega:0, phi:0, psi:0} + modelTargetBodyPoint = {x:0, y:0, z:0} + stand() }); const cacheModelFiles = async () => { @@ -136,6 +149,7 @@ const loadModel = () => { robot.rotation.z = Math.PI / 2; robot.traverse(c => c.castShadow = true); robot.updateMatrixWorld(true); + robot.scale.setScalar(10); scene.add( robot ); @@ -144,40 +158,51 @@ const loadModel = () => { const createScene = () => { scene = new Scene(); + camera = new PerspectiveCamera(); camera.position.set(-0.5, 0.5, 1); - renderer = new WebGLRenderer({ antialias: true, canvas: el }); + renderer = new WebGLRenderer({ antialias: true, canvas: canvas, alpha: true }); renderer.outputEncoding = sRGBEncoding; renderer.shadowMap.enabled = true; renderer.shadowMap.type = PCFSoftShadowMap; document.body.appendChild(renderer.domElement); - const directionalLight = new DirectionalLight(0xffffff, 1.0); + const directionalLight = new DirectionalLight(0xffffff, 0.9); directionalLight.castShadow = true; - directionalLight.shadow.mapSize.setScalar(1024); - directionalLight.position.set(5, 30, 5); + directionalLight.shadow.mapSize.setScalar(2048); + directionalLight.shadow.mapSize.width = 1024; + directionalLight.shadow.mapSize.height = 1024; + directionalLight.position.set(50, 100, 100); + directionalLight.shadow.radius = 5 scene.add(directionalLight); - const ambientLight = new AmbientLight(0xffffff, 0.2); + const ambientLight = new AmbientLight(0xffffff, 0.3); scene.add(ambientLight); - const ground = new Mesh(new PlaneGeometry(), new ShadowMaterial({ opacity: 0.25 })); + if(!showStream) scene.fog = new FogExp2( 0xcccccc, 0.015 ); + + const ground = new Mesh( new PlaneGeometry(), new ShadowMaterial({side: 2})); ground.rotation.x = -Math.PI / 2; ground.scale.setScalar(30); + ground.position.y = -2 ground.receiveShadow = true; scene.add(ground); - const size = 10; - const divisions = 50; + context = streamCanvas.getContext("2d"); + texture = new CanvasTexture( stream ); + const liveStream = new Mesh( new CircleGeometry(35, 32), new MeshBasicMaterial({ map: texture })) + liveStream.position.z = -50 + liveStream.visible = showStream + scene.add(liveStream) - const gridHelper = new GridHelper(size, divisions); - gridHelper.position.y = -0.24 + const gridHelper = new GridHelper(250, 125); + gridHelper.position.y = -2; scene.add(gridHelper); controls = new OrbitControls(camera, renderer.domElement); - controls.minDistance = 0; - controls.maxDistance = 4; + controls.minDistance = 10; + controls.maxDistance = 30; controls.update(); loadModel() @@ -199,6 +224,21 @@ const render = () => { if(!robot) return + if(!isLoaded){ + const intervalId = setInterval(() => { + robot.traverse(c => c.castShadow = true); + }, 10); + setTimeout(() => { + clearInterval(intervalId) + }, 1000); + isLoaded = true; + } + + if(isLoaded && showStream) { + context.drawImage(stream, 0, 0) + texture.needsUpdate = true; + } + for (let i = 0; i < servoNames.length; i++) { modelAngles[i] = lerp(robot.joints[servoNames[i]].angle * (180/Math.PI), modelTargetAngles[i], 0.1) robot.joints[servoNames[i]].setJointValue(MathUtils.degToRad(modelAngles[i])); @@ -206,11 +246,7 @@ const render = () => { modelBodyAngles.omega = lerp(robot.rotation.x * (180/Math.PI), modelTargeBodyAngles.omega - 90, 0.1) modelBodyAngles.phi = lerp(robot.rotation.y * (180/Math.PI), modelTargeBodyAngles.phi, 0.1) - modelBodyAngles.psi = lerp(robot.rotation.z * (180/Math.PI), modelTargeBodyAngles.psi + 90, 0.1) - - // robot.rotation.x = MathUtils.degToRad(modelBodyAngles.omega) - // robot.rotation.y = MathUtils.degToRad(modelBodyAngles.phi) - // robot.rotation.z = MathUtils.degToRad(modelBodyAngles.psi) + modelBodyAngles.psi = lerp(robot.rotation.z * (180/Math.PI), modelTargeBodyAngles.psi + 90, 0.1) } @@ -225,18 +261,18 @@ const render = () => {

Motor angles

- {#each servoNames as name, i} + {#each Object.entries(robot?.joints ?? {}).filter(x => x[1].jointValue.length > 0) as [name, joint], i}
{name}: - - + +
{/each}

Body rotation

- {#each Object.entries(modelBodyAngles) as [name, angle]} + {#each Object.keys(modelBodyAngles) as name}
{name}: @@ -247,7 +283,7 @@ const render = () => {

Body position

- {#each Object.entries(modelBodyPoint) as [name, coordinate]} + {#each Object.keys(modelBodyPoint) as name}
{name}: @@ -257,4 +293,14 @@ const render = () => {
- \ No newline at end of file +{#if showStream} + + {/if} + + \ No newline at end of file diff --git a/app/src/components/Topbar.svelte b/app/src/components/Topbar.svelte index efa556c..447f824 100644 --- a/app/src/components/Topbar.svelte +++ b/app/src/components/Topbar.svelte @@ -48,7 +48,7 @@ -
diff --git a/app/src/lib/socket.ts b/app/src/lib/socket.ts index 1b68524..0362701 100644 --- a/app/src/lib/socket.ts +++ b/app/src/lib/socket.ts @@ -22,6 +22,14 @@ export const connect = (url:string) => { _socket.onclose = _disconnected; _socket.onmessage = _message; socket.set(_socket) + + servoBuffer.subscribe(data => { + if(_socket.readyState !== 1) return + const buffer = [] + buffer[0] = 1 + buffer.push(...data) + _socket.send(new Int8Array(buffer)) + }) } const _connected = () => { diff --git a/app/src/lib/store.ts b/app/src/lib/store.ts index 30a92ec..3136c3c 100644 --- a/app/src/lib/store.ts +++ b/app/src/lib/store.ts @@ -4,6 +4,6 @@ export const sidebarOpen = writable(false); export const emulateModel = writable(true); -export const input = writable({left:{x:0, y:0}, right:{x:0, y:0}, height:0, speed:0}); +export const input = writable({left:{x:0, y:0}, right:{x:0, y:0}, height:70, speed:0}); export const outControllerData = writable(new Uint8Array(6)); diff --git a/app/src/routes/Controller.svelte b/app/src/routes/Controller.svelte index 9b788a3..bbc1d3c 100644 --- a/app/src/routes/Controller.svelte +++ b/app/src/routes/Controller.svelte @@ -2,10 +2,14 @@ import Stream from '../Views/Stream.svelte'; import Controls from '../components/Controls.svelte'; import ModelView from '../components/Model/ModelView.svelte'; + import { emulateModel } from '../lib/store';
- - + {#if $emulateModel} + + {:else} + + {/if}