Tilt sensor and acceleration

[Debenben]

In my spare-time I am building a lego robot dog and would like to teach it to walk. Each leg has a technic-hub in the upper part and a (WeDo-)tilt-sensor in the lower part to keep track of the position using acceleration. For this I am using the tilt-sensor in "CAL" mode where it reads 3 angles that are supposed to go from -45 to 45 degrees. If you shake the sensors you can easily get values above or below that interval. For that reason and to be similar to the integrated accelerometer in the technic-hub I convert the values to acceleration like this:

from pybricks.iodevices import PUPDevice
from pybricks.parameters import Port
from pybricks.tools import wait
from umath import pi, sin, cos, sqrt

# Initialize a TiltSensor as a generic PUPDevice on Port A
tiltSensor = PUPDevice(Port.A)

while(True):
    # Read the values of the sensor in mode 3 which is CAL mode
    t = tiltSensor.read(3)

    # Convert angles in degree to radiant
    t = [v*pi/180 for v in t]

    # Calculate acceleration vector from tilt angles
    acceleration = [sin(t[0])*cos(t[1])*cos(t[2]),
                    cos(t[0])*sin(t[1])*cos(t[2]),
                    cos(t[0])*cos(t[1])*sin(t[2])]
    
    # Normalize acceleration vector such that it has
    # a magnitude of 1 at rest in standard gravity
    acceleration = [v*sqrt(2) for v in acceleration]

    print(acceleration)
    wait(100)

What do you think of this? This could maybe be a nice addition to the library. Maybe as an example in the documentation of the tilt-sensor although it is not using the tilt-sensor-class? Or the "CAL" mode could be implemented and then used like this? I also made a proof of concept of implementing this conversion itself

https://github.com/pybricks/pybricks-micropython/compare/master...Debenben:pybricks-micropython:acceleration?expand=1

I did not try to optimize it for compilation size, but I imagine one could make it smaller by restructuring and possibly reusing code.

Also I wanted to format the acceleration output like that of the internal technic-hub accelerometer but it seems the code does not match the documentation. The magnitude of the vector my technic-hub returns with the current master branch of pybricks-micropython is around 1000. The documentation

https://docs.pybricks.com/en/latest/hubs/technichub.html#pybricks.hubs.TechnicHub.imu.acceleration

sais the unit is mm/s^2. Gravitational acceleration on earth is around 9.81m/s^2 = 9810 mm/s^2 which is about 10 times larger than 1000 from the technic-hub.

[laurensvalk]

I am glad you asked! I have been wondering about exactly this, and prepared some code as well (I might need to dig it up somewhere).

I haven't added it to the device class because I wasn't sure anyone else would ever need it 😄

The use case I was originally after, was to provide tilt in the full range (outside of -45/45), and optionally also outside the default orientation as we do allow for hubs.

What do you think of this? This could maybe be a nice addition to the library. Maybe as an example in the documentation of the tilt-sensor although it is not using the tilt-sensor-class?

We probably couldn't add it to the Move Hub firmware due to build size, and it would be nice to keep the classes the same on all hubs.

So having a few examples like those might be the best thing to start with indeed. It would be great if you want to make a pull request in the docs.

Here's another possibility if you want to take it further. Perhaps as an additional example: Now that we support importing code across modules in Pybricks Code, it's possible to extend the TiltSensor class with an extra method.

In your example:

    # Calculate acceleration vector from tilt angles
    acceleration = [sin(t[0])*cos(t[1])*cos(t[2]),
                    cos(t[0])*sin(t[1])*cos(t[2]),
                    cos(t[0])*cos(t[1])*sin(t[2])]

Does this work along the whole sphere of rotations? Looking back at my notes, I found that I had to use various if/else branches to ensure that I didn't use one of the values that was capped at 45 degrees, but always the other two to get tilt/acceleration.

[laurensvalk]

As far as I know, the values are capped in software at 45 degrees, which makes me think that there isn't a single continuous mapping from the call values to angles or acceleration.

Looking at my notes, I think there was always one value at or close to +/-45 degrees, so I had 6 simple variants of the following:

For example, if the third value v3 was 45 then the direction of the acceleration vector is:

And then 5 other variants like this, depending on which value was saturated.

