From 1b2d6a98506625b8a96e2be3a7c080d6294b120e Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 27 May 2024 01:24:43 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=9B=B8=20Adds=20tranformcontroller=20for?=
 =?UTF-8?q?=20body?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/components/Visualization.svelte | 141 ++++++++++++-----
 app/src/lib/kinematic.ts                    | 160 +++++++++++++++-----
 app/src/lib/sceneBuilder.ts                 |  51 ++++---
 app/src/lib/stores/model-store.ts           |   2 +-
 4 files changed, 257 insertions(+), 97 deletions(-)

diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte
index d93d584..f72a636 100644
--- a/app/src/lib/components/Visualization.svelte
+++ b/app/src/lib/components/Visualization.svelte
@@ -1,16 +1,18 @@
 <script lang="ts">
 	import { onDestroy, onMount } from 'svelte';
-	import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
+	import { BufferGeometry, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Object3D, SphereGeometry, Vector3, type NormalBufferAttributes, type Object3DEventMap } from 'three';
 	import uzip from 'uzip';
-	import { kinematicData, model, outControllerData, servoAnglesOut } from '$lib/stores';
+	import { ModesEnum, kinematicData, mode, model, outControllerData, servoAnglesOut } from '$lib/stores';
 	import { footColor, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
 	import { fileService } from '$lib/services';
 	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
 	import { lerp, degToRad } from 'three/src/math/MathUtils';
     import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
-	import Kinematic, { type position_t } from '$lib/kinematic';
+	import Kinematic, { GaitPlanner, type body_state_t } from '$lib/kinematic';
 	import { radToDeg } from 'three/src/math/MathUtils.js';
+	import type { URDFRobot } from 'urdf-loader';
+	import { get } from 'svelte/store';
 
     export let sky = true
     export let orbit = false
@@ -28,8 +30,12 @@
 
     let feet_trace = new Array(4).fill([]);
     let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
+    let target: Object3D<Object3DEventMap>;
+
+    let target_position = {x: 0, z: 0, yaw: 0}
 
     let kinematic = new Kinematic()
+    let gaitPlanner = new GaitPlanner()
     const dir = [-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1]
     const Lp = [
         [100, -100, 100, 1],
@@ -39,7 +45,9 @@
     ];
 
     let settings = {
-        'Internal kinematic':false,
+        'Internal kinematic':true,
+        'Robot transform controls':false,
+        'Auto orient robot':true,
         'Trace feet':debug,
         'Trace points': 30,
         'Fix camera on robot': true,
@@ -69,12 +77,25 @@
                 s: buffer[6],
             };
 
-            settings.omega = 0
-            settings.phi = data.rx / 4, 
-            settings.psi = data.ry / 4, 
-            settings.x = data.ly / 2, 
-            settings.y = (data.h+128)*0.75, 
-            settings.z = data.lx / 2
+            settings.y = (data.h+128)*0.75
+
+            switch (get(mode)) {
+                case ModesEnum.Stand:
+                    settings.omega = 0
+                    settings.phi = data.rx / 4 
+                    settings.psi = data.ry / 4 
+                    settings.x = data.ly / 2 
+                    settings.z = data.lx / 2
+                    break;
+
+                case ModesEnum.Walk:
+                    // target_position.yaw += data.lx / 100
+                    // const angle = radToDeg(Math.atan2(data.rx, data.ry)) - 90 + $mpu.heading + target_position.yaw;
+                    // const distance = Math.sqrt(data.rx**2 + data.ry**2) / 25
+                    // target_position.x = distance * Math.cos(degToRad(angle))
+                    // target_position.z = distance * Math.sin(degToRad(angle))
+                    break;
+            }
         })
 	});
     
@@ -96,11 +117,13 @@
 
         const general = gui_panel.addFolder('General');
         general.add(settings, 'Internal kinematic')
+        general.add(settings, 'Robot transform controls')
+        general.add(settings, 'Auto orient robot')
 
         const kinematic = gui_panel.addFolder('Kinematics');
