yostlabs 2025.1.16__py3-none-any.whl

yostlabs/communication/base.py

@@ -0,0 +1,90 @@
+from typing import Generator
+
+class ThreespaceInputStream:
+
+    """
+    Reads specified number of bytes. 
+    If that many bytes are not available after timeout, less data will be returned
+    """
+    def read(self, num_bytes: int) -> bytes:
+        raise NotImplementedError()
+    
+    def read_all(self):
+        return self.read(self.length)
+
+    def read_until(self, expected: bytes) -> bytes:
+        raise NotImplementedError()
+
+    """Allows reading without removing the data from the buffer"""
+    def peek(self, num_bytes: int) -> bytes:
+        raise NotImplementedError()
+    
+    def peek_until(self, expected: bytes, max_length: int = None) -> bytes:
+        raise NotImplementedError()
+    
+    def readline(self) -> bytes:
+        return self.read_until(b"\n")
+    
+    def peekline(self, max_length: int = None) -> bytes:
+        return self.peek_until(b"\n", max_length=max_length)    
+    
+    @property
+    def length(self) -> int:
+        raise NotImplementedError()
+    
+    @property
+    def timeout(self) -> float:
+        raise NotImplementedError()
+
+    @timeout.setter
+    def timeout(self, timeout: float):
+        raise NotImplementedError()    
+
+class ThreespaceOutputStream:
+
+    """Write the given bytes"""
+    def write(self, bytes):
+        raise NotImplementedError()
+
+class ThreespaceComClass(ThreespaceInputStream, ThreespaceOutputStream):
+    """
+    Base class for a com class to use with the sensor object.
+    Com classes should be initialized without connection and require
+    there open called before use
+    """
+    
+    def close(self):
+        raise NotImplementedError()
+    
+    def open(self) -> bool:
+        """
+        Should return True on success, False on failure
+        If already open, should stay open
+        """
+        raise NotImplementedError()
+    
+    def check_open(self) -> bool:
+        """
+        Should return True if the port is currently open, False otherwise.
+        Must give the current state, not a cached state
+        """
+        raise NotImplementedError()
+    
+    @staticmethod
+    def auto_detect() -> Generator["ThreespaceComClass", None, None]:
+        """
+        Returns a list of com classes of the same type called on nearby
+        """
+        raise NotImplementedError()
+    
+    @property
+    def reenumerates(self) -> bool:
+        """
+        If the device Re-Enumerates when going from bootloader to firmware or vice versa, this must return True.
+        This indicates to the API that it must search for the new com class representing the object when switching between bootloader and firmware
+        """
+        raise NotImplementedError
+    
+    @property
+    def name(self) -> str:
+        raise NotImplementedError()

yostlabs/communication/serial.py

