diff --git a/curveEditor.js b/curveEditor.js index f77c084..4546d16 100644 --- a/curveEditor.js +++ b/curveEditor.js @@ -11,6 +11,14 @@ export class CurveEditor { this.scaleX = 1; this.scaleY = 1; this.offset = { x: 0, y: 0 }; + + this.scaleX = 1.66; + this.scaleY = 0.41; + this.offset.x = 177; + this.offset.y = 38; + + + this.pixelsPerSecond = 48; this.exportRange = [0, 4095]; @@ -69,8 +77,8 @@ export class CurveEditor { const curveSegments = []; Object.entries(this.curveSets).forEach(([motorIDStr, segments]) => { const motorID = parseInt(motorIDStr, 10); - segments.forEach(segment => { + segments.forEach(segment => { curveSegments.push({ motorID, startTime: segment.startPoint.x, @@ -84,14 +92,14 @@ export class CurveEditor { }); }); }); - console.log(curveSegments); + //console.log(curveSegments); const curveCount = curveSegments.length; view.setUint16(offset, curveCount, true); offset += 2; // 🔹 Curve segments curveSegments.forEach(seg => { - console.log(offset, seg.motorID); + //console.log(offset, seg.motorID); view.setUint8(offset++, seg.motorID); view.setUint16(offset, seg.startTime, true); offset += 2; view.setUint16(offset, seg.endTime, true); offset += 2; @@ -101,7 +109,7 @@ export class CurveEditor { view.setUint16(offset, seg.endHandleX, true); offset += 2; view.setInt16(offset, this.yToExportRange(seg.endHandleY), true); offset += 2; view.setInt16(offset, this.yToExportRange(seg.endPointY), true); offset += 2; - console.log(seg.startPointY, seg.endPointY); + //console.log(seg.startPointY, seg.endPointY); console.log(this.yToExportRange(seg.startPointY), this.yToExportRange(seg.endPointY)); }); //console.log("🧵 Curve segments packed:", curveSegments.length); @@ -190,7 +198,7 @@ export class CurveEditor { // } valueToYNoOffset(v) { - return (this.canvas.height / 2 - v * (this.canvas.height / 2)); + return (this.logicalHeight / 2) * (1 - v); // Keep mapping logic as is } yToValueNoOffset(y) { @@ -206,42 +214,44 @@ export class CurveEditor { return ((this.logicalHeight / 2 - (y - this.offset.y) / this.scaleY) / (this.logicalHeight / 2)); } + // +yToMotorPosition(yCanvas) { + const [minOut, maxOut] = this.exportRange; // Output bounds (0 - 4095) - // Maps normalised -1 to 1 value to motor range (0, 4095) - yToExportRange(yCanvas) { + // Transform yCanvas to the original logical space by adjusting for the current offset and scale. + const yLogical = (yCanvas - this.offset.y) / this.scaleY; + + // Normalize the logical value based on the actual height of the logical range + const normalized = yLogical / 800; // This assumes yLogical ranges from 0 to 800 + + // Flip normalized to convert to desired motor range + const flipped = normalized; // This gives the inverted mapping + + // Calculate the final motor position + return Math.round(flipped * (maxOut - minOut) + minOut); +} + + + // exportRangeToY(value) { + // const [minOut, maxOut] = this.exportRange; + // const normalized = 1 - (value - minOut) / (maxOut - minOut); + // const logical = normalized * 2 - 1; + // const yLogical = this.valueToYNoOffset(logical); + // return yLogical * this.scaleY + this.offset.y; + // } + + yToExportRange(y) { const [minOut, maxOut] = this.exportRange; - - // 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; + 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] } - exportRangeToY(value) { const [minOut, maxOut] = this.exportRange; - - // normalize value into [0,1], flipped + // Inverted normalization 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; + return this.valueToYNoOffset(normalized * 2 - 1); // maps [0, 1] to [-1, 1] } @@ -406,7 +416,7 @@ export class CurveEditor { 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); // or pt.y if you want raw canvas Y + return this.yToMotorPosition(pt.y); // or pt.y if you want raw canvas Y } } @@ -731,7 +741,7 @@ export class CurveEditor { this.offset.x += (after.x - before.x) * this.scaleX; this.offset.y += (after.y - before.y) * this.scaleY; - + console.log(this.scaleX, this.scaleY, this.offset.x, this.offset.y); this.draw(); }); @@ -748,88 +758,74 @@ export class CurveEditor { } splitCurveAtTime(motorID, timeX, yPosition = null) { - const curves = this.curveSets[motorID]; - if (!curves) return; + const curves = this.curveSets[motorID]; + if (!curves) return; - for (let i = 0; i < curves.length; i++) { - const curve = curves[i]; + 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; - } - } + // --- if timeX matches an existing startPoint --- + if (Math.abs(curve.startPoint.x - timeX) < 1e-3) { + if (yPosition !== null) { + const yEditor = (yPosition * 800 / 4095); + curve.startPoint.y = yEditor; + if (curve.startPointHandle) curve.startPointHandle.y = yEditor; + } + this.draw(); + return; } + // --- if timeX matches an existing endPoint --- + if (Math.abs(curve.endPoint.x - timeX) < 1e-3) { + if (yPosition !== null) { + const yEditor = (yPosition * 800 / 4095); + curve.endPoint.y = yEditor; + if (curve.endPointHandle) curve.endPointHandle.y = yEditor; + } + 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); + left.endPoint.y = yEditor; + right.startPoint.y = yEditor; + 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; + } + } +} + + diff --git a/nodeeditor/NodeSerializer.js b/nodeeditor/NodeSerializer.js index 6f4bbef..322d896 100644 --- a/nodeeditor/NodeSerializer.js +++ b/nodeeditor/NodeSerializer.js @@ -11,7 +11,7 @@ export const NODE_TYPES = { }; function GetNodeType(node) { - console.log(node.constructor.name); + //console.log(node.constructor.name); switch (node.constructor.name) { case "ServoNode": return NODE_TYPES.Servo; diff --git a/ros_robot_visualiser/URDFEditor.js b/ros_robot_visualiser/URDFEditor.js index 7d9b037..2f2211e 100644 --- a/ros_robot_visualiser/URDFEditor.js +++ b/ros_robot_visualiser/URDFEditor.js @@ -18,11 +18,12 @@ const SyncMode = { }; export class URDFEditor { - constructor(canvas, sendMotorPosition, serial, curveEditor) { + constructor(canvas, sendMotorPosition, serial, curveEditor, tryConnect) { this.canvas = canvas; this.sendMotorPosition = sendMotorPosition; this.serial = serial; this.curveEditor = curveEditor; + this.tryConnect = tryConnect; this.scene = new THREE.Scene(); this.scene.background = new THREE.Color(0xaaaaaa); @@ -74,7 +75,7 @@ export class URDFEditor { this.setupScene(); //this.loadURDF(); - this.loadURDFFromIndexedDB(); + //this.loadURDFFromIndexedDB(); this.setupEvents(); const editorCallbacks = {} @@ -478,11 +479,14 @@ export class URDFEditor { let positions = []; allIDs.forEach(id => { let pos = this.curveEditor.getMotorPositionAtTime(id, this.curveEditor.currentTime); - this.setMotorPosition(id, pos); + if (this.currentSyncMode !== SyncMode.RealToSim) { + this.setMotorPosition(id, pos); + } ids.push(id); positions.push(pos); }); + if (this.currentSyncMode === SyncMode.SimToReal) { this.sendMotorPosition(ids, positions); } diff --git a/ros_robot_visualiser/ViewerOverlay.js b/ros_robot_visualiser/ViewerOverlay.js index 411112f..0de9a0c 100644 --- a/ros_robot_visualiser/ViewerOverlay.js +++ b/ros_robot_visualiser/ViewerOverlay.js @@ -54,7 +54,7 @@ export class ViewerOverlay { window.addEventListener('resize', resizeOverlay); resizeOverlay(); - this.panels.push(this.createAnimationControlPanel()); + //this.panels.push(this.createAnimationControlPanel()); this.panels.push(this.createSystemPanel()); const handlePointerEvent = (event) => { const rect = this.overlayCanvas.getBoundingClientRect(); @@ -100,9 +100,13 @@ export class ViewerOverlay { init(robot) { this.robot = robot; this.motorListPanel = this.createMotorListPanel(this.overlayCtx); + this.animationPanel = this.createAnimationControlPanel(this.overlayCtx); if (this.motorListPanel) { this.panels.push(this.motorListPanel); } + if (this.animationPanel) { + this.panels.push(this.animationPanel); + } this.draw(); } @@ -111,7 +115,7 @@ export class ViewerOverlay { // filter out the motorListPanel from panels this.panels = this.panels.filter(p => p !== this.motorListPanel); - + this.panels = this.panels.filter(p => p !== this.animationPanel); // clear the reference this.motorListPanel = null; } @@ -170,23 +174,23 @@ export class ViewerOverlay { ); panel.addElement(slider); - panel.addElement(new Button(panel.x + 200, panel.y + 40, 200, 24, "Record Keyframe ALL", () => { + 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++){ + 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){ + 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); + //console.log(motorPosition, this.parent.curveEditor.exportRangeToY(motorPosition), this.parent.curveEditor.yToExportRange(this.parent.curveEditor.exportRangeToY(motorPosition))); + console.log(selectedID, currentTime, motorPosition); })); return panel; @@ -202,32 +206,37 @@ export class ViewerOverlay { const panel = new Panel(x, y, w, h, "System"); + panel.addElement(new Button(x, y + 28, 50, 24, "Connect", () => { + // delegate to editor’s save + this.parent.tryConnect(); + })); + // Save button - panel.addElement(new Button(x, y + 28, 50, 24, "Save", () => { + panel.addElement(new Button(x, y + 28 + 24 * 1, 50, 24, "Save", () => { // delegate to editor’s save this.parent.saveURDFToIndexedDB(this.robot); })); // Load button - panel.addElement(new Button(x, y + 28 + 24 * 1, 50, 24, "Load", () => { + panel.addElement(new Button(x, y + 28 + 24 * 2, 50, 24, "Load", () => { // delegate to editor’s load this.parent.loadURDFFromIndexedDB(); })); - panel.addElement(new Button(x, y + 28 + 24 * 2, 50, 24, "Download", () => { + panel.addElement(new Button(x, y + 28 + 24 * 3, 50, 24, "Download", () => { // delegate to editor’s load this.parent.downloadURDF(); })); - panel.addElement(new Button(x, y + 28 + 24 * 3, 50, 24, "Upload", () => { + panel.addElement(new Button(x, y + 28 + 24 * 4, 50, 24, "Upload", () => { // delegate to editor’s load this.parent.uploadURDF(); })); - panel.addElement(new Button(x, y + 28 + 24 * 5, 50, 24, "Calibrate", () => { + panel.addElement(new Button(x, y + 28 + 24 * 6, 50, 24, "Calibrate", () => { - this.applyCalibrationOffsets(); + this.parent.applyCalibrationOffsets(); })); diff --git a/script.js b/script.js index 881700c..a9eb812 100644 --- a/script.js +++ b/script.js @@ -41,7 +41,7 @@ window.onload = () => { const serial = new SerialManager(); const urdfCanvas = document.getElementById('urdfCanvas'); - const visualEditor = new URDFEditor(urdfCanvas, sendMotorPosition, serial, curveEditor); + const visualEditor = new URDFEditor(urdfCanvas, sendMotorPosition, serial, curveEditor, tryConnect); @@ -94,6 +94,7 @@ window.onload = () => { }); nodeEditor.generateDefaultNodes(curveEditor.curveSets, motorIDList); + visualEditor.loadURDFFromIndexedDB(); } @@ -291,8 +292,7 @@ window.onload = () => { } } - // Connect button - document.getElementById('connect').addEventListener('click', async () => { + async function tryConnect() { try { await serial.connect(); statusText.textContent = 'Connected ✅'; @@ -412,6 +412,11 @@ window.onload = () => { statusText.textContent = 'Connection failed ❌'; console.error("Connection error:", err); } + } + + // Connect button + document.getElementById('connect').addEventListener('click', async () => { + tryConnect(); }); function handlePositionStreamPacket(data) {