micropython-samples/QUATERNIONS.md

1. 3D Rotation (quaternions without the maths)

This repo contains a Quaternion class and a rudimentary 3D vector graphics module. The aim is to present quaternions as an efficient way to manipulate 3d objects and data from inertial measurement units (IMUs). As such they have applications in 3D graphics and robotics.

Quaternions have a reputation for being mathematically difficult. This repo aims to make them usable with no mathematics beyond a basic grasp of Cartesian (x, y, z) coordinates. They are actually easier to use than the traditional Euler angles (heading, pitch and roll), and avoid the mathematical problems associated with Euler angles.

The following fragment shows a classic demo where a wireframe cube rotates to match rotations of an IMU:

# dobj is a display list, imu is a BNo055
# Create a unit cube, scale to 0.8 and move to centre on origin
cube = Cube(RED, BLUE, GREEN) * ( 0.8, 0.8, 0.8) - (0.4, 0.4, 0.4)
rot = Rotator()  # Rotation quaternion
while True:
    rot[:] = imu.quaternion()  # Read IMU and assign to Rotator
    dobj['cube'] = cube @ rot  # Rotate cube to match IMU
    dobj.show()  # Display on screen
    time.sleep(0.1)

In this instance the IMU (a Bosch BNO055) produces quaternion data. Similar data may be computed from other IMUs by means of the sensor fusion module.

Section 2 of this doc aims to describe basic usage of the repo with an absolute minimum of mathematics. Section 3 provides more detail for those wishing to expand the usage of quaternions beyond basic 3D transformations.

The 3D graphics is of demo quality: it is not a serious attempt at a graphics library. Its speed demonstrates that a 3D graphics library should be written in C for adequate performance. It also has known limitations and shortcomings. Its purpose is to prove and demonstrate the quaternion handling: all graphics elements use quaternions in their definitions.

2 Introduction

The graphics module enables various wireframe objects to be rendered from a "camera" view with an illusion of perspective. Objects may be moved, scaled, distorted and rotated using arithmetic operators. Thus if cube is an instance of a Cube,
cube + (0, 0.2, 0) will move it along the Y axis.
cube * (1, 0.2, 0.2) will turn it into a cuboid.
cube @ Rotator(0.1, 1, 0, 0) will rotate it around the X axis by 0.1 radians. Any axis may be specified.

I wrote this to experiment with and test quat.py rather than as a serious attempt at 3D graphics. It is written in Python so speed is an issue. Realtime display of orientation is feasible. Playable games aren't. A C port would be awesome.

2.1 Files

The library provides the following files which should be copied to the target's filesystem.
Core files:

1. quat.py Provides classes for creating and rotating points.
2. graph3d.py Provides wireframe 3D graphics classes.

Setup files. There should be a file setup3d.py defining graphics and (optionally) IMU hardware:

1. setup3d.py Assumes an SSD1351 OLED display and BNo055 IMU.
2. setup3d_lcd160cr Setup file for the official LCD. On the target rename to setup3d.py to use. Uncomment the BNo055 code if required.

Demo scripts:

1. test3d.py Test script displays various moving objects.
2. test_imu.py Wireframe cube moves to match IMU data.

Test:

1. quat_test.py Unit test for quat.py. Run under Unix build.

2.2 Display hardware

The setup3d.py script assumes an SSD1351 128*128 bit display such as this one - it may be adapted for other displays. The driver is here

The setup file must define the following data values:

1. Color values in a format suitable for its line and fill functions (see below).
2. DIMENSION = 64 This defines the number of pixels moved by a value of 1.0. By default a 128*128 pixel space represents values defined by -1.0 <= v <= 1.0.
3. imu Required for test_imu. An inertial measuring unit. It must have a quaternion method returning w, x, y, z. It may also have an euler method returning heading, roll, pitch. If euler is unsupported comment out the line in the test script that reports on those values.

The setup file must define these functions:

1. fill(color) Clear the display when called with BLACK.
2. line(xs, ys, xe, ye, color) Draw a line with args specified in absolute pixels.
3. show() Display all lines.

By default the imu object is a BNo055 however it can be created for other IMU types by means of the sensor fusion module which provides access to quaternion, heading, pitch and roll.

The test_imu rotating cube demo uses the Adafruit BNO055 breakout. The driver comprises these files: bno055.py and bno055_base.py.

The test3d.py script ends by drawing a moving sphere. This is RAM-intensive and will cause a memory error unless run on a Pyboard D.

