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>
|
<body>
|
||||||
<div id="status">Loading URDF...</div>
|
<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 -->
|
<!-- Your logic -->
|
||||||
|
|
|
||||||
360
script.js
360
script.js
|
|
@ -1,356 +1,4 @@
|
||||||
import * as THREE from 'three';
|
import { URDFEditor } from './URDFEditor.js';
|
||||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
|
||||||
import URDFLoader from './urdf/URDFLoader.js';
|
const canvas = document.getElementById('urdfCanvas');
|
||||||
|
const editor = new URDFEditor(canvas, './urdf/sample.urdf');
|
||||||
// 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,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" />
|
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</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>
|
</robot>
|
||||||
Loading…
Reference in New Issue