yostlabs 2025.9.17__py3-none-any.whl → 2025.10.6__py3-none-any.whl

yostlabs/math/quaternion.py

@@ -51,6 +51,15 @@ def quat_from_axis_angle(axis: list[float], angle: float):
     quat.append(math.cos(angle / 2))
     return quat
 
+#There are multiple valid quats that can be returned by this. The intention of this function
+#is to be able to rotate an arrow by the quat such that it points the correct direction. The rotation
+#of that arrow along its axis may differ though
+def quat_from_one_vector(vec: list[float]):
+    vec = _vec.vec_normalize(vec)
+    perpendicular = _vec.vec_normalize(_vec.vec_cross([0, 0, 1], vec))
+    angle = math.acos(_vec.vec_dot([0, 0, 1], vec))
+    return quat_from_axis_angle(perpendicular, angle)
+
 def quat_from_two_vectors(forward: list[float], down: list[float]):
     """
     This function requires two orthogonal vectors to work

yostlabs/tss3/utils/calibration.py

@@ -150,4 +150,214 @@ class ThreespaceGradientDescentCalibration:
             print(f"Final Rating: {best_rating}")
             print(f"Final Params: {best_params}")
 
-        return best_params
+        return best_params
+    
+def fibonacci_sphere(samples=1000):
+    points = []
+    phi = np.pi * (3. - np.sqrt(5.))  # golden angle
+
+    for i in range(samples):
+        y = 1 - (i / float(samples - 1)) * 2  # y goes from 1 to -1
+        radius = np.sqrt(1 - y * y)
+        theta = phi * i
+        x = np.cos(theta) * radius
+        z = np.sin(theta) * radius
+        points.append((x, y, z))
+
+    return np.array(points)
+
+class ThreespaceSphereCalibration:
+
+    def __init__(self, max_comparison_points=500):
+        self.buffer = 0.1
+        self.test_points = fibonacci_sphere(samples=max_comparison_points)
+        self.clear()
+
+    def process_point(self, raw_mag: list[float]):
+        if len(self.samples) == 0:
+            self.samples = np.array([raw_mag])
+            return True
+        
+        raw_mag = np.array(raw_mag, dtype=np.float64)
+        new_len = np.linalg.norm(raw_mag)
+
+        avg_len = np.linalg.norm(self.samples, axis=1)
+        avg_len = (avg_len + new_len) / 2 * self.buffer
+
+        dist = np.linalg.norm(self.samples - raw_mag, axis=1)
+        if np.any(dist < avg_len):
+            return False
+        
+        self.samples = np.concatenate((self.samples, [raw_mag]))
+        self.__update_density(raw_mag / new_len)
+        return True
+    
+    def __update_density(self, normalized_point: np.ndarray):
+        #First check to see if the new point is the closest point for any of the previous points
+        dots = np.sum(self.test_points * normalized_point, axis=1)
+        self.closest_dot = np.maximum(self.closest_dot, dots)
+        self.largest_delta_index = np.argmin(self.closest_dot)
+        self.largest_delta = np.rad2deg(np.acos(self.closest_dot[self.largest_delta_index]))
+
+    def clear(self):
+        self.samples = np.array([])
+        self.closest_dot = np.array([-1] * len(self.test_points))
+        self.largest_delta = 180
+        self.largest_delta_index = 0
+
+    @property
+    def sparsest_vector(self):
+        return self.test_points[self.largest_delta_index]
+
+    def calculate(self):
+        """
+        Returns matrix and bias
+        """
+        comp_A, comp_b, comp_d = ThreespaceSphereCalibration.alternating_least_squares(np.array(self.samples))
+        comp_A, comp_b = ThreespaceSphereCalibration.make_calibration_params(comp_A, comp_b, comp_d, 1)
+
+        comp_scale: float = comp_A[0][0] + comp_A[1][1] + comp_A[2][2]
+        comp_scale /= 3
+
+        comp_A /= comp_scale
+
+        return comp_A.flatten().tolist(), comp_b.tolist()
+
+    @staticmethod
+    def alternating_least_squares(data: np.ndarray[np.ndarray]) -> tuple[np.ndarray, np.ndarray, float]:
+        n = 3
+        m = len(data)
+
+        #Step 1
+        noise_variance = 0.1
+        sigma2 = noise_variance ** 2
+        rotini = data.T
+        rotini2 = rotini ** 2
+
+        T = np.zeros((5,n,m))
+        T[0,:,:] = 1
+        T[1,:,:] = rotini
+        T[2,:,:] = rotini2 - sigma2
+        T[3,:,:] = rotini2 * rotini - 3 * rotini * sigma2
+        T[4,:,:] = rotini2 * rotini2 - 6 * rotini2 * sigma2 + 3 * sigma2 * sigma2
+
+        #Step 2
+        one = np.ones((n+1,))
+        i = np.arange(1, n+1)
+        i = np.append(i, 0)
+
+        m1 = np.outer(i.T, one)
+        m2 = np.outer(one.T, i)
+
+        M = np.array([ThreespaceSphereCalibration.vec_s(m1), ThreespaceSphereCalibration.vec_s(m2)])
+
+        #Step 3
+        nb = int((n + 1) * n / 2 + n + 1)
+        R = np.zeros((nb,nb,n), dtype=np.int32)
+        
+        for p in range(nb):
+            for q in range(p, nb):
+                for i in range(1, n+1):
+                    R[p,q,i-1] = int(M[0,p] == i) + int(M[1,p] == i) + int(M[0,q] == i) + int(M[1,q] == i)
+
+        #Step 4
+        nals = np.zeros((nb, nb))
+        for p in range(nb):
+            for q in range(p, nb):
+                sum = 0
+                for l in range(m):
+                    prod = 1
+                    for i in range(n):
+                        prod *= T[R[p,q,i],i,l]
+                    sum += prod
+                nals[p,q] = sum
+
+        #Step 5
+        D2 = [i * (i + 1) / 2 for i in range(1, n+1)]
+        D = [d for d in range(1, (n+1) * n // 2 + 1) if not d in D2]
+
+        #Step 6
+        menorah_als = np.zeros((nb, nb))
+        
+        for p in range(nb):
+            for q in range(p, nb):
+                coeff = 2
+                if p + 1 in D and q + 1 in D:
+                    coeff = 4
+                elif not p + 1 in D and not q + 1 in D:
+                    coeff = 1
+                menorah_als[p,q] = coeff * nals[p,q]
+        
+        # Fill the lower triangle with the upper triangle values
+        i_lower, j_lower = np.tril_indices(nb, k=-1) 
+        menorah_als[i_lower, j_lower] = menorah_als[j_lower, i_lower]
+
+        #Step 7
+        eigenmat = menorah_als
+
+        #It is unclear if this is correct, there are differences in sign and positions
+        eigenvalues, eigenvectors = np.linalg.eig(eigenmat)
+        eigenvectors = eigenvectors.T
+
+        #Looks like this section gets the eigen vector with the largest eigen value?
+        combinedmatr = [[abs(eigenvalues[i]), eigenvectors[i]] for i in range(len(eigenvalues))]
+        combinedmatr.sort(key=lambda a: a[0], reverse=True)
+
+        bals = combinedmatr[-1][1]
+
+        #Step 8 : ensure normalized
+        bals = bals / np.linalg.norm(bals)
+
+        #Step 9:
+        triangle = n*(n+1)//2
+        A = ThreespaceSphereCalibration.inv_vec_s(bals[:triangle])
+        b = bals[triangle:nb-1]
+        d = bals[-1]
+
+        return A, b, d
+
+    @staticmethod
+    def make_calibration_params(Q: np.ndarray, u: np.ndarray, k: float, H_m: float) -> tuple[np.ndarray,np.ndarray]:
+        pa = np.linalg.inv(Q)
+        pb = u.T
+        b = np.dot(pa, pb) * 0.5
+
+        eigenvalues, V = np.linalg.eig(Q)
+        D = np.diag(eigenvalues)
+
+        vu_prod = np.dot(V.T, u.T)
+        p1a = np.dot(vu_prod.T, np.linalg.inv(D))
+        p1b = np.dot(p1a, vu_prod)
+        p1 = p1b - (4 * k)
+
+        alpha = 4 * (H_m ** 2) / p1
+
+        aD = np.diag(abs(alpha * eigenvalues) ** 0.5)
+
+        A = np.dot(np.dot(V, aD), V.T)
+        
+        return A, b
+
+    @staticmethod
+    def vec_s(matrix: np.ndarray):
+        rows, cols = np.tril_indices(matrix.shape[0])
+        return matrix[rows, cols]
+
+    @staticmethod
+    def inv_vec_s(vec: np.ndarray):
+        #Its unclear if this function works as intended. But this is how the suite does it.
+        size = int((-1 + (1 + 8 * len(vec)) ** 0.5) / 2)
+        matr = np.zeros((size,size))
+        base = 0
+        for i in range(size):
+            for j in range(i):
+                matr[i,j] = vec[base + j]
+                matr[j,i] = vec[base + j]
+            matr[i,i] = vec[base + i]
+            base += i + 1
+        
+        return matr
+
+    @property
+    def num_points(self):
+        return len(self.samples)

yostlabs/tss3/utils/parser.py

@@ -27,7 +27,7 @@ class ThreespaceBufferInputStream(ThreespaceInputStream):
         length = self.buffer.index(expected) + len(expected)
         result = self.buffer[:length]
         del self.buffer[:length]
-        raise result
+        return result
 
     """Allows reading without removing the data from the buffer"""
     def peek(self, num_bytes) -> bytes:

{yostlabs-2025.9.17.dist-info → yostlabs-2025.10.6.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: yostlabs
-Version: 2025.9.17
+Version: 2025.10.6
 Summary: Python resources and API for 3Space sensors from Yost Labs Inc.
 Project-URL: Homepage, https://yostlabs.com/
 Project-URL: Repository, https://github.com/YostLabs/3SpacePythonPackage/tree/main
@@ -13,6 +13,7 @@ Classifier: License :: OSI Approved :: MIT License
 Classifier: Operating System :: OS Independent
 Classifier: Programming Language :: Python :: 3
 Requires-Python: >=3.10
+Requires-Dist: async-timeout
 Requires-Dist: bleak
 Requires-Dist: numpy
 Requires-Dist: pyserial

{yostlabs-2025.9.17.dist-info → yostlabs-2025.10.6.dist-info}/RECORD

@@ -4,18 +4,18 @@ yostlabs/communication/base.py,sha256=ahAIQndfo9ifX6Lf2NeEaHpIhRJ_uBv6jv9P7N3Rbh
 yostlabs/communication/ble.py,sha256=UwbUDEp0lU6CQv-BWmqB3mzEUJrwwRH78eeJUmFO9c4,15498
 yostlabs/communication/serial.py,sha256=j7SksPhd2mCvcMIGVvPcAAhYOE29K6uGLwZCwD-b21E,5685
 yostlabs/math/__init__.py,sha256=JFzsPQ4AbsX1AH1brBpn1c_Pa_ItF43__D3mlPvA2a4,34
-yostlabs/math/quaternion.py,sha256=vQQmT5T0FXAOrZ27cj007Gb_sfEhs7h0Sz6nYxNc5hQ,6357
+yostlabs/math/quaternion.py,sha256=CUIh4RHUcoSt2RUyPPh7Vw-xJfcUP2Z_mAkA0EHwyn4,6865
 yostlabs/math/vector.py,sha256=9vfVFSahHa0ZZRZ_SgAU5ucVplt7J-fHQ0s8ymOanj4,2725
 yostlabs/tss3/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 yostlabs/tss3/api.py,sha256=h8K1VsWZutTE-Vu5k2xJbz-OBwKxR-IKnFOJ5qBW0xs,86165
 yostlabs/tss3/consts.py,sha256=RwhqmKIXRGpRdusss3q17ukCuRS96ZsvBl6y0mjF0b4,2404
 yostlabs/tss3/eepts.py,sha256=7A7sCyOfDiJgw5Y9pGneg-5YgNvcfKtqeS9FoVWfJO8,9540
 yostlabs/tss3/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-yostlabs/tss3/utils/calibration.py,sha256=42jCEzfTXoHuPZ4e-30N1ijOhkz9ld4PQnhX6AhTrZE,7069
-yostlabs/tss3/utils/parser.py,sha256=QfjjFeeIcnWjVbEjSx2yqOsNuoTK3DjkfT6BOMcQOsg,11346
+yostlabs/tss3/utils/calibration.py,sha256=J7RnY23MUUo4lmfapHavUqrTArFo4PxGKw68sL4DfOM,14070
+yostlabs/tss3/utils/parser.py,sha256=ij_kQpB3EukOq3O8wQTYhkqBP-OneiHMUcsHLuL6m-8,11347
 yostlabs/tss3/utils/streaming.py,sha256=G2OjSIL9zub0EbkgDGDWaqSXoRY6MJzMD4mazWOCUOA,22419
 yostlabs/tss3/utils/version.py,sha256=NT2H9l-oIRCYhV_yjf5UjkadoJQ0IN4eLl8y__pyTPc,3001
-yostlabs-2025.9.17.dist-info/METADATA,sha256=h1vomT-_6UquhivvLQHwz9SayNn0idxbHiJskrHe-z0,2722
-yostlabs-2025.9.17.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
-yostlabs-2025.9.17.dist-info/licenses/LICENSE,sha256=PtF8EXRlVhm1_ve52_3GHixSPwMn0tGajFxF3xKS-j0,1090
-yostlabs-2025.9.17.dist-info/RECORD,,
+yostlabs-2025.10.6.dist-info/METADATA,sha256=vChqVP1GJ13Cje3O40RtkMKppBYWz1olZPaHyYxJMiE,2751
+yostlabs-2025.10.6.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
+yostlabs-2025.10.6.dist-info/licenses/LICENSE,sha256=PtF8EXRlVhm1_ve52_3GHixSPwMn0tGajFxF3xKS-j0,1090
+yostlabs-2025.10.6.dist-info/RECORD,,