From 8a357546a2f1304771cad066ddf9ebb77d0ae072 Mon Sep 17 00:00:00 2001 From: Aaron Cuevas Lopez Date: Tue, 13 May 2025 18:08:58 +0200 Subject: [PATCH 1/3] Implement new quaternion to twist algorithm --- Source/CommutatorThread.cpp | 41 +++++++++++++++++++------------------ Source/CommutatorThread.h | 4 ++-- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/Source/CommutatorThread.cpp b/Source/CommutatorThread.cpp index 2cda3f3..a9e3b1c 100644 --- a/Source/CommutatorThread.cpp +++ b/Source/CommutatorThread.cpp @@ -75,7 +75,7 @@ bool CommutatorThread::isReady() const bool CommutatorThread::start() { lastTwist = std::numeric_limits::quiet_NaN(); - previousAngleAboutAxis = std::numeric_limits::quiet_NaN(); + lastQuaternion = defaultQuaternion; runningQuaternion = defaultQuaternion; if (open && rotationAxis.length() == 1) @@ -113,32 +113,28 @@ void CommutatorThread::sendTurn (double turn) int n = serial.writeBytes (reinterpret_cast (const_cast (str)), len); } -double CommutatorThread::quaternionToTwist (Quaternion quaternion) +double CommutatorThread::quaternionToTwist (Quaternion currentQuaternion, Quaternion lastQuaternion) { - // Project rotation axis onto the direction axis - double dotProduct = quaternion.vector * rotationAxis; + Quaternion lastTransposed(-lastQuaternion.vector, lastQuaternion.scalar); - Vector3D projection = rotationAxis; - double scaleFactor = dotProduct / (rotationAxis * rotationAxis); - projection *= scaleFactor; + // Get incremental rotation + Quaternion deltaQuaternion(currentQuaternion); + deltaQuaternion *= lastTransposed; //juce quaternions do not implement a*b operator, only a *= b. - Quaternion rotationAboutAxis = Quaternion (projection, quaternion.scalar).normalised(); + // Get device rotation axis in global coordinates + Quaternion deviceAxis (rotationAxis, 0); + Quaternion projection (lastQuaternion); + projection *= deviceAxis; + projection *= lastTransposed; - if (dotProduct < 0) // Account for angle-axis flipping - { - rotationAboutAxis = Quaternion (-rotationAboutAxis.vector, -rotationAboutAxis.scalar); - } - // Normalize twist feedback in units of turns - double angleAboutAxis = 2 * std::acos (rotationAboutAxis.scalar); + // Calculate rotation across the axis in global coordinates + double dotProduct = deltaQuaternion.vector * projection.vector; - double twist = ! isnan (previousAngleAboutAxis) - ? std::fmod (angleAboutAxis - previousAngleAboutAxis + 3 * MathConstants::pi, MathConstants::twoPi) - MathConstants::pi - : 0; + double incrementalAngle = 2*std::atan2 (dotProduct, deltaQuaternion.scalar); - previousAngleAboutAxis = angleAboutAxis; + return incrementalAngle / MathConstants::twoPi; - return -twist / MathConstants::twoPi; } void CommutatorThread::hiResTimerCallback() @@ -148,7 +144,12 @@ void CommutatorThread::hiResTimerCallback() if (currentQuaternion == defaultQuaternion) return; - double currentTwist = quaternionToTwist (Quaternion (currentQuaternion[1], currentQuaternion[2], currentQuaternion[3], currentQuaternion[0])); + double currentTwist = 0; + if (lastQuaternion != defaultQuaternion) + currentTwist = quaternionToTwist (Quaternion (currentQuaternion[1], currentQuaternion[2], currentQuaternion[3], currentQuaternion[0]).normalised(), + Quaternion (lastQuaternion[1], lastQuaternion[2], lastQuaternion[3], lastQuaternion[0]).normalised()); + + lastQuaternion = currentQuaternion; if (! isnan (lastTwist)) { diff --git a/Source/CommutatorThread.h b/Source/CommutatorThread.h index 40f4cce..2ea82f0 100644 --- a/Source/CommutatorThread.h +++ b/Source/CommutatorThread.h @@ -46,13 +46,13 @@ class CommutatorThread : public HighResolutionTimer private: /** Converts quaternion data to a twist. Quaternion values are expected to be ordered X/Y/Z/W. */ - double quaternionToTwist (Quaternion quaternion); + double quaternionToTwist (Quaternion currentQuaternion, Quaternion lastQuaternion); void sendTurn (double turn); ofSerial serial; double lastTwist = std::numeric_limits::quiet_NaN(); - double previousAngleAboutAxis = std::numeric_limits::quiet_NaN(); + std::array lastQuaternion; static inline const std::array defaultQuaternion { 0.0, 0.0, 0.0, 0.0 }; From d31e1411bf7ba55643ae2e84fae7f0961df2a48e Mon Sep 17 00:00:00 2001 From: Aaron Cuevas Lopez Date: Wed, 14 May 2025 13:25:24 +0200 Subject: [PATCH 2/3] Fix sign for +Z rotation --- Source/CommutatorThread.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Source/CommutatorThread.cpp b/Source/CommutatorThread.cpp index a9e3b1c..15ed8b8 100644 --- a/Source/CommutatorThread.cpp +++ b/Source/CommutatorThread.cpp @@ -115,17 +115,17 @@ void CommutatorThread::sendTurn (double turn) double CommutatorThread::quaternionToTwist (Quaternion currentQuaternion, Quaternion lastQuaternion) { - Quaternion lastTransposed(-lastQuaternion.vector, lastQuaternion.scalar); + Quaternion lastConjugated(-lastQuaternion.vector, lastQuaternion.scalar); // Get incremental rotation Quaternion deltaQuaternion(currentQuaternion); - deltaQuaternion *= lastTransposed; //juce quaternions do not implement a*b operator, only a *= b. + deltaQuaternion *= lastConjugated; //juce quaternions do not implement a*b operator, only a *= b. // Get device rotation axis in global coordinates Quaternion deviceAxis (rotationAxis, 0); Quaternion projection (lastQuaternion); projection *= deviceAxis; - projection *= lastTransposed; + projection *= lastConjugated; // Calculate rotation across the axis in global coordinates @@ -133,7 +133,7 @@ double CommutatorThread::quaternionToTwist (Quaternion currentQuaternion double incrementalAngle = 2*std::atan2 (dotProduct, deltaQuaternion.scalar); - return incrementalAngle / MathConstants::twoPi; + return -incrementalAngle / MathConstants::twoPi; } From b3cdff1575da7cc4704e2ecee1e06fb956861aa3 Mon Sep 17 00:00:00 2001 From: Aaron Cuevas Lopez Date: Wed, 17 Sep 2025 17:02:02 +0200 Subject: [PATCH 3/3] Added rotation relative to commutator axis (Need to add control to editor) --- Source/CommutatorThread.cpp | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/Source/CommutatorThread.cpp b/Source/CommutatorThread.cpp index 15ed8b8..c23ba00 100644 --- a/Source/CommutatorThread.cpp +++ b/Source/CommutatorThread.cpp @@ -115,25 +115,45 @@ void CommutatorThread::sendTurn (double turn) double CommutatorThread::quaternionToTwist (Quaternion currentQuaternion, Quaternion lastQuaternion) { + const Vector3D commutatorAxis (0, 0, 1); + const Vector3D headstageAxis (0, 0, 1); + + Vector3D localAxis = headstageAxis; + Vector3D globalAxis = rotationAxis; + Quaternion lastConjugated(-lastQuaternion.vector, lastQuaternion.scalar); // Get incremental rotation Quaternion deltaQuaternion(currentQuaternion); deltaQuaternion *= lastConjugated; //juce quaternions do not implement a*b operator, only a *= b. + deltaQuaternion = deltaQuaternion.normalised(); //remove any possible floating point error // Get device rotation axis in global coordinates - Quaternion deviceAxis (rotationAxis, 0); + Quaternion deviceAxis (localAxis, 0); Quaternion projection (lastQuaternion); projection *= deviceAxis; projection *= lastConjugated; + projection = projection.normalised(); // Calculate rotation across the axis in global coordinates - double dotProduct = deltaQuaternion.vector * projection.vector; + double localDotProduct = deltaQuaternion.vector * projection.vector; + + double incrementalAngleLocal = 2 * std::atan2 (localDotProduct, deltaQuaternion.scalar); + + //calculate the rotation across the commutator axis + double globalDotProduct = deltaQuaternion.vector * globalAxis; + double incrementalAngleGlobal = 2 * std::atan2 (globalDotProduct, deltaQuaternion.scalar); + + //get the cosine from the rotated axis and the commutator axis + //since vectors are normalised, this is just the dot product + double cos_angle = projection.vector * globalAxis; + + //Remove the local twist from the global rotation to get a weighted total rotation + double totalAngle = incrementalAngleLocal +(incrementalAngleGlobal - incrementalAngleLocal * cos_angle); - double incrementalAngle = 2*std::atan2 (dotProduct, deltaQuaternion.scalar); - return -incrementalAngle / MathConstants::twoPi; + return -totalAngle / MathConstants::twoPi; }