diff --git a/src/controls.ts b/src/controls.ts index 7a4668d..fc934a0 100644 --- a/src/controls.ts +++ b/src/controls.ts @@ -16,6 +16,7 @@ export class URDFControls extends GUI { private _workspaceFolder: any; private _sceneFolder: any; private _jointsFolder: any; + private _jointsEditorFolder: any; private _workingPath = ''; controls: any = { @@ -26,7 +27,8 @@ export class URDFControls extends GUI { height: {} }, joints: {}, - lights: {} + lights: {}, + editor: {} }; /** @@ -51,6 +53,12 @@ export class URDFControls extends GUI { this._jointsFolder = this.addFolder('Joints'); this._jointsFolder.domElement.setAttribute('class', 'dg joints-folder'); + this._jointsEditorFolder = this.addFolder('Joints Editor'); + this._jointsEditorFolder.domElement.setAttribute( + 'class', + 'dg editor-folder' + ); + this._workspaceFolder = this.addFolder('Workspace'); this._workspaceFolder.domElement.setAttribute( 'class', @@ -82,6 +90,13 @@ export class URDFControls extends GUI { return this._jointsFolder; } + /** + * Retrieves the folder with editor settings + */ + get jointsEditorFolder() { + return this._jointsEditorFolder; + } + /** * Checks if a given object is empty {} * @@ -92,6 +107,28 @@ export class URDFControls extends GUI { return Object.keys(obj).length === 0; } + /** + * Restricts input on a control to numeric and special characters. + * + * @param control - The dat.gui controller to modify. + */ + private _enforceNumericInput(control: any): void { + const inputElement = control.domElement as HTMLInputElement; + + inputElement.addEventListener('input', (event: Event) => { + const target = event.target as HTMLInputElement; + const originalValue = target.value; + + // Remove any characters that aren't digits, spaces, periods, or minus signs + const filteredValue = originalValue.replace(/[^\d.\s-]/g, ''); + + if (originalValue !== filteredValue) { + target.value = filteredValue; + control.updateDisplay(); + } + }); + } + /** * Creates an input box and a button to modify the working path * @@ -155,6 +192,9 @@ export class URDFControls extends GUI { stepSize ); + // Enforce input validation + this._enforceNumericInput(this.controls.scene.height); + this._sceneFolder.open(); } return this.controls.scene; @@ -200,7 +240,7 @@ export class URDFControls extends GUI { return; } - const stepSize = (limitMax - limitMin) / 100.0; + const stepSize = (limitMax - limitMin) / 100; const initValue = joints[name].jointValue[0]; this.controls.joints[name] = this._jointsFolder.add( @@ -210,6 +250,7 @@ export class URDFControls extends GUI { limitMax, stepSize ); + this._enforceNumericInput(this.controls.joints[name]); }); this._jointsFolder.open(); } @@ -385,11 +426,109 @@ export class URDFControls extends GUI { .name('Show Helper') }; + this._enforceNumericInput( + this.controls.lights.directional.position.altitude + ); + this._enforceNumericInput( + this.controls.lights.directional.position.azimuth + ); + this._enforceNumericInput(this.controls.lights.directional.intensity); + this._enforceNumericInput(this.controls.lights.ambient.intensity); + this._enforceNumericInput(this.controls.lights.hemisphere.intensity); + // Open Scene (lights) and directional subfolder this._sceneFolder.open(); directionalFolder.open(); } - return this.controls.lights; } + + /** + * Creates controls for the editor mode + * + * @returns - The controls to trigger callbacks when editor settings change + */ + createEditorControls(addJointCallback: () => void, linkNames: string[] = []) { + if (this._isEmpty(this.controls.editor)) { + const editorSettings = { + 'Cursor Link Selection': false, + 'Parent Link': 'none', + 'Child Link': 'none', + 'Joint Name': 'new_joint', + 'Joint Type': 'revolute', + 'Origin XYZ': '0 0 0', + 'Origin RPY': '0 0 0', + 'Axis XYZ': '0 0 1', + 'Lower Limit': '-1.0', + 'Upper Limit': '1.0', + Effort: '0.0', + Velocity: '0.0', + 'Add Joint': addJointCallback + }; + + const dropdownOptions = ['none', ...linkNames]; + + this.controls.editor.mode = this._jointsEditorFolder.add( + editorSettings, + 'Cursor Link Selection' + ); + this.controls.editor.parent = this._jointsEditorFolder + .add(editorSettings, 'Parent Link', dropdownOptions) + .listen(); + this.controls.editor.child = this._jointsEditorFolder + .add(editorSettings, 'Child Link', dropdownOptions) + .listen(); + this.controls.editor.name = this._jointsEditorFolder.add( + editorSettings, + 'Joint Name' + ); + this.controls.editor.type = this._jointsEditorFolder.add( + editorSettings, + 'Joint Type', + ['revolute', 'continuous', 'prismatic', 'fixed', 'floating', 'planar'] + ); + + // Add origin and axis controls + this.controls.editor.origin_xyz = this._jointsEditorFolder + .add(editorSettings, 'Origin XYZ') + .name('Origin XYZ'); + this.controls.editor.origin_rpy = this._jointsEditorFolder + .add(editorSettings, 'Origin RPY') + .name('Origin RPY'); + this.controls.editor.axis_xyz = this._jointsEditorFolder + .add(editorSettings, 'Axis XYZ') + .name('Axis XYZ'); + + // Add limit controls + this.controls.editor.lower = this._jointsEditorFolder + .add(editorSettings, 'Lower Limit') + .name('Lower Limit'); + this.controls.editor.upper = this._jointsEditorFolder + .add(editorSettings, 'Upper Limit') + .name('Upper Limit'); + this.controls.editor.effort = this._jointsEditorFolder + .add(editorSettings, 'Effort') + .name('Effort'); + this.controls.editor.velocity = this._jointsEditorFolder + .add(editorSettings, 'Velocity') + .name('Velocity'); + + this._enforceNumericInput(this.controls.editor.origin_xyz); + this._enforceNumericInput(this.controls.editor.origin_rpy); + this._enforceNumericInput(this.controls.editor.axis_xyz); + this._enforceNumericInput(this.controls.editor.lower); + this._enforceNumericInput(this.controls.editor.upper); + this._enforceNumericInput(this.controls.editor.effort); + this._enforceNumericInput(this.controls.editor.velocity); + + this.controls.editor.add = this._jointsEditorFolder.add( + editorSettings, + 'Add Joint' + ); + + this._jointsEditorFolder.open(); + } + + return this.controls.editor; + } } diff --git a/src/editor/linkSelector.ts b/src/editor/linkSelector.ts new file mode 100644 index 0000000..038393f --- /dev/null +++ b/src/editor/linkSelector.ts @@ -0,0 +1,277 @@ +import * as THREE from 'three'; +import { CSS2DObject } from 'three/examples/jsm/renderers/CSS2DRenderer.js'; +import { Signal } from '@lumino/signaling'; +import { URDFRenderer } from '../renderer'; + +/** + * LinkSelector: Handles all user interaction logic for the URDF link selection mode + */ +export class LinkSelector { + private _renderer: URDFRenderer; + private _raycaster: THREE.Raycaster; + private _mouse: THREE.Vector2; + private _linkSelectorMode = false; + + private _hoveredObj: { + link: any | null; + originalMaterial: THREE.Material | THREE.Material[] | null; + tag: CSS2DObject | null; + } = { link: null, originalMaterial: null, tag: null }; + + private _selectedParentObj: { + link: any | null; + originalMaterial: THREE.Material | THREE.Material[] | null; + tag: CSS2DObject | null; + } = { + link: null, + originalMaterial: null, + tag: null + }; + + private _selectedChildObj: { + link: any | null; + originalMaterial: THREE.Material | THREE.Material[] | null; + tag: CSS2DObject | null; + } = { + link: null, + originalMaterial: null, + tag: null + }; + + private _highlightMaterial: THREE.Material; + private _parentSelectMaterial: THREE.Material; + private _childSelectMaterial: THREE.Material; + + // Signals for communication with layout + readonly linkSelected = new Signal(this); + + constructor(renderer: URDFRenderer) { + this._renderer = renderer; + this._raycaster = new THREE.Raycaster(); + this._mouse = new THREE.Vector2(); + + // Create highlight and selection materials + this._highlightMaterial = new THREE.MeshBasicMaterial({ + color: 0xffff00, + transparent: true, + opacity: 0.6 + }); + this._parentSelectMaterial = new THREE.MeshBasicMaterial({ + color: 0x00ff00, + transparent: true, + opacity: 0.6 + }); + this._childSelectMaterial = new THREE.MeshBasicMaterial({ + color: 0x0000ff, + transparent: true, + opacity: 0.6 + }); + + this._setupInteractions(); + } + + /** + * Sets the link selector mode on/off + */ + setLinkSelectorMode(enabled: boolean): void { + this._linkSelectorMode = enabled; + + if (!enabled) { + this.clearHighlights(); + } + } + + /** + * Sets up mouse event listeners for picking and hovering + */ + private _setupInteractions(): void { + this._setupPicking(); + this._setupHovering(); + } + + /** + * Sets up mouse click event for selecting links + */ + private _setupPicking(): void { + this._renderer.domElement.addEventListener('click', (event: MouseEvent) => { + if (!this._linkSelectorMode) { + return; + } + + const rect = this._renderer.domElement.getBoundingClientRect(); + this._mouse.x = ((event.clientX - rect.left) / rect.width) * 2 - 1; + this._mouse.y = -((event.clientY - rect.top) / rect.height) * 2 + 1; + + this._raycaster.setFromCamera(this._mouse, this._renderer.camera); + + const robot = this._renderer.getRobot(); + if (robot) { + const intersects = this._raycaster.intersectObject(robot, true); + if (intersects.length > 0) { + this.linkSelected.emit(intersects[0].object); + } + } + }); + } + + /** + * Sets up mouse hover event for highlighting links + */ + private _setupHovering(): void { + this._renderer.domElement.addEventListener( + 'mousemove', + (event: MouseEvent) => { + if (!this._linkSelectorMode) { + return; + } + + const rect = this._renderer.domElement.getBoundingClientRect(); + this._mouse.x = ((event.clientX - rect.left) / rect.width) * 2 - 1; + this._mouse.y = -((event.clientY - rect.top) / rect.height) * 2 + 1; + + this._raycaster.setFromCamera(this._mouse, this._renderer.camera); + + const robot = this._renderer.getRobot(); + if (robot) { + if (this._hoveredObj.link) { + if ( + this._hoveredObj.link !== this._selectedParentObj.link && + this._hoveredObj.link !== this._selectedChildObj.link + ) { + this._hoveredObj.link.material = + this._hoveredObj.originalMaterial; + } + if (this._hoveredObj.tag) { + this._hoveredObj.link.remove(this._hoveredObj.tag); + } + this._hoveredObj = { + link: null, + originalMaterial: null, + tag: null + }; + this._renderer.redraw(); + } + + const intersects = this._raycaster.intersectObject(robot, true); + if (intersects.length > 0) { + const hoveredObject = intersects[0].object as any; + + // Don't highlight already selected links + if ( + hoveredObject === this._selectedParentObj.link || + hoveredObject === this._selectedChildObj.link + ) { + return; + } + + this._hoveredObj.link = hoveredObject; + this._hoveredObj.originalMaterial = hoveredObject.material; + hoveredObject.material = this._highlightMaterial; + + // Find the link name by traversing up the hierarchy + let visual: any = hoveredObject; + while (visual && !visual.isURDFVisual) { + visual = visual.parent; + } + + // Add a tag with the link's name + if (visual && visual.urdfNode?.parentElement) { + const linkName = + visual.urdfNode.parentElement.getAttribute('name'); + const tagDiv = document.createElement('div'); + tagDiv.className = 'jp-urdf-label'; + tagDiv.textContent = linkName; + + const tag = new CSS2DObject(tagDiv); + this._hoveredObj.tag = tag; + hoveredObject.add(tag); + } + + this._renderer.redraw(); + } + } + } + ); + } + + /** + * Highlights a link with the specified material + */ + highlightLink(link: any, type: 'parent' | 'child'): void { + const material = + type === 'parent' + ? this._parentSelectMaterial + : this._childSelectMaterial; + const selectedObj = + type === 'parent' ? this._selectedParentObj : this._selectedChildObj; + + // Clear previous selection of the same type + if (selectedObj.link) { + selectedObj.link.material = selectedObj.originalMaterial; + if (selectedObj.tag) { + selectedObj.link.remove(selectedObj.tag); + } + } + + // Apply new highlight + // The true original material is the one stored on hover. + selectedObj.originalMaterial = + link === this._hoveredObj.link + ? this._hoveredObj.originalMaterial + : link.material; + link.material = material; + selectedObj.link = link; + + this._renderer.redraw(); + } + + /** + * Un-highlights a single selected link. + */ + unHighlightLink(type: 'parent' | 'child'): void { + const selectedObj = + type === 'parent' ? this._selectedParentObj : this._selectedChildObj; + + if (selectedObj.link) { + selectedObj.link.material = selectedObj.originalMaterial; + if (selectedObj.tag) { + selectedObj.link.remove(selectedObj.tag); + } + selectedObj.link = null; + selectedObj.originalMaterial = null; + selectedObj.tag = null; + } + this._renderer.redraw(); + } + + /** + * Clears all highlights and tags from selected links. + */ + clearHighlights(): void { + if (this._selectedParentObj.link) { + this._selectedParentObj.link.material = + this._selectedParentObj.originalMaterial; + if (this._selectedParentObj.tag) { + this._selectedParentObj.link.remove(this._selectedParentObj.tag); + } + this._selectedParentObj = { + link: null, + originalMaterial: null, + tag: null + }; + } + if (this._selectedChildObj.link) { + this._selectedChildObj.link.material = + this._selectedChildObj.originalMaterial; + if (this._selectedChildObj.tag) { + this._selectedChildObj.link.remove(this._selectedChildObj.tag); + } + this._selectedChildObj = { + link: null, + originalMaterial: null, + tag: null + }; + } + this._renderer.redraw(); + } +} diff --git a/src/editor/urdf-editor.ts b/src/editor/urdf-editor.ts new file mode 100644 index 0000000..cefa25f --- /dev/null +++ b/src/editor/urdf-editor.ts @@ -0,0 +1,97 @@ +/** + * A class for manipulating URDF XML documents. + */ +export class URDFEditor { + private _parser = new DOMParser(); + private _serializer = new XMLSerializer(); + + /** + * Adds a new joint to a URDF string. + * + * @param urdfString - The URDF string to modify. + * @param joint - The joint to add. + * @returns The modified URDF string. + */ + addJoint( + urdfString: string, + joint: { + name: string; + type: string; + parent: string; + child: string; + origin_xyz: string; + origin_rpy: string; + axis_xyz: string; + lower: string; + upper: string; + effort: string; + velocity: string; + } + ): string { + const urdf = this._parser.parseFromString(urdfString, 'application/xml'); + const robot = urdf.getElementsByTagName('robot')[0]; + + if (!robot) { + throw new Error('No robot tag found in URDF'); + } + + // Helper to create an indented text node + const createIndent = (level: number) => + urdf.createTextNode(`\n${' '.repeat(level)}`); + + const jointElement = urdf.createElement('joint'); + jointElement.setAttribute('name', joint.name); + jointElement.setAttribute('type', joint.type); + + // Add child elements with indentation + jointElement.appendChild(createIndent(2)); + const parentElement = urdf.createElement('parent'); + parentElement.setAttribute('link', joint.parent); + jointElement.appendChild(parentElement); + + jointElement.appendChild(createIndent(2)); + const childElement = urdf.createElement('child'); + childElement.setAttribute('link', joint.child); + jointElement.appendChild(childElement); + + jointElement.appendChild(createIndent(2)); + const originElement = urdf.createElement('origin'); + originElement.setAttribute('xyz', joint.origin_xyz); + originElement.setAttribute('rpy', joint.origin_rpy); + jointElement.appendChild(originElement); + + // Add axis only for relevant joint types + if ( + joint.type === 'revolute' || + joint.type === 'continuous' || + joint.type === 'prismatic' || + joint.type === 'planar' + ) { + jointElement.appendChild(createIndent(2)); + const axisElement = urdf.createElement('axis'); + axisElement.setAttribute('xyz', joint.axis_xyz); + jointElement.appendChild(axisElement); + } + + // Add limit only for relevant joint types + if (joint.type === 'revolute' || joint.type === 'prismatic') { + jointElement.appendChild(createIndent(2)); + const limitElement = urdf.createElement('limit'); + limitElement.setAttribute('lower', joint.lower); + limitElement.setAttribute('upper', joint.upper); + limitElement.setAttribute('effort', joint.effort); + limitElement.setAttribute('velocity', joint.velocity); + jointElement.appendChild(limitElement); + } + + // Add final indent before closing tag + jointElement.appendChild(createIndent(1)); + + // Append the new joint with proper indentation + robot.appendChild(createIndent(1)); + robot.appendChild(jointElement); + robot.appendChild(createIndent(0)); + + return this._serializer.serializeToString(urdf); + } +} diff --git a/src/layout.ts b/src/layout.ts index 5c19e47..dbe18a0 100644 --- a/src/layout.ts +++ b/src/layout.ts @@ -12,6 +12,10 @@ import { URDFRenderer } from './renderer'; import { URDFLoadingManager } from './robot'; +import { URDFEditor } from './editor/urdf-editor'; + +import { LinkSelector } from './editor/linkSelector'; + interface IURDFColors { sky: Color; ground: Color; @@ -26,6 +30,16 @@ export class URDFLayout extends PanelLayout { private _renderer: URDFRenderer; private _colors: IURDFColors; private _loader: URDFLoadingManager; + private _editor: URDFEditor; + private _interactionEditor: LinkSelector; + private _context: DocumentRegistry.IContext | null = null; + private _selectedLinks: { + parent: { name: string | null; obj: any | null }; + child: { name: string | null; obj: any | null }; + } = { + parent: { name: null, obj: null }, + child: { name: null, obj: null } + }; /** * Construct a `URDFLayout` @@ -45,6 +59,8 @@ export class URDFLayout extends PanelLayout { this._renderer = new URDFRenderer(this._colors.sky, this._colors.ground); this._controlsPanel = new URDFControls(); this._loader = new URDFLoadingManager(); + this._editor = new URDFEditor(); + this._interactionEditor = new LinkSelector(this._renderer); } /** @@ -90,6 +106,7 @@ export class URDFLayout extends PanelLayout { updateURDF(urdfString: string): void { this._loader.setRobot(urdfString); this._renderer.setRobot(this._loader.robotModel); + this._refreshJointControls(); } /** @@ -98,6 +115,7 @@ export class URDFLayout extends PanelLayout { * @param context - Contains the URDF file and its parameters */ setURDF(context: DocumentRegistry.IContext): void { + this._context = context; // Default to parent directory of URDF file const filePath = context.path; const parentDir = filePath.substring(0, filePath.lastIndexOf('/')); @@ -166,6 +184,7 @@ export class URDFLayout extends PanelLayout { this._setSceneControls(); this._setJointControls(); this._setLightControls(); + this._setEditorControls(); } /** @@ -291,6 +310,230 @@ export class URDFLayout extends PanelLayout { this._renderer.setRobot(this._loader.robotModel); } + /** + * Set callbacks for the editor controls + */ + private _setEditorControls(): void { + const addJointCallback = () => { + if ( + this._context && + this._selectedLinks.parent.name && + this._selectedLinks.child.name + ) { + const urdfString = this._context.model.toString(); + const editorControls = this._controlsPanel.controls.editor; + const newUrdfString = this._editor.addJoint(urdfString, { + name: editorControls.name.getValue(), + type: editorControls.type.getValue(), + parent: this._selectedLinks.parent.name, + child: this._selectedLinks.child.name, + origin_xyz: editorControls.origin_xyz.getValue(), + origin_rpy: editorControls.origin_rpy.getValue(), + axis_xyz: editorControls.axis_xyz.getValue(), + lower: editorControls.lower.getValue(), + upper: editorControls.upper.getValue(), + effort: editorControls.effort.getValue(), + velocity: editorControls.velocity.getValue() + }); + this._context.model.fromString(newUrdfString); + + // Update the robot model and refresh joint controls + this.updateURDF(newUrdfString); + + this._selectedLinks.parent = { name: null, obj: null }; + this._selectedLinks.child = { name: null, obj: null }; + editorControls.parent.setValue('none'); + editorControls.child.setValue('none'); + this._interactionEditor.clearHighlights(); + } + }; + + const linkNames = Object.keys(this._loader.robotModel.links); + const editorControls = this._controlsPanel.createEditorControls( + addJointCallback, + linkNames + ); + + editorControls.mode.onChange((enabled: boolean) => { + this._interactionEditor.setLinkSelectorMode(enabled); + if (!enabled) { + this._selectedLinks.parent = { name: null, obj: null }; + this._selectedLinks.child = { name: null, obj: null }; + editorControls.parent.setValue('none'); + editorControls.child.setValue('none'); + } + }); + + const updateJointName = () => { + const p = this._selectedLinks.parent.name; + const c = this._selectedLinks.child.name; + let newName = 'new_joint'; + if (p && c) { + newName = `${p}_to_${c}_joint`; + } else if (p) { + newName = `${p}_to_..._joint`; + } else if (c) { + newName = `..._to_${c}_joint`; + } + editorControls.name.setValue(newName); + }; + + this._interactionEditor.linkSelected.connect((sender, selectedObject) => { + let visual: any = selectedObject; + while (visual && !visual.isURDFVisual) { + visual = visual.parent; + } + + if (!visual?.urdfNode?.parentElement) { + console.error( + 'Could not find urdfNode for selected object', + selectedObject + ); + return; + } + + const linkName = visual.urdfNode.parentElement.getAttribute('name'); + const linkObject = selectedObject; + + // Case 1: Clicked on the currently selected parent link to unselect it. + if (this._selectedLinks.parent.name === linkName) { + this._interactionEditor.unHighlightLink('parent'); + this._selectedLinks.parent = { name: null, obj: null }; + editorControls.parent.setValue('none'); + updateJointName(); + return; + } + + // Case 2: Clicked on the currently selected child link to unselect it. + if (this._selectedLinks.child.name === linkName) { + this._interactionEditor.unHighlightLink('child'); + this._selectedLinks.child = { name: null, obj: null }; + editorControls.child.setValue('none'); + updateJointName(); + return; + } + + // Prevent selecting the same link as both parent and child + if ( + this._selectedLinks.parent.name === linkName || + this._selectedLinks.child.name === linkName + ) { + return; + } + + // Case 3: No parent is selected yet. + if (!this._selectedLinks.parent.name) { + this._selectedLinks.parent = { name: linkName, obj: linkObject }; + editorControls.parent.setValue(linkName); + this._interactionEditor.highlightLink(linkObject, 'parent'); + updateJointName(); + return; + } + + // Case 4: Parent is selected, but child is not. + if (!this._selectedLinks.child.name) { + this._selectedLinks.child = { name: linkName, obj: linkObject }; + editorControls.child.setValue(linkName); + this._interactionEditor.highlightLink(linkObject, 'child'); + updateJointName(); + return; + } + + // Case 5: Both parent and child are selected, so reset and set new parent. + this._interactionEditor.clearHighlights(); + this._selectedLinks.parent = { name: linkName, obj: linkObject }; + this._selectedLinks.child = { name: null, obj: null }; + editorControls.parent.setValue(linkName); + editorControls.child.setValue('none'); + this._interactionEditor.highlightLink(linkObject, 'parent'); + updateJointName(); + }); + + // Add a handler for joint type changes to show/hide relevant fields + editorControls.type.onChange((type: string) => { + const axisControl = editorControls.axis_xyz; + const limitControls = [ + editorControls.lower, + editorControls.upper, + editorControls.effort, + editorControls.velocity + ]; + + // Show/hide axis based on type + const needsAxis = [ + 'revolute', + 'continuous', + 'prismatic', + 'planar' + ].includes(type); + axisControl.__li.style.display = needsAxis ? '' : 'none'; + + // Show/hide limits based on type + const needsLimits = ['revolute', 'prismatic'].includes(type); + limitControls.forEach(c => { + c.__li.style.display = needsLimits ? '' : 'none'; + }); + }); + + editorControls.type.domElement.dispatchEvent(new Event('change')); + + editorControls.parent.onChange((linkName: string) => { + // Prevent selecting the same link as the child + if (linkName !== 'none' && linkName === this._selectedLinks.child.name) { + editorControls.parent.setValue( + this._selectedLinks.parent.name || 'none' + ); + return; + } + if (linkName === 'none') { + this._interactionEditor.unHighlightLink('parent'); + this._selectedLinks.parent = { name: null, obj: null }; + } else { + const link = this._loader.robotModel.links[linkName]; + const linkObject = link.children.find((c: any) => c.isURDFVisual) + ?.children[0]; + this._selectedLinks.parent = { name: linkName, obj: linkObject }; + this._interactionEditor.highlightLink(linkObject, 'parent'); + } + updateJointName(); + }); + + editorControls.child.onChange((linkName: string) => { + // Prevent selecting the same link as the parent + if (linkName !== 'none' && linkName === this._selectedLinks.parent.name) { + editorControls.child.setValue(this._selectedLinks.child.name || 'none'); // Revert + return; + } + if (linkName === 'none') { + this._interactionEditor.unHighlightLink('child'); + this._selectedLinks.child = { name: null, obj: null }; + } else { + const link = this._loader.robotModel.links[linkName]; + const linkObject = link.children.find((c: any) => c.isURDFVisual) + ?.children[0]; + this._selectedLinks.child = { name: linkName, obj: linkObject }; + this._interactionEditor.highlightLink(linkObject, 'child'); + } + updateJointName(); + }); + } + + /** + * Refreshes the joint controls by clearing and recreating them + */ + private _refreshJointControls(): void { + // Clear existing joint controls + Object.keys(this._controlsPanel.controls.joints).forEach(jointName => { + this._controlsPanel.jointsFolder.remove( + this._controlsPanel.controls.joints[jointName] + ); + }); + this._controlsPanel.controls.joints = {}; + + // Recreate joint controls with updated robot model + this._setJointControls(); + } + /** * Handle `update-request` messages sent to the widget */ @@ -318,6 +561,7 @@ export class URDFLayout extends PanelLayout { protected onAfterAttach(msg: Message): void { this._renderer.redraw(); this._host.appendChild(this._renderer.domElement); + this._host.appendChild(this._renderer.css2dDomElement); this._renderer.setSize( this._renderer.domElement.clientWidth, this._renderer.domElement.clientHeight @@ -337,6 +581,10 @@ export class URDFLayout extends PanelLayout { rect?.width || currentSize.width, rect?.height || currentSize.height ); + this._renderer.setCss2dSize( + rect?.width || currentSize.width, + rect?.height || currentSize.height + ); this._renderer.redraw(); } diff --git a/src/renderer.ts b/src/renderer.ts index 93a7320..2b4eb43 100644 --- a/src/renderer.ts +++ b/src/renderer.ts @@ -1,6 +1,7 @@ import * as THREE from 'three'; import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'; +import { CSS2DRenderer } from 'three/examples/jsm/renderers/CSS2DRenderer.js'; import { URDFRobot } from 'urdf-loader'; @@ -21,6 +22,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { private _scene: THREE.Scene; private _camera: THREE.PerspectiveCamera; private _controls: OrbitControls; + private _css2dRenderer: CSS2DRenderer; private _colorSky = new THREE.Color(); private _colorGround = new THREE.Color(); private _gridHeight = 0; @@ -58,16 +60,13 @@ export class URDFRenderer extends THREE.WebGLRenderer { this._controls = new OrbitControls(this._camera, this.domElement); this._initControls(); - this.printSceneLights(); - } - printSceneLights() { - console.log('Scene Lights:'); - this._scene.children.forEach(obj => { - if (obj.type.includes('Light')) { - console.log(obj); - } - }); + // Initialize the 2D renderer for labels + this._css2dRenderer = new CSS2DRenderer(); + this._css2dRenderer.domElement.style.position = 'absolute'; + this._css2dRenderer.domElement.style.top = '0px'; + this._css2dRenderer.domElement.style.pointerEvents = 'none'; } + /** * Initializes the camera */ @@ -299,14 +298,18 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param robot */ setRobot(robot: URDFRobot): void { - if (this._robotIndex < 0) { - this._scene.add(robot); - this._robotIndex = this._scene.children - .map(i => i.name) - .indexOf(robot.name); - } else { - this._scene.children[this._robotIndex] = robot; + if (this._robotIndex !== -1) { + this._scene.children[this._robotIndex].traverse(child => { + if (child instanceof THREE.Mesh) { + child.geometry.dispose(); + child.material.dispose(); + } + }); + this._scene.children.splice(this._robotIndex, 1); } + + this._robotIndex = this._scene.children.length; + this._scene.add(robot); this.redraw(); } @@ -453,5 +456,34 @@ export class URDFRenderer extends THREE.WebGLRenderer { this._camera.aspect = renderSize.width / renderSize.height; this._camera.updateProjectionMatrix(); this.render(this._scene, this._camera); + if (this._css2dRenderer) { + this._css2dRenderer.render(this._scene, this._camera); + } + } + + /** + * Returns the CSS2D renderer's DOM element. + */ + get css2dDomElement(): HTMLElement { + return this._css2dRenderer.domElement; + } + + /** + * Sets the size of the CSS2D renderer. + * @param width - The width of the renderer. + * @param height - The height of the renderer. + */ + setCss2dSize(width: number, height: number): void { + this._css2dRenderer.setSize(width, height); + } + + get camera(): THREE.PerspectiveCamera { + return this._camera; + } + + getRobot(): URDFRobot | null { + return this._robotIndex !== -1 + ? (this._scene.children[this._robotIndex] as URDFRobot) + : null; } } diff --git a/style/base.css b/style/base.css index d78c8fc..a9cbe39 100644 --- a/style/base.css +++ b/style/base.css @@ -192,3 +192,13 @@ li.cr.number.has-slider > div { .property-name:hover::-webkit-scrollbar-thumb { background: #888; } + +/* Styling for 3D labels in editor mode */ +.jp-urdf-label { + background: yellow; + color: black; + padding: 2px 5px; + border-radius: 3px; + font-family: var(--jp-ui-font-family, sans-serif); + font-size: var(--jp-ui-font-size1, 12px); +}