-        kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition)
+        kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen();
+        kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen();
+        kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen();
         kinematic.add(settings, 'x', -90, 90).onChange(updateKinematicPosition)
         kinematic.add(settings, 'y', 0, 200).onChange(updateKinematicPosition)
         kinematic.add(settings, 'z', -130, 130).onChange(updateKinematicPosition)
@@ -146,6 +169,7 @@
 			.addAmbientLight({ color: 0xffffff, intensity: 0.6 })
 			.addFogExp2(0xcccccc, 0.015)
 			.addModel($model)
+            .addTransformControls(sceneManager.model)
 			.fillParent()
 			.addRenderCb(render)
 			.startRenderLoop();
@@ -165,6 +189,11 @@
 			trace_lines.push(geometry);
 			sceneManager.scene.add(line);
 		}
+
+        const geometry = new SphereGeometry(0.1, 32, 16 ); 
+        const material = new MeshBasicMaterial( { color: 0xffff00 } ); 
+        target = new Mesh(geometry, material); 
+        sceneManager.scene.add(target);
 	};
 
     const renderTraceLines = (foot_positions: Vector3[]) => {
@@ -183,19 +212,60 @@
     }
 
     const calculate_kinematics = () => {
-        const position:position_t = {
+        if (sceneManager.isDragging || !settings['Internal kinematic']) return
+        const position:body_state_t = {
             omega: settings.omega,
             phi: settings.phi,
             psi: settings.psi,
             xm: settings.x,
             ym: settings.y,
-            zm: settings.z
+            zm: settings.z,
+            feet: Lp
         }
 
-        let new_angles = kinematic.calcIK(Lp, position).map((x, i) => radToDeg(x * dir[i]));
+        let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]));
         modelTargetAngles = new_angles;
     }
 
