# 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.

"""