Merge 5fb0f74 into b6c3034

.github/workflows/python-app.yml

@@ -25,8 +25,8 @@ jobs:
         python-version: "3.10"
     - name: Install dependencies
       run: |
-        pip install -U pip setuptools wheel
-        pip install bleak python-osc pyinstaller
+        pip install -U pip wheel
+        pip install numpy pyquaternion bleak python-osc pyinstaller
     - name: Build with PyInstaller
       run: |
         pyinstaller -F metabow_bridge.py

calibrator.py

@@ -0,0 +1,38 @@
+
+
+# Modified by Paulo Chiliguano and Travis West
+# Directed by Dr Roberto Alonso Trillo
+# Department of Music - Hong Kong Baptist University
+# 2022
+
+"""Calibrator
+"""
+import numpy as np
+
+
+class Calibrator():
+    def __init__(self):
+        self.DEG_RAD_RATIO = np.pi / 180.0
+        self.accl_bias = np.zeros(3)
+        self.gyro_bias = np.zeros(3)
+        self.magn_bias = np.zeros(3)
+        self.accl_transform = 0.001 * np.identity(3)
+
+        # NOTE: the gyro is not yet calibrated in pyMIMU
+        self.gyro_transform = self.DEG_RAD_RATIO * np.identity(3)
+
+        self.magn_transform = np.identity(3)
+
+    def calibrate(self, vector_measurement, matrix_transform, vector_bias):
+        return np.matmul(matrix_transform, vector_measurement - vector_bias)
+
+    def calibrate_accl(self, accl):
+        return self.calibrate(accl, self.accl_transform, self.accl_bias)
+
+    def calibrate_gyro(self, gyro):
+        return self.calibrate(gyro, self.gyro_transform, self.gyro_bias)
+
+    def calibrate_magn(self, magn):
+        return self.calibrate(magn, self.magn_transform, self.magn_bias)
+
+    #TODO: Set biases function

fusion_filter.py

