extended URDFLoader.js to include transmission details (motor specs, gear ratios)
parent
52bf4dc0bc
commit
c46d457d1e
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -27,7 +27,11 @@
|
|||
|
||||
<body>
|
||||
<div id="status">Loading URDF...</div>
|
||||
<canvas id="urdf-canvas"></canvas>
|
||||
<div style="position: relative; width: 100vw; height: 100vh;">
|
||||
<canvas id="urdfCanvas" style="width: 100%; height: 100%; display: block;"></canvas>
|
||||
<canvas id="overlay-canvas" style="position: absolute; top: 0; left: 0; pointer-events: none;"></canvas>
|
||||
</div>
|
||||
|
||||
|
||||
|
||||
<!-- Your logic -->
|
||||
|
|
|
|||
360
script.js
360
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');
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -504,4 +504,19 @@
|
|||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<transmission name="spine_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_spine_1">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>22.75</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
Loading…
Reference in New Issue