🛸 Adds tranformcontroller for body

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
+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)
};
+118 -42
View File
@@ -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;
}
}
+32 -19
View File
@@ -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) =>
+1 -1
View File
@@ -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]);