[laurensvalk]

I found this code snippet in my notes and just tested it again.

Note: this snippet does all steps numerically so we can see what is going on, but each of the 6 cases can be reduced symbolically to a very short form as given in the example above.

from pybricks.parameters import Port
from pybricks.iodevices import PUPDevice
from pybricks.tools import wait

from umath import pi, cos, sin

def prod(A, B):
    return [[sum([A[i][c]*B[c][j] for c in range(3)]) for j in range(3)] for i in range(3)]


class TiltSensor(PUPDevice):

    def __init__(self, port):
        self.io = PUPDevice(port)
        self.io.read(3)

    def acceleration(self):
        v1, v2, v3 = self.read(3)
        av1 = abs(v1)
        av2 = abs(v2)
        av3 = abs(v3)
        if av1 != 45 and av2 != 45 and av3 != 45:
            # did not find 45, so make the maximum 45 anway
            if av1 >= av2 and av1 >= av3:
                v1 = 45 if v1 > 0 else -45
            if av2 >= av1 and av2 >= av3:
                v2 = 45 if v2 > 0 else -45
            else:
                v3 = 45 if v3 > 0 else -45

        # In the following 2 cases, the front side points forwards
        if v3 == 45:
            # upwards=top
            a = -v2/180*pi
            b = v1/180*pi
            # print('top   :', a, b)
            pre = [[ 1, 0, 0],
                   [ 0, 1, 0],
                   [ 0, 0, 1]]
            R_w_s = [
                [ cos(b),            0,           sin(b)],
                [ sin(a)*sin(b), cos(a), -sin(a)*cos(b)],
                [-sin(b)*cos(a), sin(a),  cos(a)*cos(b)]
            ]
        elif v3 == -45:
            # upwards=bottom
            a = -v2/180*pi
            b = -v1/180*pi
            # print('bottom:', a, b)
            pre = [[-1, 0, 0],
                   [ 0, 1, 0],
                   [ 0, 0,-1]]
            R_w_s = [[cos(b), 0, sin(b)], [-sin(a)*sin(b), cos(a), sin(a)*cos(b)], [-sin(b)*cos(a), -sin(a), cos(a)*cos(b)]]
        # In the following 4 cases, the top side points forwards
        elif v1 == 45:
            # upward=left
            a = v3/180*pi
            b = -v2/180*pi
            # print('left  :', a, b)
            pre = [[ 0,-1, 0],
                   [ 0, 0, 1],
                   [-1, 0, 0]]
            R_w_s = [[cos(a)*cos(b), -sin(b)*cos(a), -sin(a)], [sin(b), cos(b), 0], [sin(a)*cos(b), -sin(a)*sin(b), cos(a)]]
        elif v1 == -45:
            # upward=right
            a = v3/180*pi
            b = v2/180*pi
            # print('right :', a, b)
            pre = [[ 0, 1, 0],
                   [ 0, 0, 1],
                   [ 1, 0, 0]]
            R_w_s = [[cos(a)*cos(b), -sin(b)*cos(a), sin(a)], [sin(b), cos(b), 0], [-sin(a)*cos(b), sin(a)*sin(b), cos(a)]]
        elif v2 == 45:
            # upward=back
            a = v3/180*pi
            b = v1/180*pi
            # print('back  :', a, b)
            pre = [[ 1, 0, 0],
                   [ 0, 0, 1],
                   [ 0,-1, 0]]
            R_w_s = [[cos(b), -sin(b), 0], [sin(b)*cos(a), cos(a)*cos(b), -sin(a)], [sin(a)*sin(b), sin(a)*cos(b), cos(a)]]
        elif v2 == -45:
            # upward=front
            a = v3/180*pi
            b = -v1/180*pi
            # print('front :', a, b)
            pre = [[-1, 0, 0],
                   [ 0, 0, 1],
                   [ 0, 1, 0]]
            R_w_s = [[cos(b), -sin(b), 0], [sin(b)*cos(a), cos(a)*cos(b), sin(a)], [-sin(a)*sin(b), -sin(a)*cos(b), cos(a)]]

        pr = prod(pre, R_w_s)
        # for row in pr:
        #     print([round(e, 2) for e in row])
        # print("-------------")

        return tuple(round(pr[2][i]*9810) for i in range(3))