2.3 Getting started

The module quat.py provides functions producing two types of object:

1. A Point representing a point (x, y, z) in 3D space.
2. A Rotator representing an axis and an angle of rotation.

The following fragment instantiates a point and rotates it 45° around the X axis:

from quat import Point, Rotator
from math import pi
point = Point(0, 0, 1)
rot = Rotator(pi/4, 1, 0, 0)  # Rotate 45° around x axis
point @= rot  # perform the rotation in-place

This can be extended to wireframe shapes, which can also be moved and scaled. Moving is done by adding a 3-tuple representing (dx, dy, dz) distances to move. Scaling may be done by multiplying by a value to make the object larger or smaller while preserving its shape. Alternatively the object can be multiplied by a 3-tuple to scale it differently along each axis:

from quat import Rotator
from math import pi
from graph3d import Cube, DisplayDict, Axes
from setup3d import *  # Hardware setup and colors

# Objects for display are put in a `DisplayDict` which specifies the camera
# location and distance for the perspective view
dobj = DisplayDict(setup(), pi/6, 5)
dobj['axes'] = Axes(WHITE)  # Show x, y, z axes
# By default the cube is from the origin to (1.0, 1.0, 1.0). Scale it
# to length of side 0.8 and centre it at origin
cube = Cube(RED, BLUE, GREEN) * (0.8, 0.8, 0.8) - (0.4, 0.4, 0.4)
# Rotate 45° around an axis from (0, 0, 0) to (1, 1, 1)
rot = Rotator(pi/4, 1, 1, 1)
dobj['cube'] = cube @ rot
dobj.show()  # Update the physical display

2.4 The quat module

This provides the following:

1. Point Represents a point in space. Constructor takes x, y and z floating point args where -1.0 <= arg <= 1.0.
2. Rotator An object which can rotate Point instances around an axis. The axis is defined by a line from the origin to a point. Constructor args: theta=0, x=0, y=0, z=0 where theta is the amount of rotation in radians.
3. Euler Returns a Rotator defined by Euler angles. Constructor args: heading, pitch, roll specified in radians.

2.5 The 3D graphics module

Code in this module uses Point and Rotator instances to create graphics primitives. Constructors create unit sized objects at a fixed location: objects may be scaled, distorted, moved or rotated prior to display.

In all cases color is a color value defined in setup3d.py.

The module provides the following objects:

1. Line Constructor args: p0, p1, color where p0 and p1 are Point instances.
2. Axes Draws the three axes. Constructor arg: color.
3. Square Draws a unit square located in the +ve quadrant. Arg: color.
4. Circle Unit circle centred on origin. Args: color, segments=12.
5. Sphere Unit sphere centred on origin. Args: color, segments=12.
6. Cone Unit cone with apex at origin. Args: color, segments=12.
7. Cube Unit cube in +ve quadrant. Args: color, front=None, sides=None. These represent color values. If color values are passed it allows front, back and sides to have different colors enabling orientation to readily be seen.
8. DisplayDict A dictionary of objects for display.

The Sphere object is demanding of RAM. I have only succeeded in displaying it on Pyboard D hosts (SF2 and SF6).

2.5.1 The DisplayDict class

This enables objects to be transformed or deleted at run time, and provides for display refresh.

Constructor: This takes the following args: ssd, angle, distance. ssd is the display instance. The other args specify the camera angle and distance. Typical values are pi/6 and 5.

Method:

1. show Clears and refreshes the display.

3. Quaternions

The mathematics of quaternions are interesting. I don't propose to elaborate on it here as there are many excellent online texts and demonstrations. The ones I found helpful are listed [below](./quaternions.md#Appendix 1: references). The following summarises the maths necessary to read the rest of this section; it uses Python notation rather than trying to represent mathematical equations.

A quaternion comprises four numbers w, x, y, z. These define an instance
q = w + x*i + y*j + z*k where i, j, k are square roots of -1 as per Hamilton's equation:
i**2 == j**2 == k**2 == i*j*k == -1

A quaternion's w value is its scalar part, with x, y and z defining its vector part. A quaternion with w == 0 is a vector quaternion and is created by the Vector or Point constructors in the code. It may be regarded as a point in space or a vector between the origin and that point, depending on context.

Quaternions have a magnitude represented by
magnitude = sqrt(sum((w*w + x*x + y*y + z*z))) This may be accessed by issuing magnitude = abs(q).

