From 5a8dbc28a78d6361fcca71fa7112fb3526e8782a Mon Sep 17 00:00:00 2001 From: Jake Wilkinson Date: Mon, 24 Nov 2025 00:16:25 +0800 Subject: [PATCH] keyframe saving works, save/load broken --- curveEditor.js | 217 +++++++++++++++++--------- ros_robot_visualiser/URDFEditor.js | 92 +++++++++-- ros_robot_visualiser/ViewerOverlay.js | 22 ++- ros_robot_visualiser/ui/canvasui.js | 7 +- script.js | 42 +++-- 5 files changed, 282 insertions(+), 98 deletions(-) diff --git a/curveEditor.js b/curveEditor.js index eb65fe3..f77c084 100644 --- a/curveEditor.js +++ b/curveEditor.js @@ -117,19 +117,22 @@ export class CurveEditor { } addChannel(motorID) { + const midY = this.logicalHeight / 2;//this.offset.y + (this.logicalHeight / 2) * this.scaleY; + this.setCurves([ { - startPoint: { x: 0, y: this.canvas.height / 2 }, - startPointHandle: { x: this.timelineLength * this.pixelsPerSecond * 0.25, y: this.canvas.height / 2 }, - endPointHandle: { x: this.timelineLength * this.pixelsPerSecond * 0.75, y: this.canvas.height / 2 }, - endPoint: { x: this.timelineLength * this.pixelsPerSecond, y: this.canvas.height / 2 } + startPoint: { x: 0, y: midY }, + startPointHandle: { x: this.timelineLength * this.pixelsPerSecond * 0.25, y: midY }, + endPointHandle: { x: this.timelineLength * this.pixelsPerSecond * 0.75, y: midY }, + endPoint: { x: this.timelineLength * this.pixelsPerSecond, y: midY } } ]); - //console.log("TL LENGTH: " + this.timelineLength); + this.curveSets[motorID] = this.curves; } + setLength(endTime) { this.timelineLength = endTime / this.pixelsPerSecond; this.slider.max = this.timelineLength * this.pixelsPerSecond; @@ -143,7 +146,7 @@ export class CurveEditor { this.draw(); } - getLength(){ + getLength() { return this.timelineLength * this.pixelsPerSecond; } @@ -190,6 +193,11 @@ export class CurveEditor { return (this.canvas.height / 2 - v * (this.canvas.height / 2)); } + yToValueNoOffset(y) { + return 1 - (2 * y / this.canvas.height); + } + + valueToY(v) { return (this.logicalHeight / 2 - v * (this.logicalHeight / 2)) * this.scaleY + this.offset.y; } @@ -200,21 +208,46 @@ export class CurveEditor { // Maps normalised -1 to 1 value to motor range (0, 4095) - yToExportRange(y) { + yToExportRange(yCanvas) { const [minOut, maxOut] = this.exportRange; - const clampedY = Math.max(0, Math.min(y, this.logicalHeight)); // optional safety - const normalized = clampedY / this.logicalHeight; // maps to [0, 1] - return Math.round(normalized * (maxOut - minOut) + minOut); // maps to [minOut, maxOut] + + // undo offset + scale to get logical Y + const yLogical = (yCanvas - this.offset.y) / this.scaleY; + + // invert valueToYNoOffset: get back logical [-1,1] + const logical = this.yToValueNoOffset(yLogical); + + // map logical [-1,1] → normalized [0,1] + const normalized = (logical + 1) / 2; + + // flip back (since exportRangeToY used 1 - normalized) + const flipped = 1 - normalized; + + // map to export range + return (flipped * (maxOut - minOut) + minOut)/2; } + exportRangeToY(value) { const [minOut, maxOut] = this.exportRange; - const normalized = 1 - (value - minOut) / (maxOut - minOut); // flipped - return this.valueToYNoOffset(normalized * 2 - 1); + + // normalize value into [0,1], flipped + const normalized = 1 - (value - minOut) / (maxOut - minOut); + + // map into logical [-1,1] + const logical = normalized * 2 - 1; + + // convert logical → logical Y + const yLogical = this.valueToYNoOffset(logical); + + // apply scale + offset to get canvas Y + return yLogical * this.scaleY + this.offset.y; } + + valueToX(value) { return value * this.pixelsPerSecond * this.scaleX + this.offset.x; } @@ -355,16 +388,13 @@ export class CurveEditor { getMotorPositionAtTime(motorID, timeInFrames) { - if (this.curveSets[motorID] === undefined || this.curveSets[motorID].length === 0) { - console.log("THIS"); - return null; - } const curves = this.curveSets[motorID]; - const timeInSeconds = timeInFrames / this.framesPerSecond; - const currentTimeX = timeInFrames; + if (!curves || curves.length === 0) return null; + + // put time into canvas space if you’re transforming points + const currentTimeX = this.offset.x + timeInFrames * this.scaleX; for (let curve of curves) { - const p0 = this.transform(curve.startPoint); const h0 = this.transform(curve.startPointHandle); const h1 = this.transform(curve.endPointHandle); @@ -372,18 +402,20 @@ export class CurveEditor { const minX = Math.min(p0.x, p1.x); const maxX = Math.max(p0.x, p1.x); + if (currentTimeX >= minX && currentTimeX <= maxX) { const t = this.solveTForX(currentTimeX, p0, h0, h1, p1); const pt = this.cubicBezier(t, p0, h0, h1, p1); - return this.yToExportRange(pt.y); + return this.yToExportRange(pt.y); // or pt.y if you want raw canvas Y } } - return null; // No curve segment matched the time + return null; } + drawCurves(ctx, curves, opacity = 1.0, showControlPoints = true) { ctx.save(); ctx.globalAlpha = opacity; @@ -706,62 +738,101 @@ export class CurveEditor { this.canvas.addEventListener('dblclick', e => { const mouse = this.inverseTransform({ x: e.offsetX, y: e.offsetY }); + //const xPosInTicks = parseInt(this.xToValue(mouse.x)*this.pixelsPerSecond); - for (let i = 0; i < this.curves.length; i++) { - const curve = this.curves[i]; - - if (i > 0 && this.isNearPoint(mouse, curve.startPoint)) { - const prev = this.curves[i - 1]; - const merged = { - startPoint: prev.startPoint, - startPointHandle: prev.startPointHandle, - endPointHandle: curve.endPointHandle, - endPoint: curve.endPoint - }; - this.curves.splice(i - 1, 2, merged); - this.draw(); - this.curveSets[this.selectedMotorID] = this.curves; - - return; - } - - if (i < this.curves.length - 1 && this.isNearPoint(mouse, curve.endPoint)) { - const next = this.curves[i + 1]; - const merged = { - startPoint: curve.startPoint, - startPointHandle: curve.startPointHandle, - endPointHandle: next.endPointHandle, - endPoint: next.endPoint - }; - this.curves.splice(i, 2, merged); - this.draw(); - this.curveSets[this.selectedMotorID] = this.curves; - - return; - } - - const x0 = curve.startPoint.x; - const x3 = curve.endPoint.x; - if (mouse.x >= x0 && mouse.x <= x3) { - let t = 0.5, minT = 0, maxT = 1; - for (let j = 0; j < 10; j++) { - const pt = this.cubicBezier(t, curve.startPoint, curve.startPointHandle, curve.endPointHandle, curve.endPoint); - if (pt.x < mouse.x) minT = t; - else maxT = t; - t = (minT + maxT) / 2; - } - - const [left, right] = this.splitCurve(curve, t); - this.curves.splice(i, 1, left, right); - this.draw(); - this.curveSets[this.selectedMotorID] = this.curves; - - return; - } - } + //console.log(mouse.x); + this.splitCurveAtTime(this.selectedMotorID, mouse.x); }); + + } + splitCurveAtTime(motorID, timeX, yPosition = null) { + const curves = this.curveSets[motorID]; + if (!curves) return; + + for (let i = 0; i < curves.length; i++) { + const curve = curves[i]; + + // --- merge with previous if near startPoint --- + if (i > 0 && Math.abs(curve.startPoint.x - timeX) < 1e-3) { + const prev = curves[i - 1]; + const merged = { + startPoint: prev.startPoint, + startPointHandle: prev.startPointHandle, + endPointHandle: curve.endPointHandle, + endPoint: curve.endPoint + }; + curves.splice(i - 1, 2, merged); + this.curveSets[motorID] = curves; + this.draw(); + return; + } + + // --- merge with next if near endPoint --- + if (i < curves.length - 1 && Math.abs(curve.endPoint.x - timeX) < 1e-3) { + const next = curves[i + 1]; + const merged = { + startPoint: curve.startPoint, + startPointHandle: curve.startPointHandle, + endPointHandle: next.endPointHandle, + endPoint: next.endPoint + }; + + curves.splice(i, 2, merged); + this.curveSets[motorID] = curves; + this.draw(); + return; + } + + // --- split inside curve if timeX lies between start and end --- + const x0 = curve.startPoint.x; + const x3 = curve.endPoint.x; + + if (timeX >= x0 && timeX <= x3) { + // binary search for t where curve.x ≈ timeX + let t = 0.5, minT = 0, maxT = 1; + for (let j = 0; j < 10; j++) { + const pt = this.cubicBezier( + t, + curve.startPoint, + curve.startPointHandle, + curve.endPointHandle, + curve.endPoint + ); + + + if (pt.x < timeX) minT = t; + else maxT = t; + t = (minT + maxT) / 2; + } + + let [left, right] = this.splitCurve(curve, t); + + // ✅ If yPosition provided, override the split point’s Y + if (yPosition !== null) { + const yEditor = yPosition * 800 / 4095 + + // adjust the shared split point + left.endPoint.y = yEditor; + right.startPoint.y = yEditor; + + // ✅ flatten handles to same Y + if (left.endPointHandle) left.endPointHandle.y = yEditor; + if (right.startPointHandle) right.startPointHandle.y = yEditor; + } + + curves.splice(i, 1, left, right); + this.curveSets[motorID] = curves; + this.draw(); + return; + } + } + } + + + + adjustAllCurvesToDuration(newTime) { for (const [motorID, curves] of Object.entries(this.curveSets)) { if (!curves || curves.length === 0) continue; diff --git a/ros_robot_visualiser/URDFEditor.js b/ros_robot_visualiser/URDFEditor.js index 0eb6362..7d9b037 100644 --- a/ros_robot_visualiser/URDFEditor.js +++ b/ros_robot_visualiser/URDFEditor.js @@ -46,6 +46,7 @@ export class URDFEditor { this.hoveredJoint = null; this.draggedJoint = null; + this.selectedJoint = null; this.worldAxis = null; this.lastX = null; this.isDragging = false; @@ -390,22 +391,34 @@ export class URDFEditor { + // Hover highlighting // Hover highlighting if (intersects.length > 0) { const target = intersects[0].object; - if (this.hoveredJoint?.material?.emissive) { - this.hoveredJoint.material.emissive.setHex(0x000000); + + // reset previous hover if it's not selected + if (this.hoveredJoint && this.hoveredJoint !== this.selectedJoint) { + 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); + // use a different color if it's also selected + const color = (this.hoveredJoint === this.selectedJoint) ? 0x555555 : 0x333333; + this.hoveredJoint.material.emissive.setHex(color); } } else { - if (this.hoveredJoint?.material?.emissive) { - this.hoveredJoint.material.emissive.setHex(0x000000); + if (this.hoveredJoint && this.hoveredJoint !== this.selectedJoint) { + if (this.hoveredJoint?.material?.emissive) { + this.hoveredJoint.material.emissive.setHex(0x000000); + } } this.hoveredJoint = null; } + } setSyncMode(newMode) { @@ -419,6 +432,61 @@ export class URDFEditor { } } + findMotorByID(motorID) { + for (const jointName in this.robot.joints) { + const joint = this.robot.joints[jointName]; + const transmission = joint.transmission; + if (!transmission) continue; + + if (transmission.motorID === motorID) { + return transmission; + } + } + } + + findAllMotorIDs() { + let list = []; + for (const jointName in this.robot.joints) { + const joint = this.robot.joints[jointName]; + const transmission = joint.transmission; + if (!transmission) continue; + list.push(transmission.motorID); + } + return list; + } + + getMotorTicks(motorID) { + if (!this.robot?.joints) return null; + for (const [jointName, jointData] of Object.entries(this.robot.joints)) { + const t = jointData.transmission; + if (!t) continue; + //console.log(t); + if (t.motorID === motorID) { + + const angle = this.jointAngles?.[jointName] ?? 0; + return parseInt(radiansToTicks(angle, t)); + } + } + + return null; // not found + } + + syncWithCurveEditor() { + let allIDs = this.findAllMotorIDs(); + + let ids = []; + let positions = []; + allIDs.forEach(id => { + let pos = this.curveEditor.getMotorPositionAtTime(id, this.curveEditor.currentTime); + this.setMotorPosition(id, pos); + + ids.push(id); + positions.push(pos); + }); + if (this.currentSyncMode === SyncMode.SimToReal) { + this.sendMotorPosition(ids, positions); + } + } setMotorPosition(motorID, positionTicks) { for (const jointName in this.robot.joints) { @@ -459,11 +527,6 @@ export class URDFEditor { } } - - - - - onRotateMotor(motorID, positionTicks) { if (this.currentSyncMode === SyncMode.SimToReal) { this.sendMotorPosition(motorID, positionTicks); @@ -473,6 +536,15 @@ export class URDFEditor { onPointerDown(event) { if (!this.hoveredJoint) return; + if (this.hoveredJoint) { + // reset previous selection + if (this.selectedJoint) { + this.selectedJoint.material.emissive.setHex(0x000000); + } + this.selectedJoint = this.hoveredJoint; + this.selectedJoint.material.emissive.setHex(0x555555); // selection color + } + this.isDragging = true; this.lastX = event.clientX; this.controls.enabled = false; diff --git a/ros_robot_visualiser/ViewerOverlay.js b/ros_robot_visualiser/ViewerOverlay.js index bfe2e33..411112f 100644 --- a/ros_robot_visualiser/ViewerOverlay.js +++ b/ros_robot_visualiser/ViewerOverlay.js @@ -165,12 +165,30 @@ export class ViewerOverlay { if (slider.max != this.parent.curveEditor.getLength()) { slider.max = this.parent.curveEditor.getLength(); } + this.parent.syncWithCurveEditor(); } ); - - panel.addElement(slider); + panel.addElement(new Button(panel.x + 200, panel.y + 40, 200, 24, "Record Keyframe ALL", () => { + let allIDs = this.parent.findAllMotorIDs(); + for (var i = 0; i < allIDs.length; i++){ + this.parent.curveEditor.splitCurveAtTime(allIDs[i], this.parent.curveEditor.currentTime, this.parent.getMotorTicks(allIDs[i])); + } + })); + + panel.addElement(new Button(panel.x + 200, panel.y + 40 + 24*2, 200, 24, "Record Keyframe Selected", () => { + if (!this.parent.selectedJoint){ + return; + } + const selectedID = this.parent.findJointAncestor(this.parent.selectedJoint, 0).transmission.motorID + const currentTime = this.parent.curveEditor.currentTime; + const motorPosition = this.parent.getMotorTicks(selectedID) + this.parent.curveEditor.splitCurveAtTime(selectedID, currentTime, motorPosition); + console.log(motorPosition, this.parent.curveEditor.exportRangeToY(motorPosition), this.parent.curveEditor.yToExportRange(this.parent.curveEditor.exportRangeToY(motorPosition))); + //console.log(selectedID, currentTime, motorPosition); + })); + return panel; } diff --git a/ros_robot_visualiser/ui/canvasui.js b/ros_robot_visualiser/ui/canvasui.js index 51c8ba6..a825f40 100644 --- a/ros_robot_visualiser/ui/canvasui.js +++ b/ros_robot_visualiser/ui/canvasui.js @@ -8,7 +8,7 @@ class UIElement { this.hovered = false; this.active = false; - + this.tooltipDelay = 1000; // ms before showing this.hoverStart = null; // timestamp when hover began this.tooltip = tooltipText ? new Tooltip(tooltipText) : null; @@ -80,7 +80,7 @@ class UIElement { if (this.tooltip && this.hovered && this.hoverStart) { const elapsed = performance.now() - this.hoverStart; if (elapsed >= this.tooltipDelay) { - this.tooltip.show(this.x + 100, this.y + this.h*2); + this.tooltip.show(this.x + 100, this.y + this.h * 2); this.tooltip.draw(ctx); } else { this.tooltip.hide(); @@ -384,7 +384,8 @@ export class Slider extends UIElement { // knob ctx.fillStyle = this.hovered || this.dragging ? '#0f0' : '#fff'; ctx.beginPath(); - ctx.arc(knobX, knobY, this.h / 2, 0, Math.PI * 2); + const size = this.h; // or this.h/2 if you want it smaller + ctx.rect(knobX - size / 2, knobY - size / 2, size/2, size); ctx.fill(); ctx.strokeStyle = '#333'; ctx.stroke(); diff --git a/script.js b/script.js index d0ab2ce..881700c 100644 --- a/script.js +++ b/script.js @@ -33,8 +33,8 @@ window.onload = () => { let draggingKeyframe = null; // { dialIndex, originalFrame } let isDragging = false; - - + + const curveCanvas = document.getElementById('curveCanvas'); const curveEditor = new CurveEditor(curveCanvas, 10, frameSlider); @@ -226,25 +226,47 @@ window.onload = () => { }; function sendMotorPosition(motorID, position) { - const now = Date.now(); - if (now - lastSyncTime >= syncIntervalMs) { - lastSyncTime = now; - // Each payload is 3 bytes: [motorId (uint8), position (uint16 little-endian)] + if (now - lastSyncTime < syncIntervalMs) return; + lastSyncTime = now; + + // Case 1: arrays → multiple motors + if (Array.isArray(motorID) && Array.isArray(position)) { + if (motorID.length !== position.length) { + console.error("motorID and position arrays must be same length"); + return; + } + + const buffer = new ArrayBuffer(motorID.length * 3); + const view = new DataView(buffer); + + motorID.forEach((id, i) => { + const pos = position[i]; + const offset = i * 3; + view.setUint8(offset, id); + view.setUint16(offset + 1, pos, true); // little-endian + }); + + const payload = new Uint8Array(buffer); + serial.sendSetPositions(payload); + //console.log("Multi-motor payload:", payload); + } + + // Case 2: single motor + else { const buffer = new ArrayBuffer(3); const view = new DataView(buffer); view.setUint8(0, motorID); - view.setUint16(1, position, true); // little-endian + view.setUint16(1, position, true); const payload = new Uint8Array(buffer); - - // Send upward to parent / serial serial.sendSetPositions(payload); - //console.log(payload); + //console.log("Single-motor payload:", payload); } } + function syncMotorsWithTimeline() { const now = Date.now();