+    const orient_robot = (robot: URDFRobot, toes:Vector3[]) => {
+        if (settings['Robot transform controls'] || !settings['Auto orient robot']) return
+        robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
+        robot.position.z = lerp(robot.position.z, -settings.x / 100, 0.1);
+
+        robot.rotation.z = lerp(robot.rotation.z, degToRad(settings.phi + $mpu.heading + 90), 0.1);
+        robot.rotation.y = lerp(robot.rotation.y, degToRad(settings.omega - settings.z / 2.5), 0.1);
+        robot.rotation.x = lerp(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
+    }
+
+    const update_camera = (robot:URDFRobot) => {
+        if (!settings['Fix camera on robot']) return
+        sceneManager.orbit.target = robot.position.clone()
+    }
+
+    const update_gate = () => {
+        if (get(mode) != ModesEnum.Walk) return
+        const body_state = {
+            omega: settings.omega,
+            phi: settings.phi,
+            psi: settings.psi,
+            xm: settings.x,
+            ym: settings.y,
+            zm: settings.z,
+            feet: Lp
+        }
+        gaitPlanner.step(body_state, 0.1)
+
+    }
+
+    const update_robot_position = (robot:URDFRobot) => {
+        if (!settings['Robot transform controls']) return
+        settings.omega = radToDeg(robot.rotation.y)
+        settings.phi = radToDeg(robot.rotation.z) + $mpu.heading -90
+        settings.psi = radToDeg(robot.rotation.x) + 90
+        settings.x = robot.position.z * 100
+        settings.z = -robot.position.x * 100
+    }
+
 	const render = () => {
 		const robot = sceneManager.model;
 		if (!robot) return;
@@ -203,28 +273,29 @@
         const toes = toeWorldPositions(robot)
 
         renderTraceLines(toes)
-
-        if (settings['Fix camera on robot']) {
-            sceneManager.controls.target = robot.position.clone()
-        }
-
-        if (settings['Internal kinematic']) calculate_kinematics()
-
-		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
-		robot.position.z = lerp(robot.position.z, -settings.x / 100, 0.1);
-
-		robot.rotation.z = lerp(robot.rotation.z, degToRad(settings.phi + $mpu.heading + 90), 0.1);
-        robot.rotation.y = lerp(robot.rotation.y, degToRad(settings.omega - settings.z / 2.5), 0.1);
-        robot.rotation.x = lerp(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
-
+        update_camera(robot)
+        update_gate()
+        calculate_kinematics()
+        update_robot_position(robot)
+        
+        sceneManager.transformControl.showX = settings['Robot transform controls']
+        sceneManager.transformControl.showY = settings['Robot transform controls']
+        sceneManager.transformControl.showZ = settings['Robot transform controls']
+        
 		for (let i = 0; i < $jointNames.length; i++) {
-			currentModelAngles[i] = lerp(
-				(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
+            currentModelAngles[i] = lerp(
+                (robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
 				modelTargetAngles[i],
 				0.1
-			);
-			robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
-		}
+                );
+                robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
+        }
+
+        orient_robot(robot, toes) 
+
+        target.position.x = lerp(target.position.x, target_position.x, 0.5)
+        // target.position.y = lerp(target.position.y, robot.position.y, 0.5)
+        target.position.z = lerp(target.position.z, target_position.z, 0.5)
 	};
 
 
diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts
index c698e4b..9b616ef 100644
--- a/app/src/lib/kinematic.ts
+++ b/app/src/lib/kinematic.ts
@@ -1,10 +1,25 @@
-export interface position_t {
+import { radToDeg } from 'three/src/math/MathUtils.js';
+
+export interface body_state_t {
 	omega: number;
 	phi: number;
 	psi: number;
 	xm: number;
 	ym: number;
 	zm: number;
+	feet: number[][];
+}
+
+export interface position {
+	x: number;
+	y: number;
+	z: number;
+}
+
+export interface target_position {
+	x: number;
+	z: number;
+	yaw: number;
 }
 
 const { cos, sin, atan2, sqrt, acos } = Math;
@@ -78,22 +93,28 @@ export default class Kinematic {
 		];
 	}
 
-	public calcIK(Lp: number[][], position: position_t): number[] {
-		this.bodyIK(position);
+	public calcIK(body_state: body_state_t): number[] {
+		this.bodyIK(body_state);
 
 		return [
-			...this.legIK(this.multiplyVector(this.inverse(this.Tlf), Lp[0])),
+			...this.legIK(this.multiplyVector(this.inverse(this.Tlf), body_state.feet[0])),
 			...this.legIK(
-				this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))
+				this.multiplyVector(
+					this.Ix,
+					this.multiplyVector(this.inverse(this.Trf), body_state.feet[1])
+				)
 			),
-			...this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])),
+			...this.legIK(this.multiplyVector(this.inverse(this.Tlb), body_state.feet[2])),
 			...this.legIK(
-				this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3]))
+				this.multiplyVector(
+					this.Ix,
+					this.multiplyVector(this.inverse(this.Trb), body_state.feet[3])
+				)
 			)
 		];
 	}
 
