From 8634dbd0e7aec214992bed90af37cd1deaa71184 Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Wed, 20 Aug 2025 11:27:37 +0200
Subject: [PATCH 1/8] Add frame visualization

---
 src/controls.ts |  49 ++++++++
 src/layout.ts   |  31 +++++
 src/renderer.ts | 319 +++++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 398 insertions(+), 1 deletion(-)

diff --git a/src/controls.ts b/src/controls.ts
index 0260a24..0dbde07 100644
--- a/src/controls.ts
+++ b/src/controls.ts
@@ -28,6 +28,7 @@ export class URDFControls extends GUI {
     },
     joints: {},
     lights: {},
+    frames: {},
     editor: {}
   };
 
@@ -556,4 +557,52 @@ export class URDFControls extends GUI {
 
     return this.controls.editor;
   }
+
+  /**
+   * Creates controls for coordinate frame visualization
+   *
+   * @param linkNames - Array of available link names
+   * @returns - The controls to trigger callbacks when frame settings change
+   */
+  createFrameControls(linkNames: string[] = []) {
+    if (this._isEmpty(this.controls.frames)) {
+      const frameSettings = {
+        'Show Coordinate Helper': false,
+        'Frame Size': 1
+      };
+
+      this.controls.frames.showCoordinateHelper = this._sceneFolder.add(
+        frameSettings,
+        'Show Coordinate Helper'
+      );
+
+      this.controls.frames.size = this._sceneFolder.add(
+        frameSettings,
+        'Frame Size',
+        0.1,
+        10,
+        0.05
+      );
+
+      this._enforceNumericInput(this.controls.frames.size);
+
+      // Individual frame controls subfolder
+      const individualFramesFolder =
+        this._sceneFolder.addFolder('Individual Frames');
+      this.controls.frames.individual = {};
+
+      // Create checkbox for each link
+      linkNames.forEach(linkName => {
+        const linkSettings = { [linkName]: false };
+        this.controls.frames.individual[linkName] = individualFramesFolder.add(
+          linkSettings,
+          linkName
+        );
+      });
+
+      individualFramesFolder.close();
+    }
+
+    return this.controls.frames;
+  }
 }
diff --git a/src/layout.ts b/src/layout.ts
index 9c022a7..c103f78 100644
--- a/src/layout.ts
+++ b/src/layout.ts
@@ -219,6 +219,37 @@ export class URDFLayout extends PanelLayout {
     sceneControl.height.onChange((newHeight: number) =>
       this._renderer.setGridHeight(newHeight)
     );
+
+    // Add frame controls with link names
+    const linkNames = Object.keys(this._loader.robotModel.links);
+    const frameControls = this._controlsPanel.createFrameControls(linkNames);
+
+    // Coordinate helper control
+    frameControls.showCoordinateHelper.onChange((show: boolean) => {
+      this._renderer.setCoordinateHelperVisibility(show);
+    });
+
+    frameControls.size.onChange((size: number) => {
+      // Update individual frames with new size
+      if (frameControls.individual) {
+        Object.keys(frameControls.individual).forEach(linkName => {
+          const showIndividual = frameControls.individual[linkName].getValue();
+          if (showIndividual) {
+            this._renderer.setIndividualFrameVisibility(linkName, true, size);
+          }
+        });
+      }
+    });
+
+    // Individual frame controls
+    if (frameControls.individual) {
+      Object.keys(frameControls.individual).forEach(linkName => {
+        frameControls.individual[linkName].onChange((show: boolean) => {
+          const size = frameControls.size.getValue();
+          this._renderer.setIndividualFrameVisibility(linkName, show, size);
+        });
+      });
+    }
   }
 
   /**
diff --git a/src/renderer.ts b/src/renderer.ts
index 2b4eb43..648aed6 100644
--- a/src/renderer.ts
+++ b/src/renderer.ts
@@ -1,7 +1,11 @@
 import * as THREE from 'three';
 
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
-import { CSS2DRenderer } from 'three/examples/jsm/renderers/CSS2DRenderer.js';
+import {
+  CSS2DRenderer,
+  CSS2DObject
+} from 'three/examples/jsm/renderers/CSS2DRenderer.js';
+import { Group } from 'three';
 
 import { URDFRobot } from 'urdf-loader';
 
@@ -29,6 +33,12 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   private _robotIndex = -1;
   private _directionalLightHelper: THREE.DirectionalLightHelper | null = null;
   private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null;
+  private _frameHelpers: Group = new Group();
+  private _coordinateHelper: Group | null = null;
+  private _coordHelperScene: THREE.Scene | null = null;
+  private _coordHelperCamera: THREE.OrthographicCamera | null = null;
+  private _coordHelperRenderer: CSS2DRenderer | null = null;
+  private _coordHelperContainer: HTMLElement | null = null;
 
   /**
    * Creates a renderer to manage the scene elements
@@ -65,6 +75,8 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._css2dRenderer.domElement.style.position = 'absolute';
     this._css2dRenderer.domElement.style.top = '0px';
     this._css2dRenderer.domElement.style.pointerEvents = 'none';
+
+    this._scene.add(this._frameHelpers);
   }
 
   /**
@@ -310,6 +322,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
 
     this._robotIndex = this._scene.children.length;
     this._scene.add(robot);
+    this._updateAllFramePositions();
     this.redraw();
   }
 
@@ -448,6 +461,301 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     }
   }
 
+  /**
+   * Updates positions of all visible frames to match current robot pose
+   */
+  private _updateAllFramePositions(): void {
+    const robot = this.getRobot();
+    if (!robot) {
+      return;
+    }
+
+    // Update each visible frame
+    this._frameHelpers.children.forEach((frameGroup: any) => {
+      const linkName = frameGroup.userData.linkName;
+      if (linkName) {
+        robot.traverse((child: any) => {
+          if (child.isURDFLink && child.name === linkName) {
+            const worldPosition = new THREE.Vector3();
+            const worldQuaternion = new THREE.Quaternion();
+            const worldScale = new THREE.Vector3();
+            child.matrixWorld.decompose(
+              worldPosition,
+              worldQuaternion,
+              worldScale
+            );
+
+            frameGroup.position.copy(worldPosition);
+            frameGroup.quaternion.copy(worldQuaternion);
+          }
+        });
+      }
+    });
+  }
+
+  /**
+   * Creates a custom axes helper
+   */
+  private _createCustomAxesHelper(size = 0.3): THREE.Group {
+    const axesGroup = new THREE.Group();
+    axesGroup.userData.size = size;
+
+    const createArrow = (
+      direction: THREE.Vector3,
+      color: number,
+      label: string
+    ) => {
+      const arrowHelper = new THREE.ArrowHelper(
+        direction.normalize(),
+        new THREE.Vector3(0, 0, 0),
+        size,
+        color,
+        size * 0.2,
+        size * 0.1
+      );
+
+      arrowHelper.traverse(child => {
+        if (child instanceof THREE.Mesh) {
+          child.material.depthTest = false;
+          child.material.depthWrite = false;
+          child.renderOrder = 999;
+        }
+        if (child instanceof THREE.Line) {
+          child.material.depthTest = false;
+          child.material.depthWrite = false;
+          child.renderOrder = 999;
+        }
+      });
+
+      return arrowHelper;
+    };
+
+    // ROS coordinate system: X=red, Y=green, Z=blue
+    const xAxis = createArrow(new THREE.Vector3(1, 0, 0), 0xff0000, 'X');
+    const yAxis = createArrow(new THREE.Vector3(0, 1, 0), 0x00ff00, 'Y');
+    const zAxis = createArrow(new THREE.Vector3(0, 0, 1), 0x0000ff, 'Z');
+
+    axesGroup.add(xAxis);
+    axesGroup.add(yAxis);
+    axesGroup.add(zAxis);
+
+    return axesGroup;
+  }
+
+  /**
+   * Creates and shows a coordinate reference helper in the bottom-left corner
+   */
+  private _createCoordinateHelper(): void {
+    this._removeCoordinateHelper();
+
+    this._coordHelperScene = new THREE.Scene();
+
+    this._coordHelperCamera = new THREE.OrthographicCamera(-1, 1, 1, -1, 0, 10);
+    this._coordHelperCamera.position.set(0, 0, 2);
+    this._coordHelperCamera.lookAt(0, 0, 0);
+
+    const axesGroup = new THREE.Group();
+
+    const createArrow = (
+      direction: THREE.Vector3,
+      color: number,
+      label: string
+    ) => {
+      const arrowHelper = new THREE.ArrowHelper(
+        direction.normalize(),
+        new THREE.Vector3(0, 0, 0),
+        0.5,
+        color,
+        0.5 * 0.2,
+        0.5 * 0.1
+      );
+      return arrowHelper;
+    };
+
+    const xAxis = createArrow(new THREE.Vector3(1, 0, 0), 0xff0000, 'X'); // Red - X axis
+    const yAxis = createArrow(new THREE.Vector3(0, 0, 1), 0x00ff00, 'Y'); // Green - Y axis (Z in THREE.js)
+    const zAxis = createArrow(new THREE.Vector3(0, 1, 0), 0x0000ff, 'Z'); // Blue - Z axis (Y in THREE.js)
+
+    axesGroup.add(xAxis);
+    axesGroup.add(yAxis);
+    axesGroup.add(zAxis);
+
+    this._coordHelperScene.add(axesGroup);
+
+    const createLabel = (text: string, pos: THREE.Vector3, color: string) => {
+      const div = document.createElement('div');
+      div.textContent = text;
+      div.style.color = color;
+      div.style.fontFamily = 'Arial, sans-serif';
+      div.style.fontSize = '14px';
+      div.style.fontWeight = 'bold';
+      div.style.textShadow = '1px 1px 1px rgba(0,0,0,0.5)';
+      const label = new CSS2DObject(div);
+      label.position.copy(pos);
+      return label;
+    };
+
+    this._coordHelperScene.add(
+      createLabel('X', new THREE.Vector3(0.6, 0, 0), '#ff0000')
+    );
+    this._coordHelperScene.add(
+      createLabel('Y', new THREE.Vector3(0, 0, 0.6), '#00ff00')
+    );
+    this._coordHelperScene.add(
+      createLabel('Z', new THREE.Vector3(0, 0.6, 0), '#0000ff')
+    );
+
+    // WebGL renderer for axes arrows
+    const coordHelperWebGLRenderer = new THREE.WebGLRenderer({ alpha: true });
+    coordHelperWebGLRenderer.setSize(80, 80);
+    coordHelperWebGLRenderer.domElement.style.position = 'absolute';
+    coordHelperWebGLRenderer.domElement.style.left = '0px';
+    coordHelperWebGLRenderer.domElement.style.top = '0px';
+    coordHelperWebGLRenderer.domElement.style.pointerEvents = 'none';
+
+    // CSS2DRenderer for labels
+    this._coordHelperRenderer = new CSS2DRenderer();
+    this._coordHelperRenderer.setSize(80, 80);
+    this._coordHelperRenderer.domElement.style.position = 'absolute';
+    this._coordHelperRenderer.domElement.style.left = '0px';
+    this._coordHelperRenderer.domElement.style.top = '0px';
+    this._coordHelperRenderer.domElement.style.pointerEvents = 'none';
+
+    // Container for overlay
+    this._coordHelperContainer = document.createElement('div');
+    this._coordHelperContainer.style.position = 'absolute';
+    this._coordHelperContainer.style.left = '10px';
+    this._coordHelperContainer.style.bottom = '10px';
+    this._coordHelperContainer.style.width = '80px';
+    this._coordHelperContainer.style.height = '80px';
+    this._coordHelperContainer.style.pointerEvents = 'none';
+    this._coordHelperContainer.style.zIndex = '10';
+
+    // Add both renderers to container
+    this._coordHelperContainer.appendChild(coordHelperWebGLRenderer.domElement);
+    this._coordHelperContainer.appendChild(
+      this._coordHelperRenderer.domElement
+    );
+
+    // Attach to main renderer's parent
+    if (this.domElement.parentElement) {
+      this.domElement.parentElement.appendChild(this._coordHelperContainer);
+    }
+
+    // Update function - rotate the helper scene to match camera
+    const updateHelper = () => {
+      if (
+        this._coordHelperScene &&
+        this._coordHelperCamera &&
+        this._coordHelperRenderer
+      ) {
+        this._coordHelperScene.rotation.copy(this._camera.rotation);
+        coordHelperWebGLRenderer.render(
+          this._coordHelperScene,
+          this._coordHelperCamera
+        );
+        this._coordHelperRenderer.render(
+          this._coordHelperScene,
+          this._coordHelperCamera
+        );
+      }
+    };
+    (this._coordHelperContainer as any)._updateHelper = updateHelper;
+    (this._coordHelperContainer as any)._webGLRenderer =
+      coordHelperWebGLRenderer;
+
+    // Initial render
+    updateHelper();
+
+    // Update on camera changes
+    this._controls.addEventListener('change', updateHelper);
+  }
+
+  /**
+   * Removes the coordinate helper overlay and cleans up DOM
+   */
+  private _removeCoordinateHelper(): void {
+    if (
+      this._coordHelperContainer &&
+      this._coordHelperContainer.parentElement
+    ) {
+      if ((this._coordHelperContainer as any)._updateHelper) {
+        this._controls.removeEventListener(
+          'change',
+          (this._coordHelperContainer as any)._updateHelper
+        );
+      }
+
+      if ((this._coordHelperContainer as any)._webGLRenderer) {
+        (this._coordHelperContainer as any)._webGLRenderer.dispose();
+      }
+
+      this._coordHelperContainer.parentElement.removeChild(
+        this._coordHelperContainer
+      );
+    }
+    this._coordHelperContainer = null;
+    this._coordHelperRenderer = null;
+    this._coordHelperScene = null;
+    this._coordHelperCamera = null;
+  }
+
+  /**
+   * Toggle the coordinate helper visibility
+   */
+  setCoordinateHelperVisibility(visible: boolean): void {
+    if (visible) {
+      this._createCoordinateHelper();
+    } else {
+      this._removeCoordinateHelper();
+    }
+  }
+
+  /**
+   * Shows or hides coordinate frame for a specific link
+   */
+  setIndividualFrameVisibility(
+    linkName: string,
+    visible: boolean,
+    size = 0.3
+  ): void {
+    const existingFrame = this._frameHelpers.children.find(
+      (child: any) => child.userData.linkName === linkName
+    );
+    if (existingFrame) {
+      this._frameHelpers.remove(existingFrame);
+    }
+
+    if (!visible) {
+      this.redraw();
+      return;
+    }
+
+    const robot = this.getRobot();
+    if (!robot) {
+      return;
+    }
+
+    robot.traverse((child: any) => {
+      if (child.isURDFLink && child.name === linkName) {
+        const axes = this._createCustomAxesHelper(size);
+        axes.userData.linkName = linkName;
+
+        const worldPosition = new THREE.Vector3();
+        const worldQuaternion = new THREE.Quaternion();
+        const worldScale = new THREE.Vector3();
+        child.matrixWorld.decompose(worldPosition, worldQuaternion, worldScale);
+
+        axes.position.copy(worldPosition);
+        axes.quaternion.copy(worldQuaternion);
+
+        this._frameHelpers.add(axes);
+      }
+    });
+
+    this.redraw();
+  }
+
   /**
    * Refreshes the viewer by re-rendering the scene and its elements
    */