@@ -0,0 +1,141 @@
+from yostlabs.communication.base import *
+import serial
+import serial.tools.list_ports
+
+
+class ThreespaceSerialComClass(ThreespaceComClass):
+
+    PID_V3_MASK = 0x3000
+    PID_BOOTLOADER = 0x1000
+
+    VID = 0x2476
+
+    DEFAULT_BAUDRATE = 115200
+    DEFAULT_TIMEOUT = 2
+
+    def __init__(self, ser: serial.Serial | str):
+        if isinstance(ser, serial.Serial):
+            self.ser = ser
+        elif isinstance(ser, str):
+            self.ser = serial.Serial(ser, baudrate=ThreespaceSerialComClass.DEFAULT_BAUDRATE, timeout=ThreespaceSerialComClass.DEFAULT_TIMEOUT)
+        else:
+            raise TypeError("Invalid type for creating a ThreespaceSerialComClass:", type(ser), ser)
+
+        self.ser = ser
+
+        self.peek_buffer = bytearray()
+        self.peek_length = 0
+
+    def write(self, bytes: bytes):
+        self.ser.write(bytes)
+    
+    def read(self, num_bytes: int):
+        if self.peek_length >= num_bytes:
+            result = self.peek_buffer[:num_bytes]
+            self.peek_length -= num_bytes
+            del self.peek_buffer[:num_bytes]
+        else:
+            result = self.peek_buffer + self.ser.read(num_bytes - self.peek_length) #Must supply the amount desired to read instead of just buffering so the timeout works
+            self.peek_buffer.clear()
+            self.peek_length = 0
+        return result
+    
+    def peek(self, num_bytes: int):
+        if self.peek_length >= num_bytes:
+            return self.peek_buffer[:num_bytes]
+        else:
+            self.peek_buffer += self.ser.read(num_bytes - self.peek_length) #Must supply the amount desired to read instead of just buffering so the timeout works
+            self.peek_length = len(self.peek_buffer) #The read may have timed out, so calculate new size
+            return self.peek_buffer.copy()
+        
+    def read_until(self, expected: bytes) -> bytes:
+        if expected in self.peek_buffer:
+            length = self.peek_buffer.index(expected) + len(expected)
+            result = self.peek_buffer[:length]
+            self.peek_length -= length
+            del self.peek_buffer[:length]
+            return result
+        #Have to actually read from serial port until the data is available
+        result = self.peek_buffer + self.ser.read_until(expected)
+        self.peek_buffer.clear()
+        self.peek_length = 0
+        return result
+
+    def peek_until(self, expected: bytes, max_length: int = None) -> bytes:
+        if expected in self.peek_buffer:
+            length = self.peek_buffer.index(expected) + len(expected)
+            if max_length is not None and length > max_length: 
+                length = max_length
+            result = self.peek_buffer[:length]
+            return result
+        if max_length is not None and self.peek_length >= max_length:
+            return self.peek_buffer[:max_length]
+        
+        #Have to actually read from serial port until the data is available
+        if max_length is not None:
+            max_length = max(0, max_length - self.peek_length)
+        self.peek_buffer += self.ser.read_until(expected, size=max_length)
+        self.peek_length = len(self.peek_buffer)
+        return self.peek_buffer.copy()
+
+    def close(self):
+        self.ser.close()
+        self.peek_buffer.clear()
+        self.peek_length = 0
+    
+    def open(self):
+        try:
+            self.ser.open()
+        except:
+            return False
+        return True
+
+    def check_open(self):
+        try:
+            self.ser.in_waiting
+        except:
+            return False
+        return True
+
+    @property
+    def length(self):
+        return self.peek_length + self.ser.in_waiting    
+
+    @property
+    def timeout(self) -> float:
+        return self.ser.timeout
+    
+    @timeout.setter
+    def timeout(self, timeout: float):
+        self.ser.timeout = timeout    
+
+    @property
+    def reenumerates(self) -> bool:
+        return True
+    
+    @property
+    def name(self) -> str:
+        return self.ser.port
+
+    #This is not part of the ThreespaceComClass interface, but is useful as a utility for those directly using the ThreespaceSerialComClass
+    @staticmethod 
+    def enumerate_ports():
+        cls = ThreespaceSerialComClass
+        ports = serial.tools.list_ports.comports()
+        for port in ports:
+            if port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER): 
+                yield port
+
+    @staticmethod
+    def auto_detect(default_timeout: float = 2, default_baudrate: int = 115200) -> Generator["ThreespaceSerialComClass", None, None]:
+        """
+        Returns a list of com classes of the same type called on nearby.
+        These ports will start unopened. This allows the caller to get a list of ports without having to connect.
+        """
+        cls = ThreespaceSerialComClass
+        ports = serial.tools.list_ports.comports()
+        for port in ports:
+            if port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER): 
+                ser = serial.Serial(None, baudrate=default_baudrate, timeout=default_timeout) #By setting port as None, can create an object without immediately opening the port
+                ser.port = port.device #Now assign the port, allowing the serial object to exist without being opened yet
+                yield ThreespaceSerialComClass(ser)

yostlabs/math/__init__.py

@@ -0,0 +1 @@
+__all__ = ["quaternion", "vector"]

