wirepod-vector-sdk-audio 0.9.0__py3-none-any.whl

anki_vector/util.py

@@ -0,0 +1,1132 @@
+# Copyright (c) 2018 Anki, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License in the file LICENSE.txt or at
+#
+#     https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Utility functions and classes for the Vector SDK.
+"""
+
+# __all__ should order by constants, event classes, other classes, functions.
+__all__ = ['Angle',
+           'BaseOverlay',
+           'Component',
+           'Distance',
+           'ImageRect',
+           'Matrix44',
+           'Pose',
+           'Position',
+           'Quaternion',
+           'RectangleOverlay',
+           'Speed',
+           'Vector2',
+           'Vector3',
+           'angle_z_to_quaternion',
+           'block_while_none',
+           'degrees',
+           'distance_mm',
+           'distance_inches',
+           'get_class_logger',
+           'parse_command_args',
+           'radians',
+           'setup_basic_logging',
+           'speed_mmps']
+
+import argparse
+import configparser
+from functools import wraps
+import logging
+import math
+import os
+from pathlib import Path
+import sys
+import time
+from typing import Callable, Union
+
+from .exceptions import VectorConfigurationException, VectorPropertyValueNotReadyException
+from .messaging import protocol
+
+try:
+    from PIL import Image, ImageDraw
+except ImportError:
+    sys.exit("Cannot import from PIL: Do `pip3 install --user Pillow` to install")
+
+
+def parse_command_args(parser: argparse.ArgumentParser = None):
+    """
+    Parses command line arguments.
+
+    Attempts to read the robot serial number from the command line arguments. If no serial number
+    is specified, we next attempt to read the robot serial number from environment variable ANKI_ROBOT_SERIAL.
+    If ANKI_ROBOT_SERIAL is specified, the value will be used as the robot's serial number.
+
+    .. code-block:: python
+
+        import anki_vector
+
+        import argparse
+
+        parser = argparse.ArgumentParser()
+        parser.add_argument("--new_param")
+        args = anki_vector.util.parse_command_args(parser)
+
+    :param parser: To add new command line arguments,
+         pass an argparse parser with the new options
+         already defined. Leave empty to use the defaults.
+    """
+    if parser is None:
+        parser = argparse.ArgumentParser()
+    parser.add_argument("-s", "--serial", nargs='?', default=os.environ.get('ANKI_ROBOT_SERIAL', None))
+    return parser.parse_args()
+
+
+def block_while_none(interval: float = 0.1, max_iterations: int = 50):
+    """Use this to denote a property that may need some delay before it appears.
+
+    :param interval: how often to check if the property is no longer None
+    :param max_iterations: how many times to check the property before raising an error
+
+    This will raise a :class:`VectorControlTimeoutException` if the property cannot be retrieved
+    before :attr:`max_iterations`.
+    """
+    def blocker(func: Callable):
+        @wraps(func)
+        def wrapped(*args, **kwargs):
+            iterations = 0
+            result = func(*args, **kwargs)
+            while result is None:
+                time.sleep(interval)
+                iterations += 1
+                if iterations > max_iterations:
+                    raise VectorPropertyValueNotReadyException()
+                result = func(*args, **kwargs)
+            return result
+        return wrapped
+    return blocker
+
+
+def setup_basic_logging(custom_handler: logging.Handler = None,
+                        general_log_level: str = None,
+                        target: object = None):
+    """Helper to perform basic setup of the Python logger.
+
+    :param custom_handler: provide an external logger for custom logging locations
+    :param general_log_level: 'DEBUG', 'INFO', 'WARN', 'ERROR' or an equivalent
+            constant from the :mod:`logging` module. If None then a
+            value will be read from the VECTOR_LOG_LEVEL environment variable.
+    :param target: The stream to send the log data to; defaults to stderr
+    """
+    if general_log_level is None:
+        general_log_level = os.environ.get('VECTOR_LOG_LEVEL', logging.INFO)
+
+    handler = custom_handler
+    if handler is None:
+        handler = logging.StreamHandler(stream=target)
+        formatter = logging.Formatter("%(asctime)s.%(msecs)03d %(name)+25s %(levelname)+7s  %(message)s",
+                                      "%H:%M:%S")
+        handler.setFormatter(formatter)
+
+        class LogCleanup(logging.Filter):  # pylint: disable=too-few-public-methods
+            def filter(self, record):
+                # Drop 'anki_vector' from log messages
+                record.name = '.'.join(record.name.split('.')[1:])
+                # Indent past informational chunk
+                record.msg = record.msg.replace("\n", f"\n{'':48}")
+                return True
+        handler.addFilter(LogCleanup())
+
+    vector_logger = logging.getLogger('anki_vector')
+    if not vector_logger.handlers:
+        vector_logger.addHandler(handler)
+        vector_logger.setLevel(general_log_level)
+
+
+def get_class_logger(module: str, obj: object) -> logging.Logger:
+    """Helper to create logger for a given class (and module).
+
+    .. testcode::
+
+        import anki_vector
+
+        logger = anki_vector.util.get_class_logger("module_name", "object_name")
+
+    :param module: The name of the module to which the object belongs.
+    :param obj: the object that owns the logger.
+    """
+    return logging.getLogger(".".join([module, type(obj).__name__]))
+
+
+class Vector2:
+    """Represents a 2D Vector (type/units aren't specified).
+
+    :param x: X component
+    :param y: Y component
+    """
+
+    __slots__ = ('_x', '_y')
+
+    def __init__(self, x: float, y: float):
+        self._x = float(x)
+        self._y = float(y)
+
+    def set_to(self, rhs):
+        """Copy the x and y components of the given Vector2 instance.
+
+        :param rhs: The right-hand-side of this assignment - the
+                source Vector2 to copy into this Vector2 instance.
+        """
+        self._x = float(rhs.x)
+        self._y = float(rhs.y)
+
+    @property
+    def x(self) -> float:
+        """The x component."""
+        return self._x
+
+    @property
+    def y(self) -> float:
+        """The y component."""
+        return self._y
+
+    @property
+    def x_y(self):
+        """tuple (float, float): The X, Y elements of the Vector2 (x,y)"""
+        return self._x, self._y
+
+    def __repr__(self):
+        return "<%s x: %.2f y: %.2f>" % (self.__class__.__name__, self.x, self.y)
+
+    def __add__(self, other):
+        if not isinstance(other, Vector2):
+            raise TypeError("Unsupported operand for + expected Vector2")
+        return Vector2(self.x + other.x, self.y + other.y)
+
+    def __sub__(self, other):
+        if not isinstance(other, Vector2):
+            raise TypeError("Unsupported operand for - expected Vector2")
+        return Vector2(self.x - other.x, self.y - other.y)
+
+    def __mul__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for * expected number")
+        return Vector2(self.x * other, self.y * other)
+
+    def __truediv__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for / expected number")
+        return Vector2(self.x / other, self.y / other)
+
+
+class Vector3:
+    """Represents a 3D Vector (type/units aren't specified).
+
+    :param x: X component
+    :param y: Y component
+    :param z: Z component
+    """
+
+    __slots__ = ('_x', '_y', '_z')
+
+    def __init__(self, x: float, y: float, z: float):
+        self._x = float(x)
+        self._y = float(y)
+        self._z = float(z)
+
+    def set_to(self, rhs):
+        """Copy the x, y and z components of the given Vector3 instance.
+
+        :param rhs: The right-hand-side of this assignment - the
+                source Vector3 to copy into this Vector3 instance.
+        """
+        self._x = float(rhs.x)
+        self._y = float(rhs.y)
+        self._z = float(rhs.z)
+
+    @property
+    def x(self) -> float:
+        """The x component."""
+        return self._x
+
+    @property
+    def y(self) -> float:
+        """The y component."""
+        return self._y
+
+    @property
+    def z(self) -> float:
+        """The z component."""
+        return self._z
+
+    @property
+    def magnitude_squared(self) -> float:
+        """float: The magnitude of the Vector3 instance"""
+        return self._x**2 + self._y**2 + self._z**2
+
+    @property
+    def magnitude(self) -> float:
+        """The magnitude of the Vector3 instance"""
+        return math.sqrt(self.magnitude_squared)
+
+    @property
+    def normalized(self):
+        """A Vector3 instance with the same direction and unit magnitude"""
+        mag = self.magnitude
+        if mag == 0:
+            return Vector3(0, 0, 0)
+        return Vector3(self._x / mag, self._y / mag, self._z / mag)
+
+    def dot(self, other):
+        """The dot product of this and another Vector3 instance"""
+        if not isinstance(other, Vector3):
+            raise TypeError("Unsupported argument for dot product, expected Vector3")
+        return self._x * other.x + self._y * other.y + self._z * other.z
+
+    def cross(self, other):
+        """The cross product of this and another Vector3 instance"""
+        if not isinstance(other, Vector3):
+            raise TypeError("Unsupported argument for cross product, expected Vector3")
+
+        return Vector3(
+            self._y * other.z - self._z * other.y,
+            self._z * other.x - self._x * other.z,
+            self._x * other.y - self._y * other.x)
+
+    @property
+    def x_y_z(self):
+        """tuple (float, float, float): The X, Y, Z elements of the Vector3 (x,y,z)"""
+        return self._x, self._y, self._z
+
+    def __repr__(self):
+        return f"<{self.__class__.__name__} x: {self.x:.2f} y: {self.y:.2f} z: {self.z:.2f}>"
+
+    def __add__(self, other):
+        if not isinstance(other, Vector3):
+            raise TypeError("Unsupported operand for +, expected Vector3")
+        return Vector3(self.x + other.x, self.y + other.y, self.z + other.z)
+
+    def __sub__(self, other):
+        if not isinstance(other, Vector3):
+            raise TypeError("Unsupported operand for -, expected Vector3")
+        return Vector3(self.x - other.x, self.y - other.y, self.z - other.z)
+
+    def __mul__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for * expected number")
+        return Vector3(self.x * other, self.y * other, self.z * other)
+
+    def __truediv__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for / expected number")
+        return Vector3(self.x / other, self.y / other, self.z / other)
+
+
+class Angle:
+    """Represents an angle.
+
+    Use the :func:`degrees` or :func:`radians` convenience methods to generate
+    an Angle instance.
+
+    :param radians: The number of radians the angle should represent
+        (cannot be combined with ``degrees``)
+    :param degrees: The number of degress the angle should represent
+        (cannot be combined with ``radians``)
+    """
+
+    __slots__ = ('_radians')
+
+    def __init__(self, radians: float = None, degrees: float = None):  # pylint: disable=redefined-outer-name
+        if radians is None and degrees is None:
+            raise ValueError("Expected either the degrees or radians keyword argument")
+        if radians and degrees:
+            raise ValueError("Expected either the degrees or radians keyword argument, not both")
+
+        if degrees is not None:
+            radians = degrees * math.pi / 180
+        self._radians = float(radians)
+
+    @property
+    def radians(self) -> float:  # pylint: disable=redefined-outer-name
+        """The angle in radians."""
+        return self._radians
+
+    @property
+    def degrees(self) -> float:  # pylint: disable=redefined-outer-name
+        """The angle in degrees."""
+        return self._radians / math.pi * 180
+
+    def __repr__(self):
+        return f"<{self.__class__.__name__} Radians: {self.radians:.2f} Degrees: {self.degrees:.2f}>"
+
+    def __add__(self, other):
+        if not isinstance(other, Angle):
+            raise TypeError("Unsupported type for + expected Angle")
+        return Angle(radians=(self.radians + other.radians))
+
+    def __sub__(self, other):
+        if not isinstance(other, Angle):
+            raise TypeError("Unsupported type for - expected Angle")
+        return Angle(radians=(self.radians - other.radians))
+
+    def __mul__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported type for * expected number")
+        return Angle(radians=(self.radians * other))
+
+    def __truediv__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported type for / expected number")
+        return radians(self.radians / other)
+
+    def _cmp_int(self, other):
+        if not isinstance(other, Angle):
+            raise TypeError("Unsupported type for comparison expected Angle")
+        return self.radians - other.radians
+
+    def __eq__(self, other):
+        return self._cmp_int(other) == 0
+
+    def __ne__(self, other):
+        return self._cmp_int(other) != 0
+
+    def __gt__(self, other):
+        return self._cmp_int(other) > 0
+
+    def __lt__(self, other):
+        return self._cmp_int(other) < 0
+
+    def __ge__(self, other):
+        return self._cmp_int(other) >= 0
+
+    def __le__(self, other):
+        return self._cmp_int(other) <= 0
+
+    @property
+    def abs_value(self):
+        """:class:`anki_vector.util.Angle`: The absolute value of the angle.
+
+        If the Angle is positive then it returns a copy of this Angle, otherwise it returns -Angle.
+        """
+        return Angle(radians=abs(self._radians))
+
+
+def angle_z_to_quaternion(angle_z: Angle):
+    """This function converts an angle in the z axis (Euler angle z component) to a quaternion.
+
+    :param angle_z: The z axis angle.
+
+    Returns:
+        q0, q1, q2, q3 (float, float, float, float): A tuple with all the members
+            of a quaternion defined by angle_z.
+    """
+
+    # Define the quaternion to be converted from a Euler angle (x,y,z) of 0,0,angle_z
+    # These equations have their original equations above, and simplified implemented
+    # q0 = cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2)
+    q0 = math.cos(angle_z.radians / 2)
+    # q1 = sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2)
+    q1 = 0
+    # q2 = cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2)
+    q2 = 0
+    # q3 = cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)
+    q3 = math.sin(angle_z.radians / 2)
+    return q0, q1, q2, q3
+
+
+def degrees(degrees: float) -> Angle:  # pylint: disable=redefined-outer-name
+    """An Angle instance set to the specified number of degrees."""
+    return Angle(degrees=degrees)
+
+
+def radians(radians: float) -> Angle:  # pylint: disable=redefined-outer-name
+    """An Angle instance set to the specified number of radians."""
+    return Angle(radians=radians)
+
+
+class Matrix44:
+    """A 4x4 Matrix for representing the rotation and/or position of an object in the world.
+
+    Can be generated from a :class:`Quaternion` for a pure rotation matrix, or
+    combined with a position for a full translation matrix, as done by
+    :meth:`Pose.to_matrix`.
+    """
+    __slots__ = ('m00', 'm10', 'm20', 'm30',
+                 'm01', 'm11', 'm21', 'm31',
+                 'm02', 'm12', 'm22', 'm32',
+                 'm03', 'm13', 'm23', 'm33')
+
+    def __init__(self,
+                 m00: float, m10: float, m20: float, m30: float,
+                 m01: float, m11: float, m21: float, m31: float,
+                 m02: float, m12: float, m22: float, m32: float,
+                 m03: float, m13: float, m23: float, m33: float):
+        self.m00 = float(m00)
+        self.m10 = float(m10)
+        self.m20 = float(m20)
+        self.m30 = float(m30)
+
+        self.m01 = float(m01)
+        self.m11 = float(m11)
+        self.m21 = float(m21)
+        self.m31 = float(m31)
+
+        self.m02 = float(m02)
+        self.m12 = float(m12)
+        self.m22 = float(m22)
+        self.m32 = float(m32)
+
+        self.m03 = float(m03)
+        self.m13 = float(m13)
+        self.m23 = float(m23)
+        self.m33 = float(m33)
+
+    def __repr__(self):
+        return ("<%s: "
+                "%.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f "
+                "%.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f>" % (
+                    self.__class__.__name__, *self.in_row_order))
+
+    @property
+    def tabulated_string(self) -> str:
+        """A multi-line string formatted with tabs to show the matrix contents."""
+        return ("%.1f\t%.1f\t%.1f\t%.1f\n"
+                "%.1f\t%.1f\t%.1f\t%.1f\n"
+                "%.1f\t%.1f\t%.1f\t%.1f\n"
+                "%.1f\t%.1f\t%.1f\t%.1f" % self.in_row_order)
+
+    @property
+    def in_row_order(self):
+        """tuple of 16 floats: The contents of the matrix in row order."""
+        return self.m00, self.m01, self.m02, self.m03,\
+            self.m10, self.m11, self.m12, self.m13,\
+            self.m20, self.m21, self.m22, self.m23,\
+            self.m30, self.m31, self.m32, self.m33
+
+    @property
+    def in_column_order(self):
+        """tuple of 16 floats: The contents of the matrix in column order."""
+        return self.m00, self.m10, self.m20, self.m30,\
+            self.m01, self.m11, self.m21, self.m31,\
+            self.m02, self.m12, self.m22, self.m32,\
+            self.m03, self.m13, self.m23, self.m33
+
+    @property
+    def forward_xyz(self):
+        """tuple of 3 floats: The x,y,z components representing the matrix's forward vector."""
+        return self.m00, self.m01, self.m02
+
+    @property
+    def left_xyz(self):
+        """tuple of 3 floats: The x,y,z components representing the matrix's left vector."""
+        return self.m10, self.m11, self.m12
+
+    @property
+    def up_xyz(self):
+        """tuple of 3 floats: The x,y,z components representing the matrix's up vector."""
+        return self.m20, self.m21, self.m22
+
+    @property
+    def pos_xyz(self):
+        """tuple of 3 floats: The x,y,z components representing the matrix's position vector."""
+        return self.m30, self.m31, self.m32
+
+    def set_forward(self, x: float, y: float, z: float):
+        """Set the x,y,z components representing the matrix's forward vector.
+
+        :param x: The X component.
+        :param y: The Y component.
+        :param z: The Z component.
+        """
+        self.m00 = float(x)
+        self.m01 = float(y)
+        self.m02 = float(z)
+
+    def set_left(self, x: float, y: float, z: float):
+        """Set the x,y,z components representing the matrix's left vector.
+
+        :param x: The X component.
+        :param y: The Y component.
+        :param z: The Z component.
+        """
+        self.m10 = float(x)
+        self.m11 = float(y)
+        self.m12 = float(z)
+
+    def set_up(self, x: float, y: float, z: float):
+        """Set the x,y,z components representing the matrix's up vector.
+
+        :param x: The X component.
+        :param y: The Y component.
+        :param z: The Z component.
+        """
+        self.m20 = float(x)
+        self.m21 = float(y)
+        self.m22 = float(z)
+
+    def set_pos(self, x: float, y: float, z: float):
+        """Set the x,y,z components representing the matrix's position vector.
+
+        :param x: The X component.
+        :param y: The Y component.
+        :param z: The Z component.
+        """
+        self.m30 = float(x)
+        self.m31 = float(y)
+        self.m32 = float(z)
+
+
+class Quaternion:
+    """Represents the rotation of an object in the world."""
+
+    __slots__ = ('_q0', '_q1', '_q2', '_q3')
+
+    def __init__(self, q0: float = None, q1: float = None, q2: float = None, q3: float = None, angle_z: Angle = None):
+        is_quaternion = q0 is not None and q1 is not None and q2 is not None and q3 is not None
+
+        if not is_quaternion and angle_z is None:
+            raise ValueError("Expected either the q0 q1 q2 and q3 or angle_z keyword arguments")
+        if is_quaternion and angle_z:
+            raise ValueError("Expected either the q0 q1 q2 and q3 or angle_z keyword argument,"
+                             "not both")
+        if angle_z is not None:
+            if not isinstance(angle_z, Angle):
+                raise TypeError("Unsupported type for angle_z expected Angle")
+            q0, q1, q2, q3 = angle_z_to_quaternion(angle_z)
+
+        self._q0 = float(q0)
+        self._q1 = float(q1)
+        self._q2 = float(q2)
+        self._q3 = float(q3)
+
+    @property
+    def q0(self) -> float:
+        """The q0 (w) value of the quaternion."""
+        return self._q0
+
+    @property
+    def q1(self) -> float:
+        """The q1 (i) value of the quaternion."""
+        return self._q1
+
+    @property
+    def q2(self) -> float:
+        """The q2 (j) value of the quaternion."""
+        return self._q2
+
+    @property
+    def q3(self) -> float:
+        """The q3 (k) value of the quaternion."""
+        return self._q3
+
+    @property
+    def angle_z(self) -> Angle:
+        """An Angle instance representing the z Euler component of the object's rotation.
+
+        Defined as the rotation in the z axis.
+        """
+        q0, q1, q2, q3 = self.q0_q1_q2_q3
+        return Angle(radians=math.atan2(2 * (q1 * q2 + q0 * q3), 1 - 2 * (q2**2 + q3**2)))
+
+    @property
+    def q0_q1_q2_q3(self):
+        """tuple of float: Contains all elements of the quaternion (q0,q1,q2,q3)"""
+        return self._q0, self._q1, self._q2, self._q3
+
+    def to_matrix(self, pos_x: float = 0.0, pos_y: float = 0.0, pos_z: float = 0.0):
+        """Convert the Quaternion to a 4x4 matrix representing this rotation.
+
+        A position can also be provided to generate a full translation matrix.
+
+        :param pos_x: The x component for the position.
+        :param pos_y: The y component for the position.
+        :param pos_z: The z component for the position.
+
+        Returns:
+            :class:`anki_vector.util.Matrix44`: A matrix representing this Quaternion's
+            rotation, with the provided position (which defaults to 0,0,0).
+        """
+        # See https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
+        q0q0 = self.q0 * self.q0
+        q1q1 = self.q1 * self.q1
+        q2q2 = self.q2 * self.q2
+        q3q3 = self.q3 * self.q3
+
+        q0x2 = self.q0 * 2.0  # saves 2 multiplies
+        q0q1x2 = q0x2 * self.q1
+        q0q2x2 = q0x2 * self.q2
+        q0q3x2 = q0x2 * self.q3
+        q1x2 = self.q1 * 2.0  # saves 1 multiply
+        q1q2x2 = q1x2 * self.q2
+        q1q3x2 = q1x2 * self.q3
+        q2q3x2 = 2.0 * self.q2 * self.q3
+
+        m00 = (q0q0 + q1q1 - q2q2 - q3q3)
+        m01 = (q1q2x2 + q0q3x2)
+        m02 = (q1q3x2 - q0q2x2)
+
+        m10 = (q1q2x2 - q0q3x2)
+        m11 = (q0q0 - q1q1 + q2q2 - q3q3)
+        m12 = (q0q1x2 + q2q3x2)
+
+        m20 = (q0q2x2 + q1q3x2)
+        m21 = (q2q3x2 - q0q1x2)
+        m22 = (q0q0 - q1q1 - q2q2 + q3q3)
+
+        return Matrix44(m00, m10, m20, float(pos_x),
+                        m01, m11, m21, float(pos_y),
+                        m02, m12, m22, float(pos_z),
+                        0.0, 0.0, 0.0, 1.0)
+
+    def __repr__(self):
+        return (f"<{self.__class__.__name__} q0: {self.q0:.2f} q1: {self.q1:.2f}"
+                f" q2: {self.q2:.2f} q3: {self.q3:.2f} {self.angle_z}>")
+
+
+class Position(Vector3):
+    """Represents the position of an object in the world.
+
+    A position consists of its x, y and z values in millimeters.
+
+    :param x: X position in millimeters
+    :param y: Y position in millimeters
+    :param z: Z position in millimeters
+    """
+    __slots__ = ()
+
+
+class Pose:
+    """Represents where an object is in the world.
+
+    Whenever Vector is delocalized (i.e. whenever Vector no longer knows
+    where he is - e.g. when he's picked up), Vector creates a new pose starting at
+    (0,0,0) with no rotation, with origin_id incremented to show that these poses
+    cannot be compared with earlier ones. As Vector drives around, his pose (and the
+    pose of other objects he observes - e.g. faces, his LightCube, charger, etc.) is relative to this
+    initial position and orientation.
+
+    The coordinate space is relative to Vector, where Vector's origin is the
+    point on the ground between Vector's two front wheels. The X axis is Vector's forward direction,
+    the Y axis is to Vector's left, and the Z axis is up.
+
+    Only poses of the same origin_id can safely be compared or operated on.
+
+    .. testcode::
+
+        import anki_vector
+        from anki_vector.util import degrees, Pose
+
+        with anki_vector.Robot() as robot:
+            pose = Pose(x=50, y=0, z=0, angle_z=anki_vector.util.Angle(degrees=0))
+            robot.behavior.go_to_pose(pose)
+    """
+    __slots__ = ('_position', '_rotation', '_origin_id')
+
+    def __init__(self, x: float, y: float, z: float, q0: float = None, q1: float = None, q2: float = None, q3: float = None,
+                 angle_z: Angle = None, origin_id: int = -1):
+        self._position = Position(x, y, z)
+        self._rotation = Quaternion(q0, q1, q2, q3, angle_z)
+        self._origin_id = origin_id
+
+    @property
+    def position(self) -> Position:
+        """The position component of this pose."""
+        return self._position
+
+    @property
+    def rotation(self) -> Quaternion:
+        """The rotation component of this pose."""
+        return self._rotation
+
+    @property
+    def origin_id(self) -> int:
+        """An ID maintained by the robot which represents which coordinate frame this pose is in."""
+        return self._origin_id
+
+    def __repr__(self):
+        return (f"<{self.__class__.__name__}: {self._position}"
+                f" {self._rotation} <Origin Id: {self._origin_id}>>")
+
+    def define_pose_relative_this(self, new_pose):
+        """Creates a new pose such that new_pose's origin is now at the location of this pose.
+
+        :param anki_vector.util.Pose new_pose: The pose which origin is being changed.
+
+        Returns:
+            A :class:`anki_vector.util.Pose` object for which the origin was this pose's origin.
+        """
+        if not isinstance(new_pose, Pose):
+            raise TypeError("Unsupported type for new_origin, must be of type Pose")
+        x, y, z = self.position.x_y_z
+        angle_z = self.rotation.angle_z
+        new_x, new_y, new_z = new_pose.position.x_y_z
+        new_angle_z = new_pose.rotation.angle_z
+
+        cos_angle = math.cos(angle_z.radians)
+        sin_angle = math.sin(angle_z.radians)
+        res_x = x + (cos_angle * new_x) - (sin_angle * new_y)
+        res_y = y + (sin_angle * new_x) + (cos_angle * new_y)
+        res_z = z + new_z
+        res_angle = angle_z + new_angle_z
+        return Pose(res_x,
+                    res_y,
+                    res_z,
+                    angle_z=res_angle,
+                    origin_id=self._origin_id)
+
+    @property
+    def is_valid(self) -> bool:
+        """True if this is a valid, usable pose."""
+        return self.origin_id >= 0
+
+    def is_comparable(self, other_pose) -> bool:
+        """Checks whether these two poses are comparable.
+
+        Poses are comparable if they're valid and having matching origin IDs.
+
+        :param other_pose: The other pose to compare against. Type is Pose.
+
+        Returns:
+            True if the two poses are comparable, False otherwise.
+        """
+        return (self.is_valid and other_pose.is_valid
+                and (self.origin_id == other_pose.origin_id))
+
+    def to_matrix(self) -> Matrix44:
+        """Convert the Pose to a Matrix44.
+
+        Returns:
+            A matrix representing this Pose's position and rotation.
+        """
+        return self.rotation.to_matrix(*self.position.x_y_z)
+
+    def to_proto_pose_struct(self) -> protocol.PoseStruct:
+        """Converts the Pose into the robot's messaging pose format.
+        """
+        return protocol.PoseStruct(
+            x=self._position.x,
+            y=self._position.y,
+            z=self._position.z,
+            q0=self._rotation.q0,
+            q1=self._rotation.q1,
+            q2=self._rotation.q2,
+            q3=self._rotation.q3,
+            origin_id=self._origin_id)
+
+
+class ImageRect:
+    '''Defines a bounding box within an image frame.
+
+    This is used when objects and faces are observed to denote where in
+    the robot's camera view the object or face actually appears.  It's then
+    used by the annotate module to show an outline of a box around
+    the object or face.
+    '''
+
+    __slots__ = ('_x_top_left', '_y_top_left', '_width', '_height')
+
+    def __init__(self, x_top_left: float, y_top_left: float, width: float, height: float):
+        self._x_top_left = float(x_top_left)
+        self._y_top_left = float(y_top_left)
+        self._width = float(width)
+        self._height = float(height)
+
+    @property
+    def x_top_left(self) -> float:
+        """The top left x value of where the object was last visible within Vector's camera view."""
+        return self._x_top_left
+
+    @property
+    def y_top_left(self) -> float:
+        """The top left y value of where the object was last visible within Vector's camera view."""
+        return self._y_top_left
+
+    @property
+    def width(self) -> float:
+        """The width of the object from when it was last visible within Vector's camera view."""
+        return self._width
+
+    @property
+    def height(self) -> float:
+        """The height of the object from when it was last visible within Vector's camera view."""
+        return self._height
+
+    def scale_by(self, scale_multiplier: Union[int, float]) -> None:
+        """Scales the image rectangle by the multiplier provided."""
+        if not isinstance(scale_multiplier, (int, float)):
+            raise TypeError("Unsupported operand for * expected number")
+        self._x_top_left *= scale_multiplier
+        self._y_top_left *= scale_multiplier
+        self._width *= scale_multiplier
+        self._height *= scale_multiplier
+
+
+class Distance:
+    """Represents a distance.
+
+    The class allows distances to be returned in either millimeters or inches.
+
+    Use the :func:`distance_inches` or :func:`distance_mm` convenience methods to generate
+    a Distance instance.
+
+    :param distance_mm: The number of millimeters the distance should
+            represent (cannot be combined with ``distance_inches``).
+    :param distance_inches: The number of inches the distance should
+            represent (cannot be combined with ``distance_mm``).
+    """
+
+    __slots__ = ('_distance_mm')
+
+    def __init__(self, distance_mm: float = None, distance_inches: float = None):  # pylint: disable=redefined-outer-name
+        if distance_mm is None and distance_inches is None:
+            raise ValueError("Expected either the distance_mm or distance_inches keyword argument")
+        if distance_mm and distance_inches:
+            raise ValueError("Expected either the distance_mm or distance_inches keyword argument, not both")
+
+        if distance_inches is not None:
+            distance_mm = distance_inches * 25.4
+        self._distance_mm = float(distance_mm)
+
+    def __repr__(self):
+        return "<%s %.2f mm (%.2f inches)>" % (self.__class__.__name__, self.distance_mm, self.distance_inches)
+
+    def __add__(self, other):
+        if not isinstance(other, Distance):
+            raise TypeError("Unsupported operand for + expected Distance")
+        return distance_mm(self.distance_mm + other.distance_mm)
+
+    def __sub__(self, other):
+        if not isinstance(other, Distance):
+            raise TypeError("Unsupported operand for - expected Distance")
+        return distance_mm(self.distance_mm - other.distance_mm)
+
+    def __mul__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for * expected number")
+        return distance_mm(self.distance_mm * other)
+
+    def __truediv__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for / expected number")
+        return distance_mm(self.distance_mm / other)
+
+    @property
+    def distance_mm(self) -> float:  # pylint: disable=redefined-outer-name
+        """The distance in millimeters"""
+        return self._distance_mm
+
+    @property
+    def distance_inches(self) -> float:  # pylint: disable=redefined-outer-name
+        return self._distance_mm / 25.4
+
+
+def distance_mm(distance_mm: float):  # pylint: disable=redefined-outer-name
+    """Returns an :class:`anki_vector.util.Distance` instance set to the specified number of millimeters."""
+    return Distance(distance_mm=distance_mm)
+
+
+def distance_inches(distance_inches: float):  # pylint: disable=redefined-outer-name
+    """Returns an :class:`anki_vector.util.Distance` instance set to the specified number of inches."""
+    return Distance(distance_inches=distance_inches)
+
+
+class Speed:
+    """Represents a speed.
+
+    This class allows speeds to be measured in millimeters per second.
+
+    The maximum speed is 220 mm/s and is clamped internally.
+
+    Use :func:`speed_mmps` convenience methods to generate
+    a Speed instance.
+
+    :param speed_mmps: The number of millimeters per second the speed
+            should represent.
+    """
+
+    __slots__ = ('_speed_mmps')
+
+    def __init__(self, speed_mmps: float = None):  # pylint: disable=redefined-outer-name
+        if speed_mmps is None:
+            raise ValueError("Expected speed_mmps keyword argument")
+        self._speed_mmps = float(speed_mmps)
+
+    def __repr__(self):
+        return "<%s %.2f mmps>" % (self.__class__.__name__, self.speed_mmps)
+
+    def __add__(self, other):
+        if not isinstance(other, Speed):
+            raise TypeError("Unsupported operand for + expected Speed")
+        return speed_mmps(self.speed_mmps + other.speed_mmps)
+
+    def __sub__(self, other):
+        if not isinstance(other, Speed):
+            raise TypeError("Unsupported operand for - expected Speed")
+        return speed_mmps(self.speed_mmps - other.speed_mmps)
+
+    def __mul__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for * expected number")
+        return speed_mmps(self.speed_mmps * other)
+
+    def __truediv__(self, other):
+        if not isinstance(other, (int, float)):
+            raise TypeError("Unsupported operand for / expected number")
+        return speed_mmps(self.speed_mmps / other)
+
+    @property
+    def speed_mmps(self: float) -> float:  # pylint: disable=redefined-outer-name
+        """The speed in millimeters per second (mmps)."""
+        return self._speed_mmps
+
+
+def speed_mmps(speed_mmps: float):  # pylint: disable=redefined-outer-name
+    """:class:`anki_vector.util.Speed` instance set to the specified millimeters per second speed."""
+    return Speed(speed_mmps=speed_mmps)
+
+
+class BaseOverlay:
+    """A base overlay is used as a base class for other forms of overlays that can be drawn on top of an image.
+
+        :param line_thickness: The thickness of the line being drawn.
+        :param line_color: The color of the line to be drawn.
+    """
+
+    def __init__(self, line_thickness: int, line_color: tuple):
+        self._line_thickness: int = line_thickness
+        self._line_color: tuple = line_color
+
+    @property
+    def line_thickness(self) -> int:
+        """The thickness of the line being drawn."""
+        return self._line_thickness
+
+    @property
+    def line_color(self) -> tuple:
+        """The color of the line to be drawn."""
+        return self._line_color
+
+
+class RectangleOverlay(BaseOverlay):
+    """A rectangle that can be drawn on top of a given image.
+
+        :param width: The width of the rectangle to be drawn.
+        :param height: The height of the rectangle to be drawn.
+        :param line_thickness: The thickness of the line being drawn.
+        :param line_color: The color of the line to be drawn.
+    """
+
+    # @TODO Implement overlay using an ImageRect rather than a raw width & height
+    def __init__(self, width: int, height: int, line_thickness: int = 5, line_color: tuple = (255, 0, 0)):
+        super().__init__(line_thickness, line_color)
+        self._width: int = width
+        self._height: int = height
+
+    @property
+    def width(self) -> int:
+        """The width of the rectangle to be drawn."""
+        return self._width
+
+    @property
+    def height(self) -> int:
+        """The height of the rectangle to be drawn."""
+        return self._height
+
+    def apply_overlay(self, image: Image.Image) -> None:
+        """Draw a rectangle on top of the given image."""
+        d = ImageDraw.Draw(image)
+
+        image_width, image_height = image.size
+        remaining_width = image_width - self.width
+        remaining_height = image_height - self.height
+        x1, y1 = remaining_width // 2, remaining_height // 2
+        x2, y2 = (image_width - (remaining_width // 2)), (image_height - (remaining_height // 2))
+
+        for i in range(0, self.line_thickness):
+            d.rectangle([x1 + i, y1 + i, x2 - i, y2 - i], outline=self.line_color)
+
+
+class Component:
+    """ Base class for all components."""
+
+    def __init__(self, robot):
+        self.logger = get_class_logger(__name__, self)
+        self._robot = robot
+
+    @property
+    def robot(self):
+        return self._robot
+
+    @property
+    def conn(self):
+        return self._robot.conn
+
+    @property
+    def force_async(self):
+        return self._robot.force_async
+
+    @property
+    def grpc_interface(self):
+        """A direct reference to the connected aiogrpc interface.
+        """
+        return self._robot.conn.grpc_interface
+
+
+def read_configuration(serial: str, name: str, logger: logging.Logger) -> dict:
+    """Open the default conf file, and read it into a :class:`configparser.ConfigParser`
+    If :code:`serial is not None`, this method will try to find a configuration with serial
+    number :code:`serial`, and raise an exception otherwise. If :code:`serial is None` and
+    :code:`name is not None`, this method will try to find a configuration which matches
+    the provided name, and raise an exception otherwise. If both :code:`serial is None` and
+    :code:`name is None`, this method will return a configuration if exactly `1` exists, but
+    if multiple configurations exists, it will raise an exception.
+
+    :param serial: Vector's serial number
+    :param name: Vector's name
+    """
+    home = Path.home() / ".anki_vector"
+    conf_file = str(home / "sdk_config.ini")
+    parser = configparser.ConfigParser(strict=False)
+    parser.read(conf_file)
+
+    sections = parser.sections()
+    if not sections:
+        raise VectorConfigurationException('Could not find the sdk configuration file. Please run `python3 -m anki_vector.configure` or `python3 -m anki_vector.configure_pod` to set up your Vector for SDK usage.')
+    elif (serial is None) and (name is None):
+        if len(sections) == 1:
+            serial = sections[0]
+            logger.warning("No serial number or name provided. Automatically selecting {}".format(serial))
+        else:
+            raise VectorConfigurationException("Found multiple robot serial numbers. "
+                                               "Please provide the serial number or name of the Robot you want to control.\n\n"
+                                               "Example: ./01_hello_world.py --serial {{robot_serial_number}}")
+
+    config = {k.lower(): v for k, v in parser.items()}
+
+    if serial is not None:
+        serial = serial.lower()
+        try:
+            return config[serial]
+        except KeyError:
+            raise VectorConfigurationException("Could not find matching robot info for given serial number: {}. "
+                                               "Please check your serial number is correct.\n\n"
+                                               "Example: ./01_hello_world.py --serial {{robot_serial_number}}", serial)
+    else:
+        for keySerial in config:
+            for key in config[keySerial]:
+                if config[keySerial][key] == name:
+                    return config[keySerial]
+                if config[keySerial][key].lower() == name.lower():
+                    logger.warning("Using case-insensitive name match found in config. Set 'name' field to match 'Vector-A1B2' format.")
+                    return config[keySerial]
+
+        raise VectorConfigurationException("Could not find matching robot info for given name: {}. "
+                                           "Please check your name is correct.\n\n"
+                                           "Example: ./01_hello_world.py --name {{robot_name}}", name)