diff --git a/app/src/components/Topbar.svelte b/app/src/components/Topbar.svelte
index b8951c4..8d0bec9 100644
--- a/app/src/components/Topbar.svelte
+++ b/app/src/components/Topbar.svelte
@@ -2,76 +2,83 @@
import socketService from '$lib/services/socket-service';
import { Icon, Bars3, XMark, Power, Battery100, Signal, SignalSlash } from 'svelte-hero-icons';
import { emulateModel } from '$lib/store';
- import { Link, useLocation } from 'svelte-routing'
+ import { Link, useLocation } from 'svelte-routing';
- const views = ["Virtual environment", "Robot camera"]
- const modes = ["Drive", "Choreography"]
+ const views = ['Virtual environment', 'Robot camera'];
+ const modes = ['Drive', 'Choreography'];
- const location = useLocation()
+ const location = useLocation();
let selected_view = views[0];
let selected_modes = modes[0];
- let settingOpen = window.location.pathname.includes('/settings')
- let isConnected = socketService.isConnected
+ let settingOpen = window.location.pathname.includes('/settings');
+ let isConnected = socketService.isConnected;
- $: emulateModel.set(selected_view === views[0])
- $: settingOpen = $location.pathname.includes('/settings')
+ $: emulateModel.set(selected_view === views[0]);
+ $: settingOpen = $location.pathname.includes('/settings');
- const stop = () => {
- if ($isConnected) {
- socketService.send(JSON.stringify({type:"system/stop"}))
- }
- }
+ const stop = () => {
+ if ($isConnected) {
+ socketService.send(JSON.stringify({ type: 'system/stop' }));
+ }
+ };
-
\ No newline at end of file
+ .topbar {
+ height: 50px;
+ }
+ .action_button {
+ border-radius: 4px;
+ width: 34px;
+ height: 34px;
+ display: flex;
+ justify-content: center;
+ align-items: center;
+ outline: 1px solid #52525b;
+ }
+
diff --git a/app/src/components/Views/Model.svelte b/app/src/components/Views/Model.svelte
index 75c6479..fae33ed 100644
--- a/app/src/components/Views/Model.svelte
+++ b/app/src/components/Views/Model.svelte
@@ -1,149 +1,184 @@
-
-
- {/if}
+
+{/if}
-
\ No newline at end of file
+
diff --git a/app/src/components/settings/Calibration.svelte b/app/src/components/settings/Calibration.svelte
index d73ab6b..5976770 100644
--- a/app/src/components/settings/Calibration.svelte
+++ b/app/src/components/settings/Calibration.svelte
@@ -2,43 +2,43 @@
import { onMount } from 'svelte';
import { jointNames } from '../../lib/store';
- type Servo = {
- id: number;
- name: string;
- minPWM: number;
- maxPWM: number;
- pwmFor180: number;
- };
+ type Servo = {
+ id: number;
+ name: string;
+ minPWM: number;
+ maxPWM: number;
+ pwmFor180: number;
+ };
let servos: any[] = [];
- onMount(() => {
- jointNames.subscribe(data => {
- servos = data.map((name:string, i:number) => {
- return {
- id: i,
- name,
- minPWM: 0,
- maxPWM: 0,
- pwmFor180: 0
- };
- });
- })
- })
-
+ onMount(() => {
+ jointNames.subscribe((data) => {
+ servos = data.map((name: string, i: number) => {
+ return {
+ id: i,
+ name,
+ minPWM: 0,
+ maxPWM: 0,
+ pwmFor180: 0
+ };
+ });
+ });
+ });
let selectedServo: number | null = null;
- function updateServoValue(index: number, field: keyof Servo, value: number): void {
- servos[index] = { ...servos[index], [field]: value };
- }
+ function updateServoValue(index: number, field: keyof Servo, value: number): void {
+ servos[index] = { ...servos[index], [field]: value };
+ }
- const formatServo = (servo:Servo) => {
- const string = servo.name
- const name = string.charAt(0).toUpperCase() + string.split('_').join(' ').slice(1);
- return `${servo.id} ${name}`
- }
+ const formatServo = (servo: Servo) => {
+ const string = servo.name;
+ const name = string.charAt(0).toUpperCase() + string.split('_').join(' ').slice(1);
+ return `${servo.id} ${name}`;
+ };
+
diff --git a/app/src/components/settings/Configuration.svelte b/app/src/components/settings/Configuration.svelte
index 00bab52..be8290f 100644
--- a/app/src/components/settings/Configuration.svelte
+++ b/app/src/components/settings/Configuration.svelte
@@ -1,26 +1,25 @@
diff --git a/app/src/components/settings/Info.svelte b/app/src/components/settings/Info.svelte
index a333378..4cd7b14 100644
--- a/app/src/components/settings/Info.svelte
+++ b/app/src/components/settings/Info.svelte
@@ -1,31 +1,31 @@
+
@@ -49,23 +49,36 @@
-{#if selectedServo !== null}
-
-
+ {/if}
Servo {formatServo(servos[selectedServo])} Calibration
- - updateServoValue(selectedServo, 'minPWM', Number(event.target.value))} /> + {#if selectedServo !== null} +
+
-{/if}
+
+
+ updateServoValue(selectedServo, 'pwmFor180', Number(event.target.value))}
+ />
+ Servo {formatServo(servos[selectedServo])} Calibration
+ + updateServoValue(selectedServo, 'minPWM', Number(event.target.value))} + /> - - updateServoValue(selectedServo, 'maxPWM', Number(event.target.value))} /> + + updateServoValue(selectedServo, 'maxPWM', Number(event.target.value))} + /> - - updateServoValue(selectedServo, 'pwmFor180', Number(event.target.value))} /> -
-
\ No newline at end of file
+
- {#each Object.entries($settings) as entry}
-
-
-
- {/each}
- {entry[0]}:
- {entry[1]}
-
+ {#each Object.entries($settings) as entry}
+
+
+
+ {/each}
+ {entry[0]}:
+ {entry[1]}
+
-
\ No newline at end of file
+
- {#each Object.entries($systemInfo ?? {}) as entry}
-
-
-
- {/each}
- {entry[0]}:
- {#if entry[0].includes("Size") || entry[0].includes("Free") || entry[0].includes("Min")}
- {humanFileSize(entry[1])}
- {:else}
- {entry[1]}
- {/if}
-
+ {#each Object.entries($systemInfo ?? {}) as entry}
+
+
diff --git a/app/src/components/settings/Log.svelte b/app/src/components/settings/Log.svelte
index 41228e6..d3a59a3 100644
--- a/app/src/components/settings/Log.svelte
+++ b/app/src/components/settings/Log.svelte
@@ -1,21 +1,20 @@
+
+ {/each}
+ {entry[0]}:
+ {#if entry[0].includes('Size') || entry[0].includes('Free') || entry[0].includes('Min')}
+ {humanFileSize(entry[1])}
+ {:else}
+ {entry[1]}
+ {/if}
+
- {#each $log as entry}
-
\ No newline at end of file
+ {#each $log as entry}
+ {entry}
- {/each}
-{entry}
+ {/each}
+
diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts
index f1f444b..f239f9a 100644
--- a/app/src/lib/kinematic.ts
+++ b/app/src/lib/kinematic.ts
@@ -1,381 +1,393 @@
export default class Kinematic {
- private l1: number;
- private l2: number;
- private l3: number;
- private l4: number;
-
- private L: number;
- private W: number;
-
- constructor() {
- this.l1 = 50;
- this.l2 = 20;
- this.l3 = 120;
- this.l4 = 155;
-
- this.L = 140;
- this.W = 75;
- }
-
- bodyIK(omega: number, phi: number, psi: number, xm: number, ym: number, zm: number): number[][][] {
- const { cos, sin } = Math;
-
- const Rx: number[][] = [
- [1, 0, 0, 0],
- [0, cos(omega), -sin(omega), 0],
- [0, sin(omega), cos(omega), 0],
- [0, 0, 0, 1],
- ];
- const Ry: number[][] = [
- [cos(phi), 0, sin(phi), 0],
- [0, 1, 0, 0],
- [-sin(phi), 0, cos(phi), 0],
- [0, 0, 0, 1],
- ];
- const Rz: number[][] = [
- [cos(psi), -sin(psi), 0, 0],
- [sin(psi), cos(psi), 0, 0],
- [0, 0, 1, 0],
- [0, 0, 0, 1],
- ];
- const Rxyz: number[][] = this.matrixMultiply(this.matrixMultiply(Rx, Ry), Rz);
-
- const T: number[][] = [
- [0, 0, 0, xm],
- [0, 0, 0, ym],
- [0, 0, 0, zm],
- [0, 0, 0, 0],
- ];
- const Tm: number[][] = this.matrixAdd(T, Rxyz);
-
- const sHp = sin(Math.PI / 2);
- const cHp = cos(Math.PI / 2);
- const L = this.L;
- const W = this.W;
-
- return [
- this.matrixMultiply(Tm, [
- [cHp, 0, sHp, L / 2],
- [0, 1, 0, 0],
- [-sHp, 0, cHp, W / 2],
- [0, 0, 0, 1],
- ]),
- this.matrixMultiply(Tm, [
- [cHp, 0, sHp, L / 2],
- [0, 1, 0, 0],
- [-sHp, 0, cHp, -W / 2],
- [0, 0, 0, 1],
- ]),
- this.matrixMultiply(Tm, [
- [cHp, 0, sHp, -L / 2],
- [0, 1, 0, 0],
- [-sHp, 0, cHp, W / 2],
- [0, 0, 0, 1],
- ]),
- this.matrixMultiply(Tm, [
- [cHp, 0, sHp, -L / 2],
- [0, 1, 0, 0],
- [-sHp, 0, cHp, -W / 2],
- [0, 0, 0, 1],
- ]),
- ];
- }
-
- private legIK(point: number[]): number[] {
- const [x, y, z] = point;
- const { atan2, cos, sin, sqrt, acos } = Math;
- const { l1, l2, l3, l4 } = this;
-
- let F;
+ private l1: number;
+ private l2: number;
+ private l3: number;
+ private l4: number;
- try {
- F = sqrt(x ** 2 + y ** 2 - l1 ** 2);
- if(isNaN(F)) throw new Error("F is NaN")
- } catch (error) {
- //console.log(error)
- F = l1
- }
- const G = F - l2;
- const H = sqrt(G ** 2 + z ** 2);
+ private L: number;
+ private W: number;
- const theta1 = -atan2(y, x) - atan2(F, -l1);
- const D = (H ** 2 - l3 ** 2 - l4 ** 2) / (2 * l3 * l4);
- let theta3: number
- try {
- theta3 = acos(D);
- if(isNaN(theta3)) throw new Error("theta3 is NaN")
- } catch (error) {
- theta3 = 0
- }
- const theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3));
+ constructor() {
+ this.l1 = 50;
+ this.l2 = 20;
+ this.l3 = 120;
+ this.l4 = 155;
- return [theta1, theta2, theta3];
- }
-
- matrixMultiply(a: number[][], b: number[][]): number[][] {
- const result: number[][] = [];
-
- for (let i = 0; i < a.length; i++) {
- const row: number[] = [];
-
- for (let j = 0; j < b[0].length; j++) {
- let sum = 0;
-
- for (let k = 0; k < a[i].length; k++) {
- sum += a[i][k] * b[k][j];
- }
-
- row.push(sum);
- }
-
- result.push(row);
- }
-
- return result;
- }
+ this.L = 140;
+ this.W = 75;
+ }
- multiplyVector(matrix: number[][], vector: number[]): number[] {
- const rows = matrix.length;
- const cols = matrix[0].length;
- const vectorLength = vector.length;
-
- if (cols !== vectorLength) {
- throw new Error("Matrix and vector dimensions do not match for multiplication.");
- }
-
- const result = [];
-
- for (let i = 0; i < rows; i++) {
- let sum = 0;
-
- for (let j = 0; j < cols; j++) {
- sum += matrix[i][j] * vector[j];
- }
-
- result.push(sum);
- }
-
- return result;
- }
-
- private matrixAdd(a: number[][], b: number[][]): number[][] {
- const result: number[][] = [];
-
- for (let i = 0; i < a.length; i++) {
- const row: number[] = [];
-
- for (let j = 0; j < a[i].length; j++) {
- row.push(a[i][j] + b[i][j]);
- }
-
- result.push(row);
- }
-
- return result;
- }
-
- public calcLegPoints(angles: number[]): number[][] {
- const [theta1, theta2, theta3] = angles;
- const theta23 = theta2 + theta3;
-
- const T0: number[] = [0, 0, 0, 1];
- const T1: number[] = this.vectorAdd(
- T0,
- [-this.l1 * Math.cos(theta1), this.l1 * Math.sin(theta1), 0, 0]
- );
- const T2: number[] = this.vectorAdd(
- T1,
- [-this.l2 * Math.sin(theta1), -this.l2 * Math.cos(theta1), 0, 0]
- );
- const T3: number[] = this.vectorAdd(
- T2,
- [
- -this.l3 * Math.sin(theta1) * Math.cos(theta2),
- -this.l3 * Math.cos(theta1) * Math.cos(theta2),
- this.l3 * Math.sin(theta2),
- 0,
- ]
- );
- const T4: number[] = this.vectorAdd(
- T3,
- [
- -this.l4 * Math.sin(theta1) * Math.cos(theta23),
- -this.l4 * Math.cos(theta1) * Math.cos(theta23),
- this.l4 * Math.sin(theta23),
- 0,
- ]
- );
-
- return [T0, T1, T2, T3, T4];
- }
-
- public calcIK(Lp: number[][], angles: number[], center: number[]): number[][] {
- const [omega, phi, psi] = angles;
- const [xm, ym, zm] = center;
-
- const [Tlf, Trf, Tlb, Trb] = this.bodyIK(omega, phi, psi, xm, ym, zm);
-
- const Ix: number[][] = [
- [-1, 0, 0, 0],
- [0, 1, 0, 0],
- [0, 0, 1, 0],
- [0, 0, 0, 1],
- ];
+ bodyIK(
+ omega: number,
+ phi: number,
+ psi: number,
+ xm: number,
+ ym: number,
+ zm: number
+ ): number[][][] {
+ const { cos, sin } = Math;
- return [
- this.legIK(this.multiplyVector(this.matrixInverse(Tlf), Lp[0])),
- this.legIK(this.multiplyVector(Ix, this.multiplyVector(this.matrixInverse(Trf), Lp[1]))),
- this.legIK(this.multiplyVector(this.matrixInverse(Tlb), Lp[2])),
- this.legIK(this.multiplyVector(Ix, this.multiplyVector(this.matrixInverse(Trb), Lp[3]))),
- ];
- }
-
- private vectorAdd(a: number[], b: number[]): number[] {
- return a.map((val, index) => val + b[index]);
- }
-
- private matrixInverse(matrix: number[][]): number[][] {
- const det = this.determinant(matrix);
- const adjugate = this.adjugate(matrix);
- const scalar = 1 / det;
- const inverse: number[][] = [];
-
- for (let i = 0; i < matrix.length; i++) {
- const row: number[] = [];
-
- for (let j = 0; j < matrix[i].length; j++) {
- row.push(adjugate[i][j] * scalar);
- }
-
- inverse.push(row);
- }
-
- return inverse;
- }
-
- private determinant(matrix: number[][]): number {
- if (matrix.length !== matrix[0].length) {
- throw new Error("The matrix is not square.");
- }
-
- if (matrix.length === 2) {
- return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
- }
-
- let det = 0;
-
- for (let i = 0; i < matrix.length; i++) {
- const sign = i % 2 === 0 ? 1 : -1;
- const subMatrix: number[][] = [];
-
- for (let j = 1; j < matrix.length; j++) {
- const row: number[] = [];
-
- for (let k = 0; k < matrix.length; k++) {
- if (k !== i) {
- row.push(matrix[j][k]);
- }
- }
-
- subMatrix.push(row);
- }
-
- det += sign * matrix[0][i] * this.determinant(subMatrix);
- }
-
- return det;
- }
-
- private adjugate(matrix: number[][]): number[][] {
- if (matrix.length !== matrix[0].length) {
- throw new Error("The matrix is not square.");
- }
-
- const adjugate: number[][] = [];
-
- for (let i = 0; i < matrix.length; i++) {
- const row: number[] = [];
-
- for (let j = 0; j < matrix[i].length; j++) {
- const sign = (i + j) % 2 === 0 ? 1 : -1;
- const subMatrix: number[][] = [];
-
- for (let k = 0; k < matrix.length; k++) {
- if (k !== i) {
- const subRow: number[] = [];
-
- for (let l = 0; l < matrix.length; l++) {
- if (l !== j) {
- subRow.push(matrix[k][l]);
- }
- }
-
- subMatrix.push(subRow);
- }
- }
-
- const cofactor = sign * this.determinant(subMatrix);
- row.push(cofactor);
- }
-
- adjugate.push(row);
- }
-
- return this.transpose(adjugate);
- }
-
- private transpose(matrix: number[][]): number[][] {
- const transposed: number[][] = [];
-
- for (let i = 0; i < matrix.length; i++) {
- const row: number[] = [];
-
- for (let j = 0; j < matrix[i].length; j++) {
- row.push(matrix[j][i]);
- }
-
- transposed.push(row);
- }
-
- return transposed;
- }
- }
+ const Rx: number[][] = [
+ [1, 0, 0, 0],
+ [0, cos(omega), -sin(omega), 0],
+ [0, sin(omega), cos(omega), 0],
+ [0, 0, 0, 1]
+ ];
+ const Ry: number[][] = [
+ [cos(phi), 0, sin(phi), 0],
+ [0, 1, 0, 0],
+ [-sin(phi), 0, cos(phi), 0],
+ [0, 0, 0, 1]
+ ];
+ const Rz: number[][] = [
+ [cos(psi), -sin(psi), 0, 0],
+ [sin(psi), cos(psi), 0, 0],
+ [0, 0, 1, 0],
+ [0, 0, 0, 1]
+ ];
+ const Rxyz: number[][] = this.matrixMultiply(this.matrixMultiply(Rx, Ry), Rz);
+ const T: number[][] = [
+ [0, 0, 0, xm],
+ [0, 0, 0, ym],
+ [0, 0, 0, zm],
+ [0, 0, 0, 0]
+ ];
+ const Tm: number[][] = this.matrixAdd(T, Rxyz);
+
+ const sHp = sin(Math.PI / 2);
+ const cHp = cos(Math.PI / 2);
+ const L = this.L;
+ const W = this.W;
+
+ return [
+ this.matrixMultiply(Tm, [
+ [cHp, 0, sHp, L / 2],
+ [0, 1, 0, 0],
+ [-sHp, 0, cHp, W / 2],
+ [0, 0, 0, 1]
+ ]),
+ this.matrixMultiply(Tm, [
+ [cHp, 0, sHp, L / 2],
+ [0, 1, 0, 0],
+ [-sHp, 0, cHp, -W / 2],
+ [0, 0, 0, 1]
+ ]),
+ this.matrixMultiply(Tm, [
+ [cHp, 0, sHp, -L / 2],
+ [0, 1, 0, 0],
+ [-sHp, 0, cHp, W / 2],
+ [0, 0, 0, 1]
+ ]),
+ this.matrixMultiply(Tm, [
+ [cHp, 0, sHp, -L / 2],
+ [0, 1, 0, 0],
+ [-sHp, 0, cHp, -W / 2],
+ [0, 0, 0, 1]
+ ])
+ ];
+ }
+
+ private legIK(point: number[]): number[] {
+ const [x, y, z] = point;
+ const { atan2, cos, sin, sqrt, acos } = Math;
+ const { l1, l2, l3, l4 } = this;
+
+ let F;
+
+ try {
+ F = sqrt(x ** 2 + y ** 2 - l1 ** 2);
+ if (isNaN(F)) throw new Error('F is NaN');
+ } catch (error) {
+ //console.log(error)
+ F = l1;
+ }
+ const G = F - l2;
+ const H = sqrt(G ** 2 + z ** 2);
+
+ const theta1 = -atan2(y, x) - atan2(F, -l1);
+ const D = (H ** 2 - l3 ** 2 - l4 ** 2) / (2 * l3 * l4);
+ let theta3: number;
+ try {
+ theta3 = acos(D);
+ if (isNaN(theta3)) throw new Error('theta3 is NaN');
+ } catch (error) {
+ theta3 = 0;
+ }
+ const theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3));
+
+ return [theta1, theta2, theta3];
+ }
+
+ matrixMultiply(a: number[][], b: number[][]): number[][] {
+ const result: number[][] = [];
+
+ for (let i = 0; i < a.length; i++) {
+ const row: number[] = [];
+
+ for (let j = 0; j < b[0].length; j++) {
+ let sum = 0;
+
+ for (let k = 0; k < a[i].length; k++) {
+ sum += a[i][k] * b[k][j];
+ }
+
+ row.push(sum);
+ }
+
+ result.push(row);
+ }
+
+ return result;
+ }
+
+ multiplyVector(matrix: number[][], vector: number[]): number[] {
+ const rows = matrix.length;
+ const cols = matrix[0].length;
+ const vectorLength = vector.length;
+
+ if (cols !== vectorLength) {
+ throw new Error('Matrix and vector dimensions do not match for multiplication.');
+ }
+
+ const result = [];
+
+ for (let i = 0; i < rows; i++) {
+ let sum = 0;
+
+ for (let j = 0; j < cols; j++) {
+ sum += matrix[i][j] * vector[j];
+ }
+
+ result.push(sum);
+ }
+
+ return result;
+ }
+
+ private matrixAdd(a: number[][], b: number[][]): number[][] {
+ const result: number[][] = [];
+
+ for (let i = 0; i < a.length; i++) {
+ const row: number[] = [];
+
+ for (let j = 0; j < a[i].length; j++) {
+ row.push(a[i][j] + b[i][j]);
+ }
+
+ result.push(row);
+ }
+
+ return result;
+ }
+
+ public calcLegPoints(angles: number[]): number[][] {
+ const [theta1, theta2, theta3] = angles;
+ const theta23 = theta2 + theta3;
+
+ const T0: number[] = [0, 0, 0, 1];
+ const T1: number[] = this.vectorAdd(T0, [
+ -this.l1 * Math.cos(theta1),
+ this.l1 * Math.sin(theta1),
+ 0,
+ 0
+ ]);
+ const T2: number[] = this.vectorAdd(T1, [
+ -this.l2 * Math.sin(theta1),
+ -this.l2 * Math.cos(theta1),
+ 0,
+ 0
+ ]);
+ const T3: number[] = this.vectorAdd(T2, [
+ -this.l3 * Math.sin(theta1) * Math.cos(theta2),
+ -this.l3 * Math.cos(theta1) * Math.cos(theta2),
+ this.l3 * Math.sin(theta2),
+ 0
+ ]);
+ const T4: number[] = this.vectorAdd(T3, [
+ -this.l4 * Math.sin(theta1) * Math.cos(theta23),
+ -this.l4 * Math.cos(theta1) * Math.cos(theta23),
+ this.l4 * Math.sin(theta23),
+ 0
+ ]);
+
+ return [T0, T1, T2, T3, T4];
+ }
+
+ public calcIK(Lp: number[][], angles: number[], center: number[]): number[][] {
+ const [omega, phi, psi] = angles;
+ const [xm, ym, zm] = center;
+
+ const [Tlf, Trf, Tlb, Trb] = this.bodyIK(omega, phi, psi, xm, ym, zm);
+
+ const Ix: number[][] = [
+ [-1, 0, 0, 0],
+ [0, 1, 0, 0],
+ [0, 0, 1, 0],
+ [0, 0, 0, 1]
+ ];
+
+ return [
+ this.legIK(this.multiplyVector(this.matrixInverse(Tlf), Lp[0])),
+ this.legIK(this.multiplyVector(Ix, this.multiplyVector(this.matrixInverse(Trf), Lp[1]))),
+ this.legIK(this.multiplyVector(this.matrixInverse(Tlb), Lp[2])),
+ this.legIK(this.multiplyVector(Ix, this.multiplyVector(this.matrixInverse(Trb), Lp[3])))
+ ];
+ }
+
+ private vectorAdd(a: number[], b: number[]): number[] {
+ return a.map((val, index) => val + b[index]);
+ }
+
+ private matrixInverse(matrix: number[][]): number[][] {
+ const det = this.determinant(matrix);
+ const adjugate = this.adjugate(matrix);
+ const scalar = 1 / det;
+ const inverse: number[][] = [];
+
+ for (let i = 0; i < matrix.length; i++) {
+ const row: number[] = [];
+
+ for (let j = 0; j < matrix[i].length; j++) {
+ row.push(adjugate[i][j] * scalar);
+ }
+
+ inverse.push(row);
+ }
+
+ return inverse;
+ }
+
+ private determinant(matrix: number[][]): number {
+ if (matrix.length !== matrix[0].length) {
+ throw new Error('The matrix is not square.');
+ }
+
+ if (matrix.length === 2) {
+ return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
+ }
+
+ let det = 0;
+
+ for (let i = 0; i < matrix.length; i++) {
+ const sign = i % 2 === 0 ? 1 : -1;
+ const subMatrix: number[][] = [];
+
+ for (let j = 1; j < matrix.length; j++) {
+ const row: number[] = [];
+
+ for (let k = 0; k < matrix.length; k++) {
+ if (k !== i) {
+ row.push(matrix[j][k]);
+ }
+ }
+
+ subMatrix.push(row);
+ }
+
+ det += sign * matrix[0][i] * this.determinant(subMatrix);
+ }
+
+ return det;
+ }
+
+ private adjugate(matrix: number[][]): number[][] {
+ if (matrix.length !== matrix[0].length) {
+ throw new Error('The matrix is not square.');
+ }
+
+ const adjugate: number[][] = [];
+
+ for (let i = 0; i < matrix.length; i++) {
+ const row: number[] = [];
+
+ for (let j = 0; j < matrix[i].length; j++) {
+ const sign = (i + j) % 2 === 0 ? 1 : -1;
+ const subMatrix: number[][] = [];
+
+ for (let k = 0; k < matrix.length; k++) {
+ if (k !== i) {
+ const subRow: number[] = [];
+
+ for (let l = 0; l < matrix.length; l++) {
+ if (l !== j) {
+ subRow.push(matrix[k][l]);
+ }
+ }
+
+ subMatrix.push(subRow);
+ }
+ }
+
+ const cofactor = sign * this.determinant(subMatrix);
+ row.push(cofactor);
+ }
+
+ adjugate.push(row);
+ }
+
+ return this.transpose(adjugate);
+ }
+
+ private transpose(matrix: number[][]): number[][] {
+ const transposed: number[][] = [];
+
+ for (let i = 0; i < matrix.length; i++) {
+ const row: number[] = [];
+
+ for (let j = 0; j < matrix[i].length; j++) {
+ row.push(matrix[j][i]);
+ }
+
+ transposed.push(row);
+ }
+
+ 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];
- }
+ private l1: number;
+ private l2: number;
+ private l3: number;
+ private l4: number;
- 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
+ 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;
+ }
+}
diff --git a/app/src/lib/sceneBuilder.ts b/app/src/lib/sceneBuilder.ts
index 51c64fe..a1ffce8 100644
--- a/app/src/lib/sceneBuilder.ts
+++ b/app/src/lib/sceneBuilder.ts
@@ -1,293 +1,318 @@
-import { Mesh,
- PerspectiveCamera,
- PlaneGeometry,
- Scene,
- ShadowMaterial,
- WebGLRenderer,
- AmbientLight,
- DirectionalLight,
- PCFSoftShadowMap,
- GridHelper,
- ArrowHelper,
- Vector3,
- LoaderUtils,
- Object3D,
- FogExp2,
- CanvasTexture,
- type ColorRepresentation,
- type WebGLRendererParameters,
- MeshPhongMaterial,
- EquirectangularReflectionMapping,
- ACESFilmicToneMapping,
- MathUtils,
-} from "three";
+import {
+ Mesh,
+ PerspectiveCamera,
+ PlaneGeometry,
+ Scene,
+ ShadowMaterial,
+ WebGLRenderer,
+ AmbientLight,
+ DirectionalLight,
+ PCFSoftShadowMap,
+ GridHelper,
+ ArrowHelper,
+ Vector3,
+ LoaderUtils,
+ Object3D,
+ FogExp2,
+ CanvasTexture,
+ type ColorRepresentation,
+ type WebGLRendererParameters,
+ MeshPhongMaterial,
+ EquirectangularReflectionMapping,
+ ACESFilmicToneMapping,
+ MathUtils
+} from 'three';
import { Sky } from 'three/addons/objects/Sky.js';
-import { OrbitControls } from "three/examples/jsm/controls/OrbitControls";
-import { type URDFMimicJoint } from "urdf-loader";
-import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls'
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
+import { type URDFMimicJoint } from 'urdf-loader';
+import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls';
-export const addScene = () => new Scene()
+export const addScene = () => new Scene();
interface position {
- x?: number,
- y?: number,
- z?: number
+ x?: number;
+ y?: number;
+ z?: number;
}
interface light {
- color?: ColorRepresentation,
- intensity?: number
+ color?: ColorRepresentation;
+ intensity?: number;
}
interface gridOptions {
- divisions?: number,
- size?: number,
+ divisions?: number;
+ size?: number;
}
interface arrowOptions {
- origin:position,
- direction:position,
- length?:number,
- color?:ColorRepresentation
+ origin: position;
+ direction: position;
+ length?: number;
+ color?: ColorRepresentation;
}
-type directionalLight = position & light
+type directionalLight = position & light;
-type gridHelperOptions = gridOptions & position
+type gridHelperOptions = gridOptions & position;
function calculateCurrentSunElevation() {
- let now = new Date();
- let decimalTime = now.getHours() + now.getMinutes() / 60;
- let normalizedTime = ((decimalTime - 6) % 12) / 6 - 1;
- return 10 * Math.sin(normalizedTime * Math.PI);
+ let now = new Date();
+ let decimalTime = now.getHours() + now.getMinutes() / 60;
+ let normalizedTime = (decimalTime % 12) / 6 - 1;
+ return 10 * Math.sin(normalizedTime * Math.PI);
}
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: Object3D
- {#if $emulateModel}
-
diff --git a/app/src/routes/Settings.svelte b/app/src/routes/Settings.svelte
index d827f93..c9b5b89 100644
--- a/app/src/routes/Settings.svelte
+++ b/app/src/routes/Settings.svelte
@@ -3,54 +3,63 @@
import Info from '../components/settings/Info.svelte';
import Log from '../components/settings/Log.svelte';
import Configuration from '../components/settings/Configuration.svelte';
- import { Icon, Wifi, CommandLine, InformationCircle, BookOpen, AdjustmentsVertical, Cog6Tooth, Newspaper } from 'svelte-hero-icons';
+ import {
+ Icon,
+ Wifi,
+ CommandLine,
+ InformationCircle,
+ BookOpen,
+ AdjustmentsVertical,
+ Cog6Tooth,
+ Newspaper
+ } from 'svelte-hero-icons';
import Calibration from '../components/settings/Calibration.svelte';
- export const page = ""
+ export const page = '';
const menu = [
{
title: 'Calibration',
path: '/calibration',
- icon: AdjustmentsVertical,
+ icon: AdjustmentsVertical,
component: Calibration
},
{
- title: 'System info',
+ title: 'System info',
path: '/info',
- icon: InformationCircle,
+ icon: InformationCircle,
component: Info
},
- {
- title: 'Log',
+ {
+ title: 'Log',
path: '/log',
- icon: BookOpen,
+ icon: BookOpen,
component: Log
},
- {
- title: 'Settings',
- path: '/settings',
- icon: Cog6Tooth,
- component: Configuration
- },
+ {
+ title: 'Settings',
+ path: '/settings',
+ icon: Cog6Tooth,
+ component: Configuration
+ }
];