diff --git a/ros_robot_visualiser b/ros_robot_visualiser deleted file mode 160000 index 40905cb..0000000 --- a/ros_robot_visualiser +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 40905cbb36a784c7f86bd5966418d263e8143525 diff --git a/ros_robot_visualiser/JointVisualiser.js b/ros_robot_visualiser/JointVisualiser.js new file mode 100644 index 0000000..0ad7f42 --- /dev/null +++ b/ros_robot_visualiser/JointVisualiser.js @@ -0,0 +1,72 @@ +import * as THREE from 'three'; + +export function createRotationSector(axis, lower, upper, encoderRange, radius = 0.1, segments = 32) { + const motorMargin = 0.4; + const motorLower = -encoderRange/2 * (Math.PI / 180);// -Math.PI/2;//lower - motorMargin; + const motorUpper = encoderRange/2 * (Math.PI / 180);//Math.PI/2;//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/ros_robot_visualiser/URDFEditor.js b/ros_robot_visualiser/URDFEditor.js new file mode 100644 index 0000000..9cbc85a --- /dev/null +++ b/ros_robot_visualiser/URDFEditor.js @@ -0,0 +1,468 @@ +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'; +import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader.js'; +import { MTLLoader } from 'three/examples/jsm/loaders/MTLLoader.js'; +import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js'; +import { ViewerOverlay } from './ViewerOverlay.js'; +const gltfLoader = new GLTFLoader(); + + +export class URDFEditor { + constructor(canvas) { + this.canvas = canvas; + console.log(canvas); + this.scene = new THREE.Scene(); + this.scene.background = new THREE.Color(0xaaaaaa); + + this.camera = new THREE.PerspectiveCamera(60, canvas.clientWidth / canvas.clientHeight, 0.1, 100); + this.camera.position.set(-0.3, 0.5, -0.7); + this.camera.lookAt(0, 0, 0); + + this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true }); + this.renderer.setSize(canvas.clientWidth, canvas.clientHeight); + + + + this.controls = new OrbitControls(this.camera, this.renderer.domElement); + this.controls.target.set(0, 0.1, 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.loadMeshCb = (path, manager, onComplete) => { + if (path.endsWith('.glb') || path.endsWith('.gltf')) { + const gltfLoader = new GLTFLoader(manager); + gltfLoader.load(path, (gltf) => { + onComplete(gltf.scene); + }); + } else { + // fall back to default behavior (STL, DAE) + URDFLoader.prototype.loadMeshCb.call(this.loader, path, manager, onComplete); + } + }; + + this.loader.packages = { + 'Little_Sophia_Face': '/robots/LittleSophia' + }; + this.loader.parseVisual = true; + this.loader.parseCollision = false; + + + this.setupScene(); + //this.loadURDF(); + this.loadURDFFromIndexedDB(); + this.setupEvents(); + + this.overlay = new ViewerOverlay( + this.renderer, + this.robot, + this.jointAngles, + this.findObjectByName.bind(this), + this.saveURDFToIndexedDB.bind(this), + this.loadURDFFromIndexedDB.bind(this) + ); + + + // Add some lights (optional if using MeshBasicMaterial) + const ambient = new THREE.AmbientLight(0x404040); + this.scene.add(ambient); + const dirLight = new THREE.DirectionalLight(0xffffff, 1); + dirLight.position.set(5, 5, 5); + this.scene.add(dirLight); + + // this.mesh = null; + // gltfLoader.load('meshes/Little_Sophia.glb', (gltf) => { + // const model = gltf.scene; + // model.scale.set(0.001, 0.001, 0.001); // adjust if needed + // model.rotation.y = Math.PI; // radians (180°) + // this.scene.add(model); + + // // Optional: fit camera to model + // const box = new THREE.Box3().setFromObject(model); + // const center = box.getCenter(new THREE.Vector3()); + // const size = box.getSize(new THREE.Vector3()).length(); + // }); + + + this.animate(); + } + + 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() { + this.urdfPath = '/robots/LittleSophia/urdf/LittleSophia.urdf'; + + const urdfText = await fetch(this.urdfPath).then(res => res.text()); + const robot = await this.loader.loadFromString(urdfText); + //addOriginMarkers(robot); + robot.rotation.x = -Math.PI / 2; + this.scene.add(robot); + this.robot = robot; + + //this.overlay.robot = robot; + this.overlay.init(robot); + + } + + async loadURDFFromIndexedDB() { + return new Promise((resolve, reject) => { + const request = indexedDB.open("URDFEditorDB", 1); + + request.onerror = e => reject(e); + + request.onsuccess = e => { + const db = e.target.result; + const tx = db.transaction("urdfs", "readonly"); + const getReq = tx.objectStore("urdfs").get("current"); + + getReq.onsuccess = async () => { + const urdfText = getReq.result; + if (!urdfText) { + reject("No URDF stored in IndexedDB"); + return; + } + + try { + // ✅ feed the text into your loader + const robot = await this.loader.loadFromString(urdfText); + robot.rotation.x = -Math.PI / 2; + this.scene.add(robot); + this.robot = robot; + this.overlay.init(robot); + this.saveURDFToIndexedDB(robot); + resolve(robot); + } catch (err) { + reject(err); + } + }; + }; + }); + } + + // WRITE TO URDF POSITION IN ROBOT + // Find the transmission node for neck_roll + // const neckTrans = this.robot.urdfRobotNode.querySelector('transmission[name="neck_roll_trans"]'); + // if (neckTrans) { + // const actuator = neckTrans.querySelector('actuator[name="neck_roll"]'); + // if (actuator) { + // // Example: change encoderValidMin/Max + // actuator.querySelector('encoderValidMin').textContent = "100"; + // actuator.querySelector('encoderValidMax').textContent = "3900"; + // } + // } + + downloadURDF(robot, filename = "robot.urdf") { + const serializer = new XMLSerializer(); + const urdfString = serializer.serializeToString(robot.urdfRobotNode); + + const a = document.createElement("a"); + a.href = "data:text/xml;charset=utf-8," + encodeURIComponent(urdfString); + a.download = filename; + a.click(); + } + + saveURDFToIndexedDB(robot) { + const serializer = new XMLSerializer(); + const urdfString = serializer.serializeToString(robot.urdfRobotNode); + + const request = indexedDB.open("URDFEditorDB", 1); + request.onupgradeneeded = e => { + const db = e.target.result; + db.createObjectStore("urdfs"); + }; + request.onsuccess = e => { + const db = e.target.result; + const tx = db.transaction("urdfs", "readwrite"); + tx.objectStore("urdfs").put(urdfString, "current"); + console.log("saved URDF to IndexedDB"); + }; + + } + + listUrdfsFromIndexedDB() { + const request = indexedDB.open("URDFEditorDB", 1); + + request.onsuccess = e => { + const db = e.target.result; + const tx = db.transaction("urdfs", "readonly"); + const store = tx.objectStore("urdfs"); + const getAllReq = store.getAll(); + + getAllReq.onsuccess = () => { + console.log("Stored URDFs:", getAllReq.result); + }; + }; + } + + + 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) { + const rect = this.canvas.getBoundingClientRect(); + + this.mouse.x = ((event.clientX - rect.left) / rect.width) * 2 - 1; + this.mouse.y = -((event.clientY - rect.top) / rect.height) * 2 + 1; + + this.raycaster.setFromCamera(this.mouse, this.camera); + + if (!this.robot) return; + 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; + + let jointName = this.draggedJoint.name; + const { lower, upper } = getJointLimits(jointName, this.robot); + + const currentAngle = this.jointAngles[jointName] ?? 0; + const proposedAngle = currentAngle + angleDelta; + const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle)); + const actualDelta = clampedAngle - currentAngle; + + // ✅ Rotate dragged joint + this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta); + this.jointAngles[jointName] = clampedAngle; + //console.log(jointName); + // ✅ Apply mimic relationships automatically + // ✅ Enforce mimics + for (const [name, jointObj] of Object.entries(this.robot.joints)) { + if (jointObj.type === 'URDFMimicJoint') { + const { mimicJoint, multiplier, offset } = jointObj; + if (mimicJoint === jointName) { + const mimicAngle = clampedAngle * multiplier + offset; + const { lower, upper } = getJointLimits(name, this.robot); + const mimicClamped = Math.max(lower, Math.min(upper, mimicAngle)); + const mimicDelta = mimicClamped - (this.jointAngles[name] ?? 0); + + const mimicNode = this.robot.getObjectByName(name); + mimicNode.rotateOnAxis(this.worldAxis, mimicDelta); + this.jointAngles[name] = mimicClamped; + } + } + } + + // ✅ Update gizmo indicator + 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; + } + + + + // Hover highlighting + 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; + + // Decide how far up the chain to go + let levelsUp = 0; // default: climb one level from link → joint + if (event.ctrlKey) levelsUp = 1; + if (event.altKey) levelsUp = 2; + + this.draggedJoint = this.findJointAncestor(this.hoveredJoint, levelsUp); + if (!this.draggedJoint) return; + + // ✅ Skip mimic joints + if (this.draggedJoint.type === 'URDFMimicJoint') { + this.draggedJoint = findJointAncestor(this.draggedJoint, 1); + } + + + 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 { lower, upper } = getJointLimits(jointName, this.robot); + const transmission = jointData.transmission; + const encoderRange = transmission + ? transmission.encoderRange / transmission.mechanicalReduction + : Math.PI; + + const sector = createRotationSector(this.worldAxis, lower, upper, encoderRange); + 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(joint, levelsUp = 0) { + let current = joint; + let steps = 0; + + while (current && steps <= levelsUp) { + current = current.parent; + // climb until we find a URDFJoint (not a mimic) + while (current && !(current.type === 'URDFJoint')) { + current = current.parent; + } + steps++; + } + + return current; + } + + + animate() { + requestAnimationFrame(() => this.animate()); + //console.log(this.robot.joints) + this.renderer.render(this.scene, this.camera); + this.overlay.draw(); + //console.log(this.mesh); + } +} + +function ticksToRadians(ticks, actuator) { + const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75 + const centered = ticks - actuator.encoderTicks / 2; // shift so mid = 0 + const actuatorDeg = centered / ticksPerDeg; + const jointDeg = actuatorDeg / actuator.mechanicalReduction; + return jointDeg * (Math.PI / 180); // convert degrees → radians +} + +function radiansToTicks(radians, actuator) { + const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75 + const jointDeg = radians * (180 / Math.PI); // radians → degrees + const actuatorDeg = jointDeg * actuator.mechanicalReduction; // apply reduction + const centered = actuatorDeg * ticksPerDeg; // convert to ticks offset from mid + const ticks = centered + actuator.encoderTicks / 2; // shift back to full range + return ticks; +} + + +function getJointLimits(jointName, robot) { + const transmission = robot.joints[jointName].transmission; + + if (!transmission) { + return { lower: -Math.PI, upper: Math.PI }; + } + //console.log(transmission.encoderValidMin, transmission.encoderValidMax); + const lowerRad = ticksToRadians(transmission.encoderValidMin, transmission); + const upperRad = ticksToRadians(transmission.encoderValidMax, transmission); + //console.log(lowerRad, upperRad); + return { lower: lowerRad, upper: upperRad }; +} + +function getJointTransmission(jointName, robot) { + return robot.joints[jointName].transmission; +} + +function loadObjWithMtl(objPath, mtlPath, onLoad) { + const mtlLoader = new MTLLoader(); + mtlLoader.load(mtlPath, (materials) => { + materials.preload(); + const objLoader = new OBJLoader(); + objLoader.setMaterials(materials); + objLoader.load(objPath, (object) => { + onLoad(object); + }); + }); +} + +function addOriginMarkers(robot) { + console.log(robot.joints); + for (const [name, joint] of Object.entries(robot.joints)) { + const marker = new THREE.Mesh( + new THREE.SphereGeometry(0.01), + new THREE.MeshBasicMaterial({ color: 0x00ff00 }) + ); + joint.add(marker); + + const axes = new THREE.AxesHelper(0.05); + joint.add(axes); + + console.log("Added marker for joint:", name); + } + +} diff --git a/ros_robot_visualiser/ViewerOverlay.js b/ros_robot_visualiser/ViewerOverlay.js new file mode 100644 index 0000000..6ff86dd --- /dev/null +++ b/ros_robot_visualiser/ViewerOverlay.js @@ -0,0 +1,296 @@ +import { Button, Panel, Textbox } from './ui/canvasui.js'; + +export class ViewerOverlay { + constructor(renderer, robot, jointAngles, findObjectByName, saveURDF, loadURDF) { + this.renderer = renderer; + this.robot = robot; + this.jointAngles = jointAngles; + this.findObjectByName = findObjectByName; + this.saveURDF = saveURDF; // 🔑 bound to editor + this.loadURDF = loadURDF; // 🔑 bound to editor + + + this.overlayCanvas = document.getElementById('overlay-canvas'); + this.overlayCtx = this.overlayCanvas.getContext('2d'); + + this.uiElements = []; // for motor list items if you wrap them in UIElements + this.panels = []; // for modals or floating panels + + + // this.motorListPanel = this.createMotorListPanel(); + // this.panels.push(this.motorListPanel); + // console.log(this.panels); + + this.showMotorList = true; + this.showConfigModal = false; + this.configMotor = null; + this.configMotorName = null; + + // Handle resizing + const resizeOverlay = () => { + const { clientWidth, clientHeight } = this.renderer.domElement; + this.overlayCanvas.width = clientWidth; + this.overlayCanvas.height = clientHeight; + }; + window.addEventListener('resize', resizeOverlay); + resizeOverlay(); + + this.panels.push(this.createSystemPanel()); + const handlePointerEvent = (event) => { + const rect = this.overlayCanvas.getBoundingClientRect(); + const x = event.clientX - rect.left; + const y = event.clientY - rect.top; + + let consumed = false; + + // Panels get first chance + for (const panel of this.panels) { + if (panel.contains(x, y)) { + panel.handlePointerEvent(event, x, y); + consumed = true; + break; + } + } + + + + if (!consumed) { + const evt = new event.constructor(event.type, event); + this.renderer.domElement.dispatchEvent(evt); + } + + if (['pointermove', 'pointerdown', 'pointerup'].includes(event.type)) { + this.draw(); + } + }; + window.addEventListener('keydown', (event) => { + this.panels.forEach(panel => panel.handleKeyEvent(event)); + this.draw(); + }); + + + + + // Attach handlers + ['click', 'pointerdown', 'pointerup', 'pointermove', 'wheel'].forEach(type => { + this.overlayCanvas.addEventListener(type, handlePointerEvent); + }); + } + + init(robot) { + this.robot = robot; + this.motorListPanel = this.createMotorListPanel(this.overlayCtx); + if (this.motorListPanel) { + this.panels.push(this.motorListPanel); + } + this.draw(); + } + + draw() { + const ctx = this.overlayCtx; + ctx.clearRect(0, 0, this.overlayCanvas.width, this.overlayCanvas.height); + this.updateMotorLabels(); + this.panels.forEach(panel => panel.draw(ctx)); + + //console.log(this.panels); + } + + createSystemPanel() { + const w = 160; + const h = 80; + const x = this.overlayCanvas.width - w - 10; // bottom right + const y = this.overlayCanvas.height - h - 10; + console.log(this.overlayCanvas.width, this.overlayCanvas.height); + + const panel = new Panel(x, y, w, h, "System"); + + // Save button + panel.addElement(new Button(x + 20, y + 20, 120, 24, "Save", () => { + // delegate to editor’s save + this.saveURDF(this.robot); + })); + + // Load button + panel.addElement(new Button(x + 20, y + 50, 120, 24, "Load", () => { + // delegate to editor’s load + this.loadURDF(); + })); + + return panel; + } + + + createMotorListPanel() { + if (!this.robot?.joints) return null; + this.panels = this.panels.filter(p => !p.title?.startsWith("Config:")); + const panel = new Panel(0, 0, 300, this.overlayCanvas.height, "Motors"); + + let y = 10; + 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 motorName = jointData.transmission?.actuatorName || '(no motor)'; + if (motorName === "(no motor)") continue; + + // Build label string with angle/ticks + const t = jointData.transmission; + let label = `${motorName} ${degrees}°`; + if (t) { + const ticks = radiansToTicks(angle, t).toFixed(0); + label += ` ${ticks}`; + } + + + // Create button + const btn = new Button(10, y + 20, 280, 18, label, () => { + this.openMotorConfig(jointName, motorName); + }); + + btn.jointName = jointName; + btn.motorName = motorName; + + + panel.addElement(btn); + y += 22; + } + + return panel; + } + + updateMotorLabels() { + const motorPanel = this.panels.find(p => p.title === "Motors"); + if (!motorPanel || !this.robot?.joints) return; + + for (const element of motorPanel.elements) { + if (!(element instanceof Button) || !element.jointName) continue; + + const jointName = element.jointName; + const jointData = this.robot.joints[jointName]; + if (!jointData) continue; + + const angle = this.jointAngles?.[jointName] ?? 0; + const degrees = (angle * 180 / Math.PI).toFixed(1); + + const motorName = jointData.transmission?.actuatorName || '(no motor)'; + if (motorName === "(no motor)") continue; + + let label = `${motorName} ${degrees}°`; + const t = jointData.transmission; + if (t) { + const ticks = radiansToTicks(angle, t).toFixed(0); + label += ` ${ticks}`; + } + + element.label = label; // 🔄 update text + } + } + + + openMotorConfig(jointName, motorName) { + + const motor = this.robot.joints[jointName]; + + // Remove any existing config panel + this.panels = this.panels.filter(p => !p.title?.startsWith("Config:")); + + const panel = new Panel(400, 50, 400, 250, `Config: ${motorName}`); + + const motorIdBox = new Textbox(panel.x + 100, panel.y + 45, 80, 24, motor.transmission.motorID || ''); + panel.addElement(motorIdBox); + + panel.addElement(new Button(panel.x + 20, panel.y + 150, 80, 24, 'Save', () => { + const idValue = parseInt(motorIdBox.value, 10) || 0; + motor.motorId = idValue; + motor.transmission.motorID = idValue; + this.addOrUpdateActuatorElement(motorName, "motorID", idValue); + this.panels = this.panels.filter(p => p !== panel); // close panel + this.draw(); + })); + + panel.addElement(new Button(panel.x + 120, panel.y + 150, 80, 24, 'Close', () => { + this.panels = this.panels.filter(p => p !== panel); + this.draw(); + })); + + this.panels.push(panel); + this.draw(); + } + + + /** + * Ensure an element exists under a given actuator and set its text. + * @param {string} transmissionName - The transmission name + * @param {string} actuatorName - The actuator name + * @param {string} elementName - The child element to add/update (e.g. "motorID") + * @param {string|number} value - The value to set + */ + addOrUpdateActuatorElement(actuatorName, elementName, value) { + console.log("Updating URDF:", actuatorName, elementName, value); + const urdfNode = this.robot?.urdfRobotNode; + if (!urdfNode) return; + + // find actuator anywhere in the URDF + const actuatorNode = urdfNode.querySelector(`actuator[name="${actuatorName}"]`); + if (!actuatorNode) return; + + // find or create the child element + let childNode = actuatorNode.querySelector(elementName); + if (!childNode) { + childNode = urdfNode.ownerDocument.createElement(elementName); + actuatorNode.appendChild(childNode); + } + childNode.textContent = value.toString(); + + + } + + + + + + + onClick(event) { + console.log("CLICK"); + const rect = this.overlayCanvas.getBoundingClientRect(); + const x = event.clientX - rect.left; + const y = event.clientY - rect.top; + + for (const hb of this.motorHitboxes) { + if (x >= hb.x && x <= hb.x + hb.w && + y >= hb.y && y <= hb.y + hb.h) { + this.openMotorConfig(hb.jointName, hb.motorName); + break; + } + } + } + + // openMotorConfig(jointName, motorName) { + // this.showConfigModal = true; + // this.configMotorName = motorName; + // this.configMotor = this.robot.joints[jointName]; + // this.draw(); + // } +} + + +function ticksToRadians(ticks, actuator) { + const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75 + const centered = ticks - actuator.encoderTicks / 2; // shift so mid = 0 + const actuatorDeg = centered / ticksPerDeg; + const jointDeg = actuatorDeg / actuator.mechanicalReduction; + return jointDeg * (Math.PI / 180); // convert degrees → radians +} + +function radiansToTicks(radians, actuator) { + const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75 + const jointDeg = radians * (180 / Math.PI); // radians → degrees + const actuatorDeg = jointDeg * actuator.mechanicalReduction; // apply reduction + const centered = actuatorDeg * ticksPerDeg; // convert to ticks offset from mid + const ticks = centered + actuator.encoderTicks / 2; // shift back to full range + return ticks; +} \ No newline at end of file diff --git a/ros_robot_visualiser/index.html b/ros_robot_visualiser/index.html new file mode 100644 index 0000000..bff5c68 --- /dev/null +++ b/ros_robot_visualiser/index.html @@ -0,0 +1,42 @@ + + + + + + URDF Robot Viewer + + + + +
Loading URDF...
+
+ + +
+ + + + + + + + + \ No newline at end of file diff --git a/ros_robot_visualiser/package.json b/ros_robot_visualiser/package.json new file mode 100644 index 0000000..5af3046 --- /dev/null +++ b/ros_robot_visualiser/package.json @@ -0,0 +1,19 @@ +{ + "name": "ros_robot_visualiser", + "version": "1.0.0", + "description": "", + "main": "script.js", + "scripts": { + "test": "echo \"Error: no test specified\" && exit 1" + }, + "keywords": [], + "author": "", + "license": "ISC", + "type": "commonjs", + "dependencies": { + "three": "^0.181.0" + }, + "devDependencies": { + "vite": "^7.2.2" + } +} diff --git a/ros_robot_visualiser/roundURDF.py b/ros_robot_visualiser/roundURDF.py new file mode 100644 index 0000000..6041366 --- /dev/null +++ b/ros_robot_visualiser/roundURDF.py @@ -0,0 +1,43 @@ +import xml.etree.ElementTree as ET +import math + +def round_axis(x, y, z): + # Normalize vector + mag = math.sqrt(x**2 + y**2 + z**2) + if mag == 0: + return (0, 0, 0) + x, y, z = x/mag, y/mag, z/mag + + # Compare to cardinal directions + directions = { + (1, 0, 0): 'X+', + (-1, 0, 0): 'X-', + (0, 1, 0): 'Y+', + (0, -1, 0): 'Y-', + (0, 0, 1): 'Z+', + (0, 0, -1): 'Z-' + } + + best = max(directions.keys(), key=lambda d: x*d[0] + y*d[1] + z*d[2]) + return best + +def process_urdf(file_path, output_path): + tree = ET.parse(file_path) + root = tree.getroot() + + for joint in root.findall('joint'): + axis_elem = joint.find('axis') + if axis_elem is not None: + axis_str = axis_elem.attrib.get('xyz', '') + try: + x, y, z = map(float, axis_str.strip().split()) + rx, ry, rz = round_axis(x, y, z) + axis_elem.set('xyz', f"{rx} {ry} {rz}") + except: + continue + + tree.write(output_path) + print(f"Rounded axes written to {output_path}") + +# Example usage +process_urdf("./urdf/sample.urdf", "./urdf/output.urdf") diff --git a/ros_robot_visualiser/script.js b/ros_robot_visualiser/script.js new file mode 100644 index 0000000..cd1e1e4 --- /dev/null +++ b/ros_robot_visualiser/script.js @@ -0,0 +1,4 @@ +import { URDFEditor } from './URDFEditor.js'; + +const canvas = document.getElementById('urdfCanvas'); +const editor = new URDFEditor(canvas, './urdf/sample.urdf'); diff --git a/ros_robot_visualiser/script_old.js b/ros_robot_visualiser/script_old.js new file mode 100644 index 0000000..f501d97 --- /dev/null +++ b/ros_robot_visualiser/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/ros_robot_visualiser/ui/canvasui.js b/ros_robot_visualiser/ui/canvasui.js new file mode 100644 index 0000000..f112a11 --- /dev/null +++ b/ros_robot_visualiser/ui/canvasui.js @@ -0,0 +1,171 @@ + + + +class UIElement { + constructor(x, y, w, h) { + this.x = x; this.y = y; + this.w = w; this.h = h; + this.hovered = false; + this.active = false; + + } + + contains(px, py) { + return px >= this.x && px <= this.x + this.w && + py >= this.y && py <= this.y + this.h; + } + + handlePointerEvent(event, px, py) { + switch (event.type) { + case 'pointermove': + this.onPointerMove(px, py); + break; + case 'pointerdown': + this.onPointerDown(px, py); + break; + case 'pointerup': + this.onPointerUp(px, py); + break; + case 'click': + // optional: treat click as pointerup for convenience + if (this.contains(px, py)) { + this.onClick?.(); // if subclass defines onClick + } + break; + } + + } + + + onPointerMove(px, py) { + this.hovered = this.contains(px, py); + //console.log(this); + } + + onPointerDown(px, py) { + if (this.contains(px, py)) { + this.active = true; + return true; + } + return false; + } + + onPointerUp(px, py) { + if (this.active && this.contains(px, py)) { + this.active = false; + return true; + } + this.active = false; + return false; + } + + draw(ctx) { /* override in subclasses */ } +} + +export class Panel extends UIElement { + constructor(x, y, w, h, title = '') { + super(x, y, w, h); + this.title = title; + this.elements = []; + } + + addElement(el) { this.elements.push(el); } + + draw(ctx) { + ctx.fillStyle = 'rgba(0,0,0,0.5)'; + ctx.fillRect(this.x, this.y, this.w, this.h); + ctx.strokeStyle = '#fff'; + ctx.strokeRect(this.x, this.y, this.w, this.h); + + if (this.title) { + ctx.fillStyle = '#fff'; + ctx.font = '16px monospace'; + ctx.fillText(this.title, this.x + 10, this.y + 20); + } + + this.elements.forEach(el => el.draw(ctx)); + } + + handlePointerEvent(event, px, py) { + super.handlePointerEvent(event, px, py); // panel itself + this.elements.forEach(el => el.handlePointerEvent(event, px, py)); + } + + + handleKeyEvent(event) { + this.elements.forEach(el => { + if (el.onKeyDown) el.onKeyDown(event); + }); + } +} + + + +export class Button extends UIElement { + constructor(x, y, w, h, label, onClick) { + super(x, y, w, h); + this.label = label; + this.onClick = onClick; + } + + draw(ctx) { + // ctx.fillStyle = this.active ? '#225433' : + // this.hovered ? '#666' : '#333'; + // ctx.fillRect(this.x, this.y, this.w, this.h); + + ctx.fillStyle = '#fff'; + if (this.hovered) { + ctx.fillStyle = '#666'; + + } + ctx.font = '14px monospace'; + ctx.fillText(this.label, this.x + 10, this.y + this.h - 8); + } + + onPointerMove(px, py) { + this.hovered = this.contains(px, py); + //console.log(this); + } + + onPointerUp(px, py) { + const clicked = super.onPointerUp(px, py); + if (clicked && this.onClick) this.onClick(); + return clicked; + } +} + +export class Textbox extends UIElement { + constructor(x, y, w, h, initialValue = '') { + super(x, y, w, h); + this.value = initialValue; + this.focused = false; + } + + draw(ctx) { + ctx.fillStyle = '#000'; + ctx.fillRect(this.x, this.y, this.w, this.h); + + ctx.strokeStyle = this.focused ? '#0f0' : '#aaa'; + ctx.strokeRect(this.x, this.y, this.w, this.h); + + ctx.fillStyle = '#fff'; + ctx.font = '14px monospace'; + ctx.fillText(this.value, this.x + 5, this.y + this.h - 8); + } + + onPointerUp(px, py) { + const clicked = super.onPointerUp(px, py); + this.focused = clicked; + return clicked; + } + + onKeyDown(event) { + if (!this.focused) return; + if (event.key >= '0' && event.key <= '9') { + this.value += event.key; + } else if (event.key === 'Backspace') { + this.value = this.value.slice(0, -1); + } + } +} + diff --git a/ros_robot_visualiser/urdf/ExtendedURDFLoader.js b/ros_robot_visualiser/urdf/ExtendedURDFLoader.js new file mode 100644 index 0000000..203a332 --- /dev/null +++ b/ros_robot_visualiser/urdf/ExtendedURDFLoader.js @@ -0,0 +1,83 @@ +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 encoderTicks = parseInt(actuator?.querySelector('encoderTicks')?.textContent ?? '4096'); + const encoderValidMin = parseInt(actuator?.querySelector('encoderValidMin')?.textContent ?? '0'); + const encoderValidMax = parseInt(actuator?.querySelector('encoderValidMax')?.textContent ?? '4095'); + const encoderRange = parseInt(actuator?.querySelector('encoderRange')?.textContent ?? '180'); + const motorID = parseInt(actuator?.querySelector('motorID')?.textContent ?? '47'); + + if (jointName) { + this.transmissionMap[jointName] = { + mechanicalReduction: reduction, + actuatorName, + encoderTicks, + encoderValidMin, + encoderValidMax, + encoderRange, + motorID + }; + } + }); + } + + parseMimics(xml) { + const joints = xml.querySelectorAll('joint'); + joints.forEach(jointEl => { + const jointName = jointEl.getAttribute('name'); + const mimicEl = jointEl.querySelector('mimic'); + if (mimicEl && jointName) { + const target = mimicEl.getAttribute('joint'); + const multiplier = parseFloat(mimicEl.getAttribute('multiplier') ?? '1.0'); + const offset = parseFloat(mimicEl.getAttribute('offset') ?? '0.0'); + + // Attach mimic info directly to the joint object + if (this.robot?.joints[jointName]) { + this.robot.joints[jointName].mimic = { target, multiplier, offset }; + } + } + }); + + + } + + + + 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); + //this.parseMimics(xml); + + const robot = this.parse(xml); // ✅ Use inherited parse() directly + //console.log(robot.joints); + for (const jointName in robot.joints) { + if (this.transmissionMap[jointName]) { + robot.joints[jointName].transmission = this.transmissionMap[jointName]; + } + } + + return robot; + } +} diff --git a/ros_robot_visualiser/urdf/URDFClasses.js b/ros_robot_visualiser/urdf/URDFClasses.js new file mode 100644 index 0000000..88e5b14 --- /dev/null +++ b/ros_robot_visualiser/urdf/URDFClasses.js @@ -0,0 +1,469 @@ +import { Euler, Object3D, Vector3, Quaternion, Matrix4 } from 'three'; + +const _tempAxis = new Vector3(); +const _tempEuler = new Euler(); +const _tempTransform = new Matrix4(); +const _tempOrigTransform = new Matrix4(); +const _tempQuat = new Quaternion(); +const _tempScale = new Vector3(1.0, 1.0, 1.0); +const _tempPosition = new Vector3(); + +class URDFBase extends Object3D { + + constructor(...args) { + + super(...args); + this.urdfNode = null; + this.urdfName = ''; + + } + + copy(source, recursive) { + + super.copy(source, recursive); + + this.urdfNode = source.urdfNode; + this.urdfName = source.urdfName; + + return this; + + } + +} + +class URDFCollider extends URDFBase { + + constructor(...args) { + + super(...args); + this.isURDFCollider = true; + this.type = 'URDFCollider'; + + } + +} + +class URDFVisual extends URDFBase { + + constructor(...args) { + + super(...args); + this.isURDFVisual = true; + this.type = 'URDFVisual'; + + } + +} + +class URDFLink extends URDFBase { + + constructor(...args) { + + super(...args); + this.isURDFLink = true; + this.type = 'URDFLink'; + + } + +} + +class URDFJoint extends URDFBase { + + get jointType() { + + return this._jointType; + + } + + set jointType(v) { + + if (this.jointType === v) return; + this._jointType = v; + this.matrixWorldNeedsUpdate = true; + switch (v) { + + case 'fixed': + this.jointValue = []; + break; + + case 'continuous': + case 'revolute': + case 'prismatic': + this.jointValue = new Array(1).fill(0); + break; + + case 'planar': + // Planar joints are, 3dof: position XY and rotation Z. + this.jointValue = new Array(3).fill(0); + this.axis = new Vector3(0, 0, 1); + break; + + case 'floating': + this.jointValue = new Array(6).fill(0); + break; + + } + + } + + get angle() { + + return this.jointValue[0]; + + } + + constructor(...args) { + + super(...args); + + this.isURDFJoint = true; + this.type = 'URDFJoint'; + + this.jointValue = null; + this.jointType = 'fixed'; + this.axis = new Vector3(1, 0, 0); + this.limit = { lower: 0, upper: 0 }; + this.ignoreLimits = false; + + this.origPosition = null; + this.origQuaternion = null; + + this.mimicJoints = []; + + } + + /* Overrides */ + copy(source, recursive) { + + super.copy(source, recursive); + + this.jointType = source.jointType; + this.axis = source.axis.clone(); + this.limit.lower = source.limit.lower; + this.limit.upper = source.limit.upper; + this.ignoreLimits = false; + + this.jointValue = [...source.jointValue]; + + this.origPosition = source.origPosition ? source.origPosition.clone() : null; + this.origQuaternion = source.origQuaternion ? source.origQuaternion.clone() : null; + + this.mimicJoints = [...source.mimicJoints]; + + return this; + + } + + /* Public Functions */ + /** + * @param {...number|null} values The joint value components to set, optionally null for no-op + * @returns {boolean} Whether the invocation of this function resulted in an actual change to the joint value + */ + setJointValue(...values) { + + // Parse all incoming values into numbers except null, which we treat as a no-op for that value component. + values = values.map(v => v === null ? null : parseFloat(v)); + + if (!this.origPosition || !this.origQuaternion) { + + this.origPosition = this.position.clone(); + this.origQuaternion = this.quaternion.clone(); + + } + + let didUpdate = false; + + this.mimicJoints.forEach(joint => { + + didUpdate = joint.updateFromMimickedJoint(...values) || didUpdate; + + }); + + switch (this.jointType) { + + case 'fixed': { + + return didUpdate; + + } + case 'continuous': + case 'revolute': { + + let angle = values[0]; + if (angle == null) return didUpdate; + if (angle === this.jointValue[0]) return didUpdate; + + if (!this.ignoreLimits && this.jointType === 'revolute') { + + angle = Math.min(this.limit.upper, angle); + angle = Math.max(this.limit.lower, angle); + + } + + this.quaternion + .setFromAxisAngle(this.axis, angle) + .premultiply(this.origQuaternion); + + if (this.jointValue[0] !== angle) { + + this.jointValue[0] = angle; + this.matrixWorldNeedsUpdate = true; + return true; + + } else { + + return didUpdate; + + } + + } + + case 'prismatic': { + + let pos = values[0]; + if (pos == null) return didUpdate; + if (pos === this.jointValue[0]) return didUpdate; + + if (!this.ignoreLimits) { + + pos = Math.min(this.limit.upper, pos); + pos = Math.max(this.limit.lower, pos); + + } + + this.position.copy(this.origPosition); + _tempAxis.copy(this.axis).applyEuler(this.rotation); + this.position.addScaledVector(_tempAxis, pos); + + if (this.jointValue[0] !== pos) { + + this.jointValue[0] = pos; + this.matrixWorldNeedsUpdate = true; + return true; + + } else { + + return didUpdate; + + } + + } + + case 'floating': { + + // no-op if all values are identical to existing value or are null + if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate; + // Floating joints have six degrees of freedom: X, Y, Z, R, P, Y. + this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0]; + this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1]; + this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2]; + this.jointValue[3] = values[3] !== null ? values[3] : this.jointValue[3]; + this.jointValue[4] = values[4] !== null ? values[4] : this.jointValue[4]; + this.jointValue[5] = values[5] !== null ? values[5] : this.jointValue[5]; + + // Compose transform of joint origin and transform due to joint values + _tempOrigTransform.compose(this.origPosition, this.origQuaternion, _tempScale); + _tempQuat.setFromEuler( + _tempEuler.set( + this.jointValue[3], + this.jointValue[4], + this.jointValue[5], + 'XYZ', + ), + ); + _tempPosition.set(this.jointValue[0], this.jointValue[1], this.jointValue[2]); + _tempTransform.compose(_tempPosition, _tempQuat, _tempScale); + + // Calcualte new transform + _tempOrigTransform.premultiply(_tempTransform); + this.position.setFromMatrixPosition(_tempOrigTransform); + this.rotation.setFromRotationMatrix(_tempOrigTransform); + + this.matrixWorldNeedsUpdate = true; + return true; + } + + case 'planar': { + + // no-op if all values are identical to existing value or are null + if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate; + + this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0]; + this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1]; + this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2]; + + // Compose transform of joint origin and transform due to joint values + _tempOrigTransform.compose(this.origPosition, this.origQuaternion, _tempScale); + _tempQuat.setFromAxisAngle(this.axis, this.jointValue[2]); + _tempPosition.set(this.jointValue[0], this.jointValue[1], 0.0); + _tempTransform.compose(_tempPosition, _tempQuat, _tempScale); + + // Calculate new transform + _tempOrigTransform.premultiply(_tempTransform); + this.position.setFromMatrixPosition(_tempOrigTransform); + this.rotation.setFromRotationMatrix(_tempOrigTransform); + + this.matrixWorldNeedsUpdate = true; + return true; + } + + } + + return didUpdate; + + } + +} + +class URDFMimicJoint extends URDFJoint { + + constructor(...args) { + + super(...args); + this.type = 'URDFMimicJoint'; + this.mimicJoint = null; + this.offset = 0; + this.multiplier = 1; + + } + + updateFromMimickedJoint(...values) { + + const modifiedValues = values.map(x => x * this.multiplier + this.offset); + return super.setJointValue(...modifiedValues); + + } + + /* Overrides */ + copy(source, recursive) { + + super.copy(source, recursive); + + this.mimicJoint = source.mimicJoint; + this.offset = source.offset; + this.multiplier = source.multiplier; + + return this; + + } + +} + +class URDFRobot extends URDFLink { + + constructor(...args) { + + super(...args); + this.isURDFRobot = true; + this.urdfNode = null; + + this.urdfRobotNode = null; + this.robotName = null; + + this.links = null; + this.joints = null; + this.colliders = null; + this.visual = null; + this.frames = null; + + } + + copy(source, recursive) { + + super.copy(source, recursive); + + this.urdfRobotNode = source.urdfRobotNode; + this.robotName = source.robotName; + + this.links = {}; + this.joints = {}; + this.colliders = {}; + this.visual = {}; + + this.traverse(c => { + + if (c.isURDFJoint && c.urdfName in source.joints) { + + this.joints[c.urdfName] = c; + + } + + if (c.isURDFLink && c.urdfName in source.links) { + + this.links[c.urdfName] = c; + + } + + if (c.isURDFCollider && c.urdfName in source.colliders) { + + this.colliders[c.urdfName] = c; + + } + + if (c.isURDFVisual && c.urdfName in source.visual) { + + this.visual[c.urdfName] = c; + + } + + }); + + // Repair mimic joint references once we've re-accumulated all our joint data + for (const joint in this.joints) { + this.joints[joint].mimicJoints = this.joints[joint].mimicJoints.map((mimicJoint) => this.joints[mimicJoint.name]); + } + + this.frames = { + ...this.colliders, + ...this.visual, + ...this.links, + ...this.joints, + }; + + return this; + + } + + getFrame(name) { + + return this.frames[name]; + + } + + setJointValue(jointName, ...angle) { + + const joint = this.joints[jointName]; + if (joint) { + + return joint.setJointValue(...angle); + + } + + return false; + } + + setJointValues(values) { + + let didChange = false; + for (const name in values) { + + const value = values[name]; + if (Array.isArray(value)) { + + didChange = this.setJointValue(name, ...value) || didChange; + + } else { + + didChange = this.setJointValue(name, value) || didChange; + + } + + } + + return didChange; + + } + +} + +export { URDFRobot, URDFLink, URDFJoint, URDFMimicJoint, URDFVisual, URDFCollider }; \ No newline at end of file diff --git a/ros_robot_visualiser/urdf/URDFDragControls.js b/ros_robot_visualiser/urdf/URDFDragControls.js new file mode 100644 index 0000000..a0776db --- /dev/null +++ b/ros_robot_visualiser/urdf/URDFDragControls.js @@ -0,0 +1,330 @@ +import { Raycaster, Vector3, Plane, Vector2 } from 'three'; + +// Find the nearest parent that is a joint +function isJoint(j) { + + return j.isURDFJoint && j.jointType !== 'fixed'; + +}; + +function findNearestJoint(child) { + + let curr = child; + while (curr) { + + if (isJoint(curr)) { + + return curr; + + } + + curr = curr.parent; + + } + + return curr; + +}; + +const prevHitPoint = new Vector3(); +const newHitPoint = new Vector3(); +const pivotPoint = new Vector3(); +const tempVector = new Vector3(); +const tempVector2 = new Vector3(); +const projectedStartPoint = new Vector3(); +const projectedEndPoint = new Vector3(); +const plane = new Plane(); +export class URDFDragControls { + + constructor(scene) { + + this.enabled = true; + this.scene = scene; + this.raycaster = new Raycaster(); + this.initialGrabPoint = new Vector3(); + + this.hitDistance = -1; + this.hovered = null; + this.manipulating = null; + + } + + update() { + + const { + raycaster, + hovered, + manipulating, + scene, + } = this; + + if (manipulating) { + + return; + + } + + let hoveredJoint = null; + const intersections = raycaster.intersectObject(scene, true); + if (intersections.length !== 0) { + + const hit = intersections[0]; + this.hitDistance = hit.distance; + hoveredJoint = findNearestJoint(hit.object); + this.initialGrabPoint.copy(hit.point); + + } + + if (hoveredJoint !== hovered) { + + if (hovered) { + + this.onUnhover(hovered); + + } + + this.hovered = hoveredJoint; + + if (hoveredJoint) { + + this.onHover(hoveredJoint); + + } + + } + + } + + updateJoint(joint, angle) { + + joint.setJointValue(angle); + + } + + onDragStart(joint) { + + } + + onDragEnd(joint) { + + } + + onHover(joint) { + + } + + onUnhover(joint) { + + } + + getRevoluteDelta(joint, startPoint, endPoint) { + + // set up the plane + tempVector + .copy(joint.axis) + .transformDirection(joint.matrixWorld) + .normalize(); + pivotPoint + .set(0, 0, 0) + .applyMatrix4(joint.matrixWorld); + plane + .setFromNormalAndCoplanarPoint(tempVector, pivotPoint); + + // project the drag points onto the plane + plane.projectPoint(startPoint, projectedStartPoint); + plane.projectPoint(endPoint, projectedEndPoint); + + // get the directions relative to the pivot + projectedStartPoint.sub(pivotPoint); + projectedEndPoint.sub(pivotPoint); + + tempVector.crossVectors(projectedStartPoint, projectedEndPoint); + + const direction = Math.sign(tempVector.dot(plane.normal)); + return direction * projectedEndPoint.angleTo(projectedStartPoint); + + } + + getPrismaticDelta(joint, startPoint, endPoint) { + + tempVector.subVectors(endPoint, startPoint); + plane + .normal + .copy(joint.axis) + .transformDirection(joint.parent.matrixWorld) + .normalize(); + + return tempVector.dot(plane.normal); + + } + + moveRay(toRay) { + + const { raycaster, hitDistance, manipulating } = this; + const { ray } = raycaster; + + if (manipulating) { + + ray.at(hitDistance, prevHitPoint); + toRay.at(hitDistance, newHitPoint); + + let delta = 0; + if (manipulating.jointType === 'revolute' || manipulating.jointType === 'continuous') { + + delta = this.getRevoluteDelta(manipulating, prevHitPoint, newHitPoint); + + } else if (manipulating.jointType === 'prismatic') { + + delta = this.getPrismaticDelta(manipulating, prevHitPoint, newHitPoint); + + } + + if (delta) { + + this.updateJoint(manipulating, manipulating.angle + delta); + + } + + } + + this.raycaster.ray.copy(toRay); + this.update(); + + } + + setGrabbed(grabbed) { + + const { hovered, manipulating } = this; + + if (grabbed) { + + if (manipulating !== null || hovered === null) { + + return; + + } + + this.manipulating = hovered; + this.onDragStart(hovered); + + } else { + + if (this.manipulating === null) { + return; + } + + this.onDragEnd(this.manipulating); + this.manipulating = null; + this.update(); + + } + + } + +} + +export class PointerURDFDragControls extends URDFDragControls { + + constructor(scene, camera, domElement) { + + super(scene); + this.camera = camera; + this.domElement = domElement; + + const raycaster = new Raycaster(); + const mouse = new Vector2(); + + function updateMouse(e) { + + const rect = domElement.getBoundingClientRect(); + mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1; + mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1; + + } + + this._mouseDown = e => { + + updateMouse(e); + raycaster.setFromCamera(mouse, this.camera); + this.moveRay(raycaster.ray); + this.setGrabbed(true); + + }; + + this._mouseMove = e => { + + updateMouse(e); + raycaster.setFromCamera(mouse, this.camera); + this.moveRay(raycaster.ray); + + }; + + this._mouseUp = e => { + + updateMouse(e); + raycaster.setFromCamera(mouse, this.camera); + this.moveRay(raycaster.ray); + this.setGrabbed(false); + + }; + + domElement.addEventListener('mousedown', this._mouseDown); + domElement.addEventListener('mousemove', this._mouseMove); + domElement.addEventListener('mouseup', this._mouseUp); + + } + + getRevoluteDelta(joint, startPoint, endPoint) { + + const { camera, initialGrabPoint } = this; + + // set up the plane + tempVector + .copy(joint.axis) + .transformDirection(joint.matrixWorld) + .normalize(); + pivotPoint + .set(0, 0, 0) + .applyMatrix4(joint.matrixWorld); + plane + .setFromNormalAndCoplanarPoint(tempVector, pivotPoint); + + tempVector + .copy(camera.position) + .sub(initialGrabPoint) + .normalize(); + + // if looking into the plane of rotation + if (Math.abs(tempVector.dot(plane.normal)) > 0.3) { + + return super.getRevoluteDelta(joint, startPoint, endPoint); + + } else { + + // get the up direction + tempVector.set(0, 1, 0).transformDirection(camera.matrixWorld); + + // get points projected onto the plane of rotation + plane.projectPoint(startPoint, projectedStartPoint); + plane.projectPoint(endPoint, projectedEndPoint); + + tempVector.set(0, 0, -1).transformDirection(camera.matrixWorld); + tempVector.cross(plane.normal); + tempVector2.subVectors(endPoint, startPoint); + + return tempVector.dot(tempVector2); + + } + + } + + dispose() { + + const { domElement } = this; + domElement.removeEventListener('mousedown', this._mouseDown); + domElement.removeEventListener('mousemove', this._mouseMove); + domElement.removeEventListener('mouseup', this._mouseUp); + + } + +} \ No newline at end of file diff --git a/ros_robot_visualiser/urdf/URDFLoader.js b/ros_robot_visualiser/urdf/URDFLoader.js new file mode 100644 index 0000000..b7fddad --- /dev/null +++ b/ros_robot_visualiser/urdf/URDFLoader.js @@ -0,0 +1,657 @@ +import * as THREE from 'three'; +import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js'; +import { ColladaLoader } from 'three/examples/jsm/loaders/ColladaLoader.js'; +import { URDFRobot, URDFJoint, URDFLink, URDFCollider, URDFVisual, URDFMimicJoint } from './URDFClasses.js'; + +/* +Reference coordinate frames for THREE.js and ROS. +Both coordinate systems are right handed so the URDF is instantiated without +frame transforms. The resulting model can be rotated to rectify the proper up, +right, and forward directions + +THREE.js + Y + | + | + .-----X + ／ +Z + +ROS URDf + Z + | X + | ／ + Y-----. + +*/ + +const tempQuaternion = new THREE.Quaternion(); +const tempEuler = new THREE.Euler(); + +// take a vector "x y z" and process it into +// an array [x, y, z] +function processTuple(val) { + + if (!val) return [0, 0, 0]; + return val.trim().split(/\s+/g).map(num => parseFloat(num)); + +} + +// applies a rotation a threejs object in URDF order +function applyRotation(obj, rpy, additive = false) { + + // if additive is true the rotation is applied in + // addition to the existing rotation + if (!additive) obj.rotation.set(0, 0, 0); + + tempEuler.set(rpy[0], rpy[1], rpy[2], 'ZYX'); + tempQuaternion.setFromEuler(tempEuler); + tempQuaternion.multiply(obj.quaternion); + obj.quaternion.copy(tempQuaternion); + +} + +/* URDFLoader Class */ +// Loads and reads a URDF file into a THREEjs Object3D format +export default +class URDFLoader { + + constructor(manager) { + + this.manager = manager || THREE.DefaultLoadingManager; + this.loadMeshCb = this.defaultMeshLoader.bind(this); + this.parseVisual = true; + this.parseCollision = false; + this.packages = ''; + this.workingPath = ''; + this.fetchOptions = {}; + + } + + /* Public API */ + loadAsync(urdf) { + + return new Promise((resolve, reject) => { + + this.load(urdf, resolve, null, reject); + + }); + + } + + // urdf: The path to the URDF within the package OR absolute + // onComplete: Callback that is passed the model once loaded + load(urdf, onComplete, onProgress, onError) { + + // Check if a full URI is specified before + // prepending the package info + const manager = this.manager; + const workingPath = THREE.LoaderUtils.extractUrlBase(urdf); + const urdfPath = this.manager.resolveURL(urdf); + + manager.itemStart(urdfPath); + + fetch(urdfPath, this.fetchOptions) + .then(res => { + + if (res.ok) { + + if (onProgress) { + + onProgress(null); + + } + return res.text(); + + } else { + + throw new Error(`URDFLoader: Failed to load url '${ urdfPath }' with error code ${ res.status } : ${ res.statusText }.`); + + } + + }) + .then(data => { + + const model = this.parse(data, this.workingPath || workingPath); + onComplete(model); + manager.itemEnd(urdfPath); + + }) + .catch(e => { + + if (onError) { + + onError(e); + + } else { + + console.error('URDFLoader: Error loading file.', e); + + } + manager.itemError(urdfPath); + manager.itemEnd(urdfPath); + + }); + + } + + parse(content, workingPath = this.workingPath) { + + const packages = this.packages; + const loadMeshCb = this.loadMeshCb; + const parseVisual = this.parseVisual; + const parseCollision = this.parseCollision; + const manager = this.manager; + const linkMap = {}; + const jointMap = {}; + const materialMap = {}; + + // Resolves the path of mesh files + function resolvePath(path) { + + if (!/^package:\/\//.test(path)) { + + return workingPath ? workingPath + path : path; + + } + + // Remove "package://" keyword and split meshPath at the first slash + const [targetPkg, relPath] = path.replace(/^package:\/\//, '').split(/\/(.+)/); + + if (typeof packages === 'string') { + + // "pkg" is one single package + if (packages.endsWith(targetPkg)) { + + // "pkg" is the target package + return packages + '/' + relPath; + + } else { + + // Assume "pkg" is the target package's parent directory + return packages + '/' + targetPkg + '/' + relPath; + + } + + } else if (packages instanceof Function) { + + return packages(targetPkg) + '/' + relPath; + + } else if (typeof packages === 'object') { + + // "pkg" is a map of packages + if (targetPkg in packages) { + + return packages[targetPkg] + '/' + relPath; + + } else { + + console.error(`URDFLoader : ${ targetPkg } not found in provided package list.`); + return null; + + } + + } + + } + + // Process the URDF text format + function processUrdf(data) { + + let children; + if (data instanceof Document) { + + children = [ ...data.children ]; + + } else if (data instanceof Element) { + + children = [ data ]; + + } else { + + const parser = new DOMParser(); + const urdf = parser.parseFromString(data, 'text/xml'); + children = [ ...urdf.children ]; + + } + + const robotNode = children.filter(c => c.nodeName === 'robot').pop(); + return processRobot(robotNode); + + } + + // Process the node + function processRobot(robot) { + + const robotNodes = [ ...robot.children ]; + const links = robotNodes.filter(c => c.nodeName.toLowerCase() === 'link'); + const joints = robotNodes.filter(c => c.nodeName.toLowerCase() === 'joint'); + const materials = robotNodes.filter(c => c.nodeName.toLowerCase() === 'material'); + const obj = new URDFRobot(); + + obj.robotName = robot.getAttribute('name'); + obj.urdfRobotNode = robot; + + // Create the map + materials.forEach(m => { + + const name = m.getAttribute('name'); + materialMap[name] = processMaterial(m); + + }); + + // Create the map + const visualMap = {}; + const colliderMap = {}; + links.forEach(l => { + + const name = l.getAttribute('name'); + const isRoot = robot.querySelector(`child[link="${ name }"]`) === null; + linkMap[name] = processLink(l, visualMap, colliderMap, isRoot ? obj : null); + + }); + + // Create the map + joints.forEach(j => { + + const name = j.getAttribute('name'); + jointMap[name] = processJoint(j); + + }); + + obj.joints = jointMap; + obj.links = linkMap; + obj.colliders = colliderMap; + obj.visual = visualMap; + + // Link up mimic joints + const jointList = Object.values(jointMap); + jointList.forEach(j => { + + if (j instanceof URDFMimicJoint) { + + jointMap[j.mimicJoint].mimicJoints.push(j); + + } + + }); + + // Detect infinite loops of mimic joints + jointList.forEach(j => { + + const uniqueJoints = new Set(); + const iterFunction = joint => { + + if (uniqueJoints.has(joint)) { + + throw new Error('URDFLoader: Detected an infinite loop of mimic joints.'); + + } + + uniqueJoints.add(joint); + joint.mimicJoints.forEach(j => { + + iterFunction(j); + + }); + + }; + + iterFunction(j); + }); + + obj.frames = { + ...colliderMap, + ...visualMap, + ...linkMap, + ...jointMap, + }; + + return obj; + + } + + // Process joint nodes and parent them + function processJoint(joint) { + + const children = [ ...joint.children ]; + const jointType = joint.getAttribute('type'); + + let obj; + + const mimicTag = children.find(n => n.nodeName.toLowerCase() === 'mimic'); + if (mimicTag) { + + obj = new URDFMimicJoint(); + obj.mimicJoint = mimicTag.getAttribute('joint'); + obj.multiplier = parseFloat(mimicTag.getAttribute('multiplier') || 1.0); + obj.offset = parseFloat(mimicTag.getAttribute('offset') || 0.0); + + } else { + + obj = new URDFJoint(); + + } + + obj.urdfNode = joint; + obj.name = joint.getAttribute('name'); + obj.urdfName = obj.name; + obj.jointType = jointType; + + let parent = null; + let child = null; + let xyz = [0, 0, 0]; + let rpy = [0, 0, 0]; + + // Extract the attributes + children.forEach(n => { + + const type = n.nodeName.toLowerCase(); + if (type === 'origin') { + + xyz = processTuple(n.getAttribute('xyz')); + rpy = processTuple(n.getAttribute('rpy')); + + } else if (type === 'child') { + + child = linkMap[n.getAttribute('link')]; + + } else if (type === 'parent') { + + parent = linkMap[n.getAttribute('link')]; + + } else if (type === 'limit') { + + obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower); + obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper); + + } + }); + + // Join the links + parent.add(obj); + obj.add(child); + applyRotation(obj, rpy); + obj.position.set(xyz[0], xyz[1], xyz[2]); + + // Set up the rotate function + const axisNode = children.filter(n => n.nodeName.toLowerCase() === 'axis')[0]; + + if (axisNode) { + + const axisXYZ = axisNode.getAttribute('xyz').split(/\s+/g).map(num => parseFloat(num)); + obj.axis = new THREE.Vector3(axisXYZ[0], axisXYZ[1], axisXYZ[2]); + obj.axis.normalize(); + + } + + return obj; + + } + + // Process the nodes + function processLink(link, visualMap, colliderMap, target = null) { + + if (target === null) { + + target = new URDFLink(); + + } + + const children = [ ...link.children ]; + target.name = link.getAttribute('name'); + target.urdfName = target.name; + target.urdfNode = link; + + if (parseVisual) { + + const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual'); + visualNodes.forEach(vn => { + + const v = processLinkElement(vn, materialMap); + target.add(v); + + if (vn.hasAttribute('name')) { + + const name = vn.getAttribute('name'); + v.name = name; + v.urdfName = name; + visualMap[name] = v; + + } + + }); + + } + + if (parseCollision) { + + const collisionNodes = children.filter(n => n.nodeName.toLowerCase() === 'collision'); + collisionNodes.forEach(cn => { + + const c = processLinkElement(cn); + target.add(c); + + if (cn.hasAttribute('name')) { + + const name = cn.getAttribute('name'); + c.name = name; + c.urdfName = name; + colliderMap[name] = c; + + } + + }); + + } + + return target; + + } + + function processMaterial(node) { + + const matNodes = [ ...node.children ]; + const material = new THREE.MeshPhongMaterial(); + + material.name = node.getAttribute('name') || ''; + matNodes.forEach(n => { + + const type = n.nodeName.toLowerCase(); + if (type === 'color') { + + const rgba = + n + .getAttribute('rgba') + .split(/\s/g) + .map(v => parseFloat(v)); + + material.color.setRGB(rgba[0], rgba[1], rgba[2]); + material.opacity = rgba[3]; + material.transparent = rgba[3] < 1; + material.depthWrite = !material.transparent; + + } else if (type === 'texture') { + + // The URDF spec does not require that the tag include + // a filename attribute so skip loading the texture if not provided. + const filename = n.getAttribute('filename'); + if (filename) { + + const loader = new THREE.TextureLoader(manager); + const filePath = resolvePath(filename); + material.map = loader.load(filePath); + material.map.colorSpace = THREE.SRGBColorSpace; + + } + + } + }); + + return material; + + } + + // Process the visual and collision nodes into meshes + function processLinkElement(vn, materialMap = {}) { + + const isCollisionNode = vn.nodeName.toLowerCase() === 'collision'; + const children = [ ...vn.children ]; + let material = null; + + // get the material first + const materialNode = children.filter(n => n.nodeName.toLowerCase() === 'material')[0]; + if (materialNode) { + + const name = materialNode.getAttribute('name'); + if (name && name in materialMap) { + + material = materialMap[name]; + + } else { + + material = processMaterial(materialNode); + + } + + } else { + + material = new THREE.MeshPhongMaterial(); + + } + + const group = isCollisionNode ? new URDFCollider() : new URDFVisual(); + group.urdfNode = vn; + + children.forEach(n => { + + const type = n.nodeName.toLowerCase(); + if (type === 'geometry') { + + const geoType = n.children[0].nodeName.toLowerCase(); + if (geoType === 'mesh') { + + const filename = n.children[0].getAttribute('filename'); + const filePath = resolvePath(filename); + + // file path is null if a package directory is not provided. + if (filePath !== null) { + + const scaleAttr = n.children[0].getAttribute('scale'); + if (scaleAttr) { + + const scale = processTuple(scaleAttr); + group.scale.set(scale[0], scale[1], scale[2]); + + } + + loadMeshCb(filePath, manager, (obj, err) => { + + if (err) { + + console.error('URDFLoader: Error loading mesh.', err); + + } else if (obj) { + + if (obj instanceof THREE.Mesh) { + + obj.material = material; + + } + + // We don't expect non identity rotations or positions. In the case of + // COLLADA files the model might come in with a custom scale for unit + // conversion. + obj.position.set(0, 0, 0); + obj.quaternion.identity(); + group.add(obj); + + } + + }); + + } + + } else if (geoType === 'box') { + + const primitiveModel = new THREE.Mesh(); + primitiveModel.geometry = new THREE.BoxGeometry(1, 1, 1); + primitiveModel.material = material; + + const size = processTuple(n.children[0].getAttribute('size')); + primitiveModel.scale.set(size[0], size[1], size[2]); + + group.add(primitiveModel); + + } else if (geoType === 'sphere') { + + const primitiveModel = new THREE.Mesh(); + primitiveModel.geometry = new THREE.SphereGeometry(1, 30, 30); + primitiveModel.material = material; + + const radius = parseFloat(n.children[0].getAttribute('radius')) || 0; + primitiveModel.scale.set(radius, radius, radius); + + group.add(primitiveModel); + + } else if (geoType === 'cylinder') { + + const primitiveModel = new THREE.Mesh(); + primitiveModel.geometry = new THREE.CylinderGeometry(1, 1, 1, 30); + primitiveModel.material = material; + + const radius = parseFloat(n.children[0].getAttribute('radius')) || 0; + const length = parseFloat(n.children[0].getAttribute('length')) || 0; + primitiveModel.scale.set(radius, length, radius); + primitiveModel.rotation.set(Math.PI / 2, 0, 0); + + group.add(primitiveModel); + + } + + } else if (type === 'origin') { + + const xyz = processTuple(n.getAttribute('xyz')); + const rpy = processTuple(n.getAttribute('rpy')); + + group.position.set(xyz[0], xyz[1], xyz[2]); + group.rotation.set(0, 0, 0); + applyRotation(group, rpy); + + } + + }); + + return group; + + } + + return processUrdf(content); + + } + + // Default mesh loading function + defaultMeshLoader(path, manager, done) { + + if (/\.stl$/i.test(path)) { + + const loader = new STLLoader(manager); + loader.load(path, geom => { + const mesh = new THREE.Mesh(geom, new THREE.MeshPhongMaterial()); + done(mesh); + }); + + } else if (/\.dae$/i.test(path)) { + + const loader = new ColladaLoader(manager); + loader.load(path, dae => done(dae.scene)); + + } else { + + console.warn(`URDFLoader: Could not load model at ${ path }.\nNo loader available`); + + } + + } + +}; \ No newline at end of file diff --git a/ros_robot_visualiser/urdf/output.urdf b/ros_robot_visualiser/urdf/output.urdf new file mode 100644 index 0000000..f7b186e --- /dev/null +++ b/ros_robot_visualiser/urdf/output.urdf @@ -0,0 +1,446 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_robot_visualiser/urdf/sample.urdf b/ros_robot_visualiser/urdf/sample.urdf new file mode 100644 index 0000000..727e565 --- /dev/null +++ b/ros_robot_visualiser/urdf/sample.urdf @@ -0,0 +1,705 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 500 + 4095 + 270 + + + + \ No newline at end of file diff --git a/ros_robot_visualiser/urdf/test b/ros_robot_visualiser/urdf/test new file mode 100644 index 0000000..e69de29