@@ -486,4 +794,13 @@ export class URDFRenderer extends THREE.WebGLRenderer {
       ? (this._scene.children[this._robotIndex] as URDFRobot)
       : null;
   }
+
+  dispose(): void {
+    // ... existing dispose code
+    this._frameHelpers.clear();
+    if (this._coordinateHelper) {
+      this._scene.remove(this._coordinateHelper);
+    }
+    this._removeCoordinateHelper();
+  }
 }

From 40d5809e136a693e8063720f3903367f80979292 Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Wed, 27 Aug 2025 11:57:38 +0200
Subject: [PATCH 2/8] Add axis indicator

---
 src/controls.ts |  74 +++++++++++++-------
 src/layout.ts   |  43 +++++++-----
 src/renderer.ts | 181 ++++++++++++++++++++++++++++++++++++++++++++----
 3 files changed, 243 insertions(+), 55 deletions(-)

diff --git a/src/controls.ts b/src/controls.ts
index 0dbde07..30bc547 100644
--- a/src/controls.ts
+++ b/src/controls.ts
@@ -17,6 +17,7 @@ export class URDFControls extends GUI {
   private _sceneFolder: any;
   private _jointsFolder: any;
   private _jointsEditorFolder: any;
+  private _linksFolder: any; // Add new links folder
   private _workingPath = '';
 
   controls: any = {
@@ -28,7 +29,7 @@ export class URDFControls extends GUI {
     },
     joints: {},
     lights: {},
-    frames: {},
+    links: {}, // Move frames here and add new link controls
     editor: {}
   };
 
@@ -54,6 +55,10 @@ export class URDFControls extends GUI {
     this._jointsFolder = this.addFolder('Joints');
     this._jointsFolder.domElement.setAttribute('class', 'dg joints-folder');
 
+    // Add Links folder right after Joints
+    this._linksFolder = this.addFolder('Links');
+    this._linksFolder.domElement.setAttribute('class', 'dg links-folder');
+
     this._jointsEditorFolder = this.addFolder('Joints Editor');
     this._jointsEditorFolder.domElement.setAttribute(
       'class',
@@ -98,6 +103,13 @@ export class URDFControls extends GUI {
     return this._jointsEditorFolder;
   }
 
+  /**
+   * Retrieves the folder with links settings
+   */
+  get linksFolder() {
+    return this._linksFolder;
+  }
+
   /**
    * Checks if a given object is empty {}
    *
@@ -559,50 +571,64 @@ export class URDFControls extends GUI {
   }
 
   /**
-   * Creates controls for coordinate frame visualization
+   * Creates controls for link visualization (frames, opacity)
    *
    * @param linkNames - Array of available link names
-   * @returns - The controls to trigger callbacks when frame settings change
+   * @returns - The controls to trigger callbacks when link settings change
    */
-  createFrameControls(linkNames: string[] = []) {
-    if (this._isEmpty(this.controls.frames)) {
-      const frameSettings = {
-        'Show Coordinate Helper': false,
+  createLinkControls(linkNames: string[] = []) {
+    if (this._isEmpty(this.controls.links)) {
+      const globalLinkSettings = {
+        'Axis Indicator': false,
         'Frame Size': 1
       };
 
-      this.controls.frames.showCoordinateHelper = this._sceneFolder.add(
-        frameSettings,
-        'Show Coordinate Helper'
+      this.controls.links.axisIndicator = this._linksFolder.add(
+        globalLinkSettings,
+        'Axis Indicator'
       );
 
-      this.controls.frames.size = this._sceneFolder.add(
-        frameSettings,
+      this.controls.links.frameSize = this._linksFolder.add(
+        globalLinkSettings,
         'Frame Size',
         0.1,
         10,
         0.05
       );
 
-      this._enforceNumericInput(this.controls.frames.size);
+      this._enforceNumericInput(this.controls.links.frameSize);
 
-      // Individual frame controls subfolder
-      const individualFramesFolder =
-        this._sceneFolder.addFolder('Individual Frames');
-      this.controls.frames.individual = {};
+      // Individual link controls subfolder
+      const individualLinksFolder =
+        this._linksFolder.addFolder('Individual Links');
+      this.controls.links.individual = {};
 
-      // Create checkbox for each link
+      // Create controls for each link
       linkNames.forEach(linkName => {
-        const linkSettings = { [linkName]: false };
-        this.controls.frames.individual[linkName] = individualFramesFolder.add(
-          linkSettings,
-          linkName
+        const linkFolder = individualLinksFolder.addFolder(linkName);
+
+        const linkSettings = {
+          'Show Frame': false,
+          Opacity: 1.0
+        };
+
+        this.controls.links.individual[linkName] = {
+          showFrame: linkFolder.add(linkSettings, 'Show Frame'),
+          opacity: linkFolder.add(linkSettings, 'Opacity', 0.0, 1.0, 0.01)
+        };
+
+        // Enforce numeric input for opacity slider
+        this._enforceNumericInput(
+          this.controls.links.individual[linkName].opacity
         );
+
+        linkFolder.close();
       });
 
-      individualFramesFolder.close();
+      individualLinksFolder.close();
+      this._linksFolder.open();
     }
 
-    return this.controls.frames;
+    return this.controls.links;
   }
 }
diff --git a/src/layout.ts b/src/layout.ts
index c103f78..8903f76 100644
--- a/src/layout.ts
+++ b/src/layout.ts
@@ -220,20 +220,21 @@ export class URDFLayout extends PanelLayout {
       this._renderer.setGridHeight(newHeight)
     );
 
-    // Add frame controls with link names
+    // Add link controls with link names
     const linkNames = Object.keys(this._loader.robotModel.links);
-    const frameControls = this._controlsPanel.createFrameControls(linkNames);
+    const linkControls = this._controlsPanel.createLinkControls(linkNames);
 
-    // Coordinate helper control
-    frameControls.showCoordinateHelper.onChange((show: boolean) => {
-      this._renderer.setCoordinateHelperVisibility(show);
+    // Axis indicator control
+    linkControls.axisIndicator.onChange((show: boolean) => {
+      this._renderer.setAxisIndicatorVisibility(show);
     });
 
-    frameControls.size.onChange((size: number) => {
+    linkControls.frameSize.onChange((size: number) => {
       // Update individual frames with new size
-      if (frameControls.individual) {
-        Object.keys(frameControls.individual).forEach(linkName => {
-          const showIndividual = frameControls.individual[linkName].getValue();
+      if (linkControls.individual) {
+        Object.keys(linkControls.individual).forEach(linkName => {
+          const showIndividual =
+            linkControls.individual[linkName].showFrame.getValue();
           if (showIndividual) {
             this._renderer.setIndividualFrameVisibility(linkName, true, size);
           }
@@ -241,13 +242,23 @@ export class URDFLayout extends PanelLayout {
       }
     });
 
-    // Individual frame controls
-    if (frameControls.individual) {
-      Object.keys(frameControls.individual).forEach(linkName => {
-        frameControls.individual[linkName].onChange((show: boolean) => {
-          const size = frameControls.size.getValue();
-          this._renderer.setIndividualFrameVisibility(linkName, show, size);
-        });
+    // Individual link controls
+    if (linkControls.individual) {
+      Object.keys(linkControls.individual).forEach(linkName => {
+        // Frame visibility
+        linkControls.individual[linkName].showFrame.onChange(
+          (show: boolean) => {
+            const size = linkControls.frameSize.getValue();
+            this._renderer.setIndividualFrameVisibility(linkName, show, size);
+          }
+        );
+
+        // Link opacity control
+        linkControls.individual[linkName].opacity.onChange(
+          (opacity: number) => {
+            this._renderer.setLinkOpacity(linkName, opacity);
+          }
+        );
       });
     }
   }
diff --git a/src/renderer.ts b/src/renderer.ts
index 648aed6..c771516 100644
--- a/src/renderer.ts
+++ b/src/renderer.ts
@@ -34,7 +34,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   private _directionalLightHelper: THREE.DirectionalLightHelper | null = null;
   private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null;
   private _frameHelpers: Group = new Group();
-  private _coordinateHelper: Group | null = null;
+  private _axisIndicator: Group | null = null;
   private _coordHelperScene: THREE.Scene | null = null;
   private _coordHelperCamera: THREE.OrthographicCamera | null = null;
   private _coordHelperRenderer: CSS2DRenderer | null = null;
@@ -510,10 +510,11 @@ export class URDFRenderer extends THREE.WebGLRenderer {
         new THREE.Vector3(0, 0, 0),
         size,
         color,
-        size * 0.2,
-        size * 0.1
+        size * 0.15, // Head length
+        size * 0.08 // Head width
       );
 
+      // Make materials render on top and scale line width proportionally
       arrowHelper.traverse(child => {
         if (child instanceof THREE.Mesh) {
           child.material.depthTest = false;
@@ -521,6 +522,8 @@ export class URDFRenderer extends THREE.WebGLRenderer {
           child.renderOrder = 999;
         }
         if (child instanceof THREE.Line) {
+          // Scale line width more proportionally to the arrow heads
+          child.material.linewidth = Math.max(1, size * 3); // Much smaller multiplier
           child.material.depthTest = false;
           child.material.depthWrite = false;
           child.renderOrder = 999;
@@ -543,10 +546,10 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   }
 
   /**
-   * Creates and shows a coordinate reference helper in the bottom-left corner
+   * Creates and shows a coordinate reference helper Indicator in the bottom-left corner
    */
-  private _createCoordinateHelper(): void {
-    this._removeCoordinateHelper();
+  private _createAxisIndicator(): void {
+    this._removeAxisIndicator();
 
     this._coordHelperScene = new THREE.Scene();
 
@@ -672,9 +675,9 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   }
 
   /**
-   * Removes the coordinate helper overlay and cleans up DOM
+   * Removes the axis indicator overlay and cleans up DOM
    */
-  private _removeCoordinateHelper(): void {
+  private _removeAxisIndicator(): void {
     if (
       this._coordHelperContainer &&
       this._coordHelperContainer.parentElement
@@ -701,13 +704,13 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   }
 
   /**
-   * Toggle the coordinate helper visibility
+   * Toggle the axis indicator visibility
    */
-  setCoordinateHelperVisibility(visible: boolean): void {
+  setAxisIndicatorVisibility(visible: boolean): void {
     if (visible) {
-      this._createCoordinateHelper();
+      this._createAxisIndicator();
     } else {
-      this._removeCoordinateHelper();
+      this._removeAxisIndicator();
     }
   }
 
@@ -756,6 +759,154 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this.redraw();
   }
 
+  /**
+   * Helper method to recursively set opacity on meshes within a single link
+   */
+  private _setMeshOpacity(object: any, opacity: number): void {
+    if (object instanceof THREE.Mesh) {
+      // Handle both single materials and material arrays
+      const materials = Array.isArray(object.material)
+        ? object.material
+        : [object.material];
+
+      materials.forEach((material: any, index: number) => {
+        if (material) {
+          // Debug logging for problematic materials
+          console.log(
+            `Setting opacity ${opacity} for material type: ${material.type}, current opacity: ${material.opacity}`
+          );
+
+          // Check if this material is already cloned for this specific mesh
+          if (!material.userData.isCloned) {
+            const clonedMaterial = material.clone();
+            clonedMaterial.userData.isCloned = true;
+            clonedMaterial.userData.originalOpacity =
+              material.opacity !== undefined ? material.opacity : 1.0;
+
+            // Replace the material with the cloned version
+            if (Array.isArray(object.material)) {
+              object.material[index] = clonedMaterial;
+            } else {
+              object.material = clonedMaterial;
+            }
+
+            material = clonedMaterial;
+          }
+
+          // Store original opacity if not already stored
+          if (material.userData.originalOpacity === undefined) {
+            material.userData.originalOpacity =
+              material.opacity !== undefined ? material.opacity : 1.0;
+          }
+
+          // Aggressive fix for DAE materials - always replace with MeshStandardMaterial for transparency
+          if (opacity < 1.0 && !material.userData.convertedFromDAE) {
+            const standardMaterial = new THREE.MeshStandardMaterial({
+              color: material.color || new THREE.Color(0xffffff),
+              map: material.map,
+              normalMap: material.normalMap,
+              roughness: 0.5,
+              metalness: 0.1,
+              transparent: true,
+              opacity: opacity,
+              side: material.side || THREE.FrontSide,
+              depthWrite: false,
+              alphaTest: 0
+            });
+
+            standardMaterial.userData.isCloned = true;
+            standardMaterial.userData.originalOpacity =
+              material.userData.originalOpacity;
+            standardMaterial.userData.convertedFromDAE = true;
+            standardMaterial.userData.originalMaterial = material;
+
+            // Replace the material
+            if (Array.isArray(object.material)) {
+              object.material[index] = standardMaterial;
+            } else {
+              object.material = standardMaterial;
+            }
+
+            console.log(
+              `Converted ${material.type} to MeshStandardMaterial with opacity ${opacity}`
+            );
+          } else if (opacity >= 1.0 && material.userData.originalMaterial) {
+            // Restore original material for full opacity
+            const originalMaterial = material.userData.originalMaterial;
+            originalMaterial.transparent = false;
+            originalMaterial.opacity = 1.0;
+            originalMaterial.depthWrite = true;
+            originalMaterial.needsUpdate = true;
+
+            if (Array.isArray(object.material)) {
+              object.material[index] = originalMaterial;
+            } else {
+              object.material = originalMaterial;
+            }
+
+            console.log(
+              `Restored original material type: ${originalMaterial.type}`
+            );
+          } else {
+            // Standard opacity setting
+            if (opacity < 1.0) {
+              material.transparent = true;
+              material.alphaTest = 0;
+              material.depthWrite = false;
+            } else {
+              material.transparent = false;
+              material.alphaTest = 0;
+              material.depthWrite = true;
+            }
+
+            material.opacity = opacity;
+            material.needsUpdate = true;
+          }
+
+          // Handle visibility for completely transparent case
+          object.visible = opacity > 0;
+        }
+      });
+    }
+
+    // Recursively apply to children, but only within this link
+    object.children.forEach((child: any) => {
+      if (!child.isURDFLink) {
+        this._setMeshOpacity(child, opacity);
+      }
+    });
+  }
+
+  /**
+   * Sets the opacity of a specific link
+   *
+   * @param linkName - The name of the link
+   * @param opacity - Opacity value from 0 (invisible) to 1 (fully opaque)
+   */
+  setLinkOpacity(linkName: string, opacity: number): void {
+    const robot = this.getRobot();
+    if (!robot) {
+      return;
+    }
+
+    robot.traverse((child: any) => {
+      if (child.isURDFLink && child.name === linkName) {
+        // Only traverse the immediate visual children of THIS link, not nested links
+        child.children.forEach((linkChild: any) => {
+          // Skip if this child is another URDF link (to avoid affecting child links)
+          if (linkChild.isURDFLink) {
+            return;
+          }
+
+          // Recursively handle visual elements within this link only
+          this._setMeshOpacity(linkChild, opacity);
+        });
+      }
+    });
+
+    this.redraw();
+  }
+
   /**
    * Refreshes the viewer by re-rendering the scene and its elements
    */
@@ -798,9 +949,9 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   dispose(): void {
     // ... existing dispose code
     this._frameHelpers.clear();
-    if (this._coordinateHelper) {
-      this._scene.remove(this._coordinateHelper);
+    if (this._axisIndicator) {
+      this._scene.remove(this._axisIndicator);
     }
-    this._removeCoordinateHelper();
+    this._removeAxisIndicator();
   }
 }

From 6249a5267275ee94604d1e84a9b859b17fc62c9e Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Thu, 28 Aug 2025 14:17:32 +0200
Subject: [PATCH 3/8] Fix axis indicator

---
 src/controls.ts            |   5 +-
 src/links/axisIndicator.ts |  49 ++++
 src/links/linkManager.ts   | 183 +++++++++++++++
 src/renderer.ts            | 464 ++-----------------------------------
 4 files changed, 259 insertions(+), 442 deletions(-)
 create mode 100644 src/links/axisIndicator.ts
 create mode 100644 src/links/linkManager.ts

diff --git a/src/controls.ts b/src/controls.ts
index 30bc547..eb6d6c2 100644
--- a/src/controls.ts
+++ b/src/controls.ts
@@ -17,7 +17,7 @@ export class URDFControls extends GUI {
   private _sceneFolder: any;
   private _jointsFolder: any;
   private _jointsEditorFolder: any;
-  private _linksFolder: any; // Add new links folder
+  private _linksFolder: any;
   private _workingPath = '';
 
   controls: any = {
@@ -29,7 +29,7 @@ export class URDFControls extends GUI {
     },
     joints: {},
     lights: {},
-    links: {}, // Move frames here and add new link controls
+    links: {},
     editor: {}
   };
 
@@ -55,7 +55,6 @@ export class URDFControls extends GUI {
     this._jointsFolder = this.addFolder('Joints');
     this._jointsFolder.domElement.setAttribute('class', 'dg joints-folder');
 
-    // Add Links folder right after Joints
     this._linksFolder = this.addFolder('Links');
     this._linksFolder.domElement.setAttribute('class', 'dg links-folder');
 
diff --git a/src/links/axisIndicator.ts b/src/links/axisIndicator.ts
new file mode 100644
index 0000000..2d07e14
--- /dev/null
+++ b/src/links/axisIndicator.ts
@@ -0,0 +1,49 @@
+import * as THREE from 'three';
+import { ViewHelper } from 'three/examples/jsm/helpers/ViewHelper.js';
+
+/**
+ * A helper class to manage and render an axis indicator using THREE.ViewHelper.
+ */
+export class AxisIndicatorHelper {
+  private _viewHelper: ViewHelper;
+  private _proxyCamera: THREE.Camera;
+  public visible = false;
+
+  /**
+   * @param camera The main scene camera to derive orientation from.
+   * @param domElement The renderer's DOM element for ViewHelper instantiation.
+   */
+  constructor(camera: THREE.Camera, domElement: HTMLElement) {
+    // Create a dedicated camera for the ViewHelper.
+    // It will be synchronized with the main camera before rendering.
+    this._proxyCamera = camera.clone();
+    this._viewHelper = new ViewHelper(this._proxyCamera, domElement);
+  }
+
+  /**
+   * Renders the axis indicator.
+   * This should be called in the main render loop, after the main scene has been rendered.
+   * @param renderer The main WebGLRenderer instance.
+   * @param mainCamera The main scene camera to sync with.
+   */
+  public render(renderer: THREE.WebGLRenderer, mainCamera: THREE.Camera): void {
+    if (!this.visible) {
+      return;
+    }
+
+    this._proxyCamera.quaternion.copy(mainCamera.quaternion);
+    const rotationX = new THREE.Quaternion().setFromAxisAngle(
+      new THREE.Vector3(1, 0, 0),
+      Math.PI / 2
+    );
+    this._proxyCamera.quaternion.premultiply(rotationX);
+    this._viewHelper.render(renderer);
+  }
+
+  /**
+   * Disposes of the internal ViewHelper.
+   */
+  public dispose(): void {
+    this._viewHelper.dispose();
+  }
+}
diff --git a/src/links/linkManager.ts b/src/links/linkManager.ts
new file mode 100644
index 0000000..0deb86b
--- /dev/null
+++ b/src/links/linkManager.ts
@@ -0,0 +1,183 @@
+import * as THREE from 'three';
+import { URDFRobot } from 'urdf-loader';
+
+/**
+ * Manages the visual representation of links
+ */
+export class LinkManager {
+  private _robot: URDFRobot | null = null;
+  private _frameHelpers: THREE.Group;
+  private _redrawCallback: () => void;
+
+  constructor(scene: THREE.Scene, redrawCallback: () => void) {
+    this._frameHelpers = new THREE.Group();
+    scene.add(this._frameHelpers);
+    this._redrawCallback = redrawCallback;
+  }
+
+  /**
+   * Sets the current robot model for the manager to operate on.
+   * @param robot The URDFRobot model.
+   */
+  public setRobot(robot: URDFRobot | null): void {
+    this._robot = robot;
+    this._frameHelpers.clear();
+    if (robot) {
+      this.updateAllFramePositions();
+    }
+  }
+
+  /**
+   * Updates positions of all visible frames to match the current robot pose.
+   */
+  public updateAllFramePositions(): void {
+    if (!this._robot) {
+      return;
+    }
+
+    this._frameHelpers.children.forEach((frameGroup: any) => {
+      const linkName = frameGroup.userData.linkName;
+      if (linkName) {
+        this._robot?.traverse((child: any) => {
+          if (child.isURDFLink && child.name === linkName) {
+            const worldPosition = new THREE.Vector3();
+            const worldQuaternion = new THREE.Quaternion();
+            child.matrixWorld.decompose(
+              worldPosition,
+              worldQuaternion,
+              new THREE.Vector3()
+            );
+            frameGroup.position.copy(worldPosition);
+            frameGroup.quaternion.copy(worldQuaternion);
+          }
+        });
+      }
+    });
+  }
+
+  /**
+   * Shows or hides a coordinate frame for a specific link.
+   */
+  public setIndividualFrameVisibility(
+    linkName: string,
+    visible: boolean,
+    size = 0.3
+  ): void {
+    const existingFrame = this._frameHelpers.children.find(
+      (child: any) => child.userData.linkName === linkName
+    );
+    if (existingFrame) {
+      this._frameHelpers.remove(existingFrame);
+    }
+
+    if (!visible || !this._robot) {
+      this._redrawCallback();
+      return;
+    }
+
+    this._robot.traverse((child: any) => {
+      if (child.isURDFLink && child.name === linkName) {
+        const axes = this._createCustomAxesHelper(size);
+        axes.userData.linkName = linkName;
+
+        const worldPosition = new THREE.Vector3();
+        const worldQuaternion = new THREE.Quaternion();
+        child.matrixWorld.decompose(
+          worldPosition,
+          worldQuaternion,
+          new THREE.Vector3()
+        );
+
+        axes.position.copy(worldPosition);
+        axes.quaternion.copy(worldQuaternion);
+
+        this._frameHelpers.add(axes);
+      }
+    });
+
+    this._redrawCallback();
+  }
+
+  /**
+   * Sets the opacity of a specific link.
+   */
+  public setLinkOpacity(linkName: string, opacity: number): void {
+    if (!this._robot) {
+      return;
+    }
+
+    this._robot.traverse((child: any) => {
+      if (child.isURDFLink && child.name === linkName) {
+        child.children.forEach((linkChild: any) => {
+          if (!linkChild.isURDFLink) {
+            this._setMeshOpacity(linkChild, opacity);
+          }
+        });
+      }
+    });
+
+    this._redrawCallback();
+  }
+
+  /**
+   * Disposes of managed resources.
+   */
+  public dispose(): void {
+    this._frameHelpers.clear();
+    this._frameHelpers.parent?.remove(this._frameHelpers);
+  }
+
+  /**
+   * Creates a custom axes helper.
+   */
+  private _createCustomAxesHelper(size = 0.3): THREE.Group {
+    const axesGroup = new THREE.Group();
+    const createArrow = (direction: THREE.Vector3, color: number) => {
+      return new THREE.ArrowHelper(
+        direction.normalize(),
+        new THREE.Vector3(0, 0, 0),
+        size,
+        color,
+        size * 0.2,
+        size * 0.1
+      );
+    };
+
+    const xAxis = createArrow(new THREE.Vector3(1, 0, 0), 0xff0000);
+    const yAxis = createArrow(new THREE.Vector3(0, 1, 0), 0x00ff00);
+    const zAxis = createArrow(new THREE.Vector3(0, 0, 1), 0x0000ff);
+
+    axesGroup.add(xAxis, yAxis, zAxis);
+    return axesGroup;
+  }
+
+  /**
+   * Helper method to recursively set opacity on meshes within a single link.
+   */
+  private _setMeshOpacity(object: any, opacity: number): void {
+    if (object instanceof THREE.Mesh) {
+      const materials = Array.isArray(object.material)
+        ? object.material
+        : [object.material];
+      materials.forEach(material => {
+        if (material) {
+          if (opacity < 1.0) {
+            material.transparent = true;
+            material.depthWrite = false;
+          } else {
+            material.transparent = false;
+            material.depthWrite = true;
+          }
+          material.opacity = opacity;
+          material.needsUpdate = true;
+        }
+      });
+    }
+
+    object.children.forEach((child: any) => {
+      if (!child.isURDFLink) {
+        this._setMeshOpacity(child, opacity);
+      }
+    });
+  }
+}
diff --git a/src/renderer.ts b/src/renderer.ts
index c771516..56e18b7 100644
--- a/src/renderer.ts
+++ b/src/renderer.ts
@@ -1,11 +1,9 @@
 import * as THREE from 'three';
 
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
-import {
-  CSS2DRenderer,
-  CSS2DObject
-} from 'three/examples/jsm/renderers/CSS2DRenderer.js';
-import { Group } from 'three';
+import { CSS2DRenderer } from 'three/examples/jsm/renderers/CSS2DRenderer.js';
+import { AxisIndicatorHelper } from './links/axisIndicator';
+import { LinkManager } from './links/linkManager';
 
 import { URDFRobot } from 'urdf-loader';
 
@@ -33,12 +31,8 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   private _robotIndex = -1;
   private _directionalLightHelper: THREE.DirectionalLightHelper | null = null;
   private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null;
-  private _frameHelpers: Group = new Group();
-  private _axisIndicator: Group | null = null;
-  private _coordHelperScene: THREE.Scene | null = null;
-  private _coordHelperCamera: THREE.OrthographicCamera | null = null;
-  private _coordHelperRenderer: CSS2DRenderer | null = null;
-  private _coordHelperContainer: HTMLElement | null = null;
+  private _axisIndicator: AxisIndicatorHelper;
+  private _linkManager: LinkManager;
 
   /**
    * Creates a renderer to manage the scene elements
@@ -55,6 +49,9 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._colorSky = colorSky;
     this._colorGround = colorGround;
 
+    // This is needed to render the axis indicator correctly
+    this.autoClear = false;
+
     this.setClearColor(0xffffff);
     this.setClearAlpha(0);
     this.outputColorSpace = THREE.SRGBColorSpace;
@@ -76,7 +73,12 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._css2dRenderer.domElement.style.top = '0px';
     this._css2dRenderer.domElement.style.pointerEvents = 'none';
 
-    this._scene.add(this._frameHelpers);
+    // Instantiate the new managers
+    this._axisIndicator = new AxisIndicatorHelper(
+      this._camera,
+      this.domElement
+    );
+    this._linkManager = new LinkManager(this._scene, () => this.redraw());
   }
 
   /**
@@ -322,7 +324,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
 
     this._robotIndex = this._scene.children.length;
     this._scene.add(robot);
-    this._updateAllFramePositions();
+    this._linkManager.setRobot(robot);
     this.redraw();
   }
 
@@ -461,257 +463,12 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     }
   }
 
-  /**
-   * Updates positions of all visible frames to match current robot pose
-   */
-  private _updateAllFramePositions(): void {
-    const robot = this.getRobot();
-    if (!robot) {
-      return;
-    }
-
-    // Update each visible frame
-    this._frameHelpers.children.forEach((frameGroup: any) => {
-      const linkName = frameGroup.userData.linkName;
-      if (linkName) {
-        robot.traverse((child: any) => {
-          if (child.isURDFLink && child.name === linkName) {
-            const worldPosition = new THREE.Vector3();
-            const worldQuaternion = new THREE.Quaternion();
-            const worldScale = new THREE.Vector3();
-            child.matrixWorld.decompose(
-              worldPosition,
-              worldQuaternion,
-              worldScale
-            );
-
-            frameGroup.position.copy(worldPosition);
-            frameGroup.quaternion.copy(worldQuaternion);
-          }
-        });
-      }
-    });
-  }
-
-  /**
-   * Creates a custom axes helper
-   */
-  private _createCustomAxesHelper(size = 0.3): THREE.Group {
-    const axesGroup = new THREE.Group();
-    axesGroup.userData.size = size;
-
-    const createArrow = (
-      direction: THREE.Vector3,
-      color: number,
-      label: string
-    ) => {
-      const arrowHelper = new THREE.ArrowHelper(
-        direction.normalize(),
-        new THREE.Vector3(0, 0, 0),
-        size,
-        color,
-        size * 0.15, // Head length
-        size * 0.08 // Head width
-      );
-
-      // Make materials render on top and scale line width proportionally
-      arrowHelper.traverse(child => {
-        if (child instanceof THREE.Mesh) {
-          child.material.depthTest = false;
-          child.material.depthWrite = false;
-          child.renderOrder = 999;
-        }
-        if (child instanceof THREE.Line) {
-          // Scale line width more proportionally to the arrow heads
-          child.material.linewidth = Math.max(1, size * 3); // Much smaller multiplier
-          child.material.depthTest = false;
-          child.material.depthWrite = false;
-          child.renderOrder = 999;
-        }
-      });
-
-      return arrowHelper;
-    };
-
-    // ROS coordinate system: X=red, Y=green, Z=blue
-    const xAxis = createArrow(new THREE.Vector3(1, 0, 0), 0xff0000, 'X');
-    const yAxis = createArrow(new THREE.Vector3(0, 1, 0), 0x00ff00, 'Y');
-    const zAxis = createArrow(new THREE.Vector3(0, 0, 1), 0x0000ff, 'Z');
-
-    axesGroup.add(xAxis);
-    axesGroup.add(yAxis);
-    axesGroup.add(zAxis);
-
-    return axesGroup;
-  }
-
-  /**
-   * Creates and shows a coordinate reference helper Indicator in the bottom-left corner
-   */
-  private _createAxisIndicator(): void {
-    this._removeAxisIndicator();
-
-    this._coordHelperScene = new THREE.Scene();
-
-    this._coordHelperCamera = new THREE.OrthographicCamera(-1, 1, 1, -1, 0, 10);
-    this._coordHelperCamera.position.set(0, 0, 2);
-    this._coordHelperCamera.lookAt(0, 0, 0);
-
-    const axesGroup = new THREE.Group();
-
-    const createArrow = (
-      direction: THREE.Vector3,
-      color: number,
-      label: string
-    ) => {
-      const arrowHelper = new THREE.ArrowHelper(
-        direction.normalize(),
-        new THREE.Vector3(0, 0, 0),
-        0.5,
-        color,
-        0.5 * 0.2,
-        0.5 * 0.1
-      );
-      return arrowHelper;
-    };
-
-    const xAxis = createArrow(new THREE.Vector3(1, 0, 0), 0xff0000, 'X'); // Red - X axis
-    const yAxis = createArrow(new THREE.Vector3(0, 0, 1), 0x00ff00, 'Y'); // Green - Y axis (Z in THREE.js)
-    const zAxis = createArrow(new THREE.Vector3(0, 1, 0), 0x0000ff, 'Z'); // Blue - Z axis (Y in THREE.js)
-
-    axesGroup.add(xAxis);
-    axesGroup.add(yAxis);
-    axesGroup.add(zAxis);
-
-    this._coordHelperScene.add(axesGroup);
-
-    const createLabel = (text: string, pos: THREE.Vector3, color: string) => {
-      const div = document.createElement('div');
-      div.textContent = text;
-      div.style.color = color;
-      div.style.fontFamily = 'Arial, sans-serif';
-      div.style.fontSize = '14px';
-      div.style.fontWeight = 'bold';
-      div.style.textShadow = '1px 1px 1px rgba(0,0,0,0.5)';
-      const label = new CSS2DObject(div);
-      label.position.copy(pos);
-      return label;
-    };
-
-    this._coordHelperScene.add(
-      createLabel('X', new THREE.Vector3(0.6, 0, 0), '#ff0000')
-    );
-    this._coordHelperScene.add(
-      createLabel('Y', new THREE.Vector3(0, 0, 0.6), '#00ff00')
-    );
-    this._coordHelperScene.add(
-      createLabel('Z', new THREE.Vector3(0, 0.6, 0), '#0000ff')
-    );
-
-    // WebGL renderer for axes arrows
-    const coordHelperWebGLRenderer = new THREE.WebGLRenderer({ alpha: true });
-    coordHelperWebGLRenderer.setSize(80, 80);
-    coordHelperWebGLRenderer.domElement.style.position = 'absolute';
-    coordHelperWebGLRenderer.domElement.style.left = '0px';
-    coordHelperWebGLRenderer.domElement.style.top = '0px';
-    coordHelperWebGLRenderer.domElement.style.pointerEvents = 'none';
-
-    // CSS2DRenderer for labels
-    this._coordHelperRenderer = new CSS2DRenderer();
-    this._coordHelperRenderer.setSize(80, 80);
-    this._coordHelperRenderer.domElement.style.position = 'absolute';
-    this._coordHelperRenderer.domElement.style.left = '0px';
-    this._coordHelperRenderer.domElement.style.top = '0px';
-    this._coordHelperRenderer.domElement.style.pointerEvents = 'none';
-
-    // Container for overlay
-    this._coordHelperContainer = document.createElement('div');
-    this._coordHelperContainer.style.position = 'absolute';
-    this._coordHelperContainer.style.left = '10px';
-    this._coordHelperContainer.style.bottom = '10px';
-    this._coordHelperContainer.style.width = '80px';
-    this._coordHelperContainer.style.height = '80px';
-    this._coordHelperContainer.style.pointerEvents = 'none';
-    this._coordHelperContainer.style.zIndex = '10';
-
-    // Add both renderers to container
-    this._coordHelperContainer.appendChild(coordHelperWebGLRenderer.domElement);
-    this._coordHelperContainer.appendChild(
-      this._coordHelperRenderer.domElement
-    );
-
-    // Attach to main renderer's parent
-    if (this.domElement.parentElement) {
-      this.domElement.parentElement.appendChild(this._coordHelperContainer);
-    }
-
-    // Update function - rotate the helper scene to match camera
-    const updateHelper = () => {
-      if (
-        this._coordHelperScene &&
-        this._coordHelperCamera &&
-        this._coordHelperRenderer
-      ) {
-        this._coordHelperScene.rotation.copy(this._camera.rotation);
-        coordHelperWebGLRenderer.render(
-          this._coordHelperScene,
-          this._coordHelperCamera
-        );
-        this._coordHelperRenderer.render(
-          this._coordHelperScene,
-          this._coordHelperCamera
-        );
-      }
-    };
-    (this._coordHelperContainer as any)._updateHelper = updateHelper;
-    (this._coordHelperContainer as any)._webGLRenderer =
-      coordHelperWebGLRenderer;
-
-    // Initial render
-    updateHelper();
-
-    // Update on camera changes
-    this._controls.addEventListener('change', updateHelper);
-  }
-
-  /**
-   * Removes the axis indicator overlay and cleans up DOM
-   */
-  private _removeAxisIndicator(): void {
-    if (
-      this._coordHelperContainer &&
-      this._coordHelperContainer.parentElement
-    ) {
-      if ((this._coordHelperContainer as any)._updateHelper) {
-        this._controls.removeEventListener(
-          'change',
-          (this._coordHelperContainer as any)._updateHelper
-        );
-      }
-
-      if ((this._coordHelperContainer as any)._webGLRenderer) {
-        (this._coordHelperContainer as any)._webGLRenderer.dispose();
-      }
-
-      this._coordHelperContainer.parentElement.removeChild(
-        this._coordHelperContainer
-      );
-    }
-    this._coordHelperContainer = null;
-    this._coordHelperRenderer = null;
-    this._coordHelperScene = null;
-    this._coordHelperCamera = null;
-  }
-
   /**
    * Toggle the axis indicator visibility
    */
   setAxisIndicatorVisibility(visible: boolean): void {
-    if (visible) {
-      this._createAxisIndicator();
-    } else {
-      this._removeAxisIndicator();
-    }
+    this._axisIndicator.visible = visible;
+    this.redraw();
   }
 
   /**
@@ -722,159 +479,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     visible: boolean,
     size = 0.3
   ): void {
-    const existingFrame = this._frameHelpers.children.find(
-      (child: any) => child.userData.linkName === linkName
-    );
-    if (existingFrame) {
-      this._frameHelpers.remove(existingFrame);
-    }
-
-    if (!visible) {
-      this.redraw();
-      return;
-    }
-
-    const robot = this.getRobot();
-    if (!robot) {
-      return;
-    }
-
-    robot.traverse((child: any) => {
-      if (child.isURDFLink && child.name === linkName) {
-        const axes = this._createCustomAxesHelper(size);
-        axes.userData.linkName = linkName;
-
-        const worldPosition = new THREE.Vector3();
-        const worldQuaternion = new THREE.Quaternion();
-        const worldScale = new THREE.Vector3();
-        child.matrixWorld.decompose(worldPosition, worldQuaternion, worldScale);
-
-        axes.position.copy(worldPosition);
-        axes.quaternion.copy(worldQuaternion);
-
-        this._frameHelpers.add(axes);
-      }
-    });
-
-    this.redraw();
-  }
-
-  /**
-   * Helper method to recursively set opacity on meshes within a single link
-   */
-  private _setMeshOpacity(object: any, opacity: number): void {
-    if (object instanceof THREE.Mesh) {
-      // Handle both single materials and material arrays
-      const materials = Array.isArray(object.material)
-        ? object.material
-        : [object.material];
-
-      materials.forEach((material: any, index: number) => {
-        if (material) {
-          // Debug logging for problematic materials
-          console.log(
-            `Setting opacity ${opacity} for material type: ${material.type}, current opacity: ${material.opacity}`
-          );
-
-          // Check if this material is already cloned for this specific mesh
-          if (!material.userData.isCloned) {
-            const clonedMaterial = material.clone();
-            clonedMaterial.userData.isCloned = true;
-            clonedMaterial.userData.originalOpacity =
-              material.opacity !== undefined ? material.opacity : 1.0;
-
-            // Replace the material with the cloned version
-            if (Array.isArray(object.material)) {
-              object.material[index] = clonedMaterial;
-            } else {
-              object.material = clonedMaterial;
-            }
-
-            material = clonedMaterial;
-          }
-
-          // Store original opacity if not already stored
-          if (material.userData.originalOpacity === undefined) {
-            material.userData.originalOpacity =
-              material.opacity !== undefined ? material.opacity : 1.0;
-          }
-
-          // Aggressive fix for DAE materials - always replace with MeshStandardMaterial for transparency
-          if (opacity < 1.0 && !material.userData.convertedFromDAE) {
-            const standardMaterial = new THREE.MeshStandardMaterial({
-              color: material.color || new THREE.Color(0xffffff),
-              map: material.map,
-              normalMap: material.normalMap,
-              roughness: 0.5,
-              metalness: 0.1,
-              transparent: true,
-              opacity: opacity,
-              side: material.side || THREE.FrontSide,
-              depthWrite: false,
-              alphaTest: 0
-            });
-
-            standardMaterial.userData.isCloned = true;
-            standardMaterial.userData.originalOpacity =
-              material.userData.originalOpacity;
-            standardMaterial.userData.convertedFromDAE = true;
-            standardMaterial.userData.originalMaterial = material;
-
-            // Replace the material
-            if (Array.isArray(object.material)) {
-              object.material[index] = standardMaterial;
-            } else {
-              object.material = standardMaterial;
-            }
-
-            console.log(
-              `Converted ${material.type} to MeshStandardMaterial with opacity ${opacity}`
-            );
-          } else if (opacity >= 1.0 && material.userData.originalMaterial) {
-            // Restore original material for full opacity
-            const originalMaterial = material.userData.originalMaterial;
-            originalMaterial.transparent = false;
-            originalMaterial.opacity = 1.0;
-            originalMaterial.depthWrite = true;
-            originalMaterial.needsUpdate = true;
-
-            if (Array.isArray(object.material)) {
-              object.material[index] = originalMaterial;
-            } else {
-              object.material = originalMaterial;
-            }
-
-            console.log(
-              `Restored original material type: ${originalMaterial.type}`
-            );
-          } else {
-            // Standard opacity setting
-            if (opacity < 1.0) {
-              material.transparent = true;
-              material.alphaTest = 0;
-              material.depthWrite = false;
-            } else {
-              material.transparent = false;
-              material.alphaTest = 0;
-              material.depthWrite = true;
-            }
-
-            material.opacity = opacity;
-            material.needsUpdate = true;
-          }
-
-          // Handle visibility for completely transparent case
-          object.visible = opacity > 0;
-        }
-      });
-    }
-
-    // Recursively apply to children, but only within this link
-    object.children.forEach((child: any) => {
-      if (!child.isURDFLink) {
-        this._setMeshOpacity(child, opacity);
-      }
-    });
+    this._linkManager.setIndividualFrameVisibility(linkName, visible, size);
   }
 
   /**
@@ -884,27 +489,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param opacity - Opacity value from 0 (invisible) to 1 (fully opaque)
    */
   setLinkOpacity(linkName: string, opacity: number): void {
-    const robot = this.getRobot();
-    if (!robot) {
-      return;
-    }
-
-    robot.traverse((child: any) => {
-      if (child.isURDFLink && child.name === linkName) {
-        // Only traverse the immediate visual children of THIS link, not nested links
-        child.children.forEach((linkChild: any) => {
-          // Skip if this child is another URDF link (to avoid affecting child links)
-          if (linkChild.isURDFLink) {
-            return;
-          }
-
-          // Recursively handle visual elements within this link only
-          this._setMeshOpacity(linkChild, opacity);
-        });
-      }
-    });
-
-    this.redraw();
+    this._linkManager.setLinkOpacity(linkName, opacity);
   }
 
   /**
@@ -914,10 +499,15 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     const renderSize = this.getSize(new THREE.Vector2());
     this._camera.aspect = renderSize.width / renderSize.height;
     this._camera.updateProjectionMatrix();
+    this.clear();
+
     this.render(this._scene, this._camera);
+
     if (this._css2dRenderer) {
       this._css2dRenderer.render(this._scene, this._camera);
     }
+
+    this._axisIndicator.render(this, this._camera);
   }
 
   /**
@@ -948,10 +538,6 @@ export class URDFRenderer extends THREE.WebGLRenderer {
 
   dispose(): void {
     // ... existing dispose code
-    this._frameHelpers.clear();
-    if (this._axisIndicator) {
-      this._scene.remove(this._axisIndicator);
-    }
-    this._removeAxisIndicator();
+    this._axisIndicator.dispose();
   }
 }

From 221aa964e30a6a21848af178b59e54dfbf6bb068 Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Thu, 28 Aug 2025 16:08:11 +0200
Subject: [PATCH 4/8] Refactor scene code

---
 src/renderer.ts           | 280 +++-------------------------
 src/scene/sceneManager.ts | 372 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 399 insertions(+), 253 deletions(-)
 create mode 100644 src/scene/sceneManager.ts

diff --git a/src/renderer.ts b/src/renderer.ts
index 56e18b7..fd7f2c0 100644
--- a/src/renderer.ts
+++ b/src/renderer.ts
@@ -4,6 +4,7 @@ import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
 import { CSS2DRenderer } from 'three/examples/jsm/renderers/CSS2DRenderer.js';
 import { AxisIndicatorHelper } from './links/axisIndicator';
 import { LinkManager } from './links/linkManager';
+import { SceneManager } from './scene/sceneManager';
 
 import { URDFRobot } from 'urdf-loader';
 
@@ -21,16 +22,10 @@ import { URDFRobot } from 'urdf-loader';
  * URDFRenderer: a renderer to manage the view of a scene with a robot
  */
 export class URDFRenderer extends THREE.WebGLRenderer {
-  private _scene: THREE.Scene;
+  private _sceneManager: SceneManager;
   private _camera: THREE.PerspectiveCamera;
   private _controls: OrbitControls;
   private _css2dRenderer: CSS2DRenderer;
-  private _colorSky = new THREE.Color();
-  private _colorGround = new THREE.Color();
-  private _gridHeight = 0;
-  private _robotIndex = -1;
-  private _directionalLightHelper: THREE.DirectionalLightHelper | null = null;
-  private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null;
   private _axisIndicator: AxisIndicatorHelper;
   private _linkManager: LinkManager;
 
@@ -46,9 +41,6 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   ) {
     super({ antialias: true });
 
-    this._colorSky = colorSky;
-    this._colorGround = colorGround;
-
     // This is needed to render the axis indicator correctly
     this.autoClear = false;
 
@@ -58,8 +50,9 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this.shadowMap.enabled = true;
     this.shadowMap.type = THREE.PCFSoftShadowMap;
 
-    this._scene = new THREE.Scene();
-    this._initScene();
+    this._sceneManager = new SceneManager(colorSky, colorGround, () =>
+      this.redraw()
+    );
 
     this._camera = new THREE.PerspectiveCamera();
     this._initCamera();
@@ -73,12 +66,14 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._css2dRenderer.domElement.style.top = '0px';
     this._css2dRenderer.domElement.style.pointerEvents = 'none';
 
-    // Instantiate the new managers
+    // Instantiate the other managers
     this._axisIndicator = new AxisIndicatorHelper(
       this._camera,
       this.domElement
     );
-    this._linkManager = new LinkManager(this._scene, () => this.redraw());
+    this._linkManager = new LinkManager(this._sceneManager.scene, () =>
+      this.redraw()
+    );
   }
 
   /**
@@ -103,120 +98,13 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._controls.addEventListener('change', () => this.redraw());
   }
 
-  /**
-   * Initializes the scene
-   */
-  private _initScene(): void {
-    this._scene.background = this._colorSky;
-    this._scene.up = new THREE.Vector3(0, 0, 1); // Z is up
-    this._addGround();
-    this._addGrid();
-    this._addLights();
-  }
-
-  /**
-   * Adds a plane representing the ground to the scene
-   */
-  private _addGround(): void {
-    // TODO: fix shadows
-    const ground = new THREE.Mesh(
-      new THREE.PlaneGeometry(40, 40),
-      new THREE.ShadowMaterial({ opacity: 0.5 })
-    );
-    ground.rotation.x = -Math.PI / 2;
-    ground.scale.setScalar(30);
-    ground.receiveShadow = true;
-    this._scene.add(ground);
-  }
-
-  /**
-   * Adds a grid to the scene with ground color given to the constructor()
-   */
-  private _addGrid(): void {
-    const grid = new THREE.GridHelper(
-      50,
-      50,
-      this._colorGround,
-      this._colorGround
-    );
-    grid.receiveShadow = true;
-    this._scene.add(grid);
-  }
-
-  /**
-   * Adds three lights to the scene
-   */
-  private _addLights(): void {
-    // Directional light
-    const directionalLight = new THREE.DirectionalLight(0xfff2cc, 1.8);
-    directionalLight.castShadow = true;
-    directionalLight.position.set(3, 3, 3);
-    directionalLight.shadow.camera.top = 5;
-    directionalLight.shadow.camera.bottom = -5;
-    directionalLight.shadow.camera.left = -5;
-    directionalLight.shadow.camera.right = 5;
-    directionalLight.shadow.camera.near = 0.5;
-    directionalLight.shadow.camera.far = 50;
-    this._scene.add(directionalLight);
-
-    // Directional light helper
-    this._directionalLightHelper = new THREE.DirectionalLightHelper(
-      directionalLight,
-      2,
-      new THREE.Color(0x000000)
-    );
-    this._directionalLightHelper.visible = false;
-    this._scene.add(this._directionalLightHelper);
-
-    // Ambient light
-    const ambientLight = new THREE.AmbientLight(0x404040);
-    ambientLight.intensity = 0.1;
-    ambientLight.position.set(0, 5, 0);
-    this._scene.add(ambientLight);
-
-    // Hemisphere light
-    const hemisphereLight = new THREE.HemisphereLight(
-      0x8888ff, // cool sky
-      0x442200, // warm ground
-      0.4
-    );
-    this._scene.add(hemisphereLight);
-
-    // Hemisphere light helper
-    this._hemisphereLightHelper = new THREE.HemisphereLightHelper(
-      hemisphereLight,
-      2
-    );
-    this._hemisphereLightHelper.material.color.set(0x000000);
-    this._hemisphereLightHelper.visible = false; // Set to hidden by default
-    this._scene.add(this._hemisphereLightHelper);
-  }
-
-  /**
-   * Updates the hemisphere light to reflect the sky and ground colors
-   */
-  private _updateLights(): void {
-    const hemisphereLight = new THREE.HemisphereLight(
-      this._colorSky,
-      this._colorGround
-    );
-    hemisphereLight.intensity = 1;
-    const hemisphereIndex = this._scene.children
-      .map(i => i.type)
-      .indexOf('HemisphereLight');
-    this._scene.children[hemisphereIndex] = hemisphereLight;
-  }
-
   /**
    * Toggle the visibility of the directional light helper
    *
    * @param visible - Whether the helper should be visible
    */
   setDirectionalLightHelperVisibility(visible: boolean): void {
-    if (this._directionalLightHelper) {
-      this._directionalLightHelper.visible = visible;
-      this.redraw();
-    }
+    this._sceneManager.setDirectionalLightHelperVisibility(visible);
   }
 
   /**
@@ -225,10 +113,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param visible - Whether the helper should be visible
    */
   setHemisphereLightHelperVisibility(visible: boolean): void {
-    if (this._hemisphereLightHelper) {
-      this._hemisphereLightHelper.visible = visible;
-      this.redraw();
-    }
+    this._sceneManager.setHemisphereLightHelperVisibility(visible);
   }
 
   /**
@@ -241,22 +126,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     altitude: number,
     azimuth: number
   ): void {
-    const directionalLight = this._scene.children.find(
-      obj => obj.type === 'DirectionalLight'
-    ) as THREE.DirectionalLight;
-
-    if (directionalLight) {
-      const distance = 3;
-      const x = distance * Math.cos(altitude) * Math.cos(azimuth);
-      const z = distance * Math.cos(altitude) * Math.sin(azimuth);
-      const y = distance * Math.sin(altitude);
-
-      directionalLight.position.set(x, y, z);
-      if (this._directionalLightHelper) {
-        this._directionalLightHelper.update();
-      }
-      this.redraw();
-    }
+    this._sceneManager.setDirectionalLightPositionSpherical(altitude, azimuth);
   }
 
   /**
@@ -265,10 +135,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param newColor - The new background color as [R, G, B] array 0-255
    */
   setSkyColor(newColor: number[]): void {
-    this._colorSky = new THREE.Color(...newColor.map(x => x / 255));
-    this._scene.background = this._colorSky;
-    this._updateLights();
-    this.redraw();
+    this._sceneManager.setSkyColor(newColor);
   }
 
   /**
@@ -277,19 +144,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param newColor - The new background color as [R, G, B] array 0-255
    */
   setGroundColor(newColor: number[]): void {
-    this._colorGround = new THREE.Color(...newColor.map(x => x / 255));
-    const gridIndex = this._scene.children
-      .map(i => i.type)
-      .indexOf('GridHelper');
-    this._scene.children[gridIndex] = new THREE.GridHelper(
-      50,
-      50,
-      this._colorGround,
-      this._colorGround
-    );
-    this._updateLights();
-    this.setGridHeight(this._gridHeight);
-    this.redraw();
+    this._sceneManager.setGroundColor(newColor);
   }
 
   /**
@@ -298,12 +153,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param height - The height to shift the grid to
    */
   setGridHeight(height = 0): void {
-    const gridIndex = this._scene.children
-      .map(i => i.type)
-      .indexOf('GridHelper');
-    this._scene.children[gridIndex].position.y = height;
-    this._gridHeight = height;
-    this.redraw();
+    this._sceneManager.setGridHeight(height);
   }
 
   /**
@@ -312,20 +162,8 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param robot
    */
   setRobot(robot: URDFRobot): void {
-    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._sceneManager.setRobot(robot);
     this._linkManager.setRobot(robot);
-    this.redraw();
   }
 
   /**
@@ -336,17 +174,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param z - The new z position
    */
   setDirectionalLightPosition(x: number, y: number, z: number): void {
-    const directionalLight = this._scene.children.find(
-      obj => obj.type === 'DirectionalLight'
-    ) as THREE.DirectionalLight;
-
-    if (directionalLight) {
-      directionalLight.position.set(x, y, z);
-      if (this._directionalLightHelper) {
-        this._directionalLightHelper.update();
-      }
-      this.redraw();
-    }
+    this._sceneManager.setDirectionalLightPosition(x, y, z);
   }
 
   /**
@@ -355,14 +183,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param newColor - The new color as [R, G, B] array 0-255
    */
   setDirectionalLightColor(newColor: number[]): void {
-    const directionalLight = this._scene.children.find(
-      obj => obj.type === 'DirectionalLight'
-    ) as THREE.DirectionalLight;
-
-    if (directionalLight) {
-      directionalLight.color = new THREE.Color(...newColor.map(x => x / 255));
-      this.redraw();
-    }
+    this._sceneManager.setDirectionalLightColor(newColor);
   }
 
   /**
@@ -371,14 +192,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param intensity - The new intensity value
    */
   setDirectionalLightIntensity(intensity: number): void {
-    const directionalLight = this._scene.children.find(
-      obj => obj.type === 'DirectionalLight'
-    ) as THREE.DirectionalLight;
-
-    if (directionalLight) {
-      directionalLight.intensity = intensity;
-      this.redraw();
-    }
+    this._sceneManager.setDirectionalLightIntensity(intensity);
   }
 
   /**
@@ -387,14 +201,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param newColor - The new color as [R, G, B] array 0-255
    */
   setAmbientLightColor(newColor: number[]): void {
-    const ambientLight = this._scene.children.find(
-      obj => obj.type === 'AmbientLight'
-    ) as THREE.AmbientLight;
-
-    if (ambientLight) {
-      ambientLight.color = new THREE.Color(...newColor.map(x => x / 255));
-      this.redraw();
-    }
+    this._sceneManager.setAmbientLightColor(newColor);
   }
 
   /**
@@ -403,14 +210,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param intensity - The new intensity value
    */
   setAmbientLightIntensity(intensity: number): void {
-    const ambientLight = this._scene.children.find(
-      obj => obj.type === 'AmbientLight'
-    ) as THREE.AmbientLight;
-
-    if (ambientLight) {
-      ambientLight.intensity = intensity;
-      this.redraw();
-    }
+    this._sceneManager.setAmbientLightIntensity(intensity);
   }
 
   /**
@@ -419,14 +219,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param newColor - The new color as [R, G, B] array 0-255
    */
   setHemisphereLightSkyColor(newColor: number[]): void {
-    const hemisphereLight = this._scene.children.find(
-      obj => obj.type === 'HemisphereLight'
-    ) as THREE.HemisphereLight;
-
-    if (hemisphereLight) {
-      hemisphereLight.color = new THREE.Color(...newColor.map(x => x / 255));
-      this.redraw();
-    }
+    this._sceneManager.setHemisphereLightSkyColor(newColor);
   }
 
   /**
@@ -435,16 +228,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param newColor - The new color as [R, G, B] array 0-255
    */
   setHemisphereLightGroundColor(newColor: number[]): void {
-    const hemisphereLight = this._scene.children.find(
-      obj => obj.type === 'HemisphereLight'
-    ) as THREE.HemisphereLight;
-
-    if (hemisphereLight) {
-      hemisphereLight.groundColor = new THREE.Color(
-        ...newColor.map(x => x / 255)
-      );
-      this.redraw();
-    }
+    this._sceneManager.setHemisphereLightGroundColor(newColor);
   }
 
   /**
@@ -453,14 +237,7 @@ export class URDFRenderer extends THREE.WebGLRenderer {
    * @param intensity - The new intensity value
    */
   setHemisphereLightIntensity(intensity: number): void {
-    const hemisphereLight = this._scene.children.find(
-      obj => obj.type === 'HemisphereLight'
-    ) as THREE.HemisphereLight;
-
-    if (hemisphereLight) {
-      hemisphereLight.intensity = intensity;
-      this.redraw();
-    }
+    this._sceneManager.setHemisphereLightIntensity(intensity);
   }
 
   /**
@@ -501,10 +278,10 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._camera.updateProjectionMatrix();
     this.clear();
 
-    this.render(this._scene, this._camera);
+    this.render(this._sceneManager.scene, this._camera);
 
     if (this._css2dRenderer) {
-      this._css2dRenderer.render(this._scene, this._camera);
+      this._css2dRenderer.render(this._sceneManager.scene, this._camera);
     }
 
     this._axisIndicator.render(this, this._camera);
@@ -531,13 +308,10 @@ export class URDFRenderer extends THREE.WebGLRenderer {
   }
 
   getRobot(): URDFRobot | null {
-    return this._robotIndex !== -1
-      ? (this._scene.children[this._robotIndex] as URDFRobot)
-      : null;
+    return this._sceneManager.getRobot();
   }
 
   dispose(): void {
-    // ... existing dispose code
     this._axisIndicator.dispose();
   }
 }
diff --git a/src/scene/sceneManager.ts b/src/scene/sceneManager.ts
new file mode 100644
index 0000000..fb76334
--- /dev/null
+++ b/src/scene/sceneManager.ts
@@ -0,0 +1,372 @@
+import * as THREE from 'three';
+import { URDFRobot } from 'urdf-loader';
+
+/**
+ * Manages the THREE.js scene, including lights, ground, grid, and robot model.
+ */
+export class SceneManager {
+  public readonly scene: THREE.Scene;
+  private _colorSky: THREE.Color;
+  private _colorGround: THREE.Color;
+  private _gridHeight = 0;
+  private _robotIndex = -1;
+  private _directionalLightHelper: THREE.DirectionalLightHelper | null = null;
+  private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null;
+  private _redrawCallback: () => void;
+
+  constructor(
+    colorSky: THREE.Color,
+    colorGround: THREE.Color,
+    redrawCallback: () => void
+  ) {
+    this.scene = new THREE.Scene();
+    this._colorSky = colorSky;
+    this._colorGround = colorGround;
+    this._redrawCallback = redrawCallback;
+
+    this._initScene();
+  }
+
+  /**
+   * Initializes the scene
+   */
+  private _initScene(): void {
+    this.scene.background = this._colorSky;
+    this.scene.up = new THREE.Vector3(0, 0, 1); // Z is up
+    this._addGround();
+    this._addGrid();
+    this._addLights();
+  }
+
+  /**
+   * Adds a plane representing the ground to the scene
+   */
+  private _addGround(): void {
+    // TODO: fix shadows
+    const ground = new THREE.Mesh(
+      new THREE.PlaneGeometry(40, 40),
+      new THREE.ShadowMaterial({ opacity: 0.5 })
+    );
+    ground.rotation.x = -Math.PI / 2;
+    ground.scale.setScalar(30);
+    ground.receiveShadow = true;
+    this.scene.add(ground);
+  }
+
+  /**
+   * Adds a grid to the scene with ground color given to the constructor()
+   */
+  private _addGrid(): void {
+    const grid = new THREE.GridHelper(
+      50,
+      50,
+      this._colorGround,
+      this._colorGround
+    );
+    grid.receiveShadow = true;
+    this.scene.add(grid);
+  }
+
+  /**
+   * Adds three lights to the scene
+   */
+  private _addLights(): void {
+    // Directional light
+    const directionalLight = new THREE.DirectionalLight(0xfff2cc, 1.8);
+    directionalLight.castShadow = true;
+    directionalLight.position.set(3, 3, 3);
+    directionalLight.shadow.camera.top = 5;
+    directionalLight.shadow.camera.bottom = -5;
+    directionalLight.shadow.camera.left = -5;
+    directionalLight.shadow.camera.right = 5;
+    directionalLight.shadow.camera.near = 0.5;
+    directionalLight.shadow.camera.far = 50;
+    this.scene.add(directionalLight);
+
+    // Directional light helper
+    this._directionalLightHelper = new THREE.DirectionalLightHelper(
+      directionalLight,
+      2,
+      new THREE.Color(0x000000)
+    );
+    this._directionalLightHelper.visible = false;
+    this.scene.add(this._directionalLightHelper);
+
+    // Ambient light
+    const ambientLight = new THREE.AmbientLight(0x404040);
+    ambientLight.intensity = 0.1;
+    ambientLight.position.set(0, 5, 0);
+    this.scene.add(ambientLight);
+
+    // Hemisphere light
+    const hemisphereLight = new THREE.HemisphereLight(0x8888ff, 0x442200, 0.4);
+    this.scene.add(hemisphereLight);
+
+    // Hemisphere light helper
+    this._hemisphereLightHelper = new THREE.HemisphereLightHelper(
+      hemisphereLight,
+      2
+    );
+    this._hemisphereLightHelper.material.color.set(0x000000);
+    this._hemisphereLightHelper.visible = false;
+    this.scene.add(this._hemisphereLightHelper);
+  }
+
+  /**
+   * Updates the hemisphere light to reflect the sky and ground colors
+   */
+  private _updateLights(): void {
+    const hemisphereLight = new THREE.HemisphereLight(
+      this._colorSky,
+      this._colorGround
+    );
+    hemisphereLight.intensity = 1;
+    const hemisphereIndex = this.scene.children
+      .map(i => i.type)
+      .indexOf('HemisphereLight');
+    this.scene.children[hemisphereIndex] = hemisphereLight;
+  }
+
+  /**
+   * Toggle the visibility of the directional light helper
+   *
+   * @param visible - Whether the helper should be visible
+   */
+  public setDirectionalLightHelperVisibility(visible: boolean): void {
+    if (this._directionalLightHelper) {
+      this._directionalLightHelper.visible = visible;
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Toggle the visibility of the hemisphere light helper
+   *
+   * @param visible - Whether the helper should be visible
+   */
+  public setHemisphereLightHelperVisibility(visible: boolean): void {
+    if (this._hemisphereLightHelper) {
+      this._hemisphereLightHelper.visible = visible;
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the position of the directional light using spherical coordinates
+   *
+   * @param altitude - Angle in radians from the horizontal plane (elevation)
+   * @param azimuth - Angle in radians around the vertical axis
+   */
+  public setDirectionalLightPositionSpherical(
+    altitude: number,
+    azimuth: number
+  ): void {
+    const directionalLight = this.scene.children.find(
+      obj => obj.type === 'DirectionalLight'
+    ) as THREE.DirectionalLight;
+    if (directionalLight) {
+      const distance = 3;
+      const x = distance * Math.cos(altitude) * Math.cos(azimuth);
+      const z = distance * Math.cos(altitude) * Math.sin(azimuth);
+      const y = distance * Math.sin(altitude);
+      directionalLight.position.set(x, y, z);
+      if (this._directionalLightHelper) {
+        this._directionalLightHelper.update();
+      }
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Change the background color of the scene
+   *
+   * @param newColor - The new background color as [R, G, B] array 0-255
+   */
+  public setSkyColor(newColor: number[]): void {
+    this._colorSky = new THREE.Color(...newColor.map(x => x / 255));
+    this.scene.background = this._colorSky;
+    this._updateLights();
+    this._redrawCallback();
+  }
+
+  /**
+   * Change the grid color of the ground
+   *
+   * @param newColor - The new background color as [R, G, B] array 0-255
+   */
+  public setGroundColor(newColor: number[]): void {
+    this._colorGround = new THREE.Color(...newColor.map(x => x / 255));
+    const gridIndex = this.scene.children
+      .map(i => i.type)
+      .indexOf('GridHelper');
+    this.scene.children[gridIndex] = new THREE.GridHelper(
+      50,
+      50,
+      this._colorGround,
+      this._colorGround
+    );
+    this._updateLights();
+    this.setGridHeight(this._gridHeight);
+    this._redrawCallback();
+  }
+
+  /**
+   * Changes the height of the grid in the vertical axis (y-axis for three.js)
+   *
+   * @param height - The height to shift the grid to
+   */
+  public setGridHeight(height = 0): void {
+    const gridIndex = this.scene.children
+      .map(i => i.type)
+      .indexOf('GridHelper');
+    this.scene.children[gridIndex].position.y = height;
+    this._gridHeight = height;
+    this._redrawCallback();
+  }
+
+  /**
+   * Adds a robot to the scene or updates the existing robot
+   *
+   * @param robot
+   */
+  public setRobot(robot: URDFRobot): void {
+    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._redrawCallback();
+  }
+
+  public getRobot(): URDFRobot | null {
+    return this._robotIndex !== -1
+      ? (this.scene.children[this._robotIndex] as URDFRobot)
+      : null;
+  }
+
+  public setDirectionalLightPosition(x: number, y: number, z: number): void {
+    const directionalLight = this.scene.children.find(
+      obj => obj.type === 'DirectionalLight'
+    ) as THREE.DirectionalLight;
+    if (directionalLight) {
+      directionalLight.position.set(x, y, z);
+      if (this._directionalLightHelper) {
+        this._directionalLightHelper.update();
+      }
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the color of the directional light
+   *
+   * @param newColor - The new color as [R, G, B] array 0-255
+   */
+  public setDirectionalLightColor(newColor: number[]): void {
+    const directionalLight = this.scene.children.find(
+      obj => obj.type === 'DirectionalLight'
+    ) as THREE.DirectionalLight;
+    if (directionalLight) {
+      directionalLight.color = new THREE.Color(...newColor.map(x => x / 255));
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the intensity of the directional light
+   *
+   * @param intensity - The new intensity value
+   */
+  public setDirectionalLightIntensity(intensity: number): void {
+    const directionalLight = this.scene.children.find(
+      obj => obj.type === 'DirectionalLight'
+    ) as THREE.DirectionalLight;
+    if (directionalLight) {
+      directionalLight.intensity = intensity;
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the color of the ambient light
+   *
+   * @param newColor - The new color as [R, G, B] array 0-255
+   */
+  public setAmbientLightColor(newColor: number[]): void {
+    const ambientLight = this.scene.children.find(
+      obj => obj.type === 'AmbientLight'
+    ) as THREE.AmbientLight;
+    if (ambientLight) {
+      ambientLight.color = new THREE.Color(...newColor.map(x => x / 255));
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the intensity of the ambient light
+   *
+   * @param intensity - The new intensity value
+   */
+  public setAmbientLightIntensity(intensity: number): void {
+    const ambientLight = this.scene.children.find(
+      obj => obj.type === 'AmbientLight'
+    ) as THREE.AmbientLight;
+    if (ambientLight) {
+      ambientLight.intensity = intensity;
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the hemisphere light sky color
+   *
+   * @param newColor - The new color as [R, G, B] array 0-255
+   */
+  public setHemisphereLightSkyColor(newColor: number[]): void {
+    const hemisphereLight = this.scene.children.find(
+      obj => obj.type === 'HemisphereLight'
+    ) as THREE.HemisphereLight;
+    if (hemisphereLight) {
+      hemisphereLight.color = new THREE.Color(...newColor.map(x => x / 255));
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the hemisphere light ground color
+   *
+   * @param newColor - The new color as [R, G, B] array 0-255
+   */
+  public setHemisphereLightGroundColor(newColor: number[]): void {
+    const hemisphereLight = this.scene.children.find(
+      obj => obj.type === 'HemisphereLight'
+    ) as THREE.HemisphereLight;
+    if (hemisphereLight) {
+      hemisphereLight.groundColor = new THREE.Color(
+        ...newColor.map(x => x / 255)
+      );
+      this._redrawCallback();
+    }
+  }
+
+  /**
+   * Updates the hemisphere light intensity
+   *
+   * @param intensity - The new intensity value
+   */
+  public setHemisphereLightIntensity(intensity: number): void {
+    const hemisphereLight = this.scene.children.find(
+      obj => obj.type === 'HemisphereLight'
+    ) as THREE.HemisphereLight;
+    if (hemisphereLight) {
+      hemisphereLight.intensity = intensity;
+      this._redrawCallback();
+    }
+  }
+}

From b4aff0ee4a6840f0fc4dfb177fc645ecc0b318ac Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Thu, 28 Aug 2025 17:19:45 +0200
Subject: [PATCH 5/8] Fix link opacity

---
 src/layout.ts            | 34 +++++++++++---------
 src/links/linkManager.ts | 67 ++++++++++++++++++++++++++++++++--------
 2 files changed, 74 insertions(+), 27 deletions(-)

diff --git a/src/layout.ts b/src/layout.ts
index 8903f76..52a0d88 100644
--- a/src/layout.ts
+++ b/src/layout.ts
@@ -185,6 +185,7 @@ export class URDFLayout extends PanelLayout {
     this._setSceneControls();
     this._setJointControls();
     this._setLightControls();
+    this._setLinkControls();
     this._setEditorControls();
   }
 
@@ -219,7 +220,26 @@ export class URDFLayout extends PanelLayout {
     sceneControl.height.onChange((newHeight: number) =>
       this._renderer.setGridHeight(newHeight)
     );
+  }
+
+  /**
+   * Set callback for each joint when the value changes in the controls panel.
+   */
+  private _setJointControls(): void {
+    const jointControl = this._controlsPanel.createJointControls(
+      this._loader.robotModel.joints
+    );
+    Object.keys(jointControl).forEach((jointName: string) => {
+      jointControl[jointName].onChange((newValue = 0.0) => {
+        this._setJointValue(jointName, newValue);
+      });
+    });
+  }
 
+  /**
+   * Set callbacks for the link controls in the panel.
+   */
+  private _setLinkControls(): void {
     // Add link controls with link names
     const linkNames = Object.keys(this._loader.robotModel.links);
     const linkControls = this._controlsPanel.createLinkControls(linkNames);
@@ -263,20 +283,6 @@ export class URDFLayout extends PanelLayout {
     }
   }
 
-  /**
-   * Set callback for each joint when the value changes in the controls panel.
-   */
-  private _setJointControls(): void {
-    const jointControl = this._controlsPanel.createJointControls(
-      this._loader.robotModel.joints
-    );
-    Object.keys(jointControl).forEach((jointName: string) => {
-      jointControl[jointName].onChange((newValue = 0.0) => {
-        this._setJointValue(jointName, newValue);
-      });
-    });
-  }
-
   /**
    * Set callback for changing directional light position in the controls panel.
    */
diff --git a/src/links/linkManager.ts b/src/links/linkManager.ts
index 0deb86b..1f456af 100644
--- a/src/links/linkManager.ts
+++ b/src/links/linkManager.ts
@@ -153,29 +153,70 @@ export class LinkManager {
 
   /**
    * Helper method to recursively set opacity on meshes within a single link.
+   * This function clones materials to ensure that opacity changes on one link
+   * do not affect other links that might share the same material.
    */
-  private _setMeshOpacity(object: any, opacity: number): void {
+  private _setMeshOpacity(object: THREE.Object3D, opacity: number): void {
+    if ((object as any).isURDFLink) {
+      // Stop recursion if we encounter another link.
+      // This check is important if the initial call is not on a visual node.
+      return;
+    }
+
     if (object instanceof THREE.Mesh) {
-      const materials = Array.isArray(object.material)
-        ? object.material
-        : [object.material];
-      materials.forEach(material => {
-        if (material) {
-          if (opacity < 1.0) {
-            material.transparent = true;
-            material.depthWrite = false;
+      const mesh = object as THREE.Mesh;
+      const materials = Array.isArray(mesh.material)
+        ? mesh.material
+        : [mesh.material];
+
+      const newMaterials = materials.map(material => {
+        if (!material) {
+          return material;
+        }
+
+        // If we're making it transparent, clone the material to avoid side effects.
+        if (opacity < 1.0) {
+          if (!material.userData.isOpacityClone) {
+            const clonedMaterial = material.clone();
+            clonedMaterial.userData.isOpacityClone = true;
+            clonedMaterial.userData.originalMaterial = material;
+
+            clonedMaterial.transparent = true;
+            clonedMaterial.depthWrite = false;
+            clonedMaterial.opacity = opacity;
+            clonedMaterial.needsUpdate = true;
+            return clonedMaterial;
+          } else {
+            material.opacity = opacity;
+            material.needsUpdate = true;
+            return material;
+          }
+        } else {
+          if (
+            material.userData.isOpacityClone &&
+            material.userData.originalMaterial
+          ) {
+            return material.userData.originalMaterial;
           } else {
             material.transparent = false;
             material.depthWrite = true;
+            material.opacity = 1.0;
+            material.needsUpdate = true;
+            return material;
           }
-          material.opacity = opacity;
-          material.needsUpdate = true;
         }
       });
+
+      if (Array.isArray(mesh.material)) {
+        mesh.material = newMaterials;
+      } else {
+        mesh.material = newMaterials[0];
+      }
     }
 
-    object.children.forEach((child: any) => {
-      if (!child.isURDFLink) {
+    // Recurse through children, but stop if a child is another URDFLink
+    object.children.forEach(child => {
+      if (!(child as any).isURDFLink) {
         this._setMeshOpacity(child, opacity);
       }
     });

From bbee9d45fcc972c8b384230c1a0ea8045ca2f598 Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Tue, 2 Sep 2025 10:23:35 +0200
Subject: [PATCH 6/8] Fix frames for jointless robots

---
 src/links/linkManager.ts | 76 +++++++++++++++++++---------------------
 1 file changed, 37 insertions(+), 39 deletions(-)

diff --git a/src/links/linkManager.ts b/src/links/linkManager.ts
index 1f456af..fab7ebd 100644
--- a/src/links/linkManager.ts
+++ b/src/links/linkManager.ts
@@ -37,20 +37,18 @@ export class LinkManager {
 
     this._frameHelpers.children.forEach((frameGroup: any) => {
       const linkName = frameGroup.userData.linkName;
-      if (linkName) {
-        this._robot?.traverse((child: any) => {
-          if (child.isURDFLink && child.name === linkName) {
-            const worldPosition = new THREE.Vector3();
-            const worldQuaternion = new THREE.Quaternion();
-            child.matrixWorld.decompose(
-              worldPosition,
-              worldQuaternion,
-              new THREE.Vector3()
-            );
-            frameGroup.position.copy(worldPosition);
-            frameGroup.quaternion.copy(worldQuaternion);
-          }
-        });
+      const link = this._robot?.links[linkName];
+
+      if (link) {
+        const worldPosition = new THREE.Vector3();
+        const worldQuaternion = new THREE.Quaternion();
+        link.matrixWorld.decompose(
+          worldPosition,
+          worldQuaternion,
+          new THREE.Vector3()
+        );
+        frameGroup.position.copy(worldPosition);
+        frameGroup.quaternion.copy(worldQuaternion);
       }
     });
   }
@@ -75,25 +73,25 @@ export class LinkManager {
       return;
     }
 
-    this._robot.traverse((child: any) => {
-      if (child.isURDFLink && child.name === linkName) {
-        const axes = this._createCustomAxesHelper(size);
-        axes.userData.linkName = linkName;
+    const link = this._robot.links[linkName];
 
-        const worldPosition = new THREE.Vector3();
-        const worldQuaternion = new THREE.Quaternion();
-        child.matrixWorld.decompose(
-          worldPosition,
-          worldQuaternion,
-          new THREE.Vector3()
-        );
+    if (link) {
+      const axes = this._createCustomAxesHelper(size);
+      axes.userData.linkName = linkName;
 
-        axes.position.copy(worldPosition);
-        axes.quaternion.copy(worldQuaternion);
+      const worldPosition = new THREE.Vector3();
+      const worldQuaternion = new THREE.Quaternion();
+      link.matrixWorld.decompose(
+        worldPosition,
+        worldQuaternion,
+        new THREE.Vector3()
+      );
 
-        this._frameHelpers.add(axes);
-      }
-    });
+      axes.position.copy(worldPosition);
+      axes.quaternion.copy(worldQuaternion);
+
+      this._frameHelpers.add(axes);
+    }
 
     this._redrawCallback();
   }
@@ -106,15 +104,15 @@ export class LinkManager {
       return;
     }
 
-    this._robot.traverse((child: any) => {
-      if (child.isURDFLink && child.name === linkName) {
-        child.children.forEach((linkChild: any) => {
-          if (!linkChild.isURDFLink) {
-            this._setMeshOpacity(linkChild, opacity);
-          }
-        });
-      }
-    });
+    const link = this._robot.links[linkName];
+
+    if (link) {
+      link.children.forEach((linkChild: any) => {
+        if (!linkChild.isURDFLink) {
+          this._setMeshOpacity(linkChild, opacity);
+        }
+      });
+    }
 
     this._redrawCallback();
   }

From 04bfe1c46693d5642a34e4d0d8cb482f2c83fdb7 Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Mon, 8 Sep 2025 12:13:57 +0200
Subject: [PATCH 7/8] Fix jointless link mapping

---
 src/links/linkManager.ts | 199 ++++++++++++++++++++++-----------------
 1 file changed, 115 insertions(+), 84 deletions(-)

diff --git a/src/links/linkManager.ts b/src/links/linkManager.ts
index fab7ebd..3bbf895 100644
--- a/src/links/linkManager.ts
+++ b/src/links/linkManager.ts
@@ -1,5 +1,5 @@
 import * as THREE from 'three';
-import { URDFRobot } from 'urdf-loader';
+import { URDFRobot, URDFVisual } from 'urdf-loader';
 
 /**
  * Manages the visual representation of links
@@ -8,6 +8,8 @@ export class LinkManager {
   private _robot: URDFRobot | null = null;
   private _frameHelpers: THREE.Group;
   private _redrawCallback: () => void;
+  private _linkToMeshes: Map<string, THREE.Mesh[]> = new Map();
+  private _correctLinkMap: Map<string, THREE.Object3D> = new Map(); // Use base Object3D for flexibility
 
   constructor(scene: THREE.Scene, redrawCallback: () => void) {
     this._frameHelpers = new THREE.Group();
@@ -16,13 +18,22 @@ export class LinkManager {
   }
 
   /**
-   * Sets the current robot model for the manager to operate on.
-   * @param robot The URDFRobot model.
+   * Sets the current robot model, builds a correct map of all links,
+   * and then maps those links to their meshes.
    */
   public setRobot(robot: URDFRobot | null): void {
+    // If there's an old robot, remove it from the scene completely.
+    if (this._robot && this._robot.parent) {
+      this._robot.parent.remove(this._robot);
+    }
+
     this._robot = robot;
     this._frameHelpers.clear();
+    this._linkToMeshes.clear();
+    this._correctLinkMap.clear();
+
     if (robot) {
+      this._buildLinkAndMeshMaps(robot);
       this.updateAllFramePositions();
     }
   }
@@ -37,7 +48,7 @@ export class LinkManager {
 
     this._frameHelpers.children.forEach((frameGroup: any) => {
       const linkName = frameGroup.userData.linkName;
-      const link = this._robot?.links[linkName];
+      const link = this._correctLinkMap.get(linkName);
 
       if (link) {
         const worldPosition = new THREE.Vector3();
@@ -73,7 +84,7 @@ export class LinkManager {
       return;
     }
 
-    const link = this._robot.links[linkName];
+    const link = this._correctLinkMap.get(linkName);
 
     if (link) {
       const axes = this._createCustomAxesHelper(size);
@@ -97,22 +108,33 @@ export class LinkManager {
   }
 
   /**
-   * Sets the opacity of a specific link.
+   * Sets the opacity of a specific link using our custom mesh mapping.
    */
   public setLinkOpacity(linkName: string, opacity: number): void {
-    if (!this._robot) {
+    const meshes = this._linkToMeshes.get(linkName);
+    if (!meshes || meshes.length === 0) {
       return;
     }
 
-    const link = this._robot.links[linkName];
+    meshes.forEach(mesh => {
+      const materials = Array.isArray(mesh.material)
+        ? mesh.material
+        : [mesh.material];
 
-    if (link) {
-      link.children.forEach((linkChild: any) => {
-        if (!linkChild.isURDFLink) {
-          this._setMeshOpacity(linkChild, opacity);
+      materials.forEach(material => {
+        if (material) {
+          if (opacity < 1.0) {
+            material.transparent = true;
+            material.depthWrite = false;
+          } else {
+            material.transparent = false;
+            material.depthWrite = true;
+          }
+          material.opacity = opacity;
+          material.needsUpdate = true;
         }
       });
-    }
+    });
 
     this._redrawCallback();
   }
@@ -123,6 +145,86 @@ export class LinkManager {
   public dispose(): void {
     this._frameHelpers.clear();
     this._frameHelpers.parent?.remove(this._frameHelpers);
+    this._linkToMeshes.clear();
+    this._correctLinkMap.clear();
+  }
+
+  /**
+   * This builds the link and mesh maps by using the
+   * URDF's XML structure as the absolute ground truth.
+   */
+  private _buildLinkAndMeshMaps(robot: URDFRobot): void {
+    this._correctLinkMap.clear();
+    this._linkToMeshes.clear();
+
+    const rootXml = robot.urdfRobotNode;
+    if (!rootXml) {
+      return;
+    }
+
+    // Step 1: Build a  map from the <link> XML Element to the THREE.URDFVisual object.
+    // This is the bridge from the XML world to the THREE.js world.
+    const linkXmlToVisualMap = new Map<Element, URDFVisual>();
+    robot.traverse(node => {
+      if ((node as any).isURDFVisual) {
+        const visual = node as URDFVisual;
+        const visualXml = visual.urdfNode;
+        // The parent of a <visual> tag is its <link> tag.
+        if (visualXml && visualXml.parentElement) {
+          linkXmlToVisualMap.set(visualXml.parentElement, visual);
+        }
+      }
+    });
+
+    // Step 2: Get all <link> tags from the XML. This is our ground truth list of links.
+    const allLinkElements = rootXml.querySelectorAll('link');
+
+    // Step 3: Iterate through the ground truth list and populate our maps.
+    allLinkElements.forEach(linkElement => {
+      const linkName = linkElement.getAttribute('name');
+      if (!linkName) {
+        return;
+      }
+
+      const visual = linkXmlToVisualMap.get(linkElement);
+
+      if (visual) {
+        const meshes: THREE.Mesh[] = [];
+        visual.traverse(child => {
+          if (child instanceof THREE.Mesh) {
+            meshes.push(child);
+          }
+        });
+        this._linkToMeshes.set(linkName, meshes);
+
+        // Map the transform object.
+        if (visual.parent && visual.parent !== robot) {
+          // Jointed link: the parent is the distinct URDFLink object.
+          this._correctLinkMap.set(linkName, visual.parent);
+        } else {
+          // Jointless link: the visual itself is the best object representing the transform.
+          this._correctLinkMap.set(linkName, visual);
+        }
+      } else {
+        // This link has no visual component (like 'world').
+        this._linkToMeshes.set(linkName, []);
+        // The root URDFRobot object itself acts as the 'world' link.
+        if (linkName === 'world') {
+          this._correctLinkMap.set(linkName, robot);
+        }
+      }
+    });
+
+    // Step 4: Clone materials for all found meshes to ensure uniqueness.
+    for (const meshes of this._linkToMeshes.values()) {
+      meshes.forEach(mesh => {
+        if (Array.isArray(mesh.material)) {
+          mesh.material = mesh.material.map(mat => (mat ? mat.clone() : mat));
+        } else if (mesh.material) {
+          mesh.material = mesh.material.clone();
+        }
+      });
+    }
   }
 
   /**
@@ -148,75 +250,4 @@ export class LinkManager {
     axesGroup.add(xAxis, yAxis, zAxis);
     return axesGroup;
   }
-
-  /**
-   * Helper method to recursively set opacity on meshes within a single link.
-   * This function clones materials to ensure that opacity changes on one link
-   * do not affect other links that might share the same material.
-   */
-  private _setMeshOpacity(object: THREE.Object3D, opacity: number): void {
-    if ((object as any).isURDFLink) {
-      // Stop recursion if we encounter another link.
-      // This check is important if the initial call is not on a visual node.
-      return;
-    }
-
-    if (object instanceof THREE.Mesh) {
-      const mesh = object as THREE.Mesh;
-      const materials = Array.isArray(mesh.material)
-        ? mesh.material
-        : [mesh.material];
-
-      const newMaterials = materials.map(material => {
-        if (!material) {
-          return material;
-        }
-
-        // If we're making it transparent, clone the material to avoid side effects.
-        if (opacity < 1.0) {
-          if (!material.userData.isOpacityClone) {
-            const clonedMaterial = material.clone();
-            clonedMaterial.userData.isOpacityClone = true;
-            clonedMaterial.userData.originalMaterial = material;
-
-            clonedMaterial.transparent = true;
-            clonedMaterial.depthWrite = false;
-            clonedMaterial.opacity = opacity;
-            clonedMaterial.needsUpdate = true;
-            return clonedMaterial;
-          } else {
-            material.opacity = opacity;
-            material.needsUpdate = true;
-            return material;
-          }
-        } else {
-          if (
-            material.userData.isOpacityClone &&
-            material.userData.originalMaterial
-          ) {
-            return material.userData.originalMaterial;
-          } else {
-            material.transparent = false;
-            material.depthWrite = true;
-            material.opacity = 1.0;
-            material.needsUpdate = true;
-            return material;
-          }
-        }
-      });
-
-      if (Array.isArray(mesh.material)) {
-        mesh.material = newMaterials;
-      } else {
-        mesh.material = newMaterials[0];
-      }
-    }
-
-    // Recurse through children, but stop if a child is another URDFLink
-    object.children.forEach(child => {
-      if (!(child as any).isURDFLink) {
-        this._setMeshOpacity(child, opacity);
-      }
-    });
-  }
 }

From d53f503cc98a2759b5ce4d15abd38018d33b124b Mon Sep 17 00:00:00 2001
From: Yahiewi <yahiahenien@gmail.com>
Date: Wed, 10 Sep 2025 15:03:58 +0200
Subject: [PATCH 8/8] Fix link selection for Joints Editor

---
 src/layout.ts            | 29 +++++++++++++++--------------
 src/links/linkManager.ts | 32 +++++++++++++++++++++-----------
 src/renderer.ts          |  9 +++++++++
 3 files changed, 45 insertions(+), 25 deletions(-)

diff --git a/src/layout.ts b/src/layout.ts
index 52a0d88..4463a92 100644
--- a/src/layout.ts
+++ b/src/layout.ts
@@ -644,11 +644,11 @@ export class URDFLayout extends PanelLayout {
         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];
+        const linkObject = this._renderer.getLinkObject(linkName);
         this._selectedLinks.parent = { name: linkName, obj: linkObject };
-        this._interactionEditor.highlightLink(linkObject, 'parent');
+        if (linkObject) {
+          this._interactionEditor.highlightLink(linkObject, 'parent');
+        }
       }
       updateJointName();
     });
@@ -663,11 +663,11 @@ export class URDFLayout extends PanelLayout {
         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];
+        const linkObject = this._renderer.getLinkObject(linkName);
         this._selectedLinks.child = { name: linkName, obj: linkObject };
-        this._interactionEditor.highlightLink(linkObject, 'child');
+        if (linkObject) {
+          this._interactionEditor.highlightLink(linkObject, 'child');
+        }
       }
       updateJointName();
     });
@@ -703,24 +703,25 @@ export class URDFLayout extends PanelLayout {
     parentLink: string,
     childLink: string
   ): void {
+    this._interactionEditor.clearHighlights();
     if (parentLink !== 'none') {
-      const link = this._loader.robotModel.links[parentLink];
-      const linkObject = link?.children.find((c: any) => c.isURDFVisual)
-        ?.children[0];
+      const linkObject = this._renderer.getLinkObject(parentLink);
       this._selectedLinks.parent = { name: parentLink, obj: linkObject };
       if (linkObject) {
         this._interactionEditor.highlightLink(linkObject, 'parent');
       }
+    } else {
+      this._selectedLinks.parent = { name: null, obj: null };
     }
 
     if (childLink !== 'none') {
-      const link = this._loader.robotModel.links[childLink];
-      const linkObject = link?.children.find((c: any) => c.isURDFVisual)
-        ?.children[0];
+      const linkObject = this._renderer.getLinkObject(childLink);
       this._selectedLinks.child = { name: childLink, obj: linkObject };
       if (linkObject) {
         this._interactionEditor.highlightLink(linkObject, 'child');
       }
+    } else {
+      this._selectedLinks.child = { name: null, obj: null };
     }
   }
 
diff --git a/src/links/linkManager.ts b/src/links/linkManager.ts
index 3bbf895..b0e4dba 100644
--- a/src/links/linkManager.ts
+++ b/src/links/linkManager.ts
@@ -9,7 +9,7 @@ export class LinkManager {
   private _frameHelpers: THREE.Group;
   private _redrawCallback: () => void;
   private _linkToMeshes: Map<string, THREE.Mesh[]> = new Map();
-  private _correctLinkMap: Map<string, THREE.Object3D> = new Map(); // Use base Object3D for flexibility
+  private _correctLinkMap: Map<string, THREE.Object3D> = new Map();
 
   constructor(scene: THREE.Scene, redrawCallback: () => void) {
     this._frameHelpers = new THREE.Group();
@@ -139,6 +139,17 @@ export class LinkManager {
     this._redrawCallback();
   }
 
+  /**
+   * Retrieves the visual object for a given link name.
+   * @param linkName - The name of the link.
+   * @returns The THREE.Object3D associated with the link's visual, or null.
+   */
+  public getLinkObject(linkName: string): THREE.Object3D | null {
+    const meshes = this._linkToMeshes.get(linkName);
+    // Return the first mesh if it exists, otherwise null.
+    return meshes && meshes.length > 0 ? meshes[0] : null;
+  }
+
   /**
    * Disposes of managed resources.
    */
@@ -162,8 +173,7 @@ export class LinkManager {
       return;
     }
 
-    // Step 1: Build a  map from the <link> XML Element to the THREE.URDFVisual object.
-    // This is the bridge from the XML world to the THREE.js world.
+    // Step 1: Build a  map from the <link> XML Element to the THREE.URDFVisual object
     const linkXmlToVisualMap = new Map<Element, URDFVisual>();
     robot.traverse(node => {
       if ((node as any).isURDFVisual) {
@@ -176,10 +186,10 @@ export class LinkManager {
       }
     });
 
-    // Step 2: Get all <link> tags from the XML. This is our ground truth list of links.
+    // Step 2: Get all <link> tags from the XML
     const allLinkElements = rootXml.querySelectorAll('link');
 
-    // Step 3: Iterate through the ground truth list and populate our maps.
+    // Step 3: Iterate through the list and populate our maps
     allLinkElements.forEach(linkElement => {
       const linkName = linkElement.getAttribute('name');
       if (!linkName) {
@@ -199,23 +209,23 @@ export class LinkManager {
 
         // Map the transform object.
         if (visual.parent && visual.parent !== robot) {
-          // Jointed link: the parent is the distinct URDFLink object.
+          // Jointed link: the parent is the distinct URDFLink object
           this._correctLinkMap.set(linkName, visual.parent);
         } else {
-          // Jointless link: the visual itself is the best object representing the transform.
+          // Jointless link: the visual itself is the best object representing the transform
           this._correctLinkMap.set(linkName, visual);
         }
       } else {
-        // This link has no visual component (like 'world').
+        // This link has no visual component (like 'world')
         this._linkToMeshes.set(linkName, []);
-        // The root URDFRobot object itself acts as the 'world' link.
+        // The root URDFRobot object itself acts as the 'world' link
         if (linkName === 'world') {
           this._correctLinkMap.set(linkName, robot);
         }
       }
     });
 
-    // Step 4: Clone materials for all found meshes to ensure uniqueness.
+    // Step 4: Clone materials for all found meshes to ensure uniqueness
     for (const meshes of this._linkToMeshes.values()) {
       meshes.forEach(mesh => {
         if (Array.isArray(mesh.material)) {
@@ -228,7 +238,7 @@ export class LinkManager {
   }
 
   /**
-   * Creates a custom axes helper.
+   * Creates a custom axes helper
    */
   private _createCustomAxesHelper(size = 0.3): THREE.Group {
     const axesGroup = new THREE.Group();
diff --git a/src/renderer.ts b/src/renderer.ts
index fd7f2c0..af11939 100644
--- a/src/renderer.ts
+++ b/src/renderer.ts
@@ -269,6 +269,15 @@ export class URDFRenderer extends THREE.WebGLRenderer {
     this._linkManager.setLinkOpacity(linkName, opacity);
   }
 
+  /**
+   * Retrieves the visual object for a given link name.
+   * @param linkName - The name of the link.
+   * @returns The THREE.Object3D associated with the link's visual, or null.
+   */
+  getLinkObject(linkName: string): THREE.Object3D | null {
+    return this._linkManager.getLinkObject(linkName);
+  }
+
   /**
    * Refreshes the viewer by re-rendering the scene and its elements
    */