270 lines
8.4 KiB
JavaScript
270 lines
8.4 KiB
JavaScript
import * as THREE from '/urdfviewer/node_modules/three/build/three.module.js';
|
|
import { OrbitControls } from '/urdfviewer/node_modules/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;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
} |