A quaternion whose vector part is 0 (i.e. x, y, z are 0) is a scalar. If w is 1 it is known as a unit quaternion.

A quaternion with a nonzero vector part and a magnitude of 1 is a rotation quaternion. These may be produced by the Rotator constructor. If its x y z values are considered as a vector starting at the origin, w represents the amount of rotation about that axis.

Multiplication of quaternions is not commutative: in general
q1 * q2 != q2 * q1
Multiplication of quaternions produces a Hamilton product.

Quaternions have a conjugate analogous to the conjugate of a complex. If q is a quaternion its conjugate may be found with:
conj = Quaternion(q.w, -q.x, -q.y, -q.z) In code conj = q.conjugate().

A Point (vector quaternion) P may be rotated around an arbitrary axis by defining a rotation quaternion R and performing the following
P = R * P * conjugate(P)
This is performed using the matmul @ operator, e.g.: P @= R The use of this operator allows rotations to be used in arithmetic expressions.

Division of quaternions is covered in section 3.9.

3.1 The Quaternion class

This uses Python special ("dunder" or magic) methods to enable arithmetic and comparison operators and to support the iterator protocol.

q = Quaternion(5, 6, 7, 8)
q2 = q * 2
q2 = 2 * q  # See NOTE on operand ordering
print(q * 2 + (20, 30, 40))

Special constructors are provided to produce vector and rotation quaternions, with the class constructor enabling arbitrary quaternions to be created.

NOTE: Unless firmware is compiled with MICROPY_PY_REVERSE_SPECIAL_METHODS arithmetic expressions should have the quaternion on the left hand side. Thus q * 2 will work, but 2 * q will throw an exception on such builds. See build notes.

3.2 Quaternion constructors

There are constructors producing vector and rotation quaternions. In addition the actual class constructor enables any quaternion to be instantiated.

3.2.1 Vector or Point

These take 3 args x, y, z. The constructor returns a vector quaternion with w == 0.

3.2.2 Rotator

This takes 4 args theta=0, x=0, y=0, z=0. It returns a rotation quaternion with magnitude 1. The vector from the origin to x, y, z is the axis of rotation and theta is the angle in radians.

3.2.3 Euler

This takes 3 args heading, pitch, roll and returns a rotation quaternion. It aims to be based on Tait-Bryan angles, however this implementation assumes that positive z is upwards: some assume it is downwards. Euler angles have more standards than you can shake a stick at: Euler angles are horrible.

3.2.4 The Quaternion constructor

This takes four args w=1, x=0, y=0, z=0. The default is the unit quaternion.

3.3 Quaternion properties

Quaternion instances support w, x, y, z read/write properties.

3.4 Iterator protocol

Quaternion instances support this. Thus

q = Quaternion(5, 6, 7, 8)
print(q[1:])  # Prints the vector part array('f', [6.0, 7.0, 8.0])
q[1:] = (9, 10, 11)
print(q[1:])  # array('f', [9.0, 10.0, 11.0])
for x in q:
    print(x)  # It is possible to iterate over the elements

3.5 abs() len() and str()

q = Quaternion(5, 6, 7, 8)
abs(q)  # Returns its magnitude 13.19090595827292
len(q)  # Always 4
str(q)  # 'w = 5.00 x = 6.00 y = 7.00 z = 8.00'

3.6 Comparison operators

Two Quaternion instances are equal if all their w, x, y, z values match. Otherwise their maginitudes are compared. Thus q1 == q2 tests for exact equality. q1 >= q2 will return True if they are exactly equal or if abs(q1) > abs(q2).

The test for "exact" equality is subject to the issues around comparison of floating point numbers. The math.isclose method is used with a value of 0.001 (0.1%). This may be adjusted (mdelta and adelta variables).

3.7 Addition and subtraction

A Quaternion instance may have the following types added to it or subtracted from it, resulting in a new Quaternion instance being returned.

1. Another Quaternion.
2. A scalar. This modifies the w value.
3. A 4-tuple or list: modifies w, x, y, z.
4. A 3-tuple or list: modifies the vector part x, y, z and sets the scalar part w to zero returning a vector quaternion. Intended for moving Point instances.

The typical application of addition of a 3-tuple is the movement of a Point.

3.8 Multiplication

A Quaternion instance may be mutliplied by the following types resulting in a new Quaternion instance being returned.

