From 06a44e91eb5f0464ec4ed1095639298eef58963a Mon Sep 17 00:00:00 2001 From: Jake Wilkinson Date: Tue, 18 Nov 2025 11:49:13 +0800 Subject: [PATCH] joint motion and indicators now match encoder max ticks, limits, and range, as well as mechanical reduction of joint --- JointVisualiser.js | 6 +-- URDFEditor.js | 81 +++++++++++++++++++++++++++----------- urdf/ExtendedURDFLoader.js | 10 +++-- urdf/sample.urdf | 49 +++++++++++++++-------- 4 files changed, 100 insertions(+), 46 deletions(-) diff --git a/JointVisualiser.js b/JointVisualiser.js index 8559e0c..0ad7f42 100644 --- a/JointVisualiser.js +++ b/JointVisualiser.js @@ -1,9 +1,9 @@ import * as THREE from 'three'; -export function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) { +export function createRotationSector(axis, lower, upper, encoderRange, radius = 0.1, segments = 32) { const motorMargin = 0.4; - const motorLower = lower - motorMargin; - const motorUpper = upper + motorMargin; + const motorLower = -encoderRange/2 * (Math.PI / 180);// -Math.PI/2;//lower - motorMargin; + const motorUpper = encoderRange/2 * (Math.PI / 180);//Math.PI/2;//upper + motorMargin; const redMaterial = new THREE.MeshBasicMaterial({ color: 0xff0000, transparent: true, opacity: 0.2, side: THREE.DoubleSide, depthTest: false, depthWrite: false }); const whiteMaterial = new THREE.MeshBasicMaterial({ color: 0xffffff, transparent: true, opacity: 0.6, side: THREE.DoubleSide, depthTest: false, depthWrite: false }); diff --git a/URDFEditor.js b/URDFEditor.js index 8438843..f517a41 100644 --- a/URDFEditor.js +++ b/URDFEditor.js @@ -1,7 +1,6 @@ -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 * 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'; @@ -118,19 +117,21 @@ export class URDFEditor { 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; + + // ✅ Use transmission-derived limits if available + const { lower, upper } = getJointLimits(jointName, this.robot); const currentAngle = this.jointAngles[jointName] ?? 0; const proposedAngle = currentAngle + angleDelta; + + // ✅ Clamp to safe range const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle)); const actualDelta = clampedAngle - currentAngle; this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta); this.jointAngles[jointName] = clampedAngle; - + // ✅ Update gizmo indicator const gizmo = this.draggedJoint.userData.gizmo; if (gizmo?.indicator) this.draggedJoint.parent.remove(gizmo.indicator); @@ -140,17 +141,25 @@ export class URDFEditor { this.draggedJoint.userData.gizmo.indicator = newIndicator; } + // Hover highlighting if (intersects.length > 0) { const target = intersects[0].object; - if (this.hoveredJoint?.material?.emissive) this.hoveredJoint.material.emissive.setHex(0x000000); + 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); + if (this.hoveredJoint.material?.emissive) { + this.hoveredJoint.material.emissive.setHex(0x333333); + } } else { - if (this.hoveredJoint?.material?.emissive) this.hoveredJoint.material.emissive.setHex(0x000000); + if (this.hoveredJoint?.material?.emissive) { + this.hoveredJoint.material.emissive.setHex(0x000000); + } this.hoveredJoint = null; } } + onPointerDown(event) { if (!this.hoveredJoint) return; @@ -169,11 +178,14 @@ export class URDFEditor { 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); + // ✅ Use transmission-derived limits if available + const { lower, upper } = getJointLimits(jointName, this.robot); + console.log(lower, upper); + + const jointTransmission = this.robot.joints[jointName].transmission; + const encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction; + const sector = createRotationSector(this.worldAxis, lower, upper, encoderRange); const indicator = createAngleIndicator(this.worldAxis, this.jointAngles[jointName] ?? 0); sector.position.copy(this.draggedJoint.position); @@ -184,6 +196,7 @@ export class URDFEditor { this.draggedJoint.userData.gizmo = { sector, indicator }; } + onPointerUp() { this.isDragging = false; this.controls.enabled = true; @@ -249,22 +262,46 @@ export class URDFEditor { const t = jointData.transmission; if (t) { - const actuatorAngleDeg = (degrees * t.gearRatio) + 4096/2; - const ticks = Math.round(actuatorAngleDeg); + //const actuatorAngleDeg = (degrees / t.mechanicalReduction) + 4096 / 2; + const ticks = radiansToTicks(angle, t).toFixed(0); const isValid = ticks >= t.encoderValidMin && ticks <= t.encoderValidMax; //ctx.fillStyle = isValid ? '#0f0' : '#f00'; ctx.fillText(`${ticks}`, nameX + 250, y); - - } y += 18; } } +} + +function ticksToRadians(ticks, actuator) { + const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75 + const centered = ticks - actuator.encoderTicks / 2; // shift so mid = 0 + const actuatorDeg = centered / ticksPerDeg; + const jointDeg = actuatorDeg / actuator.mechanicalReduction; + return jointDeg * (Math.PI / 180); // convert degrees → radians +} + +function radiansToTicks(radians, actuator) { + const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75 + const jointDeg = radians * (180 / Math.PI); // radians → degrees + const actuatorDeg = jointDeg * actuator.mechanicalReduction; // apply reduction + const centered = actuatorDeg * ticksPerDeg; // convert to ticks offset from mid + const ticks = centered + actuator.encoderTicks / 2; // shift back to full range + return ticks; +} +function getJointLimits(jointName, robot) { + const transmission = robot.joints[jointName].transmission; + //console.log(transmission.encoderValidMin, transmission.encoderValidMax); + const lowerRad = ticksToRadians(transmission.encoderValidMin, transmission); + const upperRad = ticksToRadians(transmission.encoderValidMax, transmission); + //console.log(lowerRad, upperRad); + return { lower: lowerRad, upper: upperRad }; +} - - -} \ No newline at end of file +function getJointTransmission(jointName, robot){ + return robot.joints[jointName].transmission; +} diff --git a/urdf/ExtendedURDFLoader.js b/urdf/ExtendedURDFLoader.js index 73c7e4c..9dc2fc3 100644 --- a/urdf/ExtendedURDFLoader.js +++ b/urdf/ExtendedURDFLoader.js @@ -16,17 +16,19 @@ class ExtendedURDFLoader extends URDFLoader { 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 encoderTicks = parseInt(actuator?.querySelector('encoderTicks')?.textContent ?? '4096'); const encoderValidMin = parseInt(actuator?.querySelector('encoderValidMin')?.textContent ?? '0'); const encoderValidMax = parseInt(actuator?.querySelector('encoderValidMax')?.textContent ?? '4095'); + const encoderRange = parseInt(actuator?.querySelector('encoderRange')?.textContent ?? '180'); if (jointName) { this.transmissionMap[jointName] = { - gearRatio: reduction, + mechanicalReduction: reduction, actuatorName, - encoderMax, + encoderTicks, encoderValidMin, - encoderValidMax + encoderValidMax, + encoderRange }; } }); diff --git a/urdf/sample.urdf b/urdf/sample.urdf index dc702c5..202d7e8 100644 --- a/urdf/sample.urdf +++ b/urdf/sample.urdf @@ -512,11 +512,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -526,11 +527,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -540,11 +542,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -554,11 +557,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -568,11 +572,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -582,11 +587,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -596,11 +602,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -610,11 +617,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -624,11 +632,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -638,11 +647,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -652,11 +662,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -666,11 +677,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -680,11 +692,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -693,11 +706,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 200 3500 + 270 @@ -706,11 +720,12 @@ PositionJointInterface - 22.75 + 1 PositionJointInterface 4096 - 200 - 3500 + 500 + 4095 + 270