From e59b2b1a30742b6daa70f27bc02cee875d316437 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Fri, 2 Feb 2024 21:26:40 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=BA=20Adds=20forward=20kinematics=20an?=
 =?UTF-8?q?d=20keep=20robot=20grounded?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/components/Model/ModelView.svelte |  8 +++--
 app/src/lib/kinematic.ts                  | 39 +++++++++++++++++++++++
 2 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/app/src/components/Model/ModelView.svelte b/app/src/components/Model/ModelView.svelte
index 2406488..c447256 100644
--- a/app/src/components/Model/ModelView.svelte
+++ b/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';
@@ -154,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()
 
diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts
index a471236..f1f444b 100644
--- a/app/src/lib/kinematic.ts
+++ b/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;
+      }
+  }
\ No newline at end of file