1. Another Quaternion. Returns the Hamilton product. Multiplication is not commutative.
2. A scalar. This multiplies w, x, y, z.
3. A 4-tuple or list: multiplies w, x, y, z by each element.
4. A 3-tuple or list: multiplies the vector part x, y, z by each element and sets the scalar part w to zero returning a vector quaternion. Intended for scaling Point instances.

Multiplication by a scalar or a tuple is commutative. If a list of points comprises a shape, multiplication by a scalar can be used to resize the shape. Multiplication by a 3-tuple can transform a shape, for example changing a cube to a cuboid.

3.9 Division

It is possible to divide a scalar by a quaternion: 1/q produces the inverse of quaternion q. This may also be acquired by issuing q.inverse(). The following statements are equivalent:

q1 = (1, 2, 3) / Quaternion(4, 5, 6, 7)
q2 = Quaternion(4, 5, 6, 7).inverse() * (1, 2, 3)

Note that multiplication of a quaternion by a scalar or by a tuple is commutative, while multiplication of quaternions is not. Consequently the notation p/q where p and q are quaternions is ambiguous because it is unclear whether it corresponds to p*q.inverse() or q.inverse()*p. The Quaternion class disallows this, raising a ValueError.

See build notes.

3.10 Rotation

Rotations are not commutative. Place two books side by side. Rotate one 90° clockwise, then flip it 180° about an axis facing away from you. Repeat with the other book in the opposite order.

Rotation is implemented using the matmul @ operator. This is normally applied with a Point or Vector quaternion on the LHS and a Rotator (rotation quaternion) on the RHS.

point = Point(1, 0, 0)
rot = Rotator(pi/6, 0, 1, 0)
new_point = point @ rot
point @= rot  # Rotate in place

There is an rrot method performing order-reversed rotation:

    def __matmul__(self, rot):
        return rot * self * rot.conjugate()

    def rrot(self, rot):
        return rot.conjugate() * self * rot

3.10.1 Rotation direction

Consider these Rotator instances:

rotx = Rotator(0.1, 1, 0, 0)  # About X
roty = Rotator(0.1, 0, 1, 0)  # Y
rotz = Rotator(0.1, 0, 0, 1)  # Z
point = Point(1, 1, 1)

Applying a rotator with a positive angle to a point will cause anticlockwise rotation as seen from a positive location on the relevant axis looking towards the origin. This is analogous to 2d rotation on the complex plane where multiplying by (say) 1 + 1j will produce an anticlockwise rotation.

3.11 Other methods

1. conjugate No args, returns a new Quaternion being its conjugate.
2. inverse No args, returns a new Quaternion being its inverse.
3. to_angle_axis No args. Intended for rotation quaternions. Returns [theta, x, y, z] where theta is the rotation angle and the vector from the origin to x, y, z is the axis.
4. copy No args. Returns a copy of the instance.
5. normalise Aims to produce a rotation quaternion: returns a new quaternion instance whose maginitude is 1. Intended to compensate for any accumulation of errors in a rotation quaternion.
6. isvec No args. Returns True if it is a vector quaternion i.e. w == 0.
7. isrot No args. Returns True if it is a rotation quaternion i.e. abs(q) == 1.

3.12 The euler function

This takes a Quaternion instance as an arg. Returns heading, pitch, roll. See section 3.2.3.

3.13 Build notes

Not all ports provide math.isclose and the reverse special methods. To some extent the lack of reverse special methods can be worked around by changing the operand order in expressions to ensure that the left hand side of a binary operator is a Quaternion instance. This cannot be applied to division and the .inverse() method must be used.

Bettrr, edit ports/port/mpconfigport.h to ensure these lines are included:

#define MICROPY_PY_MATH_ISCLOSE     (1)

#define MICROPY_PY_ALL_SPECIAL_METHODS (1)  // May already be present
#define MICROPY_PY_REVERSE_SPECIAL_METHODS (1)  // Add this line

and rebuild the firmware.

Appendix 1: references

Visalising quaternions Visalising quaternions - interactive Using Quaternion to Perform 3D rotations The Wikipedia article is maths-heavy. Wikpedia The following ref is good, but its formula for multiplication reverses the order of its arguments compared to the other sources. The q1q2 from other sources corresponds to q2q1 from this paper. Rotation Quaternions, and How to Use Them

From the youtube video: p -> (q2(q1pq1**-1)q2**-1)

Why to use quaternions rather than Euler angles.