diff --git a/app/src/components/Model/ModelView.svelte b/app/src/components/Model/ModelView.svelte index ad4cfdb..2406488 100644 --- a/app/src/components/Model/ModelView.svelte +++ b/app/src/components/Model/ModelView.svelte @@ -127,6 +127,7 @@ const createScene = () => { .addArrowHelper({origin:{x:0, y:0, z:0}, direction:{x:0, y:-2, z:0}}) .addFogExp2(0xcccccc, 0.015) .loadModel('/spot_micro.urdf.xacro') + .addDragControl((name:string, angle:number) => modelTargetAngles[servoNames.indexOf(name)] = angle * (180/Math.PI)) .handleResize() .addRenderCb(render) .startRenderLoop() diff --git a/app/src/components/Model/sceneBuilder.ts b/app/src/components/Model/sceneBuilder.ts index c387771..a3603e3 100644 --- a/app/src/components/Model/sceneBuilder.ts +++ b/app/src/components/Model/sceneBuilder.ts @@ -16,9 +16,11 @@ import { Mesh, CanvasTexture, type ColorRepresentation, type WebGLRendererParameters, + MeshPhongMaterial, } from "three"; import { OrbitControls } from "three/examples/jsm/controls/OrbitControls"; -import URDFLoader from "urdf-loader"; +import URDFLoader, { type URDFMimicJoint } from "urdf-loader"; +import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls' import { XacroLoader } from "xacro-parser"; export const addScene = () => new Scene() @@ -62,6 +64,7 @@ export default class SceneBuilder { public liveStreamTexture: CanvasTexture private fog:FogExp2 private isLoaded:boolean = false + highlightMaterial: any; constructor() { this.scene = new Scene() @@ -163,6 +166,38 @@ export default class SceneBuilder { return this } + private setJointValue(jointName:string, angle:number) { + if (!this.model) return; + if (!this.model.joints[jointName]) return; + this.model.joints[jointName].setJointValue(angle) + } + + isJoint = j => j.isURDFJoint && j.jointType !== 'fixed'; + + highlightLinkGeometry = (m, revert:boolean, material) => { + const traverse = c => { + if (c.type === 'Mesh') { + if (revert) { + c.material = c.__origMaterial; + delete c.__origMaterial; + } else { + c.__origMaterial = c.material; + c.material = material; + } + } + + if (c === m || !this.isJoint(c)) { + for (let i = 0; i < c.children.length; i++) { + const child = c.children[i]; + if (!child.isURDFCollider) { + traverse(c.children[i]); + } + } + } + }; + traverse(m); + }; + public loadModel = (urlXacro:string) => { const xacroLoader = new XacroLoader(); xacroLoader.load(urlXacro, xml => { @@ -182,6 +217,36 @@ export default class SceneBuilder { return this } + public addDragControl = (updateAngle) => { + const highlightColor = '#FFFFFF' + const highlightMaterial = + new MeshPhongMaterial({ + shininess: 10, + color: highlightColor, + emissive: highlightColor, + emissiveIntensity: 0.25, + }); + + const dragControls = new PointerURDFDragControls(this.scene, this.camera, this.renderer.domElement); + dragControls.updateJoint = (joint:URDFMimicJoint, angle:number) => { + this.setJointValue(joint.name, angle); + updateAngle(joint.name, angle) + }; + dragControls.onDragStart = () => { + this.controls.enabled = false; + }; + dragControls.onDragEnd = () => { + this.controls.enabled = true; + }; + dragControls.onHover = (joint:URDFMimicJoint) => { + this.highlightLinkGeometry(joint, false, highlightMaterial); + } + dragControls.onUnhover = (joint:URDFMimicJoint) => { + this.highlightLinkGeometry(joint, true, highlightMaterial); + } + return this + } + public toggleFog = () => { this.scene.fog = this.scene.fog ? null : this.fog; }