sensor = TiltSensor(Port.A)

while True:

    print(sensor.acceleration())
    wait(100)

[Debenben]

@laurensvalk Thanks for sharing the code and the constructive feedback. You are using a more standard definition of spherical angles that would indeed fit with the assumption of one value always being at $\pm 45$ degrees. However this is not what I observe with my sensor.

For an acceleration direction $(1, 1, 0)/\sqrt 2$ two angles are $\pm 45$ degrees and the remaining one 0. This I can observe with my sensor and both your and my angle mapping would predict the same (modulo sign convention and my normalization mess).

An acceleration $(1, 1, 1)/\sqrt 3$ would be mapped to two angles at $\pm 45$ degrees and one at $\pm\arcsin(1/\sqrt 3)\approx 35$ degrees in your model. In my model it would be mapped to all three angles at $\pm\arcsin(1/\sqrt 3) \approx 35$ degrees. With my sensor I get all angles around 35 degrees in this case.

[Debenben]

Quick update: I still believe the tilt angles are to some rescaled "Coordinate Direction Angles" rather than traditional spherical angles, but not sure about the exact definition. I tried to find out by fixing the tiltsensor to a hub and comparing its readings to that of the hub accelerometer. To improve the accuracy of the hub accelerometer I made a simple calibration routine, adding an offset to each axis.

I compared my angle definition above, euler angles and a "small angle approximation" where the "tilt angles" are simply the components of the acceleration vector. All of those and some other approaches perform reasonably well, yet I was not able to reverse-engineer and find an algorithm with numerical approximation and calibration routine that would match completely.

When I saw pybricks/pybricks-micropython#135 I thought people might be interested in my accelerometer calibration. My goal is to use pybricks eventually, currently it is official lego firmware with nodejs: https://github.com/Debenben/babyele/blob/master/src/calibrator.ts. With a library for the least-squares fit, I could rewrite it in C or micropython for use on the hub and it would no longer be limited by bluetooth transfer rate. As far as I know, most accelerometer calibration routines also compute a gain factor and one could allow any three stable positions, the 6 directions are a nice to have but not required.

[Debenben]

Some time ago I did some more testing and it seems like the values that the sensor reports are neither fancy "Coordinate Direction Angles" nor Spherical Angles but rather acceleration values capped to 45 degrees. At 45 degrees, this "Small Angle Approximation" is different by about 8 percent.

Placing the hub on different cornes and edges and holding it by hand without any special calibration looks like this:
data.csv

Scripts for generating the above:
installOnHub.py.txt
runOnPc.py.txt
plot.gnuplot.txt

For use in pybricks, create a tiltsensorhelper file

from pybricks.iodevices import PUPDevice
from pybricks.parameters import Port, Axis
from pybricks.tools import vector
from umath import sqrt

_FACTOR = const(154.0966) # 9806.65/(45*Math.sqrt(2))

class TiltSensor(PUPDevice):
    def __init__(self, port):
        super().__init__(port)
    def acceleration(self, axis=None):
        [x,y,z] = self.read(3)
        if(x == 45):
            x = sqrt(2*45**2 - y**2 - z**2)
        elif(x == -45):
            x = -sqrt(2*45**2 - y**2 - z**2)
        elif(y == 45):
            y = sqrt(2*45**2 - x**2 - z**2)
        elif(y == -45):
            y = -sqrt(2*45**2 - x**2 - z**2)
        elif(z == 45):
            z = sqrt(2*45**2 - x**2 - y**2)
        elif(z == -45):
            z = -sqrt(2*45**2 - x**2 - y**2)
        
        if axis == None:
            return _FACTOR*vector(x, y, z)
        elif axis == Axis.X:
            return _FACTOR*x
        elif axis == Axis.Y:
            return _FACTOR*y
        elif axis == Axis.Z:
            return _FACTOR*z

and include it like this:

from pybricks.parameters import Port, Axis
from pybricks.tools import wait


from tiltsensorhelper import TiltSensor

sensor = TiltSensor(Port.A)
while True:
   print(sensor.acceleration())
   wait(500)
   print(sensor.acceleration(Axis.X))
   wait(500)