-	bodyIK(p: position_t) {
+	bodyIK(p: body_state_t) {
 		const cos_omega = cos(p.omega * this.DEGREES2RAD);
 		const sin_omega = sin(p.omega * this.DEGREES2RAD);
 		const cos_phi = cos(p.phi * this.DEGREES2RAD);
@@ -296,48 +317,103 @@ export default class Kinematic {
 	}
 }
 
-export class ForwardKinematics {
-	private l1: number;
-	private l2: number;
-	private l3: number;
-	private l4: number;
+const swing_time = 0.36;
+const overlap_time = 0.0;
+const dt = 0.02;
+const swing_ticks = Math.round(swing_time / dt);
+
+const num_phases = 4;
+const stance_ticks = 7 * swing_ticks;
+const overlap_ticks = Math.round(overlap_time / dt);
+
+const phase_ticks = new Array(4).fill(swing_ticks);
+const phase_length = num_phases * swing_ticks;
+
+let rb_contact_phases = [1, 0, 1, 1];
+let rf_contact_phases = [1, 1, 1, 0];
+let lf_contact_phases = [1, 0, 1, 1];
+let lb_contact_phases = [1, 1, 1, 0];
+
+export class GaitPlanner {
+	gaitCycleDuration = 10;
+	time = 0;
+	stepHeight = 30;
+	stepLength = 75;
+	num_phases = 4;
+	gaitCycle = 10;
+	phaseOffset = Math.PI;
+
+	ticks_ = 0;
+	phase_index_ = 0;
+	subphase_ticks_ = 0;
+	contact_feet_states_ = [false, false, false, false];
+	default_stance_feet_pos: number[][];
+
+	private phase: number;
+	private strideLength: number;
+	private height: number;
+	private cyclePeriod: number;
 
 	constructor() {
-		this.l1 = 50;
-		this.l2 = 20;
-		this.l3 = 120;
-		this.l4 = 155;
+		let l1 = 50;
+		let l2 = 20;
+		let l3 = 120;
+		let l4 = 155;
+
+		let L = 140;
+		let W = 75;
+
+		this.default_stance_feet_pos = [
+			[100, -100, 100, 1],
+			[100, -100, -100, 1],
+			[-W, -100, 100, 1],
+			[-W, -100, -100, 1]
+		];
+
+		this.strideLength = 2;
+		this.height = 50;
+		this.cyclePeriod = 10;
+		this.phase = 0;
 	}
 
-	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 step(bodyState: body_state_t, dt: number) {
+		this.updatePhase(dt);
+		this.updateFootPosition(bodyState);
+		this.UpdateBodyShift(bodyState);
 	}
 
-	public calculateFootpoints(angles: number[]): number[][] {
-		const footpoints: number[][] = [];
+	updatePhase(dt: number) {
+		this.time += dt;
 
-		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);
+		this.ticks_++;
+		let phase_time = this.ticks_ % phase_length;
+	}
+
+	updateFootPosition(body_state: body_state_t) {
+		for (let i = 0; i < 4; i++) {
+			let contact_mode = this.contact_feet_states_[i];
+
+			body_state.feet[i] = contact_mode
+				? this.stanceController(body_state.feet[i])
+				: this.swingLegController(body_state.feet[i], this.default_stance_feet_pos[i]);
 		}
+	}
 
-		return footpoints;
+	UpdateBodyShift(bodyState: body_state_t) {}
+
+	stanceController(foot_pos: number[]) {
+		foot_pos[0] = -100;
+		foot_pos[1] = sin(radToDeg(this.ticks_) / 2 + Math.PI / 2) * 100;
+		return foot_pos;
+	}
+
+	swingLegController(foot_pos: number[], default_stance_foot_pos: number[]) {
+		let swing_proportion = this.subphase_ticks_ / swing_ticks;
+		// foot_pos[0] = default_stance_foot_pos[0];
+		foot_pos[1] = default_stance_foot_pos[1] + 100;
+		// foot_pos[2] = default_stance_foot_pos[2];
+		// foot_pos[0] = cos(this.time / 2) * 50;
+		// foot_pos[1] = default_stance_foot_pos[1] - sin(this.time / 2 + Math.PI / 2) * 50;
+		return foot_pos;
 	}
 }
diff --git a/app/src/lib/sceneBuilder.ts b/app/src/lib/sceneBuilder.ts
index 7da392f..3dd8cf5 100644
--- a/app/src/lib/sceneBuilder.ts
+++ b/app/src/lib/sceneBuilder.ts
@@ -3,7 +3,6 @@ import {
 	PerspectiveCamera,
 	PlaneGeometry,
 	Scene,
-	ShadowMaterial,
 	WebGLRenderer,
 	AmbientLight,
 	DirectionalLight,
@@ -23,6 +22,7 @@ import {
 } from 'three';
 import { Sky } from 'three/addons/objects/Sky.js';
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
+import { TransformControls } from 'three/examples/jsm/controls/TransformControls';
 import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader';
 import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls';
 
@@ -64,19 +64,20 @@ function calculateCurrentSunElevation() {
 
 export default class SceneBuilder {
 	public scene: Scene;
-	public camera: PerspectiveCamera;
-	public ground: Mesh;
-	public renderer: WebGLRenderer;
-	public controls: OrbitControls;
-	public callback: Function;
-	public gridHelper: GridHelper;
-	public model: URDFRobot;
-	public liveStreamTexture: CanvasTexture;
-	private fog: FogExp2;
+	public camera!: PerspectiveCamera;
+	public ground!: Mesh;
+	public renderer!: WebGLRenderer;
+	public orbit: OrbitControls;
+	public callback: Function | undefined;
+	public gridHelper!: GridHelper;
+	public model!: URDFRobot;
+	public liveStreamTexture!: CanvasTexture;
+	private fog!: FogExp2;
 	private isLoaded: boolean = false;
 	public isDragging: boolean = false;
 	highlightMaterial: any;
-	sky: Sky;
+	sky!: Sky;
+	transformControl: TransformControls;
 
 	constructor() {
 		this.scene = new Scene();
@@ -144,11 +145,11 @@ export default class SceneBuilder {
 	};
 
 	public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
-		this.controls = new OrbitControls(this.camera, this.renderer.domElement);
-		this.controls.minDistance = minDistance;
-		this.controls.maxDistance = maxDistance;
-		this.controls.autoRotate = autoRotate;
-		this.controls.update();
+		this.orbit = new OrbitControls(this.camera, this.renderer.domElement);
+		this.orbit.minDistance = minDistance;
+		this.orbit.maxDistance = maxDistance;
+		this.orbit.autoRotate = autoRotate;
+		this.orbit.update();
 		return this;
 	};
 
@@ -213,7 +214,7 @@ export default class SceneBuilder {
 	public startRenderLoop = () => {
 		this.renderer.setAnimationLoop(() => {
 			this.renderer.render(this.scene, this.camera);
-			this.controls.update();
+			this.orbit.update();
 			this.handleRobotShadow();
 			if (this.callback) this.callback();
 			if (!this.liveStreamTexture) return;
@@ -274,6 +275,18 @@ export default class SceneBuilder {
 		traverse(m);
 	};
 
+	public addTransformControls = (model: any) => {
+		this.transformControl = new TransformControls(this.camera, this.renderer.domElement);
+		this.transformControl.addEventListener('dragging-changed', (event: any) => {
+			this.orbit.enabled = !event.value;
+			this.isDragging = !event.value;
+		});
+		this.transformControl.attach(model);
+		this.scene.add(this.transformControl);
+		this.transformControl.setMode('rotate');
+		return this;
+	};
+
 	public addModel = (model: any) => {
 		this.model = model;
 		this.scene.add(model);
@@ -299,11 +312,11 @@ export default class SceneBuilder {
 			updateAngle(joint.name, angle);
 		};
 		dragControls.onDragStart = () => {
-			this.controls.enabled = false;
+			this.orbit.enabled = false;
 			this.isDragging = true;
 		};
 		dragControls.onDragEnd = () => {
-			this.controls.enabled = true;
+			this.orbit.enabled = true;
 			this.isDragging = false;
 		};
 		dragControls.onHover = (joint: URDFMimicJoint) =>
diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts
index 74e90de..3427c07 100644
--- a/app/src/lib/stores/model-store.ts
+++ b/app/src/lib/stores/model-store.ts
@@ -19,7 +19,7 @@ export enum ModesEnum {
 	Walk
 }
 
-export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
+export const mode: Writable<ModesEnum> = writable(ModesEnum.Walk);
 
 export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]);