yostlabs/math/quaternion.py

@@ -0,0 +1,149 @@
+import math
+import yostlabs.math.vector as _vec
+
+def quat_mul(a: list[float], b: list[float]):
+    out = [0, 0, 0, 0]
+    x = 0; y = 1; z = 2; w = 3
+    out[w] = a[w]*b[w] - a[x]*b[x] - a[y]*b[y] - a[z]*b[z]
+    out[x] = a[w]*b[x] + a[x]*b[w] + a[y]*b[z] - a[z]*b[y]
+    out[y] = a[w]*b[y] + a[y]*b[w] + a[z]*b[x] - a[x]*b[z]
+    out[z] = a[w]*b[z] + a[z]*b[w] + a[x]*b[y] - a[y]*b[x]
+    return out
+
+#Rotates quaternion b by quaternion a, it does not combine them
+def quat_rotate(a: list[float], b: list[float]):
+    inv = a.copy()
+    inv[0] *= -1
+    inv[1] *= -1
+    inv[2] *= -1
+    axis = [b[0], b[1], b[2], 0]
+    halfway = quat_mul(a, axis)
+    final = quat_mul(halfway, inv)
+    return [*final[:3], b[3]]
+
+def quat_inverse(quat: list[float]):
+    return [-quat[0], -quat[1], -quat[2], quat[3]]
+
+def quat_rotate_vec(quat: list[float], vec: list[float]):
+    inv = quat_inverse(quat)
+    tmp = [vec[0], vec[1], vec[2], 0]
+    halfway = quat_mul(quat, tmp)
+    final = quat_mul(halfway, inv)
+    return [final[0], final[1], final[2]]
+
+def angles_to_quaternion(angles: list[float], order: str, degrees=True):
+    quat = [0, 0, 0, 1]
+    for i in range(len(angles)):
+        axis = order[i]
+        angle = angles[i]
+        if degrees:
+            angle = math.radians(angle)
+        unit_vec = _vec.axis_to_unit_vector(axis)
+        w = math.cos(angle / 2)
+        imaginary = math.sin(angle / 2)
+        unit_vec = [v * imaginary for v in unit_vec]
+        angle_quat = [*unit_vec, w]
+        #print(f"{axis} {angle} {angle_quat}")
+        quat = quat_mul(quat, angle_quat)
+    return quat
+
+def quat_from_axis_angle(axis: list[float], angle: float):
+    imaginary = math.sin(angle / 2)
+    quat = [imaginary * v for v in axis]
+    quat.append(math.cos(angle / 2))
+    return quat
+
+def quat_from_two_vectors(forward: list[float], down: list[float]):
+    """
+    This function requires two orthogonal vectors to work
+    """
+    forward_reference = [0, 0, 1]
+    down_reference = [0, -1, 0]
+
+    forward = _vec.vec_normalize(forward)
+    down = _vec.vec_normalize(down)
+
+    #Create the first rotation to align the forward axis
+    axis_of_rotation = _vec.vec_cross(forward_reference, forward)
+    axis_of_rotation = _vec.vec_normalize(axis_of_rotation)
+    if not any(abs(v) > 0 for v in axis_of_rotation):
+        axis_of_rotation = down_reference #This is just a direct 180 degree rotation around any orthogonal axis, so just use the down_ref
+    dot = min(1, max(-1, _vec.vec_dot(forward_reference, forward)))
+    angle1 = math.acos(dot)
+    imaginary = math.sin(angle1/2)
+    quat = [v * imaginary for v in axis_of_rotation] #XYZ
+    quat.append(math.cos(angle1/2)) #W
+
+    #Update the reference to figure out where it is after the turn
+    down_reference = quat_rotate_vec(quat, down_reference)
+
+    #find the rotation to make the remaining reference align with its given value
+    axis_of_rotation = _vec.vec_cross(down_reference, down)
+    axis_of_rotation = _vec.vec_normalize(axis_of_rotation)
+    if not any(abs(v) > 0 for v in axis_of_rotation):
+        axis_of_rotation = forward #Rotate along the final forward vector until up is aligned
+    dot = min(1, max(-1, _vec.vec_dot(down_reference, down)))
+    angle2 = math.acos(dot)
+    imaginary = math.sin(angle2/2)
+    rotation = [v * imaginary for v in axis_of_rotation] #XYZ
+    rotation.append(math.cos(angle2/2)) #W
+
+    quat = quat_mul(rotation, quat)
+    return quat
+
+def quaternion_to_3x3_rotation_matrix(quat):
+    """
+    Convert a quaternion in form x, y, z, w to a rotation matrix
+    """
+    x, y, z, w = quat
+
+    fTx = 2.0 * x
+    fTy = 2.0 * y
+    fTz = 2.0 * z
+    fTwx = fTx * w
+    fTwy = fTy * w
+    fTwz = fTz * w
+    fTxx = fTx * x
+    fTxy = fTy * x
+    fTxz = fTz * x
+    fTyy = fTy * y
+    fTyz = fTz * y
+    fTzz = fTz * z
+
+    out = [0] * 9
+    out[0] = 1.0-(fTyy+fTzz)
+    out[1] = fTxy-fTwz
+    out[2] = fTxz+fTwy
+    out[3] = fTxy+fTwz
+    out[4] = 1.0-(fTxx+fTzz)
+    out[5] = fTyz-fTwx
+    out[6] = fTxz-fTwy
+    out[7] = fTyz + fTwx
+    out[8] = 1.0 - (fTxx+fTyy)
+
+    return [
+        [out[0], out[1], out[2]],
+        [out[3], out[4], out[5]],
+        [out[6], out[7], out[8]],
+    ]
+
+def quaternion_global_to_local(quat, vec):
+    inverse = quat_inverse(quat)
+    return quat_rotate_vec(inverse, vec)
+
+def quaternion_local_to_global(quat, vec):
+    return quat_rotate_vec(quat, vec)
+
+#https://splines.readthedocs.io/en/latest/rotation/slerp.html
+def slerp(a, b, t):
+    dot = _vec.vec_dot(a, b)
+    if dot < 0: #To force it to be the shortest route
+        b = [-v for v in b]
+
+    theta = math.acos(dot)
+    sin_theta = math.sin(theta)
+    r1 = math.sin(1 - t) * theta / sin_theta
+    r2 = math.sin(t * theta) / sin_theta
+    a = [r1 * v for v in a]
+    b = [r2 * v for v in b]
+    return _vec.vec_normalize([v + w for v, w in zip(a, b)])