@@ -0,0 +1,118 @@
+
+
+# Modified by Paulo Chiliguano and Travis West
+# Directed by Dr Roberto Alonso Trillo
+# Department of Music - Hong Kong Baptist University
+# 2022
+
+"""Fusion filter
+"""
+import numpy as np
+from time import monotonic
+from pyquaternion import Quaternion
+
+
+class BaseFilter:
+    def __init__(self):
+        self.period = 0
+        self.current = monotonic()
+        self.previous = self.current
+        self.q       = Quaternion()
+        self.q_align = Quaternion()
+
+    def calculate_period(self):
+        self.current = monotonic()
+        self.period = self.current - self.previous
+        self.previous = self.current
+
+        return self.period
+
+    def set_alignment(self):
+        self.q_align = self.q.conjugate
+
+    def align(self, quaternion):
+        return self.q_align * quaternion
+
+    def fuse(self, omega, v_a, v_m, k_I = 1.0, k_P = 3.0, k_a=1.0, k_m=1.0):
+        return Quaternion()
+
+    def fuse_and_align(self, *args):
+        q = self.fuse(*args)
+        return self.align(q), self.q_align.rotate(self.ma_earth)
+
+
+class FusionFilter(BaseFilter):
+    def __init__(self):
+        super().__init__()
+        self.static_threshold = 0.01
+        self.galpha = 0.93
+        self.b_hat      = np.zeros(3)
+        self.h          = np.zeros(3)
+        self.b          = np.zeros(3)
+        self.ma_earth   = np.zeros(3)
+        self.ma_sensor  = np.zeros(3)
+        self.v_hat_x    = np.zeros(3)
+        self.v_hat_a    = np.zeros(3)
+        self.w_a        = np.zeros(3)
+        self.w_x        = np.zeros(3)
+        self.w_mes      = np.zeros(3)
+        self.da         = np.zeros(3)
+        self.anm1       = np.zeros(3)
+        self.aa         = np.zeros(3)
+        self.gyro_bias = np.zeros(3)
+
+
+    def set_gyro_bias_alpha(self, v):
+        self.galpha = v
+
+    def set_static_threshold(self, v):
+        self.static_threshold = v
+
+    def fuse(self, omega, v_a, v_m, k_I = 0.0, k_P = 3.0, k_a=1.0, k_m=0.0):
+        self.calculate_period()
+
+        self.aa = self.anm1 * 0.5 + self.aa * 0.5 # average accel
+        self.da = (self.aa - self.anm1) # "derivative" accel
+        self.static = np.linalg.norm(self.da) < self.static_threshold
+        self.anm1 = v_a
+
+        if self.static:
+            self.gyro_bias = (1 - self.galpha) * omega + self.galpha * self.gyro_bias
+        omega -= self.gyro_bias
+
+        rotation = self.q.rotation_matrix # the basis axes of the sensor frame expressed in the global frame
+        inverse  = self.q.conjugate.rotation_matrix # the basis axes of the global frame expressed in the sensor frame
+
+        # inverse[2] represents the global Z axis (related to gravity) expressed in the sensor coordinate frame
+        self.ma_sensor = v_a - inverse[:,2]             # zero-g in sensor frame
+        self.ma_earth  = np.matmul(inverse, self.ma_sensor) # zero-g in global frame
+
+        v_a = v_a / np.linalg.norm(v_a)
+        v_m = v_m / np.linalg.norm(v_m)
+        self.v_x = np.cross(v_a, v_m) # cross-product of accel and magnetic field (cross vector)
+
+        self.h = np.matmul(rotation, self.v_x)
+        self.b = np.array([np.sqrt(self.h[0]*self.h[0] + self.h[1]*self.h[1]), 0, self.h[2]])
+
+        self.v_hat_a = inverse[:,2] # estimated gravity vector
+        self.v_hat_x = np.matmul(inverse, self.b) # estimated cross vector
+
+        # the cross product $a x b$ gives a vector that points along the axis of rotation to move
+        # the lh operand to the rh operand, with a magnitude $||a x b|| = ||a|| ||b|| |sin(\theta)|$
+        # where $\theta$ is the angle between the vectors. When the vectors are normalized and the
+        # angle is nearly zero, $a x b$ approximates the rotation vector from a to b.
+        # see also eqs. (32c) and (48a) in Mahoney et al. 2008
+        self.w_a = np.cross(v_a, self.v_hat_a) # discrepancy based on accelerometer reading
+        self.w_x = np.cross(self.v_x, self.v_hat_x) # discrepancy based on cross vector
+        self.w_mes = self.w_a * k_a + self.w_x * k_m
+
+        # error correction is added to omega (angular rate) before integrating
+        if k_I > 0.0:
+            self.b_hat += self.static * self.w_mes * self.period # see eq. (48c)
+            omega += k_P * self.w_mes + k_I * self.b_hat
+        else:
+            self.b_hat = np.zeros(3) # Madgwick: "prevent integral windup"
+            omega += k_P * self.w_mes
+
+        self.q.integrate(omega, self.period)
+        return self.q

gesture_model.py

@@ -0,0 +1,44 @@
+
+
+# Modified by Paulo Chiliguano and Travis West
+# Directed by Dr Roberto Alonso Trillo
+# Department of Music - Hong Kong Baptist University
+# 2022
+
+"""Gesture model
+"""
+import numpy as np
+from pyquaternion import Quaternion
+from calibrator import Calibrator
+from fusion_filter import FusionFilter
+
+
+class GestureModel():
+    def __init__(self):
+        self.RAD_DEG_RATIO = 180 / np.pi
+        self.skewness = 0
+        self.tilt = 0
+        self.roll = 0
+        self.caccl = np.zeros(3)
+        self.cgyro = np.zeros(3)
+        self.cmagn = np.zeros(3)
+        self.matrix = np.identity(3)
+        self.movement_acceleration = np.zeros(3)
+        self.acceleration_derivative = np.zeros(3)
+        self.movement_velocity = np.zeros(3)
+        self.quaternion = Quaternion()
+        self.calibrator = Calibrator()
+        self.fusion_filter = FusionFilter()
+
+    def tick(self, accl, gyro, magn):
+        self.caccl = self.calibrator.calibrate_accl(accl)
+        self.cgyro = self.calibrator.calibrate_gyro(gyro)
+        self.cmagn = self.calibrator.calibrate_magn(magn)
+        self.quaternion = self.fusion_filter.fuse(self.cgyro, self.caccl, self.cmagn)
+        self.matrix = self.quaternion.rotation_matrix
+        self.skewness = self.RAD_DEG_RATIO * np.arctan2(self.matrix[1,0], self.matrix[0,0])
+        self.tilt = self.RAD_DEG_RATIO * np.arctan2(self.matrix[2,0], np.sqrt(self.matrix[2,1] * self.matrix[2,1] + self.matrix[2,2] * self.matrix[2,2]))
+        self.roll = self.RAD_DEG_RATIO * np.arctan2(self.matrix[2,1], self.matrix[2,2])
+        self.movement_acceleration = self.fusion_filter.ma_sensor
+        self.acceleration_derivative = self.fusion_filter.da
+        self.movement_velocity = self.movement_velocity * 0.9999 + self.movement_acceleration

