Merge pull request #3 from runeharlyk/More-robot-control

More robot control

app/package.json

@@ -11,31 +11,31 @@
     "format": "prettier --plugin-search-dir . --write ."
   },
   "devDependencies": {
-    "@sveltejs/vite-plugin-svelte": "^2.0.3",
+    "@sveltejs/vite-plugin-svelte": "^3.0.2",
-    "@tsconfig/svelte": "^4.0.1",
+    "@tsconfig/svelte": "^5.0.2",
-    "@types/three": "^0.154.0",
+    "@types/three": "^0.160.0",
-    "@typescript-eslint/eslint-plugin": "^5.45.0",
+    "@typescript-eslint/eslint-plugin": "^6.20.0",
-    "@typescript-eslint/parser": "^5.45.0",
+    "@typescript-eslint/parser": "^6.20.0",
-    "autoprefixer": "^10.4.14",
+    "autoprefixer": "^10.4.17",
-    "husky": "^8.0.3",
+    "husky": "^9.0.7",
-    "lint-staged": "^13.2.2",
+    "lint-staged": "^15.2.0",
-    "postcss": "^8.4.23",
+    "postcss": "^8.4.33",
-    "prettier": "2.8.8",
+    "prettier": "3.2.4",
-    "svelte": "^3.57.0",
+    "svelte": "^4.2.9",
-    "svelte-check": "^2.10.3",
+    "svelte-check": "^3.6.3",
     "svelte-hero-icons": "^5.0.0",
-    "tailwindcss": "^3.3.2",
+    "tailwindcss": "^3.4.1",
-    "tslib": "^2.5.0",
+    "tslib": "^2.6.2",
-    "typescript": "^5.0.2",
+    "typescript": "^5.3.3",
-    "vite": "^4.3.2",
+    "vite": "^5.0.12",
     "vite-plugin-compression": "^0.5.1",
-    "vite-plugin-singlefile": "^0.13.5"
+    "vite-plugin-singlefile": "^1.0.0"
   },
   "dependencies": {
     "nipplejs": "^0.10.1",
-    "svelte-routing": "^1.11.0",
+    "svelte-routing": "^2.11.0",
-    "three": "^0.154.0",
+    "three": "^0.160.1",
-    "urdf-loader": "^0.12.0",
+    "urdf-loader": "^0.12.1",
     "uzip": "^0.20201231.0",
     "xacro-parser": "^0.3.9"
   },

app/src/components/Model/ModelView.svelte

@@ -5,7 +5,7 @@ import { dataBuffer, servoBuffer } from '../../lib/socket'
 import { lerp } from '../../lib/utils';
 import uzip from 'uzip';
 import { outControllerData } from '../../lib/store';
-import Kinematic from '../../lib/kinematic';
+import Kinematic, { ForwardKinematics } from '../../lib/kinematic';
 import location from '../../lib/location';
 import FileCache from '../../lib/cache';
 import SceneBuilder from './sceneBuilder';
@@ -122,11 +122,12 @@ const createScene = () => {
         .addOrbitControls(10, 30)
         .addGroundPlane({x:0, y:-2, z:0})
         .addGridHelper({size:250, divisions:125, y:-2})
-        .addAmbientLight({color:0xffffff, intensity:0.3})
+        .addAmbientLight({color:0xffffff, intensity:0.7})
-        .addDirectionalLight({x:50, y:100, z:100, color:0xffffff, intensity:0.9})
+        .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')
+        .addDragControl((name:string, angle:number) => modelTargetAngles[servoNames.indexOf(name)] = angle * (180/Math.PI))
         .handleResize()
         .addRenderCb(render)
         .startRenderLoop()
@@ -153,7 +154,11 @@ const render = () => {
     const robot = sceneManager.model
     if(!robot) return
 
-    sceneManager.model.rotation.z = lerp(robot.rotation.z, degToRad($dataBuffer[1] + 90), 0.1)
+    const forwardKinematics = new ForwardKinematics()
+
+    const points = forwardKinematics.calculateFootpoints(modelTargetAngles.map(ang => degToRad(ang)) as number[])
+    robot.position.y = Math.max(...points.map(coord => coord[0] / 100)) - 2.7
+    robot.rotation.z = lerp(robot.rotation.z, degToRad($dataBuffer[1] + 90), 0.1)
     
     handleVideoStream()
 

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()
@@ -156,13 +159,45 @@ export default class SceneBuilder {
     }
 
     public addArrowHelper = (options?:arrowOptions) => {
-        const dir = new Vector3(options.direction.x ?? 0, options.direction.y ?? 0, options.direction.z ?? 0);
+        const dir = new Vector3(options?.direction.x ?? 0, options?.direction.y ?? 0, options?.direction.z ?? 0);
-        const origin = new Vector3(options.origin.x ?? 0, options.origin.y ?? 0, options.origin.z ?? 0);
+        const origin = new Vector3(options?.origin.x ?? 0, options?.origin.y ?? 0, options?.origin.z ?? 0);
-        const arrowHelper = new ArrowHelper( dir, origin, options.length ?? 1.5, options.color ?? 0xff0000 );
+        const arrowHelper = new ArrowHelper( dir, origin, options?.length ?? 1.5, options?.color ?? 0xff0000 );
         this.scene.add( arrowHelper );
         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;
     }

app/src/lib/kinematic.ts

@@ -340,3 +340,42 @@ export default class Kinematic {
       return transposed;
     }
   }
+
+
+export class ForwardKinematics {
+    private l1: number;
+    private l2: number;
+    private l3: number;
+    private l4: number;
+  
+    constructor() {
+      this.l1 = 50;
+      this.l2 = 20;
+      this.l3 = 120;
+      this.l4 = 155;
+    }
+  
+    public calculateFootpoint(theta1: number, theta2: number, theta3: number): number[] {
+      const { cos, sin } = Math;
+  
+      const x = this.l1 * cos(theta1) + this.l2 * cos(theta1) + this.l3 * cos(theta1 + theta2) + this.l4 * cos(theta1 + theta2 + theta3);
+      const y = this.l1 * sin(theta1) + this.l2 * sin(theta1) + this.l3 * sin(theta1 + theta2) + this.l4 * sin(theta1 + theta2 + theta3);
+      const z = 0; 
+  
+      return [x, y, z];
+    }
+
+    public calculateFootpoints(angles: number[]): number[][] {
+        const footpoints: number[][] = [];
+    
+        for (let i = 0; i < angles.length; i += 3) {
+          const theta1 = angles[i];
+          const theta2 = angles[i + 1];
+          const theta3 = angles[i + 2];
+          const footpoint = this.calculateFootpoint(theta1, theta2, theta3);
+          footpoints.push(footpoint);
+        }
+    
+        return footpoints;
+      }
+  }