diff --git a/JointVisualiser.js b/JointVisualiser.js
new file mode 100644
index 0000000..8559e0c
--- /dev/null
+++ b/JointVisualiser.js
@@ -0,0 +1,72 @@
+import * as THREE from 'three';
+
+export function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) {
+ const motorMargin = 0.4;
+ const motorLower = lower - motorMargin;
+ const motorUpper = upper + motorMargin;
+
+ const redMaterial = new THREE.MeshBasicMaterial({ color: 0xff0000, transparent: true, opacity: 0.2, side: THREE.DoubleSide, depthTest: false, depthWrite: false });
+ const whiteMaterial = new THREE.MeshBasicMaterial({ color: 0xffffff, transparent: true, opacity: 0.6, side: THREE.DoubleSide, depthTest: false, depthWrite: false });
+
+ function createArc(start, end, material) {
+ const shape = new THREE.Shape();
+ shape.moveTo(0, 0);
+ for (let i = 0; i <= segments; i++) {
+ const angle = start + (end - start) * (i / segments);
+ shape.lineTo(Math.cos(angle) * radius, Math.sin(angle) * radius);
+ }
+ shape.lineTo(0, 0);
+ return new THREE.Mesh(new THREE.ShapeGeometry(shape), material);
+ }
+
+ const redLeft = createArc(motorLower, lower, redMaterial);
+ const whiteArc = createArc(lower, upper, whiteMaterial);
+ const redRight = createArc(upper, motorUpper, redMaterial);
+
+ const up = new THREE.Vector3(0, 0, 1);
+ const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
+ const basis = new THREE.Matrix4().makeRotationFromQuaternion(quaternion);
+
+ redLeft.applyMatrix4(basis);
+ whiteArc.applyMatrix4(basis);
+ redRight.applyMatrix4(basis);
+
+ const group = new THREE.Group();
+ group.add(redLeft, whiteArc, redRight);
+ return group;
+}
+
+export function createAngleIndicator(axis, angle, radius = 0.1) {
+ const dir = new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).multiplyScalar(radius);
+ const geometry = new THREE.BufferGeometry().setFromPoints([new THREE.Vector3(0, 0, 0), dir]);
+ const material = new THREE.LineBasicMaterial({ color: 0xffffff, depthTest: false, depthWrite: false });
+ const line = new THREE.Line(geometry, material);
+
+ const up = new THREE.Vector3(0, 0, 1);
+ const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
+ line.applyQuaternion(quaternion);
+
+ return line;
+}
+
+export function createJointLabel(name, angle) {
+ const canvas = document.createElement('canvas');
+ canvas.width = 256;
+ canvas.height = 64;
+ const ctx = canvas.getContext('2d');
+
+ ctx.fillStyle = 'rgba(0,0,0,0.6)';
+ ctx.fillRect(0, 0, canvas.width, canvas.height);
+
+ ctx.font = '20px monospace';
+ ctx.fillStyle = '#ffffff';
+ ctx.fillText(`${name}: ${(angle * 180 / Math.PI).toFixed(1)}°`, 10, 40);
+
+ const texture = new THREE.CanvasTexture(canvas);
+ const material = new THREE.SpriteMaterial({ map: texture, transparent: true });
+ const sprite = new THREE.Sprite(material);
+ sprite.scale.set(0.5, 0.125, 1); // adjust size
+
+ return sprite;
+}
+
diff --git a/URDFEditor.js b/URDFEditor.js
new file mode 100644
index 0000000..40ce24c
--- /dev/null
+++ b/URDFEditor.js
@@ -0,0 +1,269 @@
+import * as THREE from 'three';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
+import URDFLoader from './urdf/ExtendedURDFLoader.js';
+import { createRotationSector, createAngleIndicator, createJointLabel } from './JointVisualiser.js';
+import ExtendedURDFLoader from './urdf/ExtendedURDFLoader.js';
+
+export class URDFEditor {
+ constructor(canvas, urdfPath = './urdf/sample.urdf') {
+ this.canvas = canvas;
+ this.urdfPath = urdfPath;
+
+ this.scene = new THREE.Scene();
+ this.scene.background = new THREE.Color(0xaaaaaa);
+
+ this.camera = new THREE.PerspectiveCamera(45, canvas.clientWidth / canvas.clientHeight, 0.1, 100);
+ this.camera.position.set(2, 2, 2);
+ this.camera.lookAt(0, 0, 0);
+
+ this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
+ this.renderer.setSize(canvas.clientWidth, canvas.clientHeight);
+
+ this.overlayCanvas = document.getElementById('overlay-canvas');
+ this.overlayCtx = this.overlayCanvas.getContext('2d');
+
+ // Match size to renderer
+ const resizeOverlay = () => {
+ const { clientWidth, clientHeight } = this.renderer.domElement;
+ this.overlayCanvas.width = clientWidth;
+ this.overlayCanvas.height = clientHeight;
+ };
+ window.addEventListener('resize', resizeOverlay);
+ resizeOverlay();
+
+
+ this.controls = new OrbitControls(this.camera, this.renderer.domElement);
+ this.controls.target.set(0, 0.5, 0);
+ this.controls.update();
+
+ this.raycaster = new THREE.Raycaster();
+ this.mouse = new THREE.Vector2();
+
+ this.hoveredJoint = null;
+ this.draggedJoint = null;
+ this.worldAxis = null;
+ this.lastX = null;
+ this.isDragging = false;
+ this.jointAngles = {};
+
+ this.loader = new ExtendedURDFLoader();
+ this.loader.packages = { '': './urdf/' };
+ this.loader.parseVisual = true;
+ this.loader.parseCollision = false;
+
+ this.setupScene();
+ this.loadURDF();
+ this.setupEvents();
+ }
+
+ setupScene() {
+ this.scene.add(new THREE.AmbientLight(0xffffff, 0.6));
+ const light1 = new THREE.DirectionalLight(0xffffff, 0.8);
+ light1.position.set(15, 10, 7);
+ this.scene.add(light1);
+
+ const light2 = new THREE.DirectionalLight(0xffffff, 0.8);
+ light2.position.set(-15, 10, -7);
+ this.scene.add(light2);
+
+ this.scene.add(new THREE.AxesHelper(1));
+ this.scene.add(new THREE.GridHelper(2, 10));
+ }
+
+ async loadURDF() {
+ const urdfText = await fetch(this.urdfPath).then(res => res.text());
+ const robot = await this.loader.loadFromString(urdfText);
+
+ robot.rotation.x = -Math.PI / 2;
+ this.scene.add(robot);
+ this.robot = robot;
+
+ for (const jointName in this.robot.joints) {
+ console.log(jointName, this.robot.joints[jointName].transmission);
+ }
+
+ this.animate();
+ }
+
+
+
+ findObjectByName(root, name) {
+ let result = null;
+ root.traverse(child => {
+ if (child.name === name) {
+ result = child;
+ }
+ });
+ return result;
+ }
+
+
+ setupEvents() {
+ this.canvas.addEventListener('pointermove', this.onPointerMove.bind(this));
+ this.canvas.addEventListener('pointerdown', this.onPointerDown.bind(this));
+ this.canvas.addEventListener('pointerup', this.onPointerUp.bind(this));
+ }
+
+ onPointerMove(event) {
+ this.mouse.x = (event.clientX / this.canvas.clientWidth) * 2 - 1;
+ this.mouse.y = -(event.clientY / this.canvas.clientHeight) * 2 + 1;
+
+ this.raycaster.setFromCamera(this.mouse, this.camera);
+ const intersects = this.raycaster.intersectObjects(this.robot.children, true);
+
+ if (this.isDragging && this.draggedJoint && this.worldAxis) {
+ const deltaX = event.clientX - this.lastX;
+ this.lastX = event.clientX;
+ const angleDelta = deltaX * 0.005;
+
+ const jointName = this.draggedJoint.name;
+ const limits = this.robot.joints[jointName]?.limit;
+ const lower = limits?.lower ?? -Math.PI;
+ const upper = limits?.upper ?? Math.PI;
+
+ const currentAngle = this.jointAngles[jointName] ?? 0;
+ const proposedAngle = currentAngle + angleDelta;
+ const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
+ const actualDelta = clampedAngle - currentAngle;
+
+ this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta);
+ this.jointAngles[jointName] = clampedAngle;
+
+
+ const gizmo = this.draggedJoint.userData.gizmo;
+ if (gizmo?.indicator) this.draggedJoint.parent.remove(gizmo.indicator);
+
+ const newIndicator = createAngleIndicator(this.worldAxis, clampedAngle);
+ newIndicator.position.copy(this.draggedJoint.position);
+ this.draggedJoint.parent.add(newIndicator);
+ this.draggedJoint.userData.gizmo.indicator = newIndicator;
+ }
+
+ if (intersects.length > 0) {
+ const target = intersects[0].object;
+ if (this.hoveredJoint?.material?.emissive) this.hoveredJoint.material.emissive.setHex(0x000000);
+ this.hoveredJoint = target;
+ if (this.hoveredJoint.material?.emissive) this.hoveredJoint.material.emissive.setHex(0x333333);
+ } else {
+ if (this.hoveredJoint?.material?.emissive) this.hoveredJoint.material.emissive.setHex(0x000000);
+ this.hoveredJoint = null;
+ }
+ }
+
+ onPointerDown(event) {
+ if (!this.hoveredJoint) return;
+
+ this.isDragging = true;
+ this.lastX = event.clientX;
+ this.controls.enabled = false;
+
+ this.draggedJoint = this.findJointAncestor(this.hoveredJoint);
+ if (!this.draggedJoint) return;
+
+ const jointName = this.draggedJoint.name;
+ const jointData = this.robot.joints?.[jointName];
+ if (!(jointData?.axis instanceof THREE.Vector3)) return;
+
+ const axis = jointData.axis.clone();
+ if (axis.lengthSq() === 0) return;
+
+ this.worldAxis = axis.normalize();
+ const limits = jointData.limit;
+ const lower = limits?.lower ?? -Math.PI;
+ const upper = limits?.upper ?? Math.PI;
+
+ const sector = createRotationSector(this.worldAxis, lower, upper);
+ const indicator = createAngleIndicator(this.worldAxis, this.jointAngles[jointName] ?? 0);
+
+ sector.position.copy(this.draggedJoint.position);
+ indicator.position.copy(this.draggedJoint.position);
+
+ this.draggedJoint.parent.add(sector);
+ this.draggedJoint.parent.add(indicator);
+ this.draggedJoint.userData.gizmo = { sector, indicator };
+ }
+
+ onPointerUp() {
+ this.isDragging = false;
+ this.controls.enabled = true;
+
+ const gizmo = this.draggedJoint?.userData.gizmo;
+ if (gizmo) {
+ this.draggedJoint.parent.remove(gizmo.sector);
+ this.draggedJoint.parent.remove(gizmo.indicator);
+ }
+
+ this.draggedJoint = null;
+ this.worldAxis = null;
+ }
+
+ findJointAncestor(object) {
+ while (object && object.parent) {
+ if (object.type === 'URDFJoint') return object;
+ object = object.parent;
+ }
+ return null;
+ }
+
+ animate() {
+ requestAnimationFrame(() => this.animate());
+ this.drawJointOverlay(); // ✅ fixed overlay
+ //console.log(this.robot.joints)
+ this.renderer.render(this.scene, this.camera);
+ }
+
+
+
+ drawJointOverlay() {
+ const ctx = this.overlayCtx;
+ if (!ctx || !this.robot?.joints) return;
+
+ ctx.clearRect(0, 0, this.overlayCanvas.width, this.overlayCanvas.height);
+ ctx.fillStyle = 'rgba(0, 0, 0, 0.5)';
+ ctx.fillRect(0, 0, 300, this.overlayCanvas.height);
+
+ ctx.font = '14px monospace';
+ ctx.fillStyle = '#0f0';
+ ctx.textBaseline = 'top';
+
+ let y = 10;
+ const nameX = 10;
+ const angleX = 220; // adjust for spacing
+
+ for (const jointName in this.robot.joints) {
+ const jointData = this.robot.joints[jointName];
+ const jointObject = this.findObjectByName(this.robot, jointName);
+ if (!jointData || !jointObject) continue;
+
+ const angle = this.jointAngles?.[jointName] ?? 0;
+ const degrees = (angle * 180 / Math.PI).toFixed(1);
+
+ const rotatedChild = jointObject.children?.[0];
+ const childName = rotatedChild?.name || '(unnamed)';
+
+ ctx.fillText(childName, nameX, y);
+ const angleStr = `${degrees}°`;
+ const angleWidth = ctx.measureText(angleStr).width;
+ ctx.fillText(angleStr, angleX - angleWidth, y);
+
+ const t = jointData.transmission;
+ if (t) {
+ const actuatorAngleDeg = (degrees * t.gearRatio) + 4096/2;
+ const ticks = Math.round(actuatorAngleDeg);
+ const isValid = ticks >= t.encoderValidMin && ticks <= t.encoderValidMax;
+
+ //ctx.fillStyle = isValid ? '#0f0' : '#f00';
+ ctx.fillText(`${ticks}`, nameX + 250, y);
+
+
+ }
+
+ y += 18;
+ }
+ }
+
+
+
+
+
+}
\ No newline at end of file
diff --git a/index.html b/index.html
index c154517..bff5c68 100644
--- a/index.html
+++ b/index.html
@@ -27,7 +27,11 @@
Loading URDF...
-
+
+
+
+
+
diff --git a/script.js b/script.js
index f501d97..cd1e1e4 100644
--- a/script.js
+++ b/script.js
@@ -1,356 +1,4 @@
-import * as THREE from 'three';
-import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
-import URDFLoader from './urdf/URDFLoader.js';
-
-// Setup scene, camera, renderer
-const canvas = document.getElementById('urdf-canvas');
-const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
-renderer.setSize(window.innerWidth, window.innerHeight);
-
-const scene = new THREE.Scene();
-scene.background = new THREE.Color(0xaaaaaa);
-
-const camera = new THREE.PerspectiveCamera(45, window.innerWidth / window.innerHeight, 0.1, 100);
-camera.position.set(2, 2, 2);
-camera.lookAt(0, 0, 0);
-
-// Lights
-scene.add(new THREE.AmbientLight(0xffffff, 0.6));
-const directional = new THREE.DirectionalLight(0xffffff, 0.8);
-directional.position.set(15, 10, 7);
-const directional2 = new THREE.DirectionalLight(0xffffff, 0.8);
-directional2.position.set(-15, 10, -7);
-scene.add(directional2);
-
-// Controls
-const controls = new OrbitControls(camera, renderer.domElement);
-controls.target.set(0, 0.5, 0);
-controls.update();
-
-// Helpers
-scene.add(new THREE.AxesHelper(1));
-const gridHelper = new THREE.GridHelper(2, 10);
-//gridHelper.rotation.x = Math.PI / 2;
-gridHelper.position.z = 0;
-scene.add(gridHelper);
-
-// Raycasting
-const raycaster = new THREE.Raycaster();
-const mouse = new THREE.Vector2();
-let hoveredJoint = null;
-let isDragging = false;
-let lastX = null;
-let worldAxis = null;
-
-let draggedJoint = null;
-let arc = null;
-const jointAngles = {}; // key: jointName, value: current angle in radians
-
-
-
-
-function findJointAncestor(object) {
- while (object && object.parent) {
- if (object.type === 'URDFJoint') return object;
- object = object.parent;
- }
- return null;
-}
-
-function createRotationArc(axis, lower, upper, radius = 0.1, segments = 32) {
- const points = [];
- console.log(axis);
- console.log(axis.clone());
- // ✅ Ensure axis is normalized
- const normalizedAxis = axis.clone().normalize();
-
- // ✅ Find a perpendicular vector
- let perp = new THREE.Vector3(0, 1, 0);
- if (Math.abs(normalizedAxis.dot(perp)) > 0.99) {
- perp = new THREE.Vector3(1, 0, 0); // fallback if axis is nearly parallel to Y
- }
- const tangent = perp.cross(normalizedAxis).normalize();
-
- for (let i = 0; i <= segments; i++) {
- const angle = lower + (upper - lower) * (i / segments);
- const point = tangent.clone().applyAxisAngle(normalizedAxis, angle).multiplyScalar(radius);
- points.push(point);
- }
-
- const geometry = new THREE.BufferGeometry().setFromPoints(points);
- const material = new THREE.LineBasicMaterial({ color: 0xffff00 });
- return new THREE.Line(geometry, material);
-}
-
-function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) {
- const motorMargin = 0.4; // hardcoded extra range beyond joint limits
- const motorLower = lower - motorMargin;
- const motorUpper = upper + motorMargin;
-
- const redMaterial = new THREE.MeshBasicMaterial({
- color: 0xff0000,
- transparent: true,
- opacity: 0.2,
- side: THREE.DoubleSide,
- depthTest: false,
- depthWrite: false
- });
-
- const whiteMaterial = new THREE.MeshBasicMaterial({
- color: 0xffffff,
- transparent: true,
- opacity: 0.6,
- side: THREE.DoubleSide,
- depthTest: false,
- depthWrite: false
- });
-
- function createArc(start, end, material) {
- const shape = new THREE.Shape();
- shape.moveTo(0, 0);
- for (let i = 0; i <= segments; i++) {
- const angle = start + (end - start) * (i / segments);
- shape.lineTo(Math.cos(angle) * radius, Math.sin(angle) * radius);
- }
- shape.lineTo(0, 0);
- const geometry = new THREE.ShapeGeometry(shape);
- return new THREE.Mesh(geometry, material);
- }
-
- const redLeft = createArc(motorLower, lower, redMaterial);
- const whiteArc = createArc(lower, upper, whiteMaterial);
- const redRight = createArc(upper, motorUpper, redMaterial);
-
- // Orient all arcs to match the axis
- const basis = new THREE.Matrix4();
- const up = new THREE.Vector3(0, 0, 1);
- const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
- basis.makeRotationFromQuaternion(quaternion);
-
- redLeft.applyMatrix4(basis);
- whiteArc.applyMatrix4(basis);
- redRight.applyMatrix4(basis);
-
- const group = new THREE.Group();
- group.add(redLeft);
- group.add(whiteArc);
- group.add(redRight);
-
- return group;
-}
-
-
-function createAngleIndicator(axis, angle, radius = 0.1) {
- const dir = new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).multiplyScalar(radius);
-
- const geometry = new THREE.BufferGeometry().setFromPoints([
- new THREE.Vector3(0, 0, 0),
- dir,
- ]);
-
- const material = new THREE.LineBasicMaterial({
- color: 0xffffff, // white
- linewidth: 2, // note: linewidth only works in some renderers
- depthTest: false, // ✅ always on top
- depthWrite: false
- });
-
- const line = new THREE.Line(geometry, material);
-
- // Rotate into axis plane
- const up = new THREE.Vector3(0, 0, 1);
- const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
- line.applyQuaternion(quaternion);
-
- return line;
-}
-
-
-
-// Load URDF
-const loader = new URDFLoader();
-loader.packages = { '': './urdf/' };
-
-function findObjectByName(root, name) {
- let result = null;
- root.traverse(child => {
- if (child.name === name) result = child;
- });
- return result;
-}
-
-loader.load('./urdf/sample.urdf', robot => {
- //scene.rotation.x = -Math.PI / 2;
- robot.rotation.x = -Math.PI / 2;
- scene.add(robot);
- document.getElementById('status').textContent = 'URDF Loaded';
-
- loader.parseVisual = true;
- loader.parseCollision = false;
-
- // Add AxesHelpers to all meshes
- robot.traverse(obj => {
- if (obj.isMesh) obj.add(new THREE.AxesHelper(0.05));
- });
-
- // Setup pointer events
- canvas.addEventListener('pointermove', event => {
- mouse.x = (event.clientX / canvas.clientWidth) * 2 - 1;
- mouse.y = -(event.clientY / canvas.clientHeight) * 2 + 1;
-
- raycaster.setFromCamera(mouse, camera);
- const intersects = raycaster.intersectObjects(robot.children, true);
-
- if (isDragging && draggedJoint && worldAxis) {
- const deltaX = event.clientX - lastX;
- lastX = event.clientX;
- const angleDelta = deltaX * 0.005;
-
- const jointName = draggedJoint.name;
- const limits = robot.joints[jointName]?.limit;
- const lower = limits?.lower ?? -Math.PI;
- const upper = limits?.upper ?? Math.PI;
-
- const currentAngle = jointAngles[jointName] ?? 0;
- const proposedAngle = currentAngle + angleDelta;
-
- // ✅ Clamp to limits
- const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
- const actualDelta = clampedAngle - currentAngle;
-
- // ✅ Apply rotation
- draggedJoint.rotateOnAxis(worldAxis, actualDelta);
- jointAngles[jointName] = clampedAngle;
-
-
- // Remove old indicator
- if (draggedJoint.userData.gizmo?.indicator) {
- draggedJoint.parent.remove(draggedJoint.userData.gizmo.indicator);
- }
-
- // Add updated indicator
- const newIndicator = createAngleIndicator(worldAxis, jointAngles[jointName]);
- newIndicator.position.copy(draggedJoint.position);
- draggedJoint.parent.add(newIndicator);
- draggedJoint.userData.gizmo.indicator = newIndicator;
-
- }
-
-
-
- if (intersects.length > 0) {
- const target = intersects[0].object;
-
- // Reset previous highlight
- if (
- hoveredJoint &&
- hoveredJoint.material &&
- 'emissive' in hoveredJoint.material &&
- typeof hoveredJoint.material.emissive.setHex === 'function'
- ) {
- hoveredJoint.material.emissive.setHex(0x000000);
- }
-
- hoveredJoint = target;
-
- // Highlight new joint
- if (
- hoveredJoint.material &&
- 'emissive' in hoveredJoint.material &&
- typeof hoveredJoint.material.emissive.setHex === 'function'
- ) {
- hoveredJoint.material.emissive.setHex(0x333333);
- }
-
- } else {
- if (
- hoveredJoint &&
- hoveredJoint.material &&
- 'emissive' in hoveredJoint.material &&
- typeof hoveredJoint.material.emissive.setHex === 'function'
- ) {
- hoveredJoint.material.emissive.setHex(0x000000);
- }
- hoveredJoint = null;
- }
-
- });
-
- canvas.addEventListener('pointerdown', event => {
- if (!hoveredJoint) return;
-
- isDragging = true;
- lastX = event.clientX;
- controls.enabled = false;
-
- draggedJoint = findJointAncestor(hoveredJoint);
-
-
- if (!draggedJoint) return;
-
- const jointName = draggedJoint.name;
- const jointData = robot.joints?.[jointName];
-
- // ✅ Check if axis is a valid THREE.Vector3
- if (!(jointData?.axis instanceof THREE.Vector3)) {
- console.warn(`Invalid axis for joint "${jointName}":`, jointData?.axis);
- return;
- }
-
- const urdfAxis = jointData.axis.clone();
- if (urdfAxis.lengthSq() === 0) {
- console.warn(`Zero-length axis for joint "${jointName}"`);
- return;
- }
-
- worldAxis = urdfAxis.normalize();
- const limits = robot.joints[jointName]?.limit;
- const lower = limits?.lower ?? -Math.PI;
- const upper = limits?.upper ?? Math.PI;
-
- const sector = createRotationSector(worldAxis, lower, upper);
- const indicator = createAngleIndicator(worldAxis, jointAngles[jointName] ?? 0);
-
- sector.position.copy(draggedJoint.position);
- indicator.position.copy(draggedJoint.position);
-
- draggedJoint.parent.add(sector);
- draggedJoint.parent.add(indicator);
-
- // Store for cleanup
- draggedJoint.userData.gizmo = { sector, indicator };
-
-
-
-
- console.log(`Dragging joint "${jointName}" on axis`, worldAxis.toArray());
- });
-
-
-
-
-
-
- canvas.addEventListener('pointerup', () => {
- isDragging = false;
- controls.enabled = true;
- const gizmo = draggedJoint?.userData.gizmo;
- if (gizmo) {
- draggedJoint.parent.remove(gizmo.sector);
- draggedJoint.parent.remove(gizmo.indicator);
- }
-
-
- draggedJoint = null;
- worldAxis = null;
- });
-
-
- animate();
-});
-
-function animate() {
- requestAnimationFrame(animate);
- renderer.render(scene, camera);
-}
-
+import { URDFEditor } from './URDFEditor.js';
+
+const canvas = document.getElementById('urdfCanvas');
+const editor = new URDFEditor(canvas, './urdf/sample.urdf');
diff --git a/script_old.js b/script_old.js
new file mode 100644
index 0000000..f501d97
--- /dev/null
+++ b/script_old.js
@@ -0,0 +1,356 @@
+import * as THREE from 'three';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
+import URDFLoader from './urdf/URDFLoader.js';
+
+// Setup scene, camera, renderer
+const canvas = document.getElementById('urdf-canvas');
+const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
+renderer.setSize(window.innerWidth, window.innerHeight);
+
+const scene = new THREE.Scene();
+scene.background = new THREE.Color(0xaaaaaa);
+
+const camera = new THREE.PerspectiveCamera(45, window.innerWidth / window.innerHeight, 0.1, 100);
+camera.position.set(2, 2, 2);
+camera.lookAt(0, 0, 0);
+
+// Lights
+scene.add(new THREE.AmbientLight(0xffffff, 0.6));
+const directional = new THREE.DirectionalLight(0xffffff, 0.8);
+directional.position.set(15, 10, 7);
+const directional2 = new THREE.DirectionalLight(0xffffff, 0.8);
+directional2.position.set(-15, 10, -7);
+scene.add(directional2);
+
+// Controls
+const controls = new OrbitControls(camera, renderer.domElement);
+controls.target.set(0, 0.5, 0);
+controls.update();
+
+// Helpers
+scene.add(new THREE.AxesHelper(1));
+const gridHelper = new THREE.GridHelper(2, 10);
+//gridHelper.rotation.x = Math.PI / 2;
+gridHelper.position.z = 0;
+scene.add(gridHelper);
+
+// Raycasting
+const raycaster = new THREE.Raycaster();
+const mouse = new THREE.Vector2();
+let hoveredJoint = null;
+let isDragging = false;
+let lastX = null;
+let worldAxis = null;
+
+let draggedJoint = null;
+let arc = null;
+const jointAngles = {}; // key: jointName, value: current angle in radians
+
+
+
+
+function findJointAncestor(object) {
+ while (object && object.parent) {
+ if (object.type === 'URDFJoint') return object;
+ object = object.parent;
+ }
+ return null;
+}
+
+function createRotationArc(axis, lower, upper, radius = 0.1, segments = 32) {
+ const points = [];
+ console.log(axis);
+ console.log(axis.clone());
+ // ✅ Ensure axis is normalized
+ const normalizedAxis = axis.clone().normalize();
+
+ // ✅ Find a perpendicular vector
+ let perp = new THREE.Vector3(0, 1, 0);
+ if (Math.abs(normalizedAxis.dot(perp)) > 0.99) {
+ perp = new THREE.Vector3(1, 0, 0); // fallback if axis is nearly parallel to Y
+ }
+ const tangent = perp.cross(normalizedAxis).normalize();
+
+ for (let i = 0; i <= segments; i++) {
+ const angle = lower + (upper - lower) * (i / segments);
+ const point = tangent.clone().applyAxisAngle(normalizedAxis, angle).multiplyScalar(radius);
+ points.push(point);
+ }
+
+ const geometry = new THREE.BufferGeometry().setFromPoints(points);
+ const material = new THREE.LineBasicMaterial({ color: 0xffff00 });
+ return new THREE.Line(geometry, material);
+}
+
+function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) {
+ const motorMargin = 0.4; // hardcoded extra range beyond joint limits
+ const motorLower = lower - motorMargin;
+ const motorUpper = upper + motorMargin;
+
+ const redMaterial = new THREE.MeshBasicMaterial({
+ color: 0xff0000,
+ transparent: true,
+ opacity: 0.2,
+ side: THREE.DoubleSide,
+ depthTest: false,
+ depthWrite: false
+ });
+
+ const whiteMaterial = new THREE.MeshBasicMaterial({
+ color: 0xffffff,
+ transparent: true,
+ opacity: 0.6,
+ side: THREE.DoubleSide,
+ depthTest: false,
+ depthWrite: false
+ });
+
+ function createArc(start, end, material) {
+ const shape = new THREE.Shape();
+ shape.moveTo(0, 0);
+ for (let i = 0; i <= segments; i++) {
+ const angle = start + (end - start) * (i / segments);
+ shape.lineTo(Math.cos(angle) * radius, Math.sin(angle) * radius);
+ }
+ shape.lineTo(0, 0);
+ const geometry = new THREE.ShapeGeometry(shape);
+ return new THREE.Mesh(geometry, material);
+ }
+
+ const redLeft = createArc(motorLower, lower, redMaterial);
+ const whiteArc = createArc(lower, upper, whiteMaterial);
+ const redRight = createArc(upper, motorUpper, redMaterial);
+
+ // Orient all arcs to match the axis
+ const basis = new THREE.Matrix4();
+ const up = new THREE.Vector3(0, 0, 1);
+ const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
+ basis.makeRotationFromQuaternion(quaternion);
+
+ redLeft.applyMatrix4(basis);
+ whiteArc.applyMatrix4(basis);
+ redRight.applyMatrix4(basis);
+
+ const group = new THREE.Group();
+ group.add(redLeft);
+ group.add(whiteArc);
+ group.add(redRight);
+
+ return group;
+}
+
+
+function createAngleIndicator(axis, angle, radius = 0.1) {
+ const dir = new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).multiplyScalar(radius);
+
+ const geometry = new THREE.BufferGeometry().setFromPoints([
+ new THREE.Vector3(0, 0, 0),
+ dir,
+ ]);
+
+ const material = new THREE.LineBasicMaterial({
+ color: 0xffffff, // white
+ linewidth: 2, // note: linewidth only works in some renderers
+ depthTest: false, // ✅ always on top
+ depthWrite: false
+ });
+
+ const line = new THREE.Line(geometry, material);
+
+ // Rotate into axis plane
+ const up = new THREE.Vector3(0, 0, 1);
+ const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
+ line.applyQuaternion(quaternion);
+
+ return line;
+}
+
+
+
+// Load URDF
+const loader = new URDFLoader();
+loader.packages = { '': './urdf/' };
+
+function findObjectByName(root, name) {
+ let result = null;
+ root.traverse(child => {
+ if (child.name === name) result = child;
+ });
+ return result;
+}
+
+loader.load('./urdf/sample.urdf', robot => {
+ //scene.rotation.x = -Math.PI / 2;
+ robot.rotation.x = -Math.PI / 2;
+ scene.add(robot);
+ document.getElementById('status').textContent = 'URDF Loaded';
+
+ loader.parseVisual = true;
+ loader.parseCollision = false;
+
+ // Add AxesHelpers to all meshes
+ robot.traverse(obj => {
+ if (obj.isMesh) obj.add(new THREE.AxesHelper(0.05));
+ });
+
+ // Setup pointer events
+ canvas.addEventListener('pointermove', event => {
+ mouse.x = (event.clientX / canvas.clientWidth) * 2 - 1;
+ mouse.y = -(event.clientY / canvas.clientHeight) * 2 + 1;
+
+ raycaster.setFromCamera(mouse, camera);
+ const intersects = raycaster.intersectObjects(robot.children, true);
+
+ if (isDragging && draggedJoint && worldAxis) {
+ const deltaX = event.clientX - lastX;
+ lastX = event.clientX;
+ const angleDelta = deltaX * 0.005;
+
+ const jointName = draggedJoint.name;
+ const limits = robot.joints[jointName]?.limit;
+ const lower = limits?.lower ?? -Math.PI;
+ const upper = limits?.upper ?? Math.PI;
+
+ const currentAngle = jointAngles[jointName] ?? 0;
+ const proposedAngle = currentAngle + angleDelta;
+
+ // ✅ Clamp to limits
+ const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
+ const actualDelta = clampedAngle - currentAngle;
+
+ // ✅ Apply rotation
+ draggedJoint.rotateOnAxis(worldAxis, actualDelta);
+ jointAngles[jointName] = clampedAngle;
+
+
+ // Remove old indicator
+ if (draggedJoint.userData.gizmo?.indicator) {
+ draggedJoint.parent.remove(draggedJoint.userData.gizmo.indicator);
+ }
+
+ // Add updated indicator
+ const newIndicator = createAngleIndicator(worldAxis, jointAngles[jointName]);
+ newIndicator.position.copy(draggedJoint.position);
+ draggedJoint.parent.add(newIndicator);
+ draggedJoint.userData.gizmo.indicator = newIndicator;
+
+ }
+
+
+
+ if (intersects.length > 0) {
+ const target = intersects[0].object;
+
+ // Reset previous highlight
+ if (
+ hoveredJoint &&
+ hoveredJoint.material &&
+ 'emissive' in hoveredJoint.material &&
+ typeof hoveredJoint.material.emissive.setHex === 'function'
+ ) {
+ hoveredJoint.material.emissive.setHex(0x000000);
+ }
+
+ hoveredJoint = target;
+
+ // Highlight new joint
+ if (
+ hoveredJoint.material &&
+ 'emissive' in hoveredJoint.material &&
+ typeof hoveredJoint.material.emissive.setHex === 'function'
+ ) {
+ hoveredJoint.material.emissive.setHex(0x333333);
+ }
+
+ } else {
+ if (
+ hoveredJoint &&
+ hoveredJoint.material &&
+ 'emissive' in hoveredJoint.material &&
+ typeof hoveredJoint.material.emissive.setHex === 'function'
+ ) {
+ hoveredJoint.material.emissive.setHex(0x000000);
+ }
+ hoveredJoint = null;
+ }
+
+ });
+
+ canvas.addEventListener('pointerdown', event => {
+ if (!hoveredJoint) return;
+
+ isDragging = true;
+ lastX = event.clientX;
+ controls.enabled = false;
+
+ draggedJoint = findJointAncestor(hoveredJoint);
+
+
+ if (!draggedJoint) return;
+
+ const jointName = draggedJoint.name;
+ const jointData = robot.joints?.[jointName];
+
+ // ✅ Check if axis is a valid THREE.Vector3
+ if (!(jointData?.axis instanceof THREE.Vector3)) {
+ console.warn(`Invalid axis for joint "${jointName}":`, jointData?.axis);
+ return;
+ }
+
+ const urdfAxis = jointData.axis.clone();
+ if (urdfAxis.lengthSq() === 0) {
+ console.warn(`Zero-length axis for joint "${jointName}"`);
+ return;
+ }
+
+ worldAxis = urdfAxis.normalize();
+ const limits = robot.joints[jointName]?.limit;
+ const lower = limits?.lower ?? -Math.PI;
+ const upper = limits?.upper ?? Math.PI;
+
+ const sector = createRotationSector(worldAxis, lower, upper);
+ const indicator = createAngleIndicator(worldAxis, jointAngles[jointName] ?? 0);
+
+ sector.position.copy(draggedJoint.position);
+ indicator.position.copy(draggedJoint.position);
+
+ draggedJoint.parent.add(sector);
+ draggedJoint.parent.add(indicator);
+
+ // Store for cleanup
+ draggedJoint.userData.gizmo = { sector, indicator };
+
+
+
+
+ console.log(`Dragging joint "${jointName}" on axis`, worldAxis.toArray());
+ });
+
+
+
+
+
+
+ canvas.addEventListener('pointerup', () => {
+ isDragging = false;
+ controls.enabled = true;
+ const gizmo = draggedJoint?.userData.gizmo;
+ if (gizmo) {
+ draggedJoint.parent.remove(gizmo.sector);
+ draggedJoint.parent.remove(gizmo.indicator);
+ }
+
+
+ draggedJoint = null;
+ worldAxis = null;
+ });
+
+
+ animate();
+});
+
+function animate() {
+ requestAnimationFrame(animate);
+ renderer.render(scene, camera);
+}
+
diff --git a/urdf/ExtendedURDFLoader.js b/urdf/ExtendedURDFLoader.js
new file mode 100644
index 0000000..73c7e4c
--- /dev/null
+++ b/urdf/ExtendedURDFLoader.js
@@ -0,0 +1,55 @@
+import URDFLoader from './URDFLoader.js';
+
+export default
+class ExtendedURDFLoader extends URDFLoader {
+ constructor(manager) {
+ super(manager);
+ this.transmissionMap = {};
+ }
+
+ parseTransmissions(xml) {
+ const transmissions = xml.querySelectorAll('transmission');
+ this.transmissionMap = {};
+
+ transmissions.forEach(trans => {
+ const jointName = trans.querySelector('joint')?.getAttribute('name');
+ const actuator = trans.querySelector('actuator');
+ const reduction = parseFloat(actuator?.querySelector('mechanicalReduction')?.textContent ?? '1');
+ const actuatorName = actuator?.getAttribute('name') ?? null;
+ const encoderMax = parseInt(actuator?.querySelector('encoderTicks')?.textContent ?? '4096');
+ const encoderValidMin = parseInt(actuator?.querySelector('encoderValidMin')?.textContent ?? '0');
+ const encoderValidMax = parseInt(actuator?.querySelector('encoderValidMax')?.textContent ?? '4095');
+
+ if (jointName) {
+ this.transmissionMap[jointName] = {
+ gearRatio: reduction,
+ actuatorName,
+ encoderMax,
+ encoderValidMin,
+ encoderValidMax
+ };
+ }
+ });
+ }
+
+ async loadFromString(urdfText, options = {}) {
+ const xml = new DOMParser().parseFromString(urdfText, 'application/xml');
+ const parserError = xml.querySelector('parsererror');
+ if (parserError) {
+ console.error('❌ XML parser error:', parserError.textContent);
+ throw new Error('Invalid URDF XML: ' + parserError.textContent);
+ }
+
+ this.parseTransmissions(xml);
+
+ const robot = this.parse(xml); // ✅ Use inherited parse() directly
+
+ for (const jointName in robot.joints) {
+ if (this.transmissionMap[jointName]) {
+ robot.joints[jointName].transmission = this.transmissionMap[jointName];
+ }
+ }
+
+ return robot;
+ }
+}
diff --git a/urdf/sample.urdf b/urdf/sample.urdf
index b474270..aff6df7 100644
--- a/urdf/sample.urdf
+++ b/urdf/sample.urdf
@@ -504,4 +504,19 @@
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ 22.75
+ PositionJointInterface
+ 4096
+ 200
+ 3500
+
+
\ No newline at end of file