💅 Updates the frontpage
This commit is contained in:
Rune Harlyk
@@ -1,345 +1,340 @@
|
||||
<script lang="ts">
|
||||
import { onDestroy, onMount } from 'svelte';
|
||||
import {
|
||||
BufferGeometry,
|
||||
Line,
|
||||
LineBasicMaterial,
|
||||
Mesh,
|
||||
MeshBasicMaterial,
|
||||
Object3D,
|
||||
SphereGeometry,
|
||||
Vector3,
|
||||
type NormalBufferAttributes,
|
||||
type Object3DEventMap
|
||||
} from 'three';
|
||||
import {
|
||||
ModesEnum,
|
||||
kinematicData,
|
||||
mode,
|
||||
model,
|
||||
outControllerData,
|
||||
servoAnglesOut,
|
||||
servoAngles,
|
||||
mpu,
|
||||
jointNames
|
||||
} from '$lib/stores';
|
||||
import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities';
|
||||
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 body_state_t } from '$lib/kinematic';
|
||||
import {
|
||||
BezierState,
|
||||
CalibrationState,
|
||||
EightPhaseWalkState,
|
||||
FourPhaseWalkState,
|
||||
IdleState,
|
||||
RestState,
|
||||
StandState
|
||||
} from '$lib/gait';
|
||||
import { radToDeg } from 'three/src/math/MathUtils.js';
|
||||
import type { URDFRobot } from 'urdf-loader';
|
||||
import { get } from 'svelte/store';
|
||||
import { onDestroy, onMount } from 'svelte'
|
||||
import {
|
||||
BufferGeometry,
|
||||
Line,
|
||||
LineBasicMaterial,
|
||||
Mesh,
|
||||
MeshBasicMaterial,
|
||||
Object3D,
|
||||
SphereGeometry,
|
||||
Vector3,
|
||||
type NormalBufferAttributes,
|
||||
type Object3DEventMap
|
||||
} from 'three'
|
||||
import {
|
||||
ModesEnum,
|
||||
kinematicData,
|
||||
mode,
|
||||
model,
|
||||
outControllerData,
|
||||
servoAnglesOut,
|
||||
servoAngles,
|
||||
mpu,
|
||||
jointNames
|
||||
} from '$lib/stores'
|
||||
import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities'
|
||||
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 body_state_t } from '$lib/kinematic'
|
||||
import {
|
||||
BezierState,
|
||||
CalibrationState,
|
||||
EightPhaseWalkState,
|
||||
FourPhaseWalkState,
|
||||
IdleState,
|
||||
RestState,
|
||||
StandState
|
||||
} from '$lib/gait'
|
||||
import { radToDeg } from 'three/src/math/MathUtils.js'
|
||||
import type { URDFRobot } from 'urdf-loader'
|
||||
import { get } from 'svelte/store'
|
||||
|
||||
interface Props {
|
||||
sky?: boolean;
|
||||
orbit?: boolean;
|
||||
panel?: boolean;
|
||||
debug?: boolean;
|
||||
ground?: boolean;
|
||||
interface Props {
|
||||
sky?: boolean
|
||||
orbit?: boolean
|
||||
panel?: boolean
|
||||
debug?: boolean
|
||||
ground?: boolean
|
||||
zoom?: number
|
||||
}
|
||||
|
||||
let {
|
||||
sky = true,
|
||||
orbit = false,
|
||||
panel = true,
|
||||
debug = false,
|
||||
ground = true,
|
||||
zoom = 8
|
||||
}: Props = $props()
|
||||
|
||||
let sceneManager = $state(new SceneBuilder())
|
||||
let canvas: HTMLCanvasElement = $state()
|
||||
|
||||
let currentModelAngles: number[] = new Array(12).fill(0)
|
||||
let modelTargetAngles: number[] = new Array(12).fill(0)
|
||||
let gui_panel: GUI
|
||||
let Throttler = new throttler()
|
||||
|
||||
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 planners = {
|
||||
[ModesEnum.Deactivated]: new IdleState(),
|
||||
[ModesEnum.Idle]: new IdleState(),
|
||||
[ModesEnum.Calibration]: new CalibrationState(),
|
||||
[ModesEnum.Rest]: new RestState(),
|
||||
[ModesEnum.Stand]: new StandState(),
|
||||
[ModesEnum.Crawl]: new EightPhaseWalkState(),
|
||||
[ModesEnum.Walk]: new BezierState()
|
||||
}
|
||||
let lastTick = performance.now()
|
||||
|
||||
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
|
||||
|
||||
let body_state = {
|
||||
omega: 0,
|
||||
phi: 0,
|
||||
psi: 0,
|
||||
xm: 0,
|
||||
ym: 0.5,
|
||||
zm: 0,
|
||||
feet: planners[ModesEnum.Idle].default_feet_pos
|
||||
}
|
||||
|
||||
let settings = {
|
||||
'Internal kinematic': true,
|
||||
'Robot transform controls': false,
|
||||
'Auto orient robot': true,
|
||||
'Trace feet': debug,
|
||||
'Target position': false,
|
||||
'Trace points': 30,
|
||||
'Fix camera on robot': true,
|
||||
'Smooth motion': true,
|
||||
omega: 0,
|
||||
phi: 0,
|
||||
psi: 0,
|
||||
xm: 0,
|
||||
ym: 0.7,
|
||||
zm: 0,
|
||||
Background: 'black'
|
||||
}
|
||||
|
||||
onMount(async () => {
|
||||
await populateModelCache()
|
||||
await createScene()
|
||||
servoAngles.subscribe(updateAnglesFromStore)
|
||||
if (panel) createPanel()
|
||||
})
|
||||
|
||||
onDestroy(() => {
|
||||
canvas.remove()
|
||||
gui_panel?.destroy()
|
||||
})
|
||||
|
||||
const updateAnglesFromStore = (angles: number[]) => {
|
||||
if (sceneManager.isDragging) return
|
||||
if (settings['Internal kinematic']) return
|
||||
modelTargetAngles = angles
|
||||
}
|
||||
|
||||
const createPanel = () => {
|
||||
gui_panel = new GUI({ width: 310 })
|
||||
gui_panel.close()
|
||||
gui_panel.domElement.id = 'three-gui-panel'
|
||||
|
||||
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).listen()
|
||||
kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen()
|
||||
kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen()
|
||||
kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen()
|
||||
kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen()
|
||||
kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen()
|
||||
|
||||
const visibility = gui_panel.addFolder('Visualization')
|
||||
visibility.add(settings, 'Trace feet')
|
||||
visibility.add(settings, 'Trace points', 1, 1000, 1)
|
||||
visibility.add(settings, 'Target position')
|
||||
visibility.add(settings, 'Smooth motion')
|
||||
visibility.addColor(settings, 'Background')
|
||||
}
|
||||
|
||||
const updateKinematicPosition = () => {
|
||||
kinematicData.set([
|
||||
settings.omega,
|
||||
settings.phi,
|
||||
settings.psi,
|
||||
settings.xm,
|
||||
settings.ym,
|
||||
settings.zm
|
||||
])
|
||||
}
|
||||
|
||||
const updateAngles = (name: string, angle: number) => {
|
||||
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
|
||||
Throttler.throttle(() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))), 100)
|
||||
}
|
||||
|
||||
const createScene = async () => {
|
||||
sceneManager
|
||||
.addRenderer({ antialias: true, canvas, alpha: true })
|
||||
.addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
|
||||
.addOrbitControls(Math.min(zoom, 8), 30, orbit)
|
||||
.addDirectionalLight({ x: 10, y: 20, z: 10, color: 0xffffff, intensity: 3 })
|
||||
.addAmbientLight({ color: 0xffffff, intensity: 0.5 })
|
||||
.addFogExp2(0xcccccc, 0.015)
|
||||
.addModel($model)
|
||||
.addTransformControls(sceneManager.model)
|
||||
.fillParent()
|
||||
.addRenderCb(render)
|
||||
.startRenderLoop()
|
||||
|
||||
if (ground) sceneManager.addGroundPlane()
|
||||
|
||||
const geometry = new SphereGeometry(0.1, 32, 16)
|
||||
const material = new MeshBasicMaterial({ color: 0xffff00 })
|
||||
target = new Mesh(geometry, material)
|
||||
sceneManager.scene.add(target)
|
||||
|
||||
if (debug) {
|
||||
sceneManager.addDragControl(updateAngles)
|
||||
}
|
||||
if (sky) sceneManager.addSky()
|
||||
|
||||
for (let i = 0; i < 4; i++) {
|
||||
const geometry = new BufferGeometry()
|
||||
const material = new LineBasicMaterial({ color: footColor() })
|
||||
const line = new Line(geometry, material)
|
||||
trace_lines.push(geometry)
|
||||
sceneManager.scene.add(line)
|
||||
}
|
||||
}
|
||||
|
||||
const renderTraceLines = (foot_positions: Vector3[]) => {
|
||||
if (!settings['Trace feet']) {
|
||||
if (!feet_trace.length) return
|
||||
trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1)))
|
||||
feet_trace = new Array(4).fill([])
|
||||
return
|
||||
}
|
||||
|
||||
let {
|
||||
sky = true,
|
||||
orbit = false,
|
||||
panel = true,
|
||||
debug = false,
|
||||
ground = true
|
||||
}: Props = $props();
|
||||
trace_lines.forEach((line, i) => {
|
||||
feet_trace[i].push(foot_positions[i])
|
||||
feet_trace[i] = feet_trace[i].slice(-settings['Trace points'])
|
||||
line.setFromPoints(feet_trace[i])
|
||||
})
|
||||
}
|
||||
|
||||
let sceneManager = $state(new SceneBuilder());
|
||||
let canvas: HTMLCanvasElement = $state();
|
||||
const calculate_kinematics = () => {
|
||||
if (sceneManager.isDragging || !settings['Internal kinematic']) return
|
||||
const position: body_state_t = {
|
||||
omega: settings.omega,
|
||||
phi: settings.phi,
|
||||
psi: settings.psi,
|
||||
xm: settings.xm,
|
||||
ym: settings.ym,
|
||||
zm: settings.zm,
|
||||
feet: body_state.feet
|
||||
}
|
||||
|
||||
let currentModelAngles: number[] = new Array(12).fill(0);
|
||||
let modelTargetAngles: number[] = new Array(12).fill(0);
|
||||
let gui_panel: GUI;
|
||||
let Throttler = new throttler();
|
||||
let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]))
|
||||
modelTargetAngles = new_angles
|
||||
}
|
||||
|
||||
let feet_trace = new Array(4).fill([]);
|
||||
let trace_lines: BufferGeometry<NormalBufferAttributes>[] = [];
|
||||
let target: Object3D<Object3DEventMap>;
|
||||
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))
|
||||
|
||||
let target_position = { x: 0, z: 0, yaw: 0 };
|
||||
robot.position.z = smooth(robot.position.z, -settings.xm, 0.1)
|
||||
robot.position.x = smooth(robot.position.x, -settings.zm, 0.1)
|
||||
|
||||
let kinematic = new Kinematic();
|
||||
robot.rotation.z = smooth(robot.rotation.z, degToRad(-settings.phi + $mpu.heading + 90), 0.1)
|
||||
robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1)
|
||||
robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1)
|
||||
}
|
||||
|
||||
let planners = {
|
||||
[ModesEnum.Deactivated]: new IdleState(),
|
||||
[ModesEnum.Idle]: new IdleState(),
|
||||
[ModesEnum.Calibration]: new CalibrationState(),
|
||||
[ModesEnum.Rest]: new RestState(),
|
||||
[ModesEnum.Stand]: new StandState(),
|
||||
[ModesEnum.Crawl]: new EightPhaseWalkState(),
|
||||
[ModesEnum.Walk]: new BezierState()
|
||||
};
|
||||
let lastTick = performance.now();
|
||||
const update_camera = (robot: URDFRobot) => {
|
||||
if (!settings['Fix camera on robot']) return
|
||||
sceneManager.orbit.target = robot.position.clone()
|
||||
}
|
||||
|
||||
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1];
|
||||
const smooth = (start: number, end: number, amount: number) => {
|
||||
return settings['Smooth motion'] ? lerp(start, end, amount) : end
|
||||
}
|
||||
|
||||
let body_state = {
|
||||
omega: 0,
|
||||
phi: 0,
|
||||
psi: 0,
|
||||
xm: 0,
|
||||
ym: 0.5,
|
||||
zm: 0,
|
||||
feet: planners[ModesEnum.Idle].default_feet_pos
|
||||
};
|
||||
const update_gait = () => {
|
||||
if (sceneManager.isDragging || !settings['Internal kinematic']) return
|
||||
const controlData = get(outControllerData)
|
||||
const data = {
|
||||
stop: controlData[0],
|
||||
lx: controlData[1],
|
||||
ly: controlData[2],
|
||||
rx: controlData[3],
|
||||
ry: controlData[4],
|
||||
h: controlData[5],
|
||||
s: controlData[6],
|
||||
s1: controlData[7]
|
||||
}
|
||||
body_state.ym = ((data.h + 127) * 0.75) / 100
|
||||
|
||||
let settings = {
|
||||
'Internal kinematic': true,
|
||||
'Robot transform controls': false,
|
||||
'Auto orient robot': true,
|
||||
'Trace feet': debug,
|
||||
'Target position': false,
|
||||
'Trace points': 30,
|
||||
'Fix camera on robot': true,
|
||||
'Smooth motion': true,
|
||||
omega: 0,
|
||||
phi: 0,
|
||||
psi: 0,
|
||||
xm: 0,
|
||||
ym: 0.7,
|
||||
zm: 0,
|
||||
Background: 'black'
|
||||
};
|
||||
let planner = planners[get(mode)]
|
||||
const delta = performance.now() - lastTick
|
||||
lastTick = performance.now()
|
||||
|
||||
onMount(async () => {
|
||||
await populateModelCache();
|
||||
await createScene();
|
||||
servoAngles.subscribe(updateAnglesFromStore);
|
||||
if (panel) createPanel();
|
||||
});
|
||||
body_state = planner.step(body_state, data, delta)
|
||||
|
||||
onDestroy(() => {
|
||||
canvas.remove();
|
||||
gui_panel?.destroy();
|
||||
});
|
||||
settings.omega = body_state.omega
|
||||
settings.phi = body_state.phi
|
||||
settings.psi = body_state.psi
|
||||
settings.xm = body_state.xm
|
||||
settings.ym = body_state.ym
|
||||
settings.zm = body_state.zm
|
||||
}
|
||||
|
||||
const updateAnglesFromStore = (angles: number[]) => {
|
||||
if (sceneManager.isDragging) return;
|
||||
if (settings['Internal kinematic']) return;
|
||||
modelTargetAngles = angles;
|
||||
};
|
||||
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.xm = robot.position.z * 100
|
||||
settings.zm = -robot.position.x * 100
|
||||
}
|
||||
|
||||
const createPanel = () => {
|
||||
gui_panel = new GUI({ width: 310 });
|
||||
gui_panel.close();
|
||||
gui_panel.domElement.id = 'three-gui-panel';
|
||||
const updateTargetPosition = () => {
|
||||
target.visible = settings['Target position']
|
||||
target.position.x = smooth(target.position.x, target_position.x, 0.5)
|
||||
target.position.z = smooth(target.position.z, target_position.z, 0.5)
|
||||
}
|
||||
|
||||
const general = gui_panel.addFolder('General');
|
||||
general.add(settings, 'Internal kinematic');
|
||||
general.add(settings, 'Robot transform controls');
|
||||
general.add(settings, 'Auto orient robot');
|
||||
const render = () => {
|
||||
const robot = sceneManager.model
|
||||
if (!robot) return
|
||||
|
||||
const kinematic = gui_panel.addFolder('Kinematics');
|
||||
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, 'xm', -1, 1).onChange(updateKinematicPosition).listen();
|
||||
kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen();
|
||||
kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen();
|
||||
const toes = toeWorldPositions(robot)
|
||||
|
||||
const visibility = gui_panel.addFolder('Visualization');
|
||||
visibility.add(settings, 'Trace feet');
|
||||
visibility.add(settings, 'Trace points', 1, 1000, 1);
|
||||
visibility.add(settings, 'Target position');
|
||||
visibility.add(settings, 'Smooth motion');
|
||||
visibility.addColor(settings, 'Background');
|
||||
};
|
||||
renderTraceLines(toes)
|
||||
update_camera(robot)
|
||||
update_gait()
|
||||
calculate_kinematics()
|
||||
update_robot_position(robot)
|
||||
|
||||
const updateKinematicPosition = () => {
|
||||
kinematicData.set([
|
||||
settings.omega,
|
||||
settings.phi,
|
||||
settings.psi,
|
||||
settings.xm,
|
||||
settings.ym,
|
||||
settings.zm
|
||||
]);
|
||||
};
|
||||
sceneManager.transformControl.showX = settings['Robot transform controls']
|
||||
sceneManager.transformControl.showY = settings['Robot transform controls']
|
||||
sceneManager.transformControl.showZ = settings['Robot transform controls']
|
||||
|
||||
const updateAngles = (name: string, angle: number) => {
|
||||
modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
|
||||
Throttler.throttle(
|
||||
() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))),
|
||||
100
|
||||
);
|
||||
};
|
||||
for (let i = 0; i < $jointNames.length; i++) {
|
||||
currentModelAngles[i] = smooth(
|
||||
(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
|
||||
modelTargetAngles[i],
|
||||
0.1
|
||||
)
|
||||
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]))
|
||||
}
|
||||
|
||||
const createScene = async () => {
|
||||
sceneManager
|
||||
.addRenderer({ antialias: true, canvas, alpha: true })
|
||||
.addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
|
||||
.addOrbitControls(8, 30, orbit)
|
||||
.addDirectionalLight({ x: 10, y: 20, z: 10, color: 0xffffff, intensity: 3 })
|
||||
.addAmbientLight({ color: 0xffffff, intensity: 0.5 })
|
||||
.addFogExp2(0xcccccc, 0.015)
|
||||
.addModel($model)
|
||||
.addTransformControls(sceneManager.model)
|
||||
.fillParent()
|
||||
.addRenderCb(render)
|
||||
.startRenderLoop();
|
||||
|
||||
if (ground) sceneManager.addGroundPlane();
|
||||
|
||||
const geometry = new SphereGeometry(0.1, 32, 16);
|
||||
const material = new MeshBasicMaterial({ color: 0xffff00 });
|
||||
target = new Mesh(geometry, material);
|
||||
sceneManager.scene.add(target);
|
||||
|
||||
if (debug) {
|
||||
sceneManager.addDragControl(updateAngles);
|
||||
}
|
||||
if (sky) sceneManager.addSky();
|
||||
|
||||
for (let i = 0; i < 4; i++) {
|
||||
const geometry = new BufferGeometry();
|
||||
const material = new LineBasicMaterial({ color: footColor() });
|
||||
const line = new Line(geometry, material);
|
||||
trace_lines.push(geometry);
|
||||
sceneManager.scene.add(line);
|
||||
}
|
||||
};
|
||||
|
||||
const renderTraceLines = (foot_positions: Vector3[]) => {
|
||||
if (!settings['Trace feet']) {
|
||||
if (!feet_trace.length) return;
|
||||
trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1)));
|
||||
feet_trace = new Array(4).fill([]);
|
||||
return;
|
||||
}
|
||||
|
||||
trace_lines.forEach((line, i) => {
|
||||
feet_trace[i].push(foot_positions[i]);
|
||||
feet_trace[i] = feet_trace[i].slice(-settings['Trace points']);
|
||||
line.setFromPoints(feet_trace[i]);
|
||||
});
|
||||
};
|
||||
|
||||
const calculate_kinematics = () => {
|
||||
if (sceneManager.isDragging || !settings['Internal kinematic']) return;
|
||||
const position: body_state_t = {
|
||||
omega: settings.omega,
|
||||
phi: settings.phi,
|
||||
psi: settings.psi,
|
||||
xm: settings.xm,
|
||||
ym: settings.ym,
|
||||
zm: settings.zm,
|
||||
feet: body_state.feet
|
||||
};
|
||||
|
||||
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 = smooth(robot.position.z, -settings.xm, 0.1);
|
||||
robot.position.x = smooth(robot.position.x, -settings.zm, 0.1);
|
||||
|
||||
robot.rotation.z = smooth(
|
||||
robot.rotation.z,
|
||||
degToRad(-settings.phi + $mpu.heading + 90),
|
||||
0.1
|
||||
);
|
||||
robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1);
|
||||
robot.rotation.x = smooth(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 smooth = (start: number, end: number, amount: number) => {
|
||||
return settings['Smooth motion'] ? lerp(start, end, amount) : end;
|
||||
};
|
||||
|
||||
const update_gait = () => {
|
||||
if (sceneManager.isDragging || !settings['Internal kinematic']) return;
|
||||
const controlData = get(outControllerData);
|
||||
const data = {
|
||||
stop: controlData[0],
|
||||
lx: controlData[1],
|
||||
ly: controlData[2],
|
||||
rx: controlData[3],
|
||||
ry: controlData[4],
|
||||
h: controlData[5],
|
||||
s: controlData[6],
|
||||
s1: controlData[7]
|
||||
};
|
||||
body_state.ym = ((data.h + 127) * 0.75) / 100;
|
||||
|
||||
let planner = planners[get(mode)];
|
||||
const delta = performance.now() - lastTick;
|
||||
lastTick = performance.now();
|
||||
|
||||
body_state = planner.step(body_state, data, delta);
|
||||
|
||||
settings.omega = body_state.omega;
|
||||
settings.phi = body_state.phi;
|
||||
settings.psi = body_state.psi;
|
||||
settings.xm = body_state.xm;
|
||||
settings.ym = body_state.ym;
|
||||
settings.zm = body_state.zm;
|
||||
};
|
||||
|
||||
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.xm = robot.position.z * 100;
|
||||
settings.zm = -robot.position.x * 100;
|
||||
};
|
||||
|
||||
const updateTargetPosition = () => {
|
||||
target.visible = settings['Target position'];
|
||||
target.position.x = smooth(target.position.x, target_position.x, 0.5);
|
||||
target.position.z = smooth(target.position.z, target_position.z, 0.5);
|
||||
};
|
||||
|
||||
const render = () => {
|
||||
const robot = sceneManager.model;
|
||||
if (!robot) return;
|
||||
|
||||
const toes = toeWorldPositions(robot);
|
||||
|
||||
renderTraceLines(toes);
|
||||
update_camera(robot);
|
||||
update_gait();
|
||||
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] = smooth(
|
||||
(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
|
||||
modelTargetAngles[i],
|
||||
0.1
|
||||
);
|
||||
robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
|
||||
}
|
||||
|
||||
orient_robot(robot, toes);
|
||||
updateTargetPosition();
|
||||
};
|
||||
orient_robot(robot, toes)
|
||||
updateTargetPosition()
|
||||
}
|
||||
</script>
|
||||
|
||||
<svelte:window onresize={sceneManager.fillParent} />
|
||||
|
||||
+348
-348
@@ -1,379 +1,379 @@
|
||||
import {
|
||||
Mesh,
|
||||
PerspectiveCamera,
|
||||
PlaneGeometry,
|
||||
Scene,
|
||||
WebGLRenderer,
|
||||
AmbientLight,
|
||||
DirectionalLight,
|
||||
PCFSoftShadowMap,
|
||||
type GridHelper,
|
||||
ArrowHelper,
|
||||
Vector3,
|
||||
FogExp2,
|
||||
CanvasTexture,
|
||||
type ColorRepresentation,
|
||||
type WebGLRendererParameters,
|
||||
MeshPhongMaterial,
|
||||
EquirectangularReflectionMapping,
|
||||
ACESFilmicToneMapping,
|
||||
MathUtils,
|
||||
Group,
|
||||
MeshBasicMaterial,
|
||||
RepeatWrapping
|
||||
} 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 { Reflector } from 'three/examples/jsm/objects/Reflector.js';
|
||||
import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader';
|
||||
import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls';
|
||||
import { sunCalculator } from './utilities/position-utilities';
|
||||
Mesh,
|
||||
PerspectiveCamera,
|
||||
PlaneGeometry,
|
||||
Scene,
|
||||
WebGLRenderer,
|
||||
AmbientLight,
|
||||
DirectionalLight,
|
||||
PCFSoftShadowMap,
|
||||
type GridHelper,
|
||||
ArrowHelper,
|
||||
Vector3,
|
||||
FogExp2,
|
||||
CanvasTexture,
|
||||
type ColorRepresentation,
|
||||
type WebGLRendererParameters,
|
||||
MeshPhongMaterial,
|
||||
EquirectangularReflectionMapping,
|
||||
ACESFilmicToneMapping,
|
||||
MathUtils,
|
||||
Group,
|
||||
MeshBasicMaterial,
|
||||
RepeatWrapping
|
||||
} 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 { Reflector } from 'three/examples/jsm/objects/Reflector.js'
|
||||
import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader'
|
||||
import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls'
|
||||
import { sunCalculator } from './utilities/position-utilities'
|
||||
|
||||
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 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
|
||||
|
||||
export default class SceneBuilder {
|
||||
public scene: Scene;
|
||||
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;
|
||||
transformControl: TransformControls;
|
||||
public modelGroup!: Group;
|
||||
public scene: Scene
|
||||
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
|
||||
transformControl: TransformControls
|
||||
public modelGroup!: Group
|
||||
|
||||
constructor() {
|
||||
this.scene = new Scene();
|
||||
if (this.scene.environment?.mapping) {
|
||||
this.scene.environment.mapping = EquirectangularReflectionMapping;
|
||||
}
|
||||
return this;
|
||||
constructor() {
|
||||
this.scene = new Scene()
|
||||
if (this.scene.environment?.mapping) {
|
||||
this.scene.environment.mapping = EquirectangularReflectionMapping
|
||||
}
|
||||
return this
|
||||
}
|
||||
|
||||
public addRenderer = (parameters?: WebGLRendererParameters) => {
|
||||
this.renderer = new WebGLRenderer(parameters)
|
||||
this.renderer.outputColorSpace = 'srgb'
|
||||
this.renderer.shadowMap.enabled = true
|
||||
this.renderer.shadowMap.type = PCFSoftShadowMap
|
||||
this.renderer.toneMapping = ACESFilmicToneMapping
|
||||
this.renderer.toneMappingExposure = 0.85
|
||||
if (!parameters?.canvas) document.body.appendChild(this.renderer.domElement)
|
||||
return this
|
||||
}
|
||||
|
||||
public addSky = () => {
|
||||
this.sky = new Sky()
|
||||
this.sky.scale.setScalar(450000)
|
||||
this.scene.add(this.sky)
|
||||
const effectController = {
|
||||
turbidity: 10,
|
||||
rayleigh: 3,
|
||||
mieCoefficient: 0.005,
|
||||
mieDirectionalG: 0.7,
|
||||
elevation: sunCalculator.calculateSunElevation(),
|
||||
azimuth: 200,
|
||||
exposure: this.renderer.toneMappingExposure
|
||||
}
|
||||
const uniforms = this.sky.material.uniforms
|
||||
uniforms['turbidity'].value = effectController.turbidity
|
||||
uniforms['rayleigh'].value = effectController.rayleigh
|
||||
uniforms['mieCoefficient'].value = effectController.mieCoefficient
|
||||
uniforms['mieDirectionalG'].value = effectController.mieDirectionalG
|
||||
this.renderer.toneMappingExposure = 0.5
|
||||
const phi = MathUtils.degToRad(90 - effectController.elevation)
|
||||
const theta = MathUtils.degToRad(effectController.azimuth)
|
||||
const sun = new Vector3()
|
||||
|
||||
sun.setFromSphericalCoords(1, phi, theta)
|
||||
uniforms['sunPosition'].value.copy(sun)
|
||||
return this
|
||||
}
|
||||
|
||||
public addPerspectiveCamera = (options: position) => {
|
||||
this.camera = new PerspectiveCamera()
|
||||
this.camera.position.set(options.x ?? 0, options.y ?? 2.7, options.z ?? 0)
|
||||
this.scene.add(this.camera)
|
||||
return this
|
||||
}
|
||||
|
||||
public addGroundPlane = (options?: position) => {
|
||||
const checkerboardTexture = this.createCheckerboardTexture(1024, 2)
|
||||
checkerboardTexture.wrapS = RepeatWrapping
|
||||
checkerboardTexture.wrapT = RepeatWrapping
|
||||
checkerboardTexture.repeat.set(100, 100)
|
||||
const checkerboardMat = new MeshBasicMaterial({
|
||||
map: checkerboardTexture,
|
||||
opacity: 0.1,
|
||||
transparent: true
|
||||
})
|
||||
|
||||
const plane = new PlaneGeometry(400, 400)
|
||||
|
||||
this.ground = new Mesh(plane, checkerboardMat)
|
||||
this.ground.rotation.x = -Math.PI / 2
|
||||
this.ground.position.set(options?.x ?? 0, options?.y ?? 0.01, options?.z ?? 0)
|
||||
this.ground.receiveShadow = true
|
||||
this.scene.add(this.ground)
|
||||
|
||||
const mirror = new Reflector(plane, {
|
||||
clipBias: 0.003,
|
||||
textureWidth: window.innerWidth * window.devicePixelRatio,
|
||||
textureHeight: window.innerHeight * window.devicePixelRatio,
|
||||
color: 0x00bfff
|
||||
})
|
||||
mirror.rotateX(-Math.PI / 2)
|
||||
this.scene.add(mirror)
|
||||
|
||||
return this
|
||||
}
|
||||
|
||||
public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
|
||||
this.orbit = new OrbitControls(this.camera, this.renderer.domElement)
|
||||
this.orbit.minDistance = 5
|
||||
this.orbit.maxDistance = maxDistance
|
||||
this.orbit.autoRotate = autoRotate
|
||||
this.orbit.update()
|
||||
return this
|
||||
}
|
||||
|
||||
public addAmbientLight = (options: light) => {
|
||||
const ambientLight = new AmbientLight(options.color, options.intensity)
|
||||
this.scene.add(ambientLight)
|
||||
return this
|
||||
}
|
||||
|
||||
public addDirectionalLight = (options: directionalLight) => {
|
||||
const directionalLight = new DirectionalLight(options.color, options.intensity)
|
||||
directionalLight.castShadow = true
|
||||
directionalLight.shadow.camera.top = 10
|
||||
directionalLight.shadow.camera.bottom = -10
|
||||
directionalLight.shadow.camera.right = 10
|
||||
directionalLight.shadow.camera.left = -10
|
||||
directionalLight.shadow.mapSize.set(4096, 4096)
|
||||
|
||||
directionalLight.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0)
|
||||
this.scene.add(directionalLight)
|
||||
return this
|
||||
}
|
||||
|
||||
private createCheckerboardTexture = (size: number, squares: number) => {
|
||||
const canvas = document.createElement('canvas')
|
||||
canvas.width = size
|
||||
canvas.height = size
|
||||
const context = canvas.getContext('2d')
|
||||
|
||||
const squareSize = size / squares
|
||||
|
||||
for (let y = 0; y < squares; y++) {
|
||||
for (let x = 0; x < squares; x++) {
|
||||
context!.fillStyle = (x + y) % 2 === 0 ? '#ffffff' : '#000000'
|
||||
context!.fillRect(x * squareSize, y * squareSize, squareSize, squareSize)
|
||||
}
|
||||
}
|
||||
|
||||
public addRenderer = (parameters?: WebGLRendererParameters) => {
|
||||
this.renderer = new WebGLRenderer(parameters);
|
||||
this.renderer.outputColorSpace = 'srgb';
|
||||
this.renderer.shadowMap.enabled = true;
|
||||
this.renderer.shadowMap.type = PCFSoftShadowMap;
|
||||
this.renderer.toneMapping = ACESFilmicToneMapping;
|
||||
this.renderer.toneMappingExposure = 0.85;
|
||||
if (!parameters?.canvas) document.body.appendChild(this.renderer.domElement);
|
||||
return this;
|
||||
};
|
||||
const texture = new CanvasTexture(canvas)
|
||||
texture.wrapS = texture.wrapT = RepeatWrapping
|
||||
texture.anisotropy = 16
|
||||
return texture
|
||||
}
|
||||
|
||||
public addSky = () => {
|
||||
this.sky = new Sky();
|
||||
this.sky.scale.setScalar(450000);
|
||||
this.scene.add(this.sky);
|
||||
const effectController = {
|
||||
turbidity: 10,
|
||||
rayleigh: 3,
|
||||
mieCoefficient: 0.005,
|
||||
mieDirectionalG: 0.7,
|
||||
elevation: sunCalculator.calculateSunElevation(),
|
||||
azimuth: 200,
|
||||
exposure: this.renderer.toneMappingExposure
|
||||
};
|
||||
const uniforms = this.sky.material.uniforms;
|
||||
uniforms['turbidity'].value = effectController.turbidity;
|
||||
uniforms['rayleigh'].value = effectController.rayleigh;
|
||||
uniforms['mieCoefficient'].value = effectController.mieCoefficient;
|
||||
uniforms['mieDirectionalG'].value = effectController.mieDirectionalG;
|
||||
this.renderer.toneMappingExposure = 0.5;
|
||||
const phi = MathUtils.degToRad(90 - effectController.elevation);
|
||||
const theta = MathUtils.degToRad(effectController.azimuth);
|
||||
const sun = new Vector3();
|
||||
public addFogExp2 = (color: ColorRepresentation, density?: number) => {
|
||||
this.scene.fog = new FogExp2(color, density)
|
||||
return this
|
||||
}
|
||||
|
||||
sun.setFromSphericalCoords(1, phi, theta);
|
||||
uniforms['sunPosition'].value.copy(sun);
|
||||
return this;
|
||||
};
|
||||
|
||||
public addPerspectiveCamera = (options: position) => {
|
||||
this.camera = new PerspectiveCamera();
|
||||
this.camera.position.set(options.x ?? 0, options.y ?? 2.7, options.z ?? 0);
|
||||
this.scene.add(this.camera);
|
||||
return this;
|
||||
};
|
||||
|
||||
public addGroundPlane = (options?: position) => {
|
||||
const checkerboardTexture = this.createCheckerboardTexture(1024, 2);
|
||||
checkerboardTexture.wrapS = RepeatWrapping;
|
||||
checkerboardTexture.wrapT = RepeatWrapping;
|
||||
checkerboardTexture.repeat.set(100, 100);
|
||||
const checkerboardMat = new MeshBasicMaterial({
|
||||
map: checkerboardTexture,
|
||||
opacity: 0.1,
|
||||
transparent: true
|
||||
});
|
||||
|
||||
const plane = new PlaneGeometry(400, 400);
|
||||
|
||||
this.ground = new Mesh(plane, checkerboardMat);
|
||||
this.ground.rotation.x = -Math.PI / 2;
|
||||
this.ground.position.set(options?.x ?? 0, options?.y ?? 0.01, options?.z ?? 0);
|
||||
this.ground.receiveShadow = true;
|
||||
this.scene.add(this.ground);
|
||||
|
||||
const mirror = new Reflector(plane, {
|
||||
clipBias: 0.003,
|
||||
textureWidth: window.innerWidth * window.devicePixelRatio,
|
||||
textureHeight: window.innerHeight * window.devicePixelRatio,
|
||||
color: 0x00bfff
|
||||
});
|
||||
mirror.rotateX(-Math.PI / 2);
|
||||
this.scene.add(mirror);
|
||||
|
||||
return this;
|
||||
};
|
||||
|
||||
public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
|
||||
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;
|
||||
};
|
||||
|
||||
public addAmbientLight = (options: light) => {
|
||||
const ambientLight = new AmbientLight(options.color, options.intensity);
|
||||
this.scene.add(ambientLight);
|
||||
return this;
|
||||
};
|
||||
|
||||
public addDirectionalLight = (options: directionalLight) => {
|
||||
const directionalLight = new DirectionalLight(options.color, options.intensity);
|
||||
directionalLight.castShadow = true;
|
||||
directionalLight.shadow.camera.top = 10;
|
||||
directionalLight.shadow.camera.bottom = -10;
|
||||
directionalLight.shadow.camera.right = 10;
|
||||
directionalLight.shadow.camera.left = -10;
|
||||
directionalLight.shadow.mapSize.set(4096, 4096);
|
||||
|
||||
directionalLight.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0);
|
||||
this.scene.add(directionalLight);
|
||||
return this;
|
||||
};
|
||||
|
||||
private createCheckerboardTexture = (size: number, squares: number) => {
|
||||
const canvas = document.createElement('canvas');
|
||||
canvas.width = size;
|
||||
canvas.height = size;
|
||||
const context = canvas.getContext('2d');
|
||||
|
||||
const squareSize = size / squares;
|
||||
|
||||
for (let y = 0; y < squares; y++) {
|
||||
for (let x = 0; x < squares; x++) {
|
||||
context!.fillStyle = (x + y) % 2 === 0 ? '#ffffff' : '#000000';
|
||||
context!.fillRect(x * squareSize, y * squareSize, squareSize, squareSize);
|
||||
}
|
||||
}
|
||||
|
||||
const texture = new CanvasTexture(canvas);
|
||||
texture.wrapS = texture.wrapT = RepeatWrapping;
|
||||
texture.anisotropy = 16;
|
||||
return texture;
|
||||
};
|
||||
|
||||
public addFogExp2 = (color: ColorRepresentation, density?: number) => {
|
||||
this.scene.fog = new FogExp2(color, density);
|
||||
return this;
|
||||
};
|
||||
|
||||
public fillParent = () => {
|
||||
const parentElement = this.renderer.domElement.parentElement;
|
||||
if (parentElement) {
|
||||
const width = parentElement.clientWidth;
|
||||
const height = parentElement.clientHeight;
|
||||
this.handleResize(width, height);
|
||||
}
|
||||
return this;
|
||||
};
|
||||
|
||||
public handleResize = (width = window.innerWidth, height = window.innerHeight) => {
|
||||
this.renderer.setSize(width, height);
|
||||
this.renderer.setPixelRatio(window.devicePixelRatio);
|
||||
this.camera.aspect = width / height;
|
||||
this.camera.updateProjectionMatrix();
|
||||
return this;
|
||||
};
|
||||
|
||||
public addRenderCb = (callback: Function) => {
|
||||
this.callback = callback;
|
||||
return this;
|
||||
};
|
||||
|
||||
public startRenderLoop = () => {
|
||||
this.renderer.setAnimationLoop(() => {
|
||||
this.renderer.render(this.scene, this.camera);
|
||||
this.orbit.update();
|
||||
this.handleRobotShadow();
|
||||
if (this.callback) this.callback();
|
||||
if (!this.liveStreamTexture) return;
|
||||
});
|
||||
return this;
|
||||
};
|
||||
|
||||
public addArrowHelper = (options?: arrowOptions) => {
|
||||
const dir = new Vector3(
|
||||
options?.direction.x ?? 0,
|
||||
options?.direction.y ?? 0,
|
||||
options?.direction.z ?? 0
|
||||
);
|
||||
const origin = new Vector3(
|
||||
options?.origin.x ?? 0,
|
||||
options?.origin.y ?? 0,
|
||||
options?.origin.z ?? 0
|
||||
);
|
||||
const arrowHelper = new ArrowHelper(
|
||||
dir,
|
||||
origin,
|
||||
options?.length ?? 1.5,
|
||||
options?.color ?? 0xff0000
|
||||
);
|
||||
this.scene.add(arrowHelper);
|
||||
return this;
|
||||
};
|
||||
|
||||
private setJointValue(jointName: string, angle: number) {
|
||||
if (!this.model) return;
|
||||
if (!this.model.joints[jointName]) return;
|
||||
this.model.joints[jointName].setJointValue(angle);
|
||||
public fillParent = () => {
|
||||
const parentElement = this.renderer.domElement.parentElement
|
||||
if (parentElement) {
|
||||
const width = parentElement.clientWidth
|
||||
const height = parentElement.clientHeight
|
||||
this.handleResize(width, height)
|
||||
}
|
||||
return this
|
||||
}
|
||||
|
||||
isJoint = (j: URDFJoint) => j.isURDFJoint && j.jointType !== 'fixed';
|
||||
public handleResize = (width = window.innerWidth, height = window.innerHeight) => {
|
||||
this.renderer.setSize(width, height)
|
||||
this.renderer.setPixelRatio(window.devicePixelRatio)
|
||||
this.camera.aspect = width / height
|
||||
this.camera.updateProjectionMatrix()
|
||||
return this
|
||||
}
|
||||
|
||||
highlightLinkGeometry = (m: URDFMimicJoint, revert: boolean, material: MeshPhongMaterial) => {
|
||||
const traverse = (c: any) => {
|
||||
if (c.type === 'Mesh') {
|
||||
if (revert) {
|
||||
c.material = c.__origMaterial;
|
||||
delete c.__origMaterial;
|
||||
} else {
|
||||
c.__origMaterial = c.material;
|
||||
c.material = material;
|
||||
}
|
||||
}
|
||||
public addRenderCb = (callback: Function) => {
|
||||
this.callback = callback
|
||||
return this
|
||||
}
|
||||
|
||||
if (c === m || !this.isJoint(c)) {
|
||||
for (let i = 0; i < c.children.length; i++) {
|
||||
const child = c.children[i];
|
||||
if (!child.isURDFCollider) {
|
||||
traverse(c.children[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
traverse(m);
|
||||
};
|
||||
public startRenderLoop = () => {
|
||||
this.renderer.setAnimationLoop(() => {
|
||||
this.renderer.render(this.scene, this.camera)
|
||||
this.orbit.update()
|
||||
this.handleRobotShadow()
|
||||
if (this.callback) this.callback()
|
||||
if (!this.liveStreamTexture) return
|
||||
})
|
||||
return this
|
||||
}
|
||||
|
||||
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 addArrowHelper = (options?: arrowOptions) => {
|
||||
const dir = new Vector3(
|
||||
options?.direction.x ?? 0,
|
||||
options?.direction.y ?? 0,
|
||||
options?.direction.z ?? 0
|
||||
)
|
||||
const origin = new Vector3(
|
||||
options?.origin.x ?? 0,
|
||||
options?.origin.y ?? 0,
|
||||
options?.origin.z ?? 0
|
||||
)
|
||||
const arrowHelper = new ArrowHelper(
|
||||
dir,
|
||||
origin,
|
||||
options?.length ?? 1.5,
|
||||
options?.color ?? 0xff0000
|
||||
)
|
||||
this.scene.add(arrowHelper)
|
||||
return this
|
||||
}
|
||||
|
||||
public addModel = (model: any) => {
|
||||
this.modelGroup = new Group();
|
||||
this.modelGroup.add(model);
|
||||
this.model = model;
|
||||
this.scene.add(this.modelGroup);
|
||||
return this;
|
||||
};
|
||||
private setJointValue(jointName: string, angle: number) {
|
||||
if (!this.model) return
|
||||
if (!this.model.joints[jointName]) return
|
||||
this.model.joints[jointName].setJointValue(angle)
|
||||
}
|
||||
|
||||
public addDragControl = (updateAngle: any) => {
|
||||
const highlightColor = '#FFFFFF';
|
||||
const highlightMaterial = new MeshPhongMaterial({
|
||||
shininess: 10,
|
||||
color: highlightColor,
|
||||
emissive: highlightColor,
|
||||
emissiveIntensity: 0.9
|
||||
});
|
||||
isJoint = (j: URDFJoint) => j.isURDFJoint && j.jointType !== 'fixed'
|
||||
|
||||
const dragControls = new PointerURDFDragControls(
|
||||
this.scene,
|
||||
this.camera,
|
||||
this.renderer.domElement
|
||||
);
|
||||
dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
|
||||
this.setJointValue(joint.name, angle);
|
||||
updateAngle(joint.name, angle);
|
||||
};
|
||||
dragControls.onDragStart = () => {
|
||||
this.orbit.enabled = false;
|
||||
this.isDragging = true;
|
||||
};
|
||||
dragControls.onDragEnd = () => {
|
||||
this.orbit.enabled = true;
|
||||
this.isDragging = false;
|
||||
};
|
||||
dragControls.onHover = (joint: URDFMimicJoint) =>
|
||||
this.highlightLinkGeometry(joint, false, highlightMaterial);
|
||||
dragControls.onUnhover = (joint: URDFMimicJoint) =>
|
||||
this.highlightLinkGeometry(joint, true, highlightMaterial);
|
||||
highlightLinkGeometry = (m: URDFMimicJoint, revert: boolean, material: MeshPhongMaterial) => {
|
||||
const traverse = (c: any) => {
|
||||
if (c.type === 'Mesh') {
|
||||
if (revert) {
|
||||
c.material = c.__origMaterial
|
||||
delete c.__origMaterial
|
||||
} else {
|
||||
c.__origMaterial = c.material
|
||||
c.material = material
|
||||
}
|
||||
}
|
||||
|
||||
this.renderer.domElement.addEventListener(
|
||||
'touchstart',
|
||||
data => dragControls._mouseDown(data.touches[0]),
|
||||
{ passive: true }
|
||||
);
|
||||
this.renderer.domElement.addEventListener(
|
||||
'touchmove',
|
||||
data => dragControls._mouseMove(data.touches[0]),
|
||||
{ passive: true }
|
||||
);
|
||||
this.renderer.domElement.addEventListener(
|
||||
'touchend',
|
||||
data => dragControls._mouseUp(data.touches[0]),
|
||||
{ passive: true }
|
||||
);
|
||||
return this;
|
||||
};
|
||||
if (c === m || !this.isJoint(c)) {
|
||||
for (let i = 0; i < c.children.length; i++) {
|
||||
const child = c.children[i]
|
||||
if (!child.isURDFCollider) {
|
||||
traverse(c.children[i])
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
traverse(m)
|
||||
}
|
||||
|
||||
public toggleFog = () => {
|
||||
this.scene.fog = this.scene.fog ? null : this.fog;
|
||||
};
|
||||
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
|
||||
}
|
||||
|
||||
private handleRobotShadow = () => {
|
||||
if (this.isLoaded) return;
|
||||
const intervalId = setInterval(() => this.model?.traverse(c => (c.castShadow = true)), 10);
|
||||
setTimeout(() => clearInterval(intervalId), 1000);
|
||||
this.isLoaded = true;
|
||||
};
|
||||
public addModel = (model: any) => {
|
||||
this.modelGroup = new Group()
|
||||
this.modelGroup.add(model)
|
||||
this.model = model
|
||||
this.scene.add(this.modelGroup)
|
||||
return this
|
||||
}
|
||||
|
||||
public addDragControl = (updateAngle: any) => {
|
||||
const highlightColor = '#FFFFFF'
|
||||
const highlightMaterial = new MeshPhongMaterial({
|
||||
shininess: 10,
|
||||
color: highlightColor,
|
||||
emissive: highlightColor,
|
||||
emissiveIntensity: 0.9
|
||||
})
|
||||
|
||||
const dragControls = new PointerURDFDragControls(
|
||||
this.scene,
|
||||
this.camera,
|
||||
this.renderer.domElement
|
||||
)
|
||||
dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
|
||||
this.setJointValue(joint.name, angle)
|
||||
updateAngle(joint.name, angle)
|
||||
}
|
||||
dragControls.onDragStart = () => {
|
||||
this.orbit.enabled = false
|
||||
this.isDragging = true
|
||||
}
|
||||
dragControls.onDragEnd = () => {
|
||||
this.orbit.enabled = true
|
||||
this.isDragging = false
|
||||
}
|
||||
dragControls.onHover = (joint: URDFMimicJoint) =>
|
||||
this.highlightLinkGeometry(joint, false, highlightMaterial)
|
||||
dragControls.onUnhover = (joint: URDFMimicJoint) =>
|
||||
this.highlightLinkGeometry(joint, true, highlightMaterial)
|
||||
|
||||
this.renderer.domElement.addEventListener(
|
||||
'touchstart',
|
||||
data => dragControls._mouseDown(data.touches[0]),
|
||||
{ passive: true }
|
||||
)
|
||||
this.renderer.domElement.addEventListener(
|
||||
'touchmove',
|
||||
data => dragControls._mouseMove(data.touches[0]),
|
||||
{ passive: true }
|
||||
)
|
||||
this.renderer.domElement.addEventListener(
|
||||
'touchend',
|
||||
data => dragControls._mouseUp(data.touches[0]),
|
||||
{ passive: true }
|
||||
)
|
||||
return this
|
||||
}
|
||||
|
||||
public toggleFog = () => {
|
||||
this.scene.fog = this.scene.fog ? null : this.fog
|
||||
}
|
||||
|
||||
private handleRobotShadow = () => {
|
||||
if (this.isLoaded) return
|
||||
const intervalId = setInterval(() => this.model?.traverse(c => (c.castShadow = true)), 10)
|
||||
setTimeout(() => clearInterval(intervalId), 1000)
|
||||
this.isLoaded = true
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user