Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🛸 Adds tranformcontroller for body

Browse Source
This commit is contained in:
Rune Harlyk
2024-05-27 01:24:43 +02:00
committed by Rune Harlyk
parent 379091433c
commit 1b2d6a9850
4 changed files with 257 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+106 -35
View File
@@ -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)
};