src/lib/components/3d/robot/URDF/primitives/UrdfJoint.svelte

<script lang="ts">
	import type IUrdfJoint from "../interfaces/IUrdfJoint";
	import { T } from "@threlte/core";
	import UrdfLink from "./UrdfLink.svelte";
	import { Vector3 } from "three";
	import {
		Billboard,
		interactivity,
		MeshLineGeometry,
		Text,
		type InteractivityProps
	} from "@threlte/extras";

	import type IUrdfLink from "../interfaces/IUrdfLink";
	import type IUrdfRobot from "../interfaces/IUrdfRobot";
	import { radiansToDegrees } from "@/utils";

	const defaultOnClick: InteractivityProps["onclick"] = (event) => {
		event.stopPropagation();
		selectedLink = undefined;
		selectedJoint = joint;
	};

	type Props = InteractivityProps & {
		robot: IUrdfRobot;
		joint: IUrdfJoint;
		selectedJoint?: IUrdfJoint;
		selectedLink?: IUrdfLink;
		showName?: boolean;
		nameHeight?: number;
		highlightColor?: string;
		jointColor?: string;
		jointIndicatorColor?: string;
		showLine?: boolean;
		opacity?: number;
		isInteractive?: boolean;
		showVisual?: boolean;
		showCollision?: boolean;
		visualOpacity?: number;
		collisionOpacity?: number;
		collisionColor?: string;
		jointNames?: boolean;
		joints?: boolean;
	};

	let {
		robot,
		joint,
		selectedJoint = $bindable(),
		selectedLink = $bindable(),
		showLine = false,
		nameHeight = 0.02,
		highlightColor = "#ff0000",
		jointColor = "#000000",
		jointIndicatorColor = "#000000",
		opacity = 0.7,
		isInteractive = false,
		showName = false,
		showVisual = true,
		showCollision = true,
		visualOpacity = 1,
		collisionOpacity = 1,
		collisionColor = "#000000",
		jointNames = true,
		onclick = defaultOnClick,
		...restProps
	}: Props = $props();

	if (isInteractive) {
		interactivity();
	}

	// Helper function to get current joint rotation value in degrees
	function getJointRotationValue(joint: any): number {
		const axis = joint.axis_xyz || [0, 0, 1];
		const rotation = joint.rotation || [0, 0, 0];

		// Find the primary axis and get the rotation value
		for (let i = 0; i < 3; i++) {
			if (Math.abs(axis[i]) > 0.001) {
				return radiansToDegrees(rotation[i] / axis[i]);
			}
		}
		return 0;
	}
</script>

{@html `<!-- Joint ${joint.name} (${joint.type}) -->`}
<!-- draw line from parent-frame to joint origin -->
{#if showLine}
	<T.Line>
		<MeshLineGeometry
			points={[
				new Vector3(0, 0, 0),
				new Vector3(joint.origin_xyz[0], joint.origin_xyz[1], joint.origin_xyz[2])
			]}
		/>
		<T.LineBasicMaterial color={jointColor} />
	</T.Line>
{/if}

<T.Group position={joint.origin_xyz} rotation={joint.origin_rpy}>
	<T.Group rotation={joint.rotation}>
		{#if joint.child}
			<UrdfLink
				{robot}
				link={joint.child}
				textScale={0.2}
				{showName}
				{showVisual}
				{showCollision}
				{visualOpacity}
				{collisionOpacity}
				{collisionColor}
				jointNames={true}
				joints={true}
				{jointColor}
				{jointIndicatorColor}
				{nameHeight}
				{selectedLink}
				{selectedJoint}
				{highlightColor}
				{showLine}
				opacity={1}
				isInteractive={true}
			/>
		{/if}

		{#if showLine}
			<T.Line>
				<MeshLineGeometry points={[new Vector3(0, 0, 0), new Vector3(0, -0.02, 0)]} />
				<T.LineBasicMaterial color={jointIndicatorColor} />
			</T.Line>

			<T.Mesh rotation={[Math.PI / 2, 0, 0]} {...restProps}>
				<T.CylinderGeometry args={[0.004, 0.004, 0.03]} />
				<T.MeshBasicMaterial
					color={selectedJoint == joint ? highlightColor : jointColor}
					{opacity}
					transparent={opacity < 1.0}
				/>
			</T.Mesh>
		{/if}
	</T.Group>
</T.Group>

{#if showName}
	<Billboard
		position.x={joint.origin_xyz[0] + 0.04}
		position.y={joint.origin_xyz[1] + 0.01}
		position.z={joint.origin_xyz[2] + 0.03}
		renderOrder={999}
		frustumCulled={false}
	>
		<Text
			scale={nameHeight}
			color={selectedJoint == joint ? highlightColor : jointColor}
			text={joint.name + " " + getJointRotationValue(joint).toFixed(0) + "°"}
			characters="ABCDEFGHIJKLMNOPQRSTUVWXYZ"
			renderOrder={999}
		></Text>
	</Billboard>
{/if}

<!-- From https://github.com/brean/urdf-viewer -->