Rune Harlyk
64 lines
1.8 KiB
TypeScript
64 lines
1.8 KiB
TypeScript
import { Color, LoaderUtils, Vector3 } from 'three';
|
|
import URDFLoader, { type URDFRobot } from 'urdf-loader';
|
|
import { XacroLoader } from 'xacro-parser';
|
|
import { Result } from '$lib/utilities';
|
|
|
|
let model_xml: XMLDocument;
|
|
|
|
export const loadModelAsync = async (
|
|
url: string
|
|
): Promise<Result<[URDFRobot, string[]], string>> => {
|
|
return new Promise((resolve, reject) => {
|
|
const xacroLoader = new XacroLoader();
|
|
const urdfLoader = new URDFLoader();
|
|
urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
|
|
|
|
xacroLoader.load(
|
|
url,
|
|
async (xml) => {
|
|
model_xml = xml;
|
|
try {
|
|
const model = urdfLoader.parse(xml);
|
|
model.rotation.x = -Math.PI / 2;
|
|
model.rotation.z = Math.PI / 2;
|
|
model.traverse((c) => (c.castShadow = true));
|
|
model.updateMatrixWorld(true);
|
|
model.scale.setScalar(10);
|
|
const joints = Object.entries(model.joints)
|
|
.filter((joint) => joint[1].jointType !== 'fixed')
|
|
.map((joint) => joint[0]);
|
|
|
|
resolve(Result.ok([model, joints]));
|
|
} catch (error) {
|
|
resolve(Result.err('Failed to load model', error));
|
|
}
|
|
},
|
|
(error) => reject(error)
|
|
);
|
|
});
|
|
};
|
|
|
|
export const toeWorldPositions = (robot: URDFRobot) => {
|
|
const toe_positions: Vector3[] = [];
|
|
robot.traverse((child) => {
|
|
if (child.name.includes('toe') && !child.name.includes('_link')) {
|
|
const worldPosition = new Vector3();
|
|
child.getWorldPosition(worldPosition);
|
|
toe_positions.push(worldPosition);
|
|
}
|
|
});
|
|
return toe_positions;
|
|
};
|
|
|
|
export const footColor = () => {
|
|
const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element;
|
|
const colorAttrStr = colorElem.getAttribute('rgba') as string;
|
|
const colorStr = colorAttrStr
|
|
.split(' ')
|
|
.slice(0, 3)
|
|
.map((val) => Math.floor(+val * 255))
|
|
.join(', ');
|
|
|
|
return new Color(`rgb(${colorStr})`);
|
|
};
|