metabow_bridge.py

@@ -13,6 +13,8 @@
 
 
 import asyncio
+from datetime import datetime
+from functools import partial
 
 from functools import partial
 
@@ -22,14 +24,16 @@
 )
 
 from pythonosc import udp_client
-
-from utils import int_list_from_bytearray, log_file_path
+from gesture_model import GestureModel
+from utils import bytearray_to_fusion_data, log_file_path
 
 
 simple_udp_client = udp_client.SimpleUDPClient("127.0.0.1", 8888)
 CHARACTERISTIC_UUID = "00E00000-0001-11E1-AC36-0002A5D5C51B"
 st_devices = {}
 filename = log_file_path()
+model = GestureModel()
+
 
 def device_found(device, _):
     if device.name == "AM1V330":
@@ -40,12 +44,15 @@ def device_found(device, _):
 def notification_handler(device_number:int, sender: int, data: bytearray):
     """Simple notification handler
     """
-    join_array = int_list_from_bytearray(data)
+    join_array = bytearray_to_fusion_data(data)
     simple_udp_client.send_message(f"/{device_number}/raw", join_array)
-
+    model.tick(join_array[1:4], join_array[4:7], join_array[7:10])
+    simple_udp_client.send_message(f"/{device_number}/quaternion", model.quaternion.elements.tolist())
+    simple_udp_client.send_message(f"/{device_number}/motion_acceleration/sensor_frame", model.movement_acceleration.tolist())
+
     with open(filename, 'a') as f:
-        join_str = " ".join(str(e) for e in join_array)
-        f.write(f"{device_number} {join_str}\n")
+        join_str = " ".join(str(e) for e in join_array[1:])
+        f.write(f'{device_number} {datetime.now().strftime("%Y%m%d_%H%M%S.%f")} {join_str}\n')
 
 
 async def connect_to_device(i, device):

utils.py

@@ -14,7 +14,42 @@
 Array = List[int]
 
 
-def int_list_from_bytearray(data: ByteString) -> Array:
+def bytearray_to_fusion_data(data: ByteString) -> List[float]:
+    """Converts a bytearray to a list of float numbers
+
+    Parameters
+    ----------
+    data : bytearray
+        bytes from BLE device
+
+    Returns
+    -------
+    list
+        A list of floats representing the timestamp and coordinates values
+    """
+    raw_list = _int_list_from_bytearray(data)
+    raw_list[4:7] = _scale_gyroscope_coordinates(raw_list[4:7])
+
+    return [float(f'{i:.1f}') for i in raw_list]
+
+
+def _scale_gyroscope_coordinates(data: Array) -> Array:
+    """Scale gyroscope data 1/10x as it comes multiplied
+
+    Parameters
+    ----------
+    data : list
+        X, Y, Z coordinates data (int16 each)
+
+    Returns
+    -------
+    list
+        A list of floats corresponding to coordinates data of the Blue ST gyroscope
+    """
+    return [0.1*i for i in data]
+
+
+def _int_list_from_bytearray(data: ByteString) -> Array:
     """Converts a bytearray to a list of signed integers
 
     Parameters
@@ -28,7 +63,7 @@ def int_list_from_bytearray(data: ByteString) -> Array:
         A list of integers representing the timestamp and coordinates values
     """
     step = 2
-    timestamp = [datetime.now().strftime("%Y%m%d_%H%M%S.%f"),]
+    timestamp = [uint16_from_bytes(data[0:step]),]
     sensor_data = [int16_from_bytes(data[i:i+step]) for i in range(step, len(data) - 1, step)]
 
     return timestamp + sensor_data