yostlabs/math/vector.py

@@ -0,0 +1,31 @@
+def vec_len(vector: list[float|int]):
+    return sum(v ** 2 for v in vector) ** 0.5
+
+def vec_dot(a: list[float], b: list[float]):
+    return sum(a[i] * b[i] for i in range(len(a)))
+
+def vec_cross(a: list[float], b: list[float]):
+    cross = [0, 0, 0]
+    cross[0] = a[1] * b[2] - a[2] * b[1]
+    cross[1] = a[2] * b[0] - a[0] * b[2]
+    cross[2] = a[0] * b[1] - a[1] * b[0]
+    return cross
+
+def vec_normalize(vec: list[float]):
+    l = vec_len(vec)
+    if l == 0:
+        return vec
+    return [v / l for v in vec]
+
+def vec_is_right_handed(order: str, negations: list[bool]):
+    right_handed = order.lower() in ("xzy", "yxz", "zyx")
+    for i in range(3):
+        if negations[i]:
+            right_handed = not right_handed
+    return right_handed
+
+def axis_to_unit_vector(axis: str):
+    axis = axis.lower()
+    if axis == 'x' or axis == 0: return [1, 0, 0]
+    if axis == 'y' or axis == 1: return [0, 1, 0]
+    if axis == 'z' or axis == 2: return [0, 0, 1]