From 8fd6341ca354cf2908171440be00cf5d94892161 Mon Sep 17 00:00:00 2001 From: Peter Hinch Date: Tue, 18 Aug 2020 18:51:29 +0100 Subject: [PATCH] Quaternions: initial commit. --- QUATERNIONS.md | 469 +++++++++++++++++++++++++++++++++ README.md | 34 ++- quaternion/graph3d.py | 190 +++++++++++++ quaternion/quat.py | 243 +++++++++++++++++ quaternion/quat_test.py | 201 ++++++++++++++ quaternion/setup3d.py | 54 ++++ quaternion/setup3d_lcd160cr.py | 49 ++++ quaternion/test3d.py | 51 ++++ quaternion/test_imu.py | 38 +++ 9 files changed, 1319 insertions(+), 10 deletions(-) create mode 100644 QUATERNIONS.md create mode 100644 quaternion/graph3d.py create mode 100644 quaternion/quat.py create mode 100644 quaternion/quat_test.py create mode 100644 quaternion/setup3d.py create mode 100644 quaternion/setup3d_lcd160cr.py create mode 100644 quaternion/test3d.py create mode 100644 quaternion/test_imu.py diff --git a/QUATERNIONS.md b/QUATERNIONS.md new file mode 100644 index 0000000..914d803 --- /dev/null +++ b/QUATERNIONS.md @@ -0,0 +1,469 @@ +# 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: + +```python +# 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](https://github.com/micropython-IMU/micropython-fusion). + +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](https://www.adafruit.com/product/1431) - it may be adapted for other +displays. The driver is [here](https://github.com/peterhinch/micropython-nano-gui/blob/master/drivers/ssd1351/ssd1351_16bit.py) + +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](https://github.com/micropython-IMU/micropython-fusion) +which provides access to quaternion, heading, pitch and roll. + +The `test_imu` rotating cube demo uses the +[Adafruit BNO055 breakout](https://www.adafruit.com/product/2472). The driver +comprises these files: +[bno055.py](https://github.com/micropython-IMU/micropython-bno055/blob/master/bno055.py) +and [bno055_base.py](https://github.com/micropython-IMU/micropython-bno055/blob/master/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: +```python +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: +```python +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](./QUATERNION.md#39-division). + +## 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. + +```python +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](./QUATERNION.md#313-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](https://github.com/moble/quaternion/wiki/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 +```python +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() + +```python +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: +```python +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](./QUATERNION.md#313-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. +```python +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: +```python +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](./quaternions.md#323-euler). + +## 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: +```C +#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](https://www.youtube.com/watch?v=d4EgbgTm0Bg) +[Visalising quaternions - interactive](https://eater.net/quaternions/video/doublecover) +[Using Quaternion to Perform 3D rotations](https://www.cprogramming.com/tutorial/3d/uses.html) +The Wikipedia article is maths-heavy. +[Wikpedia](https://en.wikipedia.org/wiki/Quaternion) +The following ref is good, but its formula for multiplication reverses the +order of its arguments compared to the other sources. The q1*q2 from +other sources corresponds to q2*q1 from this paper. +[Rotation Quaternions, and How to Use Them](http://danceswithcode.net/engineeringnotes/quaternions/quaternions.html) + +From the youtube video: p -> (q2(q1pq1**-1)q2**-1) + +Why to use quaternions rather than [Euler angles](https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible). diff --git a/README.md b/README.md index b89cfdd..43cf4ca 100644 --- a/README.md +++ b/README.md @@ -11,10 +11,10 @@ Pyboard variants. 1.4 [Buildcheck](./README.md#14-buildcheck) Check firmware build date 2. [Hardware information and drivers](./README.md#2-hardware-information-and-drivers) 2.1 [ESP32](./README.md#21-esp32) Pinout and notes on the reference board - 2.2 [SSD1306](./README.md#22-ssd1306) Write large fonts to the SSD1306 - 2.3 [Pyboard D](./README.md#23-pyboard-d) Assorted scraps of information - 2.4 [DS3231 precision RTC](./README.md#24-ds3231-precision-rtc) Use cheap hardware to calibrate Pyboard RTC - 3. [Essays](./README.md#3-essays) General thoughts + 2.2 [SSD1306](./README.md#22-ssd1306) Write large fonts to the SSD1306. + 2.3 [Pyboard D](./README.md#23-pyboard-d) Assorted scraps of information. + 2.4 [DS3231 precision RTC](./README.md#24-ds3231-precision-rtc) Use cheap hardware to calibrate Pyboard RTC. + 3. [Essays](./README.md#3-essays) General thoughts. 3.1 [Resilient](./README.md#31-resilient) A guide to writing resilient WiFi code 3.2 [Serialisation](./README.md#32-serialisation) Review of MicroPython's four serialisation libraries 3.3 [Measurement of relative timing and phase of fast analog signals](./README.md#33-measurement-of-relative-timing-and-phase-of-fast-analog-signals) For Pyboard. @@ -25,12 +25,13 @@ Pyboard variants. 4.4 [Reverse](./README.md#44-reverse) Reversal algorithms for bytearrays 4.5 [Timed function](./README.md#45-timed-function) Time execution with a decorator 4.6 [ESP8266 MQTT benchmark](./README.md#46-esp8266-mqtt-benchmark) Test performance of MQTT with official library - 4.7 [Rotary incremental encoder](./README.md#47-rotary-incremental-encoder) Fast, simple, proven algorithm + 4.7 [Rotary incremental encoder](./README.md#47-rotary-incremental-encoder) Fast, simple, proven algorithm. 4.8 [A pseudo random number generator](./README.md#48-a-pseudo-random-number-generator) - 4.9 [Verifying incrementing sequences](./README.md#49-verifying-incrementing-sequences) Test communications drivers - 4.10 [Bitmaps](./README.md#410-bitmaps) Non-allocating ways to access bitmaps - 4.11 [Functors and singletons](./README.md#411-functors-and-singletons) Useful decorators - 4.12 [A Pyboard power meter](./README.md#412-a-pyboard-power-meter) One of my own projects + 4.9 [Verifying incrementing sequences](./README.md#49-verifying-incrementing-sequences) Test communications drivers. + 4.10 [Bitmaps](./README.md#410-bitmaps) Non-allocating ways to access bitmaps. + 4.11 [Functors and singletons](./README.md#411-functors-and-singletons) Useful decorators. + 4.12 [Quaternions](./README.md#412-quaternions) Scale, move and rotate 3D objects with minimal mathematics. + 4.13 [A Pyboard power meter](./README.md#413-a-pyboard-power-meter) One of my own projects. # 1. Installation guides @@ -240,7 +241,20 @@ subsequent accesses being via `__call__`. As an object it can retain state. As an example, a functor might have a continuously running task: successive calls modify the behaviour of the task. -# 4.12 A pyboard power meter +## 4.12 Quaternions + +Quaternions have a reputation for being mathematically difficult. Surely true +if you intend using them to write Maxwell's equations; if your ambitions are +limited to manipulating three dimensional objects or processing IMU data they +can be remarkably simple. + +The `Quaternion` class lets you create, move, transform and rotate 3D objects +with no maths beyond familiarity with an `xyz` coordinate system. Includes a +demo where a wireframe cube rotates in response to movements of a BNo055 IMU, +plus a demo of moving wireframe graphics. Neither demo uses trig functions. +See [the docs](./QUATERNIONS.md). + +## 4.13 A pyboard power meter This uses a Pyboard to measure the power consumption of mains powered devices. Unlike simple commercial devices it performs a true vector (phasor) measurement diff --git a/quaternion/graph3d.py b/quaternion/graph3d.py new file mode 100644 index 0000000..c608bdb --- /dev/null +++ b/quaternion/graph3d.py @@ -0,0 +1,190 @@ +# graph3d.py 3D graphics demo of quaternion use + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +import gc +from math import pi +from quat import Rotator, Point +from setup3d import fill, line, show, DIMENSION + +class Line: + def __init__(self, p0, p1, color): + #assert p0.isvec() and p1.isvec() + self.start = p0 + self.end = p1 + self.color = color + + def show(self, ssd): + _, xs, ys, zs = self.start + _, xe, ye, ze = self.end + # Handle perspective and scale to display + w = DIMENSION # Viewing area is square + h = w + xs = round((1 + xs/zs) * w) + ys = round((1 - ys/zs) * h) + xe = round((1 + xe/ze) * w) + ye = round((1 - ye/ze) * h) + line(xs,ys, xe, ye, self.color) + + def __add__(self, to): # to is a Point or 3-tuple + return Line(self.start + to, self.end + to, self.color) + + def __sub__(self, v): # to is a Point or 3-tuple + return Line(self.start - v, self.end - v, self.color) + + def __mul__(self, by): # by is a 3-tuple + return Line(self.start * by, self.end * by, self.color) + + def __matmul__(self, rot): # rot is a rotation quaternion + #assert rot.isrot() + return Line(self.start @ rot, self.end @ rot, self.color) + + def camera(self, rot, distance): # rot is a rotation quaternion, distance is scalar + #assert rot.isrot() + gc.collect() + ps = self.start @ rot + ps = Point(ps.x * distance, ps.y * distance, distance - ps.z) + pe = self.end @ rot + pe = Point(pe.x * distance, pe.y * distance, distance - pe.z) + return Line(ps, pe, self.color) + + def __str__(self): + return 'start {} end {}'.format(self.start, self.end) + +class Shape: + def __init__(self, lines): + self.lines = lines + + def __add__(self, to): + return Shape([l + to for l in self.lines]) + + def __sub__(self, v): + return Shape([l - v for l in self.lines]) + + def __mul__(self, by): + return Shape([l * by for l in self.lines]) + + def __matmul__(self, rot): + l = [] + for line in self.lines: + l.append(line @ rot) + return Shape(l) + + def camera(self, rot, distance): + l = [] + for line in self.lines: + l.append(line.camera(rot, distance)) + return Shape(l) + + def show(self, ssd): + for line in self.lines: + line.show(ssd) + + def __str__(self): + r = '' + for line in self.lines: + r = ''.join((r, '{}\n'.format(line))) + return r + +class Axes(Shape): + def __init__(self, color): + l = (Line(Point(-1.0, 0, 0), Point(1.0, 0, 0), color), + Line(Point(0, -1.0, 0), Point(0, 1.0, 0), color), + Line(Point(0, 0, -1.0), Point(0, 0, 1.0), color)) + super().__init__(l) + +class Square(Shape): # Unit square in XY plane + def __init__(self, color): # Corner located at origin + l = (Line(Point(0, 0, 0), Point(1, 0, 0), color), + Line(Point(1, 0, 0), Point(1, 1, 0), color), + Line(Point(1, 1, 0), Point(0, 1, 0), color), + Line(Point(0, 1, 0), Point(0, 0, 0), color)) + super().__init__(l) + +class Cube(Shape): + def __init__(self, color, front=None, sides=None): # Corner located at origin + front = color if front is None else front + sides = color if sides is None else sides + l = (Line(Point(0, 0, 0), Point(1, 0, 0), color), + Line(Point(1, 0, 0), Point(1, 1, 0), color), + Line(Point(1, 1, 0), Point(0, 1, 0), color), + Line(Point(0, 1, 0), Point(0, 0, 0), color), + Line(Point(0, 0, 1), Point(1, 0, 1), front), + Line(Point(1, 0, 1), Point(1, 1, 1), front), + Line(Point(1, 1, 1), Point(0, 1, 1), front), + Line(Point(0, 1, 1), Point(0, 0, 1), front), + Line(Point(0, 0, 0), Point(0, 0, 1), sides), + Line(Point(1, 0, 0), Point(1, 0, 1), sides), + Line(Point(1, 1, 0), Point(1, 1, 1), sides), + Line(Point(0, 1, 0), Point(0, 1, 1), sides), + ) + super().__init__(l) + +class Cone(Shape): + def __init__(self, color, segments=12): + rot = Rotator(2*pi/segments, 0, 1, 0) + p0 = Point(1, 1, 0) + p1 = p0.copy() + orig = Point(0, 0, 0) + lines = [] + for _ in range(segments + 1): + p1 @= rot + lines.append(Line(p0, p1, color)) + lines.append(Line(orig, p0, color)) + p0 @= rot + super().__init__(lines) + +class Circle(Shape): # Unit circle in XY plane centred on origin + def __init__(self, color, segments=12): + rot = Rotator(2*pi/segments, 0, 1, 0) + p0 = Point(1, 0, 0) + p1 = p0.copy() + lines = [] + for _ in range(segments + 1): + p1 @= rot + lines.append(Line(p0, p1, color)) + p0 @= rot + super().__init__(lines) + +class Sphere(Shape): # Unit sphere in XY plane centred on origin + def __init__(self, color, segments=12): + lines = [] + s = Circle(color) + xrot = Rotator(2 * pi / segments, 1, 0, 0) + for _ in range(segments / 2 + 1): + gc.collect() + lines.extend(s.lines[:]) + s @= xrot + super().__init__(lines) + + +# Composition rather than inheritance as MP can't inherit builtin types. +class DisplayDict: + def __init__(self, ssd, angle, distance): + self.ssd = ssd + self.distance = distance # scalar + # Rotation quaternion for camera view + self.crot = Rotator(angle, 1, 1, 0) + self.d = {} + + def __setitem__(self, key, value): + if not isinstance(value, Shape): + raise ValueError('DisplayDict entries must be Shapes') + self.d[key] = value + + def __getitem__(self, key): + return self.d[key] + + def __delitem__(self, key): + del self.d[key] + + def show(self): + ssd = self.ssd + fill(0) + crot = self.crot + dz = self.distance + for shape in self.d.values(): + s = shape.camera(crot, dz) + s.show(ssd) + show() diff --git a/quaternion/quat.py b/quaternion/quat.py new file mode 100644 index 0000000..3b5c890 --- /dev/null +++ b/quaternion/quat.py @@ -0,0 +1,243 @@ +# quat.py "Micro" Quaternion class + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +from math import sqrt, sin, cos, acos, isclose, asin, atan2, pi +from array import array +mdelta = 0.001 # 0.1% Minimum difference considered significant for graphics +adelta = 0.001 # Absolute tolerance for components near 0 + +def _arglen(arg): + length = 0 + try: + length = len(arg) + except TypeError: + pass + if length not in (0, 3, 4): + raise ValueError('Sequence length must be 3 or 4') + return length + +# Convert a rotation quaternion to Euler angles. Beware: +# https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible +def euler(q): # Return (heading, pitch, roll) + if not q.isrot(): + raise ValueError('Must be a rotation quaternion.') + w, x, y, z = q + pitch = asin(2*(w*y - x*z)) + if isclose(pitch, pi/2, rel_tol = mdelta): + return -2 * atan2(x, w), pitch, 0 + if isclose(pitch, -pi/2, rel_tol = mdelta): + return 2 * atan2(x, w), pitch, 0 + roll = atan2(2*(w*x + y*z), w*w - x*x - y*y + z*z) + #roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)) + hdg = atan2(2*(w*z + x*y), w*w + x*x - y*y - z*z) + #hdg = atan2(2*(w*z + x*y), 1 - 2 *(y*y + z*z)) + return hdg, pitch, roll + + +class Quaternion: + + def __init__(self, w=1, x=0, y=0, z=0): # Default: the identity quaternion + self.d = array('f', (w, x, y, z)) + + @property + def w(self): + return self[0] + + @w.setter + def w(self, v): + self[0] = v + + @property + def x(self): + return self[1] + + @x.setter + def x(self, v): + self[1] = v + + @property + def y(self): + return self[2] + + @y.setter + def y(self, v): + self[2] = v + + @property + def z(self): + return self[3] + + @z.setter + def z(self, v): + self[3] = v + + def normalise(self): + if self[0] == 1: # acos(1) == 0. Identity quaternion: no rotation + return Quaternion(1, 0, 0, 0) + m = abs(self) # Magnitude + assert m > 0.1 # rotation quaternion should have magnitude ~= 1 + if isclose(m, 1.0, rel_tol=mdelta): + return self # No normalisation necessary + return Quaternion(*(a/m for a in self)) + + def __getitem__(self, key): + return self.d[key] + + def __setitem__(self, key, v): + try: + v1 = array('f', v) + except TypeError: # Scalar + v1 = v + self.d[key] = v1 + + def copy(self): + return Quaternion(*self) + + def __abs__(self): # Return magnitude + return sqrt(sum((d*d for d in self))) + + def __len__(self): + return 4 + # Comparison: == and != perform equality test of all elements + def __eq__(self, other): + return all((isclose(a, b, rel_tol=mdelta, abs_tol=adelta) for a, b in zip(self, other))) + + def __ne__(self, other): + return not self == other + + # < and > comparisons compare magnitudes. + def __gt__(self, other): + return abs(self) > abs(other) + + def __lt__(self, other): + return abs(self) < abs(other) + + # <= and >= return True for complete equality otherwise magnitudes are compared. + def __ge__(self, other): + return True if self == other else abs(self) > abs(other) + + def __le__(self, other): + return True if self == other else abs(self) < abs(other) + + def to_angle_axis(self): + q = self.normalise() + if isclose(q[0], 1.0, rel_tol = mdelta): + return 0, 1, 0, 0 + theta = 2*acos(q[0]) + s = sin(theta/2) + return [theta] + [a/s for a in q[1:]] + + def conjugate(self): + return Quaternion(self[0], *(-a for a in self[1:])) + + def inverse(self): # Reciprocal + return self.conjugate()/sum((d*d for d in self)) + + def __str__(self): + return 'w = {:4.2f} x = {:4.2f} y = {:4.2f} z = {:4.2f}'.format(*self) + + def __pos__(self): + return Quaternion(*self) + + def __neg__(self): + return Quaternion(*(-a for a in self)) + + def __truediv__(self, scalar): + if isinstance(scalar, Quaternion): # See docs for reason + raise ValueError('Cannot divide by Quaternion') + return Quaternion(*(a/scalar for a in self)) + + def __rtruediv__(self, other): + return self.inverse() * other + + # Multiply by quaternion, list, tuple, or scalar: result = self * other + def __mul__(self, other): + if isinstance(other, Quaternion): + w1, x1, y1, z1 = self + w2, x2, y2, z2 = other + w = w1*w2 - x1*x2 - y1*y2 - z1*z2 + x = w1*x2 + x1*w2 + y1*z2 - z1*y2 + y = w1*y2 - x1*z2 + y1*w2 + z1*x2 + z = w1*z2 + x1*y2 - y1*x2 + z1*w2 + return Quaternion(w, x, y, z) + length = _arglen(other) + if length == 0: # Assume other is scalar + return Quaternion(*(a * other for a in self)) + elif length == 3: + return Quaternion(0, *(a * b for a, b in zip(self[1:], other))) + # length == 4: + return Quaternion(*(a * b for a, b in zip(self, other))) + + def __rmul__(self, other): + return self * other # Multiplication by scalars and tuples is commutative + + def __add__(self, other): + if isinstance(other, Quaternion): + return Quaternion(*(a + b for a, b in zip(self, other))) + length = _arglen(other) + if length == 0: # Assume other is scalar + return Quaternion(self[0] + other, *self[1:]) # ? Is adding a scalar meaningful? + elif length == 3: + return Quaternion(0, *(a + b for a, b in zip(self[1:], other))) + # length == 4: + return Quaternion(*(a + b for a, b in zip(self, other))) + + def __radd__(self, other): + return self.__add__(other) + + def __sub__(self, other): + if isinstance(other, Quaternion): + return Quaternion(*(a - b for a, b in zip(self, other))) + length = _arglen(other) + if length == 0: # Assume other is scalar + return Quaternion(self[0] - other, *self[1:]) # ? Is this meaningful? + elif length == 3: + return Quaternion(0, *(a - b for a, b in zip(self[1:], other))) + # length == 4: + return Quaternion(*(a - b for a, b in zip(self, other))) + + def __rsub__(self, other): + return other + self.__neg__() # via __radd__ + + def isrot(self): + return isclose(abs(self), 1.0, rel_tol = mdelta) + + def isvec(self): + return isclose(self[0], 0, abs_tol = adelta) + + def __matmul__(self, rot): + return rot * self * rot.conjugate() + + def rrot(self, rot): + return rot.conjugate() * self * rot + +# A vector quaternion has real part 0. It can represent a point in space. +def Vector(x, y, z): + return Quaternion(0, x, y, z) + +Point = Vector + +# A rotation quaternion is a unit quaternion i.e. magnitude == 1 +def Rotator(theta=0, x=0, y=0, z=0): + s = sin(theta/2) + m = sqrt(x*x + y*y + z*z) # Convert to unit vector + if m > 0: + return Quaternion(cos(theta/2), s*x/m, s*y/m, s*z/m) + else: + return Quaternion(1, 0, 0, 0) # Identity quaternion + +def Euler(heading, pitch, roll): + cy = cos(heading * 0.5); + sy = sin(heading * 0.5); + cp = cos(pitch * 0.5); + sp = sin(pitch * 0.5); + cr = cos(roll * 0.5); + sr = sin(roll * 0.5); + + w = cr * cp * cy + sr * sp * sy; + x = sr * cp * cy - cr * sp * sy; + y = cr * sp * cy + sr * cp * sy; + z = cr * cp * sy - sr * sp * cy; + return Quaternion(w, x, y, z) # Tait-Bryan angles but z == towards sky diff --git a/quaternion/quat_test.py b/quaternion/quat_test.py new file mode 100644 index 0000000..7ea4c0a --- /dev/null +++ b/quaternion/quat_test.py @@ -0,0 +1,201 @@ +# quat_test.py Test for quat.py + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +from math import sin, cos, isclose, pi, sqrt +from quat import * + +print('Properties') +q1 = Quaternion(1, 2, 3, 4) +q1.w = 5 +q1.x = 6 +q1.y = 7 +q1.z = 8 +assert q1 == Quaternion(5, 6, 7, 8) +assert (q1.w, q1.x, q1.y, q1.z) == (5, 6, 7, 8) + +# Numpy demo at https://quaternion.readthedocs.io/en/latest/README.html +print('Hamilton product') +q1 = Quaternion(1, 2, 3, 4) +q2 = Quaternion(5, 6, 7, 8) +q3 = Quaternion(-60, 12, 30, 24) +assert q3 == q1 * q2 + +print('Iterator protocol') +assert Quaternion(*q1) == q1 +assert list(q1[1:]) == [2,3,4] +foo = iter(q1) +assert next(foo) == 1 + +print('Assign from tuple') +q1[1:] = (9, 10, 11) +assert list(q1[1:]) == [9, 10, 11] +q1[:] = (8, 9, 10, 99) +assert list(q1[:]) == [8, 9, 10, 99] + +print('Assign from scalar') +q1[0] = 88 +assert list(q1[:]) == [88, 9, 10, 99] + +print('Negation') +q1 = Quaternion(1, 2, 3, 4) +q2 = Quaternion(-1, -2, -3, -4) +assert -q1 == q2 + +print('Comparison operators and unary +') +assert (q1 is +q1) == False +assert q1 == +q1 +assert (q1 is q1.copy()) == False +assert q1 == q1.copy() +assert q1 >= q1.copy() +assert q1 <= q1.copy() +assert (q1 < q1.copy()) == False +assert (q1 > q1.copy()) == False + +q2 = Quaternion(1, 2.1, 3, 4) +assert q2 > q1 +assert q1 < q2 +assert q2 >= q1 +assert q1 <= q2 +assert (q1 == q2) == False +assert q1 != q2 + +print('Scalar add') +q2 = Quaternion(5, 2, 3, 4) +assert q2 == q1 + 4 + +print('Scalar subtract') +q2 = Quaternion(-3, 2, 3, 4) +assert q2 == q1 - 4 + +print('Scalar multiply') +q2 = Quaternion(2, 4, 6, 8) +assert q2 == q1 * 2 + +print('Scalar divide') +q2 = Quaternion(0.5, 1, 1.5, 2) +assert q2 == q1/2 + +print('Conjugate') +assert q1.conjugate() == Quaternion(1, -2, -3, -4) + +print('Inverse') +assert q1.inverse() * q1 == Quaternion(1, 0, 0, 0) + +print('Multiply by tuple') +assert q1*(2,3,4) == Quaternion(0, 4, 9, 16) +assert q1*(4,5,6,7) == Quaternion(4, 10, 18, 28) + +print('Add tuple') +assert q1 + (2,3,4) == Quaternion(0, 4, 6, 8) +assert q1 + (4,5,6,7) == Quaternion(5, 7, 9, 11) + +print('abs(), len(), str()') +assert abs(Quaternion(2,2,2,2)) == 4 +assert len(q1) == 4 +assert str(q1) == 'w = 1.00 x = 2.00 y = 3.00 z = 4.00' + +print('Rotation') +p = Vector(0, 1, 0) +r = Rotator(pi/4, 0, 0, 1) +rv = p @ r # Anticlockwise about z axis +assert isclose(rv.w, 0, abs_tol=mdelta) +assert isclose(rv.x, -sin(pi/4), rel_tol=mdelta) +assert isclose(rv.y, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.z, 0, abs_tol=mdelta) + + +p = Vector(1, 0, 0) +r = Rotator(-pi/4, 0, 0, 1) +rv = p @ r # Clockwise about z axis +assert isclose(rv.w, 0, abs_tol=mdelta) +assert isclose(rv.x, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.y, -sin(pi/4), rel_tol=mdelta) +assert isclose(rv.z, 0, abs_tol=mdelta) + +p = Vector(0, 1, 0) +r = Rotator(-pi/4, 1, 0, 0) +rv = p @ r # Clockwise about x axis +assert isclose(rv.w, 0, abs_tol=mdelta) +assert isclose(rv.x, 0, abs_tol=mdelta) +assert isclose(rv.y, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.z, -sin(pi/4), rel_tol=mdelta) + +print('Rotation using Euler angles') +# Tait-Brian angles DIN9300: I thought z axis is down towards ground. +# However https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +# and this implementation implies z is towards sky. +# https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible + +# Test heading +# Yaw/Heading: a +ve value is counter clockwise +p = Vector(1, 0, 0) # x is direction of motion +r = Euler(pi/4, 0, 0) # Heading 45°. +rv = p @ r +assert isclose(rv.w, 0, abs_tol=mdelta) +assert isclose(rv.x, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.y, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.z, 0, abs_tol=mdelta) + +# Test pitch +# A +ve value is aircraft nose down i.e. z +ve +p = Vector(1, 0, 0) # x is direction of motion +r = Euler(0, pi/4, 0) # Pitch 45°. +rv = p @ r +assert isclose(rv.w, 0, abs_tol=mdelta) +assert isclose(rv.x, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.y, 0, abs_tol=mdelta) +assert isclose(rv.z, -sin(pi/4), rel_tol=mdelta) # Implies z is towards sky + +# Test roll +# A +ve value is y +ve +p = Vector(0, 1, 0) # x is direction of motion. Vector is aircraft wing +r = Euler(0, 0, pi/4) # Roll 45°. +rv = p @ r +assert isclose(rv.w, 0, abs_tol=mdelta) +assert isclose(rv.x, 0, abs_tol=mdelta) +assert isclose(rv.y, sin(pi/4), rel_tol=mdelta) +assert isclose(rv.z, sin(pi/4), rel_tol=mdelta) # Implies z is towards sky + +print('euler() test') +r = Euler(pi/4, 0, 0) +assert isclose(euler(r)[0], pi/4, rel_tol=mdelta) +r = Euler(0, pi/4, 0) +assert isclose(euler(r)[1], pi/4, rel_tol=mdelta) +r = Euler(0, 0, pi/4) +assert isclose(euler(r)[2], pi/4, rel_tol=mdelta) + +print('isrot() and isvec()') +assert Quaternion(0, 1, 2, 3).isvec() +assert not Quaternion(0, 1, 2, 3).isrot() +assert not Quaternion(1, 2, 3, 4).isvec() +q = Rotator(1, 1, 1, 1) +assert q.isrot() + +print('to_angle_axis()') +t = Rotator(1, 1, 1, 1).to_angle_axis() +assert isclose(t[0], 1, rel_tol=mdelta) +for v in t[1:]: + assert isclose(v, sqrt(1/3), rel_tol=mdelta) + +s = ''' +*** Standard tests PASSED. *** + +The following test of reflected arithmetic operators will fail unless the +firmware was compiled with MICROPY_PY_REVERSE_SPECIAL_METHODS. +Runs on the Unix build.''' +print(s) + +q1 = Quaternion(1, 2, 3, 4) +assert 10 + Quaternion(1, 2, 3, 4) == Quaternion(11, 2, 3, 4) +assert 1/q1 == q1.inverse() +assert 2 * q1 == q1 + q1 +assert 1 - q1 == -q1 + 1 + +s = ''' +Reverse/reflected operators OK. + +*** All tests PASSED. *** +''' +print(s) diff --git a/quaternion/setup3d.py b/quaternion/setup3d.py new file mode 100644 index 0000000..978ecc8 --- /dev/null +++ b/quaternion/setup3d.py @@ -0,0 +1,54 @@ +# setup3d.py +# Hardware specific setup for 3D demos + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +from machine import I2C, SPI, Pin +import gc + +# This module must export the following functions +# fill(color) Fill the buffer with a color +# line(xs, ys, xe, ye, color) Draw a line to the buffer +# show() Display result +# Also dimension bound variable. + +from ssd1351_16bit import SSD1351 as SSD +_HEIGHT = const(128) # SSD1351 variant in use + +# IMU driver for test_imu only. With other IMU's the fusion module +# may be used for quaternion output. +# https://github.com/micropython-IMU/micropython-fusion +from bno055 import BNO055 +# Initialise IMU +i2c = I2C(2) +imu = BNO055(i2c) + +# Export color constants +WHITE = SSD.rgb(255, 255, 255) +GREY = SSD.rgb(100, 100, 100) +GREEN = SSD.rgb(0, 255, 0) +BLUE = SSD.rgb(0, 0, 255) +RED = SSD.rgb(255, 0, 0) +YELLOW = SSD.rgb(255, 255, 0) +CYAN = SSD.rgb(0, 255, 255) + + +# Initialise display +# Monkey patch size of square viewing area. No. of pixels for a change of 1.0 +# Viewing area is 128*128 +DIMENSION = 64 + +gc.collect() +_pdc = Pin('X1', Pin.OUT_PP, value=0) # Pins are for Pyboard +_pcs = Pin('X2', Pin.OUT_PP, value=1) +_prst = Pin('X3', Pin.OUT_PP, value=1) +_spi = SPI(2) # scl Y9 sda Y10 +_ssd = SSD(_spi, _pcs, _pdc, _prst, height=_HEIGHT) # Create a display instance + +line = _ssd.line +fill = _ssd.fill +show = _ssd.show + +def setup(): + return _ssd diff --git a/quaternion/setup3d_lcd160cr.py b/quaternion/setup3d_lcd160cr.py new file mode 100644 index 0000000..f6a5efc --- /dev/null +++ b/quaternion/setup3d_lcd160cr.py @@ -0,0 +1,49 @@ +# setup3d_lcd160cr.py +# Hardware specific setup for 3D demos + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +from machine import I2C, SPI, Pin +import framebuf +import gc +from lcd160cr import LCD160CR as SSD +from lcd160cr import LANDSCAPE +# IMU driver for test_imu only. With other IMU's the fusion module +# may be used for quaternion output. +# https://github.com/micropython-IMU/micropython-fusion +#from bno055 import BNO055 +## Initialise IMU +#i2c = I2C(1) +#imu = BNO055(i2c) + +# Export color constants +WHITE = SSD.rgb(255, 255, 255) +GREY = SSD.rgb(100, 100, 100) +GREEN = SSD.rgb(0, 255, 0) +BLUE = SSD.rgb(0, 0, 255) +RED = SSD.rgb(255, 0, 0) +YELLOW = SSD.rgb(255, 255, 0) +CYAN = SSD.rgb(0, 255, 255) +BLACK = SSD.rgb(0, 0, 0) + +# DIMENSION No. of pixels for a change of 1.0 +# Viewing area is 128*128 +DIMENSION = 64 + +gc.collect() +_buf = bytearray(160*128*2) # 40KiB +_fb = framebuf.FrameBuffer(_buf, 160, 128, framebuf.RGB565) +_lcd = SSD('Y') +_lcd.set_orient(LANDSCAPE) +_lcd.set_spi_win(0, 0, 160, 128) +# Standard functions +line = _fb.line +fill = _fb.fill + +def show(): + gc.collect() + _lcd.show_framebuf(_fb) + +def setup(): + return _lcd diff --git a/quaternion/test3d.py b/quaternion/test3d.py new file mode 100644 index 0000000..6ad610f --- /dev/null +++ b/quaternion/test3d.py @@ -0,0 +1,51 @@ +# test3d.py 3D objects created using quaternions + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +from math import pi +import gc +from quat import Rotator +import graph3d as g3d +from setup3d import * # Hardware setup and colors + +# Dict of display objects, each comprises a Shape +dobj = g3d.DisplayDict(setup(), pi/6, 5) # Camera angle and z distance + +dobj['axes'] = g3d.Axes(WHITE) # Draw axes + +def demo(): + dobj['cone'] = g3d.Cone(GREEN) * 0.7 + # Draw rectangle to check camera perspective + square = (g3d.Square(YELLOW) -(0.5, 0.5, 0)) * 1.3 + rot = Rotator(pi/12, 1, 0, 0) + dobj['perspective'] = square + for _ in range(24): + dobj['perspective'] @= rot + dobj.show() + rot = Rotator(pi/12, 0, 1, 0) + for _ in range(24): + dobj['perspective'] @= rot + dobj.show() + rot = Rotator(pi/12, 0, 0, 1) + for _ in range(24): + dobj['perspective'] @= rot + dobj.show() + dobj['perspective'] = g3d.Circle(RED) * 0.7 @ Rotator(pi/24, 0, 0, 1) # (1, 1, 0.5) for ellipse + for _ in range(24): + dobj['perspective'] @= rot + dobj.show() + del dobj['cone'] + del dobj['perspective'] + gc.collect() + print('RAM free {} alloc {}'.format(gc.mem_free(), gc.mem_alloc())) + dobj['perspective'] = g3d.Sphere(CYAN) * 0.5 - (0.5, 0.5, 0) + rot = Rotator(pi/96, 1, 0, 0) + gc.collect() + print('RAM free {} alloc {}'.format(gc.mem_free(), gc.mem_alloc())) + for _ in range(20): + dobj['perspective'] += (0.025, 0.025, 0) # @= rot + dobj.show() + del dobj['perspective'] + +demo() diff --git a/quaternion/test_imu.py b/quaternion/test_imu.py new file mode 100644 index 0000000..bd34341 --- /dev/null +++ b/quaternion/test_imu.py @@ -0,0 +1,38 @@ +# test_imu.py Demo of cube being rotated by IMU data. + +# Released under the MIT License (MIT). See LICENSE. +# Copyright (c) 2020 Peter Hinch + +from math import pi +from time import sleep +import gc +from quat import Rotator, euler +import graph3d as g3d +from setup3d import * # Hardware setup and colors + + +def imu_demo(): + # Dict of display objects, each comprises a Shape + dobj = g3d.DisplayDict(setup(), pi/6, 5) # Camera angle and z distance + dobj['axes'] = g3d.Axes(WHITE) # Draw axes + # Create a reference cube. Don't display. + cube = g3d.Cube(RED, BLUE, GREEN) * (0.8, 0.8, 0.8) - (0.4, 0.4, 0.4) + imuquat = Rotator() + x = 0 + while True: + dobj.show() + sleep(0.1) + imuquat[:] = imu.quaternion() # Assign IMU data to Quaternion instance + # imuquat.normalise() No need: BNo055 data is a rotation quaternion + dobj['cube'] = cube @ imuquat + x += 1 + if x == 10: + x = 0 + print('Quat heading {0:5.2f} roll {2:5.2f} pitch {1:5.2f}'.format(*euler(imuquat))) + print('IMU heading {:5.2f} roll {:5.2f} pitch {:5.2f}'.format(*[x * pi/180 for x in imu.euler()])) + gc.collect() + print(gc.mem_free(), gc.mem_alloc()) + del dobj['cube'] + +imu_demo() +